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 */
 26static 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 */
 49static 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 */
 80static 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 */
140static 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 */
182int 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}
220EXPORT_SYMBOL_GPL(vimc_streamer_s_stream);