1 /*
2 * Copyright 2024 NXP
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #define DT_DRV_COMPAT nxp_mipi_csi2rx
8
9 #include <zephyr/drivers/clock_control.h>
10 #include <zephyr/drivers/video.h>
11 #include <zephyr/drivers/video-controls.h>
12 #include <zephyr/kernel.h>
13 #include <zephyr/logging/log.h>
14 #include <soc.h>
15
16 #include <fsl_mipi_csi2rx.h>
17
18 LOG_MODULE_REGISTER(video_mipi_csi2rx, CONFIG_VIDEO_LOG_LEVEL);
19
20 #define MAX_SUPPORTED_PIXEL_RATE MHZ(96)
21
22 #define ABS(a, b) (a > b ? a - b : b - a)
23
24 struct mipi_csi2rx_config {
25 const MIPI_CSI2RX_Type *base;
26 const struct device *sensor_dev;
27 };
28
29 struct mipi_csi2rx_data {
30 csi2rx_config_t csi2rxConfig;
31 const struct device *clock_dev;
32 clock_control_subsys_t clock_root;
33 clock_control_subsys_t clock_ui;
34 clock_control_subsys_t clock_esc;
35 };
36
37 struct mipi_csi2rx_tHsSettleEscClk_config {
38 uint64_t pixel_rate;
39 uint8_t tHsSettle_EscClk;
40 };
41
42 /* Must be in pixel rate ascending order */
43 const struct mipi_csi2rx_tHsSettleEscClk_config tHsSettleEscClk_configs[] = {
44 {MHZ(24), 0x24},
45 {MHZ(48), 0x12},
46 {MHZ(96), 0x09},
47 };
48
mipi_csi2rx_update_settings(const struct device * dev,enum video_endpoint_id ep)49 static int mipi_csi2rx_update_settings(const struct device *dev, enum video_endpoint_id ep)
50 {
51 const struct mipi_csi2rx_config *config = dev->config;
52 struct mipi_csi2rx_data *drv_data = dev->data;
53 uint8_t bpp;
54 uint64_t sensor_pixel_rate;
55 uint32_t root_clk_rate, ui_clk_rate, sensor_byte_clk, best_match;
56 int ret, ind = 0;
57 struct video_format fmt;
58
59 ret = video_get_format(config->sensor_dev, ep, &fmt);
60 if (ret) {
61 LOG_ERR("Cannot get sensor_dev pixel format");
62 return ret;
63 }
64
65 ret = video_get_ctrl(config->sensor_dev, VIDEO_CID_PIXEL_RATE, &sensor_pixel_rate);
66 if (ret) {
67 LOG_ERR("Can not get sensor_dev pixel rate");
68 return ret;
69 }
70
71 if (sensor_pixel_rate > MAX_SUPPORTED_PIXEL_RATE) {
72 LOG_ERR("Sensor pixel rate is not supported");
73 return -ENOTSUP;
74 }
75
76 bpp = video_bits_per_pixel(fmt.pixelformat);
77 sensor_byte_clk = sensor_pixel_rate * bpp / drv_data->csi2rxConfig.laneNum / BITS_PER_BYTE;
78
79 ret = clock_control_get_rate(drv_data->clock_dev, drv_data->clock_root, &root_clk_rate);
80 if (ret) {
81 return ret;
82 }
83
84 if (sensor_byte_clk > root_clk_rate) {
85 ret = clock_control_set_rate(drv_data->clock_dev, drv_data->clock_root,
86 (clock_control_subsys_rate_t)sensor_byte_clk);
87 if (ret) {
88 return ret;
89 }
90 }
91
92 ret = clock_control_get_rate(drv_data->clock_dev, drv_data->clock_ui, &ui_clk_rate);
93 if (ret) {
94 return ret;
95 }
96
97 if (sensor_pixel_rate > ui_clk_rate) {
98 ret = clock_control_set_rate(
99 drv_data->clock_dev, drv_data->clock_ui,
100 (clock_control_subsys_rate_t)(uint32_t)sensor_pixel_rate);
101 if (ret) {
102 return ret;
103 }
104 }
105
106 /* Find the supported sensor_pixel_rate closest to the desired one */
107 best_match = tHsSettleEscClk_configs[ind].pixel_rate;
108 for (uint8_t i = 0; i < ARRAY_SIZE(tHsSettleEscClk_configs); i++) {
109 if (ABS(tHsSettleEscClk_configs[i].pixel_rate, sensor_pixel_rate) <
110 ABS(tHsSettleEscClk_configs[i].pixel_rate, best_match)) {
111 best_match = tHsSettleEscClk_configs[i].pixel_rate;
112 ind = i;
113 }
114 }
115
116 drv_data->csi2rxConfig.tHsSettle_EscClk = tHsSettleEscClk_configs[ind].tHsSettle_EscClk;
117
118 return ret;
119 }
120
mipi_csi2rx_set_fmt(const struct device * dev,enum video_endpoint_id ep,struct video_format * fmt)121 static int mipi_csi2rx_set_fmt(const struct device *dev, enum video_endpoint_id ep,
122 struct video_format *fmt)
123 {
124 const struct mipi_csi2rx_config *config = dev->config;
125
126 if (video_set_format(config->sensor_dev, ep, fmt)) {
127 return -EIO;
128 }
129
130 return mipi_csi2rx_update_settings(dev, ep);
131 }
132
mipi_csi2rx_get_fmt(const struct device * dev,enum video_endpoint_id ep,struct video_format * fmt)133 static int mipi_csi2rx_get_fmt(const struct device *dev, enum video_endpoint_id ep,
134 struct video_format *fmt)
135 {
136 const struct mipi_csi2rx_config *config = dev->config;
137
138 if (fmt == NULL || (ep != VIDEO_EP_OUT && ep != VIDEO_EP_ALL)) {
139 return -EINVAL;
140 }
141
142 if (video_get_format(config->sensor_dev, ep, fmt)) {
143 return -EIO;
144 }
145
146 return 0;
147 }
148
mipi_csi2rx_set_stream(const struct device * dev,bool enable)149 static int mipi_csi2rx_set_stream(const struct device *dev, bool enable)
150 {
151 const struct mipi_csi2rx_config *config = dev->config;
152
153 if (enable) {
154 struct mipi_csi2rx_data *drv_data = dev->data;
155
156 CSI2RX_Init((MIPI_CSI2RX_Type *)config->base, &drv_data->csi2rxConfig);
157 if (video_stream_start(config->sensor_dev)) {
158 return -EIO;
159 }
160 } else {
161 if (video_stream_stop(config->sensor_dev)) {
162 return -EIO;
163 }
164 CSI2RX_Deinit((MIPI_CSI2RX_Type *)config->base);
165 }
166
167 return 0;
168 }
169
mipi_csi2rx_get_caps(const struct device * dev,enum video_endpoint_id ep,struct video_caps * caps)170 static int mipi_csi2rx_get_caps(const struct device *dev, enum video_endpoint_id ep,
171 struct video_caps *caps)
172 {
173 const struct mipi_csi2rx_config *config = dev->config;
174
175 if (ep != VIDEO_EP_OUT && ep != VIDEO_EP_ALL) {
176 return -EINVAL;
177 }
178
179 /* Just forward to sensor dev for now */
180 return video_get_caps(config->sensor_dev, ep, caps);
181 }
182
mipi_csi2rx_set_ctrl(const struct device * dev,unsigned int cid,void * value)183 static inline int mipi_csi2rx_set_ctrl(const struct device *dev, unsigned int cid, void *value)
184 {
185 const struct mipi_csi2rx_config *config = dev->config;
186
187 if (config->sensor_dev) {
188 return video_set_ctrl(config->sensor_dev, cid, value);
189 }
190
191 return -ENOTSUP;
192 }
193
mipi_csi2rx_set_frmival(const struct device * dev,enum video_endpoint_id ep,struct video_frmival * frmival)194 static int mipi_csi2rx_set_frmival(const struct device *dev, enum video_endpoint_id ep,
195 struct video_frmival *frmival)
196 {
197 const struct mipi_csi2rx_config *config = dev->config;
198 int ret;
199
200 ret = video_set_frmival(config->sensor_dev, ep, frmival);
201 if (ret) {
202 LOG_ERR("Cannot set sensor_dev frmival");
203 return ret;
204 }
205
206 ret = mipi_csi2rx_update_settings(dev, ep);
207
208 return ret;
209 }
210
mipi_csi2rx_get_frmival(const struct device * dev,enum video_endpoint_id ep,struct video_frmival * frmival)211 static int mipi_csi2rx_get_frmival(const struct device *dev, enum video_endpoint_id ep,
212 struct video_frmival *frmival)
213 {
214 const struct mipi_csi2rx_config *config = dev->config;
215
216 return video_get_frmival(config->sensor_dev, ep, frmival);
217 }
218
mipi_csi2rx_cal_frame_size(const struct video_format * fmt)219 static uint64_t mipi_csi2rx_cal_frame_size(const struct video_format *fmt)
220 {
221 return fmt->height * fmt->width * video_bits_per_pixel(fmt->pixelformat);
222 }
223
mipi_csi2rx_estimate_pixel_rate(const struct video_frmival * cur_fmival,const struct video_frmival * fie_frmival,const struct video_format * cur_format,const struct video_format * fie_format,uint64_t cur_pixel_rate,uint8_t laneNum)224 static uint64_t mipi_csi2rx_estimate_pixel_rate(const struct video_frmival *cur_fmival,
225 const struct video_frmival *fie_frmival,
226 const struct video_format *cur_format,
227 const struct video_format *fie_format,
228 uint64_t cur_pixel_rate, uint8_t laneNum)
229 {
230 return mipi_csi2rx_cal_frame_size(cur_format) * fie_frmival->denominator *
231 cur_fmival->numerator * cur_pixel_rate /
232 (mipi_csi2rx_cal_frame_size(fie_format) * fie_frmival->numerator *
233 cur_fmival->denominator);
234 }
235
mipi_csi2rx_enum_frmival(const struct device * dev,enum video_endpoint_id ep,struct video_frmival_enum * fie)236 static int mipi_csi2rx_enum_frmival(const struct device *dev, enum video_endpoint_id ep,
237 struct video_frmival_enum *fie)
238 {
239 const struct mipi_csi2rx_config *config = dev->config;
240 struct mipi_csi2rx_data *drv_data = dev->data;
241 int ret;
242 uint64_t cur_pixel_rate, est_pixel_rate;
243 struct video_frmival cur_frmival;
244 struct video_format cur_fmt;
245
246 ret = video_enum_frmival(config->sensor_dev, ep, fie);
247 if (ret) {
248 return ret;
249 }
250
251 ret = video_get_ctrl(config->sensor_dev, VIDEO_CID_PIXEL_RATE, &cur_pixel_rate);
252 if (ret) {
253 LOG_ERR("Cannot get sensor_dev pixel rate");
254 return ret;
255 }
256
257 ret = video_get_frmival(config->sensor_dev, ep, &cur_frmival);
258 if (ret) {
259 LOG_ERR("Cannot get sensor_dev frame rate");
260 return ret;
261 }
262
263 ret = video_get_format(config->sensor_dev, ep, &cur_fmt);
264 if (ret) {
265 LOG_ERR("Cannot get sensor_dev format");
266 return ret;
267 }
268
269 if (fie->type == VIDEO_FRMIVAL_TYPE_DISCRETE) {
270 est_pixel_rate = mipi_csi2rx_estimate_pixel_rate(
271 &cur_frmival, &fie->discrete, &cur_fmt, fie->format, cur_pixel_rate,
272 drv_data->csi2rxConfig.laneNum);
273 if (est_pixel_rate > MAX_SUPPORTED_PIXEL_RATE) {
274 return -EINVAL;
275 }
276
277 } else {
278 /* Check the lane rate of the lower bound framerate */
279 est_pixel_rate = mipi_csi2rx_estimate_pixel_rate(
280 &cur_frmival, &fie->stepwise.min, &cur_fmt, fie->format, cur_pixel_rate,
281 drv_data->csi2rxConfig.laneNum);
282 if (est_pixel_rate > MAX_SUPPORTED_PIXEL_RATE) {
283 return -EINVAL;
284 }
285
286 /* Check the lane rate of the upper bound framerate */
287 est_pixel_rate = mipi_csi2rx_estimate_pixel_rate(
288 &cur_frmival, &fie->stepwise.max, &cur_fmt, fie->format, cur_pixel_rate,
289 drv_data->csi2rxConfig.laneNum);
290 if (est_pixel_rate > MAX_SUPPORTED_PIXEL_RATE) {
291 fie->stepwise.max.denominator =
292 (mipi_csi2rx_cal_frame_size(&cur_fmt) * MAX_SUPPORTED_PIXEL_RATE *
293 cur_frmival.denominator) /
294 (mipi_csi2rx_cal_frame_size(fie->format) * cur_pixel_rate *
295 cur_frmival.numerator);
296 fie->stepwise.max.numerator = 1;
297 }
298 }
299
300 return 0;
301 }
302
303 static DEVICE_API(video, mipi_csi2rx_driver_api) = {
304 .get_caps = mipi_csi2rx_get_caps,
305 .get_format = mipi_csi2rx_get_fmt,
306 .set_format = mipi_csi2rx_set_fmt,
307 .set_stream = mipi_csi2rx_set_stream,
308 .set_ctrl = mipi_csi2rx_set_ctrl,
309 .set_frmival = mipi_csi2rx_set_frmival,
310 .get_frmival = mipi_csi2rx_get_frmival,
311 .enum_frmival = mipi_csi2rx_enum_frmival,
312 };
313
mipi_csi2rx_init(const struct device * dev)314 static int mipi_csi2rx_init(const struct device *dev)
315 {
316 const struct mipi_csi2rx_config *config = dev->config;
317 struct mipi_csi2rx_data *drv_data = dev->data;
318 int ret;
319
320 /* Check if there is any sensor device */
321 if (!device_is_ready(config->sensor_dev)) {
322 return -ENODEV;
323 }
324
325 /*
326 * CSI2 escape clock should be in the range [60, 80] Mhz. We set it
327 * to 60 Mhz.
328 */
329 ret = clock_control_set_rate(drv_data->clock_dev, drv_data->clock_esc,
330 (clock_control_subsys_rate_t)MHZ(60));
331 if (ret) {
332 return ret;
333 }
334
335 return mipi_csi2rx_update_settings(dev, VIDEO_EP_ALL);
336 }
337
338 #define MIPI_CSI2RX_INIT(n) \
339 static struct mipi_csi2rx_data mipi_csi2rx_data_##n = { \
340 .csi2rxConfig.laneNum = DT_PROP_LEN(DT_INST_ENDPOINT_BY_ID(n, 1, 0), data_lanes), \
341 .clock_dev = DEVICE_DT_GET(DT_INST_CLOCKS_CTLR(n)), \
342 .clock_root = (clock_control_subsys_t)DT_INST_CLOCKS_CELL_BY_IDX(n, 0, name), \
343 .clock_ui = (clock_control_subsys_t)DT_INST_CLOCKS_CELL_BY_IDX(n, 1, name), \
344 .clock_esc = (clock_control_subsys_t)DT_INST_CLOCKS_CELL_BY_IDX(n, 2, name), \
345 }; \
346 \
347 static const struct mipi_csi2rx_config mipi_csi2rx_config_##n = { \
348 .base = (MIPI_CSI2RX_Type *)DT_INST_REG_ADDR(n), \
349 .sensor_dev = \
350 DEVICE_DT_GET(DT_NODE_REMOTE_DEVICE(DT_INST_ENDPOINT_BY_ID(n, 1, 0))), \
351 }; \
352 \
353 DEVICE_DT_INST_DEFINE(n, &mipi_csi2rx_init, NULL, &mipi_csi2rx_data_##n, \
354 &mipi_csi2rx_config_##n, POST_KERNEL, CONFIG_VIDEO_INIT_PRIORITY, \
355 &mipi_csi2rx_driver_api);
356
357 DT_INST_FOREACH_STATUS_OKAY(MIPI_CSI2RX_INIT)
358
