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 */
25static 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 */
48static 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 */
79static 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 */
148static 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 */
190int 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