test-drivers/vimc/vimc-streamer.c

0001 // SPDX-License-Identifier: GPL-2.0+
0002 /*
0003  * vimc-streamer.c Virtual Media Controller Driver
0004  *
0005  * Copyright (C) 2018 Lucas A. M. Magalhães <lucmaga@gmail.com>
0006  *
0007  */
0008
0009 #include <linux/init.h>
0010 #include <linux/freezer.h>
0011 #include <linux/kthread.h>
0012
0013 #include "vimc-streamer.h"
0014
0015 /**
0016  * vimc_get_source_entity - get the entity connected with the first sink pad
0017  *
0018  * @ent:    reference media_entity
0019  *
0020  * Helper function that returns the media entity containing the source pad
0021  * linked with the first sink pad from the given media entity pad list.
0022  *
0023  * Return: The source pad or NULL, if it wasn't found.
0024  */
0025 static struct media_entity *vimc_get_source_entity(struct media_entity *ent)
0026 {
0027     struct media_pad *pad;
0028     int i;
0029
0030     for (i = 0; i < ent->num_pads; i++) {
0031         if (ent->pads[i].flags & MEDIA_PAD_FL_SOURCE)
0032             continue;
0033         pad = media_pad_remote_pad_first(&ent->pads[i]);
0034         return pad ? pad->entity : NULL;
0035     }
0036     return NULL;
0037 }
0038
0039 /**
0040  * vimc_streamer_pipeline_terminate - Disable stream in all ved in stream
0041  *
0042  * @stream: the pointer to the stream structure with the pipeline to be
0043  *      disabled.
0044  *
0045  * Calls s_stream to disable the stream in each entity of the pipeline
0046  *
0047  */
0048 static void vimc_streamer_pipeline_terminate(struct vimc_stream *stream)
0049 {
0050     struct vimc_ent_device *ved;
0051     struct v4l2_subdev *sd;
0052
0053     while (stream->pipe_size) {
0054         stream->pipe_size--;
0055         ved = stream->ved_pipeline[stream->pipe_size];
0056         stream->ved_pipeline[stream->pipe_size] = NULL;
0057
0058         if (!is_media_entity_v4l2_subdev(ved->ent))
0059             continue;
0060
0061         sd = media_entity_to_v4l2_subdev(ved->ent);
0062         v4l2_subdev_call(sd, video, s_stream, 0);
0063     }
0064 }
0065
0066 /**
0067  * vimc_streamer_pipeline_init - Initializes the stream structure
0068  *
0069  * @stream: the pointer to the stream structure to be initialized
0070  * @ved:    the pointer to the vimc entity initializing the stream
0071  *
0072  * Initializes the stream structure. Walks through the entity graph to
0073  * construct the pipeline used later on the streamer thread.
0074  * Calls vimc_streamer_s_stream() to enable stream in all entities of
0075  * the pipeline.
0076  *
0077  * Return: 0 if success, error code otherwise.
0078  */
0079 static int vimc_streamer_pipeline_init(struct vimc_stream *stream,
0080                        struct vimc_ent_device *ved)
0081 {
0082     struct media_entity *entity;
0083     struct video_device *vdev;
0084     struct v4l2_subdev *sd;
0085     int ret = 0;
0086
0087     stream->pipe_size = 0;
0088     while (stream->pipe_size < VIMC_STREAMER_PIPELINE_MAX_SIZE) {
0089         if (!ved) {
0090             vimc_streamer_pipeline_terminate(stream);
0091             return -EINVAL;
0092         }
0093         stream->ved_pipeline[stream->pipe_size++] = ved;
0094
0095         if (is_media_entity_v4l2_subdev(ved->ent)) {
0096             sd = media_entity_to_v4l2_subdev(ved->ent);
0097             ret = v4l2_subdev_call(sd, video, s_stream, 1);
0098             if (ret && ret != -ENOIOCTLCMD) {
0099                 dev_err(ved->dev, "subdev_call error %s\n",
0100                     ved->ent->name);
0101                 vimc_streamer_pipeline_terminate(stream);
0102                 return ret;
0103             }
0104         }
0105
0106         entity = vimc_get_source_entity(ved->ent);
0107         /* Check if the end of the pipeline was reached */
0108         if (!entity) {
0109             /* the first entity of the pipe should be source only */
0110             if (!vimc_is_source(ved->ent)) {
0111                 dev_err(ved->dev,
0112                     "first entity in the pipe '%s' is not a source\n",
0113                     ved->ent->name);
0114                 vimc_streamer_pipeline_terminate(stream);
0115                 return -EPIPE;
0116             }
0117             return 0;
0118         }
0119
0120         /* Get the next device in the pipeline */
0121         if (is_media_entity_v4l2_subdev(entity)) {
0122             sd = media_entity_to_v4l2_subdev(entity);
0123             ved = v4l2_get_subdevdata(sd);
0124         } else {
0125             vdev = container_of(entity,
0126                         struct video_device,
0127                         entity);
0128             ved = video_get_drvdata(vdev);
0129         }
0130     }
0131
0132     vimc_streamer_pipeline_terminate(stream);
0133     return -EINVAL;
0134 }
0135
0136 /**
0137  * vimc_streamer_thread - Process frames through the pipeline
0138  *
0139  * @data:   vimc_stream struct of the current stream
0140  *
0141  * From the source to the sink, gets a frame from each subdevice and send to
0142  * the next one of the pipeline at a fixed framerate.
0143  *
0144  * Return:
0145  * Always zero (created as ``int`` instead of ``void`` to comply with
0146  * kthread API).
0147  */
0148 static int vimc_streamer_thread(void *data)
0149 {
0150     struct vimc_stream *stream = data;
0151     u8 *frame = NULL;
0152     int i;
0153
0154     set_freezable();
0155
0156     for (;;) {
0157         try_to_freeze();
0158         if (kthread_should_stop())
0159             break;
0160
0161         for (i = stream->pipe_size - 1; i >= 0; i--) {
0162             frame = stream->ved_pipeline[i]->process_frame(
0163                     stream->ved_pipeline[i], frame);
0164             if (!frame || IS_ERR(frame))
0165                 break;
0166         }
0167         //wait for 60hz
0168         set_current_state(TASK_UNINTERRUPTIBLE);
0169         schedule_timeout(HZ / 60);
0170     }
0171
0172     return 0;
0173 }
0174
0175 /**
0176  * vimc_streamer_s_stream - Start/stop the streaming on the media pipeline
0177  *
0178  * @stream: the pointer to the stream structure of the current stream
0179  * @ved:    pointer to the vimc entity of the entity of the stream
0180  * @enable: flag to determine if stream should start/stop
0181  *
0182  * When starting, check if there is no ``stream->kthread`` allocated. This
0183  * should indicate that a stream is already running. Then, it initializes the
0184  * pipeline, creates and runs a kthread to consume buffers through the pipeline.
0185  * When stopping, analogously check if there is a stream running, stop the
0186  * thread and terminates the pipeline.
0187  *
0188  * Return: 0 if success, error code otherwise.
0189  */
0190 int vimc_streamer_s_stream(struct vimc_stream *stream,
0191                struct vimc_ent_device *ved,
0192                int enable)
0193 {
0194     int ret;
0195
0196     if (!stream || !ved)
0197         return -EINVAL;
0198
0199     if (enable) {
0200         if (stream->kthread)
0201             return 0;
0202
0203         ret = vimc_streamer_pipeline_init(stream, ved);
0204         if (ret)
0205             return ret;
0206
0207         stream->kthread = kthread_run(vimc_streamer_thread, stream,
0208                           "vimc-streamer thread");
0209
0210         if (IS_ERR(stream->kthread)) {
0211             ret = PTR_ERR(stream->kthread);
0212             dev_err(ved->dev, "kthread_run failed with %d\n", ret);
0213             vimc_streamer_pipeline_terminate(stream);
0214             stream->kthread = NULL;
0215             return ret;
0216         }
0217
0218     } else {
0219         if (!stream->kthread)
0220             return 0;
0221
0222         ret = kthread_stop(stream->kthread);
0223         /*
0224          * kthread_stop returns -EINTR in cases when streamon was
0225          * immediately followed by streamoff, and the thread didn't had
0226          * a chance to run. Ignore errors to stop the stream in the
0227          * pipeline.
0228          */
0229         if (ret)
0230             dev_dbg(ved->dev, "kthread_stop returned '%d'\n", ret);
0231
0232         stream->kthread = NULL;
0233
0234         vimc_streamer_pipeline_terminate(stream);
0235     }
0236
0237     return 0;
0238 }