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 */
vimc_get_source_entity(struct media_entity * ent)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 */
vimc_streamer_pipeline_terminate(struct vimc_stream * stream)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 */
vimc_streamer_pipeline_init(struct vimc_stream * stream,struct vimc_ent_device * ved)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 */
vimc_streamer_thread(void * data)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 */
vimc_streamer_s_stream(struct vimc_stream * stream,struct vimc_ent_device * ved,int enable)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