| // SPDX-License-Identifier: GPL-2.0 |
| /* |
| * stf_capture.c |
| * |
| * StarFive Camera Subsystem - capture device |
| * |
| * Copyright (C) 2021-2023 StarFive Technology Co., Ltd. |
| */ |
| |
| #include "stf-camss.h" |
| |
| static const char * const stf_cap_names[] = { |
| "capture_raw", |
| "capture_yuv", |
| }; |
| |
| static const struct stfcamss_format_info stf_wr_fmts[] = { |
| { |
| .code = MEDIA_BUS_FMT_SRGGB10_1X10, |
| .pixelformat = V4L2_PIX_FMT_SRGGB10, |
| .planes = 1, |
| .vsub = { 1 }, |
| .bpp = 10, |
| }, |
| { |
| .code = MEDIA_BUS_FMT_SGRBG10_1X10, |
| .pixelformat = V4L2_PIX_FMT_SGRBG10, |
| .planes = 1, |
| .vsub = { 1 }, |
| .bpp = 10, |
| }, |
| { |
| .code = MEDIA_BUS_FMT_SGBRG10_1X10, |
| .pixelformat = V4L2_PIX_FMT_SGBRG10, |
| .planes = 1, |
| .vsub = { 1 }, |
| .bpp = 10, |
| }, |
| { |
| .code = MEDIA_BUS_FMT_SBGGR10_1X10, |
| .pixelformat = V4L2_PIX_FMT_SBGGR10, |
| .planes = 1, |
| .vsub = { 1 }, |
| .bpp = 10, |
| }, |
| }; |
| |
| static const struct stfcamss_format_info stf_isp_fmts[] = { |
| { |
| .code = MEDIA_BUS_FMT_YUYV8_1_5X8, |
| .pixelformat = V4L2_PIX_FMT_NV12, |
| .planes = 2, |
| .vsub = { 1, 2 }, |
| .bpp = 8, |
| }, |
| }; |
| |
| static inline struct stf_capture *to_stf_capture(struct stfcamss_video *video) |
| { |
| return container_of(video, struct stf_capture, video); |
| } |
| |
| static void stf_set_raw_addr(struct stfcamss *stfcamss, dma_addr_t addr) |
| { |
| stf_syscon_reg_write(stfcamss, VIN_START_ADDR_O, (long)addr); |
| stf_syscon_reg_write(stfcamss, VIN_START_ADDR_N, (long)addr); |
| } |
| |
| static void stf_set_yuv_addr(struct stfcamss *stfcamss, |
| dma_addr_t y_addr, dma_addr_t uv_addr) |
| { |
| stf_isp_reg_write(stfcamss, ISP_REG_Y_PLANE_START_ADDR, y_addr); |
| stf_isp_reg_write(stfcamss, ISP_REG_UV_PLANE_START_ADDR, uv_addr); |
| } |
| |
| static void stf_init_addrs(struct stfcamss_video *video) |
| { |
| struct stf_capture *cap = to_stf_capture(video); |
| struct stf_v_buf *output = &cap->buffers; |
| dma_addr_t addr0, addr1; |
| |
| output->active_buf = 0; |
| |
| if (!output->buf[0]) |
| return; |
| |
| addr0 = output->buf[0]->addr[0]; |
| addr1 = output->buf[0]->addr[1]; |
| |
| if (cap->type == STF_CAPTURE_RAW) |
| stf_set_raw_addr(video->stfcamss, addr0); |
| else if (cap->type == STF_CAPTURE_YUV) |
| stf_set_yuv_addr(video->stfcamss, addr0, addr1); |
| } |
| |
| static struct stfcamss_buffer *stf_buf_get_pending(struct stf_v_buf *output) |
| { |
| struct stfcamss_buffer *buffer = NULL; |
| |
| if (!list_empty(&output->pending_bufs)) { |
| buffer = list_first_entry(&output->pending_bufs, |
| struct stfcamss_buffer, |
| queue); |
| list_del(&buffer->queue); |
| } |
| |
| return buffer; |
| } |
| |
| static void stf_cap_s_cfg(struct stfcamss_video *video) |
| { |
| struct stf_capture *cap = to_stf_capture(video); |
| struct stf_v_buf *output = &cap->buffers; |
| unsigned long flags; |
| |
| spin_lock_irqsave(&output->lock, flags); |
| |
| output->state = STF_OUTPUT_IDLE; |
| output->buf[0] = stf_buf_get_pending(output); |
| |
| if (!output->buf[0] && output->buf[1]) { |
| output->buf[0] = output->buf[1]; |
| output->buf[1] = NULL; |
| } |
| |
| if (output->buf[0]) |
| output->state = STF_OUTPUT_SINGLE; |
| |
| output->sequence = 0; |
| stf_init_addrs(video); |
| |
| spin_unlock_irqrestore(&output->lock, flags); |
| } |
| |
| static int stf_cap_s_cleanup(struct stfcamss_video *video) |
| { |
| struct stf_capture *cap = to_stf_capture(video); |
| struct stf_v_buf *output = &cap->buffers; |
| unsigned long flags; |
| |
| spin_lock_irqsave(&output->lock, flags); |
| |
| output->state = STF_OUTPUT_OFF; |
| |
| spin_unlock_irqrestore(&output->lock, flags); |
| |
| return 0; |
| } |
| |
| static void stf_wr_data_en(struct stfcamss_video *video) |
| { |
| struct stf_capture *cap = to_stf_capture(video); |
| struct stfcamss *stfcamss = cap->video.stfcamss; |
| |
| stf_syscon_reg_set_bit(stfcamss, VIN_CHANNEL_SEL_EN, U0_VIN_AXIWR0_EN); |
| } |
| |
| static void stf_wr_irq_enable(struct stfcamss_video *video) |
| { |
| struct stf_capture *cap = to_stf_capture(video); |
| struct stfcamss *stfcamss = cap->video.stfcamss; |
| |
| stf_syscon_reg_clear_bit(stfcamss, VIN_INRT_PIX_CFG, U0_VIN_INTR_M); |
| } |
| |
| static void stf_wr_irq_disable(struct stfcamss_video *video) |
| { |
| struct stf_capture *cap = to_stf_capture(video); |
| struct stfcamss *stfcamss = cap->video.stfcamss; |
| |
| stf_syscon_reg_set_bit(stfcamss, VIN_INRT_PIX_CFG, U0_VIN_INTR_CLEAN); |
| stf_syscon_reg_clear_bit(stfcamss, VIN_INRT_PIX_CFG, U0_VIN_INTR_CLEAN); |
| stf_syscon_reg_set_bit(stfcamss, VIN_INRT_PIX_CFG, U0_VIN_INTR_M); |
| } |
| |
| static void stf_channel_set(struct stfcamss_video *video) |
| { |
| struct stf_capture *cap = to_stf_capture(video); |
| struct stfcamss *stfcamss = cap->video.stfcamss; |
| u32 val; |
| |
| if (cap->type == STF_CAPTURE_RAW) { |
| val = stf_syscon_reg_read(stfcamss, VIN_CHANNEL_SEL_EN); |
| val &= ~U0_VIN_CHANNEL_SEL_MASK; |
| val |= CHANNEL(0); |
| stf_syscon_reg_write(stfcamss, VIN_CHANNEL_SEL_EN, val); |
| |
| val = stf_syscon_reg_read(stfcamss, VIN_INRT_PIX_CFG); |
| val &= ~U0_VIN_PIX_CT_MASK; |
| val |= PIX_CT(1); |
| |
| val &= ~U0_VIN_PIXEL_HEIGH_BIT_SEL_MAKS; |
| val |= PIXEL_HEIGH_BIT_SEL(0); |
| |
| val &= ~U0_VIN_PIX_CNT_END_MASK; |
| val |= PIX_CNT_END(IMAGE_MAX_WIDTH / 4 - 1); |
| |
| stf_syscon_reg_write(stfcamss, VIN_INRT_PIX_CFG, val); |
| } else if (cap->type == STF_CAPTURE_YUV) { |
| val = stf_syscon_reg_read(stfcamss, VIN_CFG_REG); |
| val &= ~U0_VIN_MIPI_BYTE_EN_ISP0_MASK; |
| val |= U0_VIN_MIPI_BYTE_EN_ISP0(0); |
| |
| val &= ~U0_VIN_MIPI_CHANNEL_SEL0_MASK; |
| val |= U0_VIN_MIPI_CHANNEL_SEL0(0); |
| |
| val &= ~U0_VIN_PIX_NUM_MASK; |
| val |= U0_VIN_PIX_NUM(0); |
| |
| val &= ~U0_VIN_P_I_MIPI_HAEDER_EN0_MASK; |
| val |= U0_VIN_P_I_MIPI_HAEDER_EN0(1); |
| |
| stf_syscon_reg_write(stfcamss, VIN_CFG_REG, val); |
| } |
| } |
| |
| static void stf_capture_start(struct stfcamss_video *video) |
| { |
| struct stf_capture *cap = to_stf_capture(video); |
| |
| stf_channel_set(video); |
| if (cap->type == STF_CAPTURE_RAW) { |
| stf_wr_irq_enable(video); |
| stf_wr_data_en(video); |
| } |
| |
| stf_cap_s_cfg(video); |
| } |
| |
| static void stf_capture_stop(struct stfcamss_video *video) |
| { |
| struct stf_capture *cap = to_stf_capture(video); |
| |
| if (cap->type == STF_CAPTURE_RAW) |
| stf_wr_irq_disable(video); |
| |
| stf_cap_s_cleanup(video); |
| } |
| |
| static void stf_capture_init(struct stfcamss *stfcamss, struct stf_capture *cap) |
| { |
| cap->buffers.state = STF_OUTPUT_OFF; |
| cap->buffers.buf[0] = NULL; |
| cap->buffers.buf[1] = NULL; |
| cap->buffers.active_buf = 0; |
| atomic_set(&cap->buffers.frame_skip, 4); |
| INIT_LIST_HEAD(&cap->buffers.pending_bufs); |
| INIT_LIST_HEAD(&cap->buffers.ready_bufs); |
| spin_lock_init(&cap->buffers.lock); |
| |
| cap->video.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; |
| cap->video.stfcamss = stfcamss; |
| cap->video.bpl_alignment = 16 * 8; |
| |
| if (cap->type == STF_CAPTURE_RAW) { |
| cap->video.formats = stf_wr_fmts; |
| cap->video.nformats = ARRAY_SIZE(stf_wr_fmts); |
| cap->video.bpl_alignment = 8; |
| } else if (cap->type == STF_CAPTURE_YUV) { |
| cap->video.formats = stf_isp_fmts; |
| cap->video.nformats = ARRAY_SIZE(stf_isp_fmts); |
| cap->video.bpl_alignment = 1; |
| } |
| } |
| |
| static void stf_buf_add_ready(struct stf_v_buf *output, |
| struct stfcamss_buffer *buffer) |
| { |
| INIT_LIST_HEAD(&buffer->queue); |
| list_add_tail(&buffer->queue, &output->ready_bufs); |
| } |
| |
| static struct stfcamss_buffer *stf_buf_get_ready(struct stf_v_buf *output) |
| { |
| struct stfcamss_buffer *buffer = NULL; |
| |
| if (!list_empty(&output->ready_bufs)) { |
| buffer = list_first_entry(&output->ready_bufs, |
| struct stfcamss_buffer, |
| queue); |
| list_del(&buffer->queue); |
| } |
| |
| return buffer; |
| } |
| |
| static void stf_buf_add_pending(struct stf_v_buf *output, |
| struct stfcamss_buffer *buffer) |
| { |
| INIT_LIST_HEAD(&buffer->queue); |
| list_add_tail(&buffer->queue, &output->pending_bufs); |
| } |
| |
| static void stf_buf_update_on_last(struct stf_v_buf *output) |
| { |
| switch (output->state) { |
| case STF_OUTPUT_CONTINUOUS: |
| output->state = STF_OUTPUT_SINGLE; |
| output->active_buf = !output->active_buf; |
| break; |
| case STF_OUTPUT_SINGLE: |
| output->state = STF_OUTPUT_STOPPING; |
| break; |
| default: |
| break; |
| } |
| } |
| |
| static void stf_buf_update_on_next(struct stf_v_buf *output) |
| { |
| switch (output->state) { |
| case STF_OUTPUT_CONTINUOUS: |
| output->active_buf = !output->active_buf; |
| break; |
| case STF_OUTPUT_SINGLE: |
| default: |
| break; |
| } |
| } |
| |
| static void stf_buf_update_on_new(struct stfcamss_video *video, |
| struct stfcamss_buffer *new_buf) |
| { |
| struct stf_capture *cap = to_stf_capture(video); |
| struct stf_v_buf *output = &cap->buffers; |
| |
| switch (output->state) { |
| case STF_OUTPUT_SINGLE: |
| stf_buf_add_pending(output, new_buf); |
| break; |
| case STF_OUTPUT_IDLE: |
| if (!output->buf[0]) { |
| output->buf[0] = new_buf; |
| stf_init_addrs(video); |
| output->state = STF_OUTPUT_SINGLE; |
| } else { |
| stf_buf_add_pending(output, new_buf); |
| } |
| break; |
| case STF_OUTPUT_STOPPING: |
| if (output->last_buffer) { |
| output->buf[output->active_buf] = output->last_buffer; |
| output->last_buffer = NULL; |
| } |
| |
| output->state = STF_OUTPUT_SINGLE; |
| stf_buf_add_pending(output, new_buf); |
| break; |
| case STF_OUTPUT_CONTINUOUS: |
| default: |
| stf_buf_add_pending(output, new_buf); |
| break; |
| } |
| } |
| |
| static void stf_buf_flush(struct stf_v_buf *output, enum vb2_buffer_state state) |
| { |
| struct stfcamss_buffer *buf; |
| struct stfcamss_buffer *t; |
| |
| list_for_each_entry_safe(buf, t, &output->pending_bufs, queue) { |
| vb2_buffer_done(&buf->vb.vb2_buf, state); |
| list_del(&buf->queue); |
| } |
| list_for_each_entry_safe(buf, t, &output->ready_bufs, queue) { |
| vb2_buffer_done(&buf->vb.vb2_buf, state); |
| list_del(&buf->queue); |
| } |
| } |
| |
| static void stf_buf_done(struct stf_v_buf *output) |
| { |
| struct stfcamss_buffer *ready_buf; |
| u64 ts = ktime_get_ns(); |
| unsigned long flags; |
| |
| if (output->state == STF_OUTPUT_OFF || |
| output->state == STF_OUTPUT_RESERVED) |
| return; |
| |
| spin_lock_irqsave(&output->lock, flags); |
| |
| while ((ready_buf = stf_buf_get_ready(output))) { |
| ready_buf->vb.vb2_buf.timestamp = ts; |
| ready_buf->vb.sequence = output->sequence++; |
| |
| vb2_buffer_done(&ready_buf->vb.vb2_buf, VB2_BUF_STATE_DONE); |
| } |
| |
| spin_unlock_irqrestore(&output->lock, flags); |
| } |
| |
| static void stf_change_buffer(struct stf_v_buf *output) |
| { |
| struct stf_capture *cap = container_of(output, struct stf_capture, |
| buffers); |
| struct stfcamss *stfcamss = cap->video.stfcamss; |
| struct stfcamss_buffer *ready_buf; |
| dma_addr_t *new_addr; |
| unsigned long flags; |
| u32 active_index; |
| |
| if (output->state == STF_OUTPUT_OFF || |
| output->state == STF_OUTPUT_STOPPING || |
| output->state == STF_OUTPUT_RESERVED || |
| output->state == STF_OUTPUT_IDLE) |
| return; |
| |
| spin_lock_irqsave(&output->lock, flags); |
| |
| active_index = output->active_buf; |
| |
| ready_buf = output->buf[active_index]; |
| if (!ready_buf) { |
| dev_dbg(stfcamss->dev, "missing ready buf %d %d.\n", |
| active_index, output->state); |
| active_index = !active_index; |
| ready_buf = output->buf[active_index]; |
| if (!ready_buf) { |
| dev_dbg(stfcamss->dev, |
| "missing ready buf2 %d %d.\n", |
| active_index, output->state); |
| goto out_unlock; |
| } |
| } |
| |
| /* Get next buffer */ |
| output->buf[active_index] = stf_buf_get_pending(output); |
| if (!output->buf[active_index]) { |
| new_addr = ready_buf->addr; |
| stf_buf_update_on_last(output); |
| } else { |
| new_addr = output->buf[active_index]->addr; |
| stf_buf_update_on_next(output); |
| } |
| |
| if (output->state == STF_OUTPUT_STOPPING) { |
| output->last_buffer = ready_buf; |
| } else { |
| if (cap->type == STF_CAPTURE_RAW) |
| stf_set_raw_addr(stfcamss, new_addr[0]); |
| else if (cap->type == STF_CAPTURE_YUV) |
| stf_set_yuv_addr(stfcamss, new_addr[0], new_addr[1]); |
| |
| stf_buf_add_ready(output, ready_buf); |
| } |
| |
| out_unlock: |
| spin_unlock_irqrestore(&output->lock, flags); |
| } |
| |
| irqreturn_t stf_wr_irq_handler(int irq, void *priv) |
| { |
| struct stfcamss *stfcamss = priv; |
| struct stf_capture *cap = &stfcamss->captures[STF_CAPTURE_RAW]; |
| |
| if (atomic_dec_if_positive(&cap->buffers.frame_skip) < 0) { |
| stf_change_buffer(&cap->buffers); |
| stf_buf_done(&cap->buffers); |
| } |
| |
| stf_syscon_reg_set_bit(stfcamss, VIN_INRT_PIX_CFG, U0_VIN_INTR_CLEAN); |
| stf_syscon_reg_clear_bit(stfcamss, VIN_INRT_PIX_CFG, U0_VIN_INTR_CLEAN); |
| |
| return IRQ_HANDLED; |
| } |
| |
| irqreturn_t stf_isp_irq_handler(int irq, void *priv) |
| { |
| struct stfcamss *stfcamss = priv; |
| struct stf_capture *cap = &stfcamss->captures[STF_CAPTURE_YUV]; |
| u32 status; |
| |
| status = stf_isp_reg_read(stfcamss, ISP_REG_ISP_CTRL_0); |
| if (status & ISPC_ISP) { |
| if (status & ISPC_ENUO) |
| stf_buf_done(&cap->buffers); |
| |
| stf_isp_reg_write(stfcamss, ISP_REG_ISP_CTRL_0, |
| (status & ~ISPC_INT_ALL_MASK) | |
| ISPC_ISP | ISPC_CSI | ISPC_SC); |
| } |
| |
| return IRQ_HANDLED; |
| } |
| |
| irqreturn_t stf_line_irq_handler(int irq, void *priv) |
| { |
| struct stfcamss *stfcamss = priv; |
| struct stf_capture *cap = &stfcamss->captures[STF_CAPTURE_YUV]; |
| u32 status; |
| |
| status = stf_isp_reg_read(stfcamss, ISP_REG_ISP_CTRL_0); |
| if (status & ISPC_LINE) { |
| if (atomic_dec_if_positive(&cap->buffers.frame_skip) < 0) { |
| if ((status & ISPC_ENUO)) |
| stf_change_buffer(&cap->buffers); |
| } |
| |
| stf_isp_reg_set_bit(stfcamss, ISP_REG_CSIINTS, |
| CSI_INTS_MASK, CSI_INTS(0x3)); |
| stf_isp_reg_set_bit(stfcamss, ISP_REG_IESHD, |
| SHAD_UP_M | SHAD_UP_EN, 0x3); |
| |
| stf_isp_reg_write(stfcamss, ISP_REG_ISP_CTRL_0, |
| (status & ~ISPC_INT_ALL_MASK) | ISPC_LINE); |
| } |
| |
| return IRQ_HANDLED; |
| } |
| |
| static int stf_queue_buffer(struct stfcamss_video *video, |
| struct stfcamss_buffer *buf) |
| { |
| struct stf_capture *cap = to_stf_capture(video); |
| struct stf_v_buf *v_bufs = &cap->buffers; |
| unsigned long flags; |
| |
| spin_lock_irqsave(&v_bufs->lock, flags); |
| stf_buf_update_on_new(video, buf); |
| spin_unlock_irqrestore(&v_bufs->lock, flags); |
| |
| return 0; |
| } |
| |
| static int stf_flush_buffers(struct stfcamss_video *video, |
| enum vb2_buffer_state state) |
| { |
| struct stf_capture *cap = to_stf_capture(video); |
| struct stf_v_buf *v_bufs = &cap->buffers; |
| unsigned long flags; |
| unsigned int i; |
| |
| spin_lock_irqsave(&v_bufs->lock, flags); |
| |
| stf_buf_flush(v_bufs, state); |
| |
| for (i = 0; i < ARRAY_SIZE(v_bufs->buf); i++) { |
| if (v_bufs->buf[i]) |
| vb2_buffer_done(&v_bufs->buf[i]->vb.vb2_buf, state); |
| |
| v_bufs->buf[i] = NULL; |
| } |
| |
| if (v_bufs->last_buffer) { |
| vb2_buffer_done(&v_bufs->last_buffer->vb.vb2_buf, state); |
| v_bufs->last_buffer = NULL; |
| } |
| |
| spin_unlock_irqrestore(&v_bufs->lock, flags); |
| return 0; |
| } |
| |
| static const struct stfcamss_video_ops stf_capture_ops = { |
| .queue_buffer = stf_queue_buffer, |
| .flush_buffers = stf_flush_buffers, |
| .start_streaming = stf_capture_start, |
| .stop_streaming = stf_capture_stop, |
| }; |
| |
| static void stf_capture_unregister_one(struct stf_capture *cap) |
| { |
| if (!video_is_registered(&cap->video.vdev)) |
| return; |
| |
| media_entity_cleanup(&cap->video.vdev.entity); |
| vb2_video_unregister_device(&cap->video.vdev); |
| } |
| |
| void stf_capture_unregister(struct stfcamss *stfcamss) |
| { |
| struct stf_capture *cap_raw = &stfcamss->captures[STF_CAPTURE_RAW]; |
| struct stf_capture *cap_yuv = &stfcamss->captures[STF_CAPTURE_YUV]; |
| |
| stf_capture_unregister_one(cap_raw); |
| stf_capture_unregister_one(cap_yuv); |
| } |
| |
| int stf_capture_register(struct stfcamss *stfcamss, |
| struct v4l2_device *v4l2_dev) |
| { |
| unsigned int i; |
| int ret; |
| |
| for (i = 0; i < ARRAY_SIZE(stfcamss->captures); i++) { |
| struct stf_capture *capture = &stfcamss->captures[i]; |
| |
| capture->type = i; |
| capture->video.ops = &stf_capture_ops; |
| stf_capture_init(stfcamss, capture); |
| |
| ret = stf_video_register(&capture->video, v4l2_dev, |
| stf_cap_names[i]); |
| if (ret < 0) { |
| dev_err(stfcamss->dev, |
| "Failed to register video node: %d\n", ret); |
| stf_capture_unregister(stfcamss); |
| return ret; |
| } |
| } |
| |
| return 0; |
| } |