drivers/media/test_drivers/vimc/vimc-streamer.c

   1 // SPDX-License-Identifier: GPL-2.0+
   2 /*
   3  * vimc-streamer.c Virtual Media Controller Driver
   4  *
   5  * Copyright (C) 2018 Lucas A. M. Magalhães <lucmaga@gmail.com>
   6  *
   7  */
   8
   9 #include <linux/init.h>
  10 #include <linux/freezer.h>
  11 #include <linux/kthread.h>
  12
  13 #include "vimc-streamer.h"
  14
  15 /**
  16  * vimc_get_source_entity - get the entity connected with the first sink pad
  17  *
  18  * @ent:        reference media_entity
  19  *
  20  * Helper function that returns the media entity containing the source pad
  21  * linked with the first sink pad from the given media entity pad list.
  22  *
  23  * Return: The source pad or NULL, if it wasn't found.
  24  */
  25 static struct media_entity *vimc_get_source_entity(struct media_entity *ent)
  26 {
  27         struct media_pad *pad;
  28         int i;
  29
  30         for (i = 0; i < ent->num_pads; i++) {
  31                 if (ent->pads[i].flags & MEDIA_PAD_FL_SOURCE)
  32                         continue;
  33                 pad = media_entity_remote_pad(&ent->pads[i]);
  34                 return pad ? pad->entity : NULL;
  35         }
  36         return NULL;
  37 }
  38
  39 /**
  40  * vimc_streamer_pipeline_terminate - Disable stream in all ved in stream
  41  *
  42  * @stream: the pointer to the stream structure with the pipeline to be
  43  *          disabled.
  44  *
  45  * Calls s_stream to disable the stream in each entity of the pipeline
  46  *
  47  */
  48 static void vimc_streamer_pipeline_terminate(struct vimc_stream *stream)
  49 {
  50         struct vimc_ent_device *ved;
  51         struct v4l2_subdev *sd;
  52
  53         while (stream->pipe_size) {
  54                 stream->pipe_size--;
  55                 ved = stream->ved_pipeline[stream->pipe_size];
  56                 stream->ved_pipeline[stream->pipe_size] = NULL;
  57
  58                 if (!is_media_entity_v4l2_subdev(ved->ent))
  59                         continue;
  60
  61                 sd = media_entity_to_v4l2_subdev(ved->ent);
  62                 v4l2_subdev_call(sd, video, s_stream, 0);
  63         }
  64 }
  65
  66 /**
  67  * vimc_streamer_pipeline_init - Initializes the stream structure
  68  *
  69  * @stream: the pointer to the stream structure to be initialized
  70  * @ved:    the pointer to the vimc entity initializing the stream
  71  *
  72  * Initializes the stream structure. Walks through the entity graph to
  73  * construct the pipeline used later on the streamer thread.
  74  * Calls vimc_streamer_s_stream() to enable stream in all entities of
  75  * the pipeline.
  76  *
  77  * Return: 0 if success, error code otherwise.
  78  */
  79 static int vimc_streamer_pipeline_init(struct vimc_stream *stream,
  80                                        struct vimc_ent_device *ved)
  81 {
  82         struct media_entity *entity;
  83         struct video_device *vdev;
  84         struct v4l2_subdev *sd;
  85         int ret = 0;
  86
  87         stream->pipe_size = 0;
  88         while (stream->pipe_size < VIMC_STREAMER_PIPELINE_MAX_SIZE) {
  89                 if (!ved) {
  90                         vimc_streamer_pipeline_terminate(stream);
  91                         return -EINVAL;
  92                 }
  93                 stream->ved_pipeline[stream->pipe_size++] = ved;
  94
  95                 if (is_media_entity_v4l2_subdev(ved->ent)) {
  96                         sd = media_entity_to_v4l2_subdev(ved->ent);
  97                         ret = v4l2_subdev_call(sd, video, s_stream, 1);
  98                         if (ret && ret != -ENOIOCTLCMD) {
  99                                 dev_err(ved->dev, "subdev_call error %s\n",
 100                                         ved->ent->name);
 101                                 vimc_streamer_pipeline_terminate(stream);
 102                                 return ret;
 103                         }
 104                 }
 105
 106                 entity = vimc_get_source_entity(ved->ent);
 107                 /* Check if the end of the pipeline was reached */
 108                 if (!entity) {
 109                         /* the first entity of the pipe should be source only */
 110                         if (!vimc_is_source(ved->ent)) {
 111                                 dev_err(ved->dev,
 112                                         "first entity in the pipe '%s' is not a source\n",
 113                                         ved->ent->name);
 114                                 vimc_streamer_pipeline_terminate(stream);
 115                                 return -EPIPE;
 116                         }
 117                         return 0;
 118                 }
 119
 120                 /* Get the next device in the pipeline */
 121                 if (is_media_entity_v4l2_subdev(entity)) {
 122                         sd = media_entity_to_v4l2_subdev(entity);
 123                         ved = v4l2_get_subdevdata(sd);
 124                 } else {
 125                         vdev = container_of(entity,
 126                                             struct video_device,
 127                                             entity);
 128                         ved = video_get_drvdata(vdev);
 129                 }
 130         }
 131
 132         vimc_streamer_pipeline_terminate(stream);
 133         return -EINVAL;
 134 }
 135
 136 /**
 137  * vimc_streamer_thread - Process frames through the pipeline
 138  *
 139  * @data:       vimc_stream struct of the current stream
 140  *
 141  * From the source to the sink, gets a frame from each subdevice and send to
 142  * the next one of the pipeline at a fixed framerate.
 143  *
 144  * Return:
 145  * Always zero (created as ``int`` instead of ``void`` to comply with
 146  * kthread API).
 147  */
 148 static int vimc_streamer_thread(void *data)
 149 {
 150         struct vimc_stream *stream = data;
 151         u8 *frame = NULL;
 152         int i;
 153
 154         set_freezable();
 155
 156         for (;;) {
 157                 try_to_freeze();
 158                 if (kthread_should_stop())
 159                         break;
 160
 161                 for (i = stream->pipe_size - 1; i >= 0; i--) {
 162                         frame = stream->ved_pipeline[i]->process_frame(
 163                                         stream->ved_pipeline[i], frame);
 164                         if (!frame || IS_ERR(frame))
 165                                 break;
 166                 }
 167                 //wait for 60hz
 168                 set_current_state(TASK_UNINTERRUPTIBLE);
 169                 schedule_timeout(HZ / 60);
 170         }
 171
 172         return 0;
 173 }
 174
 175 /**
 176  * vimc_streamer_s_stream - Start/stop the streaming on the media pipeline
 177  *
 178  * @stream:     the pointer to the stream structure of the current stream
 179  * @ved:        pointer to the vimc entity of the entity of the stream
 180  * @enable:     flag to determine if stream should start/stop
 181  *
 182  * When starting, check if there is no ``stream->kthread`` allocated. This
 183  * should indicate that a stream is already running. Then, it initializes the
 184  * pipeline, creates and runs a kthread to consume buffers through the pipeline.
 185  * When stopping, analogously check if there is a stream running, stop the
 186  * thread and terminates the pipeline.
 187  *
 188  * Return: 0 if success, error code otherwise.
 189  */
 190 int vimc_streamer_s_stream(struct vimc_stream *stream,
 191                            struct vimc_ent_device *ved,
 192                            int enable)
 193 {
 194         int ret;
 195
 196         if (!stream || !ved)
 197                 return -EINVAL;
 198
 199         if (enable) {
 200                 if (stream->kthread)
 201                         return 0;
 202
 203                 ret = vimc_streamer_pipeline_init(stream, ved);
 204                 if (ret)
 205                         return ret;
 206
 207                 stream->kthread = kthread_run(vimc_streamer_thread, stream,
 208                                               "vimc-streamer thread");
 209
 210                 if (IS_ERR(stream->kthread)) {
 211                         ret = PTR_ERR(stream->kthread);
 212                         dev_err(ved->dev, "kthread_run failed with %d\n", ret);
 213                         vimc_streamer_pipeline_terminate(stream);
 214                         stream->kthread = NULL;
 215                         return ret;
 216                 }
 217
 218         } else {
 219                 if (!stream->kthread)
 220                         return 0;
 221
 222                 ret = kthread_stop(stream->kthread);
 223                 /*
 224                  * kthread_stop returns -EINTR in cases when streamon was
 225                  * immediately followed by streamoff, and the thread didn't had
 226                  * a chance to run. Ignore errors to stop the stream in the
 227                  * pipeline.
 228                  */
 229                 if (ret)
 230                         dev_dbg(ved->dev, "kthread_stop returned '%d'\n", ret);
 231
 232                 stream->kthread = NULL;
 233
 234                 vimc_streamer_pipeline_terminate(stream);
 235         }
 236
 237         return 0;
 238 }