media: stm32-dcmi: add support of several sub-devices

Add support of several sub-devices within pipeline instead
of a single one.
This allows to support a CSI-2 camera sensor connected
through a CSI-2 to parallel bridge.

Signed-off-by: Hugues Fruchet <hugues.fruchet@st.com>
Signed-off-by: Hans Verkuil <hverkuil-cisco@xs4all.nl>
Signed-off-by: Mauro Carvalho Chehab <mchehab+samsung@kernel.org>

diff --git a/drivers/media/platform/stm32/stm32-dcmi.c b/drivers/media/platform/stm32/stm32-dcmi.c
index 10c8d4f..cb1daf8 100644 (file)
--- a/drivers/media/platform/stm32/stm32-dcmi.c
+++ b/drivers/media/platform/stm32/stm32-dcmi.c
@@ -172,6 +172,7 @@ struct stm32_dcmi {
 
        struct media_device             mdev;
        struct media_pad                vid_cap_pad;
+       struct media_pipeline           pipeline;
 };
 
 static inline struct stm32_dcmi *notifier_to_dcmi(struct v4l2_async_notifier *n)
@@ -583,6 +584,144 @@ static void dcmi_buf_queue(struct vb2_buffer *vb)
        spin_unlock_irq(&dcmi->irqlock);
 }
 
+static struct media_entity *dcmi_find_source(struct stm32_dcmi *dcmi)
+{
+       struct media_entity *entity = &dcmi->vdev->entity;
+       struct media_pad *pad;
+
+       /* Walk searching for entity having no sink */
+       while (1) {
+               pad = &entity->pads[0];
+               if (!(pad->flags & MEDIA_PAD_FL_SINK))
+                       break;
+
+               pad = media_entity_remote_pad(pad);
+               if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
+                       break;
+
+               entity = pad->entity;
+       }
+
+       return entity;
+}
+
+static int dcmi_pipeline_s_fmt(struct stm32_dcmi *dcmi,
+                              struct v4l2_subdev_pad_config *pad_cfg,
+                              struct v4l2_subdev_format *format)
+{
+       struct media_entity *entity = &dcmi->entity.source->entity;
+       struct v4l2_subdev *subdev;
+       struct media_pad *sink_pad = NULL;
+       struct media_pad *src_pad = NULL;
+       struct media_pad *pad = NULL;
+       struct v4l2_subdev_format fmt = *format;
+       bool found = false;
+       int ret;
+
+       /*
+        * Starting from sensor subdevice, walk within
+        * pipeline and set format on each subdevice
+        */
+       while (1) {
+               unsigned int i;
+
+               /* Search if current entity has a source pad */
+               for (i = 0; i < entity->num_pads; i++) {
+                       pad = &entity->pads[i];
+                       if (pad->flags & MEDIA_PAD_FL_SOURCE) {
+                               src_pad = pad;
+                               found = true;
+                               break;
+                       }
+               }
+               if (!found)
+                       break;
+
+               subdev = media_entity_to_v4l2_subdev(entity);
+
+               /* Propagate format on sink pad if any, otherwise source pad */
+               if (sink_pad)
+                       pad = sink_pad;
+
+               dev_dbg(dcmi->dev, "\"%s\":%d pad format set to 0x%x %ux%u\n",
+                       subdev->name, pad->index, format->format.code,
+                       format->format.width, format->format.height);
+
+               fmt.pad = pad->index;
+               ret = v4l2_subdev_call(subdev, pad, set_fmt, pad_cfg, &fmt);
+               if (ret < 0) {
+                       dev_err(dcmi->dev, "%s: Failed to set format 0x%x %ux%u on \"%s\":%d pad (%d)\n",
+                               __func__, format->format.code,
+                               format->format.width, format->format.height,
+                               subdev->name, pad->index, ret);
+                       return ret;
+               }
+
+               if (fmt.format.code != format->format.code ||
+                   fmt.format.width != format->format.width ||
+                   fmt.format.height != format->format.height) {
+                       dev_dbg(dcmi->dev, "\"%s\":%d pad format has been changed to 0x%x %ux%u\n",
+                               subdev->name, pad->index, fmt.format.code,
+                               fmt.format.width, fmt.format.height);
+               }
+
+               /* Walk to next entity */
+               sink_pad = media_entity_remote_pad(src_pad);
+               if (!sink_pad || !is_media_entity_v4l2_subdev(sink_pad->entity))
+                       break;
+
+               entity = sink_pad->entity;
+       }
+       *format = fmt;
+
+       return 0;
+}
+
+static int dcmi_pipeline_s_stream(struct stm32_dcmi *dcmi, int state)
+{
+       struct media_entity *entity = &dcmi->vdev->entity;
+       struct v4l2_subdev *subdev;
+       struct media_pad *pad;
+       int ret;
+
+       /* Start/stop all entities within pipeline */
+       while (1) {
+               pad = &entity->pads[0];
+               if (!(pad->flags & MEDIA_PAD_FL_SINK))
+                       break;
+
+               pad = media_entity_remote_pad(pad);
+               if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
+                       break;
+
+               entity = pad->entity;
+               subdev = media_entity_to_v4l2_subdev(entity);
+
+               ret = v4l2_subdev_call(subdev, video, s_stream, state);
+               if (ret < 0 && ret != -ENOIOCTLCMD) {
+                       dev_err(dcmi->dev, "%s: \"%s\" failed to %s streaming (%d)\n",
+                               __func__, subdev->name,
+                               state ? "start" : "stop", ret);
+                       return ret;
+               }
+
+               dev_dbg(dcmi->dev, "\"%s\" is %s\n",
+                       subdev->name, state ? "started" : "stopped");
+       }
+
+       return 0;
+}
+
+static int dcmi_pipeline_start(struct stm32_dcmi *dcmi)
+{
+       return dcmi_pipeline_s_stream(dcmi, 1);
+}
+
+static void dcmi_pipeline_stop(struct stm32_dcmi *dcmi)
+{
+       dcmi_pipeline_s_stream(dcmi, 0);
+}
+
 static int dcmi_start_streaming(struct vb2_queue *vq, unsigned int count)
 {
        struct stm32_dcmi *dcmi = vb2_get_drv_priv(vq);
@@ -597,14 +736,17 @@ static int dcmi_start_streaming(struct vb2_queue *vq, unsigned int count)
                goto err_release_buffers;
        }
 
