1 // SPDX-License-Identifier: GPL-2.0
2 /*
3  * V4L2 Capture CSI Subdev for Freescale i.MX6UL/L / i.MX7 SOC
4  *
5  * Copyright (c) 2019 Linaro Ltd
6  *
7  */
8 
9 #include <linux/clk.h>
10 #include <linux/delay.h>
11 #include <linux/interrupt.h>
12 #include <linux/math.h>
13 #include <linux/mfd/syscon.h>
14 #include <linux/minmax.h>
15 #include <linux/module.h>
16 #include <linux/of.h>
17 #include <linux/of_graph.h>
18 #include <linux/pinctrl/consumer.h>
19 #include <linux/platform_device.h>
20 #include <linux/regmap.h>
21 #include <linux/slab.h>
22 #include <linux/spinlock.h>
23 #include <linux/types.h>
24 
25 #include <media/v4l2-device.h>
26 #include <media/v4l2-fwnode.h>
27 #include <media/v4l2-ioctl.h>
28 #include <media/v4l2-mc.h>
29 #include <media/v4l2-subdev.h>
30 #include <media/videobuf2-dma-contig.h>
31 
32 #define IMX7_CSI_PAD_SINK		0
33 #define IMX7_CSI_PAD_SRC		1
34 #define IMX7_CSI_PADS_NUM		2
35 
36 /* csi control reg 1 */
37 #define BIT_SWAP16_EN			BIT(31)
38 #define BIT_EXT_VSYNC			BIT(30)
39 #define BIT_EOF_INT_EN			BIT(29)
40 #define BIT_PRP_IF_EN			BIT(28)
41 #define BIT_CCIR_MODE			BIT(27)
42 #define BIT_COF_INT_EN			BIT(26)
43 #define BIT_SF_OR_INTEN			BIT(25)
44 #define BIT_RF_OR_INTEN			BIT(24)
45 #define BIT_SFF_DMA_DONE_INTEN		BIT(22)
46 #define BIT_STATFF_INTEN		BIT(21)
47 #define BIT_FB2_DMA_DONE_INTEN		BIT(20)
48 #define BIT_FB1_DMA_DONE_INTEN		BIT(19)
49 #define BIT_RXFF_INTEN			BIT(18)
50 #define BIT_SOF_POL			BIT(17)
51 #define BIT_SOF_INTEN			BIT(16)
52 #define BIT_MCLKDIV(n)			((n) << 12)
53 #define BIT_MCLKDIV_MASK		(0xf << 12)
54 #define BIT_HSYNC_POL			BIT(11)
55 #define BIT_CCIR_EN			BIT(10)
56 #define BIT_MCLKEN			BIT(9)
57 #define BIT_FCC				BIT(8)
58 #define BIT_PACK_DIR			BIT(7)
59 #define BIT_CLR_STATFIFO		BIT(6)
60 #define BIT_CLR_RXFIFO			BIT(5)
61 #define BIT_GCLK_MODE			BIT(4)
62 #define BIT_INV_DATA			BIT(3)
63 #define BIT_INV_PCLK			BIT(2)
64 #define BIT_REDGE			BIT(1)
65 #define BIT_PIXEL_BIT			BIT(0)
66 
67 /* control reg 2 */
68 #define BIT_DMA_BURST_TYPE_RFF_INCR4	(1 << 30)
69 #define BIT_DMA_BURST_TYPE_RFF_INCR8	(2 << 30)
70 #define BIT_DMA_BURST_TYPE_RFF_INCR16	(3 << 30)
71 #define BIT_DMA_BURST_TYPE_RFF_MASK	(3 << 30)
72 
73 /* control reg 3 */
74 #define BIT_FRMCNT(n)			((n) << 16)
75 #define BIT_FRMCNT_MASK			(0xffff << 16)
76 #define BIT_FRMCNT_RST			BIT(15)
77 #define BIT_DMA_REFLASH_RFF		BIT(14)
78 #define BIT_DMA_REFLASH_SFF		BIT(13)
79 #define BIT_DMA_REQ_EN_RFF		BIT(12)
80 #define BIT_DMA_REQ_EN_SFF		BIT(11)
81 #define BIT_STATFF_LEVEL(n)		((n) << 8)
82 #define BIT_STATFF_LEVEL_MASK		(0x7 << 8)
83 #define BIT_HRESP_ERR_EN		BIT(7)
84 #define BIT_RXFF_LEVEL(n)		((n) << 4)
85 #define BIT_RXFF_LEVEL_MASK		(0x7 << 4)
86 #define BIT_TWO_8BIT_SENSOR		BIT(3)
87 #define BIT_ZERO_PACK_EN		BIT(2)
88 #define BIT_ECC_INT_EN			BIT(1)
89 #define BIT_ECC_AUTO_EN			BIT(0)
90 
91 /* csi status reg */
92 #define BIT_ADDR_CH_ERR_INT		BIT(28)
93 #define BIT_FIELD0_INT			BIT(27)
94 #define BIT_FIELD1_INT			BIT(26)
95 #define BIT_SFF_OR_INT			BIT(25)
96 #define BIT_RFF_OR_INT			BIT(24)
97 #define BIT_DMA_TSF_DONE_SFF		BIT(22)
98 #define BIT_STATFF_INT			BIT(21)
99 #define BIT_DMA_TSF_DONE_FB2		BIT(20)
100 #define BIT_DMA_TSF_DONE_FB1		BIT(19)
101 #define BIT_RXFF_INT			BIT(18)
102 #define BIT_EOF_INT			BIT(17)
103 #define BIT_SOF_INT			BIT(16)
104 #define BIT_F2_INT			BIT(15)
105 #define BIT_F1_INT			BIT(14)
106 #define BIT_COF_INT			BIT(13)
107 #define BIT_HRESP_ERR_INT		BIT(7)
108 #define BIT_ECC_INT			BIT(1)
109 #define BIT_DRDY			BIT(0)
110 
111 /* csi image parameter reg */
112 #define BIT_IMAGE_WIDTH(n)		((n) << 16)
113 #define BIT_IMAGE_HEIGHT(n)		(n)
114 
115 /* csi control reg 18 */
116 #define BIT_CSI_HW_ENABLE		BIT(31)
117 #define BIT_MIPI_DATA_FORMAT_RAW8	(0x2a << 25)
118 #define BIT_MIPI_DATA_FORMAT_RAW10	(0x2b << 25)
119 #define BIT_MIPI_DATA_FORMAT_RAW12	(0x2c << 25)
120 #define BIT_MIPI_DATA_FORMAT_RAW14	(0x2d << 25)
121 #define BIT_MIPI_DATA_FORMAT_YUV422_8B	(0x1e << 25)
122 #define BIT_MIPI_DATA_FORMAT_MASK	(0x3f << 25)
123 #define BIT_DATA_FROM_MIPI		BIT(22)
124 #define BIT_MIPI_YU_SWAP		BIT(21)
125 #define BIT_MIPI_DOUBLE_CMPNT		BIT(20)
126 #define BIT_MASK_OPTION_FIRST_FRAME	(0 << 18)
127 #define BIT_MASK_OPTION_CSI_EN		(1 << 18)
128 #define BIT_MASK_OPTION_SECOND_FRAME	(2 << 18)
129 #define BIT_MASK_OPTION_ON_DATA		(3 << 18)
130 #define BIT_BASEADDR_CHG_ERR_EN		BIT(9)
131 #define BIT_BASEADDR_SWITCH_SEL		BIT(5)
132 #define BIT_BASEADDR_SWITCH_EN		BIT(4)
133 #define BIT_PARALLEL24_EN		BIT(3)
134 #define BIT_DEINTERLACE_EN		BIT(2)
135 #define BIT_TVDECODER_IN_EN		BIT(1)
136 #define BIT_NTSC_EN			BIT(0)
137 
138 #define CSI_MCLK_VF			1
139 #define CSI_MCLK_ENC			2
140 #define CSI_MCLK_RAW			4
141 #define CSI_MCLK_I2C			8
142 
143 #define CSI_CSICR1			0x00
144 #define CSI_CSICR2			0x04
145 #define CSI_CSICR3			0x08
146 #define CSI_STATFIFO			0x0c
147 #define CSI_CSIRXFIFO			0x10
148 #define CSI_CSIRXCNT			0x14
149 #define CSI_CSISR			0x18
150 
151 #define CSI_CSIDBG			0x1c
152 #define CSI_CSIDMASA_STATFIFO		0x20
153 #define CSI_CSIDMATS_STATFIFO		0x24
154 #define CSI_CSIDMASA_FB1		0x28
155 #define CSI_CSIDMASA_FB2		0x2c
156 #define CSI_CSIFBUF_PARA		0x30
157 #define CSI_CSIIMAG_PARA		0x34
158 
159 #define CSI_CSICR18			0x48
160 #define CSI_CSICR19			0x4c
161 
162 #define IMX7_CSI_VIDEO_NAME		"imx-capture"
163 /* In bytes, per queue */
164 #define IMX7_CSI_VIDEO_MEM_LIMIT	SZ_512M
165 #define IMX7_CSI_VIDEO_EOF_TIMEOUT	2000
166 
167 #define IMX7_CSI_DEF_MBUS_CODE		MEDIA_BUS_FMT_UYVY8_2X8
168 #define IMX7_CSI_DEF_PIX_FORMAT		V4L2_PIX_FMT_UYVY
169 #define IMX7_CSI_DEF_PIX_WIDTH		640
170 #define IMX7_CSI_DEF_PIX_HEIGHT		480
171 
172 enum imx_csi_model {
173 	IMX7_CSI_IMX7 = 0,
174 	IMX7_CSI_IMX8MQ,
175 };
176 
177 struct imx7_csi_pixfmt {
178 	/* the in-memory FourCC pixel format */
179 	u32     fourcc;
180 	/*
181 	 * the set of equivalent media bus codes for the fourcc.
182 	 * NOTE! codes pointer is NULL for in-memory-only formats.
183 	 */
184 	const u32 *codes;
185 	int     bpp;     /* total bpp */
186 	bool	yuv;
187 };
188 
189 struct imx7_csi_vb2_buffer {
190 	struct vb2_v4l2_buffer vbuf;
191 	struct list_head list;
192 };
193 
194 static inline struct imx7_csi_vb2_buffer *
195 to_imx7_csi_vb2_buffer(struct vb2_buffer *vb)
196 {
197 	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
198 
199 	return container_of(vbuf, struct imx7_csi_vb2_buffer, vbuf);
200 }
201 
202 struct imx7_csi_dma_buf {
203 	void *virt;
204 	dma_addr_t dma_addr;
205 	unsigned long len;
206 };
207 
208 struct imx7_csi {
209 	struct device *dev;
210 
211 	/* Resources and locks */
212 	void __iomem *regbase;
213 	int irq;
214 	struct clk *mclk;
215 
216 	spinlock_t irqlock; /* Protects last_eof */
217 
218 	/* Media and V4L2 device */
219 	struct media_device mdev;
220 	struct v4l2_device v4l2_dev;
221 	struct v4l2_async_notifier notifier;
222 	struct media_pipeline pipe;
223 
224 	struct v4l2_subdev *src_sd;
225 	bool is_csi2;
226 
227 	/* V4L2 subdev */
228 	struct v4l2_subdev sd;
229 	struct media_pad pad[IMX7_CSI_PADS_NUM];
230 
231 	/* Video device */
232 	struct video_device *vdev;		/* Video device */
233 	struct media_pad vdev_pad;		/* Video device pad */
234 
235 	struct v4l2_pix_format vdev_fmt;	/* The user format */
236 	const struct imx7_csi_pixfmt *vdev_cc;
237 	struct v4l2_rect vdev_compose;		/* The compose rectangle */
238 
239 	struct mutex vdev_mutex;		/* Protect vdev operations */
240 
241 	struct vb2_queue q;			/* The videobuf2 queue */
242 	struct list_head ready_q;		/* List of queued buffers */
243 	spinlock_t q_lock;			/* Protect ready_q */
244 
245 	/* Buffers and streaming state */
246 	struct imx7_csi_vb2_buffer *active_vb2_buf[2];
247 	struct imx7_csi_dma_buf underrun_buf;
248 
249 	bool is_streaming;
250 	int buf_num;
251 	u32 frame_sequence;
252 
253 	bool last_eof;
254 	struct completion last_eof_completion;
255 
256 	enum imx_csi_model model;
257 };
258 
259 static struct imx7_csi *
260 imx7_csi_notifier_to_dev(struct v4l2_async_notifier *n)
261 {
262 	return container_of(n, struct imx7_csi, notifier);
263 }
264 
265 /* -----------------------------------------------------------------------------
266  * Hardware Configuration
267  */
268 
269 static u32 imx7_csi_reg_read(struct imx7_csi *csi, unsigned int offset)
270 {
271 	return readl(csi->regbase + offset);
272 }
273 
274 static void imx7_csi_reg_write(struct imx7_csi *csi, unsigned int value,
275 			       unsigned int offset)
276 {
277 	writel(value, csi->regbase + offset);
278 }
279 
280 static u32 imx7_csi_irq_clear(struct imx7_csi *csi)
281 {
282 	u32 isr;
283 
284 	isr = imx7_csi_reg_read(csi, CSI_CSISR);
285 	imx7_csi_reg_write(csi, isr, CSI_CSISR);
286 
287 	return isr;
288 }
289 
290 static void imx7_csi_init_default(struct imx7_csi *csi)
291 {
292 	imx7_csi_reg_write(csi, BIT_SOF_POL | BIT_REDGE | BIT_GCLK_MODE |
293 			   BIT_HSYNC_POL | BIT_FCC | BIT_MCLKDIV(1) |
294 			   BIT_MCLKEN, CSI_CSICR1);
295 	imx7_csi_reg_write(csi, 0, CSI_CSICR2);
296 	imx7_csi_reg_write(csi, BIT_FRMCNT_RST, CSI_CSICR3);
297 
298 	imx7_csi_reg_write(csi, BIT_IMAGE_WIDTH(IMX7_CSI_DEF_PIX_WIDTH) |
299 			   BIT_IMAGE_HEIGHT(IMX7_CSI_DEF_PIX_HEIGHT),
300 			   CSI_CSIIMAG_PARA);
301 
302 	imx7_csi_reg_write(csi, BIT_DMA_REFLASH_RFF, CSI_CSICR3);
303 }
304 
305 static void imx7_csi_hw_enable_irq(struct imx7_csi *csi)
306 {
307 	u32 cr1 = imx7_csi_reg_read(csi, CSI_CSICR1);
308 
309 	cr1 |= BIT_RFF_OR_INT;
310 	cr1 |= BIT_FB1_DMA_DONE_INTEN;
311 	cr1 |= BIT_FB2_DMA_DONE_INTEN;
312 
313 	imx7_csi_reg_write(csi, cr1, CSI_CSICR1);
314 }
315 
316 static void imx7_csi_hw_disable_irq(struct imx7_csi *csi)
317 {
318 	u32 cr1 = imx7_csi_reg_read(csi, CSI_CSICR1);
319 
320 	cr1 &= ~BIT_RFF_OR_INT;
321 	cr1 &= ~BIT_FB1_DMA_DONE_INTEN;
322 	cr1 &= ~BIT_FB2_DMA_DONE_INTEN;
323 
324 	imx7_csi_reg_write(csi, cr1, CSI_CSICR1);
325 }
326 
327 static void imx7_csi_hw_enable(struct imx7_csi *csi)
328 {
329 	u32 cr = imx7_csi_reg_read(csi, CSI_CSICR18);
330 
331 	cr |= BIT_CSI_HW_ENABLE;
332 
333 	imx7_csi_reg_write(csi, cr, CSI_CSICR18);
334 }
335 
336 static void imx7_csi_hw_disable(struct imx7_csi *csi)
337 {
338 	u32 cr = imx7_csi_reg_read(csi, CSI_CSICR18);
339 
340 	cr &= ~BIT_CSI_HW_ENABLE;
341 
342 	imx7_csi_reg_write(csi, cr, CSI_CSICR18);
343 }
344 
345 static void imx7_csi_dma_reflash(struct imx7_csi *csi)
346 {
347 	u32 cr3;
348 
349 	cr3 = imx7_csi_reg_read(csi, CSI_CSICR3);
350 	cr3 |= BIT_DMA_REFLASH_RFF;
351 	imx7_csi_reg_write(csi, cr3, CSI_CSICR3);
352 }
353 
354 static void imx7_csi_rx_fifo_clear(struct imx7_csi *csi)
355 {
356 	u32 cr1 = imx7_csi_reg_read(csi, CSI_CSICR1) & ~BIT_FCC;
357 
358 	imx7_csi_reg_write(csi, cr1, CSI_CSICR1);
359 	imx7_csi_reg_write(csi, cr1 | BIT_CLR_RXFIFO, CSI_CSICR1);
360 	imx7_csi_reg_write(csi, cr1 | BIT_FCC, CSI_CSICR1);
361 }
362 
363 static void imx7_csi_dmareq_rff_enable(struct imx7_csi *csi)
364 {
365 	u32 cr3 = imx7_csi_reg_read(csi, CSI_CSICR3);
366 
367 	cr3 |= BIT_DMA_REQ_EN_RFF;
368 	cr3 |= BIT_HRESP_ERR_EN;
369 	cr3 &= ~BIT_RXFF_LEVEL_MASK;
370 	cr3 |= BIT_RXFF_LEVEL(2);
371 
372 	imx7_csi_reg_write(csi, cr3, CSI_CSICR3);
373 }
374 
375 static void imx7_csi_dmareq_rff_disable(struct imx7_csi *csi)
376 {
377 	u32 cr3 = imx7_csi_reg_read(csi, CSI_CSICR3);
378 
379 	cr3 &= ~BIT_DMA_REQ_EN_RFF;
380 	cr3 &= ~BIT_HRESP_ERR_EN;
381 	imx7_csi_reg_write(csi, cr3, CSI_CSICR3);
382 }
383 
384 static void imx7_csi_update_buf(struct imx7_csi *csi, dma_addr_t dma_addr,
385 				int buf_num)
386 {
387 	if (buf_num == 1)
388 		imx7_csi_reg_write(csi, dma_addr, CSI_CSIDMASA_FB2);
389 	else
390 		imx7_csi_reg_write(csi, dma_addr, CSI_CSIDMASA_FB1);
391 }
392 
393 static struct imx7_csi_vb2_buffer *imx7_csi_video_next_buf(struct imx7_csi *csi);
394 
395 static void imx7_csi_setup_vb2_buf(struct imx7_csi *csi)
396 {
397 	struct imx7_csi_vb2_buffer *buf;
398 	struct vb2_buffer *vb2_buf;
399 	int i;
400 
401 	for (i = 0; i < 2; i++) {
402 		dma_addr_t dma_addr;
403 
404 		buf = imx7_csi_video_next_buf(csi);
405 		if (buf) {
406 			csi->active_vb2_buf[i] = buf;
407 			vb2_buf = &buf->vbuf.vb2_buf;
408 			dma_addr = vb2_dma_contig_plane_dma_addr(vb2_buf, 0);
409 		} else {
410 			csi->active_vb2_buf[i] = NULL;
411 			dma_addr = csi->underrun_buf.dma_addr;
412 		}
413 
414 		imx7_csi_update_buf(csi, dma_addr, i);
415 	}
416 }
417 
418 static void imx7_csi_dma_unsetup_vb2_buf(struct imx7_csi *csi,
419 					 enum vb2_buffer_state return_status)
420 {
421 	struct imx7_csi_vb2_buffer *buf;
422 	int i;
423 
424 	/* return any remaining active frames with return_status */
425 	for (i = 0; i < 2; i++) {
426 		buf = csi->active_vb2_buf[i];
427 		if (buf) {
428 			struct vb2_buffer *vb = &buf->vbuf.vb2_buf;
429 
430 			vb->timestamp = ktime_get_ns();
431 			vb2_buffer_done(vb, return_status);
432 			csi->active_vb2_buf[i] = NULL;
433 		}
434 	}
435 }
436 
437 static void imx7_csi_free_dma_buf(struct imx7_csi *csi,
438 				  struct imx7_csi_dma_buf *buf)
439 {
440 	if (buf->virt)
441 		dma_free_coherent(csi->dev, buf->len, buf->virt, buf->dma_addr);
442 
443 	buf->virt = NULL;
444 	buf->dma_addr = 0;
445 }
446 
447 static int imx7_csi_alloc_dma_buf(struct imx7_csi *csi,
448 				  struct imx7_csi_dma_buf *buf, int size)
449 {
450 	imx7_csi_free_dma_buf(csi, buf);
451 
452 	buf->len = PAGE_ALIGN(size);
453 	buf->virt = dma_alloc_coherent(csi->dev, buf->len, &buf->dma_addr,
454 				       GFP_DMA | GFP_KERNEL);
455 	if (!buf->virt)
456 		return -ENOMEM;
457 
458 	return 0;
459 }
460 
461 static int imx7_csi_dma_setup(struct imx7_csi *csi)
462 {
463 	int ret;
464 
465 	ret = imx7_csi_alloc_dma_buf(csi, &csi->underrun_buf,
466 				     csi->vdev_fmt.sizeimage);
467 	if (ret < 0) {
468 		v4l2_warn(&csi->sd, "consider increasing the CMA area\n");
469 		return ret;
470 	}
471 
472 	csi->frame_sequence = 0;
473 	csi->last_eof = false;
474 	init_completion(&csi->last_eof_completion);
475 
476 	imx7_csi_setup_vb2_buf(csi);
477 
478 	return 0;
479 }
480 
481 static void imx7_csi_dma_cleanup(struct imx7_csi *csi,
482 				 enum vb2_buffer_state return_status)
483 {
484 	imx7_csi_dma_unsetup_vb2_buf(csi, return_status);
485 	imx7_csi_free_dma_buf(csi, &csi->underrun_buf);
486 }
487 
488 static void imx7_csi_dma_stop(struct imx7_csi *csi)
489 {
490 	unsigned long timeout_jiffies;
491 	unsigned long flags;
492 	int ret;
493 
494 	/* mark next EOF interrupt as the last before stream off */
495 	spin_lock_irqsave(&csi->irqlock, flags);
496 	csi->last_eof = true;
497 	spin_unlock_irqrestore(&csi->irqlock, flags);
498 
499 	/*
500 	 * and then wait for interrupt handler to mark completion.
501 	 */
502 	timeout_jiffies = msecs_to_jiffies(IMX7_CSI_VIDEO_EOF_TIMEOUT);
503 	ret = wait_for_completion_timeout(&csi->last_eof_completion,
504 					  timeout_jiffies);
505 	if (ret == 0)
506 		v4l2_warn(&csi->sd, "wait last EOF timeout\n");
507 
508 	imx7_csi_hw_disable_irq(csi);
509 }
510 
511 static void imx7_csi_configure(struct imx7_csi *csi,
512 			       struct v4l2_subdev_state *sd_state)
513 {
514 	struct v4l2_pix_format *out_pix = &csi->vdev_fmt;
515 	int width = out_pix->width;
516 	u32 stride = 0;
517 	u32 cr3 = BIT_FRMCNT_RST;
518 	u32 cr1, cr18;
519 
520 	cr18 = imx7_csi_reg_read(csi, CSI_CSICR18);
521 
522 	cr18 &= ~(BIT_CSI_HW_ENABLE | BIT_MIPI_DATA_FORMAT_MASK |
523 		  BIT_DATA_FROM_MIPI | BIT_MIPI_DOUBLE_CMPNT |
524 		  BIT_BASEADDR_CHG_ERR_EN | BIT_BASEADDR_SWITCH_SEL |
525 		  BIT_BASEADDR_SWITCH_EN | BIT_DEINTERLACE_EN);
526 
527 	if (out_pix->field == V4L2_FIELD_INTERLACED) {
528 		cr18 |= BIT_DEINTERLACE_EN;
529 		stride = out_pix->width;
530 	}
531 
532 	if (!csi->is_csi2) {
533 		cr1 = BIT_SOF_POL | BIT_REDGE | BIT_GCLK_MODE | BIT_HSYNC_POL
534 		    | BIT_FCC | BIT_MCLKDIV(1) | BIT_MCLKEN;
535 
536 		cr18 |= BIT_BASEADDR_SWITCH_EN | BIT_BASEADDR_SWITCH_SEL |
537 			BIT_BASEADDR_CHG_ERR_EN;
538 
539 		if (out_pix->pixelformat == V4L2_PIX_FMT_UYVY ||
540 		    out_pix->pixelformat == V4L2_PIX_FMT_YUYV)
541 			width *= 2;
542 	} else {
543 		const struct v4l2_mbus_framefmt *sink_fmt;
544 
545 		sink_fmt = v4l2_subdev_get_pad_format(&csi->sd, sd_state,
546 						      IMX7_CSI_PAD_SINK);
547 
548 		cr1 = BIT_SOF_POL | BIT_REDGE | BIT_HSYNC_POL | BIT_FCC
549 		    | BIT_MCLKDIV(1) | BIT_MCLKEN;
550 
551 		cr18 |= BIT_DATA_FROM_MIPI;
552 
553 		switch (sink_fmt->code) {
554 		case MEDIA_BUS_FMT_Y8_1X8:
555 		case MEDIA_BUS_FMT_SBGGR8_1X8:
556 		case MEDIA_BUS_FMT_SGBRG8_1X8:
557 		case MEDIA_BUS_FMT_SGRBG8_1X8:
558 		case MEDIA_BUS_FMT_SRGGB8_1X8:
559 			cr18 |= BIT_MIPI_DATA_FORMAT_RAW8;
560 			break;
561 		case MEDIA_BUS_FMT_Y10_1X10:
562 		case MEDIA_BUS_FMT_SBGGR10_1X10:
563 		case MEDIA_BUS_FMT_SGBRG10_1X10:
564 		case MEDIA_BUS_FMT_SGRBG10_1X10:
565 		case MEDIA_BUS_FMT_SRGGB10_1X10:
566 			cr3 |= BIT_TWO_8BIT_SENSOR;
567 			cr18 |= BIT_MIPI_DATA_FORMAT_RAW10;
568 			break;
569 		case MEDIA_BUS_FMT_Y12_1X12:
570 		case MEDIA_BUS_FMT_SBGGR12_1X12:
571 		case MEDIA_BUS_FMT_SGBRG12_1X12:
572 		case MEDIA_BUS_FMT_SGRBG12_1X12:
573 		case MEDIA_BUS_FMT_SRGGB12_1X12:
574 			cr3 |= BIT_TWO_8BIT_SENSOR;
575 			cr18 |= BIT_MIPI_DATA_FORMAT_RAW12;
576 			break;
577 		case MEDIA_BUS_FMT_Y14_1X14:
578 		case MEDIA_BUS_FMT_SBGGR14_1X14:
579 		case MEDIA_BUS_FMT_SGBRG14_1X14:
580 		case MEDIA_BUS_FMT_SGRBG14_1X14:
581 		case MEDIA_BUS_FMT_SRGGB14_1X14:
582 			cr3 |= BIT_TWO_8BIT_SENSOR;
583 			cr18 |= BIT_MIPI_DATA_FORMAT_RAW14;
584 			break;
585 
586 		/*
587 		 * The CSI bridge has a 16-bit input bus. Depending on the
588 		 * connected source, data may be transmitted with 8 or 10 bits
589 		 * per clock sample (in bits [9:2] or [9:0] respectively) or
590 		 * with 16 bits per clock sample (in bits [15:0]). The data is
591 		 * then packed into a 32-bit FIFO (as shown in figure 13-11 of
592 		 * the i.MX8MM reference manual rev. 3).
593 		 *
594 		 * The data packing in a 32-bit FIFO input word is controlled by
595 		 * the CR3 TWO_8BIT_SENSOR field (also known as SENSOR_16BITS in
596 		 * the i.MX8MM reference manual). When set to 0, data packing
597 		 * groups four 8-bit input samples (bits [9:2]). When set to 1,
598 		 * data packing groups two 16-bit input samples (bits [15:0]).
599 		 *
600 		 * The register field CR18 MIPI_DOUBLE_CMPNT also needs to be
601 		 * configured according to the input format for YUV 4:2:2 data.
602 		 * The field controls the gasket between the CSI-2 receiver and
603 		 * the CSI bridge. On i.MX7 and i.MX8MM, the field must be set
604 		 * to 1 when the CSIS outputs 16-bit samples. On i.MX8MQ, the
605 		 * gasket ignores the MIPI_DOUBLE_CMPNT bit and YUV 4:2:2 always
606 		 * uses 16-bit samples. Setting MIPI_DOUBLE_CMPNT in that case
607 		 * has no effect, but doesn't cause any issue.
608 		 */
609 		case MEDIA_BUS_FMT_UYVY8_2X8:
610 		case MEDIA_BUS_FMT_YUYV8_2X8:
611 			cr18 |= BIT_MIPI_DATA_FORMAT_YUV422_8B;
612 			break;
613 		case MEDIA_BUS_FMT_UYVY8_1X16:
614 		case MEDIA_BUS_FMT_YUYV8_1X16:
615 			cr3 |= BIT_TWO_8BIT_SENSOR;
616 			cr18 |= BIT_MIPI_DATA_FORMAT_YUV422_8B |
617 				BIT_MIPI_DOUBLE_CMPNT;
618 			break;
619 		}
620 	}
621 
622 	imx7_csi_reg_write(csi, cr1, CSI_CSICR1);
623 	imx7_csi_reg_write(csi, BIT_DMA_BURST_TYPE_RFF_INCR16, CSI_CSICR2);
624 	imx7_csi_reg_write(csi, cr3, CSI_CSICR3);
625 	imx7_csi_reg_write(csi, cr18, CSI_CSICR18);
626 
627 	imx7_csi_reg_write(csi, (width * out_pix->height) >> 2, CSI_CSIRXCNT);
628 	imx7_csi_reg_write(csi, BIT_IMAGE_WIDTH(width) |
629 			   BIT_IMAGE_HEIGHT(out_pix->height),
630 			   CSI_CSIIMAG_PARA);
631 	imx7_csi_reg_write(csi, stride, CSI_CSIFBUF_PARA);
632 }
633 
634 static int imx7_csi_init(struct imx7_csi *csi,
635 			 struct v4l2_subdev_state *sd_state)
636 {
637 	int ret;
638 
639 	ret = clk_prepare_enable(csi->mclk);
640 	if (ret < 0)
641 		return ret;
642 
643 	imx7_csi_configure(csi, sd_state);
644 
645 	ret = imx7_csi_dma_setup(csi);
646 	if (ret < 0) {
647 		clk_disable_unprepare(csi->mclk);
648 		return ret;
649 	}
650 
651 	return 0;
652 }
653 
654 static void imx7_csi_deinit(struct imx7_csi *csi,
655 			    enum vb2_buffer_state return_status)
656 {
657 	imx7_csi_dma_cleanup(csi, return_status);
658 	imx7_csi_init_default(csi);
659 	imx7_csi_dmareq_rff_disable(csi);
660 	clk_disable_unprepare(csi->mclk);
661 }
662 
663 static void imx7_csi_baseaddr_switch_on_second_frame(struct imx7_csi *csi)
664 {
665 	u32 cr18 = imx7_csi_reg_read(csi, CSI_CSICR18);
666 
667 	cr18 |= BIT_BASEADDR_SWITCH_EN | BIT_BASEADDR_SWITCH_SEL |
668 		BIT_BASEADDR_CHG_ERR_EN;
669 	cr18 |= BIT_MASK_OPTION_SECOND_FRAME;
670 	imx7_csi_reg_write(csi, cr18, CSI_CSICR18);
671 }
672 
673 static void imx7_csi_enable(struct imx7_csi *csi)
674 {
675 	/* Clear the Rx FIFO and reflash the DMA controller. */
676 	imx7_csi_rx_fifo_clear(csi);
677 	imx7_csi_dma_reflash(csi);
678 
679 	usleep_range(2000, 3000);
680 
681 	/* Clear and enable the interrupts. */
682 	imx7_csi_irq_clear(csi);
683 	imx7_csi_hw_enable_irq(csi);
684 
685 	/* Enable the RxFIFO DMA and the CSI. */
686 	imx7_csi_dmareq_rff_enable(csi);
687 	imx7_csi_hw_enable(csi);
688 
689 	if (csi->model == IMX7_CSI_IMX8MQ)
690 		imx7_csi_baseaddr_switch_on_second_frame(csi);
691 }
692 
693 static void imx7_csi_disable(struct imx7_csi *csi)
694 {
695 	imx7_csi_dma_stop(csi);
696 
697 	imx7_csi_dmareq_rff_disable(csi);
698 
699 	imx7_csi_hw_disable_irq(csi);
700 
701 	imx7_csi_hw_disable(csi);
702 }
703 
704 /* -----------------------------------------------------------------------------
705  * Interrupt Handling
706  */
707 
708 static void imx7_csi_error_recovery(struct imx7_csi *csi)
709 {
710 	imx7_csi_hw_disable(csi);
711 
712 	imx7_csi_rx_fifo_clear(csi);
713 
714 	imx7_csi_dma_reflash(csi);
715 
716 	imx7_csi_hw_enable(csi);
717 }
718 
719 static void imx7_csi_vb2_buf_done(struct imx7_csi *csi)
720 {
721 	struct imx7_csi_vb2_buffer *done, *next;
722 	struct vb2_buffer *vb;
723 	dma_addr_t dma_addr;
724 
725 	done = csi->active_vb2_buf[csi->buf_num];
726 	if (done) {
727 		done->vbuf.field = csi->vdev_fmt.field;
728 		done->vbuf.sequence = csi->frame_sequence;
729 		vb = &done->vbuf.vb2_buf;
730 		vb->timestamp = ktime_get_ns();
731 		vb2_buffer_done(vb, VB2_BUF_STATE_DONE);
732 	}
733 	csi->frame_sequence++;
734 
735 	/* get next queued buffer */
736 	next = imx7_csi_video_next_buf(csi);
737 	if (next) {
738 		dma_addr = vb2_dma_contig_plane_dma_addr(&next->vbuf.vb2_buf, 0);
739 		csi->active_vb2_buf[csi->buf_num] = next;
740 	} else {
741 		dma_addr = csi->underrun_buf.dma_addr;
742 		csi->active_vb2_buf[csi->buf_num] = NULL;
743 	}
744 
745 	imx7_csi_update_buf(csi, dma_addr, csi->buf_num);
746 }
747 
748 static irqreturn_t imx7_csi_irq_handler(int irq, void *data)
749 {
750 	struct imx7_csi *csi =  data;
751 	u32 status;
752 
753 	spin_lock(&csi->irqlock);
754 
755 	status = imx7_csi_irq_clear(csi);
756 
757 	if (status & BIT_RFF_OR_INT) {
758 		dev_warn(csi->dev, "Rx fifo overflow\n");
759 		imx7_csi_error_recovery(csi);
760 	}
761 
762 	if (status & BIT_HRESP_ERR_INT) {
763 		dev_warn(csi->dev, "Hresponse error detected\n");
764 		imx7_csi_error_recovery(csi);
765 	}
766 
767 	if (status & BIT_ADDR_CH_ERR_INT) {
768 		imx7_csi_hw_disable(csi);
769 
770 		imx7_csi_dma_reflash(csi);
771 
772 		imx7_csi_hw_enable(csi);
773 	}
774 
775 	if ((status & BIT_DMA_TSF_DONE_FB1) &&
776 	    (status & BIT_DMA_TSF_DONE_FB2)) {
777 		/*
778 		 * For both FB1 and FB2 interrupter bits set case,
779 		 * CSI DMA is work in one of FB1 and FB2 buffer,
780 		 * but software can not know the state.
781 		 * Skip it to avoid base address updated
782 		 * when csi work in field0 and field1 will write to
783 		 * new base address.
784 		 */
785 	} else if (status & BIT_DMA_TSF_DONE_FB1) {
786 		csi->buf_num = 0;
787 	} else if (status & BIT_DMA_TSF_DONE_FB2) {
788 		csi->buf_num = 1;
789 	}
790 
791 	if ((status & BIT_DMA_TSF_DONE_FB1) ||
792 	    (status & BIT_DMA_TSF_DONE_FB2)) {
793 		imx7_csi_vb2_buf_done(csi);
794 
795 		if (csi->last_eof) {
796 			complete(&csi->last_eof_completion);
797 			csi->last_eof = false;
798 		}
799 	}
800 
801 	spin_unlock(&csi->irqlock);
802 
803 	return IRQ_HANDLED;
804 }
805 
806 /* -----------------------------------------------------------------------------
807  * Format Helpers
808  */
809 
810 #define IMX_BUS_FMTS(fmt...) (const u32[]) {fmt, 0}
811 
812 /*
813  * List of supported pixel formats for the subdevs. Keep V4L2_PIX_FMT_UYVY and
814  * MEDIA_BUS_FMT_UYVY8_2X8 first to match IMX7_CSI_DEF_PIX_FORMAT and
815  * IMX7_CSI_DEF_MBUS_CODE.
816  *
817  * TODO: Restrict the supported formats list based on the SoC integration.
818  *
819  * The CSI bridge can be configured to sample pixel components from the Rx queue
820  * in single (8bpp) or double (16bpp) component modes. Image format variants
821  * with different sample sizes (ie YUYV_2X8 vs YUYV_1X16) determine the pixel
822  * components sampling size per each clock cycle and their packing mode (see
823  * imx7_csi_configure() for details).
824  *
825  * As the CSI bridge can be interfaced with different IP blocks depending on the
826  * SoC model it is integrated on, the Rx queue sampling size should match the
827  * size of the samples transferred by the transmitting IP block. To avoid
828  * misconfigurations of the capture pipeline, the enumeration of the supported
829  * formats should be restricted to match the pixel source transmitting mode.
830  *
831  * Example: i.MX8MM SoC integrates the CSI bridge with the Samsung CSIS CSI-2
832  * receiver which operates in dual pixel sampling mode. The CSI bridge should
833  * only expose the 1X16 formats variant which instructs it to operate in dual
834  * pixel sampling mode. When the CSI bridge is instead integrated on an i.MX7,
835  * which supports both serial and parallel input, it should expose both
836  * variants.
837  *
838  * This currently only applies to YUYV formats, but other formats might need to
839  * be handled in the same way.
840  */
841 static const struct imx7_csi_pixfmt pixel_formats[] = {
842 	/*** YUV formats start here ***/
843 	{
844 		.fourcc	= V4L2_PIX_FMT_UYVY,
845 		.codes  = IMX_BUS_FMTS(
846 			MEDIA_BUS_FMT_UYVY8_2X8,
847 			MEDIA_BUS_FMT_UYVY8_1X16
848 		),
849 		.yuv	= true,
850 		.bpp    = 16,
851 	}, {
852 		.fourcc	= V4L2_PIX_FMT_YUYV,
853 		.codes  = IMX_BUS_FMTS(
854 			MEDIA_BUS_FMT_YUYV8_2X8,
855 			MEDIA_BUS_FMT_YUYV8_1X16
856 		),
857 		.yuv	= true,
858 		.bpp    = 16,
859 	},
860 	/*** raw bayer and grayscale formats start here ***/
861 	{
862 		.fourcc = V4L2_PIX_FMT_SBGGR8,
863 		.codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_SBGGR8_1X8),
864 		.bpp    = 8,
865 	}, {
866 		.fourcc = V4L2_PIX_FMT_SGBRG8,
867 		.codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_SGBRG8_1X8),
868 		.bpp    = 8,
869 	}, {
870 		.fourcc = V4L2_PIX_FMT_SGRBG8,
871 		.codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_SGRBG8_1X8),
872 		.bpp    = 8,
873 	}, {
874 		.fourcc = V4L2_PIX_FMT_SRGGB8,
875 		.codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_SRGGB8_1X8),
876 		.bpp    = 8,
877 	}, {
878 		.fourcc = V4L2_PIX_FMT_SBGGR10,
879 		.codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_SBGGR10_1X10),
880 		.bpp    = 16,
881 	}, {
882 		.fourcc = V4L2_PIX_FMT_SGBRG10,
883 		.codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_SGBRG10_1X10),
884 		.bpp    = 16,
885 	}, {
886 		.fourcc = V4L2_PIX_FMT_SGRBG10,
887 		.codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_SGRBG10_1X10),
888 		.bpp    = 16,
889 	}, {
890 		.fourcc = V4L2_PIX_FMT_SRGGB10,
891 		.codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_SRGGB10_1X10),
892 		.bpp    = 16,
893 	}, {
894 		.fourcc = V4L2_PIX_FMT_SBGGR12,
895 		.codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_SBGGR12_1X12),
896 		.bpp    = 16,
897 	}, {
898 		.fourcc = V4L2_PIX_FMT_SGBRG12,
899 		.codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_SGBRG12_1X12),
900 		.bpp    = 16,
901 	}, {
902 		.fourcc = V4L2_PIX_FMT_SGRBG12,
903 		.codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_SGRBG12_1X12),
904 		.bpp    = 16,
905 	}, {
906 		.fourcc = V4L2_PIX_FMT_SRGGB12,
907 		.codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_SRGGB12_1X12),
908 		.bpp    = 16,
909 	}, {
910 		.fourcc = V4L2_PIX_FMT_SBGGR14,
911 		.codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_SBGGR14_1X14),
912 		.bpp    = 16,
913 	}, {
914 		.fourcc = V4L2_PIX_FMT_SGBRG14,
915 		.codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_SGBRG14_1X14),
916 		.bpp    = 16,
917 	}, {
918 		.fourcc = V4L2_PIX_FMT_SGRBG14,
919 		.codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_SGRBG14_1X14),
920 		.bpp    = 16,
921 	}, {
922 		.fourcc = V4L2_PIX_FMT_SRGGB14,
923 		.codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_SRGGB14_1X14),
924 		.bpp    = 16,
925 	}, {
926 		.fourcc = V4L2_PIX_FMT_GREY,
927 		.codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_Y8_1X8),
928 		.bpp    = 8,
929 	}, {
930 		.fourcc = V4L2_PIX_FMT_Y10,
931 		.codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_Y10_1X10),
932 		.bpp    = 16,
933 	}, {
934 		.fourcc = V4L2_PIX_FMT_Y12,
935 		.codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_Y12_1X12),
936 		.bpp    = 16,
937 	}, {
938 		.fourcc = V4L2_PIX_FMT_Y14,
939 		.codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_Y14_1X14),
940 		.bpp    = 16,
941 	},
942 };
943 
944 /*
945  * Search in the pixel_formats[] array for an entry with the given fourcc
946  * return it.
947  */
948 static const struct imx7_csi_pixfmt *imx7_csi_find_pixel_format(u32 fourcc)
949 {
950 	unsigned int i;
951 
952 	for (i = 0; i < ARRAY_SIZE(pixel_formats); i++) {
953 		const struct imx7_csi_pixfmt *fmt = &pixel_formats[i];
954 
955 		if (fmt->fourcc == fourcc)
956 			return fmt;
957 	}
958 
959 	return NULL;
960 }
961 
962 /*
963  * Search in the pixel_formats[] array for an entry with the given media
964  * bus code and return it.
965  */
966 static const struct imx7_csi_pixfmt *imx7_csi_find_mbus_format(u32 code)
967 {
968 	unsigned int i;
969 
970 	for (i = 0; i < ARRAY_SIZE(pixel_formats); i++) {
971 		const struct imx7_csi_pixfmt *fmt = &pixel_formats[i];
972 		unsigned int j;
973 
974 		if (!fmt->codes)
975 			continue;
976 
977 		for (j = 0; fmt->codes[j]; j++) {
978 			if (code == fmt->codes[j])
979 				return fmt;
980 		}
981 	}
982 
983 	return NULL;
984 }
985 
986 /*
987  * Enumerate entries in the pixel_formats[] array that match the
988  * requested search criteria. Return the media-bus code that matches
989  * the search criteria at the requested match index.
990  *
991  * @code: The returned media-bus code that matches the search criteria at
992  *        the requested match index.
993  * @index: The requested match index.
994  */
995 static int imx7_csi_enum_mbus_formats(u32 *code, u32 index)
996 {
997 	unsigned int i;
998 
999 	for (i = 0; i < ARRAY_SIZE(pixel_formats); i++) {
1000 		const struct imx7_csi_pixfmt *fmt = &pixel_formats[i];
1001 		unsigned int j;
1002 
1003 		if (!fmt->codes)
1004 			continue;
1005 
1006 		for (j = 0; fmt->codes[j]; j++) {
1007 			if (index == 0) {
1008 				*code = fmt->codes[j];
1009 				return 0;
1010 			}
1011 
1012 			index--;
1013 		}
1014 	}
1015 
1016 	return -EINVAL;
1017 }
1018 
1019 /* -----------------------------------------------------------------------------
1020  * Video Capture Device - IOCTLs
1021  */
1022 
1023 static int imx7_csi_video_querycap(struct file *file, void *fh,
1024 				   struct v4l2_capability *cap)
1025 {
1026 	struct imx7_csi *csi = video_drvdata(file);
1027 
1028 	strscpy(cap->driver, IMX7_CSI_VIDEO_NAME, sizeof(cap->driver));
1029 	strscpy(cap->card, IMX7_CSI_VIDEO_NAME, sizeof(cap->card));
1030 	snprintf(cap->bus_info, sizeof(cap->bus_info),
1031 		 "platform:%s", dev_name(csi->dev));
1032 
1033 	return 0;
1034 }
1035 
1036 static int imx7_csi_video_enum_fmt_vid_cap(struct file *file, void *fh,
1037 					   struct v4l2_fmtdesc *f)
1038 {
1039 	unsigned int index = f->index;
1040 	unsigned int i;
1041 
1042 	for (i = 0; i < ARRAY_SIZE(pixel_formats); i++) {
1043 		const struct imx7_csi_pixfmt *fmt = &pixel_formats[i];
1044 
1045 		/*
1046 		 * If a media bus code is specified, only consider formats that
1047 		 * match it.
1048 		 */
1049 		if (f->mbus_code) {
1050 			unsigned int j;
1051 
1052 			if (!fmt->codes)
1053 				continue;
1054 
1055 			for (j = 0; fmt->codes[j]; j++) {
1056 				if (f->mbus_code == fmt->codes[j])
1057 					break;
1058 			}
1059 
1060 			if (!fmt->codes[j])
1061 				continue;
1062 		}
1063 
1064 		if (index == 0) {
1065 			f->pixelformat = fmt->fourcc;
1066 			return 0;
1067 		}
1068 
1069 		index--;
1070 	}
1071 
1072 	return -EINVAL;
1073 }
1074 
1075 static int imx7_csi_video_enum_framesizes(struct file *file, void *fh,
1076 					  struct v4l2_frmsizeenum *fsize)
1077 {
1078 	const struct imx7_csi_pixfmt *cc;
1079 	u32 walign;
1080 
1081 	if (fsize->index > 0)
1082 		return -EINVAL;
1083 
1084 	cc = imx7_csi_find_pixel_format(fsize->pixel_format);
1085 	if (!cc)
1086 		return -EINVAL;
1087 
1088 	/*
1089 	 * The width alignment is 8 bytes as indicated by the
1090 	 * CSI_IMAG_PARA.IMAGE_WIDTH documentation. Convert it to pixels.
1091 	 */
1092 	walign = 8 * 8 / cc->bpp;
1093 
1094 	fsize->type = V4L2_FRMSIZE_TYPE_CONTINUOUS;
1095 	fsize->stepwise.min_width = walign;
1096 	fsize->stepwise.max_width = round_down(65535U, walign);
1097 	fsize->stepwise.min_height = 1;
1098 	fsize->stepwise.max_height = 65535;
1099 	fsize->stepwise.step_width = walign;
1100 	fsize->stepwise.step_height = 1;
1101 
1102 	return 0;
1103 }
1104 
1105 static int imx7_csi_video_g_fmt_vid_cap(struct file *file, void *fh,
1106 					struct v4l2_format *f)
1107 {
1108 	struct imx7_csi *csi = video_drvdata(file);
1109 
1110 	f->fmt.pix = csi->vdev_fmt;
1111 
1112 	return 0;
1113 }
1114 
1115 static const struct imx7_csi_pixfmt *
1116 __imx7_csi_video_try_fmt(struct v4l2_pix_format *pixfmt,
1117 			 struct v4l2_rect *compose)
1118 {
1119 	const struct imx7_csi_pixfmt *cc;
1120 	u32 walign;
1121 
1122 	if (compose) {
1123 		compose->width = pixfmt->width;
1124 		compose->height = pixfmt->height;
1125 	}
1126 
1127 	/*
1128 	 * Find the pixel format, default to the first supported format if not
1129 	 * found.
1130 	 */
1131 	cc = imx7_csi_find_pixel_format(pixfmt->pixelformat);
1132 	if (!cc) {
1133 		pixfmt->pixelformat = IMX7_CSI_DEF_PIX_FORMAT;
1134 		cc = imx7_csi_find_pixel_format(pixfmt->pixelformat);
1135 	}
1136 
1137 	/*
1138 	 * The width alignment is 8 bytes as indicated by the
1139 	 * CSI_IMAG_PARA.IMAGE_WIDTH documentation. Convert it to pixels.
1140 	 *
1141 	 * TODO: Implement configurable stride support.
1142 	 */
1143 	walign = 8 * 8 / cc->bpp;
1144 	pixfmt->width = clamp(round_up(pixfmt->width, walign), walign,
1145 			      round_down(65535U, walign));
1146 	pixfmt->height = clamp(pixfmt->height, 1U, 65535U);
1147 
1148 	pixfmt->bytesperline = pixfmt->width * cc->bpp / 8;
1149 	pixfmt->sizeimage = pixfmt->bytesperline * pixfmt->height;
1150 	pixfmt->field = V4L2_FIELD_NONE;
1151 
1152 	return cc;
1153 }
1154 
1155 static int imx7_csi_video_try_fmt_vid_cap(struct file *file, void *fh,
1156 					  struct v4l2_format *f)
1157 {
1158 	__imx7_csi_video_try_fmt(&f->fmt.pix, NULL);
1159 	return 0;
1160 }
1161 
1162 static int imx7_csi_video_s_fmt_vid_cap(struct file *file, void *fh,
1163 					struct v4l2_format *f)
1164 {
1165 	struct imx7_csi *csi = video_drvdata(file);
1166 	const struct imx7_csi_pixfmt *cc;
1167 
1168 	if (vb2_is_busy(&csi->q)) {
1169 		dev_err(csi->dev, "%s queue busy\n", __func__);
1170 		return -EBUSY;
1171 	}
1172 
1173 	cc = __imx7_csi_video_try_fmt(&f->fmt.pix, &csi->vdev_compose);
1174 
1175 	csi->vdev_cc = cc;
1176 	csi->vdev_fmt = f->fmt.pix;
1177 
1178 	return 0;
1179 }
1180 
1181 static int imx7_csi_video_g_selection(struct file *file, void *fh,
1182 				      struct v4l2_selection *s)
1183 {
1184 	struct imx7_csi *csi = video_drvdata(file);
1185 
1186 	if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
1187 		return -EINVAL;
1188 
1189 	switch (s->target) {
1190 	case V4L2_SEL_TGT_COMPOSE:
1191 	case V4L2_SEL_TGT_COMPOSE_DEFAULT:
1192 	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
1193 		/* The compose rectangle is fixed to the source format. */
1194 		s->r = csi->vdev_compose;
1195 		break;
1196 	case V4L2_SEL_TGT_COMPOSE_PADDED:
1197 		/*
1198 		 * The hardware writes with a configurable but fixed DMA burst
1199 		 * size. If the source format width is not burst size aligned,
1200 		 * the written frame contains padding to the right.
1201 		 */
1202 		s->r.left = 0;
1203 		s->r.top = 0;
1204 		s->r.width = csi->vdev_fmt.width;
1205 		s->r.height = csi->vdev_fmt.height;
1206 		break;
1207 	default:
1208 		return -EINVAL;
1209 	}
1210 
1211 	return 0;
1212 }
1213 
1214 static const struct v4l2_ioctl_ops imx7_csi_video_ioctl_ops = {
1215 	.vidioc_querycap		= imx7_csi_video_querycap,
1216 
1217 	.vidioc_enum_fmt_vid_cap	= imx7_csi_video_enum_fmt_vid_cap,
1218 	.vidioc_enum_framesizes		= imx7_csi_video_enum_framesizes,
1219 
1220 	.vidioc_g_fmt_vid_cap		= imx7_csi_video_g_fmt_vid_cap,
1221 	.vidioc_try_fmt_vid_cap		= imx7_csi_video_try_fmt_vid_cap,
1222 	.vidioc_s_fmt_vid_cap		= imx7_csi_video_s_fmt_vid_cap,
1223 
1224 	.vidioc_g_selection		= imx7_csi_video_g_selection,
1225 
1226 	.vidioc_reqbufs			= vb2_ioctl_reqbufs,
1227 	.vidioc_create_bufs		= vb2_ioctl_create_bufs,
1228 	.vidioc_prepare_buf		= vb2_ioctl_prepare_buf,
1229 	.vidioc_querybuf		= vb2_ioctl_querybuf,
1230 	.vidioc_qbuf			= vb2_ioctl_qbuf,
1231 	.vidioc_dqbuf			= vb2_ioctl_dqbuf,
1232 	.vidioc_expbuf			= vb2_ioctl_expbuf,
1233 	.vidioc_streamon		= vb2_ioctl_streamon,
1234 	.vidioc_streamoff		= vb2_ioctl_streamoff,
1235 };
1236 
1237 /* -----------------------------------------------------------------------------
1238  * Video Capture Device - Queue Operations
1239  */
1240 
1241 static int imx7_csi_video_queue_setup(struct vb2_queue *vq,
1242 				      unsigned int *nbuffers,
1243 				      unsigned int *nplanes,
1244 				      unsigned int sizes[],
1245 				      struct device *alloc_devs[])
1246 {
1247 	struct imx7_csi *csi = vb2_get_drv_priv(vq);
1248 	struct v4l2_pix_format *pix = &csi->vdev_fmt;
1249 	unsigned int count = *nbuffers;
1250 
1251 	if (vq->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
1252 		return -EINVAL;
1253 
1254 	if (*nplanes) {
1255 		if (*nplanes != 1 || sizes[0] < pix->sizeimage)
1256 			return -EINVAL;
1257 		count += vq->num_buffers;
1258 	}
1259 
1260 	count = min_t(__u32, IMX7_CSI_VIDEO_MEM_LIMIT / pix->sizeimage, count);
1261 
1262 	if (*nplanes)
1263 		*nbuffers = (count < vq->num_buffers) ? 0 :
1264 			count - vq->num_buffers;
1265 	else
1266 		*nbuffers = count;
1267 
1268 	*nplanes = 1;
1269 	sizes[0] = pix->sizeimage;
1270 
1271 	return 0;
1272 }
1273 
1274 static int imx7_csi_video_buf_init(struct vb2_buffer *vb)
1275 {
1276 	struct imx7_csi_vb2_buffer *buf = to_imx7_csi_vb2_buffer(vb);
1277 
1278 	INIT_LIST_HEAD(&buf->list);
1279 
1280 	return 0;
1281 }
1282 
1283 static int imx7_csi_video_buf_prepare(struct vb2_buffer *vb)
1284 {
1285 	struct imx7_csi *csi = vb2_get_drv_priv(vb->vb2_queue);
1286 	struct v4l2_pix_format *pix = &csi->vdev_fmt;
1287 
1288 	if (vb2_plane_size(vb, 0) < pix->sizeimage) {
1289 		dev_err(csi->dev,
1290 			"data will not fit into plane (%lu < %lu)\n",
1291 			vb2_plane_size(vb, 0), (long)pix->sizeimage);
1292 		return -EINVAL;
1293 	}
1294 
1295 	vb2_set_plane_payload(vb, 0, pix->sizeimage);
1296 
1297 	return 0;
1298 }
1299 
1300 static bool imx7_csi_fast_track_buffer(struct imx7_csi *csi,
1301 				       struct imx7_csi_vb2_buffer *buf)
1302 {
1303 	unsigned long flags;
1304 	dma_addr_t dma_addr;
1305 	int buf_num;
1306 	u32 isr;
1307 
1308 	if (!csi->is_streaming)
1309 		return false;
1310 
1311 	dma_addr = vb2_dma_contig_plane_dma_addr(&buf->vbuf.vb2_buf, 0);
1312 
1313 	/*
1314 	 * buf_num holds the framebuffer ID of the most recently (*not* the
1315 	 * next anticipated) triggered interrupt. Without loss of generality,
1316 	 * if buf_num is 0, the hardware is capturing to FB2. If FB1 has been
1317 	 * programmed with a dummy buffer (as indicated by active_vb2_buf[0]
1318 	 * being NULL), then we can fast-track the new buffer by programming
1319 	 * its address in FB1 before the hardware completes FB2, instead of
1320 	 * adding it to the buffer queue and incurring a delay of one
1321 	 * additional frame.
1322 	 *
1323 	 * The irqlock prevents races with the interrupt handler that updates
1324 	 * buf_num when it programs the next buffer, but we can still race with
1325 	 * the hardware if we program the buffer in FB1 just after the hardware
1326 	 * completes FB2 and switches to FB1 and before buf_num can be updated
1327 	 * by the interrupt handler for FB2.  The fast-tracked buffer would
1328 	 * then be ignored by the hardware while the driver would think it has
1329 	 * successfully been processed.
1330 	 *
1331 	 * To avoid this problem, if we can't avoid the race, we can detect
1332 	 * that we have lost it by checking, after programming the buffer in
1333 	 * FB1, if the interrupt flag indicating completion of FB2 has been
1334 	 * raised. If that is not the case, fast-tracking succeeded, and we can
1335 	 * update active_vb2_buf[0]. Otherwise, we may or may not have lost the
1336 	 * race (as the interrupt flag may have been raised just after
1337 	 * programming FB1 and before we read the interrupt status register),
1338 	 * and we need to assume the worst case of a race loss and queue the
1339 	 * buffer through the slow path.
1340 	 */
1341 
1342 	spin_lock_irqsave(&csi->irqlock, flags);
1343 
1344 	buf_num = csi->buf_num;
1345 	if (csi->active_vb2_buf[buf_num]) {
1346 		spin_unlock_irqrestore(&csi->irqlock, flags);
1347 		return false;
1348 	}
1349 
1350 	imx7_csi_update_buf(csi, dma_addr, buf_num);
1351 
1352 	isr = imx7_csi_reg_read(csi, CSI_CSISR);
1353 	if (isr & (buf_num ? BIT_DMA_TSF_DONE_FB1 : BIT_DMA_TSF_DONE_FB2)) {
1354 		/*
1355 		 * The interrupt for the /other/ FB just came (the isr hasn't
1356 		 * run yet though, because we have the lock here); we can't be
1357 		 * sure we've programmed buf_num FB in time, so queue the buffer
1358 		 * to the buffer queue normally. No need to undo writing the FB
1359 		 * register, since we won't return it as active_vb2_buf is NULL,
1360 		 * so it's okay to potentially write it to both FB1 and FB2;
1361 		 * only the one where it was queued normally will be returned.
1362 		 */
1363 		spin_unlock_irqrestore(&csi->irqlock, flags);
1364 		return false;
1365 	}
1366 
1367 	csi->active_vb2_buf[buf_num] = buf;
1368 
1369 	spin_unlock_irqrestore(&csi->irqlock, flags);
1370 	return true;
1371 }
1372 
1373 static void imx7_csi_video_buf_queue(struct vb2_buffer *vb)
1374 {
1375 	struct imx7_csi *csi = vb2_get_drv_priv(vb->vb2_queue);
1376 	struct imx7_csi_vb2_buffer *buf = to_imx7_csi_vb2_buffer(vb);
1377 	unsigned long flags;
1378 
1379 	if (imx7_csi_fast_track_buffer(csi, buf))
1380 		return;
1381 
1382 	spin_lock_irqsave(&csi->q_lock, flags);
1383 
1384 	list_add_tail(&buf->list, &csi->ready_q);
1385 
1386 	spin_unlock_irqrestore(&csi->q_lock, flags);
1387 }
1388 
1389 static int imx7_csi_video_validate_fmt(struct imx7_csi *csi)
1390 {
1391 	struct v4l2_subdev_format fmt_src = {
1392 		.pad = IMX7_CSI_PAD_SRC,
1393 		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
1394 	};
1395 	const struct imx7_csi_pixfmt *cc;
1396 	int ret;
1397 
1398 	/* Retrieve the media bus format on the source subdev. */
1399 	ret = v4l2_subdev_call_state_active(&csi->sd, pad, get_fmt, &fmt_src);
1400 	if (ret)
1401 		return ret;
1402 
1403 	/*
1404 	 * Verify that the media bus size matches the size set on the video
1405 	 * node. It is sufficient to check the compose rectangle size without
1406 	 * checking the rounded size from pix_fmt, as the rounded size is
1407 	 * derived directly from the compose rectangle size, and will thus
1408 	 * always match if the compose rectangle matches.
1409 	 */
1410 	if (csi->vdev_compose.width != fmt_src.format.width ||
1411 	    csi->vdev_compose.height != fmt_src.format.height)
1412 		return -EPIPE;
1413 
1414 	/*
1415 	 * Verify that the media bus code is compatible with the pixel format
1416 	 * set on the video node.
1417 	 */
1418 	cc = imx7_csi_find_mbus_format(fmt_src.format.code);
1419 	if (!cc || csi->vdev_cc->yuv != cc->yuv)
1420 		return -EPIPE;
1421 
1422 	return 0;
1423 }
1424 
1425 static int imx7_csi_video_start_streaming(struct vb2_queue *vq,
1426 					  unsigned int count)
1427 {
1428 	struct imx7_csi *csi = vb2_get_drv_priv(vq);
1429 	struct imx7_csi_vb2_buffer *buf, *tmp;
1430 	unsigned long flags;
1431 	int ret;
1432 
1433 	ret = imx7_csi_video_validate_fmt(csi);
1434 	if (ret) {
1435 		dev_err(csi->dev, "capture format not valid\n");
1436 		goto err_buffers;
1437 	}
1438 
1439 	mutex_lock(&csi->mdev.graph_mutex);
1440 
1441 	ret = __video_device_pipeline_start(csi->vdev, &csi->pipe);
1442 	if (ret)
1443 		goto err_unlock;
1444 
1445 	ret = v4l2_subdev_call(&csi->sd, video, s_stream, 1);
1446 	if (ret)
1447 		goto err_stop;
1448 
1449 	mutex_unlock(&csi->mdev.graph_mutex);
1450 
1451 	return 0;
1452 
1453 err_stop:
1454 	__video_device_pipeline_stop(csi->vdev);
1455 err_unlock:
1456 	mutex_unlock(&csi->mdev.graph_mutex);
1457 	dev_err(csi->dev, "pipeline start failed with %d\n", ret);
1458 err_buffers:
1459 	spin_lock_irqsave(&csi->q_lock, flags);
1460 	list_for_each_entry_safe(buf, tmp, &csi->ready_q, list) {
1461 		list_del(&buf->list);
1462 		vb2_buffer_done(&buf->vbuf.vb2_buf, VB2_BUF_STATE_QUEUED);
1463 	}
1464 	spin_unlock_irqrestore(&csi->q_lock, flags);
1465 	return ret;
1466 }
1467 
1468 static void imx7_csi_video_stop_streaming(struct vb2_queue *vq)
1469 {
1470 	struct imx7_csi *csi = vb2_get_drv_priv(vq);
1471 	struct imx7_csi_vb2_buffer *frame;
1472 	struct imx7_csi_vb2_buffer *tmp;
1473 	unsigned long flags;
1474 
1475 	mutex_lock(&csi->mdev.graph_mutex);
1476 	v4l2_subdev_call(&csi->sd, video, s_stream, 0);
1477 	__video_device_pipeline_stop(csi->vdev);
1478 	mutex_unlock(&csi->mdev.graph_mutex);
1479 
1480 	/* release all active buffers */
1481 	spin_lock_irqsave(&csi->q_lock, flags);
1482 	list_for_each_entry_safe(frame, tmp, &csi->ready_q, list) {
1483 		list_del(&frame->list);
1484 		vb2_buffer_done(&frame->vbuf.vb2_buf, VB2_BUF_STATE_ERROR);
1485 	}
1486 	spin_unlock_irqrestore(&csi->q_lock, flags);
1487 }
1488 
1489 static const struct vb2_ops imx7_csi_video_qops = {
1490 	.queue_setup	 = imx7_csi_video_queue_setup,
1491 	.buf_init        = imx7_csi_video_buf_init,
1492 	.buf_prepare	 = imx7_csi_video_buf_prepare,
1493 	.buf_queue	 = imx7_csi_video_buf_queue,
1494 	.wait_prepare	 = vb2_ops_wait_prepare,
1495 	.wait_finish	 = vb2_ops_wait_finish,
1496 	.start_streaming = imx7_csi_video_start_streaming,
1497 	.stop_streaming  = imx7_csi_video_stop_streaming,
1498 };
1499 
1500 /* -----------------------------------------------------------------------------
1501  * Video Capture Device - File Operations
1502  */
1503 
1504 static int imx7_csi_video_open(struct file *file)
1505 {
1506 	struct imx7_csi *csi = video_drvdata(file);
1507 	int ret;
1508 
1509 	if (mutex_lock_interruptible(&csi->vdev_mutex))
1510 		return -ERESTARTSYS;
1511 
1512 	ret = v4l2_fh_open(file);
1513 	if (ret) {
1514 		dev_err(csi->dev, "v4l2_fh_open failed\n");
1515 		goto out;
1516 	}
1517 
1518 	ret = v4l2_pipeline_pm_get(&csi->vdev->entity);
1519 	if (ret)
1520 		v4l2_fh_release(file);
1521 
1522 out:
1523 	mutex_unlock(&csi->vdev_mutex);
1524 	return ret;
1525 }
1526 
1527 static int imx7_csi_video_release(struct file *file)
1528 {
1529 	struct imx7_csi *csi = video_drvdata(file);
1530 	struct vb2_queue *vq = &csi->q;
1531 
1532 	mutex_lock(&csi->vdev_mutex);
1533 
1534 	if (file->private_data == vq->owner) {
1535 		vb2_queue_release(vq);
1536 		vq->owner = NULL;
1537 	}
1538 
1539 	v4l2_pipeline_pm_put(&csi->vdev->entity);
1540 
1541 	v4l2_fh_release(file);
1542 	mutex_unlock(&csi->vdev_mutex);
1543 	return 0;
1544 }
1545 
1546 static const struct v4l2_file_operations imx7_csi_video_fops = {
1547 	.owner		= THIS_MODULE,
1548 	.open		= imx7_csi_video_open,
1549 	.release	= imx7_csi_video_release,
1550 	.poll		= vb2_fop_poll,
1551 	.unlocked_ioctl	= video_ioctl2,
1552 	.mmap		= vb2_fop_mmap,
1553 };
1554 
1555 /* -----------------------------------------------------------------------------
1556  * Video Capture Device - Init & Cleanup
1557  */
1558 
1559 static struct imx7_csi_vb2_buffer *imx7_csi_video_next_buf(struct imx7_csi *csi)
1560 {
1561 	struct imx7_csi_vb2_buffer *buf = NULL;
1562 	unsigned long flags;
1563 
1564 	spin_lock_irqsave(&csi->q_lock, flags);
1565 
1566 	/* get next queued buffer */
1567 	if (!list_empty(&csi->ready_q)) {
1568 		buf = list_entry(csi->ready_q.next, struct imx7_csi_vb2_buffer,
1569 				 list);
1570 		list_del(&buf->list);
1571 	}
1572 
1573 	spin_unlock_irqrestore(&csi->q_lock, flags);
1574 
1575 	return buf;
1576 }
1577 
1578 static void imx7_csi_video_init_format(struct imx7_csi *csi)
1579 {
1580 	struct v4l2_pix_format *pixfmt = &csi->vdev_fmt;
1581 
1582 	pixfmt->width = IMX7_CSI_DEF_PIX_WIDTH;
1583 	pixfmt->height = IMX7_CSI_DEF_PIX_HEIGHT;
1584 
1585 	csi->vdev_cc = __imx7_csi_video_try_fmt(pixfmt, &csi->vdev_compose);
1586 }
1587 
1588 static int imx7_csi_video_register(struct imx7_csi *csi)
1589 {
1590 	struct v4l2_subdev *sd = &csi->sd;
1591 	struct v4l2_device *v4l2_dev = sd->v4l2_dev;
1592 	struct video_device *vdev = csi->vdev;
1593 	int ret;
1594 
1595 	vdev->v4l2_dev = v4l2_dev;
1596 
1597 	/* Initialize the default format and compose rectangle. */
1598 	imx7_csi_video_init_format(csi);
1599 
1600 	/* Register the video device. */
1601 	ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1);
1602 	if (ret) {
1603 		dev_err(csi->dev, "Failed to register video device\n");
1604 		return ret;
1605 	}
1606 
1607 	dev_info(csi->dev, "Registered %s as /dev/%s\n", vdev->name,
1608 		 video_device_node_name(vdev));
1609 
1610 	/* Create the link from the CSI subdev to the video device. */
1611 	ret = media_create_pad_link(&sd->entity, IMX7_CSI_PAD_SRC,
1612 				    &vdev->entity, 0, MEDIA_LNK_FL_IMMUTABLE |
1613 				    MEDIA_LNK_FL_ENABLED);
1614 	if (ret) {
1615 		dev_err(csi->dev, "failed to create link to device node\n");
1616 		video_unregister_device(vdev);
1617 		return ret;
1618 	}
1619 
1620 	return 0;
1621 }
1622 
1623 static void imx7_csi_video_unregister(struct imx7_csi *csi)
1624 {
1625 	media_entity_cleanup(&csi->vdev->entity);
1626 	video_unregister_device(csi->vdev);
1627 }
1628 
1629 static int imx7_csi_video_init(struct imx7_csi *csi)
1630 {
1631 	struct video_device *vdev;
1632 	struct vb2_queue *vq;
1633 	int ret;
1634 
1635 	mutex_init(&csi->vdev_mutex);
1636 	INIT_LIST_HEAD(&csi->ready_q);
1637 	spin_lock_init(&csi->q_lock);
1638 
1639 	/* Allocate and initialize the video device. */
1640 	vdev = video_device_alloc();
1641 	if (!vdev)
1642 		return -ENOMEM;
1643 
1644 	vdev->fops = &imx7_csi_video_fops;
1645 	vdev->ioctl_ops = &imx7_csi_video_ioctl_ops;
1646 	vdev->minor = -1;
1647 	vdev->release = video_device_release;
1648 	vdev->vfl_dir = VFL_DIR_RX;
1649 	vdev->tvnorms = V4L2_STD_NTSC | V4L2_STD_PAL | V4L2_STD_SECAM;
1650 	vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING
1651 			 | V4L2_CAP_IO_MC;
1652 	vdev->lock = &csi->vdev_mutex;
1653 	vdev->queue = &csi->q;
1654 
1655 	snprintf(vdev->name, sizeof(vdev->name), "%s capture", csi->sd.name);
1656 
1657 	video_set_drvdata(vdev, csi);
1658 	csi->vdev = vdev;
1659 
1660 	/* Initialize the video device pad. */
1661 	csi->vdev_pad.flags = MEDIA_PAD_FL_SINK;
1662 	ret = media_entity_pads_init(&vdev->entity, 1, &csi->vdev_pad);
1663 	if (ret) {
1664 		video_device_release(vdev);
1665 		return ret;
1666 	}
1667 
1668 	/* Initialize the vb2 queue. */
1669 	vq = &csi->q;
1670 	vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1671 	vq->io_modes = VB2_MMAP | VB2_DMABUF;
1672 	vq->drv_priv = csi;
1673 	vq->buf_struct_size = sizeof(struct imx7_csi_vb2_buffer);
1674 	vq->ops = &imx7_csi_video_qops;
1675 	vq->mem_ops = &vb2_dma_contig_memops;
1676 	vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
1677 	vq->lock = &csi->vdev_mutex;
1678 	vq->min_buffers_needed = 2;
1679 	vq->dev = csi->dev;
1680 
1681 	ret = vb2_queue_init(vq);
1682 	if (ret) {
1683 		dev_err(csi->dev, "vb2_queue_init failed\n");
1684 		video_device_release(vdev);
1685 		return ret;
1686 	}
1687 
1688 	return 0;
1689 }
1690 
1691 /* -----------------------------------------------------------------------------
1692  * V4L2 Subdev Operations
1693  */
1694 
1695 static int imx7_csi_s_stream(struct v4l2_subdev *sd, int enable)
1696 {
1697 	struct imx7_csi *csi = v4l2_get_subdevdata(sd);
1698 	struct v4l2_subdev_state *sd_state;
1699 	int ret = 0;
1700 
1701 	sd_state = v4l2_subdev_lock_and_get_active_state(sd);
1702 
1703 	if (enable) {
1704 		ret = imx7_csi_init(csi, sd_state);
1705 		if (ret < 0)
1706 			goto out_unlock;
1707 
1708 		ret = v4l2_subdev_call(csi->src_sd, video, s_stream, 1);
1709 		if (ret < 0) {
1710 			imx7_csi_deinit(csi, VB2_BUF_STATE_QUEUED);
1711 			goto out_unlock;
1712 		}
1713 
1714 		imx7_csi_enable(csi);
1715 	} else {
1716 		imx7_csi_disable(csi);
1717 
1718 		v4l2_subdev_call(csi->src_sd, video, s_stream, 0);
1719 
1720 		imx7_csi_deinit(csi, VB2_BUF_STATE_ERROR);
1721 	}
1722 
1723 	csi->is_streaming = !!enable;
1724 
1725 out_unlock:
1726 	v4l2_subdev_unlock_state(sd_state);
1727 
1728 	return ret;
1729 }
1730 
1731 static int imx7_csi_init_cfg(struct v4l2_subdev *sd,
1732 			     struct v4l2_subdev_state *sd_state)
1733 {
1734 	const struct imx7_csi_pixfmt *cc;
1735 	int i;
1736 
1737 	cc = imx7_csi_find_mbus_format(IMX7_CSI_DEF_MBUS_CODE);
1738 
1739 	for (i = 0; i < IMX7_CSI_PADS_NUM; i++) {
1740 		struct v4l2_mbus_framefmt *mf =
1741 			v4l2_subdev_get_pad_format(sd, sd_state, i);
1742 
1743 		mf->code = IMX7_CSI_DEF_MBUS_CODE;
1744 		mf->width = IMX7_CSI_DEF_PIX_WIDTH;
1745 		mf->height = IMX7_CSI_DEF_PIX_HEIGHT;
1746 		mf->field = V4L2_FIELD_NONE;
1747 
1748 		mf->colorspace = V4L2_COLORSPACE_SRGB;
1749 		mf->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(mf->colorspace);
1750 		mf->ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(mf->colorspace);
1751 		mf->quantization = V4L2_MAP_QUANTIZATION_DEFAULT(!cc->yuv,
1752 					mf->colorspace, mf->ycbcr_enc);
1753 	}
1754 
1755 	return 0;
1756 }
1757 
1758 static int imx7_csi_enum_mbus_code(struct v4l2_subdev *sd,
1759 				   struct v4l2_subdev_state *sd_state,
1760 				   struct v4l2_subdev_mbus_code_enum *code)
1761 {
1762 	struct v4l2_mbus_framefmt *in_fmt;
1763 	int ret = 0;
1764 
1765 	in_fmt = v4l2_subdev_get_pad_format(sd, sd_state, IMX7_CSI_PAD_SINK);
1766 
1767 	switch (code->pad) {
1768 	case IMX7_CSI_PAD_SINK:
1769 		ret = imx7_csi_enum_mbus_formats(&code->code, code->index);
1770 		break;
1771 
1772 	case IMX7_CSI_PAD_SRC:
1773 		if (code->index != 0) {
1774 			ret = -EINVAL;
1775 			break;
1776 		}
1777 
1778 		code->code = in_fmt->code;
1779 		break;
1780 
1781 	default:
1782 		ret = -EINVAL;
1783 		break;
1784 	}
1785 
1786 	return ret;
1787 }
1788 
1789 /*
1790  * Default the colorspace in tryfmt to SRGB if set to an unsupported
1791  * colorspace or not initialized. Then set the remaining colorimetry
1792  * parameters based on the colorspace if they are uninitialized.
1793  *
1794  * tryfmt->code must be set on entry.
1795  */
1796 static void imx7_csi_try_colorimetry(struct v4l2_mbus_framefmt *tryfmt)
1797 {
1798 	const struct imx7_csi_pixfmt *cc;
1799 	bool is_rgb = false;
1800 
1801 	cc = imx7_csi_find_mbus_format(tryfmt->code);
1802 	if (cc && !cc->yuv)
1803 		is_rgb = true;
1804 
1805 	switch (tryfmt->colorspace) {
1806 	case V4L2_COLORSPACE_SMPTE170M:
1807 	case V4L2_COLORSPACE_REC709:
1808 	case V4L2_COLORSPACE_JPEG:
1809 	case V4L2_COLORSPACE_SRGB:
1810 	case V4L2_COLORSPACE_BT2020:
1811 	case V4L2_COLORSPACE_OPRGB:
1812 	case V4L2_COLORSPACE_DCI_P3:
1813 	case V4L2_COLORSPACE_RAW:
1814 		break;
1815 	default:
1816 		tryfmt->colorspace = V4L2_COLORSPACE_SRGB;
1817 		break;
1818 	}
1819 
1820 	if (tryfmt->xfer_func == V4L2_XFER_FUNC_DEFAULT)
1821 		tryfmt->xfer_func =
1822 			V4L2_MAP_XFER_FUNC_DEFAULT(tryfmt->colorspace);
1823 
1824 	if (tryfmt->ycbcr_enc == V4L2_YCBCR_ENC_DEFAULT)
1825 		tryfmt->ycbcr_enc =
1826 			V4L2_MAP_YCBCR_ENC_DEFAULT(tryfmt->colorspace);
1827 
1828 	if (tryfmt->quantization == V4L2_QUANTIZATION_DEFAULT)
1829 		tryfmt->quantization =
1830 			V4L2_MAP_QUANTIZATION_DEFAULT(is_rgb,
1831 						      tryfmt->colorspace,
1832 						      tryfmt->ycbcr_enc);
1833 }
1834 
1835 static void imx7_csi_try_fmt(struct v4l2_subdev *sd,
1836 			     struct v4l2_subdev_state *sd_state,
1837 			     struct v4l2_subdev_format *sdformat,
1838 			     const struct imx7_csi_pixfmt **cc)
1839 {
1840 	const struct imx7_csi_pixfmt *in_cc;
1841 	struct v4l2_mbus_framefmt *in_fmt;
1842 	u32 code;
1843 
1844 	in_fmt = v4l2_subdev_get_pad_format(sd, sd_state, IMX7_CSI_PAD_SINK);
1845 
1846 	switch (sdformat->pad) {
1847 	case IMX7_CSI_PAD_SRC:
1848 		in_cc = imx7_csi_find_mbus_format(in_fmt->code);
1849 
1850 		sdformat->format.width = in_fmt->width;
1851 		sdformat->format.height = in_fmt->height;
1852 		sdformat->format.code = in_fmt->code;
1853 		sdformat->format.field = in_fmt->field;
1854 		*cc = in_cc;
1855 
1856 		sdformat->format.colorspace = in_fmt->colorspace;
1857 		sdformat->format.xfer_func = in_fmt->xfer_func;
1858 		sdformat->format.quantization = in_fmt->quantization;
1859 		sdformat->format.ycbcr_enc = in_fmt->ycbcr_enc;
1860 		break;
1861 
1862 	case IMX7_CSI_PAD_SINK:
1863 		*cc = imx7_csi_find_mbus_format(sdformat->format.code);
1864 		if (!*cc) {
1865 			code = IMX7_CSI_DEF_MBUS_CODE;
1866 			*cc = imx7_csi_find_mbus_format(code);
1867 			sdformat->format.code = code;
1868 		}
1869 
1870 		if (sdformat->format.field != V4L2_FIELD_INTERLACED)
1871 			sdformat->format.field = V4L2_FIELD_NONE;
1872 		break;
1873 	}
1874 
1875 	imx7_csi_try_colorimetry(&sdformat->format);
1876 }
1877 
1878 static int imx7_csi_set_fmt(struct v4l2_subdev *sd,
1879 			    struct v4l2_subdev_state *sd_state,
1880 			    struct v4l2_subdev_format *sdformat)
1881 {
1882 	struct imx7_csi *csi = v4l2_get_subdevdata(sd);
1883 	const struct imx7_csi_pixfmt *outcc;
1884 	struct v4l2_mbus_framefmt *outfmt;
1885 	const struct imx7_csi_pixfmt *cc;
1886 	struct v4l2_mbus_framefmt *fmt;
1887 	struct v4l2_subdev_format format;
1888 
1889 	if (csi->is_streaming)
1890 		return -EBUSY;
1891 
1892 	imx7_csi_try_fmt(sd, sd_state, sdformat, &cc);
1893 
1894 	fmt = v4l2_subdev_get_pad_format(sd, sd_state, sdformat->pad);
1895 
1896 	*fmt = sdformat->format;
1897 
1898 	if (sdformat->pad == IMX7_CSI_PAD_SINK) {
1899 		/* propagate format to source pads */
1900 		format.pad = IMX7_CSI_PAD_SRC;
1901 		format.which = sdformat->which;
1902 		format.format = sdformat->format;
1903 		imx7_csi_try_fmt(sd, sd_state, &format, &outcc);
1904 
1905 		outfmt = v4l2_subdev_get_pad_format(sd, sd_state,
1906 						    IMX7_CSI_PAD_SRC);
1907 		*outfmt = format.format;
1908 	}
1909 
1910 	return 0;
1911 }
1912 
1913 static int imx7_csi_pad_link_validate(struct v4l2_subdev *sd,
1914 				      struct media_link *link,
1915 				      struct v4l2_subdev_format *source_fmt,
1916 				      struct v4l2_subdev_format *sink_fmt)
1917 {
1918 	struct imx7_csi *csi = v4l2_get_subdevdata(sd);
1919 	struct media_pad *pad = NULL;
1920 	unsigned int i;
1921 	int ret;
1922 
1923 	/*
1924 	 * Validate the source link, and record whether the source uses the
1925 	 * parallel input or the CSI-2 receiver.
1926 	 */
1927 	ret = v4l2_subdev_link_validate_default(sd, link, source_fmt, sink_fmt);
1928 	if (ret)
1929 		return ret;
1930 
1931 	switch (csi->src_sd->entity.function) {
1932 	case MEDIA_ENT_F_VID_IF_BRIDGE:
1933 		/* The input is the CSI-2 receiver. */
1934 		csi->is_csi2 = true;
1935 		break;
1936 
1937 	case MEDIA_ENT_F_VID_MUX:
1938 		/* The input is the mux, check its input. */
1939 		for (i = 0; i < csi->src_sd->entity.num_pads; i++) {
1940 			struct media_pad *spad = &csi->src_sd->entity.pads[i];
1941 
1942 			if (!(spad->flags & MEDIA_PAD_FL_SINK))
1943 				continue;
1944 
1945 			pad = media_pad_remote_pad_first(spad);
1946 			if (pad)
1947 				break;
1948 		}
1949 
1950 		if (!pad)
1951 			return -ENODEV;
1952 
1953 		csi->is_csi2 = pad->entity->function == MEDIA_ENT_F_VID_IF_BRIDGE;
1954 		break;
1955 
1956 	default:
1957 		/*
1958 		 * The input is an external entity, it must use the parallel
1959 		 * bus.
1960 		 */
1961 		csi->is_csi2 = false;
1962 		break;
1963 	}
1964 
1965 	return 0;
1966 }
1967 
1968 static int imx7_csi_registered(struct v4l2_subdev *sd)
1969 {
1970 	struct imx7_csi *csi = v4l2_get_subdevdata(sd);
1971 	int ret;
1972 
1973 	ret = imx7_csi_video_init(csi);
1974 	if (ret)
1975 		return ret;
1976 
1977 	ret = imx7_csi_video_register(csi);
1978 	if (ret)
1979 		return ret;
1980 
1981 	ret = v4l2_device_register_subdev_nodes(&csi->v4l2_dev);
1982 	if (ret)
1983 		goto err_unreg;
1984 
1985 	ret = media_device_register(&csi->mdev);
1986 	if (ret)
1987 		goto err_unreg;
1988 
1989 	return 0;
1990 
1991 err_unreg:
1992 	imx7_csi_video_unregister(csi);
1993 	return ret;
1994 }
1995 
1996 static void imx7_csi_unregistered(struct v4l2_subdev *sd)
1997 {
1998 	struct imx7_csi *csi = v4l2_get_subdevdata(sd);
1999 
2000 	imx7_csi_video_unregister(csi);
2001 }
2002 
2003 static const struct v4l2_subdev_video_ops imx7_csi_video_ops = {
2004 	.s_stream	= imx7_csi_s_stream,
2005 };
2006 
2007 static const struct v4l2_subdev_pad_ops imx7_csi_pad_ops = {
2008 	.init_cfg	= imx7_csi_init_cfg,
2009 	.enum_mbus_code	= imx7_csi_enum_mbus_code,
2010 	.get_fmt	= v4l2_subdev_get_fmt,
2011 	.set_fmt	= imx7_csi_set_fmt,
2012 	.link_validate	= imx7_csi_pad_link_validate,
2013 };
2014 
2015 static const struct v4l2_subdev_ops imx7_csi_subdev_ops = {
2016 	.video		= &imx7_csi_video_ops,
2017 	.pad		= &imx7_csi_pad_ops,
2018 };
2019 
2020 static const struct v4l2_subdev_internal_ops imx7_csi_internal_ops = {
2021 	.registered	= imx7_csi_registered,
2022 	.unregistered	= imx7_csi_unregistered,
2023 };
2024 
2025 /* -----------------------------------------------------------------------------
2026  * Media Entity Operations
2027  */
2028 
2029 static const struct media_entity_operations imx7_csi_entity_ops = {
2030 	.link_validate	= v4l2_subdev_link_validate,
2031 	.get_fwnode_pad = v4l2_subdev_get_fwnode_pad_1_to_1,
2032 };
2033 
2034 /* -----------------------------------------------------------------------------
2035  * Probe & Remove
2036  */
2037 
2038 static int imx7_csi_notify_bound(struct v4l2_async_notifier *notifier,
2039 				 struct v4l2_subdev *sd,
2040 				 struct v4l2_async_connection *asd)
2041 {
2042 	struct imx7_csi *csi = imx7_csi_notifier_to_dev(notifier);
2043 	struct media_pad *sink = &csi->sd.entity.pads[IMX7_CSI_PAD_SINK];
2044 
2045 	csi->src_sd = sd;
2046 
2047 	return v4l2_create_fwnode_links_to_pad(sd, sink, MEDIA_LNK_FL_ENABLED |
2048 					       MEDIA_LNK_FL_IMMUTABLE);
2049 }
2050 
2051 static int imx7_csi_notify_complete(struct v4l2_async_notifier *notifier)
2052 {
2053 	struct imx7_csi *csi = imx7_csi_notifier_to_dev(notifier);
2054 
2055 	return v4l2_device_register_subdev_nodes(&csi->v4l2_dev);
2056 }
2057 
2058 static const struct v4l2_async_notifier_operations imx7_csi_notify_ops = {
2059 	.bound = imx7_csi_notify_bound,
2060 	.complete = imx7_csi_notify_complete,
2061 };
2062 
2063 static int imx7_csi_async_register(struct imx7_csi *csi)
2064 {
2065 	struct v4l2_async_connection *asd;
2066 	struct fwnode_handle *ep;
2067 	int ret;
2068 
2069 	v4l2_async_nf_init(&csi->notifier, &csi->v4l2_dev);
2070 
2071 	ep = fwnode_graph_get_endpoint_by_id(dev_fwnode(csi->dev), 0, 0,
2072 					     FWNODE_GRAPH_ENDPOINT_NEXT);
2073 	if (!ep) {
2074 		ret = dev_err_probe(csi->dev, -ENOTCONN,
2075 				    "Failed to get remote endpoint\n");
2076 		goto error;
2077 	}
2078 
2079 	asd = v4l2_async_nf_add_fwnode_remote(&csi->notifier, ep,
2080 					      struct v4l2_async_connection);
2081 
2082 	fwnode_handle_put(ep);
2083 
2084 	if (IS_ERR(asd)) {
2085 		ret = dev_err_probe(csi->dev, PTR_ERR(asd),
2086 				    "Failed to add remote subdev to notifier\n");
2087 		goto error;
2088 	}
2089 
2090 	csi->notifier.ops = &imx7_csi_notify_ops;
2091 
2092 	ret = v4l2_async_nf_register(&csi->notifier);
2093 	if (ret)
2094 		goto error;
2095 
2096 	return 0;
2097 
2098 error:
2099 	v4l2_async_nf_cleanup(&csi->notifier);
2100 	return ret;
2101 }
2102 
2103 static void imx7_csi_media_cleanup(struct imx7_csi *csi)
2104 {
2105 	v4l2_device_unregister(&csi->v4l2_dev);
2106 	media_device_unregister(&csi->mdev);
2107 	v4l2_subdev_cleanup(&csi->sd);
2108 	media_device_cleanup(&csi->mdev);
2109 }
2110 
2111 static const struct media_device_ops imx7_csi_media_ops = {
2112 	.link_notify = v4l2_pipeline_link_notify,
2113 };
2114 
2115 static int imx7_csi_media_dev_init(struct imx7_csi *csi)
2116 {
2117 	int ret;
2118 
2119 	strscpy(csi->mdev.model, "imx-media", sizeof(csi->mdev.model));
2120 	csi->mdev.ops = &imx7_csi_media_ops;
2121 	csi->mdev.dev = csi->dev;
2122 
2123 	csi->v4l2_dev.mdev = &csi->mdev;
2124 	strscpy(csi->v4l2_dev.name, "imx-media",
2125 		sizeof(csi->v4l2_dev.name));
2126 	snprintf(csi->mdev.bus_info, sizeof(csi->mdev.bus_info),
2127 		 "platform:%s", dev_name(csi->mdev.dev));
2128 
2129 	media_device_init(&csi->mdev);
2130 
2131 	ret = v4l2_device_register(csi->dev, &csi->v4l2_dev);
2132 	if (ret < 0) {
2133 		v4l2_err(&csi->v4l2_dev,
2134 			 "Failed to register v4l2_device: %d\n", ret);
2135 		goto cleanup;
2136 	}
2137 
2138 	return 0;
2139 
2140 cleanup:
2141 	media_device_cleanup(&csi->mdev);
2142 
2143 	return ret;
2144 }
2145 
2146 static int imx7_csi_media_init(struct imx7_csi *csi)
2147 {
2148 	unsigned int i;
2149 	int ret;
2150 
2151 	/* add media device */
2152 	ret = imx7_csi_media_dev_init(csi);
2153 	if (ret)
2154 		return ret;
2155 
2156 	v4l2_subdev_init(&csi->sd, &imx7_csi_subdev_ops);
2157 	v4l2_set_subdevdata(&csi->sd, csi);
2158 	csi->sd.internal_ops = &imx7_csi_internal_ops;
2159 	csi->sd.entity.ops = &imx7_csi_entity_ops;
2160 	csi->sd.entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
2161 	csi->sd.dev = csi->dev;
2162 	csi->sd.owner = THIS_MODULE;
2163 	csi->sd.flags = V4L2_SUBDEV_FL_HAS_DEVNODE;
2164 	snprintf(csi->sd.name, sizeof(csi->sd.name), "csi");
2165 
2166 	for (i = 0; i < IMX7_CSI_PADS_NUM; i++)
2167 		csi->pad[i].flags = (i == IMX7_CSI_PAD_SINK) ?
2168 			MEDIA_PAD_FL_SINK : MEDIA_PAD_FL_SOURCE;
2169 
2170 	ret = media_entity_pads_init(&csi->sd.entity, IMX7_CSI_PADS_NUM,
2171 				     csi->pad);
2172 	if (ret)
2173 		goto error;
2174 
2175 	ret = v4l2_subdev_init_finalize(&csi->sd);
2176 	if (ret)
2177 		goto error;
2178 
2179 	ret = v4l2_device_register_subdev(&csi->v4l2_dev, &csi->sd);
2180 	if (ret)
2181 		goto error;
2182 
2183 	return 0;
2184 
2185 error:
2186 	imx7_csi_media_cleanup(csi);
2187 	return ret;
2188 }
2189 
2190 static int imx7_csi_probe(struct platform_device *pdev)
2191 {
2192 	struct device *dev = &pdev->dev;
2193 	struct imx7_csi *csi;
2194 	int ret;
2195 
2196 	csi = devm_kzalloc(&pdev->dev, sizeof(*csi), GFP_KERNEL);
2197 	if (!csi)
2198 		return -ENOMEM;
2199 
2200 	csi->dev = dev;
2201 	platform_set_drvdata(pdev, csi);
2202 
2203 	spin_lock_init(&csi->irqlock);
2204 
2205 	/* Acquire resources and install interrupt handler. */
2206 	csi->mclk = devm_clk_get(&pdev->dev, "mclk");
2207 	if (IS_ERR(csi->mclk)) {
2208 		ret = PTR_ERR(csi->mclk);
2209 		dev_err(dev, "Failed to get mclk: %d", ret);
2210 		return ret;
2211 	}
2212 
2213 	csi->irq = platform_get_irq(pdev, 0);
2214 	if (csi->irq < 0)
2215 		return csi->irq;
2216 
2217 	csi->regbase = devm_platform_ioremap_resource(pdev, 0);
2218 	if (IS_ERR(csi->regbase))
2219 		return PTR_ERR(csi->regbase);
2220 
2221 	csi->model = (enum imx_csi_model)(uintptr_t)of_device_get_match_data(&pdev->dev);
2222 
2223 	ret = devm_request_irq(dev, csi->irq, imx7_csi_irq_handler, 0, "csi",
2224 			       (void *)csi);
2225 	if (ret < 0) {
2226 		dev_err(dev, "Request CSI IRQ failed.\n");
2227 		return ret;
2228 	}
2229 
2230 	/* Initialize all the media device infrastructure. */
2231 	ret = imx7_csi_media_init(csi);
2232 	if (ret)
2233 		return ret;
2234 
2235 	ret = imx7_csi_async_register(csi);
2236 	if (ret)
2237 		goto err_media_cleanup;
2238 
2239 	return 0;
2240 
2241 err_media_cleanup:
2242 	imx7_csi_media_cleanup(csi);
2243 
2244 	return ret;
2245 }
2246 
2247 static void imx7_csi_remove(struct platform_device *pdev)
2248 {
2249 	struct imx7_csi *csi = platform_get_drvdata(pdev);
2250 
2251 	imx7_csi_media_cleanup(csi);
2252 
2253 	v4l2_async_nf_unregister(&csi->notifier);
2254 	v4l2_async_nf_cleanup(&csi->notifier);
2255 	v4l2_async_unregister_subdev(&csi->sd);
2256 }
2257 
2258 static const struct of_device_id imx7_csi_of_match[] = {
2259 	{ .compatible = "fsl,imx8mq-csi", .data = (void *)IMX7_CSI_IMX8MQ },
2260 	{ .compatible = "fsl,imx7-csi", .data = (void *)IMX7_CSI_IMX7 },
2261 	{ .compatible = "fsl,imx6ul-csi", .data = (void *)IMX7_CSI_IMX7 },
2262 	{ },
2263 };
2264 MODULE_DEVICE_TABLE(of, imx7_csi_of_match);
2265 
2266 static struct platform_driver imx7_csi_driver = {
2267 	.probe = imx7_csi_probe,
2268 	.remove_new = imx7_csi_remove,
2269 	.driver = {
2270 		.of_match_table = imx7_csi_of_match,
2271 		.name = "imx7-csi",
2272 	},
2273 };
2274 module_platform_driver(imx7_csi_driver);
2275 
2276 MODULE_DESCRIPTION("i.MX7 CSI subdev driver");
2277 MODULE_AUTHOR("Rui Miguel Silva <rui.silva@linaro.org>");
2278 MODULE_LICENSE("GPL v2");
2279 MODULE_ALIAS("platform:imx7-csi");
2280