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 | 1011 +++++++++++++++++++++++++++++++++
 2 files changed, 1012 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..034a1ff
--- /dev/null
+++ b/drivers/staging/media/imx/imx-camif.c
@@ -0,0 +1,1011 @@
+/*
+ * 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

_______________________________________________
devel mailing list
de...@linuxdriverproject.org
http://driverdev.linuxdriverproject.org/mailman/listinfo/driverdev-devel

Reply via email to