media: vimc: Add vimc-streamer for stream control

Add a linear pipeline logic for the stream control. It's created by
walking backwards on the entity graph. When the stream starts it will
simply loop through the pipeline calling the respective process_frame
function of each entity.

Fixes: f2fe89061d797 ("vimc: Virtual Media Controller core, capture
and sensor")

Cc: stable@vger.kernel.org # for v4.20
Signed-off-by: Lucas A. M. Magalhães <lucmaga@gmail.com>
Acked-by: Helen Koike <helen.koike@collabora.com>
Signed-off-by: Hans Verkuil <hverkuil-cisco@xs4all.nl>
[hverkuil-cisco@xs4all.nl: fixed small space-after-tab issue in the patch]
Signed-off-by: Mauro Carvalho Chehab <mchehab+samsung@kernel.org>

1 files changed, 188 insertions, 0 deletions

diff --git a/drivers/media/platform/vimc/vimc-streamer.c b/drivers/media/platform/vimc/vimc-streamer.c
new file mode 100644
index 000000000000..fcc897fb247b
--- /dev/null
+++ b/drivers/media/platform/vimc/vimc-streamer.c
@@ -0,0 +1,188 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * vimc-streamer.c Virtual Media Controller Driver
+ *
+ * Copyright (C) 2018 Lucas A. M. Magalhães <lucmaga@gmail.com>
+ *
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/freezer.h>
+#include <linux/kthread.h>
+
+#include "vimc-streamer.h"
+
+/**
+ * vimc_get_source_entity - get the entity connected with the first sink pad
+ *
+ * @ent:        reference media_entity
+ *
+ * Helper function that returns the media entity containing the source pad
+ * linked with the first sink pad from the given media entity pad list.
+ */
+static struct media_entity *vimc_get_source_entity(struct media_entity *ent)
+{
+        struct media_pad *pad;
+        int i;
+
+        for (i = 0; i < ent->num_pads; i++) {
+                if (ent->pads[i].flags & MEDIA_PAD_FL_SOURCE)
+                        continue;
+                pad = media_entity_remote_pad(&ent->pads[i]);
+                return pad ? pad->entity : NULL;
+        }
+        return NULL;
+}
+
+/*
+ * vimc_streamer_pipeline_terminate - Disable stream in all ved in stream
+ *
+ * @stream: the pointer to the stream structure with the pipeline to be
+ *          disabled.
+ *
+ * Calls s_stream to disable the stream in each entity of the pipeline
+ *
+ */
+static void vimc_streamer_pipeline_terminate(struct vimc_stream *stream)
+{
+        struct media_entity *entity;
+        struct v4l2_subdev *sd;
+
+        while (stream->pipe_size) {
+                stream->pipe_size--;
+                entity = stream->ved_pipeline[stream->pipe_size]->ent;
+                entity = vimc_get_source_entity(entity);
+                stream->ved_pipeline[stream->pipe_size] = NULL;
+
+                if (!is_media_entity_v4l2_subdev(entity))
+                        continue;
+
+                sd = media_entity_to_v4l2_subdev(entity);
+                v4l2_subdev_call(sd, video, s_stream, 0);
+        }
+}
+
+/*
+ * vimc_streamer_pipeline_init - initializes the stream structure
+ *
+ * @stream: the pointer to the stream structure to be initialized
+ * @ved:    the pointer to the vimc entity initializing the stream
+ *
+ * Initializes the stream structure. Walks through the entity graph to
+ * construct the pipeline used later on the streamer thread.
+ * Calls s_stream to enable stream in all entities of the pipeline.
+ */
+static int vimc_streamer_pipeline_init(struct vimc_stream *stream,
+                                       struct vimc_ent_device *ved)
+{
+        struct media_entity *entity;
+        struct video_device *vdev;
+        struct v4l2_subdev *sd;
+        int ret = 0;
+
+        stream->pipe_size = 0;
+        while (stream->pipe_size < VIMC_STREAMER_PIPELINE_MAX_SIZE) {
+                if (!ved) {
+                        vimc_streamer_pipeline_terminate(stream);
+                        return -EINVAL;
+                }
+                stream->ved_pipeline[stream->pipe_size++] = ved;
+
+                entity = vimc_get_source_entity(ved->ent);
+                /* Check if the end of the pipeline was reached*/
+                if (!entity)
+                        return 0;
+
+                if (is_media_entity_v4l2_subdev(entity)) {
+                        sd = media_entity_to_v4l2_subdev(entity);
+                        ret = v4l2_subdev_call(sd, video, s_stream, 1);
+                        if (ret && ret != -ENOIOCTLCMD) {
+                                vimc_streamer_pipeline_terminate(stream);
+                                return ret;
+                        }
+                        ved = v4l2_get_subdevdata(sd);
+                } else {
+                        vdev = container_of(entity,
+                                            struct video_device,
+                                            entity);
+                        ved = video_get_drvdata(vdev);
+                }
+        }
+
+        vimc_streamer_pipeline_terminate(stream);
+        return -EINVAL;
+}
+
+static int vimc_streamer_thread(void *data)
+{
+        struct vimc_stream *stream = data;
+        int i;
+
+        set_freezable();
+        set_current_state(TASK_UNINTERRUPTIBLE);
+
+        for (;;) {
+                try_to_freeze();
+                if (kthread_should_stop())
+                        break;
+
+                for (i = stream->pipe_size - 1; i >= 0; i--) {
+                        stream->frame = stream->ved_pipeline[i]->process_frame(
+                                        stream->ved_pipeline[i],
+                                        stream->frame);
+                        if (!stream->frame)
+                                break;
+                        if (IS_ERR(stream->frame))
+                                break;
+                }
+                //wait for 60hz
+                schedule_timeout(HZ / 60);
+        }
+
+        return 0;
+}
+
+int vimc_streamer_s_stream(struct vimc_stream *stream,
+                           struct vimc_ent_device *ved,
+                           int enable)
+{
+        int ret;
+
+        if (!stream || !ved)
+                return -EINVAL;
+
+        if (enable) {
+                if (stream->kthread)
+                        return 0;
+
+                ret = vimc_streamer_pipeline_init(stream, ved);
+                if (ret)
+                        return ret;
+
+                stream->kthread = kthread_run(vimc_streamer_thread, stream,
+                                              "vimc-streamer thread");
+
+                if (IS_ERR(stream->kthread))
+                        return PTR_ERR(stream->kthread);
+
+        } else {
+                if (!stream->kthread)
+                        return 0;
+
+                ret = kthread_stop(stream->kthread);
+                if (ret)
+                        return ret;
+
+                stream->kthread = NULL;
+
+                vimc_streamer_pipeline_terminate(stream);
+        }
+
+        return 0;
+}
+EXPORT_SYMBOL_GPL(vimc_streamer_s_stream);
+
+MODULE_DESCRIPTION("Virtual Media Controller Driver (VIMC) Streamer");
+MODULE_AUTHOR("Lucas A. M. Magalhães <lucmaga@gmail.com>");
+MODULE_LICENSE("GPL");