1 /*
2 * Copyright (c) 2019, Linaro Limited
3 * Copyright (c) 2024-2025, tinyVision.ai Inc.
4 *
5 * SPDX-License-Identifier: Apache-2.0
6 */
7
8 #include <string.h>
9
10 #include <zephyr/device.h>
11 #include <zephyr/drivers/i2c.h>
12 #include <zephyr/drivers/video.h>
13 #include <zephyr/drivers/video-controls.h>
14 #include <zephyr/kernel.h>
15 #include <zephyr/logging/log.h>
16 #include <zephyr/sys/byteorder.h>
17 #include <zephyr/sys/util.h>
18
19 #include "video_common.h"
20
21 LOG_MODULE_REGISTER(video_common, CONFIG_VIDEO_LOG_LEVEL);
22
23 #if defined(CONFIG_VIDEO_BUFFER_USE_SHARED_MULTI_HEAP)
24 #include <zephyr/multi_heap/shared_multi_heap.h>
25
26 #define VIDEO_COMMON_HEAP_ALLOC(align, size, timeout) \
27 shared_multi_heap_aligned_alloc(CONFIG_VIDEO_BUFFER_SMH_ATTRIBUTE, align, size)
28 #define VIDEO_COMMON_FREE(block) shared_multi_heap_free(block)
29 #else
30
31 #if !defined(CONFIG_VIDEO_BUFFER_POOL_ZEPHYR_REGION)
32 #define VIDEO_BUFFER_POOL_REGION_NAME __noinit_named(kheap_buf_video_buffer_pool)
33 #else
34 #define VIDEO_BUFFER_POOL_REGION_NAME Z_GENERIC_SECTION(CONFIG_VIDEO_BUFFER_POOL_ZEPHYR_REGION_NAME)
35 #endif
36
37 /*
38 * The k_heap is manually initialized instead of using directly Z_HEAP_DEFINE_IN_SECT
39 * since the section might not be yet accessible from the beginning, making it impossible
40 * to initialize it if done via Z_HEAP_DEFINE_IN_SECT
41 */
42 static char VIDEO_BUFFER_POOL_REGION_NAME __aligned(8)
43 video_buffer_pool_mem[MAX(CONFIG_VIDEO_BUFFER_POOL_HEAP_SIZE, Z_HEAP_MIN_SIZE)];
44 static struct k_heap video_buffer_pool;
45 static bool video_buffer_pool_initialized;
46
video_buffer_k_heap_aligned_alloc(size_t align,size_t bytes,k_timeout_t timeout)47 static void *video_buffer_k_heap_aligned_alloc(size_t align, size_t bytes, k_timeout_t timeout)
48 {
49 if (!video_buffer_pool_initialized) {
50 k_heap_init(&video_buffer_pool, video_buffer_pool_mem,
51 MAX(CONFIG_VIDEO_BUFFER_POOL_HEAP_SIZE, Z_HEAP_MIN_SIZE));
52 video_buffer_pool_initialized = true;
53 }
54
55 return k_heap_aligned_alloc(&video_buffer_pool, align, bytes, timeout);
56 }
57
58 #define VIDEO_COMMON_HEAP_ALLOC(align, size, timeout) \
59 video_buffer_k_heap_aligned_alloc(align, size, timeout)
60 #define VIDEO_COMMON_FREE(block) k_heap_free(&video_buffer_pool, block)
61 #endif
62
63 static struct video_buffer video_buf[CONFIG_VIDEO_BUFFER_POOL_NUM_MAX];
64
65 struct mem_block {
66 void *data;
67 };
68
69 static struct mem_block video_block[CONFIG_VIDEO_BUFFER_POOL_NUM_MAX];
70
video_buffer_aligned_alloc(size_t size,size_t align,k_timeout_t timeout)71 struct video_buffer *video_buffer_aligned_alloc(size_t size, size_t align, k_timeout_t timeout)
72 {
73 struct video_buffer *vbuf = NULL;
74 struct mem_block *block;
75 int i;
76
77 /* find available video buffer */
78 for (i = 0; i < ARRAY_SIZE(video_buf); i++) {
79 if (video_buf[i].buffer == NULL) {
80 vbuf = &video_buf[i];
81 block = &video_block[i];
82 break;
83 }
84 }
85
86 if (vbuf == NULL) {
87 return NULL;
88 }
89
90 /* Alloc buffer memory */
91 block->data = VIDEO_COMMON_HEAP_ALLOC(align, size, timeout);
92 if (block->data == NULL) {
93 return NULL;
94 }
95
96 vbuf->buffer = block->data;
97 vbuf->size = size;
98 vbuf->bytesused = 0;
99
100 return vbuf;
101 }
102
video_buffer_alloc(size_t size,k_timeout_t timeout)103 struct video_buffer *video_buffer_alloc(size_t size, k_timeout_t timeout)
104 {
105 return video_buffer_aligned_alloc(size, sizeof(void *), timeout);
106 }
107
video_buffer_release(struct video_buffer * vbuf)108 void video_buffer_release(struct video_buffer *vbuf)
109 {
110 struct mem_block *block = NULL;
111 int i;
112
113 __ASSERT_NO_MSG(vbuf != NULL);
114
115 /* vbuf to block */
116 for (i = 0; i < ARRAY_SIZE(video_block); i++) {
117 if (video_block[i].data == vbuf->buffer) {
118 block = &video_block[i];
119 break;
120 }
121 }
122
123 vbuf->buffer = NULL;
124 if (block) {
125 VIDEO_COMMON_FREE(block->data);
126 }
127 }
128
video_format_caps_index(const struct video_format_cap * fmts,const struct video_format * fmt,size_t * idx)129 int video_format_caps_index(const struct video_format_cap *fmts, const struct video_format *fmt,
130 size_t *idx)
131 {
132 __ASSERT_NO_MSG(fmts != NULL);
133 __ASSERT_NO_MSG(fmt != NULL);
134 __ASSERT_NO_MSG(idx != NULL);
135
136 for (int i = 0; fmts[i].pixelformat != 0; i++) {
137 if (fmts[i].pixelformat == fmt->pixelformat &&
138 IN_RANGE(fmt->width, fmts[i].width_min, fmts[i].width_max) &&
139 IN_RANGE(fmt->height, fmts[i].height_min, fmts[i].height_max)) {
140 *idx = i;
141 return 0;
142 }
143 }
144 return -ENOENT;
145 }
146
video_closest_frmival_stepwise(const struct video_frmival_stepwise * stepwise,const struct video_frmival * desired,struct video_frmival * match)147 void video_closest_frmival_stepwise(const struct video_frmival_stepwise *stepwise,
148 const struct video_frmival *desired,
149 struct video_frmival *match)
150 {
151 __ASSERT_NO_MSG(stepwise != NULL);
152 __ASSERT_NO_MSG(desired != NULL);
153 __ASSERT_NO_MSG(match != NULL);
154
155 uint64_t min = stepwise->min.numerator;
156 uint64_t max = stepwise->max.numerator;
157 uint64_t step = stepwise->step.numerator;
158 uint64_t goal = desired->numerator;
159
160 /* Set a common denominator to all values */
161 min *= stepwise->max.denominator * stepwise->step.denominator * desired->denominator;
162 max *= stepwise->min.denominator * stepwise->step.denominator * desired->denominator;
163 step *= stepwise->min.denominator * stepwise->max.denominator * desired->denominator;
164 goal *= stepwise->min.denominator * stepwise->max.denominator * stepwise->step.denominator;
165
166 __ASSERT_NO_MSG(step != 0U);
167 /* Prevent division by zero */
168 if (step == 0U) {
169 return;
170 }
171 /* Saturate the desired value to the min/max supported */
172 goal = CLAMP(goal, min, max);
173
174 /* Compute a numerator and denominator */
175 match->numerator = min + DIV_ROUND_CLOSEST(goal - min, step) * step;
176 match->denominator = stepwise->min.denominator * stepwise->max.denominator *
177 stepwise->step.denominator * desired->denominator;
178 }
179
video_closest_frmival(const struct device * dev,struct video_frmival_enum * match)180 void video_closest_frmival(const struct device *dev, struct video_frmival_enum *match)
181 {
182 __ASSERT_NO_MSG(dev != NULL);
183 __ASSERT_NO_MSG(match != NULL);
184
185 struct video_frmival desired = match->discrete;
186 struct video_frmival_enum fie = {.format = match->format};
187 uint64_t best_diff_nsec = INT32_MAX;
188 uint64_t goal_nsec = video_frmival_nsec(&desired);
189
190 __ASSERT(match->type != VIDEO_FRMIVAL_TYPE_STEPWISE,
191 "cannot find range matching the range, only a value matching the range");
192
193 for (fie.index = 0; video_enum_frmival(dev, &fie) == 0; fie.index++) {
194 struct video_frmival tmp = {0};
195 uint64_t diff_nsec = 0;
196 uint64_t tmp_nsec;
197
198 switch (fie.type) {
199 case VIDEO_FRMIVAL_TYPE_DISCRETE:
200 tmp = fie.discrete;
201 break;
202 case VIDEO_FRMIVAL_TYPE_STEPWISE:
203 video_closest_frmival_stepwise(&fie.stepwise, &desired, &tmp);
204 break;
205 default:
206 CODE_UNREACHABLE;
207 }
208
209 tmp_nsec = video_frmival_nsec(&tmp);
210 diff_nsec = tmp_nsec > goal_nsec ? tmp_nsec - goal_nsec : goal_nsec - tmp_nsec;
211
212 if (diff_nsec < best_diff_nsec) {
213 best_diff_nsec = diff_nsec;
214 match->index = fie.index;
215 match->discrete = tmp;
216 }
217
218 if (diff_nsec == 0) {
219 /* Exact match, stop searching a better match */
220 break;
221 }
222 }
223 }
224
video_read_reg_retry(const struct i2c_dt_spec * i2c,uint8_t * buf_w,size_t size_w,uint8_t * buf_r,size_t size_r)225 static int video_read_reg_retry(const struct i2c_dt_spec *i2c, uint8_t *buf_w, size_t size_w,
226 uint8_t *buf_r, size_t size_r)
227 {
228 int ret;
229
230 for (int i = 0;; i++) {
231 ret = i2c_write_read_dt(i2c, buf_w, size_w, buf_r, size_r);
232 if (ret == 0) {
233 break;
234 }
235 if (i == CONFIG_VIDEO_I2C_RETRY_NUM) {
236 LOG_HEXDUMP_ERR(buf_w, size_w, "failed to write-read to I2C register");
237 return ret;
238 }
239 if (CONFIG_VIDEO_I2C_RETRY_NUM > 0) {
240 k_sleep(K_MSEC(1));
241 }
242 }
243
244 return 0;
245 }
246
video_read_cci_reg(const struct i2c_dt_spec * i2c,uint32_t reg_addr,uint32_t * reg_data)247 int video_read_cci_reg(const struct i2c_dt_spec *i2c, uint32_t reg_addr, uint32_t *reg_data)
248 {
249 size_t addr_size = FIELD_GET(VIDEO_REG_ADDR_SIZE_MASK, reg_addr);
250 size_t data_size = FIELD_GET(VIDEO_REG_DATA_SIZE_MASK, reg_addr);
251 bool big_endian = FIELD_GET(VIDEO_REG_ENDIANNESS_MASK, reg_addr);
252 uint16_t addr = FIELD_GET(VIDEO_REG_ADDR_MASK, reg_addr);
253 uint8_t buf_w[sizeof(uint16_t)] = {0};
254 uint8_t *data_ptr;
255 int ret;
256
257 __ASSERT_NO_MSG(i2c != NULL);
258 __ASSERT_NO_MSG(reg_data != NULL);
259 __ASSERT(addr_size > 0, "The address must have a address size flag");
260 __ASSERT(data_size > 0, "The address must have a data size flag");
261
262 *reg_data = 0;
263
264 if (big_endian) {
265 /* Casting between data sizes in big-endian requires re-aligning */
266 data_ptr = (uint8_t *)reg_data + sizeof(*reg_data) - data_size;
267 } else {
268 /* Casting between data sizes in little-endian is a no-op */
269 data_ptr = (uint8_t *)reg_data;
270 }
271
272 for (int i = 0; i < data_size; i++) {
273 if (addr_size == 1) {
274 buf_w[0] = addr + i;
275 } else {
276 sys_put_be16(addr + i, &buf_w[0]);
277 }
278
279 ret = video_read_reg_retry(i2c, buf_w, addr_size, &data_ptr[i], 1);
280 if (ret < 0) {
281 LOG_ERR("Failed to read from register 0x%x", addr + i);
282 return ret;
283 }
284
285 LOG_HEXDUMP_DBG(buf_w, addr_size, "Data written to the I2C device...");
286 LOG_HEXDUMP_DBG(&data_ptr[i], 1, "... data read back from the I2C device");
287 }
288
289 *reg_data = big_endian ? sys_be32_to_cpu(*reg_data) : sys_le32_to_cpu(*reg_data);
290
291 return 0;
292 }
293
video_write_reg_retry(const struct i2c_dt_spec * i2c,uint8_t * buf_w,size_t size)294 static int video_write_reg_retry(const struct i2c_dt_spec *i2c, uint8_t *buf_w, size_t size)
295 {
296 int ret;
297
298 __ASSERT_NO_MSG(i2c != NULL);
299 __ASSERT_NO_MSG(buf_w != NULL);
300
301 for (int i = 0;; i++) {
302 ret = i2c_write_dt(i2c, buf_w, size);
303 if (ret == 0) {
304 break;
305 }
306 if (i == CONFIG_VIDEO_I2C_RETRY_NUM) {
307 LOG_HEXDUMP_ERR(buf_w, size, "failed to write to I2C register");
308 return ret;
309 }
310 if (CONFIG_VIDEO_I2C_RETRY_NUM > 0) {
311 k_sleep(K_MSEC(1));
312 }
313 }
314
315 return 0;
316 }
317
video_write_cci_reg(const struct i2c_dt_spec * i2c,uint32_t reg_addr,uint32_t reg_data)318 int video_write_cci_reg(const struct i2c_dt_spec *i2c, uint32_t reg_addr, uint32_t reg_data)
319 {
320 size_t addr_size = FIELD_GET(VIDEO_REG_ADDR_SIZE_MASK, reg_addr);
321 size_t data_size = FIELD_GET(VIDEO_REG_DATA_SIZE_MASK, reg_addr);
322 bool big_endian = FIELD_GET(VIDEO_REG_ENDIANNESS_MASK, reg_addr);
323 uint16_t addr = FIELD_GET(VIDEO_REG_ADDR_MASK, reg_addr);
324 uint8_t buf_w[sizeof(uint16_t) + sizeof(uint32_t)] = {0};
325 uint8_t *data_ptr;
326 int ret;
327
328 __ASSERT_NO_MSG(i2c != NULL);
329 __ASSERT(addr_size > 0, "The address must have a address size flag");
330 __ASSERT(data_size > 0, "The address must have a data size flag");
331
332 if (big_endian) {
333 /* Casting between data sizes in big-endian requires re-aligning */
334 reg_data = sys_cpu_to_be32(reg_data);
335 data_ptr = (uint8_t *)®_data + sizeof(reg_data) - data_size;
336 } else {
337 /* Casting between data sizes in little-endian is a no-op */
338 reg_data = sys_cpu_to_le32(reg_data);
339 data_ptr = (uint8_t *)®_data;
340 }
341
342 for (int i = 0; i < data_size; i++) {
343 /* The address is always big-endian as per CCI standard */
344 if (addr_size == 1) {
345 buf_w[0] = addr + i;
346 } else {
347 sys_put_be16(addr + i, &buf_w[0]);
348 }
349
350 buf_w[addr_size] = data_ptr[i];
351
352 LOG_HEXDUMP_DBG(buf_w, addr_size + 1, "Data written to the I2C device");
353
354 ret = video_write_reg_retry(i2c, buf_w, addr_size + 1);
355 if (ret < 0) {
356 LOG_ERR("Failed to write to register 0x%x", addr + i);
357 return ret;
358 }
359 }
360
361 return 0;
362 }
363
video_modify_cci_reg(const struct i2c_dt_spec * i2c,uint32_t reg_addr,uint32_t field_mask,uint32_t field_value)364 int video_modify_cci_reg(const struct i2c_dt_spec *i2c, uint32_t reg_addr, uint32_t field_mask,
365 uint32_t field_value)
366 {
367 uint32_t reg;
368 int ret;
369
370 ret = video_read_cci_reg(i2c, reg_addr, ®);
371 if (ret < 0) {
372 return ret;
373 }
374
375 return video_write_cci_reg(i2c, reg_addr, (reg & ~field_mask) | field_value);
376 }
377
video_write_cci_multiregs(const struct i2c_dt_spec * i2c,const struct video_reg * regs,size_t num_regs)378 int video_write_cci_multiregs(const struct i2c_dt_spec *i2c, const struct video_reg *regs,
379 size_t num_regs)
380 {
381 int ret;
382
383 __ASSERT_NO_MSG(regs != NULL);
384
385 for (int i = 0; i < num_regs; i++) {
386 ret = video_write_cci_reg(i2c, regs[i].addr, regs[i].data);
387 if (ret < 0) {
388 return ret;
389 }
390 }
391
392 return 0;
393 }
394
video_write_cci_multiregs8(const struct i2c_dt_spec * i2c,const struct video_reg8 * regs,size_t num_regs)395 int video_write_cci_multiregs8(const struct i2c_dt_spec *i2c, const struct video_reg8 *regs,
396 size_t num_regs)
397 {
398 int ret;
399
400 __ASSERT_NO_MSG(regs != NULL);
401
402 for (int i = 0; i < num_regs; i++) {
403 ret = video_write_cci_reg(i2c, regs[i].addr | VIDEO_REG_ADDR8_DATA8, regs[i].data);
404 if (ret < 0) {
405 return ret;
406 }
407 }
408
409 return 0;
410 }
411
video_write_cci_multiregs16(const struct i2c_dt_spec * i2c,const struct video_reg16 * regs,size_t num_regs)412 int video_write_cci_multiregs16(const struct i2c_dt_spec *i2c, const struct video_reg16 *regs,
413 size_t num_regs)
414 {
415 int ret;
416
417 __ASSERT_NO_MSG(regs != NULL);
418
419 for (int i = 0; i < num_regs; i++) {
420 ret = video_write_cci_reg(i2c, regs[i].addr | VIDEO_REG_ADDR16_DATA8, regs[i].data);
421 if (ret < 0) {
422 return ret;
423 }
424 }
425
426 return 0;
427 }
428
video_get_csi_link_freq(const struct device * dev,uint8_t bpp,uint8_t lane_nb)429 int64_t video_get_csi_link_freq(const struct device *dev, uint8_t bpp, uint8_t lane_nb)
430 {
431 struct video_control ctrl = {
432 .id = VIDEO_CID_LINK_FREQ,
433 };
434 struct video_ctrl_query ctrl_query = {
435 .dev = dev,
436 .id = VIDEO_CID_LINK_FREQ,
437 };
438 int ret;
439
440 /* Try to get the LINK_FREQ value from the source device */
441 ret = video_get_ctrl(dev, &ctrl);
442 if (ret < 0) {
443 goto fallback;
444 }
445
446 ret = video_query_ctrl(&ctrl_query);
447 if (ret < 0) {
448 return ret;
449 }
450
451 if (!IN_RANGE(ctrl.val, ctrl_query.range.min, ctrl_query.range.max)) {
452 return -ERANGE;
453 }
454
455 if (ctrl_query.int_menu == NULL) {
456 return -EINVAL;
457 }
458
459 return (int64_t)ctrl_query.int_menu[ctrl.val];
460
461 fallback:
462 /* If VIDEO_CID_LINK_FREQ is not available, approximate from VIDEO_CID_PIXEL_RATE */
463 ctrl.id = VIDEO_CID_PIXEL_RATE;
464 ret = video_get_ctrl(dev, &ctrl);
465 if (ret < 0) {
466 return ret;
467 }
468
469 /* CSI D-PHY is using a DDR data bus so bitrate is twice the frequency */
470 return ctrl.val64 * bpp / (2 * lane_nb);
471 }
472
video_estimate_fmt_size(struct video_format * fmt)473 int video_estimate_fmt_size(struct video_format *fmt)
474 {
475 if (fmt == NULL) {
476 return -EINVAL;
477 }
478
479 switch (fmt->pixelformat) {
480 case VIDEO_PIX_FMT_JPEG:
481 case VIDEO_PIX_FMT_H264:
482 /* Rough estimate for the worst case (quality = 100) */
483 fmt->pitch = 0;
484 fmt->size = fmt->width * fmt->height * 2;
485 break;
486 default:
487 /* Uncompressed format */
488 fmt->pitch = fmt->width * video_bits_per_pixel(fmt->pixelformat) / BITS_PER_BYTE;
489 if (fmt->pitch == 0) {
490 return -ENOTSUP;
491 }
492 fmt->size = fmt->pitch * fmt->height;
493 break;
494 }
495
496 return 0;
497 }
498
video_set_compose_format(const struct device * dev,struct video_format * fmt)499 int video_set_compose_format(const struct device *dev, struct video_format *fmt)
500 {
501 struct video_selection sel = {
502 .type = fmt->type,
503 .target = VIDEO_SEL_TGT_COMPOSE,
504 .rect.left = 0,
505 .rect.top = 0,
506 .rect.width = fmt->width,
507 .rect.height = fmt->height,
508 };
509 int ret;
510
511 ret = video_set_selection(dev, &sel);
512 if (ret < 0 && ret != -ENOSYS) {
513 LOG_ERR("Unable to set selection compose");
514 return ret;
515 }
516
517 return video_set_format(dev, fmt);
518 }
519
video_transfer_buffer(const struct device * src,const struct device * sink,enum video_buf_type src_type,enum video_buf_type sink_type,k_timeout_t timeout)520 int video_transfer_buffer(const struct device *src, const struct device *sink,
521 enum video_buf_type src_type, enum video_buf_type sink_type,
522 k_timeout_t timeout)
523 {
524 struct video_buffer *buf = &(struct video_buffer){.type = src_type};
525 int ret;
526
527 ret = video_dequeue(src, &buf, timeout);
528 if (ret < 0) {
529 return ret;
530 }
531
532 buf->type = sink_type;
533
534 return video_enqueue(sink, buf);
535 }
536
