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(pad: &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(entity: 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 vimc_streamer_s_stream() to enable stream in all entities of |
75 | * the pipeline. |
76 | * |
77 | * Return: 0 if success, error code otherwise. |
78 | */ |
79 | static int vimc_streamer_pipeline_init(struct vimc_stream *stream, |
80 | struct vimc_ent_device *ved) |
81 | { |
82 | struct media_entity *entity; |
83 | struct video_device *vdev; |
84 | struct v4l2_subdev *sd; |
85 | int ret = 0; |
86 | |
87 | stream->pipe_size = 0; |
88 | while (stream->pipe_size < VIMC_STREAMER_PIPELINE_MAX_SIZE) { |
89 | if (!ved) { |
90 | vimc_streamer_pipeline_terminate(stream); |
91 | return -EINVAL; |
92 | } |
93 | stream->ved_pipeline[stream->pipe_size++] = ved; |
94 | |
95 | if (is_media_entity_v4l2_subdev(entity: ved->ent)) { |
96 | sd = media_entity_to_v4l2_subdev(ved->ent); |
97 | ret = v4l2_subdev_call(sd, video, s_stream, 1); |
98 | if (ret && ret != -ENOIOCTLCMD) { |
99 | dev_err(ved->dev, "subdev_call error %s\n" , |
100 | ved->ent->name); |
101 | vimc_streamer_pipeline_terminate(stream); |
102 | return ret; |
103 | } |
104 | } |
105 | |
106 | entity = vimc_get_source_entity(ent: ved->ent); |
107 | /* Check if the end of the pipeline was reached */ |
108 | if (!entity) { |
109 | /* the first entity of the pipe should be source only */ |
110 | if (!vimc_is_source(ent: ved->ent)) { |
111 | dev_err(ved->dev, |
112 | "first entity in the pipe '%s' is not a source\n" , |
113 | ved->ent->name); |
114 | vimc_streamer_pipeline_terminate(stream); |
115 | return -EPIPE; |
116 | } |
117 | return 0; |
118 | } |
119 | |
120 | /* Get the next device in the pipeline */ |
121 | if (is_media_entity_v4l2_subdev(entity)) { |
122 | sd = media_entity_to_v4l2_subdev(entity); |
123 | ved = v4l2_get_subdevdata(sd); |
124 | } else { |
125 | vdev = container_of(entity, |
126 | struct video_device, |
127 | entity); |
128 | ved = video_get_drvdata(vdev); |
129 | } |
130 | } |
131 | |
132 | vimc_streamer_pipeline_terminate(stream); |
133 | return -EINVAL; |
134 | } |
135 | |
136 | /** |
137 | * vimc_streamer_thread - Process frames through the pipeline |
138 | * |
139 | * @data: vimc_stream struct of the current stream |
140 | * |
141 | * From the source to the sink, gets a frame from each subdevice and send to |
142 | * the next one of the pipeline at a fixed framerate. |
143 | * |
144 | * Return: |
145 | * Always zero (created as ``int`` instead of ``void`` to comply with |
146 | * kthread API). |
147 | */ |
148 | static int vimc_streamer_thread(void *data) |
149 | { |
150 | struct vimc_stream *stream = data; |
151 | u8 *frame = NULL; |
152 | int i; |
153 | |
154 | set_freezable(); |
155 | |
156 | for (;;) { |
157 | try_to_freeze(); |
158 | if (kthread_should_stop()) |
159 | break; |
160 | |
161 | for (i = stream->pipe_size - 1; i >= 0; i--) { |
162 | frame = stream->ved_pipeline[i]->process_frame( |
163 | stream->ved_pipeline[i], frame); |
164 | if (!frame || IS_ERR(ptr: frame)) |
165 | break; |
166 | } |
167 | //wait for 60hz |
168 | set_current_state(TASK_UNINTERRUPTIBLE); |
169 | schedule_timeout(HZ / 60); |
170 | } |
171 | |
172 | return 0; |
173 | } |
174 | |
175 | /** |
176 | * vimc_streamer_s_stream - Start/stop the streaming on the media pipeline |
177 | * |
178 | * @stream: the pointer to the stream structure of the current stream |
179 | * @ved: pointer to the vimc entity of the entity of the stream |
180 | * @enable: flag to determine if stream should start/stop |
181 | * |
182 | * When starting, check if there is no ``stream->kthread`` allocated. This |
183 | * should indicate that a stream is already running. Then, it initializes the |
184 | * pipeline, creates and runs a kthread to consume buffers through the pipeline. |
185 | * When stopping, analogously check if there is a stream running, stop the |
186 | * thread and terminates the pipeline. |
187 | * |
188 | * Return: 0 if success, error code otherwise. |
189 | */ |
190 | int vimc_streamer_s_stream(struct vimc_stream *stream, |
191 | struct vimc_ent_device *ved, |
192 | int enable) |
193 | { |
194 | int ret; |
195 | |
196 | if (!stream || !ved) |
197 | return -EINVAL; |
198 | |
199 | if (enable) { |
200 | if (stream->kthread) |
201 | return 0; |
202 | |
203 | ret = vimc_streamer_pipeline_init(stream, ved); |
204 | if (ret) |
205 | return ret; |
206 | |
207 | stream->kthread = kthread_run(vimc_streamer_thread, stream, |
208 | "vimc-streamer thread" ); |
209 | |
210 | if (IS_ERR(ptr: stream->kthread)) { |
211 | ret = PTR_ERR(ptr: stream->kthread); |
212 | dev_err(ved->dev, "kthread_run failed with %d\n" , ret); |
213 | vimc_streamer_pipeline_terminate(stream); |
214 | stream->kthread = NULL; |
215 | return ret; |
216 | } |
217 | |
218 | } else { |
219 | if (!stream->kthread) |
220 | return 0; |
221 | |
222 | ret = kthread_stop(k: stream->kthread); |
223 | /* |
224 | * kthread_stop returns -EINTR in cases when streamon was |
225 | * immediately followed by streamoff, and the thread didn't had |
226 | * a chance to run. Ignore errors to stop the stream in the |
227 | * pipeline. |
228 | */ |
229 | if (ret) |
230 | dev_dbg(ved->dev, "kthread_stop returned '%d'\n" , ret); |
231 | |
232 | stream->kthread = NULL; |
233 | |
234 | vimc_streamer_pipeline_terminate(stream); |
235 | } |
236 | |
237 | return 0; |
238 | } |
239 | |