239 lines · c
1// SPDX-License-Identifier: GPL-2.0+2/*3 * vimc-streamer.c Virtual Media Controller Driver4 *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 pad17 *18 * @ent: reference media_entity19 *20 * Helper function that returns the media entity containing the source pad21 * 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 stream41 *42 * @stream: the pointer to the stream structure with the pipeline to be43 * disabled.44 *45 * Calls s_stream to disable the stream in each entity of the pipeline46 *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 structure68 *69 * @stream: the pointer to the stream structure to be initialized70 * @ved: the pointer to the vimc entity initializing the stream71 *72 * Initializes the stream structure. Walks through the entity graph to73 * construct the pipeline used later on the streamer thread.74 * Calls vimc_streamer_s_stream() to enable stream in all entities of75 * 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 pipeline138 *139 * @data: vimc_stream struct of the current stream140 *141 * From the source to the sink, gets a frame from each subdevice and send to142 * the next one of the pipeline at a fixed framerate.143 *144 * Return:145 * Always zero (created as ``int`` instead of ``void`` to comply with146 * 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 60hz168 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 pipeline177 *178 * @stream: the pointer to the stream structure of the current stream179 * @ved: pointer to the vimc entity of the entity of the stream180 * @enable: flag to determine if stream should start/stop181 *182 * When starting, check if there is no ``stream->kthread`` allocated. This183 * should indicate that a stream is already running. Then, it initializes the184 * pipeline, creates and runs a kthread to consume buffers through the pipeline.185 * When stopping, analogously check if there is a stream running, stop the186 * 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 was225 * immediately followed by streamoff, and the thread didn't had226 * a chance to run. Ignore errors to stop the stream in the227 * 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