This is the camera interface driver that provides the v4l2
user interface. Frames can be received from various sources:
- directly from SMFC for capturing unconverted images directly from
camera sensors.
- from the IC pre-process encode task.
- from the IC pre-process viewfinder task.
- from the IC post-process task.
Signed-off-by: Steve Longerbeam <steve_longerb...@mentor.com>
---
drivers/staging/media/imx/Makefile | 2 +-
drivers/staging/media/imx/imx-camif.c | 1010 +++++++++++++++++++++++++++++++++
2 files changed, 1011 insertions(+), 1 deletion(-)
create mode 100644 drivers/staging/media/imx/imx-camif.c
diff --git a/drivers/staging/media/imx/Makefile
b/drivers/staging/media/imx/Makefile
index d2a962c..fe9e992 100644
--- a/drivers/staging/media/imx/Makefile
+++ b/drivers/staging/media/imx/Makefile
@@ -8,4 +8,4 @@ obj-$(CONFIG_VIDEO_IMX_MEDIA) += imx-ic.o
obj-$(CONFIG_VIDEO_IMX_CAMERA) += imx-csi.o
obj-$(CONFIG_VIDEO_IMX_CAMERA) += imx-smfc.o
-
+obj-$(CONFIG_VIDEO_IMX_CAMERA) += imx-camif.o
diff --git a/drivers/staging/media/imx/imx-camif.c
b/drivers/staging/media/imx/imx-camif.c
new file mode 100644
index 0000000..3cf167e
--- /dev/null
+++ b/drivers/staging/media/imx/imx-camif.c
@@ -0,0 +1,1010 @@
+/*
+ * Video Camera Capture Subdev for Freescale i.MX5/6 SOC
+ *
+ * Copyright (c) 2012-2016 Mentor Graphics Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+#include <linux/module.h>
+#include <linux/delay.h>
+#include <linux/fs.h>
+#include <linux/timer.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/platform_device.h>
+#include <linux/pinctrl/consumer.h>
+#include <linux/of_platform.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ioctl.h>
+#include <media/videobuf2-dma-contig.h>
+#include <media/v4l2-subdev.h>
+#include <media/v4l2-of.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-event.h>
+#include <video/imx-ipu-v3.h>
+#include <media/imx.h>
+#include "imx-media.h"
+
+#define DEVICE_NAME "imx-media-camif"
+
+#define CAMIF_NUM_PADS 2
+
+#define CAMIF_DQ_TIMEOUT 5000
+
+struct camif_priv;
+
+struct camif_priv {
+ struct device *dev;
+ struct video_device vfd;
+ struct media_pipeline mp;
+ struct imx_media_dev *md;
+ struct v4l2_subdev sd;
+ struct media_pad pad[CAMIF_NUM_PADS];
+ struct media_pad vd_pad;
+ int id;
+ int input_pad;
+ int output_pad;
+
+ struct v4l2_mbus_framefmt format_mbus[CAMIF_NUM_PADS];
+ const struct imx_media_pixfmt *cc[CAMIF_NUM_PADS];
+
+ /* dma buffer ring */
+ struct imx_media_dma_buf_ring *in_ring;
+ struct v4l2_subdev *src_sd;
+
+ struct mutex mutex; /* capture device mutex */
+ spinlock_t q_lock; /* protect ready_q */
+
+ /* buffer queue used in videobuf2 */
+ struct vb2_queue buffer_queue;
+
+ /* streaming buffer queue */
+ struct list_head ready_q;
+
+ /* controls inherited from subdevs */
+ struct v4l2_ctrl_handler ctrl_hdlr;
+
+ /* misc status */
+ int current_input; /* the current input */
+ v4l2_std_id current_std; /* current standard */
+ bool stop; /* streaming is stopping */
+};
+
+/* In bytes, per queue */
+#define VID_MEM_LIMIT SZ_64M
+
+static struct vb2_ops camif_qops;
+
+/*
+ * Video ioctls follow
+ */
+
+static int vidioc_querycap(struct file *file, void *fh,
+ struct v4l2_capability *cap)
+{
+ strncpy(cap->driver, DEVICE_NAME, sizeof(cap->driver) - 1);
+ strncpy(cap->card, DEVICE_NAME, sizeof(cap->card) - 1);
+ cap->bus_info[0] = 0;
+ cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
+ cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+
+ return 0;
+}
+
+static int camif_enum_fmt_vid_cap(struct file *file, void *fh,
+ struct v4l2_fmtdesc *f)
+{
+ const struct imx_media_pixfmt *cc;
+ u32 code;
+ int ret;
+
+ ret = imx_media_enum_format(&code, f->index, true, true);
+ if (ret)
+ return ret;
+ cc = imx_media_find_format(0, code, true, true);
+ if (!cc)
+ return -EINVAL;
+
+ f->pixelformat = cc->fourcc;
+
+ return 0;
+}
+
+static int camif_g_fmt_vid_cap(struct file *file, void *fh,
+ struct v4l2_format *f)
+{
+ struct camif_priv *priv = video_drvdata(file);
+ struct v4l2_mbus_framefmt *outfmt;
+
+ /* user format is the same as the format from output pad */
+ outfmt = &priv->format_mbus[priv->output_pad];
+ return imx_media_mbus_fmt_to_pix_fmt(&f->fmt.pix, outfmt);
+}
+
+static int camif_try_fmt_vid_cap(struct file *file, void *fh,
+ struct v4l2_format *f)
+{
+ return camif_g_fmt_vid_cap(file, fh, f);
+}
+
+static int camif_s_fmt_vid_cap(struct file *file, void *fh,
+ struct v4l2_format *f)
+{
+ struct camif_priv *priv = video_drvdata(file);
+
+ if (vb2_is_busy(&priv->buffer_queue)) {
+ v4l2_err(&priv->sd, "%s queue busy\n", __func__);
+ return -EBUSY;
+ }
+
+ return camif_try_fmt_vid_cap(file, priv, f);
+}
+
+static int camif_querystd(struct file *file, void *fh, v4l2_std_id *std)
+{
+ struct camif_priv *priv = video_drvdata(file);
+ struct imx_media_subdev *sensor;
+
+ sensor = imx_media_find_sensor(priv->md, &priv->sd.entity);
+ if (IS_ERR(sensor)) {
+ v4l2_err(&priv->sd, "no sensor attached\n");
+ return PTR_ERR(sensor);
+ }
+
+ return v4l2_subdev_call(sensor->sd, video, querystd, std);
+}
+
+static int camif_g_std(struct file *file, void *fh, v4l2_std_id *std)
+{
+ struct camif_priv *priv = video_drvdata(file);
+
+ *std = priv->current_std;
+ return 0;
+}
+
+static int camif_s_std(struct file *file, void *fh, v4l2_std_id std)
+{
+ struct camif_priv *priv = video_drvdata(file);
+ struct imx_media_subdev *sensor;
+ int ret;
+
+ sensor = imx_media_find_sensor(priv->md, &priv->sd.entity);
+ if (IS_ERR(sensor)) {
+ v4l2_err(&priv->sd, "no sensor attached\n");
+ return PTR_ERR(sensor);
+ }
+
+ if (vb2_is_busy(&priv->buffer_queue))
+ return -EBUSY;
+
+ ret = v4l2_subdev_call(sensor->sd, video, s_std, std);
+ if (ret < 0)
+ return ret;
+
+ priv->current_std = std;
+ return 0;
+}
+
+static int camif_enum_input(struct file *file, void *fh,
+ struct v4l2_input *input)
+{
+ struct camif_priv *priv = video_drvdata(file);
+ struct imx_media_subdev *sensor;
+ int index = input->index;
+
+ sensor = imx_media_find_sensor(priv->md, &priv->sd.entity);
+ if (IS_ERR(sensor)) {
+ v4l2_err(&priv->sd, "no sensor attached\n");
+ return PTR_ERR(sensor);
+ }
+
+ if (index >= sensor->input.num)
+ return -EINVAL;
+
+ input->type = V4L2_INPUT_TYPE_CAMERA;
+ strncpy(input->name, sensor->input.name[index], sizeof(input->name));
+
+ if (index == priv->current_input) {
+ v4l2_subdev_call(sensor->sd, video, g_input_status,
+ &input->status);
+ v4l2_subdev_call(sensor->sd, video, querystd, &input->std);
+ } else {
+ input->status = V4L2_IN_ST_NO_SIGNAL;
+ input->std = V4L2_STD_UNKNOWN;
+ }
+
+ return 0;
+}
+
+static int camif_g_input(struct file *file, void *fh, unsigned int *index)
+{
+ struct camif_priv *priv = video_drvdata(file);
+
+ *index = priv->current_input;
+ return 0;
+}
+
+static int camif_s_input(struct file *file, void *fh, unsigned int index)
+{
+ struct camif_priv *priv = video_drvdata(file);
+ struct imx_media_subdev *sensor;
+ int ret;
+
+ sensor = imx_media_find_sensor(priv->md, &priv->sd.entity);
+ if (IS_ERR(sensor)) {
+ v4l2_err(&priv->sd, "no sensor attached\n");
+ return PTR_ERR(sensor);
+ }
+
+ if (index >= sensor->input.num)
+ return -EINVAL;
+
+ if (index == priv->current_input)
+ return 0;
+
+ /* select the sensor's input */
+ ret = v4l2_subdev_call(sensor->sd, video, s_routing,
+ sensor->input.value[index], 0, 0);
+ if (!ret)
+ priv->current_input = index;
+
+ return ret;
+}
+
+static int camif_g_parm(struct file *file, void *fh,
+ struct v4l2_streamparm *a)
+{
+ struct camif_priv *priv = video_drvdata(file);
+ struct imx_media_subdev *sensor;
+
+ sensor = imx_media_find_sensor(priv->md, &priv->sd.entity);
+ if (IS_ERR(sensor)) {
+ v4l2_err(&priv->sd, "no sensor attached\n");
+ return PTR_ERR(sensor);
+ }
+
+ if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+ return -EINVAL;
+
+ return v4l2_subdev_call(sensor->sd, video, g_parm, a);
+}
+
+static int camif_s_parm(struct file *file, void *fh,
+ struct v4l2_streamparm *a)
+{
+ struct camif_priv *priv = video_drvdata(file);
+ struct imx_media_subdev *sensor;
+
+ sensor = imx_media_find_sensor(priv->md, &priv->sd.entity);
+ if (IS_ERR(sensor)) {
+ v4l2_err(&priv->sd, "no sensor attached\n");
+ return PTR_ERR(sensor);
+ }
+
+ if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+ return -EINVAL;
+
+ return v4l2_subdev_call(sensor->sd, video, s_parm, a);
+}
+
+static const struct v4l2_ioctl_ops camif_ioctl_ops = {
+ .vidioc_querycap = vidioc_querycap,
+
+ .vidioc_enum_fmt_vid_cap = camif_enum_fmt_vid_cap,
+ .vidioc_g_fmt_vid_cap = camif_g_fmt_vid_cap,
+ .vidioc_try_fmt_vid_cap = camif_try_fmt_vid_cap,
+ .vidioc_s_fmt_vid_cap = camif_s_fmt_vid_cap,
+
+ .vidioc_querystd = camif_querystd,
+ .vidioc_g_std = camif_g_std,
+ .vidioc_s_std = camif_s_std,
+
+ .vidioc_enum_input = camif_enum_input,
+ .vidioc_g_input = camif_g_input,
+ .vidioc_s_input = camif_s_input,
+
+ .vidioc_g_parm = camif_g_parm,
+ .vidioc_s_parm = camif_s_parm,
+
+ .vidioc_reqbufs = vb2_ioctl_reqbufs,
+ .vidioc_create_bufs = vb2_ioctl_create_bufs,
+ .vidioc_prepare_buf = vb2_ioctl_prepare_buf,
+ .vidioc_querybuf = vb2_ioctl_querybuf,
+ .vidioc_qbuf = vb2_ioctl_qbuf,
+ .vidioc_dqbuf = vb2_ioctl_dqbuf,
+ .vidioc_expbuf = vb2_ioctl_expbuf,
+ .vidioc_streamon = vb2_ioctl_streamon,
+ .vidioc_streamoff = vb2_ioctl_streamoff,
+};
+
+/*
+ * Queue operations
+ */
+
+static u32 camif_get_sizeimage(struct camif_priv *priv)
+{
+ struct v4l2_mbus_framefmt *outfmt;
+ const struct imx_media_pixfmt *outcc;
+
+ outfmt = &priv->format_mbus[priv->output_pad];
+ outcc = priv->cc[priv->output_pad];
+ return (outfmt->width * outfmt->height * outcc->bpp) >> 3;
+}
+
+static int camif_queue_setup(struct vb2_queue *vq,
+ unsigned int *nbuffers, unsigned int *nplanes,
+ unsigned int sizes[], struct device *alloc_devs[])
+{
+ struct camif_priv *priv = vb2_get_drv_priv(vq);
+ unsigned int count = *nbuffers;
+ u32 sizeimage;
+
+ if (!priv->src_sd)
+ return -EPIPE;
+
+ if (vq->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+ return -EINVAL;
+
+ sizeimage = camif_get_sizeimage(priv);
+
+ count = min_t(unsigned int, count, IMX_MEDIA_MAX_RING_BUFS);
+ count = max_t(unsigned int, count, IMX_MEDIA_MIN_RING_BUFS);
+
+ while (sizeimage * count > VID_MEM_LIMIT)
+ count--;
+
+ if (count < IMX_MEDIA_MIN_RING_BUFS)
+ return -EINVAL;
+
+ *nplanes = 1;
+ *nbuffers = count;
+ sizes[0] = sizeimage;
+
+ return 0;
+}
+
+static int camif_buf_init(struct vb2_buffer *vb)
+{
+ struct vb2_queue *vq = vb->vb2_queue;
+ struct camif_priv *priv = vb2_get_drv_priv(vq);
+ struct imx_media_buffer *buf = to_imx_media_vb(vb);
+
+ if (!priv->src_sd)
+ return -EPIPE;
+
+ INIT_LIST_HEAD(&buf->list);
+
+ return 0;
+}
+
+static int camif_buf_prepare(struct vb2_buffer *vb)
+{
+ struct vb2_queue *vq = vb->vb2_queue;
+ struct camif_priv *priv = vb2_get_drv_priv(vq);
+ u32 sizeimage = camif_get_sizeimage(priv);
+ int ret;
+
+ if (!priv->src_sd)
+ return -EPIPE;
+
+ if (vb2_plane_size(vb, 0) < sizeimage) {
+ v4l2_err(&priv->sd,
+ "data will not fit into plane (%lu < %lu)\n",
+ vb2_plane_size(vb, 0), (long)sizeimage);
+ return -EINVAL;
+ }
+
+ vb2_set_plane_payload(vb, 0, sizeimage);
+
+ if (!priv->in_ring) {
+ priv->in_ring = imx_media_alloc_dma_buf_ring(
+ priv->md, &priv->src_sd->entity, &priv->sd.entity,
+ sizeimage, vq->num_buffers, false);
+ if (IS_ERR(priv->in_ring)) {
+ v4l2_err(&priv->sd, "failed to alloc dma-buf ring\n");
+ ret = PTR_ERR(priv->in_ring);
+ priv->in_ring = NULL;
+ return ret;
+ }
+ }
+
+ ret = imx_media_dma_buf_queue_from_vb(priv->in_ring, vb);
+ if (ret)
+ goto free_ring;
+
+ return 0;
+
+free_ring:
+ imx_media_free_dma_buf_ring(priv->in_ring);
+ priv->in_ring = NULL;
+ return ret;
+}
+
+static void camif_buf_queue(struct vb2_buffer *vb)
+{
+ struct camif_priv *priv = vb2_get_drv_priv(vb->vb2_queue);
+ struct imx_media_buffer *buf = to_imx_media_vb(vb);
+ unsigned long flags;
+
+ spin_lock_irqsave(&priv->q_lock, flags);
+
+ list_add_tail(&buf->list, &priv->ready_q);
+
+ spin_unlock_irqrestore(&priv->q_lock, flags);
+}
+
+static int camif_start_streaming(struct vb2_queue *vq, unsigned int count)
+{
+ struct camif_priv *priv = vb2_get_drv_priv(vq);
+ u32 sizeimage = camif_get_sizeimage(priv);
+ struct imx_media_buffer *buf, *tmp;
+ unsigned long flags;
+ int ret;
+
+ if (vb2_is_streaming(vq))
+ return 0;
+
+ if (!priv->src_sd)
+ return -EPIPE;
+
+ if (!priv->in_ring) {
+ priv->in_ring = imx_media_alloc_dma_buf_ring(
+ priv->md, &priv->src_sd->entity, &priv->sd.entity,
+ sizeimage, vq->num_buffers, false);
+ if (IS_ERR(priv->in_ring)) {
+ v4l2_err(&priv->sd, "failed to alloc dma-buf ring\n");
+ ret = PTR_ERR(priv->in_ring);
+ priv->in_ring = NULL;
+ goto return_bufs;
+ }
+ }
+
+ ret = imx_media_pipeline_set_stream(priv->md, &priv->sd.entity,
+ &priv->mp, true);
+ if (ret) {
+ v4l2_err(&priv->sd, "pipeline_set_stream failed with %d\n",
+ ret);
+ goto free_ring;
+ }
+
+ priv->stop = false;
+
+ return 0;
+
+free_ring:
+ imx_media_free_dma_buf_ring(priv->in_ring);
+ priv->in_ring = NULL;
+return_bufs:
+ spin_lock_irqsave(&priv->q_lock, flags);
+ list_for_each_entry_safe(buf, tmp, &priv->ready_q, list) {
+ list_del(&buf->list);
+ vb2_buffer_done(&buf->vbuf.vb2_buf, VB2_BUF_STATE_QUEUED);
+ }
+ spin_unlock_irqrestore(&priv->q_lock, flags);
+ return ret;
+}
+
+static void camif_stop_streaming(struct vb2_queue *vq)
+{
+ struct camif_priv *priv = vb2_get_drv_priv(vq);
+ struct imx_media_buffer *frame;
+ unsigned long flags;
+ int ret;
+
+ if (!vb2_is_streaming(vq))
+ return;
+
+ spin_lock_irqsave(&priv->q_lock, flags);
+ priv->stop = true;
+ spin_unlock_irqrestore(&priv->q_lock, flags);
+
+ ret = imx_media_pipeline_set_stream(priv->md, &priv->sd.entity,
+ &priv->mp, false);
+ if (ret)
+ v4l2_warn(&priv->sd, "pipeline_set_stream failed with %d\n",
+ ret);
+
+ if (priv->in_ring) {
+ v4l2_warn(&priv->sd, "%s: in_ring was not freed\n",
+ __func__);
+ imx_media_free_dma_buf_ring(priv->in_ring);
+ priv->in_ring = NULL;
+ }
+
+ /* release all active buffers */
+ spin_lock_irqsave(&priv->q_lock, flags);
+ while (!list_empty(&priv->ready_q)) {
+ frame = list_entry(priv->ready_q.next,
+ struct imx_media_buffer, list);
+ list_del(&frame->list);
+ vb2_buffer_done(&frame->vbuf.vb2_buf, VB2_BUF_STATE_ERROR);
+ }
+ spin_unlock_irqrestore(&priv->q_lock, flags);
+}
+
+static struct vb2_ops camif_qops = {
+ .queue_setup = camif_queue_setup,
+ .buf_init = camif_buf_init,
+ .buf_prepare = camif_buf_prepare,
+ .buf_queue = camif_buf_queue,
+ .wait_prepare = vb2_ops_wait_prepare,
+ .wait_finish = vb2_ops_wait_finish,
+ .start_streaming = camif_start_streaming,
+ .stop_streaming = camif_stop_streaming,
+};
+
+/*
+ * File operations
+ */
+static int camif_open(struct file *file)
+{
+ struct camif_priv *priv = video_drvdata(file);
+ int ret;
+
+ if (mutex_lock_interruptible(&priv->mutex))
+ return -ERESTARTSYS;
+
+ ret = v4l2_fh_open(file);
+ if (ret)
+ v4l2_err(&priv->sd, "v4l2_fh_open failed\n");
+
+ mutex_unlock(&priv->mutex);
+ return ret;
+}
+
+static int camif_release(struct file *file)
+{
+ struct camif_priv *priv = video_drvdata(file);
+ struct vb2_queue *vq = &priv->buffer_queue;
+ int ret = 0;
+
+ mutex_lock(&priv->mutex);
+
+ if (file->private_data == vq->owner) {
+ vb2_queue_release(vq);
+ vq->owner = NULL;
+ }
+
+ v4l2_fh_release(file);
+ mutex_unlock(&priv->mutex);
+ return ret;
+}
+
+static const struct v4l2_file_operations camif_fops = {
+ .owner = THIS_MODULE,
+ .open = camif_open,
+ .release = camif_release,
+ .poll = vb2_fop_poll,
+ .unlocked_ioctl = video_ioctl2,
+ .mmap = vb2_fop_mmap,
+};
+
+static struct video_device camif_videodev = {
+ .fops = &camif_fops,
+ .ioctl_ops = &camif_ioctl_ops,
+ .minor = -1,
+ .release = video_device_release,
+ .vfl_dir = VFL_DIR_RX,
+ .tvnorms = V4L2_STD_NTSC | V4L2_STD_PAL | V4L2_STD_SECAM,
+};
+
+/*
+ * Subdev and media entity operations
+ */
+
+static void camif_new_dma_buf(struct camif_priv *priv)
+{
+ struct imx_media_dma_buf *dmabuf;
+ struct imx_media_buffer *buf;
+ enum vb2_buffer_state state;
+ struct vb2_buffer *vb;
+ unsigned long flags;
+
+ spin_lock_irqsave(&priv->q_lock, flags);
+
+ if (priv->stop || list_empty(&priv->ready_q))
+ goto unlock;
+
+ dmabuf = imx_media_dma_buf_dequeue(priv->in_ring);
+ if (!dmabuf)
+ goto unlock;
+
+ vb = dmabuf->vb;
+ buf = to_imx_media_vb(vb);
+ if (list_empty(&buf->list)) {
+ dev_dbg(priv->dev, "%s: buf%d not queued\n", __func__,
+ vb->index);
+ goto unlock;
+ }
+
+ dev_dbg(priv->dev, "%s: new buf%d\n", __func__, vb->index);
+ vb->timestamp = ktime_get_ns();
+ list_del_init(&buf->list);
+ state = dmabuf->status == IMX_MEDIA_BUF_STATUS_DONE ?
+ VB2_BUF_STATE_DONE : VB2_BUF_STATE_ERROR;
+ vb2_buffer_done(vb, state);
+unlock:
+ spin_unlock_irqrestore(&priv->q_lock, flags);
+}
+
+static long camif_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void *arg)
+{
+ struct camif_priv *priv = v4l2_get_subdevdata(sd);
+ struct imx_media_dma_buf_ring **ring;
+
+ switch (cmd) {
+ case IMX_MEDIA_REQ_DMA_BUF_SINK_RING:
+ if (!priv->in_ring)
+ return -EINVAL;
+ ring = (struct imx_media_dma_buf_ring **)arg;
+ *ring = priv->in_ring;
+ break;
+ case IMX_MEDIA_NEW_DMA_BUF:
+ camif_new_dma_buf(priv);
+ break;
+ case IMX_MEDIA_REL_DMA_BUF_SINK_RING:
+ /* src indicates sink buffer ring can be freed */
+ if (!priv->in_ring)
+ return 0;
+ v4l2_info(sd, "%s: freeing sink ring\n", __func__);
+ imx_media_free_dma_buf_ring(priv->in_ring);
+ priv->in_ring = NULL;
+ break;
+ default:
+ return -EINVAL;
+ }
+
+ return 0;
+}
+
+static int camif_link_setup(struct media_entity *entity,
+ const struct media_pad *local,
+ const struct media_pad *remote, u32 flags)
+{
+ struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
+ struct camif_priv *priv = v4l2_get_subdevdata(sd);
+ struct video_device *vfd = &priv->vfd;
+ struct v4l2_subdev *remote_sd;
+ int ret = 0;
+
+ dev_dbg(priv->dev, "link setup %s -> %s", remote->entity->name,
+ local->entity->name);
+
+ if (is_media_entity_v4l2_video_device(remote->entity))
+ return 0;
+
+ WARN_ON(local->flags & MEDIA_PAD_FL_SOURCE);
+
+ remote_sd = media_entity_to_v4l2_subdev(remote->entity);
+
+ /* reset controls to refresh with inherited from subdevs */
+ v4l2_ctrl_handler_free(vfd->ctrl_handler);
+ v4l2_ctrl_handler_init(vfd->ctrl_handler, 0);
+
+ if (flags & MEDIA_LNK_FL_ENABLED) {
+ if (priv->src_sd)
+ return -EBUSY;
+ priv->src_sd = remote_sd;
+ ret = imx_media_inherit_controls(priv->md, vfd,
+ &priv->src_sd->entity);
+ } else {
+ priv->src_sd = NULL;
+ }
+
+ return ret;
+}
+
+static int camif_enum_mbus_code(struct v4l2_subdev *sd,
+ struct v4l2_subdev_pad_config *cfg,
+ struct v4l2_subdev_mbus_code_enum *code)
+{
+ if (code->pad >= CAMIF_NUM_PADS)
+ return -EINVAL;
+
+ return imx_media_enum_format(&code->code, code->index, true, true);
+}
+
+static int camif_get_fmt(struct v4l2_subdev *sd,
+ struct v4l2_subdev_pad_config *cfg,
+ struct v4l2_subdev_format *sdformat)
+{
+ struct camif_priv *priv = v4l2_get_subdevdata(sd);
+
+ if (sdformat->pad >= CAMIF_NUM_PADS)
+ return -EINVAL;
+
+ sdformat->format = priv->format_mbus[sdformat->pad];
+
+ return 0;
+}
+
+static int camif_set_fmt(struct v4l2_subdev *sd,
+ struct v4l2_subdev_pad_config *cfg,
+ struct v4l2_subdev_format *sdformat)
+{
+ struct camif_priv *priv = v4l2_get_subdevdata(sd);
+ const struct imx_media_pixfmt *cc;
+ u32 code;
+
+ if (sdformat->pad >= CAMIF_NUM_PADS)
+ return -EINVAL;
+
+ if (vb2_is_busy(&priv->buffer_queue)) {
+ v4l2_err(&priv->sd, "%s queue busy\n", __func__);
+ return -EBUSY;
+ }
+
+ cc = imx_media_find_format(0, sdformat->format.code, true, true);
+ if (!cc) {
+ imx_media_enum_format(&code, 0, true, true);
+ cc = imx_media_find_format(0, code, true, true);
+ sdformat->format.code = cc->codes[0];
+ }
+
+ /* Output pad mirrors input pad, no limitations on input pads */
+ if (sdformat->pad == priv->output_pad)
+ sdformat->format = priv->format_mbus[priv->input_pad];
+
+ if (sdformat->which == V4L2_SUBDEV_FORMAT_TRY) {
+ cfg->try_fmt = sdformat->format;
+ } else {
+ priv->format_mbus[sdformat->pad] = sdformat->format;
+ priv->cc[sdformat->pad] = cc;
+ }
+
+ return 0;
+}
+
+static int camif_init_pads(struct camif_priv *priv)
+{
+ struct video_device *vfd = &priv->vfd;
+ struct imx_media_subdev *imxsd;
+ struct imx_media_pad *pad;
+ int i, ret;
+
+ imxsd = imx_media_find_subdev_by_sd(priv->md, &priv->sd);
+ if (IS_ERR(imxsd))
+ return PTR_ERR(imxsd);
+
+ if (imxsd->num_sink_pads != 1 || imxsd->num_src_pads != 1) {
+ v4l2_err(&priv->sd, "invalid num pads %d/%d\n",
+ imxsd->num_sink_pads, imxsd->num_src_pads);
+ return -EINVAL;
+ }
+
+ priv->vd_pad.flags = MEDIA_PAD_FL_SINK;
+ ret = media_entity_pads_init(&vfd->entity, 1, &priv->vd_pad);
+ if (ret) {
+ v4l2_err(&priv->sd, "failed to init device node pad\n");
+ return ret;
+ }
+
+ for (i = 0; i < CAMIF_NUM_PADS; i++) {
+ pad = &imxsd->pad[i];
+ priv->pad[i] = pad->pad;
+ if (priv->pad[i].flags & MEDIA_PAD_FL_SINK)
+ priv->input_pad = i;
+ else
+ priv->output_pad = i;
+
+ /* set a default mbus format */
+ ret = imx_media_init_mbus_fmt(&priv->format_mbus[i],
+ 640, 480, 0, V4L2_FIELD_NONE,
+ &priv->cc[i]);
+ if (ret)
+ return ret;
+ }
+
+ return media_entity_pads_init(&priv->sd.entity, CAMIF_NUM_PADS,
+ priv->pad);
+}
+
+static int camif_registered(struct v4l2_subdev *sd)
+{
+ struct camif_priv *priv = v4l2_get_subdevdata(sd);
+ struct vb2_queue *vq = &priv->buffer_queue;
+ struct video_device *vfd = &priv->vfd;
+ struct v4l2_mbus_framefmt *infmt, *outfmt;
+ int ret;
+
+ /* get media device */
+ priv->md = dev_get_drvdata(sd->v4l2_dev->dev);
+
+ vfd->v4l2_dev = sd->v4l2_dev;
+
+ ret = video_register_device(vfd, VFL_TYPE_GRABBER, 0);
+ if (ret) {
+ v4l2_err(sd, "Failed to register video device\n");
+ return ret;
+ }
+
+ vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ vq->io_modes = VB2_MMAP | VB2_DMABUF;
+ vq->drv_priv = priv;
+ vq->buf_struct_size = sizeof(struct imx_media_buffer);
+ vq->ops = &camif_qops;
+ vq->mem_ops = &vb2_dma_contig_memops;
+ vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+ vq->lock = &priv->mutex;
+ vq->min_buffers_needed = 2;
+ vq->dev = priv->dev;
+
+ ret = vb2_queue_init(vq);
+ if (ret) {
+ v4l2_err(sd, "vb2_queue_init failed\n");
+ goto unreg;
+ }
+
+ INIT_LIST_HEAD(&priv->ready_q);
+
+ ret = camif_init_pads(priv);
+ if (ret) {
+ v4l2_err(sd, "camif_init_pads failed\n");
+ goto unreg;
+ }
+
+ /* create the link to our device node */
+ ret = media_create_pad_link(&sd->entity, priv->output_pad,
+ &vfd->entity, 0,
+ MEDIA_LNK_FL_IMMUTABLE |
+ MEDIA_LNK_FL_ENABLED);
+ if (ret) {
+ v4l2_err(sd, "failed to create link to device node\n");
+ goto unreg;
+ }
+
+ /* setup default pad formats */
+ infmt = &priv->format_mbus[priv->input_pad];
+ outfmt = &priv->format_mbus[priv->output_pad];
+ ret = imx_media_init_mbus_fmt(outfmt, 640, 480, 0, V4L2_FIELD_NONE,
+ &priv->cc[priv->output_pad]);
+ if (ret)
+ goto unreg;
+
+ *infmt = *outfmt;
+ priv->cc[priv->input_pad] = priv->cc[priv->output_pad];
+
+ priv->current_std = V4L2_STD_UNKNOWN;
+
+ v4l2_info(sd, "Registered %s as /dev/%s\n", vfd->name,
+ video_device_node_name(vfd));
+
+ vfd->ctrl_handler = &priv->ctrl_hdlr;
+
+ return 0;
+unreg:
+ video_unregister_device(vfd);
+ return ret;
+}
+
+static void camif_unregistered(struct v4l2_subdev *sd)
+{
+ struct camif_priv *priv = v4l2_get_subdevdata(sd);
+ struct video_device *vfd = &priv->vfd;
+
+ mutex_lock(&priv->mutex);
+
+ if (video_is_registered(vfd)) {
+ video_unregister_device(vfd);
+ media_entity_cleanup(&vfd->entity);
+ }
+
+ mutex_unlock(&priv->mutex);
+}
+
+static const struct v4l2_subdev_internal_ops camif_internal_ops = {
+ .registered = camif_registered,
+ .unregistered = camif_unregistered,
+};
+
+static struct v4l2_subdev_core_ops camif_core_ops = {
+ .ioctl = camif_ioctl,
+};
+
+static struct media_entity_operations camif_entity_ops = {
+ .link_setup = camif_link_setup,
+ .link_validate = v4l2_subdev_link_validate,
+};
+
+static struct v4l2_subdev_pad_ops camif_pad_ops = {
+ .enum_mbus_code = camif_enum_mbus_code,
+ .get_fmt = camif_get_fmt,
+ .set_fmt = camif_set_fmt,
+};
+
+static struct v4l2_subdev_ops camif_subdev_ops = {
+ .pad = &camif_pad_ops,
+ .core = &camif_core_ops,
+};
+
+static int camif_probe(struct platform_device *pdev)
+{
+ struct imx_media_internal_sd_platformdata *pdata;
+ struct camif_priv *priv;
+ struct video_device *vfd;
+ int ret;
+
+ priv = devm_kzalloc(&pdev->dev, sizeof(*priv), GFP_KERNEL);
+ if (!priv)
+ return -ENOMEM;
+
+ platform_set_drvdata(pdev, priv);
+ priv->dev = &pdev->dev;
+
+ pdata = priv->dev->platform_data;
+
+ mutex_init(&priv->mutex);
+ spin_lock_init(&priv->q_lock);
+
+ v4l2_subdev_init(&priv->sd, &camif_subdev_ops);
+ v4l2_set_subdevdata(&priv->sd, priv);
+ priv->sd.internal_ops = &camif_internal_ops;
+ priv->sd.entity.ops = &camif_entity_ops;
+ priv->sd.entity.function = MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER;
+ priv->sd.dev = &pdev->dev;
+ priv->sd.owner = THIS_MODULE;
+ /* get our group id and camif id */
+ priv->sd.grp_id = pdata->grp_id;
+ priv->id = (pdata->grp_id >> IMX_MEDIA_GRP_ID_CAMIF_BIT) - 1;
+ strncpy(priv->sd.name, pdata->sd_name, sizeof(priv->sd.name));
+ snprintf(camif_videodev.name, sizeof(camif_videodev.name),
+ "%s devnode", pdata->sd_name);
+
+ priv->sd.flags = V4L2_SUBDEV_FL_HAS_DEVNODE | V4L2_SUBDEV_FL_HAS_EVENTS;
+
+ vfd = &priv->vfd;
+ *vfd = camif_videodev;
+ vfd->lock = &priv->mutex;
+ vfd->queue = &priv->buffer_queue;
+
+ video_set_drvdata(vfd, priv);
+
+ v4l2_ctrl_handler_init(&priv->ctrl_hdlr, 0);
+
+ ret = v4l2_async_register_subdev(&priv->sd);
+ if (ret)
+ goto free_ctrls;
+
+ return 0;
+free_ctrls:
+ v4l2_ctrl_handler_free(&priv->ctrl_hdlr);
+ return ret;
+}
+
+static int camif_remove(struct platform_device *pdev)
+{
+ struct camif_priv *priv =
+ (struct camif_priv *)platform_get_drvdata(pdev);
+
+ v4l2_ctrl_handler_free(&priv->ctrl_hdlr);
+ v4l2_async_unregister_subdev(&priv->sd);
+ media_entity_cleanup(&priv->sd.entity);
+ v4l2_device_unregister_subdev(&priv->sd);
+
+ return 0;
+}
+
+static const struct platform_device_id camif_ids[] = {
+ { .name = DEVICE_NAME },
+ { },
+};
+MODULE_DEVICE_TABLE(platform, camif_ids);
+
+static struct platform_driver imx_camif_driver = {
+ .probe = camif_probe,
+ .remove = camif_remove,
+ .driver = {
+ .name = DEVICE_NAME,
+ .owner = THIS_MODULE,
+ },
+};
+
+module_platform_driver(imx_camif_driver);
+
+MODULE_DESCRIPTION("i.MX camera interface subdev driver");
+MODULE_AUTHOR("Steve Longerbeam <steve_longerb...@mentor.com>");
+MODULE_LICENSE("GPL");
--
2.7.4
