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_thread - Process frames through the pipeline 144 * 145 * @data: vimc_stream struct of the current stream 146 * 147 * From the source to the sink, gets a frame from each subdevice and send to 148 * the next one of the pipeline at a fixed framerate. 149 * 150 * Return: 151 * Always zero (created as ``int`` instead of ``void`` to comply with 152 * kthread API). 153 */ 154 static int vimc_streamer_thread(void *data) 155 { 156 struct vimc_stream *stream = data; 157 u8 *frame = NULL; 158 int i; 159 160 set_freezable(); 161 162 for (;;) { 163 try_to_freeze(); 164 if (kthread_should_stop()) 165 break; 166 167 for (i = stream->pipe_size - 1; i >= 0; i--) { 168 frame = stream->ved_pipeline[i]->process_frame( 169 stream->ved_pipeline[i], frame); 170 if (!frame || IS_ERR(frame)) 171 break; 172 } 173 //wait for 60hz 174 set_current_state(TASK_UNINTERRUPTIBLE); 175 schedule_timeout(HZ / 60); 176 } 177 178 return 0; 179 } 180 181 /** 182 * vimc_streamer_s_stream - Start/stop the streaming on the media pipeline 183 * 184 * @stream: the pointer to the stream structure of the current stream 185 * @ved: pointer to the vimc entity of the entity of the stream 186 * @enable: flag to determine if stream should start/stop 187 * 188 * When starting, check if there is no ``stream->kthread`` allocated. This 189 * should indicate that a stream is already running. Then, it initializes the 190 * pipeline, creates and runs a kthread to consume buffers through the pipeline. 191 * When stopping, analogously check if there is a stream running, stop the 192 * thread and terminates the pipeline. 193 * 194 * Return: 0 if success, error code otherwise. 195 */ 196 int vimc_streamer_s_stream(struct vimc_stream *stream, 197 struct vimc_ent_device *ved, 198 int enable) 199 { 200 int ret; 201 202 if (!stream || !ved) 203 return -EINVAL; 204 205 if (enable) { 206 if (stream->kthread) 207 return 0; 208 209 ret = vimc_streamer_pipeline_init(stream, ved); 210 if (ret) 211 return ret; 212 213 stream->kthread = kthread_run(vimc_streamer_thread, stream, 214 "vimc-streamer thread"); 215 216 if (IS_ERR(stream->kthread)) { 217 ret = PTR_ERR(stream->kthread); 218 dev_err(ved->dev, "kthread_run failed with %d\n", ret); 219 vimc_streamer_pipeline_terminate(stream); 220 stream->kthread = NULL; 221 return ret; 222 } 223 224 } else { 225 if (!stream->kthread) 226 return 0; 227 228 ret = kthread_stop(stream->kthread); 229 /* 230 * kthread_stop returns -EINTR in cases when streamon was 231 * immediately followed by streamoff, and the thread didn't had 232 * a chance to run. Ignore errors to stop the stream in the 233 * pipeline. 234 */ 235 if (ret) 236 dev_dbg(ved->dev, "kthread_stop returned '%d'\n", ret); 237 238 stream->kthread = NULL; 239 240 vimc_streamer_pipeline_terminate(stream); 241 } 242 243 return 0; 244 } 245