1 /*
2 * Copyright (c) 2019, Linaro Limited
3 * Copyright (c) 2024, tinyVision.ai Inc.
4 *
5 * SPDX-License-Identifier: Apache-2.0
6 */
7
8 #include <string.h>
9
10 #include <zephyr/kernel.h>
11 #include <zephyr/drivers/video.h>
12
13 #if defined(CONFIG_VIDEO_BUFFER_USE_SHARED_MULTI_HEAP)
14 #include <zephyr/multi_heap/shared_multi_heap.h>
15
16 #define VIDEO_COMMON_HEAP_ALLOC(align, size, timeout) \
17 shared_multi_heap_aligned_alloc(CONFIG_VIDEO_BUFFER_SMH_ATTRIBUTE, align, size)
18 #define VIDEO_COMMON_FREE(block) shared_multi_heap_free(block)
19 #else
20 K_HEAP_DEFINE(video_buffer_pool, CONFIG_VIDEO_BUFFER_POOL_SZ_MAX*CONFIG_VIDEO_BUFFER_POOL_NUM_MAX);
21 #define VIDEO_COMMON_HEAP_ALLOC(align, size, timeout) \
22 k_heap_aligned_alloc(&video_buffer_pool, align, size, timeout);
23 #define VIDEO_COMMON_FREE(block) k_heap_free(&video_buffer_pool, block)
24 #endif
25
26 static struct video_buffer video_buf[CONFIG_VIDEO_BUFFER_POOL_NUM_MAX];
27
28 struct mem_block {
29 void *data;
30 };
31
32 static struct mem_block video_block[CONFIG_VIDEO_BUFFER_POOL_NUM_MAX];
33
video_buffer_aligned_alloc(size_t size,size_t align,k_timeout_t timeout)34 struct video_buffer *video_buffer_aligned_alloc(size_t size, size_t align, k_timeout_t timeout)
35 {
36 struct video_buffer *vbuf = NULL;
37 struct mem_block *block;
38 int i;
39
40 /* find available video buffer */
41 for (i = 0; i < ARRAY_SIZE(video_buf); i++) {
42 if (video_buf[i].buffer == NULL) {
43 vbuf = &video_buf[i];
44 block = &video_block[i];
45 break;
46 }
47 }
48
49 if (vbuf == NULL) {
50 return NULL;
51 }
52
53 /* Alloc buffer memory */
54 block->data = VIDEO_COMMON_HEAP_ALLOC(align, size, timeout);
55 if (block->data == NULL) {
56 return NULL;
57 }
58
59 vbuf->buffer = block->data;
60 vbuf->size = size;
61 vbuf->bytesused = 0;
62
63 return vbuf;
64 }
65
video_buffer_alloc(size_t size,k_timeout_t timeout)66 struct video_buffer *video_buffer_alloc(size_t size, k_timeout_t timeout)
67 {
68 return video_buffer_aligned_alloc(size, sizeof(void *), timeout);
69 }
70
video_buffer_release(struct video_buffer * vbuf)71 void video_buffer_release(struct video_buffer *vbuf)
72 {
73 struct mem_block *block = NULL;
74 int i;
75
76 /* vbuf to block */
77 for (i = 0; i < ARRAY_SIZE(video_block); i++) {
78 if (video_block[i].data == vbuf->buffer) {
79 block = &video_block[i];
80 break;
81 }
82 }
83
84 vbuf->buffer = NULL;
85 if (block) {
86 VIDEO_COMMON_FREE(block->data);
87 }
88 }
89
video_format_caps_index(const struct video_format_cap * fmts,const struct video_format * fmt,size_t * idx)90 int video_format_caps_index(const struct video_format_cap *fmts, const struct video_format *fmt,
91 size_t *idx)
92 {
93 for (int i = 0; fmts[i].pixelformat != 0; i++) {
94 if (fmts[i].pixelformat == fmt->pixelformat &&
95 IN_RANGE(fmt->width, fmts[i].width_min, fmts[i].width_max) &&
96 IN_RANGE(fmt->height, fmts[i].height_min, fmts[i].height_max)) {
97 *idx = i;
98 return 0;
99 }
100 }
101 return -ENOENT;
102 }
103
video_closest_frmival_stepwise(const struct video_frmival_stepwise * stepwise,const struct video_frmival * desired,struct video_frmival * match)104 void video_closest_frmival_stepwise(const struct video_frmival_stepwise *stepwise,
105 const struct video_frmival *desired,
106 struct video_frmival *match)
107 {
108 uint64_t min = stepwise->min.numerator;
109 uint64_t max = stepwise->max.numerator;
110 uint64_t step = stepwise->step.numerator;
111 uint64_t goal = desired->numerator;
112
113 /* Set a common denominator to all values */
114 min *= stepwise->max.denominator * stepwise->step.denominator * desired->denominator;
115 max *= stepwise->min.denominator * stepwise->step.denominator * desired->denominator;
116 step *= stepwise->min.denominator * stepwise->max.denominator * desired->denominator;
117 goal *= stepwise->min.denominator * stepwise->max.denominator * stepwise->step.denominator;
118
119 /* Saturate the desired value to the min/max supported */
120 goal = CLAMP(goal, min, max);
121
122 /* Compute a numerator and denominator */
123 match->numerator = min + DIV_ROUND_CLOSEST(goal - min, step) * step;
124 match->denominator = stepwise->min.denominator * stepwise->max.denominator *
125 stepwise->step.denominator * desired->denominator;
126 }
127
video_closest_frmival(const struct device * dev,enum video_endpoint_id ep,struct video_frmival_enum * match)128 void video_closest_frmival(const struct device *dev, enum video_endpoint_id ep,
129 struct video_frmival_enum *match)
130 {
131 uint64_t best_diff_nsec = INT32_MAX;
132 struct video_frmival desired = match->discrete;
133 struct video_frmival_enum fie = {.format = match->format};
134
135 __ASSERT(match->type != VIDEO_FRMIVAL_TYPE_STEPWISE,
136 "cannot find range matching the range, only a value matching the range");
137
138 while (video_enum_frmival(dev, ep, &fie) == 0) {
139 struct video_frmival tmp = {0};
140 uint64_t diff_nsec = 0, a, b;
141
142 switch (fie.type) {
143 case VIDEO_FRMIVAL_TYPE_DISCRETE:
144 tmp = fie.discrete;
145 break;
146 case VIDEO_FRMIVAL_TYPE_STEPWISE:
147 video_closest_frmival_stepwise(&fie.stepwise, &desired, &tmp);
148 break;
149 default:
150 __ASSERT(false, "invalid answer from the queried video device");
151 }
152
153 a = video_frmival_nsec(&desired);
154 b = video_frmival_nsec(&tmp);
155 diff_nsec = a > b ? a - b : b - a;
156 if (diff_nsec < best_diff_nsec) {
157 best_diff_nsec = diff_nsec;
158 memcpy(&match->discrete, &tmp, sizeof(tmp));
159
160 /* The video_enum_frmival() function will increment fie.index every time.
161 * Compensate for it to get the current index, not the next index.
162 */
163 match->index = fie.index - 1;
164 }
165 }
166 }
167
