1 /*
2 * Copyright 2024 NXP
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #define DT_DRV_COMPAT nxp_video_smartdma
8
9 #include <fsl_smartdma.h>
10 #include <fsl_inputmux.h>
11
12 #include <zephyr/drivers/dma.h>
13 #include <zephyr/drivers/dma/dma_mcux_smartdma.h>
14 #include <zephyr/drivers/video.h>
15 #include <zephyr/drivers/pinctrl.h>
16
17 #define LOG_LEVEL CONFIG_LOG_DEFAULT_LEVEL
18 #include <zephyr/logging/log.h>
19 LOG_MODULE_REGISTER(nxp_video_sdma);
20
21 struct nxp_video_sdma_config {
22 const struct device *dma_dev;
23 const struct device *sensor_dev;
24 const struct pinctrl_dev_config *pincfg;
25 uint8_t vsync_pin;
26 uint8_t hsync_pin;
27 uint8_t pclk_pin;
28 };
29
30 /* Firmware reads 30 lines of data per video buffer */
31 #define SDMA_LINE_COUNT 30
32 /* Firmware only supports 320x240 */
33 #define SDMA_VBUF_HEIGHT 240
34 #define SDMA_VBUF_WIDTH 320
35
36 struct nxp_video_sdma_data {
37 /* Must be aligned on 4 byte boundary, as lower 2 bits of ARM2SDMA register
38 * are used to enable interrupts
39 */
40 smartdma_camera_param_t params __aligned(4);
41 uint32_t smartdma_stack[64] __aligned(32);
42 struct k_fifo fifo_in;
43 struct k_fifo fifo_out;
44 struct k_sem stream_empty; /* Signals stream has run out of buffers */
45 bool stream_starved;
46 bool buf_reload_flag;
47 struct video_buffer *active_buf;
48 struct video_buffer *queued_buf;
49 const struct nxp_video_sdma_config *config;
50 uint32_t frame_idx;
51 };
52
53 /* Executed in interrupt context */
nxp_video_sdma_callback(const struct device * dev,void * user_data,uint32_t channel,int status)54 static void nxp_video_sdma_callback(const struct device *dev, void *user_data,
55 uint32_t channel, int status)
56 {
57 struct nxp_video_sdma_data *data = user_data;
58
59 if (status < 0) {
60 LOG_ERR("Transfer failed: %d, stopping DMA", status);
61 dma_stop(data->config->dma_dev, 0);
62 return;
63 }
64 /*
65 * SmartDMA engine streams 15 lines of RGB565 data, then interrupts the
66 * system. The engine will reload the framebuffer pointer after sending
67 * the first interrupt, and before sending the second interrupt.
68 *
69 * Based on this, we alternate between reloading the framebuffer
70 * pointer and queueing a completed frame every other interrupt
71 */
72 if (data->buf_reload_flag) {
73 /* Save old framebuffer, we will dequeue it next interrupt */
74 data->active_buf = data->queued_buf;
75 /* Load new framebuffer */
76 data->queued_buf = k_fifo_get(&data->fifo_in, K_NO_WAIT);
77 if (data->queued_buf == NULL) {
78 data->stream_starved = true;
79 } else {
80 data->params.p_buffer_ping_pong = (uint32_t *)data->queued_buf->buffer;
81 }
82 } else {
83 if (data->stream_starved) {
84 /* Signal any waiting threads */
85 k_sem_give(&data->stream_empty);
86 }
87 data->active_buf->line_offset = (data->frame_idx / 2) * SDMA_LINE_COUNT;
88 data->active_buf->timestamp = k_uptime_get_32();
89 k_fifo_put(&data->fifo_out, data->active_buf);
90
91 }
92 /* Toggle buffer reload flag*/
93 data->buf_reload_flag = !data->buf_reload_flag;
94 }
95
nxp_video_sdma_set_stream(const struct device * dev,bool enable)96 static int nxp_video_sdma_set_stream(const struct device *dev, bool enable)
97 {
98 const struct nxp_video_sdma_config *config = dev->config;
99 struct nxp_video_sdma_data *data = dev->data;
100 struct dma_config sdma_config = {0};
101 int ret;
102
103 if (!enable) {
104 return dma_stop(config->dma_dev, 0);
105 }
106
107 /* Setup dma configuration for SmartDMA */
108 sdma_config.dma_slot = kSMARTDMA_CameraDiv16FrameQVGA;
109 sdma_config.dma_callback = nxp_video_sdma_callback;
110 sdma_config.user_data = data;
111 /* Setting bit 1 here enables the SmartDMA to interrupt ARM core
112 * when writing to SMARTDMA2ARM register
113 */
114 sdma_config.head_block = (struct dma_block_config *)(((uint32_t)&data->params) | 0x2);
115
116 /* Setup parameters for SmartDMA engine */
117 data->params.smartdma_stack = data->smartdma_stack;
118 /* SmartDMA continuously streams data once started. If user
119 * has not provided a framebuffer, we can't start DMA.
120 */
121 data->queued_buf = k_fifo_get(&data->fifo_in, K_NO_WAIT);
122 if (data->queued_buf == NULL) {
123 return -EIO;
124 }
125 data->params.p_buffer_ping_pong = (uint32_t *)data->queued_buf->buffer;
126 /* The firmware writes the index of the frame slice
127 * (from 0-15) into this buffer
128 */
129 data->params.p_stripe_index = &data->frame_idx;
130
131 /* Start DMA engine */
132 ret = dma_config(config->dma_dev, 0, &sdma_config);
133 if (ret < 0) {
134 return ret;
135 }
136 /* Reset stream state variables */
137 k_sem_reset(&data->stream_empty);
138 data->buf_reload_flag = true;
139 data->stream_starved = false;
140
141 ret = dma_start(config->dma_dev, 0);
142 if (ret < 0) {
143 return ret;
144 }
145
146 return 0;
147 }
148
nxp_video_sdma_enqueue(const struct device * dev,enum video_endpoint_id ep,struct video_buffer * vbuf)149 static int nxp_video_sdma_enqueue(const struct device *dev,
150 enum video_endpoint_id ep,
151 struct video_buffer *vbuf)
152 {
153 struct nxp_video_sdma_data *data = dev->data;
154
155 if (ep != VIDEO_EP_OUT) {
156 return -EINVAL;
157 }
158
159 /* SmartDMA will read 30 lines of RGB565 video data into framebuffer */
160 vbuf->bytesused = SDMA_VBUF_WIDTH * SDMA_LINE_COUNT * sizeof(uint16_t);
161 if (vbuf->size < vbuf->bytesused) {
162 return -EINVAL;
163 }
164
165 /* Put buffer into FIFO */
166 k_fifo_put(&data->fifo_in, vbuf);
167 if (data->stream_starved) {
168 /* Kick SmartDMA off */
169 nxp_video_sdma_set_stream(dev, true);
170 }
171 return 0;
172 }
173
nxp_video_sdma_dequeue(const struct device * dev,enum video_endpoint_id ep,struct video_buffer ** vbuf,k_timeout_t timeout)174 static int nxp_video_sdma_dequeue(const struct device *dev,
175 enum video_endpoint_id ep,
176 struct video_buffer **vbuf,
177 k_timeout_t timeout)
178 {
179 struct nxp_video_sdma_data *data = dev->data;
180
181 if (ep != VIDEO_EP_OUT) {
182 return -EINVAL;
183 }
184
185 *vbuf = k_fifo_get(&data->fifo_out, timeout);
186 if (*vbuf == NULL) {
187 return -EAGAIN;
188 }
189
190 return 0;
191 }
192
nxp_video_sdma_flush(const struct device * dev,enum video_endpoint_id ep,bool cancel)193 static int nxp_video_sdma_flush(const struct device *dev,
194 enum video_endpoint_id ep,
195 bool cancel)
196 {
197 const struct nxp_video_sdma_config *config = dev->config;
198 struct nxp_video_sdma_data *data = dev->data;
199 struct video_buf *vbuf;
200
201 if (!cancel) {
202 /* Wait for DMA to signal it is empty */
203 k_sem_take(&data->stream_empty, K_FOREVER);
204 } else {
205 /* Stop DMA engine */
206 dma_stop(config->dma_dev, 0);
207 /* Forward all buffers in fifo_in to fifo_out */
208 while ((vbuf = k_fifo_get(&data->fifo_in, K_NO_WAIT))) {
209 k_fifo_put(&data->fifo_out, vbuf);
210 }
211 }
212 return 0;
213 }
214
215 /* SDMA only supports 320x240 RGB565 */
216 static const struct video_format_cap fmts[] = {
217 {
218 .pixelformat = VIDEO_PIX_FMT_RGB565,
219 .width_min = SDMA_VBUF_WIDTH,
220 .width_max = SDMA_VBUF_WIDTH,
221 .height_min = SDMA_VBUF_HEIGHT,
222 .height_max = SDMA_VBUF_HEIGHT,
223 .width_step = 0,
224 .height_step = 0,
225 },
226 { 0 },
227 };
228
nxp_video_sdma_set_format(const struct device * dev,enum video_endpoint_id ep,struct video_format * fmt)229 static int nxp_video_sdma_set_format(const struct device *dev,
230 enum video_endpoint_id ep,
231 struct video_format *fmt)
232 {
233 const struct nxp_video_sdma_config *config = dev->config;
234
235 if (fmt == NULL || ep != VIDEO_EP_OUT) {
236 return -EINVAL;
237 }
238
239 if (!device_is_ready(config->sensor_dev)) {
240 LOG_ERR("Sensor device not ready");
241 return -ENODEV;
242 }
243
244 if ((fmt->pixelformat != fmts[0].pixelformat) ||
245 (fmt->width != fmts[0].width_min) ||
246 (fmt->height != fmts[0].height_min) ||
247 (fmt->pitch != fmts[0].width_min * 2)) {
248 LOG_ERR("Unsupported format");
249 return -ENOTSUP;
250 }
251
252 /* Forward format to sensor device */
253 return video_set_format(config->sensor_dev, ep, fmt);
254 }
255
nxp_video_sdma_get_format(const struct device * dev,enum video_endpoint_id ep,struct video_format * fmt)256 static int nxp_video_sdma_get_format(const struct device *dev,
257 enum video_endpoint_id ep,
258 struct video_format *fmt)
259 {
260 const struct nxp_video_sdma_config *config = dev->config;
261 int ret;
262
263 if (fmt == NULL || ep != VIDEO_EP_OUT) {
264 return -EINVAL;
265 }
266
267 if (!device_is_ready(config->sensor_dev)) {
268 LOG_ERR("Sensor device not ready");
269 return -ENODEV;
270 }
271
272 /*
273 * Check sensor format. If it is not RGB565 320x240
274 * reconfigure the sensor,
275 * as this is the only format supported.
276 */
277 ret = video_get_format(config->sensor_dev, VIDEO_EP_OUT, fmt);
278 if (ret < 0) {
279 return ret;
280 }
281
282 /* Verify that format is RGB565 */
283 if ((fmt->pixelformat != fmts[0].pixelformat) ||
284 (fmt->width != fmts[0].width_min) ||
285 (fmt->height != fmts[0].height_min) ||
286 (fmt->pitch != fmts[0].width_min * 2)) {
287 /* Update format of sensor */
288 fmt->pixelformat = fmts[0].pixelformat;
289 fmt->width = fmts[0].width_min;
290 fmt->height = fmts[0].height_min;
291 fmt->pitch = fmts[0].width_min * 2;
292 ret = video_set_format(config->sensor_dev, VIDEO_EP_OUT, fmt);
293 if (ret < 0) {
294 LOG_ERR("Sensor device does not support RGB565");
295 return ret;
296 }
297 }
298
299 return 0;
300 }
301
nxp_video_sdma_get_caps(const struct device * dev,enum video_endpoint_id ep,struct video_caps * caps)302 static int nxp_video_sdma_get_caps(const struct device *dev,
303 enum video_endpoint_id ep,
304 struct video_caps *caps)
305 {
306 if (ep != VIDEO_EP_OUT) {
307 return -EINVAL;
308 }
309
310 /* SmartDMA needs at least two buffers allocated before starting */
311 caps->min_vbuf_count = 2;
312 /* Firmware reads 30 lines per queued vbuf */
313 caps->min_line_count = caps->max_line_count = SDMA_LINE_COUNT;
314 caps->format_caps = fmts;
315 return 0;
316 }
317
nxp_video_sdma_init(const struct device * dev)318 static int nxp_video_sdma_init(const struct device *dev)
319 {
320 const struct nxp_video_sdma_config *config = dev->config;
321 struct nxp_video_sdma_data *data = dev->data;
322 int ret;
323
324 if (!device_is_ready(config->dma_dev)) {
325 LOG_ERR("SmartDMA not ready");
326 return -ENODEV;
327 }
328
329 INPUTMUX_Init(INPUTMUX0);
330 /* Attach Camera VSYNC, HSYNC, and PCLK as inputs 0, 1, and 2 of the SmartDMA */
331 INPUTMUX_AttachSignal(INPUTMUX0, 0,
332 config->vsync_pin + (SMARTDMAARCHB_INMUX0 << PMUX_SHIFT));
333 INPUTMUX_AttachSignal(INPUTMUX0, 1,
334 config->hsync_pin + (SMARTDMAARCHB_INMUX0 << PMUX_SHIFT));
335 INPUTMUX_AttachSignal(INPUTMUX0, 2,
336 config->pclk_pin + (SMARTDMAARCHB_INMUX0 << PMUX_SHIFT));
337 /* Turnoff clock to inputmux to save power. Clock is only needed to make changes */
338 INPUTMUX_Deinit(INPUTMUX0);
339
340 k_fifo_init(&data->fifo_in);
341 k_fifo_init(&data->fifo_out);
342 /* Given to when the DMA engine runs out of buffers */
343 k_sem_init(&data->stream_empty, 0, 1);
344
345 /* Install camera firmware used by SmartDMA */
346 dma_smartdma_install_fw(config->dma_dev, (uint8_t *)s_smartdmaCameraFirmware,
347 s_smartdmaCameraFirmwareSize);
348 ret = pinctrl_apply_state(config->pincfg, PINCTRL_STATE_DEFAULT);
349 if (ret < 0) {
350 return ret;
351 }
352
353 return 0;
354 }
355
356 static DEVICE_API(video, nxp_video_sdma_api) = {
357 .get_format = nxp_video_sdma_get_format,
358 .set_format = nxp_video_sdma_set_format,
359 .get_caps = nxp_video_sdma_get_caps,
360 .set_stream = nxp_video_sdma_set_stream,
361 .enqueue = nxp_video_sdma_enqueue,
362 .dequeue = nxp_video_sdma_dequeue,
363 .flush = nxp_video_sdma_flush
364 };
365
366 #define NXP_VIDEO_SDMA_INIT(inst) \
367 PINCTRL_DT_INST_DEFINE(inst); \
368 const struct nxp_video_sdma_config sdma_config_##inst = { \
369 .dma_dev = DEVICE_DT_GET(DT_INST_PARENT(inst)), \
370 .sensor_dev = DEVICE_DT_GET(DT_INST_PHANDLE(inst, sensor)), \
371 .pincfg = PINCTRL_DT_INST_DEV_CONFIG_GET(inst), \
372 .vsync_pin = DT_INST_PROP(inst, vsync_pin), \
373 .hsync_pin = DT_INST_PROP(inst, hsync_pin), \
374 .pclk_pin = DT_INST_PROP(inst, pclk_pin), \
375 }; \
376 struct nxp_video_sdma_data sdma_data_##inst = { \
377 .config = &sdma_config_##inst, \
378 }; \
379 \
380 DEVICE_DT_INST_DEFINE(inst, nxp_video_sdma_init, NULL, \
381 &sdma_data_##inst, &sdma_config_##inst, \
382 POST_KERNEL, \
383 CONFIG_KERNEL_INIT_PRIORITY_DEVICE, \
384 &nxp_video_sdma_api);
385
386 DT_INST_FOREACH_STATUS_OKAY(NXP_VIDEO_SDMA_INIT)
387
