From 272ee62d6410319ab4d73997de32776cc3e274cb Mon Sep 17 00:00:00 2001 From: Naushir Patuck Date: Thu, 16 Apr 2020 11:35:41 +0100 Subject: [PATCH] media: bcm2835-unicam: Add embedded data node. This patch adds a new node in the bcm2835-unicam driver to support CSI-2 embedded data streams. The subdevice is queried to see if embedded data is available from the sensor. Signed-off-by: Naushir Patuck --- .../media/platform/bcm2835/bcm2835-unicam.c | 667 +++++++++++++----- 1 file changed, 474 insertions(+), 193 deletions(-) --- a/drivers/media/platform/bcm2835/bcm2835-unicam.c +++ b/drivers/media/platform/bcm2835/bcm2835-unicam.c @@ -109,8 +109,15 @@ MODULE_PARM_DESC(debug, "Debug level 0-3 /* Define a nominal minimum image size */ #define MIN_WIDTH 16 #define MIN_HEIGHT 16 -/* Maximum number of simulataneous streams Uncaim can handle. */ -#define MAX_NODES 2 +/* Default size of the embedded buffer */ +#define UNICAM_EMBEDDED_SIZE 8192 + +enum pad_types { + IMAGE_PAD, + METADATA_PAD, + MAX_NODES +}; + /* * struct unicam_fmt - Unicam media bus format information * @pixelformat: V4L2 pixel format FCC identifier. 0 if n/a. @@ -327,6 +334,12 @@ static const struct unicam_fmt formats[] .depth = 12, .csi_dt = 0x2c, }, + /* Embedded data format */ + { + .fourcc = V4L2_META_FMT_SENSOR_DATA, + .code = MEDIA_BUS_FMT_SENSOR_DATA, + .depth = 8, + } }; struct unicam_dmaqueue { @@ -348,7 +361,9 @@ struct unicam_cfg { #define MAX_POSSIBLE_PIX_FMTS (ARRAY_SIZE(formats)) struct unicam_node { - bool registered; + int registered; + int open; + int streaming; unsigned int pad_id; /* Pointer pointing to current v4l2_buffer */ struct unicam_buffer *cur_frm; @@ -374,6 +389,7 @@ struct unicam_node { struct unicam_device *dev; struct media_pad pad; struct v4l2_ctrl_handler ctrl_handler; + unsigned int embedded_lines; }; struct unicam_device { @@ -401,8 +417,6 @@ struct unicam_device { struct v4l2_subdev *sensor; /* Pad config for the sensor */ struct v4l2_subdev_pad_config *sensor_config; - /* current input at the sub device */ - int current_input; unsigned int virtual_channel; enum v4l2_mbus_type bus_type; @@ -413,10 +427,7 @@ struct unicam_device { unsigned int bus_flags; unsigned int max_data_lanes; unsigned int active_data_lanes; - - struct v4l2_rect crop; - /* Flag to denote that we are processing buffers */ - int streaming; + bool sensor_embedded_data; struct unicam_node node[MAX_NODES]; }; @@ -488,6 +499,7 @@ static int check_mbus_format(struct unic for (i = 0; !ret && i < MAX_ENUM_MBUS_CODE; i++) { memset(&mbus_code, 0, sizeof(mbus_code)); mbus_code.index = i; + mbus_code.pad = IMAGE_PAD; mbus_code.which = V4L2_SUBDEV_FORMAT_ACTIVE; ret = v4l2_subdev_call(dev->sensor, pad, enum_mbus_code, @@ -552,10 +564,11 @@ static int __subdev_get_format(struct un } static int __subdev_set_format(struct unicam_device *dev, - struct v4l2_mbus_framefmt *fmt) + struct v4l2_mbus_framefmt *fmt, int pad_id) { struct v4l2_subdev_format sd_fmt = { .which = V4L2_SUBDEV_FORMAT_ACTIVE, + .pad = pad_id }; int ret; @@ -566,8 +579,12 @@ static int __subdev_set_format(struct un if (ret < 0) return ret; - unicam_dbg(1, dev, "%s %dx%d code:%04x\n", __func__, - fmt->width, fmt->height, fmt->code); + if (pad_id == IMAGE_PAD) + unicam_dbg(1, dev, "%s %dx%d code:%04x\n", __func__, fmt->width, + fmt->height, fmt->code); + else + unicam_dbg(1, dev, "%s Embedded data code:%04x\n", __func__, + sd_fmt.format.code); return 0; } @@ -609,46 +626,70 @@ static int unicam_reset_format(struct un struct v4l2_mbus_framefmt mbus_fmt; int ret; - ret = __subdev_get_format(dev, &mbus_fmt, node->pad_id); - if (ret) { - unicam_err(dev, "Failed to get_format - ret %d\n", ret); - return ret; - } + if (dev->sensor_embedded_data || node->pad_id != METADATA_PAD) { + ret = __subdev_get_format(dev, &mbus_fmt, node->pad_id); + if (ret) { + unicam_err(dev, "Failed to get_format - ret %d\n", ret); + return ret; + } - if (mbus_fmt.code != dev->node[0].fmt->code) { - unicam_err(dev, "code mismatch - fmt->code %08x, mbus_fmt.code %08x\n", - dev->node[0].fmt->code, mbus_fmt.code); - return ret; + if (mbus_fmt.code != node->fmt->code) { + unicam_err(dev, "code mismatch - fmt->code %08x, mbus_fmt.code %08x\n", + node->fmt->code, mbus_fmt.code); + return ret; + } } - v4l2_fill_pix_format(&dev->node[0].v_fmt.fmt.pix, &mbus_fmt); - dev->node[0].v_fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; - - unicam_calc_format_size_bpl(dev, dev->node[0].fmt, &dev->node[0].v_fmt); - - dev->node[0].m_fmt = mbus_fmt; + if (node->pad_id == IMAGE_PAD) { + v4l2_fill_pix_format(&node->v_fmt.fmt.pix, &mbus_fmt); + node->v_fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + unicam_calc_format_size_bpl(dev, node->fmt, &node->v_fmt); + } else { + node->v_fmt.type = V4L2_BUF_TYPE_META_CAPTURE; + node->v_fmt.fmt.meta.dataformat = V4L2_META_FMT_SENSOR_DATA; + if (dev->sensor_embedded_data) { + node->v_fmt.fmt.meta.buffersize = + mbus_fmt.width * mbus_fmt.height; + node->embedded_lines = mbus_fmt.height; + } else { + node->v_fmt.fmt.meta.buffersize = UNICAM_EMBEDDED_SIZE; + node->embedded_lines = 1; + } + } + node->m_fmt = mbus_fmt; return 0; } -static void unicam_wr_dma_addr(struct unicam_device *dev, dma_addr_t dmaaddr) +static void unicam_wr_dma_addr(struct unicam_device *dev, dma_addr_t dmaaddr, + int pad_id) { + dma_addr_t endaddr; + /* * dmaaddr should be a 32-bit address with the top two bits set to 0x3 * to signify uncached access through the Videocore memory controller. */ BUG_ON((dmaaddr >> 30) != 0x3); - reg_write(&dev->cfg, UNICAM_IBSA0, dmaaddr); - reg_write(&dev->cfg, UNICAM_IBEA0, - dmaaddr + dev->node[0].v_fmt.fmt.pix.sizeimage); + if (pad_id == IMAGE_PAD) { + endaddr = dmaaddr + + dev->node[IMAGE_PAD].v_fmt.fmt.pix.sizeimage; + reg_write(&dev->cfg, UNICAM_IBSA0, dmaaddr); + reg_write(&dev->cfg, UNICAM_IBEA0, endaddr); + } else { + endaddr = dmaaddr + + dev->node[METADATA_PAD].v_fmt.fmt.meta.buffersize; + reg_write(&dev->cfg, UNICAM_DBSA0, dmaaddr); + reg_write(&dev->cfg, UNICAM_DBEA0, endaddr); + } } static inline unsigned int unicam_get_lines_done(struct unicam_device *dev) { dma_addr_t start_addr, cur_addr; - unsigned int stride = dev->node[0].v_fmt.fmt.pix.bytesperline; - struct unicam_buffer *frm = dev->node[0].cur_frm; + unsigned int stride = dev->node[IMAGE_PAD].v_fmt.fmt.pix.bytesperline; + struct unicam_buffer *frm = dev->node[IMAGE_PAD].cur_frm; if (!frm) return 0; @@ -658,27 +699,51 @@ static inline unsigned int unicam_get_li return (unsigned int)(cur_addr - start_addr) / stride; } -static inline void unicam_schedule_next_buffer(struct unicam_device *dev) +static inline void unicam_schedule_next_buffer(struct unicam_node *node) { - struct unicam_dmaqueue *dma_q = &dev->node[0].dma_queue; + struct unicam_device *dev = node->dev; + struct unicam_dmaqueue *dma_q = &node->dma_queue; struct unicam_buffer *buf; dma_addr_t addr; buf = list_entry(dma_q->active.next, struct unicam_buffer, list); - dev->node[0].next_frm = buf; + node->next_frm = buf; list_del(&buf->list); addr = vb2_dma_contig_plane_dma_addr(&buf->vb.vb2_buf, 0); - unicam_wr_dma_addr(dev, addr); + unicam_wr_dma_addr(dev, addr, node->pad_id); } -static inline void unicam_process_buffer_complete(struct unicam_device *dev) +static inline void unicam_process_buffer_complete(struct unicam_node *node, + unsigned int sequence) { - dev->node[0].cur_frm->vb.field = dev->node[0].m_fmt.field; - dev->node[0].cur_frm->vb.sequence = dev->sequence++; + node->cur_frm->vb.field = node->m_fmt.field; + node->cur_frm->vb.sequence = sequence; + + vb2_buffer_done(&node->cur_frm->vb.vb2_buf, VB2_BUF_STATE_DONE); + node->cur_frm = node->next_frm; +} - vb2_buffer_done(&dev->node[0].cur_frm->vb.vb2_buf, VB2_BUF_STATE_DONE); - dev->node[0].cur_frm = dev->node[0].next_frm; +static int unicam_num_nodes_streaming(struct unicam_device *dev) +{ + return dev->node[IMAGE_PAD].streaming + + dev->node[METADATA_PAD].streaming; +} + +static int unicam_all_nodes_streaming(struct unicam_device *dev) +{ + int ret; + + ret = dev->node[IMAGE_PAD].open && dev->node[IMAGE_PAD].streaming; + ret &= !dev->node[METADATA_PAD].open || + dev->node[METADATA_PAD].streaming; + return ret; +} + +static int unicam_all_nodes_disabled(struct unicam_device *dev) +{ + return !dev->node[IMAGE_PAD].streaming && + !dev->node[METADATA_PAD].streaming; } /* @@ -693,10 +758,12 @@ static irqreturn_t unicam_isr(int irq, v { struct unicam_device *unicam = (struct unicam_device *)dev; struct unicam_cfg *cfg = &unicam->cfg; - struct unicam_dmaqueue *dma_q = &unicam->node[0].dma_queue; unsigned int lines_done = unicam_get_lines_done(dev); unsigned int sequence = unicam->sequence; + int num_nodes_streaming = unicam_num_nodes_streaming(dev); int ista, sta; + u64 ts; + int i; /* * Don't service interrupts if not streaming. @@ -704,7 +771,7 @@ static irqreturn_t unicam_isr(int irq, v * peripheral without the kernel knowing (that * shouldn't happen, but causes issues if it does). */ - if (!unicam->streaming) + if (unicam_all_nodes_disabled(unicam)) return IRQ_HANDLED; sta = reg_read(cfg, UNICAM_STA); @@ -726,9 +793,12 @@ static irqreturn_t unicam_isr(int irq, v * Timestamp is to be when the first data byte was captured, * aka frame start. */ - if (unicam->node[0].cur_frm) - unicam->node[0].cur_frm->vb.vb2_buf.timestamp = - ktime_get_ns(); + ts = ktime_get_ns(); + for (i = 0; i < num_nodes_streaming; i++) { + if (unicam->node[i].cur_frm) + unicam->node[i].cur_frm->vb.vb2_buf.timestamp = + ts; + } } if (ista & UNICAM_FEI || sta & UNICAM_PI0) { /* @@ -736,9 +806,13 @@ static irqreturn_t unicam_isr(int irq, v * stop the peripheral. Overwrite the frame we've just * captured instead. */ - if (unicam->node[0].cur_frm && - unicam->node[0].cur_frm != unicam->node[0].next_frm) - unicam_process_buffer_complete(unicam); + for (i = 0; i < num_nodes_streaming; i++) { + if (unicam->node[i].cur_frm && + unicam->node[i].cur_frm != unicam->node[i].next_frm) + unicam_process_buffer_complete(&unicam->node[i], + sequence); + } + unicam->sequence++; } /* Cannot swap buffer at frame end, there may be a race condition @@ -746,11 +820,13 @@ static irqreturn_t unicam_isr(int irq, v * already started. */ if (ista & (UNICAM_FSI | UNICAM_LCI) && !(ista & UNICAM_FEI)) { - spin_lock(&unicam->node[0].dma_queue_lock); - if (!list_empty(&dma_q->active) && - unicam->node[0].cur_frm == unicam->node[0].next_frm) - unicam_schedule_next_buffer(unicam); - spin_unlock(&unicam->node[0].dma_queue_lock); + for (i = 0; i < num_nodes_streaming; i++) { + spin_lock(&unicam->node[i].dma_queue_lock); + if (!list_empty(&unicam->node[i].dma_queue.active) && + unicam->node[i].cur_frm == unicam->node[i].next_frm) + unicam_schedule_next_buffer(&unicam->node[i]); + spin_unlock(&unicam->node[i].dma_queue_lock); + } } if (reg_read(&unicam->cfg, UNICAM_ICTL) & UNICAM_FCM) { @@ -773,6 +849,15 @@ static int unicam_querycap(struct file * snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s", dev->v4l2_dev.name); + cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING | + V4L2_CAP_READWRITE | V4L2_CAP_DEVICE_CAPS | + V4L2_CAP_META_CAPTURE; + + if (node->pad_id == IMAGE_PAD) + cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING; + else + cap->device_caps = V4L2_CAP_META_CAPTURE | V4L2_CAP_STREAMING; + return 0; } @@ -787,9 +872,14 @@ static int unicam_enum_fmt_vid_cap(struc int ret = 0; int i; + if (node->pad_id == METADATA_PAD) + return -EINVAL; + for (i = 0; !ret && i < MAX_ENUM_MBUS_CODE; i++) { memset(&mbus_code, 0, sizeof(mbus_code)); mbus_code.index = i; + mbus_code.pad = IMAGE_PAD; + mbus_code.which = V4L2_SUBDEV_FORMAT_ACTIVE; ret = v4l2_subdev_call(dev->sensor, pad, enum_mbus_code, NULL, &mbus_code); @@ -827,6 +917,9 @@ static int unicam_g_fmt_vid_cap(struct f { struct unicam_node *node = video_drvdata(file); + if (node->pad_id == METADATA_PAD) + return -EINVAL; + *f = node->v_fmt; return 0; @@ -843,6 +936,9 @@ const struct unicam_fmt *get_first_suppo for (j = 0; ret != -EINVAL && ret != -ENOIOCTLCMD; ++j) { memset(&mbus_code, 0, sizeof(mbus_code)); mbus_code.index = j; + mbus_code.pad = IMAGE_PAD; + mbus_code.which = V4L2_SUBDEV_FORMAT_ACTIVE; + ret = v4l2_subdev_call(dev->sensor, pad, enum_mbus_code, NULL, &mbus_code); if (ret < 0) { @@ -873,12 +969,15 @@ static int unicam_try_fmt_vid_cap(struct struct unicam_device *dev = node->dev; struct v4l2_subdev_format sd_fmt = { .which = V4L2_SUBDEV_FORMAT_TRY, - .pad = 0 + .pad = IMAGE_PAD }; struct v4l2_mbus_framefmt *mbus_fmt = &sd_fmt.format; const struct unicam_fmt *fmt; int ret; + if (node->pad_id == METADATA_PAD) + return -EINVAL; + fmt = find_format_by_pix(dev, f->fmt.pix.pixelformat); if (!fmt) { /* Pixel format not supported by unicam. Choose the first @@ -983,7 +1082,7 @@ static int unicam_s_fmt_vid_cap(struct f v4l2_fill_mbus_format(&mbus_fmt, &f->fmt.pix, fmt->code); - ret = __subdev_set_format(dev, &mbus_fmt); + ret = __subdev_set_format(dev, &mbus_fmt, node->pad_id); if (ret) { unicam_dbg(3, dev, "%s __subdev_set_format failed %d\n", __func__, ret); @@ -1014,6 +1113,106 @@ static int unicam_s_fmt_vid_cap(struct f return 0; } +static int unicam_enum_fmt_meta_cap(struct file *file, void *priv, + struct v4l2_fmtdesc *f) +{ + struct unicam_node *node = video_drvdata(file); + struct unicam_device *dev = node->dev; + struct v4l2_subdev_mbus_code_enum mbus_code; + const struct unicam_fmt *fmt = NULL; + int ret = 0; + + if (node->pad_id != METADATA_PAD || f->index != 0) + return -EINVAL; + + if (dev->sensor_embedded_data) { + memset(&mbus_code, 0, sizeof(mbus_code)); + mbus_code.index = f->index; + mbus_code.which = V4L2_SUBDEV_FORMAT_ACTIVE; + mbus_code.pad = METADATA_PAD; + + ret = v4l2_subdev_call(dev->sensor, pad, enum_mbus_code, NULL, + &mbus_code); + if (ret < 0) { + unicam_dbg(2, dev, + "subdev->enum_mbus_code idx 0 returned %d - index invalid\n", + ret); + return -EINVAL; + } + } else { + mbus_code.code = MEDIA_BUS_FMT_SENSOR_DATA; + } + + fmt = find_format_by_code(mbus_code.code); + if (fmt) + f->pixelformat = fmt->fourcc; + + return 0; +} + +static int unicam_g_fmt_meta_cap(struct file *file, void *priv, + struct v4l2_format *f) +{ + struct unicam_node *node = video_drvdata(file); + + if (node->pad_id != METADATA_PAD) + return -EINVAL; + + *f = node->v_fmt; + + return 0; +} + +static int unicam_try_fmt_meta_cap(struct file *file, void *priv, + struct v4l2_format *f) +{ + struct unicam_node *node = video_drvdata(file); + + if (node->pad_id != METADATA_PAD) + return -EINVAL; + + *f = node->v_fmt; + + return 0; +} + +static int unicam_s_fmt_meta_cap(struct file *file, void *priv, + struct v4l2_format *f) +{ + struct unicam_node *node = video_drvdata(file); + struct unicam_device *dev = node->dev; + struct v4l2_mbus_framefmt mbus_fmt = { 0 }; + const struct unicam_fmt *fmt; + int ret; + + if (node->pad_id == IMAGE_PAD) + return -EINVAL; + + if (dev->sensor_embedded_data) { + fmt = find_format_by_pix(dev, f->fmt.meta.dataformat); + if (!fmt) { + unicam_err(dev, "unknown format: V4L2 pix 0x%08x\n", + f->fmt.meta.dataformat); + return -EINVAL; + } + mbus_fmt.code = fmt->code; + ret = __subdev_set_format(dev, &mbus_fmt, node->pad_id); + if (ret) { + unicam_dbg(3, dev, "%s __subdev_set_format failed %d\n", + __func__, ret); + return ret; + } + } + + *f = node->v_fmt; + + unicam_dbg(3, dev, "%s size %d, V4L2 pix 0x%08x\n", + __func__, node->v_fmt.fmt.meta.buffersize, + node->v_fmt.fmt.meta.dataformat); + + return 0; +} + static int unicam_queue_setup(struct vb2_queue *vq, unsigned int *nbuffers, unsigned int *nplanes, @@ -1022,7 +1221,9 @@ static int unicam_queue_setup(struct vb2 { struct unicam_node *node = vb2_get_drv_priv(vq); struct unicam_device *dev = node->dev; - unsigned int size = node->v_fmt.fmt.pix.sizeimage; + unsigned int size = node->pad_id == IMAGE_PAD ? + node->v_fmt.fmt.pix.sizeimage : + node->v_fmt.fmt.meta.buffersize; if (vq->num_buffers + *nbuffers < 3) *nbuffers = 3 - vq->num_buffers; @@ -1053,7 +1254,8 @@ static int unicam_buffer_prepare(struct if (WARN_ON(!node->fmt)) return -EINVAL; - size = node->v_fmt.fmt.pix.sizeimage; + size = node->pad_id == IMAGE_PAD ? node->v_fmt.fmt.pix.sizeimage : + node->v_fmt.fmt.meta.buffersize; if (vb2_plane_size(vb, 0) < size) { unicam_err(dev, "data will not fit into plane (%lu < %lu)\n", vb2_plane_size(vb, 0), size); @@ -1082,12 +1284,12 @@ static void unicam_set_packing_config(st int pack, unpack; u32 val; - if (dev->node[0].v_fmt.fmt.pix.pixelformat == - dev->node[0].fmt->fourcc) { + if (dev->node[IMAGE_PAD].v_fmt.fmt.pix.pixelformat == + dev->node[IMAGE_PAD].fmt->fourcc) { unpack = UNICAM_PUM_NONE; pack = UNICAM_PPM_NONE; } else { - switch (dev->node[0].fmt->depth) { + switch (dev->node[IMAGE_PAD].fmt->depth) { case 8: unpack = UNICAM_PUM_UNPACK8; break; @@ -1125,17 +1327,31 @@ static void unicam_cfg_image_id(struct u if (dev->bus_type == V4L2_MBUS_CSI2_DPHY) { /* CSI2 mode */ reg_write(cfg, UNICAM_IDI0, - (dev->virtual_channel << 6) | dev->node[0].fmt->csi_dt); + (dev->virtual_channel << 6) | + dev->node[IMAGE_PAD].fmt->csi_dt); } else { /* CCP2 mode */ - reg_write(cfg, UNICAM_IDI0, (0x80 | dev->node[0].fmt->csi_dt)); + reg_write(cfg, UNICAM_IDI0, + 0x80 | dev->node[IMAGE_PAD].fmt->csi_dt); } } -static void unicam_start_rx(struct unicam_device *dev, unsigned long addr) +static void unicam_enable_ed(struct unicam_device *dev) +{ + struct unicam_cfg *cfg = &dev->cfg; + u32 val = reg_read(cfg, UNICAM_DCS); + + set_field(&val, 2, UNICAM_EDL_MASK); + /* Do not wrap at the end of the embedded data buffer */ + set_field(&val, 0, UNICAM_DBOB); + + reg_write(cfg, UNICAM_DCS, val); +} + +static void unicam_start_rx(struct unicam_device *dev, dma_addr_t *addr) { struct unicam_cfg *cfg = &dev->cfg; - int line_int_freq = dev->node[0].v_fmt.fmt.pix.height >> 2; + int line_int_freq = dev->node[IMAGE_PAD].v_fmt.fmt.pix.height >> 2; unsigned int i; u32 val; @@ -1284,27 +1500,31 @@ static void unicam_start_rx(struct unica } reg_write(&dev->cfg, UNICAM_IBLS, - dev->node[0].v_fmt.fmt.pix.bytesperline); - unicam_wr_dma_addr(dev, addr); + dev->node[IMAGE_PAD].v_fmt.fmt.pix.bytesperline); + unicam_wr_dma_addr(dev, addr[IMAGE_PAD], IMAGE_PAD); unicam_set_packing_config(dev); unicam_cfg_image_id(dev); - /* Disabled embedded data */ - val = 0; - set_field(&val, 0, UNICAM_EDL_MASK); - reg_write(cfg, UNICAM_DCS, val); - val = reg_read(cfg, UNICAM_MISC); set_field(&val, 1, UNICAM_FL0); set_field(&val, 1, UNICAM_FL1); reg_write(cfg, UNICAM_MISC, val); + if (dev->node[METADATA_PAD].streaming && dev->sensor_embedded_data) { + unicam_enable_ed(dev); + unicam_wr_dma_addr(dev, addr[METADATA_PAD], METADATA_PAD); + } + /* Enable peripheral */ reg_write_field(cfg, UNICAM_CTRL, 1, UNICAM_CPE); /* Load image pointers */ reg_write_field(cfg, UNICAM_ICTL, 1, UNICAM_LIP_MASK); + /* Load embedded data buffer pointers if needed */ + if (dev->node[METADATA_PAD].streaming && dev->sensor_embedded_data) + reg_write_field(cfg, UNICAM_DCS, 1, UNICAM_LDP); + /* * Enable trigger only for the first frame to * sync correctly to the FS from the source. @@ -1339,6 +1559,9 @@ static void unicam_disable(struct unicam /* Disable peripheral */ reg_write_field(cfg, UNICAM_CTRL, 0, UNICAM_CPE); + /* Clear ED setup */ + reg_write(cfg, UNICAM_DCS, 0); + /* Disable all lane clocks */ clk_write(cfg, 0); } @@ -1347,26 +1570,23 @@ static int unicam_start_streaming(struct { struct unicam_node *node = vb2_get_drv_priv(vq); struct unicam_device *dev = node->dev; - struct unicam_dmaqueue *dma_q = &node->dma_queue; - struct unicam_buffer *buf, *tmp; - unsigned long addr = 0; + struct unicam_buffer *buf; + dma_addr_t buffer_addr[MAX_NODES] = { 0 }; + int num_nodes_streaming; unsigned long flags; - int ret; + int ret, i; - spin_lock_irqsave(&node->dma_queue_lock, flags); - buf = list_entry(dma_q->active.next, struct unicam_buffer, list); - node->cur_frm = buf; - node->next_frm = buf; - list_del(&buf->list); - spin_unlock_irqrestore(&node->dma_queue_lock, flags); + node->streaming = 1; + if (!unicam_all_nodes_streaming(dev)) { + unicam_dbg(3, dev, "Not all nodes are streaming yet."); + return 0; + } - addr = vb2_dma_contig_plane_dma_addr(&node->cur_frm->vb.vb2_buf, 0); dev->sequence = 0; - ret = unicam_runtime_get(dev); if (ret < 0) { unicam_dbg(3, dev, "unicam_runtime_get failed\n"); - goto err_release_buffers; + return ret; } dev->active_data_lanes = dev->max_data_lanes; @@ -1388,7 +1608,7 @@ static int unicam_start_streaming(struct dev->active_data_lanes = dev->max_data_lanes; } if (dev->active_data_lanes > dev->max_data_lanes) { - unicam_err(dev, "Device has requested %u data lanes, which is >%u configured in DT\n", + unicam_err(dev, "Device has requested %u data lanes, which is >%u configured in DT\n", dev->active_data_lanes, dev->max_data_lanes); ret = -EINVAL; goto err_pm_put; @@ -1408,9 +1628,22 @@ static int unicam_start_streaming(struct unicam_err(dev, "Failed to enable CSI clock: %d\n", ret); goto err_pm_put; } - dev->streaming = 1; - unicam_start_rx(dev, addr); + num_nodes_streaming = unicam_num_nodes_streaming(dev); + for (i = 0; i < num_nodes_streaming; i++) { + spin_lock_irqsave(&dev->node[i].dma_queue_lock, flags); + buf = list_entry(dev->node[i].dma_queue.active.next, + struct unicam_buffer, list); + dev->node[i].cur_frm = buf; + dev->node[i].next_frm = buf; + list_del(&buf->list); + spin_unlock_irqrestore(&dev->node[i].dma_queue_lock, flags); + buffer_addr[i] = + vb2_dma_contig_plane_dma_addr(&dev->node[i].cur_frm->vb.vb2_buf, + 0); + } + + unicam_start_rx(dev, buffer_addr); ret = v4l2_subdev_call(dev->sensor, video, s_stream, 1); if (ret < 0) { @@ -1421,21 +1654,11 @@ static int unicam_start_streaming(struct return 0; err_disable_unicam: + node->streaming = 0; unicam_disable(dev); clk_disable_unprepare(dev->clock); err_pm_put: unicam_runtime_put(dev); -err_release_buffers: - list_for_each_entry_safe(buf, tmp, &dma_q->active, list) { - list_del(&buf->list); - vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_QUEUED); - } - if (node->cur_frm != node->next_frm) - vb2_buffer_done(&node->next_frm->vb.vb2_buf, - VB2_BUF_STATE_QUEUED); - vb2_buffer_done(&node->cur_frm->vb.vb2_buf, VB2_BUF_STATE_QUEUED); - node->next_frm = NULL; - node->cur_frm = NULL; return ret; } @@ -1448,33 +1671,47 @@ static void unicam_stop_streaming(struct struct unicam_buffer *buf, *tmp; unsigned long flags; - if (v4l2_subdev_call(dev->sensor, video, s_stream, 0) < 0) - unicam_err(dev, "stream off failed in subdev\n"); + node->streaming = 0; - unicam_disable(dev); + if (node->pad_id == IMAGE_PAD) { + /* Stop streaming the sensor and disable the peripheral. + * We cannot continue streaming embedded data with the + * image pad disabled. + */ + if (v4l2_subdev_call(dev->sensor, video, s_stream, 0) < 0) + unicam_err(dev, "stream off failed in subdev\n"); - /* Release all active buffers */ + unicam_disable(dev); + clk_disable_unprepare(dev->clock); + unicam_runtime_put(dev); + + } else if (node->pad_id == METADATA_PAD) { + /* Null out the embedded data buffer address so the HW does + * not use it. This is only really needed if the embedded data + * pad is disabled before the image pad. The 0x3 in the top two + * bits signifies uncached accesses through the Videocore + * memory controller. + */ + unicam_wr_dma_addr(dev, 0xc0000000, METADATA_PAD); + } + + /* Clear all queued buffers for the node */ spin_lock_irqsave(&node->dma_queue_lock, flags); list_for_each_entry_safe(buf, tmp, &dma_q->active, list) { list_del(&buf->list); vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR); } - if (node->cur_frm == node->next_frm) { - vb2_buffer_done(&node->cur_frm->vb.vb2_buf, - VB2_BUF_STATE_ERROR); - } else { + if (node->cur_frm) vb2_buffer_done(&node->cur_frm->vb.vb2_buf, VB2_BUF_STATE_ERROR); + if (node->next_frm && node->cur_frm != node->next_frm) vb2_buffer_done(&node->next_frm->vb.vb2_buf, VB2_BUF_STATE_ERROR); - } + node->cur_frm = NULL; node->next_frm = NULL; spin_unlock_irqrestore(&node->dma_queue_lock, flags); - - clk_disable_unprepare(dev->clock); - unicam_runtime_put(dev); } static int unicam_enum_input(struct file *file, void *priv, @@ -1595,17 +1832,23 @@ static int unicam_enum_framesizes(struct struct v4l2_subdev_frame_size_enum fse; int ret; - /* check for valid format */ - fmt = find_format_by_pix(dev, fsize->pixel_format); - if (!fmt) { - unicam_dbg(3, dev, "Invalid pixel code: %x\n", - fsize->pixel_format); - return -EINVAL; + if (node->pad_id == IMAGE_PAD) { + /* check for valid format */ + fmt = find_format_by_pix(dev, fsize->pixel_format); + if (!fmt) { + unicam_dbg(3, dev, "Invalid pixel code: %x\n", + fsize->pixel_format); + return -EINVAL; + } + fse.code = fmt->code; + } else { + /* This pad is for embedded data, so just set the format */ + fse.code = MEDIA_BUS_FMT_SENSOR_DATA; } + fse.which = V4L2_SUBDEV_FORMAT_ACTIVE; fse.index = fsize->index; - fse.pad = 0; - fse.code = fmt->code; + fse.pad = node->pad_id; ret = v4l2_subdev_call(dev->sensor, pad, enum_frame_size, NULL, &fse); if (ret) @@ -1782,7 +2025,7 @@ static void unicam_notify(struct v4l2_su switch (notification) { case V4L2_DEVICE_NOTIFY_EVENT: - v4l2_event_queue(&dev->node[0].video_dev, arg); + v4l2_event_queue(&dev->node[IMAGE_PAD].video_dev, arg); break; default: break; @@ -1826,6 +2069,7 @@ static int unicam_open(struct file *file goto unlock; } + node->open++; ret = 0; unlock: @@ -1850,6 +2094,10 @@ static int unicam_release(struct file *f if (fh_singular) v4l2_subdev_call(sd, core, s_power, 0); + if (node->streaming) + unicam_stop_streaming(&node->buffer_queue); + + node->open--; mutex_unlock(&node->lock); return ret; @@ -1874,6 +2122,11 @@ static const struct v4l2_ioctl_ops unica .vidioc_s_fmt_vid_cap = unicam_s_fmt_vid_cap, .vidioc_try_fmt_vid_cap = unicam_try_fmt_vid_cap, + .vidioc_enum_fmt_meta_cap = unicam_enum_fmt_meta_cap, + .vidioc_g_fmt_meta_cap = unicam_g_fmt_meta_cap, + .vidioc_s_fmt_meta_cap = unicam_s_fmt_meta_cap, + .vidioc_try_fmt_meta_cap = unicam_try_fmt_meta_cap, + .vidioc_enum_input = unicam_enum_input, .vidioc_g_input = unicam_g_input, .vidioc_s_input = unicam_s_input, @@ -1941,42 +2194,53 @@ static int register_node(struct unicam_d const struct unicam_fmt *fmt; int ret; - ret = __subdev_get_format(unicam, &mbus_fmt, pad_id); - if (ret) { - unicam_err(unicam, "Failed to get_format - ret %d\n", ret); - return ret; - } - - fmt = find_format_by_code(mbus_fmt.code); - if (!fmt) { - /* Find the first format that the sensor and unicam both - * support - */ - fmt = get_first_supported_format(unicam); + if (unicam->sensor_embedded_data || pad_id != METADATA_PAD) { + ret = __subdev_get_format(unicam, &mbus_fmt, pad_id); + if (ret) { + unicam_err(unicam, "Failed to get_format - ret %d\n", + ret); + return ret; + } - if (!fmt) - /* No compatible formats */ - return -EINVAL; + fmt = find_format_by_code(mbus_fmt.code); + if (!fmt) { + /* Find the first format that the sensor and unicam both + * support + */ + fmt = get_first_supported_format(unicam); - mbus_fmt.code = fmt->code; - ret = __subdev_set_format(unicam, &mbus_fmt); - if (ret) - return -EINVAL; - } - if (mbus_fmt.field != V4L2_FIELD_NONE) { - /* Interlaced not supported - disable it now. */ - mbus_fmt.field = V4L2_FIELD_NONE; - ret = __subdev_set_format(unicam, &mbus_fmt); - if (ret) - return -EINVAL; + if (!fmt) + /* No compatible formats */ + return -EINVAL; + + mbus_fmt.code = fmt->code; + ret = __subdev_set_format(unicam, &mbus_fmt, pad_id); + if (ret) + return -EINVAL; + } + if (mbus_fmt.field != V4L2_FIELD_NONE) { + /* Interlaced not supported - disable it now. */ + mbus_fmt.field = V4L2_FIELD_NONE; + ret = __subdev_set_format(unicam, &mbus_fmt, pad_id); + if (ret) + return -EINVAL; + } + } else { + /* Fix this node format as embedded data. */ + fmt = find_format_by_code(MEDIA_BUS_FMT_SENSOR_DATA); } + node->dev = unicam; node->pad_id = pad_id; node->fmt = fmt; - if (fmt->fourcc) - node->v_fmt.fmt.pix.pixelformat = fmt->fourcc; - else + if (fmt->fourcc) { + if (fmt->fourcc != V4L2_META_FMT_SENSOR_DATA) + node->v_fmt.fmt.pix.pixelformat = fmt->fourcc; + else + node->v_fmt.fmt.meta.dataformat = fmt->fourcc; + } else { node->v_fmt.fmt.pix.pixelformat = fmt->repacked_fourcc; + } /* Read current subdev format */ unicam_reset_format(node); @@ -2002,13 +2266,21 @@ static int register_node(struct unicam_d spin_lock_init(&node->dma_queue_lock); mutex_init(&node->lock); - if (type == V4L2_BUF_TYPE_VIDEO_CAPTURE) { + vdev = &node->video_dev; + if (pad_id == IMAGE_PAD) { /* Add controls from the subdevice */ ret = v4l2_ctrl_add_handler(&node->ctrl_handler, unicam->sensor->ctrl_handler, NULL, true); if (ret < 0) return ret; + + /* + * If the sensor subdevice has any controls, associate the node + * with the ctrl handler to allow access from userland. + */ + if (!list_empty(&node->ctrl_handler.ctrls)) + vdev->ctrl_handler = &node->ctrl_handler; } q = &node->buffer_queue; @@ -2031,8 +2303,6 @@ static int register_node(struct unicam_d INIT_LIST_HEAD(&node->dma_queue.active); - vdev = &node->video_dev; - strlcpy(vdev->name, UNICAM_MODULE_NAME, sizeof(vdev->name)); vdev->release = video_device_release_empty; vdev->fops = &unicam_fops; vdev->ioctl_ops = &unicam_ioctl_ops; @@ -2040,24 +2310,28 @@ static int register_node(struct unicam_d vdev->vfl_dir = VFL_DIR_RX; vdev->queue = q; vdev->lock = &node->lock; - vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING | - V4L2_CAP_READWRITE; - /* If the source has no controls then remove our ctrl handler. */ - if (list_empty(&node->ctrl_handler.ctrls)) - unicam->v4l2_dev.ctrl_handler = NULL; + vdev->device_caps = (pad_id == IMAGE_PAD) ? + (V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING) : + (V4L2_CAP_META_CAPTURE | V4L2_CAP_STREAMING); + + /* Define the device names */ + snprintf(vdev->name, sizeof(vdev->name), "%s-%s", UNICAM_MODULE_NAME, + node->pad_id == IMAGE_PAD ? "image" : "embedded"); - node->dev = unicam; video_set_drvdata(vdev, node); vdev->entity.flags |= MEDIA_ENT_FL_DEFAULT; - if (!v4l2_subdev_has_op(unicam->sensor, video, s_std)) { + if (node->pad_id == METADATA_PAD || + !v4l2_subdev_has_op(unicam->sensor, video, s_std)) { v4l2_disable_ioctl(&node->video_dev, VIDIOC_S_STD); v4l2_disable_ioctl(&node->video_dev, VIDIOC_G_STD); v4l2_disable_ioctl(&node->video_dev, VIDIOC_ENUMSTD); } - if (!v4l2_subdev_has_op(unicam->sensor, video, querystd)) + if (node->pad_id == METADATA_PAD || + !v4l2_subdev_has_op(unicam->sensor, video, querystd)) v4l2_disable_ioctl(&node->video_dev, VIDIOC_QUERYSTD); - if (!v4l2_subdev_has_op(unicam->sensor, video, s_dv_timings)) { + if (node->pad_id == METADATA_PAD || + !v4l2_subdev_has_op(unicam->sensor, video, s_dv_timings)) { v4l2_disable_ioctl(&node->video_dev, VIDIOC_S_EDID); v4l2_disable_ioctl(&node->video_dev, VIDIOC_G_EDID); v4l2_disable_ioctl(&node->video_dev, VIDIOC_DV_TIMINGS_CAP); @@ -2066,15 +2340,19 @@ static int register_node(struct unicam_d v4l2_disable_ioctl(&node->video_dev, VIDIOC_ENUM_DV_TIMINGS); v4l2_disable_ioctl(&node->video_dev, VIDIOC_QUERY_DV_TIMINGS); } - if (!v4l2_subdev_has_op(unicam->sensor, pad, enum_frame_interval)) + if (node->pad_id == METADATA_PAD || + !v4l2_subdev_has_op(unicam->sensor, pad, enum_frame_interval)) v4l2_disable_ioctl(&node->video_dev, VIDIOC_ENUM_FRAMEINTERVALS); - if (!v4l2_subdev_has_op(unicam->sensor, video, g_frame_interval)) + if (node->pad_id == METADATA_PAD || + !v4l2_subdev_has_op(unicam->sensor, video, g_frame_interval)) v4l2_disable_ioctl(&node->video_dev, VIDIOC_G_PARM); - if (!v4l2_subdev_has_op(unicam->sensor, video, s_frame_interval)) + if (node->pad_id == METADATA_PAD || + !v4l2_subdev_has_op(unicam->sensor, video, s_frame_interval)) v4l2_disable_ioctl(&node->video_dev, VIDIOC_S_PARM); - if (!v4l2_subdev_has_op(unicam->sensor, pad, enum_frame_size)) + if (node->pad_id == METADATA_PAD || + !v4l2_subdev_has_op(unicam->sensor, pad, enum_frame_size)) v4l2_disable_ioctl(&node->video_dev, VIDIOC_ENUM_FRAMESIZES); ret = video_register_device(vdev, VFL_TYPE_GRABBER, -1); @@ -2082,27 +2360,29 @@ static int register_node(struct unicam_d unicam_err(unicam, "Unable to register video device.\n"); return ret; } - node->registered = true; + node->registered = 1; - ret = media_create_pad_link(&unicam->sensor->entity, - 0, &node->video_dev.entity, 0, - MEDIA_LNK_FL_ENABLED | - MEDIA_LNK_FL_IMMUTABLE); - if (ret) - unicam_err(unicam, "Unable to create pad links.\n"); + if (unicam->sensor_embedded_data) { + ret = media_create_pad_link(&unicam->sensor->entity, pad_id, + &node->video_dev.entity, 0, + MEDIA_LNK_FL_ENABLED | + MEDIA_LNK_FL_IMMUTABLE); + if (ret) + unicam_err(unicam, "Unable to create pad links.\n"); + } return ret; } static void unregister_nodes(struct unicam_device *unicam) { - if (unicam->node[0].registered) { - video_unregister_device(&unicam->node[0].video_dev); - unicam->node[0].registered = false; - } - if (unicam->node[1].registered) { - video_unregister_device(&unicam->node[1].video_dev); - unicam->node[1].registered = false; + if (unicam->node[IMAGE_PAD].registered) { + video_unregister_device(&unicam->node[IMAGE_PAD].video_dev); + unicam->node[IMAGE_PAD].registered = 0; + } + if (unicam->node[METADATA_PAD].registered) { + video_unregister_device(&unicam->node[METADATA_PAD].video_dev); + unicam->node[METADATA_PAD].registered = 0; } } @@ -2118,20 +2398,20 @@ static int unicam_probe_complete(struct if (!unicam->sensor_config) return -ENOMEM; - ret = register_node(unicam, &unicam->node[0], - V4L2_BUF_TYPE_VIDEO_CAPTURE, 0); + unicam->sensor_embedded_data = (unicam->sensor->entity.num_pads >= 2); + + ret = register_node(unicam, &unicam->node[IMAGE_PAD], + V4L2_BUF_TYPE_VIDEO_CAPTURE, IMAGE_PAD); if (ret) { unicam_err(unicam, "Unable to register subdev node 0.\n"); goto unregister; } - if (unicam->sensor->entity.num_pads >= 2) { - ret = register_node(unicam, &unicam->node[1], - V4L2_BUF_TYPE_META_CAPTURE, 1); - if (ret) { - unicam_err(unicam, - "Unable to register subdev node 1.\n"); - goto unregister; - } + + ret = register_node(unicam, &unicam->node[METADATA_PAD], + V4L2_BUF_TYPE_META_CAPTURE, METADATA_PAD); + if (ret) { + unicam_err(unicam, "Unable to register subdev node 1.\n"); + goto unregister; } ret = v4l2_device_register_ro_subdev_nodes(&unicam->v4l2_dev); @@ -2355,8 +2635,10 @@ static int unicam_probe(struct platform_ pdev->dev.driver->name, dev_name(&pdev->dev)); unicam->mdev.hw_revision = 1; - media_entity_pads_init(&unicam->node[0].video_dev.entity, 1, - &unicam->node[0].pad); + media_entity_pads_init(&unicam->node[IMAGE_PAD].video_dev.entity, 1, + &unicam->node[IMAGE_PAD].pad); + media_entity_pads_init(&unicam->node[METADATA_PAD].video_dev.entity, 1, + &unicam->node[METADATA_PAD].pad); media_device_init(&unicam->mdev); unicam->v4l2_dev.mdev = &unicam->mdev; @@ -2376,11 +2658,10 @@ static int unicam_probe(struct platform_ } /* Reserve space for the controls */ - hdl = &unicam->node[0].ctrl_handler; + hdl = &unicam->node[IMAGE_PAD].ctrl_handler; ret = v4l2_ctrl_handler_init(hdl, 16); if (ret < 0) goto media_unregister; - unicam->v4l2_dev.ctrl_handler = hdl; /* set the driver data in platform device */ platform_set_drvdata(pdev, unicam); @@ -2417,7 +2698,7 @@ static int unicam_remove(struct platform pm_runtime_disable(&pdev->dev); v4l2_async_notifier_unregister(&unicam->notifier); - v4l2_ctrl_handler_free(&unicam->node[0].ctrl_handler); + v4l2_ctrl_handler_free(&unicam->node[IMAGE_PAD].ctrl_handler); v4l2_device_unregister(&unicam->v4l2_dev); unregister_nodes(unicam); if (unicam->sensor_config)