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_pix_fmt_bpp(fmt.pixelformat) * 8;
77 sensor_byte_clk = sensor_pixel_rate * bpp / drv_data->csi2rxConfig.laneNum / 8;
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_stream_start(const struct device * dev)149 static int mipi_csi2rx_stream_start(const struct device *dev)
150 {
151 const struct mipi_csi2rx_config *config = dev->config;
152 struct mipi_csi2rx_data *drv_data = dev->data;
153
154 CSI2RX_Init((MIPI_CSI2RX_Type *)config->base, &drv_data->csi2rxConfig);
155
156 if (video_stream_start(config->sensor_dev)) {
157 return -EIO;
158 }
159
160 return 0;
161 }
162
mipi_csi2rx_stream_stop(const struct device * dev)163 static int mipi_csi2rx_stream_stop(const struct device *dev)
164 {
165 const struct mipi_csi2rx_config *config = dev->config;
166
167 if (video_stream_stop(config->sensor_dev)) {
168 return -EIO;
169 }
170
171 CSI2RX_Deinit((MIPI_CSI2RX_Type *)config->base);
172
173 return 0;
174 }
175
mipi_csi2rx_get_caps(const struct device * dev,enum video_endpoint_id ep,struct video_caps * caps)176 static int mipi_csi2rx_get_caps(const struct device *dev, enum video_endpoint_id ep,
177 struct video_caps *caps)
178 {
179 const struct mipi_csi2rx_config *config = dev->config;
180
181 if (ep != VIDEO_EP_OUT && ep != VIDEO_EP_ALL) {
182 return -EINVAL;
183 }
184
185 /* Just forward to sensor dev for now */
186 return video_get_caps(config->sensor_dev, ep, caps);
187 }
188
mipi_csi2rx_set_ctrl(const struct device * dev,unsigned int cid,void * value)189 static inline int mipi_csi2rx_set_ctrl(const struct device *dev, unsigned int cid, void *value)
190 {
191 const struct mipi_csi2rx_config *config = dev->config;
192
193 if (config->sensor_dev) {
194 return video_set_ctrl(config->sensor_dev, cid, value);
195 }
196
197 return -ENOTSUP;
198 }
199
mipi_csi2rx_set_frmival(const struct device * dev,enum video_endpoint_id ep,struct video_frmival * frmival)200 static int mipi_csi2rx_set_frmival(const struct device *dev, enum video_endpoint_id ep,
201 struct video_frmival *frmival)
202 {
203 const struct mipi_csi2rx_config *config = dev->config;
204 int ret;
205
206 ret = video_set_frmival(config->sensor_dev, ep, frmival);
207 if (ret) {
208 LOG_ERR("Cannot set sensor_dev frmival");
209 return ret;
210 }
211
212 ret = mipi_csi2rx_update_settings(dev, ep);
213
214 return ret;
215 }
216
mipi_csi2rx_get_frmival(const struct device * dev,enum video_endpoint_id ep,struct video_frmival * frmival)217 static int mipi_csi2rx_get_frmival(const struct device *dev, enum video_endpoint_id ep,
218 struct video_frmival *frmival)
219 {
220 const struct mipi_csi2rx_config *config = dev->config;
221
222 return video_get_frmival(config->sensor_dev, ep, frmival);
223 }
224
mipi_csi2rx_cal_frame_size(const struct video_format * fmt)225 static uint64_t mipi_csi2rx_cal_frame_size(const struct video_format *fmt)
226 {
227 return fmt->height * fmt->width * video_pix_fmt_bpp(fmt->pixelformat) * 8;
228 }
229
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)230 static uint64_t mipi_csi2rx_estimate_pixel_rate(const struct video_frmival *cur_fmival,
231 const struct video_frmival *fie_frmival,
232 const struct video_format *cur_format,
233 const struct video_format *fie_format,
234 uint64_t cur_pixel_rate, uint8_t laneNum)
235 {
236 return mipi_csi2rx_cal_frame_size(cur_format) * fie_frmival->denominator *
237 cur_fmival->numerator * cur_pixel_rate /
238 (mipi_csi2rx_cal_frame_size(fie_format) * fie_frmival->numerator *
239 cur_fmival->denominator);
240 }
241
mipi_csi2rx_enum_frmival(const struct device * dev,enum video_endpoint_id ep,struct video_frmival_enum * fie)242 static int mipi_csi2rx_enum_frmival(const struct device *dev, enum video_endpoint_id ep,
243 struct video_frmival_enum *fie)
244 {
245 const struct mipi_csi2rx_config *config = dev->config;
246 struct mipi_csi2rx_data *drv_data = dev->data;
247 int ret;
248 uint64_t cur_pixel_rate, est_pixel_rate;
249 struct video_frmival cur_frmival;
250 struct video_format cur_fmt;
251
252 ret = video_enum_frmival(config->sensor_dev, ep, fie);
253 if (ret) {
254 return ret;
255 }
256
257 ret = video_get_ctrl(config->sensor_dev, VIDEO_CID_PIXEL_RATE, &cur_pixel_rate);
258 if (ret) {
259 LOG_ERR("Cannot get sensor_dev pixel rate");
260 return ret;
261 }
262
263 ret = video_get_frmival(config->sensor_dev, ep, &cur_frmival);
264 if (ret) {
265 LOG_ERR("Cannot get sensor_dev frame rate");
266 return ret;
267 }
268
269 ret = video_get_format(config->sensor_dev, ep, &cur_fmt);
270 if (ret) {
271 LOG_ERR("Cannot get sensor_dev format");
272 return ret;
273 }
274
275 if (fie->type == VIDEO_FRMIVAL_TYPE_DISCRETE) {
276 est_pixel_rate = mipi_csi2rx_estimate_pixel_rate(
277 &cur_frmival, &fie->discrete, &cur_fmt, fie->format, cur_pixel_rate,
278 drv_data->csi2rxConfig.laneNum);
279 if (est_pixel_rate > MAX_SUPPORTED_PIXEL_RATE) {
280 return -EINVAL;
281 }
282
283 } else {
284 /* Check the lane rate of the lower bound framerate */
285 est_pixel_rate = mipi_csi2rx_estimate_pixel_rate(
286 &cur_frmival, &fie->stepwise.min, &cur_fmt, fie->format, cur_pixel_rate,
287 drv_data->csi2rxConfig.laneNum);
288 if (est_pixel_rate > MAX_SUPPORTED_PIXEL_RATE) {
289 return -EINVAL;
290 }
291
292 /* Check the lane rate of the upper bound framerate */
293 est_pixel_rate = mipi_csi2rx_estimate_pixel_rate(
294 &cur_frmival, &fie->stepwise.max, &cur_fmt, fie->format, cur_pixel_rate,
295 drv_data->csi2rxConfig.laneNum);
296 if (est_pixel_rate > MAX_SUPPORTED_PIXEL_RATE) {
297 fie->stepwise.max.denominator =
298 (mipi_csi2rx_cal_frame_size(&cur_fmt) * MAX_SUPPORTED_PIXEL_RATE *
299 cur_frmival.denominator) /
300 (mipi_csi2rx_cal_frame_size(fie->format) * cur_pixel_rate *
301 cur_frmival.numerator);
302 fie->stepwise.max.numerator = 1;
303 }
304 }
305
306 return 0;
307 }
308
309 static DEVICE_API(video, mipi_csi2rx_driver_api) = {
310 .get_caps = mipi_csi2rx_get_caps,
311 .get_format = mipi_csi2rx_get_fmt,
312 .set_format = mipi_csi2rx_set_fmt,
313 .stream_start = mipi_csi2rx_stream_start,
314 .stream_stop = mipi_csi2rx_stream_stop,
315 .set_ctrl = mipi_csi2rx_set_ctrl,
316 .set_frmival = mipi_csi2rx_set_frmival,
317 .get_frmival = mipi_csi2rx_get_frmival,
318 .enum_frmival = mipi_csi2rx_enum_frmival,
319 };
320
mipi_csi2rx_init(const struct device * dev)321 static int mipi_csi2rx_init(const struct device *dev)
322 {
323 const struct mipi_csi2rx_config *config = dev->config;
324 struct mipi_csi2rx_data *drv_data = dev->data;
325 int ret;
326
327 /* Check if there is any sensor device */
328 if (!device_is_ready(config->sensor_dev)) {
329 return -ENODEV;
330 }
331
332 /*
333 * CSI2 escape clock should be in the range [60, 80] Mhz. We set it
334 * to 60 Mhz.
335 */
336 ret = clock_control_set_rate(drv_data->clock_dev, drv_data->clock_esc,
337 (clock_control_subsys_rate_t)MHZ(60));
338 if (ret) {
339 return ret;
340 }
341
342 return mipi_csi2rx_update_settings(dev, VIDEO_EP_ALL);
343 }
344
345 #define MIPI_CSI2RX_INIT(n) \
346 static struct mipi_csi2rx_data mipi_csi2rx_data_##n = { \
347 .csi2rxConfig.laneNum = DT_PROP_LEN(DT_INST_ENDPOINT_BY_ID(n, 1, 0), data_lanes), \
348 .clock_dev = DEVICE_DT_GET(DT_INST_CLOCKS_CTLR(n)), \
349 .clock_root = (clock_control_subsys_t)DT_INST_CLOCKS_CELL_BY_IDX(n, 0, name), \
350 .clock_ui = (clock_control_subsys_t)DT_INST_CLOCKS_CELL_BY_IDX(n, 1, name), \
351 .clock_esc = (clock_control_subsys_t)DT_INST_CLOCKS_CELL_BY_IDX(n, 2, name), \
352 }; \
353 \
354 static const struct mipi_csi2rx_config mipi_csi2rx_config_##n = { \
355 .base = (MIPI_CSI2RX_Type *)DT_INST_REG_ADDR(n), \
356 .sensor_dev = \
357 DEVICE_DT_GET(DT_NODE_REMOTE_DEVICE(DT_INST_ENDPOINT_BY_ID(n, 1, 0))), \
358 }; \
359 \
360 DEVICE_DT_INST_DEFINE(n, &mipi_csi2rx_init, NULL, &mipi_csi2rx_data_##n, \
361 &mipi_csi2rx_config_##n, POST_KERNEL, CONFIG_VIDEO_INIT_PRIORITY, \
362 &mipi_csi2rx_driver_api);
363
364 DT_INST_FOREACH_STATUS_OKAY(MIPI_CSI2RX_INIT)
365
