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_pad_remote_pad_first(&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 /* 63 * Do not call .s_stream() to stop an already 64 * stopped/unstarted subdev. 65 */ 66 if (!v4l2_subdev_is_streaming(sd)) 67 continue; 68 v4l2_subdev_call(sd, video, s_stream, 0); 69 } 70 } 71 72 /** 73 * vimc_streamer_pipeline_init - Initializes the stream structure 74 * 75 * @stream: the pointer to the stream structure to be initialized 76 * @ved: the pointer to the vimc entity initializing the stream 77 * 78 * Initializes the stream structure. Walks through the entity graph to 79 * construct the pipeline used later on the streamer thread. 80 * Calls vimc_streamer_s_stream() to enable stream in all entities of 81 * the pipeline. 82 * 83 * Return: 0 if success, error code otherwise. 84 */ 85 static int vimc_streamer_pipeline_init(struct vimc_stream *stream, 86 struct vimc_ent_device *ved) 87 { 88 struct media_entity *entity; 89 struct video_device *vdev; 90 struct v4l2_subdev *sd; 91 int ret = 0; 92 93 stream->pipe_size = 0; 94 while (stream->pipe_size < VIMC_STREAMER_PIPELINE_MAX_SIZE) { 95 if (!ved) { 96 vimc_streamer_pipeline_terminate(stream); 97 return -EINVAL; 98 } 99 stream->ved_pipeline[stream->pipe_size++] = ved; 100 101 if (is_media_entity_v4l2_subdev(ved->ent)) { 102 sd = media_entity_to_v4l2_subdev(ved->ent); 103 ret = v4l2_subdev_call(sd, video, s_stream, 1); 104 if (ret && ret != -ENOIOCTLCMD) { 105 dev_err(ved->dev, "subdev_call error %s\n", 106 ved->ent->name); 107 vimc_streamer_pipeline_terminate(stream); 108 return ret; 109 } 110 } 111 112 entity = vimc_get_source_entity(ved->ent); 113 /* Check if the end of the pipeline was reached */ 114 if (!entity) { 115 /* the first entity of the pipe should be source only */ 116 if (!vimc_is_source(ved->ent)) { 117 dev_err(ved->dev, 118 "first entity in the pipe '%s' is not a source\n", 119 ved->ent->name); 120 vimc_streamer_pipeline_terminate(stream); 121 return -EPIPE; 122 } 123 return 0; 124 } 125 126 /* Get the next device in the pipeline */ 127 if (is_media_entity_v4l2_subdev(entity)) { 128 sd = media_entity_to_v4l2_subdev(entity); 129 ved = v4l2_get_subdevdata(sd); 130 } else { 131 vdev = container_of(entity, 132 struct video_device, 133 entity); 134 ved = video_get_drvdata(vdev); 135 } 136 } 137 138 vimc_streamer_pipeline_terminate(stream); 139 return -EINVAL; 140 } 141 142 /** 143 * vimc_streamer_get_sensor() - Get sensor from pipeline 144 * @stream: the pipeline 145 * 146 * Helper function to find the sensor device in the pipeline. 147 * Returns pointer to sensor device or NULL if not found. 148 */ 149 static struct vimc_sensor_device *vimc_streamer_get_sensor(struct vimc_stream *stream) 150 { 151 int i; 152 153 for (i = 0; i < stream->pipe_size; i++) { 154 struct vimc_ent_device *ved = stream->ved_pipeline[i]; 155 156 if (ved && ved->ent && 157 ved->ent->function == MEDIA_ENT_F_CAM_SENSOR) { 158 return container_of(ved, struct vimc_sensor_device, ved); 159 } 160 } 161 162 return NULL; 163 } 164 165 /** 166 * vimc_streamer_thread - Process frames through the pipeline 167 * 168 * @data: vimc_stream struct of the current stream 169 * 170 * From the source to the sink, gets a frame from each subdevice and send to 171 * the next one of the pipeline at a fixed framerate. 172 * 173 * Return: 174 * Always zero (created as ``int`` instead of ``void`` to comply with 175 * kthread API). 176 */ 177 static int vimc_streamer_thread(void *data) 178 { 179 struct vimc_stream *stream = data; 180 struct vimc_sensor_device *vsensor; 181 u8 *frame = NULL; 182 int i; 183 unsigned long fps_jiffies; 184 const unsigned long default_jiffies = HZ / 30; 185 186 set_freezable(); 187 vsensor = vimc_streamer_get_sensor(stream); 188 189 for (;;) { 190 try_to_freeze(); 191 if (kthread_should_stop()) 192 break; 193 194 /* Read from hardware configuration */ 195 fps_jiffies = vsensor ? vsensor->hw.fps_jiffies : default_jiffies; 196 197 for (i = stream->pipe_size - 1; i >= 0; i--) { 198 frame = stream->ved_pipeline[i]->process_frame( 199 stream->ved_pipeline[i], frame); 200 if (!frame || IS_ERR(frame)) 201 break; 202 } 203 set_current_state(TASK_UNINTERRUPTIBLE); 204 schedule_timeout(fps_jiffies); 205 } 206 207 return 0; 208 } 209 210 /** 211 * vimc_streamer_s_stream - Start/stop the streaming on the media pipeline 212 * 213 * @stream: the pointer to the stream structure of the current stream 214 * @ved: pointer to the vimc entity of the entity of the stream 215 * @enable: flag to determine if stream should start/stop 216 * 217 * When starting, check if there is no ``stream->kthread`` allocated. This 218 * should indicate that a stream is already running. Then, it initializes the 219 * pipeline, creates and runs a kthread to consume buffers through the pipeline. 220 * When stopping, analogously check if there is a stream running, stop the 221 * thread and terminates the pipeline. 222 * 223 * Return: 0 if success, error code otherwise. 224 */ 225 int vimc_streamer_s_stream(struct vimc_stream *stream, 226 struct vimc_ent_device *ved, 227 int enable) 228 { 229 int ret; 230 231 if (!stream || !ved) 232 return -EINVAL; 233 234 if (enable) { 235 if (stream->kthread) 236 return 0; 237 238 ret = vimc_streamer_pipeline_init(stream, ved); 239 if (ret) 240 return ret; 241 242 stream->kthread = kthread_run(vimc_streamer_thread, stream, 243 "vimc-streamer thread"); 244 245 if (IS_ERR(stream->kthread)) { 246 ret = PTR_ERR(stream->kthread); 247 dev_err(ved->dev, "kthread_run failed with %d\n", ret); 248 vimc_streamer_pipeline_terminate(stream); 249 stream->kthread = NULL; 250 return ret; 251 } 252 253 } else { 254 if (!stream->kthread) 255 return 0; 256 257 ret = kthread_stop(stream->kthread); 258 /* 259 * kthread_stop returns -EINTR in cases when streamon was 260 * immediately followed by streamoff, and the thread didn't had 261 * a chance to run. Ignore errors to stop the stream in the 262 * pipeline. 263 */ 264 if (ret) 265 dev_dbg(ved->dev, "kthread_stop returned '%d'\n", ret); 266 267 stream->kthread = NULL; 268 269 vimc_streamer_pipeline_terminate(stream); 270 } 271 272 return 0; 273 } 274