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 <[email protected]>
   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 static int vimc_streamer_thread(void *data)
 126 {
 127         struct vimc_stream *stream = data;
 128         u8 *frame = NULL;
 129         int i;
 130
 131         set_freezable();
 132
 133         for (;;) {
 134                 try_to_freeze();
 135                 if (kthread_should_stop())
 136                         break;
 137
 138                 for (i = stream->pipe_size - 1; i >= 0; i--) {
 139                         frame = stream->ved_pipeline[i]->process_frame(
 140                                         stream->ved_pipeline[i], frame);
 141                         if (!frame || IS_ERR(frame))
 142                                 break;
 143                 }
 144                 //wait for 60hz
 145                 set_current_state(TASK_UNINTERRUPTIBLE);
 146                 schedule_timeout(HZ / 60);
 147         }
 148
 149         return 0;
 150 }
 151
 152 int vimc_streamer_s_stream(struct vimc_stream *stream,
 153                            struct vimc_ent_device *ved,
 154                            int enable)
 155 {
 156         int ret;
 157
 158         if (!stream || !ved)
 159                 return -EINVAL;
 160
 161         if (enable) {
 162                 if (stream->kthread)
 163                         return 0;
 164
 165                 ret = vimc_streamer_pipeline_init(stream, ved);
 166                 if (ret)
 167                         return ret;
 168
 169                 stream->kthread = kthread_run(vimc_streamer_thread, stream,
 170                                               "vimc-streamer thread");
 171
 172                 if (IS_ERR(stream->kthread))
 173                         return PTR_ERR(stream->kthread);
 174
 175         } else {
 176                 if (!stream->kthread)
 177                         return 0;
 178
 179                 ret = kthread_stop(stream->kthread);
 180                 if (ret)
 181                         return ret;
 182
 183                 stream->kthread = NULL;
 184
 185                 vimc_streamer_pipeline_terminate(stream);
 186         }
 187
 188         return 0;
 189 }
 190 EXPORT_SYMBOL_GPL(vimc_streamer_s_stream);
 191
 192 MODULE_DESCRIPTION("Virtual Media Controller Driver (VIMC) Streamer");
 193 MODULE_AUTHOR("Lucas A. M. Magalhães <[email protected]>");
 194 MODULE_LICENSE("GPL");