1 /*
2 * Copyright (c) 2024 tinyVision.ai Inc.
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #define DT_DRV_COMPAT zephyr_video_emul_rx
8
9 #include <string.h>
10
11 #include <zephyr/kernel.h>
12 #include <zephyr/device.h>
13 #include <zephyr/sys/util.h>
14 #include <zephyr/drivers/video.h>
15 #include <zephyr/drivers/i2c.h>
16 #include <zephyr/logging/log.h>
17
18 LOG_MODULE_REGISTER(video_emul_rx, CONFIG_VIDEO_LOG_LEVEL);
19
20 struct emul_rx_config {
21 const struct device *source_dev;
22 };
23
24 struct emul_rx_data {
25 const struct device *dev;
26 struct video_format fmt;
27 struct k_work work;
28 struct k_fifo fifo_in;
29 struct k_fifo fifo_out;
30 };
31
emul_rx_set_ctrl(const struct device * dev,unsigned int cid,void * value)32 static int emul_rx_set_ctrl(const struct device *dev, unsigned int cid, void *value)
33 {
34 const struct emul_rx_config *cfg = dev->config;
35
36 /* Forward all controls to the source */
37 return video_set_ctrl(cfg->source_dev, cid, value);
38 }
39
emul_rx_get_ctrl(const struct device * dev,unsigned int cid,void * value)40 static int emul_rx_get_ctrl(const struct device *dev, unsigned int cid, void *value)
41 {
42 const struct emul_rx_config *cfg = dev->config;
43
44 /* Forward all controls to the source */
45 return video_get_ctrl(cfg->source_dev, cid, value);
46 }
47
emul_rx_set_frmival(const struct device * dev,enum video_endpoint_id ep,struct video_frmival * frmival)48 static int emul_rx_set_frmival(const struct device *dev, enum video_endpoint_id ep,
49 struct video_frmival *frmival)
50 {
51 const struct emul_rx_config *cfg = dev->config;
52
53 /* Input/output timing is driven by the source */
54 if (ep != VIDEO_EP_IN && ep != VIDEO_EP_OUT && ep != VIDEO_EP_ALL) {
55 return -EINVAL;
56 }
57 return video_set_frmival(cfg->source_dev, VIDEO_EP_OUT, frmival);
58 }
59
emul_rx_get_frmival(const struct device * dev,enum video_endpoint_id ep,struct video_frmival * frmival)60 static int emul_rx_get_frmival(const struct device *dev, enum video_endpoint_id ep,
61 struct video_frmival *frmival)
62 {
63 const struct emul_rx_config *cfg = dev->config;
64
65 /* Input/output timing is driven by the source */
66 if (ep != VIDEO_EP_IN && ep != VIDEO_EP_OUT && ep != VIDEO_EP_ALL) {
67 return -EINVAL;
68 }
69 return video_get_frmival(cfg->source_dev, VIDEO_EP_OUT, frmival);
70 }
71
emul_rx_enum_frmival(const struct device * dev,enum video_endpoint_id ep,struct video_frmival_enum * fie)72 static int emul_rx_enum_frmival(const struct device *dev, enum video_endpoint_id ep,
73 struct video_frmival_enum *fie)
74 {
75 const struct emul_rx_config *cfg = dev->config;
76
77 /* Input/output timing is driven by the source */
78 if (ep != VIDEO_EP_IN && ep != VIDEO_EP_OUT && ep != VIDEO_EP_ALL) {
79 return -EINVAL;
80 }
81 return video_enum_frmival(cfg->source_dev, VIDEO_EP_OUT, fie);
82 }
83
emul_rx_set_fmt(const struct device * const dev,enum video_endpoint_id ep,struct video_format * fmt)84 static int emul_rx_set_fmt(const struct device *const dev, enum video_endpoint_id ep,
85 struct video_format *fmt)
86 {
87 const struct emul_rx_config *cfg = dev->config;
88 struct emul_rx_data *data = dev->data;
89 int ret;
90
91 /* The same format is shared between input and output: data is just passed through */
92 if (ep != VIDEO_EP_IN && ep != VIDEO_EP_OUT && ep != VIDEO_EP_ALL) {
93 return -EINVAL;
94 }
95
96 /* Propagate the format selection to the source */
97 ret = video_set_format(cfg->source_dev, VIDEO_EP_OUT, fmt);
98 if (ret < 0) {
99 LOG_DBG("Failed to set %s format to %x %ux%u", cfg->source_dev->name,
100 fmt->pixelformat, fmt->width, fmt->height);
101 return -EINVAL;
102 }
103
104 /* Cache the format selected locally to use it for getting the size of the buffer */
105 data->fmt = *fmt;
106 return 0;
107 }
108
emul_rx_get_fmt(const struct device * dev,enum video_endpoint_id ep,struct video_format * fmt)109 static int emul_rx_get_fmt(const struct device *dev, enum video_endpoint_id ep,
110 struct video_format *fmt)
111 {
112 struct emul_rx_data *data = dev->data;
113
114 /* Input/output caps are the same as the source: data is just passed through */
115 if (ep != VIDEO_EP_IN && ep != VIDEO_EP_OUT && ep != VIDEO_EP_ALL) {
116 return -EINVAL;
117 }
118 *fmt = data->fmt;
119 return 0;
120 }
121
emul_rx_get_caps(const struct device * dev,enum video_endpoint_id ep,struct video_caps * caps)122 static int emul_rx_get_caps(const struct device *dev, enum video_endpoint_id ep,
123 struct video_caps *caps)
124 {
125 const struct emul_rx_config *cfg = dev->config;
126
127 /* Input/output caps are the same as the source: data is just passed through */
128 if (ep != VIDEO_EP_IN && ep != VIDEO_EP_OUT && ep != VIDEO_EP_ALL) {
129 return -EINVAL;
130 }
131 return video_get_caps(cfg->source_dev, VIDEO_EP_OUT, caps);
132 }
133
emul_rx_stream_start(const struct device * dev)134 static int emul_rx_stream_start(const struct device *dev)
135 {
136 const struct emul_rx_config *cfg = dev->config;
137
138 /* A real hardware driver would first start its own peripheral */
139 return video_stream_start(cfg->source_dev);
140 }
141
emul_rx_stream_stop(const struct device * dev)142 static int emul_rx_stream_stop(const struct device *dev)
143 {
144 const struct emul_rx_config *cfg = dev->config;
145
146 return video_stream_stop(cfg->source_dev);
147 /* A real hardware driver would then stop its own peripheral */
148 }
149
emul_rx_worker(struct k_work * work)150 static void emul_rx_worker(struct k_work *work)
151 {
152 struct emul_rx_data *data = CONTAINER_OF(work, struct emul_rx_data, work);
153 const struct device *dev = data->dev;
154 const struct emul_rx_config *cfg = dev->config;
155 struct video_format *fmt = &data->fmt;
156 struct video_buffer *vbuf = vbuf;
157
158 LOG_DBG("Queueing a frame of %u bytes in format %x %ux%u", fmt->pitch * fmt->height,
159 fmt->pixelformat, fmt->width, fmt->height);
160
161 while ((vbuf = k_fifo_get(&data->fifo_in, K_NO_WAIT)) != NULL) {
162 vbuf->bytesused = fmt->pitch * fmt->height;
163 vbuf->line_offset = 0;
164
165 LOG_DBG("Inserting %u bytes into buffer %p", vbuf->bytesused, vbuf->buffer);
166
167 /* Simulate the MIPI/DVP hardware transferring image data from the imager to the
168 * video buffer memory using DMA. The vbuf->size is checked in emul_rx_enqueue().
169 */
170 memcpy(vbuf->buffer, cfg->source_dev->data, vbuf->bytesused);
171
172 /* Once the buffer is completed, submit it to the video buffer */
173 k_fifo_put(&data->fifo_out, vbuf);
174 }
175 }
176
emul_rx_enqueue(const struct device * dev,enum video_endpoint_id ep,struct video_buffer * vbuf)177 static int emul_rx_enqueue(const struct device *dev, enum video_endpoint_id ep,
178 struct video_buffer *vbuf)
179 {
180 struct emul_rx_data *data = dev->data;
181 struct video_format *fmt = &data->fmt;
182
183 /* Can only enqueue a buffer to get data out, data input is from hardware */
184 if (ep != VIDEO_EP_OUT && ep != VIDEO_EP_ALL) {
185 return -EINVAL;
186 }
187
188 if (vbuf->size < fmt->pitch * fmt->height) {
189 LOG_ERR("Buffer too small for a full frame");
190 return -ENOMEM;
191 }
192
193 /* The buffer has not been filled yet: flag as emtpy */
194 vbuf->bytesused = 0;
195
196 /* Submit the buffer for processing in the worker, where everything happens */
197 k_fifo_put(&data->fifo_in, vbuf);
198 k_work_submit(&data->work);
199
200 return 0;
201 }
202
emul_rx_dequeue(const struct device * dev,enum video_endpoint_id ep,struct video_buffer ** vbufp,k_timeout_t timeout)203 static int emul_rx_dequeue(const struct device *dev, enum video_endpoint_id ep,
204 struct video_buffer **vbufp, k_timeout_t timeout)
205 {
206 struct emul_rx_data *data = dev->data;
207
208 /* Can only dequeue a buffer to get data out, data input is from hardware */
209 if (ep != VIDEO_EP_OUT && ep != VIDEO_EP_ALL) {
210 return -EINVAL;
211 }
212
213 /* All the processing is expected to happen in the worker */
214 *vbufp = k_fifo_get(&data->fifo_out, timeout);
215 if (*vbufp == NULL) {
216 return -EAGAIN;
217 }
218
219 return 0;
220 }
221
emul_rx_flush(const struct device * dev,enum video_endpoint_id ep,bool cancel)222 static int emul_rx_flush(const struct device *dev, enum video_endpoint_id ep, bool cancel)
223 {
224 struct emul_rx_data *data = dev->data;
225 struct k_work_sync sync;
226
227 /* Can only flush the buffer going out, data input is from hardware */
228 if (ep != VIDEO_EP_OUT && ep != VIDEO_EP_ALL) {
229 return -EINVAL;
230 }
231
232 if (cancel) {
233 struct video_buffer *vbuf;
234
235 /* First, stop the hardware processing */
236 emul_rx_stream_stop(dev);
237
238 /* Cancel the jobs that were not running */
239 k_work_cancel(&data->work);
240
241 /* Flush the jobs that were still running */
242 k_work_flush(&data->work, &sync);
243
244 /* Empty all the cancelled items */
245 while ((vbuf = k_fifo_get(&data->fifo_in, K_NO_WAIT))) {
246 k_fifo_put(&data->fifo_out, vbuf);
247 }
248 } else {
249 /* Process all the remaining items from the queue */
250 k_work_flush(&data->work, &sync);
251 }
252
253 return 0;
254 }
255
256 static DEVICE_API(video, emul_rx_driver_api) = {
257 .set_ctrl = emul_rx_set_ctrl,
258 .get_ctrl = emul_rx_get_ctrl,
259 .set_frmival = emul_rx_set_frmival,
260 .get_frmival = emul_rx_get_frmival,
261 .enum_frmival = emul_rx_enum_frmival,
262 .set_format = emul_rx_set_fmt,
263 .get_format = emul_rx_get_fmt,
264 .get_caps = emul_rx_get_caps,
265 .stream_start = emul_rx_stream_start,
266 .stream_stop = emul_rx_stream_stop,
267 .enqueue = emul_rx_enqueue,
268 .dequeue = emul_rx_dequeue,
269 .flush = emul_rx_flush,
270 };
271
emul_rx_init(const struct device * dev)272 int emul_rx_init(const struct device *dev)
273 {
274 struct emul_rx_data *data = dev->data;
275 const struct emul_rx_config *cfg = dev->config;
276 int ret;
277
278 data->dev = dev;
279
280 if (!device_is_ready(cfg->source_dev)) {
281 LOG_ERR("Source device %s is not ready", cfg->source_dev->name);
282 return -ENODEV;
283 }
284
285 ret = video_get_format(cfg->source_dev, VIDEO_EP_OUT, &data->fmt);
286 if (ret < 0) {
287 return ret;
288 }
289
290 k_fifo_init(&data->fifo_in);
291 k_fifo_init(&data->fifo_out);
292 k_work_init(&data->work, &emul_rx_worker);
293
294 return 0;
295 }
296
297 #define EMUL_RX_DEFINE(n) \
298 static const struct emul_rx_config emul_rx_cfg_##n = { \
299 .source_dev = \
300 DEVICE_DT_GET(DT_NODE_REMOTE_DEVICE(DT_INST_ENDPOINT_BY_ID(n, 0, 0))), \
301 }; \
302 \
303 static struct emul_rx_data emul_rx_data_##n = { \
304 .dev = DEVICE_DT_INST_GET(n), \
305 }; \
306 \
307 DEVICE_DT_INST_DEFINE(n, &emul_rx_init, NULL, &emul_rx_data_##n, &emul_rx_cfg_##n, \
308 POST_KERNEL, CONFIG_VIDEO_INIT_PRIORITY, &emul_rx_driver_api);
309
310 DT_INST_FOREACH_STATUS_OKAY(EMUL_RX_DEFINE)
311
