1 // SPDX-License-Identifier: GPL-2.0+
2 /*
3  * Driver for Renesas RZ/G2L CRU
4  *
5  * Copyright (C) 2022 Renesas Electronics Corp.
6  *
7  * Based on Renesas R-Car VIN
8  * Copyright (C) 2016 Renesas Electronics Corp.
9  * Copyright (C) 2011-2013 Renesas Solutions Corp.
10  * Copyright (C) 2013 Cogent Embedded, Inc., <source@cogentembedded.com>
11  * Copyright (C) 2008 Magnus Damm
12  */
13 
14 #include <linux/clk.h>
15 #include <linux/delay.h>
16 #include <linux/pm_runtime.h>
17 
18 #include <media/v4l2-ioctl.h>
19 #include <media/videobuf2-dma-contig.h>
20 
21 #include "rzg2l-cru.h"
22 
23 /* HW CRU Registers Definition */
24 
25 /* CRU Control Register */
26 #define CRUnCTRL			0x0
27 #define CRUnCTRL_VINSEL(x)		((x) << 0)
28 
29 /* CRU Interrupt Enable Register */
30 #define CRUnIE				0x4
31 #define CRUnIE_EFE			BIT(17)
32 
33 /* CRU Interrupt Status Register */
34 #define CRUnINTS			0x8
35 #define CRUnINTS_SFS			BIT(16)
36 
37 /* CRU Reset Register */
38 #define CRUnRST				0xc
39 #define CRUnRST_VRESETN			BIT(0)
40 
41 /* Memory Bank Base Address (Lower) Register for CRU Image Data */
42 #define AMnMBxADDRL(x)			(0x100 + ((x) * 8))
43 
44 /* Memory Bank Base Address (Higher) Register for CRU Image Data */
45 #define AMnMBxADDRH(x)			(0x104 + ((x) * 8))
46 
47 /* Memory Bank Enable Register for CRU Image Data */
48 #define AMnMBVALID			0x148
49 #define AMnMBVALID_MBVALID(x)		GENMASK(x, 0)
50 
51 /* Memory Bank Status Register for CRU Image Data */
52 #define AMnMBS				0x14c
53 #define AMnMBS_MBSTS			0x7
54 
55 /* AXI Master FIFO Pointer Register for CRU Image Data */
56 #define AMnFIFOPNTR			0x168
57 #define AMnFIFOPNTR_FIFOWPNTR		GENMASK(7, 0)
58 #define AMnFIFOPNTR_FIFORPNTR_Y		GENMASK(23, 16)
59 
60 /* AXI Master Transfer Stop Register for CRU Image Data */
61 #define AMnAXISTP			0x174
62 #define AMnAXISTP_AXI_STOP		BIT(0)
63 
64 /* AXI Master Transfer Stop Status Register for CRU Image Data */
65 #define AMnAXISTPACK			0x178
66 #define AMnAXISTPACK_AXI_STOP_ACK	BIT(0)
67 
68 /* CRU Image Processing Enable Register */
69 #define ICnEN				0x200
70 #define ICnEN_ICEN			BIT(0)
71 
72 /* CRU Image Processing Main Control Register */
73 #define ICnMC				0x208
74 #define ICnMC_CSCTHR			BIT(5)
75 #define ICnMC_INF_YUV8_422		(0x1e << 16)
76 #define ICnMC_INF_USER			(0x30 << 16)
77 #define ICnMC_VCSEL(x)			((x) << 22)
78 #define ICnMC_INF_MASK			GENMASK(21, 16)
79 
80 /* CRU Module Status Register */
81 #define ICnMS				0x254
82 #define ICnMS_IA			BIT(2)
83 
84 /* CRU Data Output Mode Register */
85 #define ICnDMR				0x26c
86 #define ICnDMR_YCMODE_UYVY		(1 << 4)
87 
88 #define RZG2L_TIMEOUT_MS		100
89 #define RZG2L_RETRIES			10
90 
91 #define RZG2L_CRU_DEFAULT_FORMAT	V4L2_PIX_FMT_UYVY
92 #define RZG2L_CRU_DEFAULT_WIDTH		RZG2L_CRU_MIN_INPUT_WIDTH
93 #define RZG2L_CRU_DEFAULT_HEIGHT	RZG2L_CRU_MIN_INPUT_HEIGHT
94 #define RZG2L_CRU_DEFAULT_FIELD		V4L2_FIELD_NONE
95 #define RZG2L_CRU_DEFAULT_COLORSPACE	V4L2_COLORSPACE_SRGB
96 
97 struct rzg2l_cru_buffer {
98 	struct vb2_v4l2_buffer vb;
99 	struct list_head list;
100 };
101 
102 #define to_buf_list(vb2_buffer) \
103 	(&container_of(vb2_buffer, struct rzg2l_cru_buffer, vb)->list)
104 
105 /* -----------------------------------------------------------------------------
106  * DMA operations
107  */
108 static void rzg2l_cru_write(struct rzg2l_cru_dev *cru, u32 offset, u32 value)
109 {
110 	iowrite32(value, cru->base + offset);
111 }
112 
113 static u32 rzg2l_cru_read(struct rzg2l_cru_dev *cru, u32 offset)
114 {
115 	return ioread32(cru->base + offset);
116 }
117 
118 /* Need to hold qlock before calling */
119 static void return_unused_buffers(struct rzg2l_cru_dev *cru,
120 				  enum vb2_buffer_state state)
121 {
122 	struct rzg2l_cru_buffer *buf, *node;
123 	unsigned long flags;
124 	unsigned int i;
125 
126 	spin_lock_irqsave(&cru->qlock, flags);
127 	for (i = 0; i < cru->num_buf; i++) {
128 		if (cru->queue_buf[i]) {
129 			vb2_buffer_done(&cru->queue_buf[i]->vb2_buf,
130 					state);
131 			cru->queue_buf[i] = NULL;
132 		}
133 	}
134 
135 	list_for_each_entry_safe(buf, node, &cru->buf_list, list) {
136 		vb2_buffer_done(&buf->vb.vb2_buf, state);
137 		list_del(&buf->list);
138 	}
139 	spin_unlock_irqrestore(&cru->qlock, flags);
140 }
141 
142 static int rzg2l_cru_queue_setup(struct vb2_queue *vq, unsigned int *nbuffers,
143 				 unsigned int *nplanes, unsigned int sizes[],
144 				 struct device *alloc_devs[])
145 {
146 	struct rzg2l_cru_dev *cru = vb2_get_drv_priv(vq);
147 
148 	/* Make sure the image size is large enough. */
149 	if (*nplanes)
150 		return sizes[0] < cru->format.sizeimage ? -EINVAL : 0;
151 
152 	*nplanes = 1;
153 	sizes[0] = cru->format.sizeimage;
154 
155 	return 0;
156 };
157 
158 static int rzg2l_cru_buffer_prepare(struct vb2_buffer *vb)
159 {
160 	struct rzg2l_cru_dev *cru = vb2_get_drv_priv(vb->vb2_queue);
161 	unsigned long size = cru->format.sizeimage;
162 
163 	if (vb2_plane_size(vb, 0) < size) {
164 		dev_err(cru->dev, "buffer too small (%lu < %lu)\n",
165 			vb2_plane_size(vb, 0), size);
166 		return -EINVAL;
167 	}
168 
169 	vb2_set_plane_payload(vb, 0, size);
170 
171 	return 0;
172 }
173 
174 static void rzg2l_cru_buffer_queue(struct vb2_buffer *vb)
175 {
176 	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
177 	struct rzg2l_cru_dev *cru = vb2_get_drv_priv(vb->vb2_queue);
178 	unsigned long flags;
179 
180 	spin_lock_irqsave(&cru->qlock, flags);
181 
182 	list_add_tail(to_buf_list(vbuf), &cru->buf_list);
183 
184 	spin_unlock_irqrestore(&cru->qlock, flags);
185 }
186 
187 static int rzg2l_cru_mc_validate_format(struct rzg2l_cru_dev *cru,
188 					struct v4l2_subdev *sd,
189 					struct media_pad *pad)
190 {
191 	struct v4l2_subdev_format fmt = {
192 		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
193 	};
194 
195 	fmt.pad = pad->index;
196 	if (v4l2_subdev_call_state_active(sd, pad, get_fmt, &fmt))
197 		return -EPIPE;
198 
199 	switch (fmt.format.code) {
200 	case MEDIA_BUS_FMT_UYVY8_1X16:
201 		break;
202 	default:
203 		return -EPIPE;
204 	}
205 
206 	switch (fmt.format.field) {
207 	case V4L2_FIELD_TOP:
208 	case V4L2_FIELD_BOTTOM:
209 	case V4L2_FIELD_NONE:
210 	case V4L2_FIELD_INTERLACED_TB:
211 	case V4L2_FIELD_INTERLACED_BT:
212 	case V4L2_FIELD_INTERLACED:
213 	case V4L2_FIELD_SEQ_TB:
214 	case V4L2_FIELD_SEQ_BT:
215 		break;
216 	default:
217 		return -EPIPE;
218 	}
219 
220 	if (fmt.format.width != cru->format.width ||
221 	    fmt.format.height != cru->format.height)
222 		return -EPIPE;
223 
224 	return 0;
225 }
226 
227 static void rzg2l_cru_set_slot_addr(struct rzg2l_cru_dev *cru,
228 				    int slot, dma_addr_t addr)
229 {
230 	/*
231 	 * The address needs to be 512 bytes aligned. Driver should never accept
232 	 * settings that do not satisfy this in the first place...
233 	 */
234 	if (WARN_ON((addr) & RZG2L_CRU_HW_BUFFER_MASK))
235 		return;
236 
237 	/* Currently, we just use the buffer in 32 bits address */
238 	rzg2l_cru_write(cru, AMnMBxADDRL(slot), addr);
239 	rzg2l_cru_write(cru, AMnMBxADDRH(slot), 0);
240 }
241 
242 /*
243  * Moves a buffer from the queue to the HW slot. If no buffer is
244  * available use the scratch buffer. The scratch buffer is never
245  * returned to userspace, its only function is to enable the capture
246  * loop to keep running.
247  */
248 static void rzg2l_cru_fill_hw_slot(struct rzg2l_cru_dev *cru, int slot)
249 {
250 	struct vb2_v4l2_buffer *vbuf;
251 	struct rzg2l_cru_buffer *buf;
252 	dma_addr_t phys_addr;
253 
254 	/* A already populated slot shall never be overwritten. */
255 	if (WARN_ON(cru->queue_buf[slot]))
256 		return;
257 
258 	dev_dbg(cru->dev, "Filling HW slot: %d\n", slot);
259 
260 	if (list_empty(&cru->buf_list)) {
261 		cru->queue_buf[slot] = NULL;
262 		phys_addr = cru->scratch_phys;
263 	} else {
264 		/* Keep track of buffer we give to HW */
265 		buf = list_entry(cru->buf_list.next,
266 				 struct rzg2l_cru_buffer, list);
267 		vbuf = &buf->vb;
268 		list_del_init(to_buf_list(vbuf));
269 		cru->queue_buf[slot] = vbuf;
270 
271 		/* Setup DMA */
272 		phys_addr = vb2_dma_contig_plane_dma_addr(&vbuf->vb2_buf, 0);
273 	}
274 
275 	rzg2l_cru_set_slot_addr(cru, slot, phys_addr);
276 }
277 
278 static void rzg2l_cru_initialize_axi(struct rzg2l_cru_dev *cru)
279 {
280 	unsigned int slot;
281 
282 	/*
283 	 * Set image data memory banks.
284 	 * Currently, we will use maximum address.
285 	 */
286 	rzg2l_cru_write(cru, AMnMBVALID, AMnMBVALID_MBVALID(cru->num_buf - 1));
287 
288 	for (slot = 0; slot < cru->num_buf; slot++)
289 		rzg2l_cru_fill_hw_slot(cru, slot);
290 }
291 
292 static void rzg2l_cru_csi2_setup(struct rzg2l_cru_dev *cru, bool *input_is_yuv,
293 				 struct v4l2_mbus_framefmt *ip_sd_fmt)
294 {
295 	u32 icnmc;
296 
297 	switch (ip_sd_fmt->code) {
298 	case MEDIA_BUS_FMT_UYVY8_1X16:
299 		icnmc = ICnMC_INF_YUV8_422;
300 		*input_is_yuv = true;
301 		break;
302 	default:
303 		*input_is_yuv = false;
304 		icnmc = ICnMC_INF_USER;
305 		break;
306 	}
307 
308 	icnmc |= (rzg2l_cru_read(cru, ICnMC) & ~ICnMC_INF_MASK);
309 
310 	/* Set virtual channel CSI2 */
311 	icnmc |= ICnMC_VCSEL(cru->csi.channel);
312 
313 	rzg2l_cru_write(cru, ICnMC, icnmc);
314 }
315 
316 static int rzg2l_cru_initialize_image_conv(struct rzg2l_cru_dev *cru,
317 					   struct v4l2_mbus_framefmt *ip_sd_fmt)
318 {
319 	bool output_is_yuv = false;
320 	bool input_is_yuv = false;
321 	u32 icndmr;
322 
323 	rzg2l_cru_csi2_setup(cru, &input_is_yuv, ip_sd_fmt);
324 
325 	/* Output format */
326 	switch (cru->format.pixelformat) {
327 	case V4L2_PIX_FMT_UYVY:
328 		icndmr = ICnDMR_YCMODE_UYVY;
329 		output_is_yuv = true;
330 		break;
331 	default:
332 		dev_err(cru->dev, "Invalid pixelformat (0x%x)\n",
333 			cru->format.pixelformat);
334 		return -EINVAL;
335 	}
336 
337 	/* If input and output use same colorspace, do bypass mode */
338 	if (output_is_yuv == input_is_yuv)
339 		rzg2l_cru_write(cru, ICnMC,
340 				rzg2l_cru_read(cru, ICnMC) | ICnMC_CSCTHR);
341 	else
342 		rzg2l_cru_write(cru, ICnMC,
343 				rzg2l_cru_read(cru, ICnMC) & (~ICnMC_CSCTHR));
344 
345 	/* Set output data format */
346 	rzg2l_cru_write(cru, ICnDMR, icndmr);
347 
348 	return 0;
349 }
350 
351 void rzg2l_cru_stop_image_processing(struct rzg2l_cru_dev *cru)
352 {
353 	u32 amnfifopntr, amnfifopntr_w, amnfifopntr_r_y;
354 	unsigned int retries = 0;
355 	unsigned long flags;
356 	u32 icnms;
357 
358 	spin_lock_irqsave(&cru->qlock, flags);
359 
360 	/* Disable and clear the interrupt */
361 	rzg2l_cru_write(cru, CRUnIE, 0);
362 	rzg2l_cru_write(cru, CRUnINTS, 0x001F0F0F);
363 
364 	/* Stop the operation of image conversion */
365 	rzg2l_cru_write(cru, ICnEN, 0);
366 
367 	/* Wait for streaming to stop */
368 	while ((rzg2l_cru_read(cru, ICnMS) & ICnMS_IA) && retries++ < RZG2L_RETRIES) {
369 		spin_unlock_irqrestore(&cru->qlock, flags);
370 		msleep(RZG2L_TIMEOUT_MS);
371 		spin_lock_irqsave(&cru->qlock, flags);
372 	}
373 
374 	icnms = rzg2l_cru_read(cru, ICnMS) & ICnMS_IA;
375 	if (icnms)
376 		dev_err(cru->dev, "Failed stop HW, something is seriously broken\n");
377 
378 	cru->state = RZG2L_CRU_DMA_STOPPED;
379 
380 	/* Wait until the FIFO becomes empty */
381 	for (retries = 5; retries > 0; retries--) {
382 		amnfifopntr = rzg2l_cru_read(cru, AMnFIFOPNTR);
383 
384 		amnfifopntr_w = amnfifopntr & AMnFIFOPNTR_FIFOWPNTR;
385 		amnfifopntr_r_y =
386 			(amnfifopntr & AMnFIFOPNTR_FIFORPNTR_Y) >> 16;
387 		if (amnfifopntr_w == amnfifopntr_r_y)
388 			break;
389 
390 		usleep_range(10, 20);
391 	}
392 
393 	/* Notify that FIFO is not empty here */
394 	if (!retries)
395 		dev_err(cru->dev, "Failed to empty FIFO\n");
396 
397 	/* Stop AXI bus */
398 	rzg2l_cru_write(cru, AMnAXISTP, AMnAXISTP_AXI_STOP);
399 
400 	/* Wait until the AXI bus stop */
401 	for (retries = 5; retries > 0; retries--) {
402 		if (rzg2l_cru_read(cru, AMnAXISTPACK) &
403 			AMnAXISTPACK_AXI_STOP_ACK)
404 			break;
405 
406 		usleep_range(10, 20);
407 	}
408 
409 	/* Notify that AXI bus can not stop here */
410 	if (!retries)
411 		dev_err(cru->dev, "Failed to stop AXI bus\n");
412 
413 	/* Cancel the AXI bus stop request */
414 	rzg2l_cru_write(cru, AMnAXISTP, 0);
415 
416 	/* Reset the CRU (AXI-master) */
417 	reset_control_assert(cru->aresetn);
418 
419 	/* Resets the image processing module */
420 	rzg2l_cru_write(cru, CRUnRST, 0);
421 
422 	spin_unlock_irqrestore(&cru->qlock, flags);
423 }
424 
425 int rzg2l_cru_start_image_processing(struct rzg2l_cru_dev *cru)
426 {
427 	struct v4l2_mbus_framefmt *fmt = rzg2l_cru_ip_get_src_fmt(cru);
428 	unsigned long flags;
429 	int ret;
430 
431 	spin_lock_irqsave(&cru->qlock, flags);
432 
433 	/* Initialize image convert */
434 	ret = rzg2l_cru_initialize_image_conv(cru, fmt);
435 	if (ret) {
436 		spin_unlock_irqrestore(&cru->qlock, flags);
437 		return ret;
438 	}
439 
440 	/* Select a video input */
441 	rzg2l_cru_write(cru, CRUnCTRL, CRUnCTRL_VINSEL(0));
442 
443 	/* Cancel the software reset for image processing block */
444 	rzg2l_cru_write(cru, CRUnRST, CRUnRST_VRESETN);
445 
446 	/* Disable and clear the interrupt before using */
447 	rzg2l_cru_write(cru, CRUnIE, 0);
448 	rzg2l_cru_write(cru, CRUnINTS, 0x001f000f);
449 
450 	/* Initialize the AXI master */
451 	rzg2l_cru_initialize_axi(cru);
452 
453 	/* Enable interrupt */
454 	rzg2l_cru_write(cru, CRUnIE, CRUnIE_EFE);
455 
456 	/* Enable image processing reception */
457 	rzg2l_cru_write(cru, ICnEN, ICnEN_ICEN);
458 
459 	spin_unlock_irqrestore(&cru->qlock, flags);
460 
461 	return 0;
462 }
463 
464 void rzg2l_cru_vclk_unprepare(struct rzg2l_cru_dev *cru)
465 {
466 	clk_disable_unprepare(cru->vclk);
467 }
468 
469 int rzg2l_cru_vclk_prepare(struct rzg2l_cru_dev *cru)
470 {
471 	return clk_prepare_enable(cru->vclk);
472 }
473 
474 static int rzg2l_cru_set_stream(struct rzg2l_cru_dev *cru, int on)
475 {
476 	struct media_pipeline *pipe;
477 	struct v4l2_subdev *sd;
478 	struct media_pad *pad;
479 	int ret;
480 
481 	pad = media_pad_remote_pad_first(&cru->pad);
482 	if (!pad)
483 		return -EPIPE;
484 
485 	sd = media_entity_to_v4l2_subdev(pad->entity);
486 
487 	if (!on) {
488 		int stream_off_ret = 0;
489 
490 		ret = v4l2_subdev_call(sd, video, s_stream, 0);
491 		if (ret)
492 			stream_off_ret = ret;
493 
494 		ret = v4l2_subdev_call(sd, video, post_streamoff);
495 		if (ret == -ENOIOCTLCMD)
496 			ret = 0;
497 		if (ret && !stream_off_ret)
498 			stream_off_ret = ret;
499 
500 		video_device_pipeline_stop(&cru->vdev);
501 
502 		pm_runtime_put_sync(cru->dev);
503 		clk_disable_unprepare(cru->vclk);
504 
505 		return stream_off_ret;
506 	}
507 
508 	ret = pm_runtime_resume_and_get(cru->dev);
509 	if (ret)
510 		return ret;
511 
512 	ret = clk_prepare_enable(cru->vclk);
513 	if (ret)
514 		goto err_pm_put;
515 
516 	ret = rzg2l_cru_mc_validate_format(cru, sd, pad);
517 	if (ret)
518 		goto err_vclk_disable;
519 
520 	pipe = media_entity_pipeline(&sd->entity) ? : &cru->vdev.pipe;
521 	ret = video_device_pipeline_start(&cru->vdev, pipe);
522 	if (ret)
523 		goto err_vclk_disable;
524 
525 	ret = v4l2_subdev_call(sd, video, pre_streamon, 0);
526 	if (ret == -ENOIOCTLCMD)
527 		ret = 0;
528 	if (ret)
529 		goto pipe_line_stop;
530 
531 	ret = v4l2_subdev_call(sd, video, s_stream, 1);
532 	if (ret == -ENOIOCTLCMD)
533 		ret = 0;
534 	if (ret)
535 		goto err_s_stream;
536 
537 	return 0;
538 
539 err_s_stream:
540 	v4l2_subdev_call(sd, video, post_streamoff);
541 
542 pipe_line_stop:
543 	video_device_pipeline_stop(&cru->vdev);
544 
545 err_vclk_disable:
546 	clk_disable_unprepare(cru->vclk);
547 
548 err_pm_put:
549 	pm_runtime_put_sync(cru->dev);
550 
551 	return ret;
552 }
553 
554 static void rzg2l_cru_stop_streaming(struct rzg2l_cru_dev *cru)
555 {
556 	cru->state = RZG2L_CRU_DMA_STOPPING;
557 
558 	rzg2l_cru_set_stream(cru, 0);
559 }
560 
561 static irqreturn_t rzg2l_cru_irq(int irq, void *data)
562 {
563 	struct rzg2l_cru_dev *cru = data;
564 	unsigned int handled = 0;
565 	unsigned long flags;
566 	u32 irq_status;
567 	u32 amnmbs;
568 	int slot;
569 
570 	spin_lock_irqsave(&cru->qlock, flags);
571 
572 	irq_status = rzg2l_cru_read(cru, CRUnINTS);
573 	if (!irq_status)
574 		goto done;
575 
576 	handled = 1;
577 
578 	rzg2l_cru_write(cru, CRUnINTS, rzg2l_cru_read(cru, CRUnINTS));
579 
580 	/* Nothing to do if capture status is 'RZG2L_CRU_DMA_STOPPED' */
581 	if (cru->state == RZG2L_CRU_DMA_STOPPED) {
582 		dev_dbg(cru->dev, "IRQ while state stopped\n");
583 		goto done;
584 	}
585 
586 	/* Increase stop retries if capture status is 'RZG2L_CRU_DMA_STOPPING' */
587 	if (cru->state == RZG2L_CRU_DMA_STOPPING) {
588 		if (irq_status & CRUnINTS_SFS)
589 			dev_dbg(cru->dev, "IRQ while state stopping\n");
590 		goto done;
591 	}
592 
593 	/* Prepare for capture and update state */
594 	amnmbs = rzg2l_cru_read(cru, AMnMBS);
595 	slot = amnmbs & AMnMBS_MBSTS;
596 
597 	/*
598 	 * AMnMBS.MBSTS indicates the destination of Memory Bank (MB).
599 	 * Recalculate to get the current transfer complete MB.
600 	 */
601 	if (slot == 0)
602 		slot = cru->num_buf - 1;
603 	else
604 		slot--;
605 
606 	/*
607 	 * To hand buffers back in a known order to userspace start
608 	 * to capture first from slot 0.
609 	 */
610 	if (cru->state == RZG2L_CRU_DMA_STARTING) {
611 		if (slot != 0) {
612 			dev_dbg(cru->dev, "Starting sync slot: %d\n", slot);
613 			goto done;
614 		}
615 
616 		dev_dbg(cru->dev, "Capture start synced!\n");
617 		cru->state = RZG2L_CRU_DMA_RUNNING;
618 	}
619 
620 	/* Capture frame */
621 	if (cru->queue_buf[slot]) {
622 		cru->queue_buf[slot]->field = cru->format.field;
623 		cru->queue_buf[slot]->sequence = cru->sequence;
624 		cru->queue_buf[slot]->vb2_buf.timestamp = ktime_get_ns();
625 		vb2_buffer_done(&cru->queue_buf[slot]->vb2_buf,
626 				VB2_BUF_STATE_DONE);
627 		cru->queue_buf[slot] = NULL;
628 	} else {
629 		/* Scratch buffer was used, dropping frame. */
630 		dev_dbg(cru->dev, "Dropping frame %u\n", cru->sequence);
631 	}
632 
633 	cru->sequence++;
634 
635 	/* Prepare for next frame */
636 	rzg2l_cru_fill_hw_slot(cru, slot);
637 
638 done:
639 	spin_unlock_irqrestore(&cru->qlock, flags);
640 
641 	return IRQ_RETVAL(handled);
642 }
643 
644 static int rzg2l_cru_start_streaming_vq(struct vb2_queue *vq, unsigned int count)
645 {
646 	struct rzg2l_cru_dev *cru = vb2_get_drv_priv(vq);
647 	int ret;
648 
649 	/* Release reset state */
650 	ret = reset_control_deassert(cru->aresetn);
651 	if (ret) {
652 		dev_err(cru->dev, "failed to deassert aresetn\n");
653 		return ret;
654 	}
655 
656 	ret = reset_control_deassert(cru->presetn);
657 	if (ret) {
658 		reset_control_assert(cru->aresetn);
659 		dev_err(cru->dev, "failed to deassert presetn\n");
660 		return ret;
661 	}
662 
663 	ret = request_irq(cru->image_conv_irq, rzg2l_cru_irq,
664 			  IRQF_SHARED, KBUILD_MODNAME, cru);
665 	if (ret) {
666 		dev_err(cru->dev, "failed to request irq\n");
667 		goto assert_resets;
668 	}
669 
670 	/* Allocate scratch buffer. */
671 	cru->scratch = dma_alloc_coherent(cru->dev, cru->format.sizeimage,
672 					  &cru->scratch_phys, GFP_KERNEL);
673 	if (!cru->scratch) {
674 		return_unused_buffers(cru, VB2_BUF_STATE_QUEUED);
675 		dev_err(cru->dev, "Failed to allocate scratch buffer\n");
676 		ret = -ENOMEM;
677 		goto free_image_conv_irq;
678 	}
679 
680 	cru->sequence = 0;
681 
682 	ret = rzg2l_cru_set_stream(cru, 1);
683 	if (ret) {
684 		return_unused_buffers(cru, VB2_BUF_STATE_QUEUED);
685 		goto out;
686 	}
687 
688 	cru->state = RZG2L_CRU_DMA_STARTING;
689 	dev_dbg(cru->dev, "Starting to capture\n");
690 	return 0;
691 
692 out:
693 	if (ret)
694 		dma_free_coherent(cru->dev, cru->format.sizeimage, cru->scratch,
695 				  cru->scratch_phys);
696 free_image_conv_irq:
697 	free_irq(cru->image_conv_irq, cru);
698 
699 assert_resets:
700 	reset_control_assert(cru->presetn);
701 	reset_control_assert(cru->aresetn);
702 
703 	return ret;
704 }
705 
706 static void rzg2l_cru_stop_streaming_vq(struct vb2_queue *vq)
707 {
708 	struct rzg2l_cru_dev *cru = vb2_get_drv_priv(vq);
709 
710 	rzg2l_cru_stop_streaming(cru);
711 
712 	/* Free scratch buffer */
713 	dma_free_coherent(cru->dev, cru->format.sizeimage,
714 			  cru->scratch, cru->scratch_phys);
715 
716 	free_irq(cru->image_conv_irq, cru);
717 	reset_control_assert(cru->presetn);
718 
719 	return_unused_buffers(cru, VB2_BUF_STATE_ERROR);
720 }
721 
722 static const struct vb2_ops rzg2l_cru_qops = {
723 	.queue_setup		= rzg2l_cru_queue_setup,
724 	.buf_prepare		= rzg2l_cru_buffer_prepare,
725 	.buf_queue		= rzg2l_cru_buffer_queue,
726 	.start_streaming	= rzg2l_cru_start_streaming_vq,
727 	.stop_streaming		= rzg2l_cru_stop_streaming_vq,
728 	.wait_prepare		= vb2_ops_wait_prepare,
729 	.wait_finish		= vb2_ops_wait_finish,
730 };
731 
732 void rzg2l_cru_dma_unregister(struct rzg2l_cru_dev *cru)
733 {
734 	mutex_destroy(&cru->lock);
735 
736 	v4l2_device_unregister(&cru->v4l2_dev);
737 	vb2_queue_release(&cru->queue);
738 }
739 
740 int rzg2l_cru_dma_register(struct rzg2l_cru_dev *cru)
741 {
742 	struct vb2_queue *q = &cru->queue;
743 	unsigned int i;
744 	int ret;
745 
746 	/* Initialize the top-level structure */
747 	ret = v4l2_device_register(cru->dev, &cru->v4l2_dev);
748 	if (ret)
749 		return ret;
750 
751 	mutex_init(&cru->lock);
752 	INIT_LIST_HEAD(&cru->buf_list);
753 
754 	spin_lock_init(&cru->qlock);
755 
756 	cru->state = RZG2L_CRU_DMA_STOPPED;
757 
758 	for (i = 0; i < RZG2L_CRU_HW_BUFFER_MAX; i++)
759 		cru->queue_buf[i] = NULL;
760 
761 	/* buffer queue */
762 	q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
763 	q->io_modes = VB2_MMAP | VB2_DMABUF;
764 	q->lock = &cru->lock;
765 	q->drv_priv = cru;
766 	q->buf_struct_size = sizeof(struct rzg2l_cru_buffer);
767 	q->ops = &rzg2l_cru_qops;
768 	q->mem_ops = &vb2_dma_contig_memops;
769 	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
770 	q->min_buffers_needed = 4;
771 	q->dev = cru->dev;
772 
773 	ret = vb2_queue_init(q);
774 	if (ret < 0) {
775 		dev_err(cru->dev, "failed to initialize VB2 queue\n");
776 		goto error;
777 	}
778 
779 	return 0;
780 
781 error:
782 	mutex_destroy(&cru->lock);
783 	v4l2_device_unregister(&cru->v4l2_dev);
784 	return ret;
785 }
786 
787 /* -----------------------------------------------------------------------------
788  * V4L2 stuff
789  */
790 
791 static const struct v4l2_format_info rzg2l_cru_formats[] = {
792 	{
793 		.format = V4L2_PIX_FMT_UYVY,
794 		.bpp[0] = 2,
795 	},
796 };
797 
798 const struct v4l2_format_info *rzg2l_cru_format_from_pixel(u32 format)
799 {
800 	unsigned int i;
801 
802 	for (i = 0; i < ARRAY_SIZE(rzg2l_cru_formats); i++)
803 		if (rzg2l_cru_formats[i].format == format)
804 			return rzg2l_cru_formats + i;
805 
806 	return NULL;
807 }
808 
809 static u32 rzg2l_cru_format_bytesperline(struct v4l2_pix_format *pix)
810 {
811 	const struct v4l2_format_info *fmt;
812 
813 	fmt = rzg2l_cru_format_from_pixel(pix->pixelformat);
814 
815 	if (WARN_ON(!fmt))
816 		return -EINVAL;
817 
818 	return pix->width * fmt->bpp[0];
819 }
820 
821 static u32 rzg2l_cru_format_sizeimage(struct v4l2_pix_format *pix)
822 {
823 	return pix->bytesperline * pix->height;
824 }
825 
826 static void rzg2l_cru_format_align(struct rzg2l_cru_dev *cru,
827 				   struct v4l2_pix_format *pix)
828 {
829 	if (!rzg2l_cru_format_from_pixel(pix->pixelformat))
830 		pix->pixelformat = RZG2L_CRU_DEFAULT_FORMAT;
831 
832 	switch (pix->field) {
833 	case V4L2_FIELD_TOP:
834 	case V4L2_FIELD_BOTTOM:
835 	case V4L2_FIELD_NONE:
836 	case V4L2_FIELD_INTERLACED_TB:
837 	case V4L2_FIELD_INTERLACED_BT:
838 	case V4L2_FIELD_INTERLACED:
839 		break;
840 	default:
841 		pix->field = RZG2L_CRU_DEFAULT_FIELD;
842 		break;
843 	}
844 
845 	/* Limit to CRU capabilities */
846 	v4l_bound_align_image(&pix->width, 320, RZG2L_CRU_MAX_INPUT_WIDTH, 1,
847 			      &pix->height, 240, RZG2L_CRU_MAX_INPUT_HEIGHT, 2, 0);
848 
849 	pix->bytesperline = rzg2l_cru_format_bytesperline(pix);
850 	pix->sizeimage = rzg2l_cru_format_sizeimage(pix);
851 
852 	dev_dbg(cru->dev, "Format %ux%u bpl: %u size: %u\n",
853 		pix->width, pix->height, pix->bytesperline, pix->sizeimage);
854 }
855 
856 static void rzg2l_cru_try_format(struct rzg2l_cru_dev *cru,
857 				 struct v4l2_pix_format *pix)
858 {
859 	/*
860 	 * The V4L2 specification clearly documents the colorspace fields
861 	 * as being set by drivers for capture devices. Using the values
862 	 * supplied by userspace thus wouldn't comply with the API. Until
863 	 * the API is updated force fixed values.
864 	 */
865 	pix->colorspace = RZG2L_CRU_DEFAULT_COLORSPACE;
866 	pix->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(pix->colorspace);
867 	pix->ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(pix->colorspace);
868 	pix->quantization = V4L2_MAP_QUANTIZATION_DEFAULT(true, pix->colorspace,
869 							  pix->ycbcr_enc);
870 
871 	rzg2l_cru_format_align(cru, pix);
872 }
873 
874 static int rzg2l_cru_querycap(struct file *file, void *priv,
875 			      struct v4l2_capability *cap)
876 {
877 	strscpy(cap->driver, KBUILD_MODNAME, sizeof(cap->driver));
878 	strscpy(cap->card, "RZG2L_CRU", sizeof(cap->card));
879 
880 	return 0;
881 }
882 
883 static int rzg2l_cru_try_fmt_vid_cap(struct file *file, void *priv,
884 				     struct v4l2_format *f)
885 {
886 	struct rzg2l_cru_dev *cru = video_drvdata(file);
887 
888 	rzg2l_cru_try_format(cru, &f->fmt.pix);
889 
890 	return 0;
891 }
892 
893 static int rzg2l_cru_s_fmt_vid_cap(struct file *file, void *priv,
894 				   struct v4l2_format *f)
895 {
896 	struct rzg2l_cru_dev *cru = video_drvdata(file);
897 
898 	if (vb2_is_busy(&cru->queue))
899 		return -EBUSY;
900 
901 	rzg2l_cru_try_format(cru, &f->fmt.pix);
902 
903 	cru->format = f->fmt.pix;
904 
905 	return 0;
906 }
907 
908 static int rzg2l_cru_g_fmt_vid_cap(struct file *file, void *priv,
909 				   struct v4l2_format *f)
910 {
911 	struct rzg2l_cru_dev *cru = video_drvdata(file);
912 
913 	f->fmt.pix = cru->format;
914 
915 	return 0;
916 }
917 
918 static int rzg2l_cru_enum_fmt_vid_cap(struct file *file, void *priv,
919 				      struct v4l2_fmtdesc *f)
920 {
921 	if (f->index >= ARRAY_SIZE(rzg2l_cru_formats))
922 		return -EINVAL;
923 
924 	f->pixelformat = rzg2l_cru_formats[f->index].format;
925 
926 	return 0;
927 }
928 
929 static const struct v4l2_ioctl_ops rzg2l_cru_ioctl_ops = {
930 	.vidioc_querycap		= rzg2l_cru_querycap,
931 	.vidioc_try_fmt_vid_cap		= rzg2l_cru_try_fmt_vid_cap,
932 	.vidioc_g_fmt_vid_cap		= rzg2l_cru_g_fmt_vid_cap,
933 	.vidioc_s_fmt_vid_cap		= rzg2l_cru_s_fmt_vid_cap,
934 	.vidioc_enum_fmt_vid_cap	= rzg2l_cru_enum_fmt_vid_cap,
935 
936 	.vidioc_reqbufs			= vb2_ioctl_reqbufs,
937 	.vidioc_create_bufs		= vb2_ioctl_create_bufs,
938 	.vidioc_querybuf		= vb2_ioctl_querybuf,
939 	.vidioc_qbuf			= vb2_ioctl_qbuf,
940 	.vidioc_dqbuf			= vb2_ioctl_dqbuf,
941 	.vidioc_expbuf			= vb2_ioctl_expbuf,
942 	.vidioc_prepare_buf		= vb2_ioctl_prepare_buf,
943 	.vidioc_streamon		= vb2_ioctl_streamon,
944 	.vidioc_streamoff		= vb2_ioctl_streamoff,
945 };
946 
947 /* -----------------------------------------------------------------------------
948  * Media controller file operations
949  */
950 
951 static int rzg2l_cru_open(struct file *file)
952 {
953 	struct rzg2l_cru_dev *cru = video_drvdata(file);
954 	int ret;
955 
956 	ret = mutex_lock_interruptible(&cru->lock);
957 	if (ret)
958 		return ret;
959 
960 	file->private_data = cru;
961 	ret = v4l2_fh_open(file);
962 	if (ret)
963 		goto err_unlock;
964 
965 	mutex_unlock(&cru->lock);
966 
967 	return 0;
968 
969 err_unlock:
970 	mutex_unlock(&cru->lock);
971 
972 	return ret;
973 }
974 
975 static int rzg2l_cru_release(struct file *file)
976 {
977 	struct rzg2l_cru_dev *cru = video_drvdata(file);
978 	int ret;
979 
980 	mutex_lock(&cru->lock);
981 
982 	/* the release helper will cleanup any on-going streaming. */
983 	ret = _vb2_fop_release(file, NULL);
984 
985 	mutex_unlock(&cru->lock);
986 
987 	return ret;
988 }
989 
990 static const struct v4l2_file_operations rzg2l_cru_fops = {
991 	.owner		= THIS_MODULE,
992 	.unlocked_ioctl	= video_ioctl2,
993 	.open		= rzg2l_cru_open,
994 	.release	= rzg2l_cru_release,
995 	.poll		= vb2_fop_poll,
996 	.mmap		= vb2_fop_mmap,
997 	.read		= vb2_fop_read,
998 };
999 
1000 static void rzg2l_cru_v4l2_init(struct rzg2l_cru_dev *cru)
1001 {
1002 	struct video_device *vdev = &cru->vdev;
1003 
1004 	vdev->v4l2_dev = &cru->v4l2_dev;
1005 	vdev->queue = &cru->queue;
1006 	snprintf(vdev->name, sizeof(vdev->name), "CRU output");
1007 	vdev->release = video_device_release_empty;
1008 	vdev->lock = &cru->lock;
1009 	vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
1010 	vdev->device_caps |= V4L2_CAP_IO_MC;
1011 	vdev->fops = &rzg2l_cru_fops;
1012 	vdev->ioctl_ops = &rzg2l_cru_ioctl_ops;
1013 
1014 	/* Set a default format */
1015 	cru->format.pixelformat	= RZG2L_CRU_DEFAULT_FORMAT;
1016 	cru->format.width = RZG2L_CRU_DEFAULT_WIDTH;
1017 	cru->format.height = RZG2L_CRU_DEFAULT_HEIGHT;
1018 	cru->format.field = RZG2L_CRU_DEFAULT_FIELD;
1019 	cru->format.colorspace = RZG2L_CRU_DEFAULT_COLORSPACE;
1020 	rzg2l_cru_format_align(cru, &cru->format);
1021 }
1022 
1023 void rzg2l_cru_video_unregister(struct rzg2l_cru_dev *cru)
1024 {
1025 	media_device_unregister(&cru->mdev);
1026 	video_unregister_device(&cru->vdev);
1027 }
1028 
1029 int rzg2l_cru_video_register(struct rzg2l_cru_dev *cru)
1030 {
1031 	struct video_device *vdev = &cru->vdev;
1032 	int ret;
1033 
1034 	if (video_is_registered(&cru->vdev)) {
1035 		struct media_entity *entity;
1036 
1037 		entity = &cru->vdev.entity;
1038 		if (!entity->graph_obj.mdev)
1039 			entity->graph_obj.mdev = &cru->mdev;
1040 		return 0;
1041 	}
1042 
1043 	rzg2l_cru_v4l2_init(cru);
1044 	video_set_drvdata(vdev, cru);
1045 	ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1);
1046 	if (ret) {
1047 		dev_err(cru->dev, "Failed to register video device\n");
1048 		return ret;
1049 	}
1050 
1051 	ret = media_device_register(&cru->mdev);
1052 	if (ret) {
1053 		video_unregister_device(&cru->vdev);
1054 		return ret;
1055 	}
1056 
1057 	return 0;
1058 }
1059