-       /* Enable stream on the sub device */
-       ret = v4l2_subdev_call(dcmi->entity.source, video, s_stream, 1);
-       if (ret && ret != -ENOIOCTLCMD) {
-               dev_err(dcmi->dev, "%s: Failed to start streaming, subdev streamon error",
-                       __func__);
+       ret = media_pipeline_start(&dcmi->vdev->entity, &dcmi->pipeline);
+       if (ret < 0) {
+               dev_err(dcmi->dev, "%s: Failed to start streaming, media pipeline start error (%d)\n",
+                       __func__, ret);
                goto err_pm_put;
        }
 
+       ret = dcmi_pipeline_start(dcmi);
+       if (ret)
+               goto err_media_pipeline_stop;
+
        spin_lock_irq(&dcmi->irqlock);
 
        /* Set bus width */
@@ -676,7 +818,7 @@ static int dcmi_start_streaming(struct vb2_queue *vq, unsigned int count)
        if (ret) {
                dev_err(dcmi->dev, "%s: Start streaming failed, cannot start capture\n",
                        __func__);
-               goto err_subdev_streamoff;
+               goto err_pipeline_stop;
        }
 
        /* Enable interruptions */
@@ -687,8 +829,11 @@ static int dcmi_start_streaming(struct vb2_queue *vq, unsigned int count)
 
        return 0;
 
-err_subdev_streamoff:
-       v4l2_subdev_call(dcmi->entity.source, video, s_stream, 0);
+err_pipeline_stop:
+       dcmi_pipeline_stop(dcmi);
+
+err_media_pipeline_stop:
+       media_pipeline_stop(&dcmi->vdev->entity);
 
 err_pm_put:
        pm_runtime_put(dcmi->dev);
@@ -713,13 +858,10 @@ static void dcmi_stop_streaming(struct vb2_queue *vq)
 {
        struct stm32_dcmi *dcmi = vb2_get_drv_priv(vq);
        struct dcmi_buf *buf, *node;
-       int ret;
 
-       /* Disable stream on the sub device */
-       ret = v4l2_subdev_call(dcmi->entity.source, video, s_stream, 0);
-       if (ret && ret != -ENOIOCTLCMD)
-               dev_err(dcmi->dev, "%s: Failed to stop streaming, subdev streamoff error (%d)\n",
-                       __func__, ret);
+       dcmi_pipeline_stop(dcmi);
+
+       media_pipeline_stop(&dcmi->vdev->entity);
 
        spin_lock_irq(&dcmi->irqlock);
 
@@ -937,8 +1079,7 @@ static int dcmi_set_fmt(struct stm32_dcmi *dcmi, struct v4l2_format *f)
        mf->width = sd_framesize.width;
        mf->height = sd_framesize.height;
 
-       ret = v4l2_subdev_call(dcmi->entity.source, pad,
-                              set_fmt, NULL, &format);
+       ret = dcmi_pipeline_s_fmt(dcmi, NULL, &format);
        if (ret < 0)
                return ret;
 
@@ -1412,6 +1553,12 @@ static int dcmi_set_default_fmt(struct stm32_dcmi *dcmi)
        return 0;
 }
 
+/*
+ * FIXME: For the time being we only support subdevices
+ * which expose RGB & YUV "parallel form" mbus code (_2X8).
+ * Nevertheless, this allows to support serial source subdevices
+ * and serial to parallel bridges which conform to this.
+ */
 static const struct dcmi_format dcmi_formats[] = {
        {
                .fourcc = V4L2_PIX_FMT_RGB565,
@@ -1537,7 +1684,20 @@ static int dcmi_graph_notify_complete(struct v4l2_async_notifier *notifier)
        struct stm32_dcmi *dcmi = notifier_to_dcmi(notifier);
        int ret;
 
+       /*
+        * Now that the graph is complete,
+        * we search for the source subdevice
+        * in order to expose it through V4L2 interface
+        */
+       dcmi->entity.source =
+               media_entity_to_v4l2_subdev(dcmi_find_source(dcmi));
+       if (!dcmi->entity.source) {
+               dev_err(dcmi->dev, "Source subdevice not found\n");
+               return -ENODEV;
+       }
+
        dcmi->vdev->ctrl_handler = dcmi->entity.source->ctrl_handler;
+
        ret = dcmi_formats_init(dcmi);
        if (ret) {
                dev_err(dcmi->dev, "No supported mediabus format found\n");
@@ -1582,12 +1742,31 @@ static int dcmi_graph_notify_bound(struct v4l2_async_notifier *notifier,
                                   struct v4l2_async_subdev *asd)
 {
        struct stm32_dcmi *dcmi = notifier_to_dcmi(notifier);
+       unsigned int ret;
+       int src_pad;
 
-       dev_dbg(dcmi->dev, "Subdev %s bound\n", subdev->name);
+       dev_dbg(dcmi->dev, "Subdev \"%s\" bound\n", subdev->name);
 
-       dcmi->entity.source = subdev;
+       /*
+        * Link this sub-device to DCMI, it could be
+        * a parallel camera sensor or a bridge
+        */
+       src_pad = media_entity_get_fwnode_pad(&subdev->entity,
+                                             subdev->fwnode,
+                                             MEDIA_PAD_FL_SOURCE);
+
+       ret = media_create_pad_link(&subdev->entity, src_pad,
+                                   &dcmi->vdev->entity, 0,
+                                   MEDIA_LNK_FL_IMMUTABLE |
+                                   MEDIA_LNK_FL_ENABLED);
+       if (ret)
+               dev_err(dcmi->dev, "Failed to create media pad link with subdev \"%s\"\n",
+                       subdev->name);
+       else
+               dev_dbg(dcmi->dev, "DCMI is now linked to \"%s\"\n",
+                       subdev->name);
 
-       return 0;
+       return ret;
 }
 
 static const struct v4l2_async_notifier_operations dcmi_graph_notify_ops = {