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 */
vimc_get_source_entity(struct media_entity * ent)24 static struct media_entity *vimc_get_source_entity(struct media_entity *ent)
25 {
26 struct media_pad *pad;
27 int i;
28
29 for (i = 0; i < ent->num_pads; i++) {
30 if (ent->pads[i].flags & MEDIA_PAD_FL_SOURCE)
31 continue;
32 pad = media_entity_remote_pad(&ent->pads[i]);
33 return pad ? pad->entity : NULL;
34 }
35 return NULL;
36 }
37
38 /*
39 * vimc_streamer_pipeline_terminate - Disable stream in all ved in stream
40 *
41 * @stream: the pointer to the stream structure with the pipeline to be
42 * disabled.
43 *
44 * Calls s_stream to disable the stream in each entity of the pipeline
45 *
46 */
vimc_streamer_pipeline_terminate(struct vimc_stream * stream)47 static void vimc_streamer_pipeline_terminate(struct vimc_stream *stream)
48 {
49 struct media_entity *entity;
50 struct v4l2_subdev *sd;
51
52 while (stream->pipe_size) {
53 stream->pipe_size--;
54 entity = stream->ved_pipeline[stream->pipe_size]->ent;
55 entity = vimc_get_source_entity(entity);
56 stream->ved_pipeline[stream->pipe_size] = NULL;
57
58 if (!is_media_entity_v4l2_subdev(entity))
59 continue;
60
61 sd = media_entity_to_v4l2_subdev(entity);
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 s_stream to enable stream in all entities of the pipeline.
75 */
vimc_streamer_pipeline_init(struct vimc_stream * stream,struct vimc_ent_device * ved)76 static int vimc_streamer_pipeline_init(struct vimc_stream *stream,
77 struct vimc_ent_device *ved)
78 {
79 struct media_entity *entity;
80 struct video_device *vdev;
81 struct v4l2_subdev *sd;
82 int ret = 0;
83
84 stream->pipe_size = 0;
85 while (stream->pipe_size < VIMC_STREAMER_PIPELINE_MAX_SIZE) {
86 if (!ved) {
87 vimc_streamer_pipeline_terminate(stream);
88 return -EINVAL;
89 }
90 stream->ved_pipeline[stream->pipe_size++] = ved;
91
92 entity = vimc_get_source_entity(ved->ent);
93 /* Check if the end of the pipeline was reached*/
94 if (!entity)
95 return 0;
96
97 if (is_media_entity_v4l2_subdev(entity)) {
98 sd = media_entity_to_v4l2_subdev(entity);
99 ret = v4l2_subdev_call(sd, video, s_stream, 1);
100 if (ret && ret != -ENOIOCTLCMD) {
101 vimc_streamer_pipeline_terminate(stream);
102 return ret;
103 }
104 ved = v4l2_get_subdevdata(sd);
105 } else {
106 vdev = container_of(entity,
107 struct video_device,
108 entity);
109 ved = video_get_drvdata(vdev);
110 }
111 }
112
113 vimc_streamer_pipeline_terminate(stream);
114 return -EINVAL;
115 }
116
vimc_streamer_thread(void * data)117 static int vimc_streamer_thread(void *data)
118 {
119 struct vimc_stream *stream = data;
120 int i;
121
122 set_freezable();
123
124 for (;;) {
125 try_to_freeze();
126 if (kthread_should_stop())
127 break;
128
129 for (i = stream->pipe_size - 1; i >= 0; i--) {
130 stream->frame = stream->ved_pipeline[i]->process_frame(
131 stream->ved_pipeline[i],
132 stream->frame);
133 if (!stream->frame)
134 break;
135 if (IS_ERR(stream->frame))
136 break;
137 }
138 //wait for 60hz
139 set_current_state(TASK_UNINTERRUPTIBLE);
140 schedule_timeout(HZ / 60);
141 }
142
143 return 0;
144 }
145
vimc_streamer_s_stream(struct vimc_stream * stream,struct vimc_ent_device * ved,int enable)146 int vimc_streamer_s_stream(struct vimc_stream *stream,
147 struct vimc_ent_device *ved,
148 int enable)
149 {
150 int ret;
151
152 if (!stream || !ved)
153 return -EINVAL;
154
155 if (enable) {
156 if (stream->kthread)
157 return 0;
158
159 ret = vimc_streamer_pipeline_init(stream, ved);
160 if (ret)
161 return ret;
162
163 stream->kthread = kthread_run(vimc_streamer_thread, stream,
164 "vimc-streamer thread");
165
166 if (IS_ERR(stream->kthread))
167 return PTR_ERR(stream->kthread);
168
169 } else {
170 if (!stream->kthread)
171 return 0;
172
173 ret = kthread_stop(stream->kthread);
174 if (ret)
175 return ret;
176
177 stream->kthread = NULL;
178
179 vimc_streamer_pipeline_terminate(stream);
180 }
181
182 return 0;
183 }
184 EXPORT_SYMBOL_GPL(vimc_streamer_s_stream);
185
186 MODULE_DESCRIPTION("Virtual Media Controller Driver (VIMC) Streamer");
187 MODULE_AUTHOR("Lucas A. M. Magalhães <lucmaga@gmail.com>");
188 MODULE_LICENSE("GPL");
189