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_stream_start(const struct device * dev)96 static int nxp_video_sdma_stream_start(const struct device *dev)
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 	/* Setup dma configuration for SmartDMA */
104 	sdma_config.dma_slot = kSMARTDMA_CameraDiv16FrameQVGA;
105 	sdma_config.dma_callback = nxp_video_sdma_callback;
106 	sdma_config.user_data = data;
107 	/* Setting bit 1 here enables the SmartDMA to interrupt ARM core
108 	 * when writing to SMARTDMA2ARM register
109 	 */
110 	sdma_config.head_block = (struct dma_block_config *)(((uint32_t)&data->params) | 0x2);
111 
112 	/* Setup parameters for SmartDMA engine */
113 	data->params.smartdma_stack = data->smartdma_stack;
114 	/* SmartDMA continuously streams data once started. If user
115 	 * has not provided a framebuffer, we can't start DMA.
116 	 */
117 	data->queued_buf = k_fifo_get(&data->fifo_in, K_NO_WAIT);
118 	if (data->queued_buf == NULL) {
119 		return -EIO;
120 	}
121 	data->params.p_buffer_ping_pong = (uint32_t *)data->queued_buf->buffer;
122 	/* The firmware writes the index of the frame slice
123 	 * (from 0-15) into this buffer
124 	 */
125 	data->params.p_stripe_index = &data->frame_idx;
126 
127 	/* Start DMA engine */
128 	ret = dma_config(config->dma_dev, 0, &sdma_config);
129 	if (ret < 0) {
130 		return ret;
131 	}
132 	/* Reset stream state variables */
133 	k_sem_reset(&data->stream_empty);
134 	data->buf_reload_flag = true;
135 	data->stream_starved = false;
136 
137 	ret = dma_start(config->dma_dev, 0);
138 	if (ret < 0) {
139 		return ret;
140 	}
141 
142 	return 0;
143 }
144 
nxp_video_sdma_stream_stop(const struct device * dev)145 static int nxp_video_sdma_stream_stop(const struct device *dev)
146 {
147 	const struct nxp_video_sdma_config *config = dev->config;
148 
149 	/* Stop DMA engine */
150 	return dma_stop(config->dma_dev, 0);
151 }
152 
nxp_video_sdma_enqueue(const struct device * dev,enum video_endpoint_id ep,struct video_buffer * vbuf)153 static int nxp_video_sdma_enqueue(const struct device *dev,
154 				  enum video_endpoint_id ep,
155 				  struct video_buffer *vbuf)
156 {
157 	struct nxp_video_sdma_data *data = dev->data;
158 
159 	if (ep != VIDEO_EP_OUT) {
160 		return -EINVAL;
161 	}
162 
163 	/* SmartDMA will read 30 lines of RGB565 video data into framebuffer */
164 	vbuf->bytesused = SDMA_VBUF_WIDTH * SDMA_LINE_COUNT * sizeof(uint16_t);
165 	if (vbuf->size < vbuf->bytesused) {
166 		return -EINVAL;
167 	}
168 
169 	/* Put buffer into FIFO */
170 	k_fifo_put(&data->fifo_in, vbuf);
171 	if (data->stream_starved) {
172 		/* Kick SmartDMA off */
173 		nxp_video_sdma_stream_start(dev);
174 	}
175 	return 0;
176 }
177 
nxp_video_sdma_dequeue(const struct device * dev,enum video_endpoint_id ep,struct video_buffer ** vbuf,k_timeout_t timeout)178 static int nxp_video_sdma_dequeue(const struct device *dev,
179 				  enum video_endpoint_id ep,
180 				  struct video_buffer **vbuf,
181 				  k_timeout_t timeout)
182 {
183 	struct nxp_video_sdma_data *data = dev->data;
184 
185 	if (ep != VIDEO_EP_OUT) {
186 		return -EINVAL;
187 	}
188 
189 	*vbuf = k_fifo_get(&data->fifo_out, timeout);
190 	if (*vbuf == NULL) {
191 		return -EAGAIN;
192 	}
193 
194 	return 0;
195 }
196 
nxp_video_sdma_flush(const struct device * dev,enum video_endpoint_id ep,bool cancel)197 static int nxp_video_sdma_flush(const struct device *dev,
198 				enum video_endpoint_id ep,
199 				bool cancel)
200 {
201 	const struct nxp_video_sdma_config *config = dev->config;
202 	struct nxp_video_sdma_data *data = dev->data;
203 	struct video_buf *vbuf;
204 
205 	if (!cancel) {
206 		/* Wait for DMA to signal it is empty */
207 		k_sem_take(&data->stream_empty, K_FOREVER);
208 	} else {
209 		/* Stop DMA engine */
210 		dma_stop(config->dma_dev, 0);
211 		/* Forward all buffers in fifo_in to fifo_out */
212 		while ((vbuf = k_fifo_get(&data->fifo_in, K_NO_WAIT))) {
213 			k_fifo_put(&data->fifo_out, vbuf);
214 		}
215 	}
216 	return 0;
217 }
218 
219 /* SDMA only supports 320x240 RGB565 */
220 static const struct video_format_cap fmts[] = {
221 	{
222 		.pixelformat = VIDEO_PIX_FMT_RGB565,
223 		.width_min = SDMA_VBUF_WIDTH,
224 		.width_max = SDMA_VBUF_WIDTH,
225 		.height_min = SDMA_VBUF_HEIGHT,
226 		.height_max = SDMA_VBUF_HEIGHT,
227 		.width_step = 0,
228 		.height_step = 0,
229 	},
230 	{ 0 },
231 };
232 
nxp_video_sdma_set_format(const struct device * dev,enum video_endpoint_id ep,struct video_format * fmt)233 static int nxp_video_sdma_set_format(const struct device *dev,
234 				     enum video_endpoint_id ep,
235 				     struct video_format *fmt)
236 {
237 	const struct nxp_video_sdma_config *config = dev->config;
238 
239 	if (fmt == NULL || ep != VIDEO_EP_OUT)  {
240 		return -EINVAL;
241 	}
242 
243 	if (!device_is_ready(config->sensor_dev)) {
244 		LOG_ERR("Sensor device not ready");
245 		return -ENODEV;
246 	}
247 
248 	if ((fmt->pixelformat != fmts[0].pixelformat) ||
249 	    (fmt->width != fmts[0].width_min) ||
250 	    (fmt->height != fmts[0].height_min) ||
251 	    (fmt->pitch != fmts[0].width_min * 2)) {
252 		LOG_ERR("Unsupported format");
253 		return -ENOTSUP;
254 	}
255 
256 	/* Forward format to sensor device */
257 	return video_set_format(config->sensor_dev, ep, fmt);
258 }
259 
nxp_video_sdma_get_format(const struct device * dev,enum video_endpoint_id ep,struct video_format * fmt)260 static int nxp_video_sdma_get_format(const struct device *dev,
261 				     enum video_endpoint_id ep,
262 				     struct video_format *fmt)
263 {
264 	const struct nxp_video_sdma_config *config = dev->config;
265 	int ret;
266 
267 	if (fmt == NULL || ep != VIDEO_EP_OUT)  {
268 		return -EINVAL;
269 	}
270 
271 	if (!device_is_ready(config->sensor_dev)) {
272 		LOG_ERR("Sensor device not ready");
273 		return -ENODEV;
274 	}
275 
276 	/*
277 	 * Check sensor format. If it is not RGB565 320x240
278 	 * reconfigure the sensor,
279 	 * as this is the only format supported.
280 	 */
281 	ret = video_get_format(config->sensor_dev, VIDEO_EP_OUT, fmt);
282 	if (ret < 0) {
283 		return ret;
284 	}
285 
286 	/* Verify that format is RGB565 */
287 	if ((fmt->pixelformat != fmts[0].pixelformat) ||
288 	    (fmt->width != fmts[0].width_min) ||
289 	    (fmt->height != fmts[0].height_min) ||
290 	    (fmt->pitch != fmts[0].width_min * 2)) {
291 		/* Update format of sensor */
292 		fmt->pixelformat = fmts[0].pixelformat;
293 		fmt->width = fmts[0].width_min;
294 		fmt->height = fmts[0].height_min;
295 		fmt->pitch = fmts[0].width_min * 2;
296 		ret = video_set_format(config->sensor_dev, VIDEO_EP_OUT, fmt);
297 		if (ret < 0) {
298 			LOG_ERR("Sensor device does not support RGB565");
299 			return ret;
300 		}
301 	}
302 
303 	return 0;
304 }
305 
nxp_video_sdma_get_caps(const struct device * dev,enum video_endpoint_id ep,struct video_caps * caps)306 static int nxp_video_sdma_get_caps(const struct device *dev,
307 				   enum video_endpoint_id ep,
308 				   struct video_caps *caps)
309 {
310 	if (ep != VIDEO_EP_OUT) {
311 		return -EINVAL;
312 	}
313 
314 	/* SmartDMA needs at least two buffers allocated before starting */
315 	caps->min_vbuf_count = 2;
316 	/* Firmware reads 30 lines per queued vbuf */
317 	caps->min_line_count = caps->max_line_count = SDMA_LINE_COUNT;
318 	caps->format_caps = fmts;
319 	return 0;
320 }
321 
nxp_video_sdma_init(const struct device * dev)322 static int nxp_video_sdma_init(const struct device *dev)
323 {
324 	const struct nxp_video_sdma_config *config = dev->config;
325 	struct nxp_video_sdma_data *data = dev->data;
326 	int ret;
327 
328 	if (!device_is_ready(config->dma_dev)) {
329 		LOG_ERR("SmartDMA not ready");
330 		return -ENODEV;
331 	}
332 
333 	INPUTMUX_Init(INPUTMUX0);
334 	/* Attach Camera VSYNC, HSYNC, and PCLK as inputs 0, 1, and 2 of the SmartDMA */
335 	INPUTMUX_AttachSignal(INPUTMUX0, 0,
336 			      config->vsync_pin + (SMARTDMAARCHB_INMUX0 << PMUX_SHIFT));
337 	INPUTMUX_AttachSignal(INPUTMUX0, 1,
338 			      config->hsync_pin + (SMARTDMAARCHB_INMUX0 << PMUX_SHIFT));
339 	INPUTMUX_AttachSignal(INPUTMUX0, 2,
340 			      config->pclk_pin + (SMARTDMAARCHB_INMUX0 << PMUX_SHIFT));
341 	/* Turnoff clock to inputmux to save power. Clock is only needed to make changes */
342 	INPUTMUX_Deinit(INPUTMUX0);
343 
344 	k_fifo_init(&data->fifo_in);
345 	k_fifo_init(&data->fifo_out);
346 	/* Given to when the DMA engine runs out of buffers */
347 	k_sem_init(&data->stream_empty, 0, 1);
348 
349 	/* Install camera firmware used by SmartDMA */
350 	dma_smartdma_install_fw(config->dma_dev, (uint8_t *)s_smartdmaCameraFirmware,
351 				s_smartdmaCameraFirmwareSize);
352 	ret = pinctrl_apply_state(config->pincfg, PINCTRL_STATE_DEFAULT);
353 	if (ret < 0) {
354 		return ret;
355 	}
356 
357 	return 0;
358 }
359 
360 static DEVICE_API(video, nxp_video_sdma_api) = {
361 	.get_format = nxp_video_sdma_get_format,
362 	.set_format = nxp_video_sdma_set_format,
363 	.get_caps = nxp_video_sdma_get_caps,
364 	.stream_start = nxp_video_sdma_stream_start,
365 	.stream_stop = nxp_video_sdma_stream_stop,
366 	.enqueue = nxp_video_sdma_enqueue,
367 	.dequeue = nxp_video_sdma_dequeue,
368 	.flush = nxp_video_sdma_flush
369 };
370 
371 #define NXP_VIDEO_SDMA_INIT(inst)						\
372 	PINCTRL_DT_INST_DEFINE(inst);						\
373 	const struct nxp_video_sdma_config sdma_config_##inst = {		\
374 		.dma_dev = DEVICE_DT_GET(DT_INST_PARENT(inst)),			\
375 		.sensor_dev = DEVICE_DT_GET(DT_INST_PHANDLE(inst, sensor)),	\
376 		.pincfg = PINCTRL_DT_INST_DEV_CONFIG_GET(inst),			\
377 		.vsync_pin = DT_INST_PROP(inst, vsync_pin),			\
378 		.hsync_pin = DT_INST_PROP(inst, hsync_pin),			\
379 		.pclk_pin = DT_INST_PROP(inst, pclk_pin),			\
380 	};									\
381 	struct nxp_video_sdma_data sdma_data_##inst = {				\
382 		.config = &sdma_config_##inst,					\
383 	};									\
384 										\
385 	DEVICE_DT_INST_DEFINE(inst, nxp_video_sdma_init, NULL,			\
386 				&sdma_data_##inst, &sdma_config_##inst,		\
387 				POST_KERNEL,					\
388 				CONFIG_KERNEL_INIT_PRIORITY_DEVICE,		\
389 				&nxp_video_sdma_api);
390 
391 DT_INST_FOREACH_STATUS_OKAY(NXP_VIDEO_SDMA_INIT)
392