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 		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(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(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(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(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(stream->kthread)) {
211 			ret = PTR_ERR(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(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