drivers/media/platform/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/module.h>
  11 #include <linux/freezer.h>
  12 #include <linux/kthread.h>
  13
  14 #include "vimc-streamer.h"
  15
  16 /**
  17  * vimc_get_source_entity - get the entity connected with the first sink pad
  18  *
  19  * @ent:        reference media_entity
  20  *
  21  * Helper function that returns the media entity containing the source pad
  22  * linked with the first sink pad from the given media entity pad list.
  23  */
  24 static struct media_entity *vimc_get_source_entity(struct media_entity *ent)
  25 {
  26         struct media_pad *pad;
  27         int i;
  28
  29         for (i = 0; i < ent->num_pads; i++) {
  30                 if (ent->pads[i].flags & MEDIA_PAD_FL_SOURCE)
  31                         continue;
  32                 pad = media_entity_remote_pad(&ent->pads[i]);
  33                 return pad ? pad->entity : NULL;
  34         }
  35         return NULL;
  36 }
  37
  38 /*
  39  * vimc_streamer_pipeline_terminate - Disable stream in all ved in stream
  40  *
  41  * @stream: the pointer to the stream structure with the pipeline to be
  42  *          disabled.
  43  *
  44  * Calls s_stream to disable the stream in each entity of the pipeline
  45  *
  46  */
  47 static void vimc_streamer_pipeline_terminate(struct vimc_stream *stream)
  48 {
  49         struct vimc_ent_device *ved;
  50         struct v4l2_subdev *sd;
  51
  52         while (stream->pipe_size) {
  53                 stream->pipe_size--;
  54                 ved = stream->ved_pipeline[stream->pipe_size];
  55                 ved->stream = NULL;
  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 s_stream to enable stream in all entities of the pipeline.
  75  */
  76 static int vimc_streamer_pipeline_init(struct vimc_stream *stream,
  77                                        struct vimc_ent_device *ved)
  78 {
  79         struct media_entity *entity;
  80         struct video_device *vdev;
  81         struct v4l2_subdev *sd;
  82         int ret = 0;
  83
  84         stream->pipe_size = 0;
  85         while (stream->pipe_size < VIMC_STREAMER_PIPELINE_MAX_SIZE) {
  86                 if (!ved) {
  87                         vimc_streamer_pipeline_terminate(stream);
  88                         return -EINVAL;
  89                 }
  90                 stream->ved_pipeline[stream->pipe_size++] = ved;
  91                 ved->stream = stream;
  92
  93                 if (is_media_entity_v4l2_subdev(ved->ent)) {
  94                         sd = media_entity_to_v4l2_subdev(ved->ent);
  95                         ret = v4l2_subdev_call(sd, video, s_stream, 1);
  96                         if (ret && ret != -ENOIOCTLCMD) {
  97                                 pr_err("subdev_call error %s\n",
  98                                        ved->ent->name);
  99                                 vimc_streamer_pipeline_terminate(stream);
 100                                 return ret;
 101                         }
 102                 }
 103
 104                 entity = vimc_get_source_entity(ved->ent);
 105                 /* Check if the end of the pipeline was reached*/
 106                 if (!entity)
 107                         return 0;
 108
 109                 /* Get the next device in the pipeline */
 110                 if (is_media_entity_v4l2_subdev(entity)) {
 111                         sd = media_entity_to_v4l2_subdev(entity);
 112                         ved = v4l2_get_subdevdata(sd);
 113                 } else {
 114                         vdev = container_of(entity,
 115                                             struct video_device,
 116                                             entity);
 117                         ved = video_get_drvdata(vdev);
 118                 }
 119         }
 120
 121         vimc_streamer_pipeline_terminate(stream);
 122         return -EINVAL;
 123 }
 124
 125 /*
 126  * vimc_streamer_thread - process frames through the pipeline
 127  *
 128  * @data:       vimc_stream struct of the current stream
 129  *
 130  * From the source to the sink, gets a frame from each subdevice and send to
 131  * the next one of the pipeline at a fixed framerate.
 132  */
 133 static int vimc_streamer_thread(void *data)
 134 {
 135         struct vimc_stream *stream = data;
 136         u8 *frame = NULL;
 137         int i;
 138
 139         set_freezable();
 140
 141         for (;;) {
 142                 try_to_freeze();
 143                 if (kthread_should_stop())
 144                         break;
 145
 146                 for (i = stream->pipe_size - 1; i >= 0; i--) {
 147                         frame = stream->ved_pipeline[i]->process_frame(
 148                                         stream->ved_pipeline[i], frame);
 149                         if (!frame || IS_ERR(frame))
 150                                 break;
 151                 }
 152                 //wait for 60hz
 153                 set_current_state(TASK_UNINTERRUPTIBLE);
 154                 schedule_timeout(HZ / 60);
 155         }
 156
 157         return 0;
 158 }
 159
 160 /*
 161  * vimc_streamer_s_stream - start/stop the streaming on the media pipeline
 162  *
 163  * @stream:     the pointer to the stream structure of the current stream
 164  * @ved:        pointer to the vimc entity of the entity of the stream
 165  * @enable:     flag to determine if stream should start/stop
 166  *
 167  * When starting, check if there is no stream->kthread allocated. This should
 168  * indicate that a stream is already running. Then, it initializes
 169  * the pipeline, creates and runs a kthread to consume buffers through the
 170  * pipeline.
 171  * When stopping, analogously check if there is a stream running, stop
 172  * the thread and terminates the pipeline.
 173  */
 174 int vimc_streamer_s_stream(struct vimc_stream *stream,
 175                            struct vimc_ent_device *ved,
 176                            int enable)
 177 {
 178         int ret;
 179
 180         if (!stream || !ved)
 181                 return -EINVAL;
 182
 183         if (enable) {
 184                 if (stream->kthread)
 185                         return 0;
 186
 187                 ret = vimc_streamer_pipeline_init(stream, ved);
 188                 if (ret)
 189                         return ret;
 190
 191                 stream->kthread = kthread_run(vimc_streamer_thread, stream,
 192                                               "vimc-streamer thread");
 193
 194                 if (IS_ERR(stream->kthread))
 195                         return PTR_ERR(stream->kthread);
 196
 197         } else {
 198                 if (!stream->kthread)
 199                         return 0;
 200
 201                 ret = kthread_stop(stream->kthread);
 202                 if (ret)
 203                         return ret;
 204
 205                 stream->kthread = NULL;
 206
 207                 vimc_streamer_pipeline_terminate(stream);
 208         }
 209
 210         return 0;
 211 }
 212 EXPORT_SYMBOL_GPL(vimc_streamer_s_stream);