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/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 * Return: The source pad or NULL, if it wasn't found.
25 */
vimc_get_source_entity(struct media_entity * ent)26 static struct media_entity *vimc_get_source_entity(struct media_entity *ent)
27 {
28 struct media_pad *pad;
29 int i;
30
31 for (i = 0; i < ent->num_pads; i++) {
32 if (ent->pads[i].flags & MEDIA_PAD_FL_SOURCE)
33 continue;
34 pad = media_entity_remote_pad(&ent->pads[i]);
35 return pad ? pad->entity : NULL;
36 }
37 return NULL;
38 }
39
40 /**
41 * vimc_streamer_pipeline_terminate - Disable stream in all ved in stream
42 *
43 * @stream: the pointer to the stream structure with the pipeline to be
44 * disabled.
45 *
46 * Calls s_stream to disable the stream in each entity of the pipeline
47 *
48 */
vimc_streamer_pipeline_terminate(struct vimc_stream * stream)49 static void vimc_streamer_pipeline_terminate(struct vimc_stream *stream)
50 {
51 struct vimc_ent_device *ved;
52 struct v4l2_subdev *sd;
53
54 while (stream->pipe_size) {
55 stream->pipe_size--;
56 ved = stream->ved_pipeline[stream->pipe_size];
57 stream->ved_pipeline[stream->pipe_size] = NULL;
58
59 if (!is_media_entity_v4l2_subdev(ved->ent))
60 continue;
61
62 sd = media_entity_to_v4l2_subdev(ved->ent);
63 v4l2_subdev_call(sd, video, s_stream, 0);
64 }
65 }
66
67 /**
68 * vimc_streamer_pipeline_init - Initializes the stream structure
69 *
70 * @stream: the pointer to the stream structure to be initialized
71 * @ved: the pointer to the vimc entity initializing the stream
72 *
73 * Initializes the stream structure. Walks through the entity graph to
74 * construct the pipeline used later on the streamer thread.
75 * Calls vimc_streamer_s_stream() to enable stream in all entities of
76 * the pipeline.
77 *
78 * Return: 0 if success, error code otherwise.
79 */
vimc_streamer_pipeline_init(struct vimc_stream * stream,struct vimc_ent_device * ved)80 static int vimc_streamer_pipeline_init(struct vimc_stream *stream,
81 struct vimc_ent_device *ved)
82 {
83 struct media_entity *entity;
84 struct video_device *vdev;
85 struct v4l2_subdev *sd;
86 int ret = 0;
87
88 stream->pipe_size = 0;
89 while (stream->pipe_size < VIMC_STREAMER_PIPELINE_MAX_SIZE) {
90 if (!ved) {
91 vimc_streamer_pipeline_terminate(stream);
92 return -EINVAL;
93 }
94 stream->ved_pipeline[stream->pipe_size++] = ved;
95
96 if (is_media_entity_v4l2_subdev(ved->ent)) {
97 sd = media_entity_to_v4l2_subdev(ved->ent);
98 ret = v4l2_subdev_call(sd, video, s_stream, 1);
99 if (ret && ret != -ENOIOCTLCMD) {
100 pr_err("subdev_call error %s\n",
101 ved->ent->name);
102 vimc_streamer_pipeline_terminate(stream);
103 return ret;
104 }
105 }
106
107 entity = vimc_get_source_entity(ved->ent);
108 /* Check if the end of the pipeline was reached*/
109 if (!entity)
110 return 0;
111
112 /* Get the next device in the pipeline */
113 if (is_media_entity_v4l2_subdev(entity)) {
114 sd = media_entity_to_v4l2_subdev(entity);
115 ved = v4l2_get_subdevdata(sd);
116 } else {
117 vdev = container_of(entity,
118 struct video_device,
119 entity);
120 ved = video_get_drvdata(vdev);
121 }
122 }
123
124 vimc_streamer_pipeline_terminate(stream);
125 return -EINVAL;
126 }
127
128 /**
129 * vimc_streamer_thread - Process frames through the pipeline
130 *
131 * @data: vimc_stream struct of the current stream
132 *
133 * From the source to the sink, gets a frame from each subdevice and send to
134 * the next one of the pipeline at a fixed framerate.
135 *
136 * Return:
137 * Always zero (created as ``int`` instead of ``void`` to comply with
138 * kthread API).
139 */
vimc_streamer_thread(void * data)140 static int vimc_streamer_thread(void *data)
141 {
142 struct vimc_stream *stream = data;
143 u8 *frame = NULL;
144 int i;
145
146 set_freezable();
147
148 for (;;) {
149 try_to_freeze();
150 if (kthread_should_stop())
151 break;
152
153 for (i = stream->pipe_size - 1; i >= 0; i--) {
154 frame = stream->ved_pipeline[i]->process_frame(
155 stream->ved_pipeline[i], frame);
156 if (!frame || IS_ERR(frame))
157 break;
158 }
159 //wait for 60hz
160 set_current_state(TASK_UNINTERRUPTIBLE);
161 schedule_timeout(HZ / 60);
162 }
163
164 return 0;
165 }
166
167 /**
168 * vimc_streamer_s_stream - Start/stop the streaming on the media pipeline
169 *
170 * @stream: the pointer to the stream structure of the current stream
171 * @ved: pointer to the vimc entity of the entity of the stream
172 * @enable: flag to determine if stream should start/stop
173 *
174 * When starting, check if there is no ``stream->kthread`` allocated. This
175 * should indicate that a stream is already running. Then, it initializes the
176 * pipeline, creates and runs a kthread to consume buffers through the pipeline.
177 * When stopping, analogously check if there is a stream running, stop the
178 * thread and terminates the pipeline.
179 *
180 * Return: 0 if success, error code otherwise.
181 */
vimc_streamer_s_stream(struct vimc_stream * stream,struct vimc_ent_device * ved,int enable)182 int vimc_streamer_s_stream(struct vimc_stream *stream,
183 struct vimc_ent_device *ved,
184 int enable)
185 {
186 int ret;
187
188 if (!stream || !ved)
189 return -EINVAL;
190
191 if (enable) {
192 if (stream->kthread)
193 return 0;
194
195 ret = vimc_streamer_pipeline_init(stream, ved);
196 if (ret)
197 return ret;
198
199 stream->kthread = kthread_run(vimc_streamer_thread, stream,
200 "vimc-streamer thread");
201
202 if (IS_ERR(stream->kthread))
203 return PTR_ERR(stream->kthread);
204
205 } else {
206 if (!stream->kthread)
207 return 0;
208
209 ret = kthread_stop(stream->kthread);
210 if (ret)
211 return ret;
212
213 stream->kthread = NULL;
214
215 vimc_streamer_pipeline_terminate(stream);
216 }
217
218 return 0;
219 }
220 EXPORT_SYMBOL_GPL(vimc_streamer_s_stream);
221