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_imager
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/sys/byteorder.h>
15 #include <zephyr/drivers/video.h>
16 #include <zephyr/drivers/video-controls.h>
17 #include <zephyr/drivers/i2c.h>
18 #include <zephyr/logging/log.h>
19
20 LOG_MODULE_REGISTER(video_emul_imager, CONFIG_VIDEO_LOG_LEVEL);
21
22 #define EMUL_IMAGER_REG_SENSOR_ID 0x0000
23 #define EMUL_IMAGER_SENSOR_ID 0x99
24 #define EMUL_IMAGER_REG_CTRL 0x0001
25 #define EMUL_IMAGER_REG_INIT1 0x0002
26 #define EMUL_IMAGER_REG_INIT2 0x0003
27 #define EMUL_IMAGER_REG_TIMING1 0x0004
28 #define EMUL_IMAGER_REG_TIMING2 0x0005
29 #define EMUL_IMAGER_REG_TIMING3 0x0006
30 #define EMUL_IMAGER_REG_EXPOSURE 0x0007
31 #define EMUL_IMAGER_REG_GAIN 0x0008
32 #define EMUL_IMAGER_REG_PATTERN 0x0009
33 #define EMUL_IMAGER_PATTERN_OFF 0x00
34 #define EMUL_IMAGER_PATTERN_BARS1 0x01
35 #define EMUL_IMAGER_PATTERN_BARS2 0x02
36
37 /* Emulated register bank */
38 uint8_t emul_imager_fake_regs[10];
39
40 enum emul_imager_fmt_id {
41 RGB565_64x20,
42 YUYV_64x20,
43 };
44
45 struct emul_imager_reg {
46 uint16_t addr;
47 uint8_t value;
48 };
49
50 struct emul_imager_mode {
51 uint8_t fps;
52 /* List of registers lists to configure the various properties of the sensor.
53 * This permits to deduplicate the list of registers in case some lare sections
54 * are repeated across modes, such as the resolution for different FPS.
55 */
56 const struct emul_imager_reg *regs[2];
57 /* More fields can be added according to the needs of the sensor driver */
58 };
59
60 struct emul_imager_config {
61 struct i2c_dt_spec i2c;
62 };
63
64 struct emul_imager_data {
65 /* First field is a framebuffer for I/O emulation purpose */
66 uint8_t framebuffer[CONFIG_VIDEO_EMUL_IMAGER_FRAMEBUFFER_SIZE];
67 /* Other fields are shared with real hardware drivers */
68 const struct emul_imager_mode *mode;
69 enum emul_imager_fmt_id fmt_id;
70 struct video_format fmt;
71 };
72
73 /* Initial parameters of the sensors common to all modes. */
74 static const struct emul_imager_reg emul_imager_init_regs[] = {
75 {EMUL_IMAGER_REG_CTRL, 0x00},
76 /* Example comment about REG_INIT1 */
77 {EMUL_IMAGER_REG_INIT1, 0x10},
78 {EMUL_IMAGER_REG_INIT2, 0x00},
79 {0},
80 };
81
82 /* List of registers aggregated together in "modes" that can be applied
83 * to set the timing parameters and other mode-dependent configuration.
84 */
85
86 static const struct emul_imager_reg emul_imager_rgb565_64x20[] = {
87 {EMUL_IMAGER_REG_TIMING1, 0x64},
88 {EMUL_IMAGER_REG_TIMING2, 0x20},
89 {0},
90 };
91 static const struct emul_imager_reg emul_imager_rgb565_64x20_15fps[] = {
92 {EMUL_IMAGER_REG_TIMING3, 15},
93 {0},
94 };
95 static const struct emul_imager_reg emul_imager_rgb565_64x20_30fps[] = {
96 {EMUL_IMAGER_REG_TIMING3, 30},
97 {0},
98 };
99 static const struct emul_imager_reg emul_imager_rgb565_64x20_60fps[] = {
100 {EMUL_IMAGER_REG_TIMING3, 60},
101 {0},
102 };
103 struct emul_imager_mode emul_imager_rgb565_64x20_modes[] = {
104 {.fps = 15, .regs = {emul_imager_rgb565_64x20, emul_imager_rgb565_64x20_15fps}},
105 {.fps = 30, .regs = {emul_imager_rgb565_64x20, emul_imager_rgb565_64x20_30fps}},
106 {.fps = 60, .regs = {emul_imager_rgb565_64x20, emul_imager_rgb565_64x20_60fps}},
107 {0},
108 };
109
110 static const struct emul_imager_reg emul_imager_yuyv_64x20[] = {
111 {EMUL_IMAGER_REG_TIMING1, 0x64},
112 {EMUL_IMAGER_REG_TIMING2, 0x20},
113 {0},
114 };
115 static const struct emul_imager_reg emul_imager_yuyv_64x20_15fps[] = {
116 {EMUL_IMAGER_REG_TIMING3, 15},
117 {0},
118 };
119 static const struct emul_imager_reg emul_imager_yuyv_64x20_30fps[] = {
120 {EMUL_IMAGER_REG_TIMING3, 30},
121 {0},
122 };
123 struct emul_imager_mode emul_imager_yuyv_64x20_modes[] = {
124 {.fps = 15, .regs = {emul_imager_yuyv_64x20, emul_imager_yuyv_64x20_15fps}},
125 {.fps = 30, .regs = {emul_imager_yuyv_64x20, emul_imager_yuyv_64x20_30fps}},
126 {0},
127 };
128
129 /* Summary of all the modes of all the frame formats, with the format ID as
130 * index, matching fmts[].
131 */
132 static const struct emul_imager_mode *emul_imager_modes[] = {
133 [RGB565_64x20] = emul_imager_rgb565_64x20_modes,
134 [YUYV_64x20] = emul_imager_yuyv_64x20_modes,
135 };
136
137 /* Video device capabilities where the supported resolutions and pixel formats are listed.
138 * The format ID is used as index to fetch the matching mode from the list above.
139 */
140 #define EMUL_IMAGER_VIDEO_FORMAT_CAP(width, height, format) \
141 { \
142 .pixelformat = (format), \
143 .width_min = (width), \
144 .width_max = (width), \
145 .height_min = (height), \
146 .height_max = (height), \
147 .width_step = 0, \
148 .height_step = 0, \
149 }
150 static const struct video_format_cap fmts[] = {
151 [RGB565_64x20] = EMUL_IMAGER_VIDEO_FORMAT_CAP(64, 20, VIDEO_PIX_FMT_RGB565),
152 [YUYV_64x20] = EMUL_IMAGER_VIDEO_FORMAT_CAP(64, 20, VIDEO_PIX_FMT_YUYV),
153 {0},
154 };
155
156 /* Emulated I2C register interface, to replace with actual I2C calls for real hardware */
emul_imager_read_reg(const struct device * const dev,uint8_t reg_addr,uint8_t * value)157 static int emul_imager_read_reg(const struct device *const dev, uint8_t reg_addr, uint8_t *value)
158 {
159 LOG_DBG("%s placeholder for I2C read from 0x%02x", dev->name, reg_addr);
160 switch (reg_addr) {
161 case EMUL_IMAGER_REG_SENSOR_ID:
162 *value = EMUL_IMAGER_SENSOR_ID;
163 break;
164 default:
165 *value = emul_imager_fake_regs[reg_addr];
166 }
167 return 0;
168 }
169
170 /* Helper to read a full integer directly from a register */
emul_imager_read_int(const struct device * const dev,uint8_t reg_addr,int * value)171 static int emul_imager_read_int(const struct device *const dev, uint8_t reg_addr, int *value)
172 {
173 uint8_t val8;
174 int ret;
175
176 ret = emul_imager_read_reg(dev, reg_addr, &val8);
177 *value = val8;
178 return ret;
179 }
180
181 /* Some sensors will need reg8 or reg16 variants. */
emul_imager_write_reg(const struct device * const dev,uint8_t reg_addr,uint8_t value)182 static int emul_imager_write_reg(const struct device *const dev, uint8_t reg_addr, uint8_t value)
183 {
184 LOG_DBG("%s placeholder for I2C write 0x%08x to 0x%02x", dev->name, value, reg_addr);
185 emul_imager_fake_regs[reg_addr] = value;
186 return 0;
187 }
188
emul_imager_write_multi(const struct device * const dev,const struct emul_imager_reg * regs)189 static int emul_imager_write_multi(const struct device *const dev,
190 const struct emul_imager_reg *regs)
191 {
192 int ret;
193
194 for (int i = 0; regs[i].addr != 0; i++) {
195 ret = emul_imager_write_reg(dev, regs[i].addr, regs[i].value);
196 if (ret < 0) {
197 return ret;
198 }
199 }
200 return 0;
201 }
202
emul_imager_set_ctrl(const struct device * dev,unsigned int cid,void * value)203 static int emul_imager_set_ctrl(const struct device *dev, unsigned int cid, void *value)
204 {
205 switch (cid) {
206 case VIDEO_CID_EXPOSURE:
207 return emul_imager_write_reg(dev, EMUL_IMAGER_REG_EXPOSURE, (int)value);
208 case VIDEO_CID_GAIN:
209 return emul_imager_write_reg(dev, EMUL_IMAGER_REG_GAIN, (int)value);
210 case VIDEO_CID_TEST_PATTERN:
211 return emul_imager_write_reg(dev, EMUL_IMAGER_REG_PATTERN, (int)value);
212 default:
213 return -ENOTSUP;
214 }
215 }
216
emul_imager_get_ctrl(const struct device * dev,unsigned int cid,void * value)217 static int emul_imager_get_ctrl(const struct device *dev, unsigned int cid, void *value)
218 {
219 struct emul_imager_data *data = dev->data;
220
221 switch (cid) {
222 case VIDEO_CID_EXPOSURE:
223 return emul_imager_read_int(dev, EMUL_IMAGER_REG_EXPOSURE, value);
224 case VIDEO_CID_GAIN:
225 return emul_imager_read_int(dev, EMUL_IMAGER_REG_GAIN, value);
226 case VIDEO_CID_TEST_PATTERN:
227 return emul_imager_read_int(dev, EMUL_IMAGER_REG_PATTERN, value);
228 case VIDEO_CID_PIXEL_RATE:
229 *(int64_t *)value = (int64_t)data->fmt.width * data->fmt.pitch * data->mode->fps;
230 return 0;
231 default:
232 return -ENOTSUP;
233 }
234 }
235
236 /* Customize this function according to your "struct emul_imager_mode". */
emul_imager_set_mode(const struct device * dev,const struct emul_imager_mode * mode)237 static int emul_imager_set_mode(const struct device *dev, const struct emul_imager_mode *mode)
238 {
239 struct emul_imager_data *data = dev->data;
240 int ret;
241
242 if (data->mode == mode) {
243 return 0;
244 }
245
246 LOG_DBG("Applying mode %p at %d FPS", mode, mode->fps);
247
248 /* Apply all the configuration registers for that mode */
249 for (int i = 0; i < 2; i++) {
250 ret = emul_imager_write_multi(dev, mode->regs[i]);
251 if (ret < 0) {
252 goto err;
253 }
254 }
255
256 data->mode = mode;
257 return 0;
258 err:
259 LOG_ERR("Could not apply %s mode %p (%u FPS)", dev->name, mode, mode->fps);
260 return ret;
261 }
262
emul_imager_set_frmival(const struct device * dev,enum video_endpoint_id ep,struct video_frmival * frmival)263 static int emul_imager_set_frmival(const struct device *dev, enum video_endpoint_id ep,
264 struct video_frmival *frmival)
265 {
266 struct emul_imager_data *data = dev->data;
267 struct video_frmival_enum fie = {.format = &data->fmt, .discrete = *frmival};
268
269 if (ep != VIDEO_EP_OUT && ep != VIDEO_EP_ALL) {
270 return -EINVAL;
271 }
272
273 video_closest_frmival(dev, ep, &fie);
274 LOG_DBG("Applying frame interval number %u", fie.index);
275 return emul_imager_set_mode(dev, &emul_imager_modes[data->fmt_id][fie.index]);
276 }
277
emul_imager_get_frmival(const struct device * dev,enum video_endpoint_id ep,struct video_frmival * frmival)278 static int emul_imager_get_frmival(const struct device *dev, enum video_endpoint_id ep,
279 struct video_frmival *frmival)
280 {
281 struct emul_imager_data *data = dev->data;
282
283 if (ep != VIDEO_EP_OUT && ep != VIDEO_EP_ALL) {
284 return -EINVAL;
285 }
286
287 frmival->numerator = 1;
288 frmival->denominator = data->mode->fps;
289 return 0;
290 }
291
emul_imager_enum_frmival(const struct device * dev,enum video_endpoint_id ep,struct video_frmival_enum * fie)292 static int emul_imager_enum_frmival(const struct device *dev, enum video_endpoint_id ep,
293 struct video_frmival_enum *fie)
294 {
295 const struct emul_imager_mode *mode;
296 size_t fmt_id;
297 int ret;
298
299 if (ep != VIDEO_EP_OUT && ep != VIDEO_EP_ALL) {
300 return -EINVAL;
301 }
302
303 ret = video_format_caps_index(fmts, fie->format, &fmt_id);
304 if (ret < 0) {
305 return ret;
306 }
307
308 mode = &emul_imager_modes[fmt_id][fie->index];
309
310 fie->type = VIDEO_FRMIVAL_TYPE_DISCRETE;
311 fie->discrete.numerator = 1;
312 fie->discrete.denominator = mode->fps;
313 fie->index++;
314
315 return mode->fps == 0;
316 }
317
318 /* White, Yellow, Cyan, Green, Magenta, Red, Blue, Black */
319 static const uint16_t pattern_8bars_yuv[8][3] = {
320 {0xFF, 0x7F, 0x7F}, {0xFF, 0x00, 0xFF}, {0xFF, 0xFF, 0x00}, {0x7F, 0x00, 0x00},
321 {0x00, 0xFF, 0xFF}, {0x00, 0x00, 0xFF}, {0x00, 0xFF, 0x00}, {0x00, 0x7F, 0x7F}};
322 static const uint16_t pattern_8bars_rgb[8][3] = {
323 {0xFF, 0xFF, 0xFF}, {0xFF, 0xFF, 0x00}, {0x00, 0xFF, 0xFF}, {0x00, 0xFF, 0x00},
324 {0xFF, 0x00, 0xFF}, {0xFF, 0x00, 0x00}, {0x00, 0x00, 0xFF}, {0x00, 0x00, 0x00}};
emul_imager_fill_framebuffer(const struct device * const dev,struct video_format * fmt)325 static void emul_imager_fill_framebuffer(const struct device *const dev, struct video_format *fmt)
326 {
327 struct emul_imager_data *data = dev->data;
328 uint16_t *fb16 = (uint16_t *)data->framebuffer;
329 uint16_t r, g, b, y, uv;
330
331 /* Fill the first row of the emulated framebuffer */
332 switch (fmt->pixelformat) {
333 case VIDEO_PIX_FMT_YUYV:
334 for (size_t i = 0; i < fmt->width; i++) {
335 y = pattern_8bars_yuv[i * 8 / fmt->width][0];
336 uv = pattern_8bars_yuv[i * 8 / fmt->width][1 + i % 2];
337 fb16[i] = sys_cpu_to_be16(y << 8 | uv << 0);
338 }
339 break;
340 case VIDEO_PIX_FMT_RGB565:
341 for (size_t i = 0; i < fmt->width; i++) {
342 r = pattern_8bars_rgb[i * 8 / fmt->width][0] >> (8 - 5);
343 g = pattern_8bars_rgb[i * 8 / fmt->width][1] >> (8 - 6);
344 b = pattern_8bars_rgb[i * 8 / fmt->width][2] >> (8 - 5);
345 fb16[i] = sys_cpu_to_le16((r << 11) | (g << 6) | (b << 0));
346 }
347 break;
348 default:
349 LOG_WRN("Unsupported pixel format %x, supported: %x, %x", fmt->pixelformat,
350 VIDEO_PIX_FMT_YUYV, VIDEO_PIX_FMT_RGB565);
351 memset(fb16, 0, fmt->pitch);
352 }
353
354 /* Duplicate the first row over the whole frame */
355 for (size_t i = 1; i < fmt->height; i++) {
356 memcpy(data->framebuffer + fmt->pitch * i, data->framebuffer, fmt->pitch);
357 }
358 }
359
emul_imager_set_fmt(const struct device * const dev,enum video_endpoint_id ep,struct video_format * fmt)360 static int emul_imager_set_fmt(const struct device *const dev, enum video_endpoint_id ep,
361 struct video_format *fmt)
362 {
363 struct emul_imager_data *data = dev->data;
364 size_t fmt_id;
365 int ret;
366
367 if (ep != VIDEO_EP_OUT && ep != VIDEO_EP_ALL) {
368 return -EINVAL;
369 }
370
371 if (fmt->pitch * fmt->height > CONFIG_VIDEO_EMUL_IMAGER_FRAMEBUFFER_SIZE) {
372 LOG_ERR("%s has %u bytes of memory, unable to support %x %ux%u (%u bytes)",
373 dev->name, CONFIG_VIDEO_EMUL_IMAGER_FRAMEBUFFER_SIZE, fmt->pixelformat,
374 fmt->width, fmt->height, fmt->pitch * fmt->height);
375 return -ENOMEM;
376 }
377
378 if (memcmp(&data->fmt, fmt, sizeof(data->fmt)) == 0) {
379 return 0;
380 }
381
382 ret = video_format_caps_index(fmts, fmt, &fmt_id);
383 if (ret < 0) {
384 LOG_ERR("Format %x %ux%u not found for %s", fmt->pixelformat, fmt->width,
385 fmt->height, dev->name);
386 return ret;
387 }
388
389 ret = emul_imager_set_mode(dev, &emul_imager_modes[fmt_id][0]);
390 if (ret < 0) {
391 return ret;
392 }
393
394 /* Change the image pattern on the framebuffer */
395 emul_imager_fill_framebuffer(dev, fmt);
396
397 data->fmt_id = fmt_id;
398 data->fmt = *fmt;
399 return 0;
400 }
401
emul_imager_get_fmt(const struct device * dev,enum video_endpoint_id ep,struct video_format * fmt)402 static int emul_imager_get_fmt(const struct device *dev, enum video_endpoint_id ep,
403 struct video_format *fmt)
404 {
405 struct emul_imager_data *data = dev->data;
406
407 if (ep != VIDEO_EP_OUT && ep != VIDEO_EP_ALL) {
408 return -EINVAL;
409 }
410
411 *fmt = data->fmt;
412 return 0;
413 }
414
emul_imager_get_caps(const struct device * dev,enum video_endpoint_id ep,struct video_caps * caps)415 static int emul_imager_get_caps(const struct device *dev, enum video_endpoint_id ep,
416 struct video_caps *caps)
417 {
418 if (ep != VIDEO_EP_OUT && ep != VIDEO_EP_ALL) {
419 return -EINVAL;
420 }
421
422 caps->format_caps = fmts;
423 return 0;
424 }
425
emul_imager_stream_start(const struct device * dev)426 static int emul_imager_stream_start(const struct device *dev)
427 {
428 return emul_imager_write_reg(dev, EMUL_IMAGER_REG_CTRL, 1);
429 }
430
emul_imager_stream_stop(const struct device * dev)431 static int emul_imager_stream_stop(const struct device *dev)
432 {
433 return emul_imager_write_reg(dev, EMUL_IMAGER_REG_CTRL, 0);
434 }
435
436 static DEVICE_API(video, emul_imager_driver_api) = {
437 .set_ctrl = emul_imager_set_ctrl,
438 .get_ctrl = emul_imager_get_ctrl,
439 .set_frmival = emul_imager_set_frmival,
440 .get_frmival = emul_imager_get_frmival,
441 .enum_frmival = emul_imager_enum_frmival,
442 .set_format = emul_imager_set_fmt,
443 .get_format = emul_imager_get_fmt,
444 .get_caps = emul_imager_get_caps,
445 .stream_start = emul_imager_stream_start,
446 .stream_stop = emul_imager_stream_stop,
447 };
448
emul_imager_init(const struct device * dev)449 int emul_imager_init(const struct device *dev)
450 {
451 struct video_format fmt;
452 uint8_t sensor_id;
453 int ret;
454
455 if (/* !i2c_is_ready_dt(&cfg->i2c) */ false) {
456 /* LOG_ERR("Bus %s is not ready", cfg->i2c.bus->name); */
457 return -ENODEV;
458 }
459
460 ret = emul_imager_read_reg(dev, EMUL_IMAGER_REG_SENSOR_ID, &sensor_id);
461 if (ret < 0 || sensor_id != EMUL_IMAGER_SENSOR_ID) {
462 LOG_ERR("Failed to get %s correct sensor ID (0x%x", dev->name, sensor_id);
463 return ret;
464 }
465
466 ret = emul_imager_write_multi(dev, emul_imager_init_regs);
467 if (ret < 0) {
468 LOG_ERR("Could not set %s initial registers", dev->name);
469 return ret;
470 }
471
472 fmt.pixelformat = fmts[0].pixelformat;
473 fmt.width = fmts[0].width_min;
474 fmt.height = fmts[0].height_min;
475 fmt.pitch = fmt.width * 2;
476
477 ret = emul_imager_set_fmt(dev, VIDEO_EP_OUT, &fmt);
478 if (ret < 0) {
479 LOG_ERR("Failed to set %s to default format %x %ux%u", dev->name, fmt.pixelformat,
480 fmt.width, fmt.height);
481 }
482
483 return 0;
484 }
485
486 #define EMUL_IMAGER_DEFINE(inst) \
487 static struct emul_imager_data emul_imager_data_##inst; \
488 \
489 static const struct emul_imager_config emul_imager_cfg_##inst = { \
490 .i2c = /* I2C_DT_SPEC_INST_GET(inst) */ {0}, \
491 }; \
492 \
493 DEVICE_DT_INST_DEFINE(inst, &emul_imager_init, NULL, &emul_imager_data_##inst, \
494 &emul_imager_cfg_##inst, POST_KERNEL, CONFIG_VIDEO_INIT_PRIORITY, \
495 &emul_imager_driver_api);
496
497 DT_INST_FOREACH_STATUS_OKAY(EMUL_IMAGER_DEFINE)
498
