1 // SPDX-License-Identifier: GPL-2.0
2 /*
3  * Copyright (C) 2017,2020 Intel Corporation
4  *
5  * Based partially on Intel IPU4 driver written by
6  *  Sakari Ailus <sakari.ailus@linux.intel.com>
7  *  Samu Onkalo <samu.onkalo@intel.com>
8  *  Jouni Högander <jouni.hogander@intel.com>
9  *  Jouni Ukkonen <jouni.ukkonen@intel.com>
10  *  Antti Laakso <antti.laakso@intel.com>
11  * et al.
12  */
13 
14 #include <linux/bitops.h>
15 #include <linux/delay.h>
16 #include <linux/interrupt.h>
17 #include <linux/iopoll.h>
18 #include <linux/mm.h>
19 #include <linux/module.h>
20 #include <linux/pci.h>
21 #include <linux/pfn.h>
22 #include <linux/pm_runtime.h>
23 #include <linux/property.h>
24 #include <linux/vmalloc.h>
25 
26 #include <media/ipu-bridge.h>
27 #include <media/v4l2-ctrls.h>
28 #include <media/v4l2-device.h>
29 #include <media/v4l2-event.h>
30 #include <media/v4l2-fwnode.h>
31 #include <media/v4l2-ioctl.h>
32 #include <media/videobuf2-dma-sg.h>
33 
34 #include "ipu3-cio2.h"
35 
36 struct ipu3_cio2_fmt {
37 	u32 mbus_code;
38 	u32 fourcc;
39 	u8 mipicode;
40 	u8 bpp;
41 };
42 
43 /*
44  * These are raw formats used in Intel's third generation of
45  * Image Processing Unit known as IPU3.
46  * 10bit raw bayer packed, 32 bytes for every 25 pixels,
47  * last LSB 6 bits unused.
48  */
49 static const struct ipu3_cio2_fmt formats[] = {
50 	{	/* put default entry at beginning */
51 		.mbus_code	= MEDIA_BUS_FMT_SGRBG10_1X10,
52 		.fourcc		= V4L2_PIX_FMT_IPU3_SGRBG10,
53 		.mipicode	= 0x2b,
54 		.bpp		= 10,
55 	}, {
56 		.mbus_code	= MEDIA_BUS_FMT_SGBRG10_1X10,
57 		.fourcc		= V4L2_PIX_FMT_IPU3_SGBRG10,
58 		.mipicode	= 0x2b,
59 		.bpp		= 10,
60 	}, {
61 		.mbus_code	= MEDIA_BUS_FMT_SBGGR10_1X10,
62 		.fourcc		= V4L2_PIX_FMT_IPU3_SBGGR10,
63 		.mipicode	= 0x2b,
64 		.bpp		= 10,
65 	}, {
66 		.mbus_code	= MEDIA_BUS_FMT_SRGGB10_1X10,
67 		.fourcc		= V4L2_PIX_FMT_IPU3_SRGGB10,
68 		.mipicode	= 0x2b,
69 		.bpp		= 10,
70 	}, {
71 		.mbus_code	= MEDIA_BUS_FMT_Y10_1X10,
72 		.fourcc		= V4L2_PIX_FMT_IPU3_Y10,
73 		.mipicode	= 0x2b,
74 		.bpp		= 10,
75 	},
76 };
77 
78 /*
79  * cio2_find_format - lookup color format by fourcc or/and media bus code
80  * @pixelformat: fourcc to match, ignored if null
81  * @mbus_code: media bus code to match, ignored if null
82  */
83 static const struct ipu3_cio2_fmt *cio2_find_format(const u32 *pixelformat,
84 						    const u32 *mbus_code)
85 {
86 	unsigned int i;
87 
88 	for (i = 0; i < ARRAY_SIZE(formats); i++) {
89 		if (pixelformat && *pixelformat != formats[i].fourcc)
90 			continue;
91 		if (mbus_code && *mbus_code != formats[i].mbus_code)
92 			continue;
93 
94 		return &formats[i];
95 	}
96 
97 	return NULL;
98 }
99 
100 static inline u32 cio2_bytesperline(const unsigned int width)
101 {
102 	/*
103 	 * 64 bytes for every 50 pixels, the line length
104 	 * in bytes is multiple of 64 (line end alignment).
105 	 */
106 	return DIV_ROUND_UP(width, 50) * 64;
107 }
108 
109 /**************** FBPT operations ****************/
110 
111 static void cio2_fbpt_exit_dummy(struct cio2_device *cio2)
112 {
113 	struct device *dev = &cio2->pci_dev->dev;
114 
115 	if (cio2->dummy_lop) {
116 		dma_free_coherent(dev, PAGE_SIZE, cio2->dummy_lop,
117 				  cio2->dummy_lop_bus_addr);
118 		cio2->dummy_lop = NULL;
119 	}
120 	if (cio2->dummy_page) {
121 		dma_free_coherent(dev, PAGE_SIZE, cio2->dummy_page,
122 				  cio2->dummy_page_bus_addr);
123 		cio2->dummy_page = NULL;
124 	}
125 }
126 
127 static int cio2_fbpt_init_dummy(struct cio2_device *cio2)
128 {
129 	struct device *dev = &cio2->pci_dev->dev;
130 	unsigned int i;
131 
132 	cio2->dummy_page = dma_alloc_coherent(dev, PAGE_SIZE,
133 					      &cio2->dummy_page_bus_addr,
134 					      GFP_KERNEL);
135 	cio2->dummy_lop = dma_alloc_coherent(dev, PAGE_SIZE,
136 					     &cio2->dummy_lop_bus_addr,
137 					     GFP_KERNEL);
138 	if (!cio2->dummy_page || !cio2->dummy_lop) {
139 		cio2_fbpt_exit_dummy(cio2);
140 		return -ENOMEM;
141 	}
142 	/*
143 	 * List of Pointers(LOP) contains 1024x32b pointers to 4KB page each
144 	 * Initialize each entry to dummy_page bus base address.
145 	 */
146 	for (i = 0; i < CIO2_LOP_ENTRIES; i++)
147 		cio2->dummy_lop[i] = PFN_DOWN(cio2->dummy_page_bus_addr);
148 
149 	return 0;
150 }
151 
152 static void cio2_fbpt_entry_enable(struct cio2_device *cio2,
153 				   struct cio2_fbpt_entry entry[CIO2_MAX_LOPS])
154 {
155 	/*
156 	 * The CPU first initializes some fields in fbpt, then sets
157 	 * the VALID bit, this barrier is to ensure that the DMA(device)
158 	 * does not see the VALID bit enabled before other fields are
159 	 * initialized; otherwise it could lead to havoc.
160 	 */
161 	dma_wmb();
162 
163 	/*
164 	 * Request interrupts for start and completion
165 	 * Valid bit is applicable only to 1st entry
166 	 */
167 	entry[0].first_entry.ctrl = CIO2_FBPT_CTRL_VALID |
168 		CIO2_FBPT_CTRL_IOC | CIO2_FBPT_CTRL_IOS;
169 }
170 
171 /* Initialize fpbt entries to point to dummy frame */
172 static void cio2_fbpt_entry_init_dummy(struct cio2_device *cio2,
173 				       struct cio2_fbpt_entry
174 				       entry[CIO2_MAX_LOPS])
175 {
176 	unsigned int i;
177 
178 	entry[0].first_entry.first_page_offset = 0;
179 	entry[1].second_entry.num_of_pages = CIO2_LOP_ENTRIES * CIO2_MAX_LOPS;
180 	entry[1].second_entry.last_page_available_bytes = PAGE_SIZE - 1;
181 
182 	for (i = 0; i < CIO2_MAX_LOPS; i++)
183 		entry[i].lop_page_addr = PFN_DOWN(cio2->dummy_lop_bus_addr);
184 
185 	cio2_fbpt_entry_enable(cio2, entry);
186 }
187 
188 /* Initialize fpbt entries to point to a given buffer */
189 static void cio2_fbpt_entry_init_buf(struct cio2_device *cio2,
190 				     struct cio2_buffer *b,
191 				     struct cio2_fbpt_entry
192 				     entry[CIO2_MAX_LOPS])
193 {
194 	struct vb2_buffer *vb = &b->vbb.vb2_buf;
195 	unsigned int length = vb->planes[0].length;
196 	int remaining, i;
197 
198 	entry[0].first_entry.first_page_offset = b->offset;
199 	remaining = length + entry[0].first_entry.first_page_offset;
200 	entry[1].second_entry.num_of_pages = PFN_UP(remaining);
201 	/*
202 	 * last_page_available_bytes has the offset of the last byte in the
203 	 * last page which is still accessible by DMA. DMA cannot access
204 	 * beyond this point. Valid range for this is from 0 to 4095.
205 	 * 0 indicates 1st byte in the page is DMA accessible.
206 	 * 4095 (PAGE_SIZE - 1) means every single byte in the last page
207 	 * is available for DMA transfer.
208 	 */
209 	remaining = offset_in_page(remaining) ?: PAGE_SIZE;
210 	entry[1].second_entry.last_page_available_bytes = remaining - 1;
211 	/* Fill FBPT */
212 	remaining = length;
213 	i = 0;
214 	while (remaining > 0) {
215 		entry->lop_page_addr = PFN_DOWN(b->lop_bus_addr[i]);
216 		remaining -= CIO2_LOP_ENTRIES * PAGE_SIZE;
217 		entry++;
218 		i++;
219 	}
220 
221 	/*
222 	 * The first not meaningful FBPT entry should point to a valid LOP
223 	 */
224 	entry->lop_page_addr = PFN_DOWN(cio2->dummy_lop_bus_addr);
225 
226 	cio2_fbpt_entry_enable(cio2, entry);
227 }
228 
229 static int cio2_fbpt_init(struct cio2_device *cio2, struct cio2_queue *q)
230 {
231 	struct device *dev = &cio2->pci_dev->dev;
232 
233 	q->fbpt = dma_alloc_coherent(dev, CIO2_FBPT_SIZE, &q->fbpt_bus_addr,
234 				     GFP_KERNEL);
235 	if (!q->fbpt)
236 		return -ENOMEM;
237 
238 	return 0;
239 }
240 
241 static void cio2_fbpt_exit(struct cio2_queue *q, struct device *dev)
242 {
243 	dma_free_coherent(dev, CIO2_FBPT_SIZE, q->fbpt, q->fbpt_bus_addr);
244 }
245 
246 /**************** CSI2 hardware setup ****************/
247 
248 /*
249  * The CSI2 receiver has several parameters affecting
250  * the receiver timings. These depend on the MIPI bus frequency
251  * F in Hz (sensor transmitter rate) as follows:
252  *     register value = (A/1e9 + B * UI) / COUNT_ACC
253  * where
254  *      UI = 1 / (2 * F) in seconds
255  *      COUNT_ACC = counter accuracy in seconds
256  *      For IPU3 COUNT_ACC = 0.0625
257  *
258  * A and B are coefficients from the table below,
259  * depending whether the register minimum or maximum value is
260  * calculated.
261  *                                     Minimum     Maximum
262  * Clock lane                          A     B     A     B
263  * reg_rx_csi_dly_cnt_termen_clane     0     0    38     0
264  * reg_rx_csi_dly_cnt_settle_clane    95    -8   300   -16
265  * Data lanes
266  * reg_rx_csi_dly_cnt_termen_dlane0    0     0    35     4
267  * reg_rx_csi_dly_cnt_settle_dlane0   85    -2   145    -6
268  * reg_rx_csi_dly_cnt_termen_dlane1    0     0    35     4
269  * reg_rx_csi_dly_cnt_settle_dlane1   85    -2   145    -6
270  * reg_rx_csi_dly_cnt_termen_dlane2    0     0    35     4
271  * reg_rx_csi_dly_cnt_settle_dlane2   85    -2   145    -6
272  * reg_rx_csi_dly_cnt_termen_dlane3    0     0    35     4
273  * reg_rx_csi_dly_cnt_settle_dlane3   85    -2   145    -6
274  *
275  * We use the minimum values of both A and B.
276  */
277 
278 /*
279  * shift for keeping value range suitable for 32-bit integer arithmetic
280  */
281 #define LIMIT_SHIFT	8
282 
283 static s32 cio2_rx_timing(s32 a, s32 b, s64 freq, int def)
284 {
285 	const u32 accinv = 16; /* invert of counter resolution */
286 	const u32 uiinv = 500000000; /* 1e9 / 2 */
287 	s32 r;
288 
289 	freq >>= LIMIT_SHIFT;
290 
291 	if (WARN_ON(freq <= 0 || freq > S32_MAX))
292 		return def;
293 	/*
294 	 * b could be 0, -2 or -8, so |accinv * b| is always
295 	 * less than (1 << ds) and thus |r| < 500000000.
296 	 */
297 	r = accinv * b * (uiinv >> LIMIT_SHIFT);
298 	r = r / (s32)freq;
299 	/* max value of a is 95 */
300 	r += accinv * a;
301 
302 	return r;
303 };
304 
305 /* Calculate the delay value for termination enable of clock lane HS Rx */
306 static int cio2_csi2_calc_timing(struct cio2_device *cio2, struct cio2_queue *q,
307 				 struct cio2_csi2_timing *timing,
308 				 unsigned int bpp, unsigned int lanes)
309 {
310 	struct device *dev = &cio2->pci_dev->dev;
311 	s64 freq;
312 
313 	if (!q->sensor)
314 		return -ENODEV;
315 
316 	freq = v4l2_get_link_freq(q->sensor->ctrl_handler, bpp, lanes * 2);
317 	if (freq < 0) {
318 		dev_err(dev, "error %lld, invalid link_freq\n", freq);
319 		return freq;
320 	}
321 
322 	timing->clk_termen = cio2_rx_timing(CIO2_CSIRX_DLY_CNT_TERMEN_CLANE_A,
323 					    CIO2_CSIRX_DLY_CNT_TERMEN_CLANE_B,
324 					    freq,
325 					    CIO2_CSIRX_DLY_CNT_TERMEN_DEFAULT);
326 	timing->clk_settle = cio2_rx_timing(CIO2_CSIRX_DLY_CNT_SETTLE_CLANE_A,
327 					    CIO2_CSIRX_DLY_CNT_SETTLE_CLANE_B,
328 					    freq,
329 					    CIO2_CSIRX_DLY_CNT_SETTLE_DEFAULT);
330 	timing->dat_termen = cio2_rx_timing(CIO2_CSIRX_DLY_CNT_TERMEN_DLANE_A,
331 					    CIO2_CSIRX_DLY_CNT_TERMEN_DLANE_B,
332 					    freq,
333 					    CIO2_CSIRX_DLY_CNT_TERMEN_DEFAULT);
334 	timing->dat_settle = cio2_rx_timing(CIO2_CSIRX_DLY_CNT_SETTLE_DLANE_A,
335 					    CIO2_CSIRX_DLY_CNT_SETTLE_DLANE_B,
336 					    freq,
337 					    CIO2_CSIRX_DLY_CNT_SETTLE_DEFAULT);
338 
339 	dev_dbg(dev, "freq ct value is %d\n", timing->clk_termen);
340 	dev_dbg(dev, "freq cs value is %d\n", timing->clk_settle);
341 	dev_dbg(dev, "freq dt value is %d\n", timing->dat_termen);
342 	dev_dbg(dev, "freq ds value is %d\n", timing->dat_settle);
343 
344 	return 0;
345 };
346 
347 static int cio2_hw_init(struct cio2_device *cio2, struct cio2_queue *q)
348 {
349 	static const int NUM_VCS = 4;
350 	static const int SID;	/* Stream id */
351 	static const int ENTRY;
352 	static const int FBPT_WIDTH = DIV_ROUND_UP(CIO2_MAX_LOPS,
353 					CIO2_FBPT_SUBENTRY_UNIT);
354 	const u32 num_buffers1 = CIO2_MAX_BUFFERS - 1;
355 	const struct ipu3_cio2_fmt *fmt;
356 	void __iomem *const base = cio2->base;
357 	u8 lanes, csi2bus = q->csi2.port;
358 	u8 sensor_vc = SENSOR_VIR_CH_DFLT;
359 	struct cio2_csi2_timing timing = { 0 };
360 	int i, r;
361 
362 	fmt = cio2_find_format(NULL, &q->subdev_fmt.code);
363 	if (!fmt)
364 		return -EINVAL;
365 
366 	lanes = q->csi2.lanes;
367 
368 	r = cio2_csi2_calc_timing(cio2, q, &timing, fmt->bpp, lanes);
369 	if (r)
370 		return r;
371 
372 	writel(timing.clk_termen, q->csi_rx_base +
373 		CIO2_REG_CSIRX_DLY_CNT_TERMEN(CIO2_CSIRX_DLY_CNT_CLANE_IDX));
374 	writel(timing.clk_settle, q->csi_rx_base +
375 		CIO2_REG_CSIRX_DLY_CNT_SETTLE(CIO2_CSIRX_DLY_CNT_CLANE_IDX));
376 
377 	for (i = 0; i < lanes; i++) {
378 		writel(timing.dat_termen, q->csi_rx_base +
379 			CIO2_REG_CSIRX_DLY_CNT_TERMEN(i));
380 		writel(timing.dat_settle, q->csi_rx_base +
381 			CIO2_REG_CSIRX_DLY_CNT_SETTLE(i));
382 	}
383 
384 	writel(CIO2_PBM_WMCTRL1_MIN_2CK |
385 	       CIO2_PBM_WMCTRL1_MID1_2CK |
386 	       CIO2_PBM_WMCTRL1_MID2_2CK, base + CIO2_REG_PBM_WMCTRL1);
387 	writel(CIO2_PBM_WMCTRL2_HWM_2CK << CIO2_PBM_WMCTRL2_HWM_2CK_SHIFT |
388 	       CIO2_PBM_WMCTRL2_LWM_2CK << CIO2_PBM_WMCTRL2_LWM_2CK_SHIFT |
389 	       CIO2_PBM_WMCTRL2_OBFFWM_2CK <<
390 	       CIO2_PBM_WMCTRL2_OBFFWM_2CK_SHIFT |
391 	       CIO2_PBM_WMCTRL2_TRANSDYN << CIO2_PBM_WMCTRL2_TRANSDYN_SHIFT |
392 	       CIO2_PBM_WMCTRL2_OBFF_MEM_EN, base + CIO2_REG_PBM_WMCTRL2);
393 	writel(CIO2_PBM_ARB_CTRL_LANES_DIV <<
394 	       CIO2_PBM_ARB_CTRL_LANES_DIV_SHIFT |
395 	       CIO2_PBM_ARB_CTRL_LE_EN |
396 	       CIO2_PBM_ARB_CTRL_PLL_POST_SHTDN <<
397 	       CIO2_PBM_ARB_CTRL_PLL_POST_SHTDN_SHIFT |
398 	       CIO2_PBM_ARB_CTRL_PLL_AHD_WK_UP <<
399 	       CIO2_PBM_ARB_CTRL_PLL_AHD_WK_UP_SHIFT,
400 	       base + CIO2_REG_PBM_ARB_CTRL);
401 	writel(CIO2_CSIRX_STATUS_DLANE_HS_MASK,
402 	       q->csi_rx_base + CIO2_REG_CSIRX_STATUS_DLANE_HS);
403 	writel(CIO2_CSIRX_STATUS_DLANE_LP_MASK,
404 	       q->csi_rx_base + CIO2_REG_CSIRX_STATUS_DLANE_LP);
405 
406 	writel(CIO2_FB_HPLL_FREQ, base + CIO2_REG_FB_HPLL_FREQ);
407 	writel(CIO2_ISCLK_RATIO, base + CIO2_REG_ISCLK_RATIO);
408 
409 	/* Configure MIPI backend */
410 	for (i = 0; i < NUM_VCS; i++)
411 		writel(1, q->csi_rx_base + CIO2_REG_MIPIBE_SP_LUT_ENTRY(i));
412 
413 	/* There are 16 short packet LUT entry */
414 	for (i = 0; i < 16; i++)
415 		writel(CIO2_MIPIBE_LP_LUT_ENTRY_DISREGARD,
416 		       q->csi_rx_base + CIO2_REG_MIPIBE_LP_LUT_ENTRY(i));
417 	writel(CIO2_MIPIBE_GLOBAL_LUT_DISREGARD,
418 	       q->csi_rx_base + CIO2_REG_MIPIBE_GLOBAL_LUT_DISREGARD);
419 
420 	writel(CIO2_INT_EN_EXT_IE_MASK, base + CIO2_REG_INT_EN_EXT_IE);
421 	writel(CIO2_IRQCTRL_MASK, q->csi_rx_base + CIO2_REG_IRQCTRL_MASK);
422 	writel(CIO2_IRQCTRL_MASK, q->csi_rx_base + CIO2_REG_IRQCTRL_ENABLE);
423 	writel(0, q->csi_rx_base + CIO2_REG_IRQCTRL_EDGE);
424 	writel(0, q->csi_rx_base + CIO2_REG_IRQCTRL_LEVEL_NOT_PULSE);
425 	writel(CIO2_INT_EN_EXT_OE_MASK, base + CIO2_REG_INT_EN_EXT_OE);
426 
427 	writel(CIO2_REG_INT_EN_IRQ | CIO2_INT_IOC(CIO2_DMA_CHAN) |
428 	       CIO2_REG_INT_EN_IOS(CIO2_DMA_CHAN),
429 	       base + CIO2_REG_INT_EN);
430 
431 	writel((CIO2_PXM_PXF_FMT_CFG_BPP_10 | CIO2_PXM_PXF_FMT_CFG_PCK_64B)
432 	       << CIO2_PXM_PXF_FMT_CFG_SID0_SHIFT,
433 	       base + CIO2_REG_PXM_PXF_FMT_CFG0(csi2bus));
434 	writel(SID << CIO2_MIPIBE_LP_LUT_ENTRY_SID_SHIFT |
435 	       sensor_vc << CIO2_MIPIBE_LP_LUT_ENTRY_VC_SHIFT |
436 	       fmt->mipicode << CIO2_MIPIBE_LP_LUT_ENTRY_FORMAT_TYPE_SHIFT,
437 	       q->csi_rx_base + CIO2_REG_MIPIBE_LP_LUT_ENTRY(ENTRY));
438 	writel(0, q->csi_rx_base + CIO2_REG_MIPIBE_COMP_FORMAT(sensor_vc));
439 	writel(0, q->csi_rx_base + CIO2_REG_MIPIBE_FORCE_RAW8);
440 	writel(0, base + CIO2_REG_PXM_SID2BID0(csi2bus));
441 
442 	writel(lanes, q->csi_rx_base + CIO2_REG_CSIRX_NOF_ENABLED_LANES);
443 	writel(CIO2_CGC_PRIM_TGE |
444 	       CIO2_CGC_SIDE_TGE |
445 	       CIO2_CGC_XOSC_TGE |
446 	       CIO2_CGC_D3I3_TGE |
447 	       CIO2_CGC_CSI2_INTERFRAME_TGE |
448 	       CIO2_CGC_CSI2_PORT_DCGE |
449 	       CIO2_CGC_SIDE_DCGE |
450 	       CIO2_CGC_PRIM_DCGE |
451 	       CIO2_CGC_ROSC_DCGE |
452 	       CIO2_CGC_XOSC_DCGE |
453 	       CIO2_CGC_CLKGATE_HOLDOFF << CIO2_CGC_CLKGATE_HOLDOFF_SHIFT |
454 	       CIO2_CGC_CSI_CLKGATE_HOLDOFF
455 	       << CIO2_CGC_CSI_CLKGATE_HOLDOFF_SHIFT, base + CIO2_REG_CGC);
456 	writel(CIO2_LTRCTRL_LTRDYNEN, base + CIO2_REG_LTRCTRL);
457 	writel(CIO2_LTRVAL0_VAL << CIO2_LTRVAL02_VAL_SHIFT |
458 	       CIO2_LTRVAL0_SCALE << CIO2_LTRVAL02_SCALE_SHIFT |
459 	       CIO2_LTRVAL1_VAL << CIO2_LTRVAL13_VAL_SHIFT |
460 	       CIO2_LTRVAL1_SCALE << CIO2_LTRVAL13_SCALE_SHIFT,
461 	       base + CIO2_REG_LTRVAL01);
462 	writel(CIO2_LTRVAL2_VAL << CIO2_LTRVAL02_VAL_SHIFT |
463 	       CIO2_LTRVAL2_SCALE << CIO2_LTRVAL02_SCALE_SHIFT |
464 	       CIO2_LTRVAL3_VAL << CIO2_LTRVAL13_VAL_SHIFT |
465 	       CIO2_LTRVAL3_SCALE << CIO2_LTRVAL13_SCALE_SHIFT,
466 	       base + CIO2_REG_LTRVAL23);
467 
468 	for (i = 0; i < CIO2_NUM_DMA_CHAN; i++) {
469 		writel(0, base + CIO2_REG_CDMABA(i));
470 		writel(0, base + CIO2_REG_CDMAC0(i));
471 		writel(0, base + CIO2_REG_CDMAC1(i));
472 	}
473 
474 	/* Enable DMA */
475 	writel(PFN_DOWN(q->fbpt_bus_addr), base + CIO2_REG_CDMABA(CIO2_DMA_CHAN));
476 
477 	writel(num_buffers1 << CIO2_CDMAC0_FBPT_LEN_SHIFT |
478 	       FBPT_WIDTH << CIO2_CDMAC0_FBPT_WIDTH_SHIFT |
479 	       CIO2_CDMAC0_DMA_INTR_ON_FE |
480 	       CIO2_CDMAC0_FBPT_UPDATE_FIFO_FULL |
481 	       CIO2_CDMAC0_DMA_EN |
482 	       CIO2_CDMAC0_DMA_INTR_ON_FS |
483 	       CIO2_CDMAC0_DMA_HALTED, base + CIO2_REG_CDMAC0(CIO2_DMA_CHAN));
484 
485 	writel(1 << CIO2_CDMAC1_LINENUMUPDATE_SHIFT,
486 	       base + CIO2_REG_CDMAC1(CIO2_DMA_CHAN));
487 
488 	writel(0, base + CIO2_REG_PBM_FOPN_ABORT);
489 
490 	writel(CIO2_PXM_FRF_CFG_CRC_TH << CIO2_PXM_FRF_CFG_CRC_TH_SHIFT |
491 	       CIO2_PXM_FRF_CFG_MSK_ECC_DPHY_NR |
492 	       CIO2_PXM_FRF_CFG_MSK_ECC_RE |
493 	       CIO2_PXM_FRF_CFG_MSK_ECC_DPHY_NE,
494 	       base + CIO2_REG_PXM_FRF_CFG(q->csi2.port));
495 
496 	/* Clear interrupts */
497 	writel(CIO2_IRQCTRL_MASK, q->csi_rx_base + CIO2_REG_IRQCTRL_CLEAR);
498 	writel(~0, base + CIO2_REG_INT_STS_EXT_OE);
499 	writel(~0, base + CIO2_REG_INT_STS_EXT_IE);
500 	writel(~0, base + CIO2_REG_INT_STS);
501 
502 	/* Enable devices, starting from the last device in the pipe */
503 	writel(1, q->csi_rx_base + CIO2_REG_MIPIBE_ENABLE);
504 	writel(1, q->csi_rx_base + CIO2_REG_CSIRX_ENABLE);
505 
506 	return 0;
507 }
508 
509 static void cio2_hw_exit(struct cio2_device *cio2, struct cio2_queue *q)
510 {
511 	struct device *dev = &cio2->pci_dev->dev;
512 	void __iomem *const base = cio2->base;
513 	unsigned int i;
514 	u32 value;
515 	int ret;
516 
517 	/* Disable CSI receiver and MIPI backend devices */
518 	writel(0, q->csi_rx_base + CIO2_REG_IRQCTRL_MASK);
519 	writel(0, q->csi_rx_base + CIO2_REG_IRQCTRL_ENABLE);
520 	writel(0, q->csi_rx_base + CIO2_REG_CSIRX_ENABLE);
521 	writel(0, q->csi_rx_base + CIO2_REG_MIPIBE_ENABLE);
522 
523 	/* Halt DMA */
524 	writel(0, base + CIO2_REG_CDMAC0(CIO2_DMA_CHAN));
525 	ret = readl_poll_timeout(base + CIO2_REG_CDMAC0(CIO2_DMA_CHAN),
526 				 value, value & CIO2_CDMAC0_DMA_HALTED,
527 				 4000, 2000000);
528 	if (ret)
529 		dev_err(dev, "DMA %i can not be halted\n", CIO2_DMA_CHAN);
530 
531 	for (i = 0; i < CIO2_NUM_PORTS; i++) {
532 		writel(readl(base + CIO2_REG_PXM_FRF_CFG(i)) |
533 		       CIO2_PXM_FRF_CFG_ABORT, base + CIO2_REG_PXM_FRF_CFG(i));
534 		writel(readl(base + CIO2_REG_PBM_FOPN_ABORT) |
535 		       CIO2_PBM_FOPN_ABORT(i), base + CIO2_REG_PBM_FOPN_ABORT);
536 	}
537 }
538 
539 static void cio2_buffer_done(struct cio2_device *cio2, unsigned int dma_chan)
540 {
541 	struct device *dev = &cio2->pci_dev->dev;
542 	struct cio2_queue *q = cio2->cur_queue;
543 	struct cio2_fbpt_entry *entry;
544 	u64 ns = ktime_get_ns();
545 
546 	if (dma_chan >= CIO2_QUEUES) {
547 		dev_err(dev, "bad DMA channel %i\n", dma_chan);
548 		return;
549 	}
550 
551 	entry = &q->fbpt[q->bufs_first * CIO2_MAX_LOPS];
552 	if (entry->first_entry.ctrl & CIO2_FBPT_CTRL_VALID) {
553 		dev_warn(dev, "no ready buffers found on DMA channel %u\n",
554 			 dma_chan);
555 		return;
556 	}
557 
558 	/* Find out which buffer(s) are ready */
559 	do {
560 		struct cio2_buffer *b;
561 
562 		b = q->bufs[q->bufs_first];
563 		if (b) {
564 			unsigned int received = entry[1].second_entry.num_of_bytes;
565 			unsigned long payload =
566 				vb2_get_plane_payload(&b->vbb.vb2_buf, 0);
567 
568 			q->bufs[q->bufs_first] = NULL;
569 			atomic_dec(&q->bufs_queued);
570 			dev_dbg(dev, "buffer %i done\n", b->vbb.vb2_buf.index);
571 
572 			b->vbb.vb2_buf.timestamp = ns;
573 			b->vbb.field = V4L2_FIELD_NONE;
574 			b->vbb.sequence = atomic_read(&q->frame_sequence);
575 			if (payload != received)
576 				dev_warn(dev,
577 					 "payload length is %lu, received %u\n",
578 					 payload, received);
579 			vb2_buffer_done(&b->vbb.vb2_buf, VB2_BUF_STATE_DONE);
580 		}
581 		atomic_inc(&q->frame_sequence);
582 		cio2_fbpt_entry_init_dummy(cio2, entry);
583 		q->bufs_first = (q->bufs_first + 1) % CIO2_MAX_BUFFERS;
584 		entry = &q->fbpt[q->bufs_first * CIO2_MAX_LOPS];
585 	} while (!(entry->first_entry.ctrl & CIO2_FBPT_CTRL_VALID));
586 }
587 
588 static void cio2_queue_event_sof(struct cio2_device *cio2, struct cio2_queue *q)
589 {
590 	/*
591 	 * For the user space camera control algorithms it is essential
592 	 * to know when the reception of a frame has begun. That's often
593 	 * the best timing information to get from the hardware.
594 	 */
595 	struct v4l2_event event = {
596 		.type = V4L2_EVENT_FRAME_SYNC,
597 		.u.frame_sync.frame_sequence = atomic_read(&q->frame_sequence),
598 	};
599 
600 	v4l2_event_queue(q->subdev.devnode, &event);
601 }
602 
603 static const char *const cio2_irq_errs[] = {
604 	"single packet header error corrected",
605 	"multiple packet header errors detected",
606 	"payload checksum (CRC) error",
607 	"fifo overflow",
608 	"reserved short packet data type detected",
609 	"reserved long packet data type detected",
610 	"incomplete long packet detected",
611 	"frame sync error",
612 	"line sync error",
613 	"DPHY start of transmission error",
614 	"DPHY synchronization error",
615 	"escape mode error",
616 	"escape mode trigger event",
617 	"escape mode ultra-low power state for data lane(s)",
618 	"escape mode ultra-low power state exit for clock lane",
619 	"inter-frame short packet discarded",
620 	"inter-frame long packet discarded",
621 	"non-matching Long Packet stalled",
622 };
623 
624 static void cio2_irq_log_irq_errs(struct device *dev, u8 port, u32 status)
625 {
626 	unsigned long csi2_status = status;
627 	unsigned int i;
628 
629 	for_each_set_bit(i, &csi2_status, ARRAY_SIZE(cio2_irq_errs))
630 		dev_err(dev, "CSI-2 receiver port %i: %s\n",
631 			port, cio2_irq_errs[i]);
632 
633 	if (fls_long(csi2_status) >= ARRAY_SIZE(cio2_irq_errs))
634 		dev_warn(dev, "unknown CSI2 error 0x%lx on port %i\n",
635 			 csi2_status, port);
636 }
637 
638 static const char *const cio2_port_errs[] = {
639 	"ECC recoverable",
640 	"DPHY not recoverable",
641 	"ECC not recoverable",
642 	"CRC error",
643 	"INTERFRAMEDATA",
644 	"PKT2SHORT",
645 	"PKT2LONG",
646 };
647 
648 static void cio2_irq_log_port_errs(struct device *dev, u8 port, u32 status)
649 {
650 	unsigned long port_status = status;
651 	unsigned int i;
652 
653 	for_each_set_bit(i, &port_status, ARRAY_SIZE(cio2_port_errs))
654 		dev_err(dev, "port %i error %s\n", port, cio2_port_errs[i]);
655 }
656 
657 static void cio2_irq_handle_once(struct cio2_device *cio2, u32 int_status)
658 {
659 	struct device *dev = &cio2->pci_dev->dev;
660 	void __iomem *const base = cio2->base;
661 
662 	if (int_status & CIO2_INT_IOOE) {
663 		/*
664 		 * Interrupt on Output Error:
665 		 * 1) SRAM is full and FS received, or
666 		 * 2) An invalid bit detected by DMA.
667 		 */
668 		u32 oe_status, oe_clear;
669 
670 		oe_clear = readl(base + CIO2_REG_INT_STS_EXT_OE);
671 		oe_status = oe_clear;
672 
673 		if (oe_status & CIO2_INT_EXT_OE_DMAOE_MASK) {
674 			dev_err(dev, "DMA output error: 0x%x\n",
675 				(oe_status & CIO2_INT_EXT_OE_DMAOE_MASK)
676 				>> CIO2_INT_EXT_OE_DMAOE_SHIFT);
677 			oe_status &= ~CIO2_INT_EXT_OE_DMAOE_MASK;
678 		}
679 		if (oe_status & CIO2_INT_EXT_OE_OES_MASK) {
680 			dev_err(dev, "DMA output error on CSI2 buses: 0x%x\n",
681 				(oe_status & CIO2_INT_EXT_OE_OES_MASK)
682 				>> CIO2_INT_EXT_OE_OES_SHIFT);
683 			oe_status &= ~CIO2_INT_EXT_OE_OES_MASK;
684 		}
685 		writel(oe_clear, base + CIO2_REG_INT_STS_EXT_OE);
686 		if (oe_status)
687 			dev_warn(dev, "unknown interrupt 0x%x on OE\n",
688 				 oe_status);
689 		int_status &= ~CIO2_INT_IOOE;
690 	}
691 
692 	if (int_status & CIO2_INT_IOC_MASK) {
693 		/* DMA IO done -- frame ready */
694 		u32 clr = 0;
695 		unsigned int d;
696 
697 		for (d = 0; d < CIO2_NUM_DMA_CHAN; d++)
698 			if (int_status & CIO2_INT_IOC(d)) {
699 				clr |= CIO2_INT_IOC(d);
700 				cio2_buffer_done(cio2, d);
701 			}
702 		int_status &= ~clr;
703 	}
704 
705 	if (int_status & CIO2_INT_IOS_IOLN_MASK) {
706 		/* DMA IO starts or reached specified line */
707 		u32 clr = 0;
708 		unsigned int d;
709 
710 		for (d = 0; d < CIO2_NUM_DMA_CHAN; d++)
711 			if (int_status & CIO2_INT_IOS_IOLN(d)) {
712 				clr |= CIO2_INT_IOS_IOLN(d);
713 				if (d == CIO2_DMA_CHAN)
714 					cio2_queue_event_sof(cio2,
715 							     cio2->cur_queue);
716 			}
717 		int_status &= ~clr;
718 	}
719 
720 	if (int_status & (CIO2_INT_IOIE | CIO2_INT_IOIRQ)) {
721 		/* CSI2 receiver (error) interrupt */
722 		unsigned int port;
723 		u32 ie_status;
724 
725 		ie_status = readl(base + CIO2_REG_INT_STS_EXT_IE);
726 
727 		for (port = 0; port < CIO2_NUM_PORTS; port++) {
728 			u32 port_status = (ie_status >> (port * 8)) & 0xff;
729 
730 			cio2_irq_log_port_errs(dev, port, port_status);
731 
732 			if (ie_status & CIO2_INT_EXT_IE_IRQ(port)) {
733 				void __iomem *csi_rx_base =
734 						base + CIO2_REG_PIPE_BASE(port);
735 				u32 csi2_status;
736 
737 				csi2_status = readl(csi_rx_base +
738 						CIO2_REG_IRQCTRL_STATUS);
739 
740 				cio2_irq_log_irq_errs(dev, port, csi2_status);
741 
742 				writel(csi2_status,
743 				       csi_rx_base + CIO2_REG_IRQCTRL_CLEAR);
744 			}
745 		}
746 
747 		writel(ie_status, base + CIO2_REG_INT_STS_EXT_IE);
748 
749 		int_status &= ~(CIO2_INT_IOIE | CIO2_INT_IOIRQ);
750 	}
751 
752 	if (int_status)
753 		dev_warn(dev, "unknown interrupt 0x%x on INT\n", int_status);
754 }
755 
756 static irqreturn_t cio2_irq(int irq, void *cio2_ptr)
757 {
758 	struct cio2_device *cio2 = cio2_ptr;
759 	void __iomem *const base = cio2->base;
760 	struct device *dev = &cio2->pci_dev->dev;
761 	u32 int_status;
762 
763 	int_status = readl(base + CIO2_REG_INT_STS);
764 	dev_dbg(dev, "isr enter - interrupt status 0x%x\n", int_status);
765 	if (!int_status)
766 		return IRQ_NONE;
767 
768 	do {
769 		writel(int_status, base + CIO2_REG_INT_STS);
770 		cio2_irq_handle_once(cio2, int_status);
771 		int_status = readl(base + CIO2_REG_INT_STS);
772 		if (int_status)
773 			dev_dbg(dev, "pending status 0x%x\n", int_status);
774 	} while (int_status);
775 
776 	return IRQ_HANDLED;
777 }
778 
779 /**************** Videobuf2 interface ****************/
780 
781 static void cio2_vb2_return_all_buffers(struct cio2_queue *q,
782 					enum vb2_buffer_state state)
783 {
784 	unsigned int i;
785 
786 	for (i = 0; i < CIO2_MAX_BUFFERS; i++) {
787 		if (q->bufs[i]) {
788 			atomic_dec(&q->bufs_queued);
789 			vb2_buffer_done(&q->bufs[i]->vbb.vb2_buf,
790 					state);
791 			q->bufs[i] = NULL;
792 		}
793 	}
794 }
795 
796 static int cio2_vb2_queue_setup(struct vb2_queue *vq,
797 				unsigned int *num_buffers,
798 				unsigned int *num_planes,
799 				unsigned int sizes[],
800 				struct device *alloc_devs[])
801 {
802 	struct cio2_device *cio2 = vb2_get_drv_priv(vq);
803 	struct device *dev = &cio2->pci_dev->dev;
804 	struct cio2_queue *q = vb2q_to_cio2_queue(vq);
805 	unsigned int i;
806 
807 	if (*num_planes && *num_planes < q->format.num_planes)
808 		return -EINVAL;
809 
810 	for (i = 0; i < q->format.num_planes; ++i) {
811 		if (*num_planes && sizes[i] < q->format.plane_fmt[i].sizeimage)
812 			return -EINVAL;
813 		sizes[i] = q->format.plane_fmt[i].sizeimage;
814 		alloc_devs[i] = dev;
815 	}
816 
817 	*num_planes = q->format.num_planes;
818 	*num_buffers = clamp_val(*num_buffers, 1, CIO2_MAX_BUFFERS);
819 
820 	/* Initialize buffer queue */
821 	for (i = 0; i < CIO2_MAX_BUFFERS; i++) {
822 		q->bufs[i] = NULL;
823 		cio2_fbpt_entry_init_dummy(cio2, &q->fbpt[i * CIO2_MAX_LOPS]);
824 	}
825 	atomic_set(&q->bufs_queued, 0);
826 	q->bufs_first = 0;
827 	q->bufs_next = 0;
828 
829 	return 0;
830 }
831 
832 /* Called after each buffer is allocated */
833 static int cio2_vb2_buf_init(struct vb2_buffer *vb)
834 {
835 	struct cio2_device *cio2 = vb2_get_drv_priv(vb->vb2_queue);
836 	struct device *dev = &cio2->pci_dev->dev;
837 	struct cio2_buffer *b = to_cio2_buffer(vb);
838 	unsigned int pages = PFN_UP(vb->planes[0].length);
839 	unsigned int lops = DIV_ROUND_UP(pages + 1, CIO2_LOP_ENTRIES);
840 	struct sg_table *sg;
841 	struct sg_dma_page_iter sg_iter;
842 	unsigned int i, j;
843 
844 	if (lops <= 0 || lops > CIO2_MAX_LOPS) {
845 		dev_err(dev, "%s: bad buffer size (%i)\n", __func__,
846 			vb->planes[0].length);
847 		return -ENOSPC;		/* Should never happen */
848 	}
849 
850 	memset(b->lop, 0, sizeof(b->lop));
851 	/* Allocate LOP table */
852 	for (i = 0; i < lops; i++) {
853 		b->lop[i] = dma_alloc_coherent(dev, PAGE_SIZE,
854 					       &b->lop_bus_addr[i], GFP_KERNEL);
855 		if (!b->lop[i])
856 			goto fail;
857 	}
858 
859 	/* Fill LOP */
860 	sg = vb2_dma_sg_plane_desc(vb, 0);
861 	if (!sg)
862 		return -ENOMEM;
863 
864 	if (sg->nents && sg->sgl)
865 		b->offset = sg->sgl->offset;
866 
867 	i = j = 0;
868 	for_each_sg_dma_page(sg->sgl, &sg_iter, sg->nents, 0) {
869 		if (!pages--)
870 			break;
871 		b->lop[i][j] = PFN_DOWN(sg_page_iter_dma_address(&sg_iter));
872 		j++;
873 		if (j == CIO2_LOP_ENTRIES) {
874 			i++;
875 			j = 0;
876 		}
877 	}
878 
879 	b->lop[i][j] = PFN_DOWN(cio2->dummy_page_bus_addr);
880 	return 0;
881 fail:
882 	while (i--)
883 		dma_free_coherent(dev, PAGE_SIZE, b->lop[i], b->lop_bus_addr[i]);
884 	return -ENOMEM;
885 }
886 
887 /* Transfer buffer ownership to cio2 */
888 static void cio2_vb2_buf_queue(struct vb2_buffer *vb)
889 {
890 	struct cio2_device *cio2 = vb2_get_drv_priv(vb->vb2_queue);
891 	struct device *dev = &cio2->pci_dev->dev;
892 	struct cio2_queue *q =
893 		container_of(vb->vb2_queue, struct cio2_queue, vbq);
894 	struct cio2_buffer *b = to_cio2_buffer(vb);
895 	struct cio2_fbpt_entry *entry;
896 	unsigned long flags;
897 	unsigned int i, j, next = q->bufs_next;
898 	int bufs_queued = atomic_inc_return(&q->bufs_queued);
899 	u32 fbpt_rp;
900 
901 	dev_dbg(dev, "queue buffer %d\n", vb->index);
902 
903 	/*
904 	 * This code queues the buffer to the CIO2 DMA engine, which starts
905 	 * running once streaming has started. It is possible that this code
906 	 * gets pre-empted due to increased CPU load. Upon this, the driver
907 	 * does not get an opportunity to queue new buffers to the CIO2 DMA
908 	 * engine. When the DMA engine encounters an FBPT entry without the
909 	 * VALID bit set, the DMA engine halts, which requires a restart of
910 	 * the DMA engine and sensor, to continue streaming.
911 	 * This is not desired and is highly unlikely given that there are
912 	 * 32 FBPT entries that the DMA engine needs to process, to run into
913 	 * an FBPT entry, without the VALID bit set. We try to mitigate this
914 	 * by disabling interrupts for the duration of this queueing.
915 	 */
916 	local_irq_save(flags);
917 
918 	fbpt_rp = (readl(cio2->base + CIO2_REG_CDMARI(CIO2_DMA_CHAN))
919 		   >> CIO2_CDMARI_FBPT_RP_SHIFT)
920 		   & CIO2_CDMARI_FBPT_RP_MASK;
921 
922 	/*
923 	 * fbpt_rp is the fbpt entry that the dma is currently working
924 	 * on, but since it could jump to next entry at any time,
925 	 * assume that we might already be there.
926 	 */
927 	fbpt_rp = (fbpt_rp + 1) % CIO2_MAX_BUFFERS;
928 
929 	if (bufs_queued <= 1 || fbpt_rp == next)
930 		/* Buffers were drained */
931 		next = (fbpt_rp + 1) % CIO2_MAX_BUFFERS;
932 
933 	for (i = 0; i < CIO2_MAX_BUFFERS; i++) {
934 		/*
935 		 * We have allocated CIO2_MAX_BUFFERS circularly for the
936 		 * hw, the user has requested N buffer queue. The driver
937 		 * ensures N <= CIO2_MAX_BUFFERS and guarantees that whenever
938 		 * user queues a buffer, there necessarily is a free buffer.
939 		 */
940 		if (!q->bufs[next]) {
941 			q->bufs[next] = b;
942 			entry = &q->fbpt[next * CIO2_MAX_LOPS];
943 			cio2_fbpt_entry_init_buf(cio2, b, entry);
944 			local_irq_restore(flags);
945 			q->bufs_next = (next + 1) % CIO2_MAX_BUFFERS;
946 			for (j = 0; j < vb->num_planes; j++)
947 				vb2_set_plane_payload(vb, j,
948 					q->format.plane_fmt[j].sizeimage);
949 			return;
950 		}
951 
952 		dev_dbg(dev, "entry %i was full!\n", next);
953 		next = (next + 1) % CIO2_MAX_BUFFERS;
954 	}
955 
956 	local_irq_restore(flags);
957 	dev_err(dev, "error: all cio2 entries were full!\n");
958 	atomic_dec(&q->bufs_queued);
959 	vb2_buffer_done(vb, VB2_BUF_STATE_ERROR);
960 }
961 
962 /* Called when each buffer is freed */
963 static void cio2_vb2_buf_cleanup(struct vb2_buffer *vb)
964 {
965 	struct cio2_device *cio2 = vb2_get_drv_priv(vb->vb2_queue);
966 	struct device *dev = &cio2->pci_dev->dev;
967 	struct cio2_buffer *b = to_cio2_buffer(vb);
968 	unsigned int i;
969 
970 	/* Free LOP table */
971 	for (i = 0; i < CIO2_MAX_LOPS; i++) {
972 		if (b->lop[i])
973 			dma_free_coherent(dev, PAGE_SIZE,
974 					  b->lop[i], b->lop_bus_addr[i]);
975 	}
976 }
977 
978 static int cio2_vb2_start_streaming(struct vb2_queue *vq, unsigned int count)
979 {
980 	struct cio2_queue *q = vb2q_to_cio2_queue(vq);
981 	struct cio2_device *cio2 = vb2_get_drv_priv(vq);
982 	struct device *dev = &cio2->pci_dev->dev;
983 	int r;
984 
985 	cio2->cur_queue = q;
986 	atomic_set(&q->frame_sequence, 0);
987 
988 	r = pm_runtime_resume_and_get(dev);
989 	if (r < 0) {
990 		dev_info(dev, "failed to set power %d\n", r);
991 		return r;
992 	}
993 
994 	r = video_device_pipeline_start(&q->vdev, &q->pipe);
995 	if (r)
996 		goto fail_pipeline;
997 
998 	r = cio2_hw_init(cio2, q);
999 	if (r)
1000 		goto fail_hw;
1001 
1002 	/* Start streaming on sensor */
1003 	r = v4l2_subdev_call(q->sensor, video, s_stream, 1);
1004 	if (r)
1005 		goto fail_csi2_subdev;
1006 
1007 	cio2->streaming = true;
1008 
1009 	return 0;
1010 
1011 fail_csi2_subdev:
1012 	cio2_hw_exit(cio2, q);
1013 fail_hw:
1014 	video_device_pipeline_stop(&q->vdev);
1015 fail_pipeline:
1016 	dev_dbg(dev, "failed to start streaming (%d)\n", r);
1017 	cio2_vb2_return_all_buffers(q, VB2_BUF_STATE_QUEUED);
1018 	pm_runtime_put(dev);
1019 
1020 	return r;
1021 }
1022 
1023 static void cio2_vb2_stop_streaming(struct vb2_queue *vq)
1024 {
1025 	struct cio2_queue *q = vb2q_to_cio2_queue(vq);
1026 	struct cio2_device *cio2 = vb2_get_drv_priv(vq);
1027 	struct device *dev = &cio2->pci_dev->dev;
1028 
1029 	if (v4l2_subdev_call(q->sensor, video, s_stream, 0))
1030 		dev_err(dev, "failed to stop sensor streaming\n");
1031 
1032 	cio2_hw_exit(cio2, q);
1033 	synchronize_irq(cio2->pci_dev->irq);
1034 	cio2_vb2_return_all_buffers(q, VB2_BUF_STATE_ERROR);
1035 	video_device_pipeline_stop(&q->vdev);
1036 	pm_runtime_put(dev);
1037 	cio2->streaming = false;
1038 }
1039 
1040 static const struct vb2_ops cio2_vb2_ops = {
1041 	.buf_init = cio2_vb2_buf_init,
1042 	.buf_queue = cio2_vb2_buf_queue,
1043 	.buf_cleanup = cio2_vb2_buf_cleanup,
1044 	.queue_setup = cio2_vb2_queue_setup,
1045 	.start_streaming = cio2_vb2_start_streaming,
1046 	.stop_streaming = cio2_vb2_stop_streaming,
1047 	.wait_prepare = vb2_ops_wait_prepare,
1048 	.wait_finish = vb2_ops_wait_finish,
1049 };
1050 
1051 /**************** V4L2 interface ****************/
1052 
1053 static int cio2_v4l2_querycap(struct file *file, void *fh,
1054 			      struct v4l2_capability *cap)
1055 {
1056 	strscpy(cap->driver, CIO2_NAME, sizeof(cap->driver));
1057 	strscpy(cap->card, CIO2_DEVICE_NAME, sizeof(cap->card));
1058 
1059 	return 0;
1060 }
1061 
1062 static int cio2_v4l2_enum_fmt(struct file *file, void *fh,
1063 			      struct v4l2_fmtdesc *f)
1064 {
1065 	if (f->index >= ARRAY_SIZE(formats))
1066 		return -EINVAL;
1067 
1068 	f->pixelformat = formats[f->index].fourcc;
1069 
1070 	return 0;
1071 }
1072 
1073 /* The format is validated in cio2_video_link_validate() */
1074 static int cio2_v4l2_g_fmt(struct file *file, void *fh, struct v4l2_format *f)
1075 {
1076 	struct cio2_queue *q = file_to_cio2_queue(file);
1077 
1078 	f->fmt.pix_mp = q->format;
1079 
1080 	return 0;
1081 }
1082 
1083 static int cio2_v4l2_try_fmt(struct file *file, void *fh, struct v4l2_format *f)
1084 {
1085 	const struct ipu3_cio2_fmt *fmt;
1086 	struct v4l2_pix_format_mplane *mpix = &f->fmt.pix_mp;
1087 
1088 	fmt = cio2_find_format(&mpix->pixelformat, NULL);
1089 	if (!fmt)
1090 		fmt = &formats[0];
1091 
1092 	/* Only supports up to 4224x3136 */
1093 	if (mpix->width > CIO2_IMAGE_MAX_WIDTH)
1094 		mpix->width = CIO2_IMAGE_MAX_WIDTH;
1095 	if (mpix->height > CIO2_IMAGE_MAX_HEIGHT)
1096 		mpix->height = CIO2_IMAGE_MAX_HEIGHT;
1097 
1098 	mpix->num_planes = 1;
1099 	mpix->pixelformat = fmt->fourcc;
1100 	mpix->colorspace = V4L2_COLORSPACE_RAW;
1101 	mpix->field = V4L2_FIELD_NONE;
1102 	mpix->plane_fmt[0].bytesperline = cio2_bytesperline(mpix->width);
1103 	mpix->plane_fmt[0].sizeimage = mpix->plane_fmt[0].bytesperline *
1104 							mpix->height;
1105 
1106 	/* use default */
1107 	mpix->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
1108 	mpix->quantization = V4L2_QUANTIZATION_DEFAULT;
1109 	mpix->xfer_func = V4L2_XFER_FUNC_DEFAULT;
1110 
1111 	return 0;
1112 }
1113 
1114 static int cio2_v4l2_s_fmt(struct file *file, void *fh, struct v4l2_format *f)
1115 {
1116 	struct cio2_queue *q = file_to_cio2_queue(file);
1117 
1118 	cio2_v4l2_try_fmt(file, fh, f);
1119 	q->format = f->fmt.pix_mp;
1120 
1121 	return 0;
1122 }
1123 
1124 static int
1125 cio2_video_enum_input(struct file *file, void *fh, struct v4l2_input *input)
1126 {
1127 	if (input->index > 0)
1128 		return -EINVAL;
1129 
1130 	strscpy(input->name, "camera", sizeof(input->name));
1131 	input->type = V4L2_INPUT_TYPE_CAMERA;
1132 
1133 	return 0;
1134 }
1135 
1136 static int
1137 cio2_video_g_input(struct file *file, void *fh, unsigned int *input)
1138 {
1139 	*input = 0;
1140 
1141 	return 0;
1142 }
1143 
1144 static int
1145 cio2_video_s_input(struct file *file, void *fh, unsigned int input)
1146 {
1147 	return input == 0 ? 0 : -EINVAL;
1148 }
1149 
1150 static const struct v4l2_file_operations cio2_v4l2_fops = {
1151 	.owner = THIS_MODULE,
1152 	.unlocked_ioctl = video_ioctl2,
1153 	.open = v4l2_fh_open,
1154 	.release = vb2_fop_release,
1155 	.poll = vb2_fop_poll,
1156 	.mmap = vb2_fop_mmap,
1157 };
1158 
1159 static const struct v4l2_ioctl_ops cio2_v4l2_ioctl_ops = {
1160 	.vidioc_querycap = cio2_v4l2_querycap,
1161 	.vidioc_enum_fmt_vid_cap = cio2_v4l2_enum_fmt,
1162 	.vidioc_g_fmt_vid_cap_mplane = cio2_v4l2_g_fmt,
1163 	.vidioc_s_fmt_vid_cap_mplane = cio2_v4l2_s_fmt,
1164 	.vidioc_try_fmt_vid_cap_mplane = cio2_v4l2_try_fmt,
1165 	.vidioc_reqbufs = vb2_ioctl_reqbufs,
1166 	.vidioc_create_bufs = vb2_ioctl_create_bufs,
1167 	.vidioc_prepare_buf = vb2_ioctl_prepare_buf,
1168 	.vidioc_querybuf = vb2_ioctl_querybuf,
1169 	.vidioc_qbuf = vb2_ioctl_qbuf,
1170 	.vidioc_dqbuf = vb2_ioctl_dqbuf,
1171 	.vidioc_streamon = vb2_ioctl_streamon,
1172 	.vidioc_streamoff = vb2_ioctl_streamoff,
1173 	.vidioc_expbuf = vb2_ioctl_expbuf,
1174 	.vidioc_enum_input = cio2_video_enum_input,
1175 	.vidioc_g_input	= cio2_video_g_input,
1176 	.vidioc_s_input	= cio2_video_s_input,
1177 };
1178 
1179 static int cio2_subdev_subscribe_event(struct v4l2_subdev *sd,
1180 				       struct v4l2_fh *fh,
1181 				       struct v4l2_event_subscription *sub)
1182 {
1183 	if (sub->type != V4L2_EVENT_FRAME_SYNC)
1184 		return -EINVAL;
1185 
1186 	/* Line number. For now only zero accepted. */
1187 	if (sub->id != 0)
1188 		return -EINVAL;
1189 
1190 	return v4l2_event_subscribe(fh, sub, 0, NULL);
1191 }
1192 
1193 static int cio2_subdev_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
1194 {
1195 	struct v4l2_mbus_framefmt *format;
1196 	const struct v4l2_mbus_framefmt fmt_default = {
1197 		.width = 1936,
1198 		.height = 1096,
1199 		.code = formats[0].mbus_code,
1200 		.field = V4L2_FIELD_NONE,
1201 		.colorspace = V4L2_COLORSPACE_RAW,
1202 		.ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT,
1203 		.quantization = V4L2_QUANTIZATION_DEFAULT,
1204 		.xfer_func = V4L2_XFER_FUNC_DEFAULT,
1205 	};
1206 
1207 	/* Initialize try_fmt */
1208 	format = v4l2_subdev_get_try_format(sd, fh->state, CIO2_PAD_SINK);
1209 	*format = fmt_default;
1210 
1211 	/* same as sink */
1212 	format = v4l2_subdev_get_try_format(sd, fh->state, CIO2_PAD_SOURCE);
1213 	*format = fmt_default;
1214 
1215 	return 0;
1216 }
1217 
1218 /*
1219  * cio2_subdev_get_fmt - Handle get format by pads subdev method
1220  * @sd : pointer to v4l2 subdev structure
1221  * @cfg: V4L2 subdev pad config
1222  * @fmt: pointer to v4l2 subdev format structure
1223  * return -EINVAL or zero on success
1224  */
1225 static int cio2_subdev_get_fmt(struct v4l2_subdev *sd,
1226 			       struct v4l2_subdev_state *sd_state,
1227 			       struct v4l2_subdev_format *fmt)
1228 {
1229 	struct cio2_queue *q = container_of(sd, struct cio2_queue, subdev);
1230 
1231 	mutex_lock(&q->subdev_lock);
1232 
1233 	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY)
1234 		fmt->format = *v4l2_subdev_get_try_format(sd, sd_state,
1235 							  fmt->pad);
1236 	else
1237 		fmt->format = q->subdev_fmt;
1238 
1239 	mutex_unlock(&q->subdev_lock);
1240 
1241 	return 0;
1242 }
1243 
1244 /*
1245  * cio2_subdev_set_fmt - Handle set format by pads subdev method
1246  * @sd : pointer to v4l2 subdev structure
1247  * @cfg: V4L2 subdev pad config
1248  * @fmt: pointer to v4l2 subdev format structure
1249  * return -EINVAL or zero on success
1250  */
1251 static int cio2_subdev_set_fmt(struct v4l2_subdev *sd,
1252 			       struct v4l2_subdev_state *sd_state,
1253 			       struct v4l2_subdev_format *fmt)
1254 {
1255 	struct cio2_queue *q = container_of(sd, struct cio2_queue, subdev);
1256 	struct v4l2_mbus_framefmt *mbus;
1257 	u32 mbus_code = fmt->format.code;
1258 	unsigned int i;
1259 
1260 	/*
1261 	 * Only allow setting sink pad format;
1262 	 * source always propagates from sink
1263 	 */
1264 	if (fmt->pad == CIO2_PAD_SOURCE)
1265 		return cio2_subdev_get_fmt(sd, sd_state, fmt);
1266 
1267 	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY)
1268 		mbus = v4l2_subdev_get_try_format(sd, sd_state, fmt->pad);
1269 	else
1270 		mbus = &q->subdev_fmt;
1271 
1272 	fmt->format.code = formats[0].mbus_code;
1273 
1274 	for (i = 0; i < ARRAY_SIZE(formats); i++) {
1275 		if (formats[i].mbus_code == mbus_code) {
1276 			fmt->format.code = mbus_code;
1277 			break;
1278 		}
1279 	}
1280 
1281 	fmt->format.width = min(fmt->format.width, CIO2_IMAGE_MAX_WIDTH);
1282 	fmt->format.height = min(fmt->format.height, CIO2_IMAGE_MAX_HEIGHT);
1283 	fmt->format.field = V4L2_FIELD_NONE;
1284 
1285 	mutex_lock(&q->subdev_lock);
1286 	*mbus = fmt->format;
1287 	mutex_unlock(&q->subdev_lock);
1288 
1289 	return 0;
1290 }
1291 
1292 static int cio2_subdev_enum_mbus_code(struct v4l2_subdev *sd,
1293 				      struct v4l2_subdev_state *sd_state,
1294 				      struct v4l2_subdev_mbus_code_enum *code)
1295 {
1296 	if (code->index >= ARRAY_SIZE(formats))
1297 		return -EINVAL;
1298 
1299 	code->code = formats[code->index].mbus_code;
1300 	return 0;
1301 }
1302 
1303 static int cio2_subdev_link_validate_get_format(struct media_pad *pad,
1304 						struct v4l2_subdev_format *fmt)
1305 {
1306 	if (is_media_entity_v4l2_subdev(pad->entity)) {
1307 		struct v4l2_subdev *sd =
1308 			media_entity_to_v4l2_subdev(pad->entity);
1309 
1310 		memset(fmt, 0, sizeof(*fmt));
1311 		fmt->which = V4L2_SUBDEV_FORMAT_ACTIVE;
1312 		fmt->pad = pad->index;
1313 		return v4l2_subdev_call(sd, pad, get_fmt, NULL, fmt);
1314 	}
1315 
1316 	return -EINVAL;
1317 }
1318 
1319 static int cio2_video_link_validate(struct media_link *link)
1320 {
1321 	struct media_entity *entity = link->sink->entity;
1322 	struct video_device *vd = media_entity_to_video_device(entity);
1323 	struct cio2_queue *q = container_of(vd, struct cio2_queue, vdev);
1324 	struct cio2_device *cio2 = video_get_drvdata(vd);
1325 	struct device *dev = &cio2->pci_dev->dev;
1326 	struct v4l2_subdev_format source_fmt;
1327 	int ret;
1328 
1329 	if (!media_pad_remote_pad_first(entity->pads)) {
1330 		dev_info(dev, "video node %s pad not connected\n", vd->name);
1331 		return -ENOTCONN;
1332 	}
1333 
1334 	ret = cio2_subdev_link_validate_get_format(link->source, &source_fmt);
1335 	if (ret < 0)
1336 		return 0;
1337 
1338 	if (source_fmt.format.width != q->format.width ||
1339 	    source_fmt.format.height != q->format.height) {
1340 		dev_err(dev, "Wrong width or height %ux%u (%ux%u expected)\n",
1341 			q->format.width, q->format.height,
1342 			source_fmt.format.width, source_fmt.format.height);
1343 		return -EINVAL;
1344 	}
1345 
1346 	if (!cio2_find_format(&q->format.pixelformat, &source_fmt.format.code))
1347 		return -EINVAL;
1348 
1349 	return 0;
1350 }
1351 
1352 static const struct v4l2_subdev_core_ops cio2_subdev_core_ops = {
1353 	.subscribe_event = cio2_subdev_subscribe_event,
1354 	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
1355 };
1356 
1357 static const struct v4l2_subdev_internal_ops cio2_subdev_internal_ops = {
1358 	.open = cio2_subdev_open,
1359 };
1360 
1361 static const struct v4l2_subdev_pad_ops cio2_subdev_pad_ops = {
1362 	.link_validate = v4l2_subdev_link_validate_default,
1363 	.get_fmt = cio2_subdev_get_fmt,
1364 	.set_fmt = cio2_subdev_set_fmt,
1365 	.enum_mbus_code = cio2_subdev_enum_mbus_code,
1366 };
1367 
1368 static const struct v4l2_subdev_ops cio2_subdev_ops = {
1369 	.core = &cio2_subdev_core_ops,
1370 	.pad = &cio2_subdev_pad_ops,
1371 };
1372 
1373 /******* V4L2 sub-device asynchronous registration callbacks***********/
1374 
1375 struct sensor_async_subdev {
1376 	struct v4l2_async_connection asd;
1377 	struct csi2_bus_info csi2;
1378 };
1379 
1380 #define to_sensor_asd(__asd)	\
1381 	container_of_const(__asd, struct sensor_async_subdev, asd)
1382 
1383 /* The .bound() notifier callback when a match is found */
1384 static int cio2_notifier_bound(struct v4l2_async_notifier *notifier,
1385 			       struct v4l2_subdev *sd,
1386 			       struct v4l2_async_connection *asd)
1387 {
1388 	struct cio2_device *cio2 = to_cio2_device(notifier);
1389 	struct sensor_async_subdev *s_asd = to_sensor_asd(asd);
1390 	struct cio2_queue *q;
1391 	int ret;
1392 
1393 	if (cio2->queue[s_asd->csi2.port].sensor)
1394 		return -EBUSY;
1395 
1396 	ret = ipu_bridge_instantiate_vcm(sd->dev);
1397 	if (ret)
1398 		return ret;
1399 
1400 	q = &cio2->queue[s_asd->csi2.port];
1401 
1402 	q->csi2 = s_asd->csi2;
1403 	q->sensor = sd;
1404 	q->csi_rx_base = cio2->base + CIO2_REG_PIPE_BASE(q->csi2.port);
1405 
1406 	return 0;
1407 }
1408 
1409 /* The .unbind callback */
1410 static void cio2_notifier_unbind(struct v4l2_async_notifier *notifier,
1411 				 struct v4l2_subdev *sd,
1412 				 struct v4l2_async_connection *asd)
1413 {
1414 	struct cio2_device *cio2 = to_cio2_device(notifier);
1415 	struct sensor_async_subdev *s_asd = to_sensor_asd(asd);
1416 
1417 	cio2->queue[s_asd->csi2.port].sensor = NULL;
1418 }
1419 
1420 /* .complete() is called after all subdevices have been located */
1421 static int cio2_notifier_complete(struct v4l2_async_notifier *notifier)
1422 {
1423 	struct cio2_device *cio2 = to_cio2_device(notifier);
1424 	struct device *dev = &cio2->pci_dev->dev;
1425 	struct sensor_async_subdev *s_asd;
1426 	struct v4l2_async_connection *asd;
1427 	struct cio2_queue *q;
1428 	int ret;
1429 
1430 	list_for_each_entry(asd, &cio2->notifier.done_list, asc_entry) {
1431 		s_asd = to_sensor_asd(asd);
1432 		q = &cio2->queue[s_asd->csi2.port];
1433 
1434 		ret = media_entity_get_fwnode_pad(&q->sensor->entity,
1435 						  s_asd->asd.match.fwnode,
1436 						  MEDIA_PAD_FL_SOURCE);
1437 		if (ret < 0) {
1438 			dev_err(dev, "no pad for endpoint %pfw (%d)\n",
1439 				s_asd->asd.match.fwnode, ret);
1440 			return ret;
1441 		}
1442 
1443 		ret = media_create_pad_link(&q->sensor->entity, ret,
1444 					    &q->subdev.entity, CIO2_PAD_SINK,
1445 					    0);
1446 		if (ret) {
1447 			dev_err(dev, "failed to create link for %s (endpoint %pfw, error %d)\n",
1448 				q->sensor->name, s_asd->asd.match.fwnode, ret);
1449 			return ret;
1450 		}
1451 	}
1452 
1453 	return v4l2_device_register_subdev_nodes(&cio2->v4l2_dev);
1454 }
1455 
1456 static const struct v4l2_async_notifier_operations cio2_async_ops = {
1457 	.bound = cio2_notifier_bound,
1458 	.unbind = cio2_notifier_unbind,
1459 	.complete = cio2_notifier_complete,
1460 };
1461 
1462 static int cio2_parse_firmware(struct cio2_device *cio2)
1463 {
1464 	struct device *dev = &cio2->pci_dev->dev;
1465 	unsigned int i;
1466 	int ret;
1467 
1468 	for (i = 0; i < CIO2_NUM_PORTS; i++) {
1469 		struct v4l2_fwnode_endpoint vep = {
1470 			.bus_type = V4L2_MBUS_CSI2_DPHY
1471 		};
1472 		struct sensor_async_subdev *s_asd;
1473 		struct fwnode_handle *ep;
1474 
1475 		ep = fwnode_graph_get_endpoint_by_id(dev_fwnode(dev), i, 0,
1476 						FWNODE_GRAPH_ENDPOINT_NEXT);
1477 		if (!ep)
1478 			continue;
1479 
1480 		ret = v4l2_fwnode_endpoint_parse(ep, &vep);
1481 		if (ret)
1482 			goto err_parse;
1483 
1484 		s_asd = v4l2_async_nf_add_fwnode_remote(&cio2->notifier, ep,
1485 							struct
1486 							sensor_async_subdev);
1487 		if (IS_ERR(s_asd)) {
1488 			ret = PTR_ERR(s_asd);
1489 			goto err_parse;
1490 		}
1491 
1492 		s_asd->csi2.port = vep.base.port;
1493 		s_asd->csi2.lanes = vep.bus.mipi_csi2.num_data_lanes;
1494 
1495 		fwnode_handle_put(ep);
1496 
1497 		continue;
1498 
1499 err_parse:
1500 		fwnode_handle_put(ep);
1501 		return ret;
1502 	}
1503 
1504 	/*
1505 	 * Proceed even without sensors connected to allow the device to
1506 	 * suspend.
1507 	 */
1508 	cio2->notifier.ops = &cio2_async_ops;
1509 	ret = v4l2_async_nf_register(&cio2->notifier);
1510 	if (ret)
1511 		dev_err(dev, "failed to register async notifier : %d\n", ret);
1512 
1513 	return ret;
1514 }
1515 
1516 /**************** Queue initialization ****************/
1517 static const struct media_entity_operations cio2_media_ops = {
1518 	.link_validate = v4l2_subdev_link_validate,
1519 };
1520 
1521 static const struct media_entity_operations cio2_video_entity_ops = {
1522 	.link_validate = cio2_video_link_validate,
1523 };
1524 
1525 static int cio2_queue_init(struct cio2_device *cio2, struct cio2_queue *q)
1526 {
1527 	static const u32 default_width = 1936;
1528 	static const u32 default_height = 1096;
1529 	const struct ipu3_cio2_fmt dflt_fmt = formats[0];
1530 	struct device *dev = &cio2->pci_dev->dev;
1531 	struct video_device *vdev = &q->vdev;
1532 	struct vb2_queue *vbq = &q->vbq;
1533 	struct v4l2_subdev *subdev = &q->subdev;
1534 	struct v4l2_mbus_framefmt *fmt;
1535 	int r;
1536 
1537 	/* Initialize miscellaneous variables */
1538 	mutex_init(&q->lock);
1539 	mutex_init(&q->subdev_lock);
1540 
1541 	/* Initialize formats to default values */
1542 	fmt = &q->subdev_fmt;
1543 	fmt->width = default_width;
1544 	fmt->height = default_height;
1545 	fmt->code = dflt_fmt.mbus_code;
1546 	fmt->field = V4L2_FIELD_NONE;
1547 
1548 	q->format.width = default_width;
1549 	q->format.height = default_height;
1550 	q->format.pixelformat = dflt_fmt.fourcc;
1551 	q->format.colorspace = V4L2_COLORSPACE_RAW;
1552 	q->format.field = V4L2_FIELD_NONE;
1553 	q->format.num_planes = 1;
1554 	q->format.plane_fmt[0].bytesperline =
1555 				cio2_bytesperline(q->format.width);
1556 	q->format.plane_fmt[0].sizeimage = q->format.plane_fmt[0].bytesperline *
1557 						q->format.height;
1558 
1559 	/* Initialize fbpt */
1560 	r = cio2_fbpt_init(cio2, q);
1561 	if (r)
1562 		goto fail_fbpt;
1563 
1564 	/* Initialize media entities */
1565 	q->subdev_pads[CIO2_PAD_SINK].flags = MEDIA_PAD_FL_SINK |
1566 		MEDIA_PAD_FL_MUST_CONNECT;
1567 	q->subdev_pads[CIO2_PAD_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
1568 	subdev->entity.ops = &cio2_media_ops;
1569 	subdev->internal_ops = &cio2_subdev_internal_ops;
1570 	r = media_entity_pads_init(&subdev->entity, CIO2_PADS, q->subdev_pads);
1571 	if (r) {
1572 		dev_err(dev, "failed initialize subdev media entity (%d)\n", r);
1573 		goto fail_subdev_media_entity;
1574 	}
1575 
1576 	q->vdev_pad.flags = MEDIA_PAD_FL_SINK | MEDIA_PAD_FL_MUST_CONNECT;
1577 	vdev->entity.ops = &cio2_video_entity_ops;
1578 	r = media_entity_pads_init(&vdev->entity, 1, &q->vdev_pad);
1579 	if (r) {
1580 		dev_err(dev, "failed initialize videodev media entity (%d)\n",
1581 			r);
1582 		goto fail_vdev_media_entity;
1583 	}
1584 
1585 	/* Initialize subdev */
1586 	v4l2_subdev_init(subdev, &cio2_subdev_ops);
1587 	subdev->flags = V4L2_SUBDEV_FL_HAS_DEVNODE | V4L2_SUBDEV_FL_HAS_EVENTS;
1588 	subdev->owner = THIS_MODULE;
1589 	snprintf(subdev->name, sizeof(subdev->name),
1590 		 CIO2_ENTITY_NAME " %td", q - cio2->queue);
1591 	subdev->entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
1592 	v4l2_set_subdevdata(subdev, cio2);
1593 	r = v4l2_device_register_subdev(&cio2->v4l2_dev, subdev);
1594 	if (r) {
1595 		dev_err(dev, "failed initialize subdev (%d)\n", r);
1596 		goto fail_subdev;
1597 	}
1598 
1599 	/* Initialize vbq */
1600 	vbq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
1601 	vbq->io_modes = VB2_USERPTR | VB2_MMAP | VB2_DMABUF;
1602 	vbq->ops = &cio2_vb2_ops;
1603 	vbq->mem_ops = &vb2_dma_sg_memops;
1604 	vbq->buf_struct_size = sizeof(struct cio2_buffer);
1605 	vbq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
1606 	vbq->min_buffers_needed = 1;
1607 	vbq->drv_priv = cio2;
1608 	vbq->lock = &q->lock;
1609 	r = vb2_queue_init(vbq);
1610 	if (r) {
1611 		dev_err(dev, "failed to initialize videobuf2 queue (%d)\n", r);
1612 		goto fail_subdev;
1613 	}
1614 
1615 	/* Initialize vdev */
1616 	snprintf(vdev->name, sizeof(vdev->name),
1617 		 "%s %td", CIO2_NAME, q - cio2->queue);
1618 	vdev->release = video_device_release_empty;
1619 	vdev->fops = &cio2_v4l2_fops;
1620 	vdev->ioctl_ops = &cio2_v4l2_ioctl_ops;
1621 	vdev->lock = &cio2->lock;
1622 	vdev->v4l2_dev = &cio2->v4l2_dev;
1623 	vdev->queue = &q->vbq;
1624 	vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE_MPLANE | V4L2_CAP_STREAMING;
1625 	video_set_drvdata(vdev, cio2);
1626 	r = video_register_device(vdev, VFL_TYPE_VIDEO, -1);
1627 	if (r) {
1628 		dev_err(dev, "failed to register video device (%d)\n", r);
1629 		goto fail_vdev;
1630 	}
1631 
1632 	/* Create link from CIO2 subdev to output node */
1633 	r = media_create_pad_link(
1634 		&subdev->entity, CIO2_PAD_SOURCE, &vdev->entity, 0,
1635 		MEDIA_LNK_FL_ENABLED | MEDIA_LNK_FL_IMMUTABLE);
1636 	if (r)
1637 		goto fail_link;
1638 
1639 	return 0;
1640 
1641 fail_link:
1642 	vb2_video_unregister_device(&q->vdev);
1643 fail_vdev:
1644 	v4l2_device_unregister_subdev(subdev);
1645 fail_subdev:
1646 	media_entity_cleanup(&vdev->entity);
1647 fail_vdev_media_entity:
1648 	media_entity_cleanup(&subdev->entity);
1649 fail_subdev_media_entity:
1650 	cio2_fbpt_exit(q, dev);
1651 fail_fbpt:
1652 	mutex_destroy(&q->subdev_lock);
1653 	mutex_destroy(&q->lock);
1654 
1655 	return r;
1656 }
1657 
1658 static void cio2_queue_exit(struct cio2_device *cio2, struct cio2_queue *q)
1659 {
1660 	vb2_video_unregister_device(&q->vdev);
1661 	media_entity_cleanup(&q->vdev.entity);
1662 	v4l2_device_unregister_subdev(&q->subdev);
1663 	media_entity_cleanup(&q->subdev.entity);
1664 	cio2_fbpt_exit(q, &cio2->pci_dev->dev);
1665 	mutex_destroy(&q->subdev_lock);
1666 	mutex_destroy(&q->lock);
1667 }
1668 
1669 static int cio2_queues_init(struct cio2_device *cio2)
1670 {
1671 	int i, r;
1672 
1673 	for (i = 0; i < CIO2_QUEUES; i++) {
1674 		r = cio2_queue_init(cio2, &cio2->queue[i]);
1675 		if (r)
1676 			break;
1677 	}
1678 
1679 	if (i == CIO2_QUEUES)
1680 		return 0;
1681 
1682 	for (i--; i >= 0; i--)
1683 		cio2_queue_exit(cio2, &cio2->queue[i]);
1684 
1685 	return r;
1686 }
1687 
1688 static void cio2_queues_exit(struct cio2_device *cio2)
1689 {
1690 	unsigned int i;
1691 
1692 	for (i = 0; i < CIO2_QUEUES; i++)
1693 		cio2_queue_exit(cio2, &cio2->queue[i]);
1694 }
1695 
1696 static int cio2_check_fwnode_graph(struct fwnode_handle *fwnode)
1697 {
1698 	struct fwnode_handle *endpoint;
1699 
1700 	if (IS_ERR_OR_NULL(fwnode))
1701 		return -EINVAL;
1702 
1703 	endpoint = fwnode_graph_get_next_endpoint(fwnode, NULL);
1704 	if (endpoint) {
1705 		fwnode_handle_put(endpoint);
1706 		return 0;
1707 	}
1708 
1709 	return cio2_check_fwnode_graph(fwnode->secondary);
1710 }
1711 
1712 /**************** PCI interface ****************/
1713 
1714 static int cio2_pci_probe(struct pci_dev *pci_dev,
1715 			  const struct pci_device_id *id)
1716 {
1717 	struct device *dev = &pci_dev->dev;
1718 	struct fwnode_handle *fwnode = dev_fwnode(dev);
1719 	struct cio2_device *cio2;
1720 	int r;
1721 
1722 	/*
1723 	 * On some platforms no connections to sensors are defined in firmware,
1724 	 * if the device has no endpoints then we can try to build those as
1725 	 * software_nodes parsed from SSDB.
1726 	 */
1727 	r = cio2_check_fwnode_graph(fwnode);
1728 	if (r) {
1729 		if (fwnode && !IS_ERR_OR_NULL(fwnode->secondary)) {
1730 			dev_err(dev, "fwnode graph has no endpoints connected\n");
1731 			return -EINVAL;
1732 		}
1733 
1734 		r = ipu_bridge_init(dev, ipu_bridge_parse_ssdb);
1735 		if (r)
1736 			return r;
1737 	}
1738 
1739 	cio2 = devm_kzalloc(dev, sizeof(*cio2), GFP_KERNEL);
1740 	if (!cio2)
1741 		return -ENOMEM;
1742 	cio2->pci_dev = pci_dev;
1743 
1744 	r = pcim_enable_device(pci_dev);
1745 	if (r) {
1746 		dev_err(dev, "failed to enable device (%d)\n", r);
1747 		return r;
1748 	}
1749 
1750 	dev_info(dev, "device 0x%x (rev: 0x%x)\n",
1751 		 pci_dev->device, pci_dev->revision);
1752 
1753 	r = pcim_iomap_regions(pci_dev, 1 << CIO2_PCI_BAR, pci_name(pci_dev));
1754 	if (r) {
1755 		dev_err(dev, "failed to remap I/O memory (%d)\n", r);
1756 		return -ENODEV;
1757 	}
1758 
1759 	cio2->base = pcim_iomap_table(pci_dev)[CIO2_PCI_BAR];
1760 
1761 	pci_set_drvdata(pci_dev, cio2);
1762 
1763 	pci_set_master(pci_dev);
1764 
1765 	r = dma_set_mask(&pci_dev->dev, CIO2_DMA_MASK);
1766 	if (r) {
1767 		dev_err(dev, "failed to set DMA mask (%d)\n", r);
1768 		return -ENODEV;
1769 	}
1770 
1771 	r = pci_enable_msi(pci_dev);
1772 	if (r) {
1773 		dev_err(dev, "failed to enable MSI (%d)\n", r);
1774 		return r;
1775 	}
1776 
1777 	r = cio2_fbpt_init_dummy(cio2);
1778 	if (r)
1779 		return r;
1780 
1781 	mutex_init(&cio2->lock);
1782 
1783 	cio2->media_dev.dev = dev;
1784 	strscpy(cio2->media_dev.model, CIO2_DEVICE_NAME,
1785 		sizeof(cio2->media_dev.model));
1786 	cio2->media_dev.hw_revision = 0;
1787 
1788 	media_device_init(&cio2->media_dev);
1789 	r = media_device_register(&cio2->media_dev);
1790 	if (r < 0)
1791 		goto fail_mutex_destroy;
1792 
1793 	cio2->v4l2_dev.mdev = &cio2->media_dev;
1794 	r = v4l2_device_register(dev, &cio2->v4l2_dev);
1795 	if (r) {
1796 		dev_err(dev, "failed to register V4L2 device (%d)\n", r);
1797 		goto fail_media_device_unregister;
1798 	}
1799 
1800 	r = cio2_queues_init(cio2);
1801 	if (r)
1802 		goto fail_v4l2_device_unregister;
1803 
1804 	v4l2_async_nf_init(&cio2->notifier, &cio2->v4l2_dev);
1805 
1806 	/* Register notifier for subdevices we care */
1807 	r = cio2_parse_firmware(cio2);
1808 	if (r)
1809 		goto fail_clean_notifier;
1810 
1811 	r = devm_request_irq(dev, pci_dev->irq, cio2_irq, IRQF_SHARED,
1812 			     CIO2_NAME, cio2);
1813 	if (r) {
1814 		dev_err(dev, "failed to request IRQ (%d)\n", r);
1815 		goto fail_clean_notifier;
1816 	}
1817 
1818 	pm_runtime_put_noidle(dev);
1819 	pm_runtime_allow(dev);
1820 
1821 	return 0;
1822 
1823 fail_clean_notifier:
1824 	v4l2_async_nf_unregister(&cio2->notifier);
1825 	v4l2_async_nf_cleanup(&cio2->notifier);
1826 	cio2_queues_exit(cio2);
1827 fail_v4l2_device_unregister:
1828 	v4l2_device_unregister(&cio2->v4l2_dev);
1829 fail_media_device_unregister:
1830 	media_device_unregister(&cio2->media_dev);
1831 	media_device_cleanup(&cio2->media_dev);
1832 fail_mutex_destroy:
1833 	mutex_destroy(&cio2->lock);
1834 	cio2_fbpt_exit_dummy(cio2);
1835 
1836 	return r;
1837 }
1838 
1839 static void cio2_pci_remove(struct pci_dev *pci_dev)
1840 {
1841 	struct cio2_device *cio2 = pci_get_drvdata(pci_dev);
1842 
1843 	media_device_unregister(&cio2->media_dev);
1844 	v4l2_async_nf_unregister(&cio2->notifier);
1845 	v4l2_async_nf_cleanup(&cio2->notifier);
1846 	cio2_queues_exit(cio2);
1847 	cio2_fbpt_exit_dummy(cio2);
1848 	v4l2_device_unregister(&cio2->v4l2_dev);
1849 	media_device_cleanup(&cio2->media_dev);
1850 	mutex_destroy(&cio2->lock);
1851 
1852 	pm_runtime_forbid(&pci_dev->dev);
1853 	pm_runtime_get_noresume(&pci_dev->dev);
1854 }
1855 
1856 static int __maybe_unused cio2_runtime_suspend(struct device *dev)
1857 {
1858 	struct pci_dev *pci_dev = to_pci_dev(dev);
1859 	struct cio2_device *cio2 = pci_get_drvdata(pci_dev);
1860 	void __iomem *const base = cio2->base;
1861 	u16 pm;
1862 
1863 	writel(CIO2_D0I3C_I3, base + CIO2_REG_D0I3C);
1864 	dev_dbg(dev, "cio2 runtime suspend.\n");
1865 
1866 	pci_read_config_word(pci_dev, pci_dev->pm_cap + CIO2_PMCSR_OFFSET, &pm);
1867 	pm = (pm >> CIO2_PMCSR_D0D3_SHIFT) << CIO2_PMCSR_D0D3_SHIFT;
1868 	pm |= CIO2_PMCSR_D3;
1869 	pci_write_config_word(pci_dev, pci_dev->pm_cap + CIO2_PMCSR_OFFSET, pm);
1870 
1871 	return 0;
1872 }
1873 
1874 static int __maybe_unused cio2_runtime_resume(struct device *dev)
1875 {
1876 	struct pci_dev *pci_dev = to_pci_dev(dev);
1877 	struct cio2_device *cio2 = pci_get_drvdata(pci_dev);
1878 	void __iomem *const base = cio2->base;
1879 	u16 pm;
1880 
1881 	writel(CIO2_D0I3C_RR, base + CIO2_REG_D0I3C);
1882 	dev_dbg(dev, "cio2 runtime resume.\n");
1883 
1884 	pci_read_config_word(pci_dev, pci_dev->pm_cap + CIO2_PMCSR_OFFSET, &pm);
1885 	pm = (pm >> CIO2_PMCSR_D0D3_SHIFT) << CIO2_PMCSR_D0D3_SHIFT;
1886 	pci_write_config_word(pci_dev, pci_dev->pm_cap + CIO2_PMCSR_OFFSET, pm);
1887 
1888 	return 0;
1889 }
1890 
1891 /*
1892  * Helper function to advance all the elements of a circular buffer by "start"
1893  * positions
1894  */
1895 static void arrange(void *ptr, size_t elem_size, size_t elems, size_t start)
1896 {
1897 	struct {
1898 		size_t begin, end;
1899 	} arr[2] = {
1900 		{ 0, start - 1 },
1901 		{ start, elems - 1 },
1902 	};
1903 
1904 #define CHUNK_SIZE(a) ((a)->end - (a)->begin + 1)
1905 
1906 	/* Loop as long as we have out-of-place entries */
1907 	while (CHUNK_SIZE(&arr[0]) && CHUNK_SIZE(&arr[1])) {
1908 		size_t size0, i;
1909 
1910 		/*
1911 		 * Find the number of entries that can be arranged on this
1912 		 * iteration.
1913 		 */
1914 		size0 = min(CHUNK_SIZE(&arr[0]), CHUNK_SIZE(&arr[1]));
1915 
1916 		/* Swap the entries in two parts of the array. */
1917 		for (i = 0; i < size0; i++) {
1918 			u8 *d = ptr + elem_size * (arr[1].begin + i);
1919 			u8 *s = ptr + elem_size * (arr[0].begin + i);
1920 			size_t j;
1921 
1922 			for (j = 0; j < elem_size; j++)
1923 				swap(d[j], s[j]);
1924 		}
1925 
1926 		if (CHUNK_SIZE(&arr[0]) > CHUNK_SIZE(&arr[1])) {
1927 			/* The end of the first array remains unarranged. */
1928 			arr[0].begin += size0;
1929 		} else {
1930 			/*
1931 			 * The first array is fully arranged so we proceed
1932 			 * handling the next one.
1933 			 */
1934 			arr[0].begin = arr[1].begin;
1935 			arr[0].end = arr[1].begin + size0 - 1;
1936 			arr[1].begin += size0;
1937 		}
1938 	}
1939 }
1940 
1941 static void cio2_fbpt_rearrange(struct cio2_device *cio2, struct cio2_queue *q)
1942 {
1943 	unsigned int i, j;
1944 
1945 	for (i = 0, j = q->bufs_first; i < CIO2_MAX_BUFFERS;
1946 		i++, j = (j + 1) % CIO2_MAX_BUFFERS)
1947 		if (q->bufs[j])
1948 			break;
1949 
1950 	if (i == CIO2_MAX_BUFFERS)
1951 		return;
1952 
1953 	if (j) {
1954 		arrange(q->fbpt, sizeof(struct cio2_fbpt_entry) * CIO2_MAX_LOPS,
1955 			CIO2_MAX_BUFFERS, j);
1956 		arrange(q->bufs, sizeof(struct cio2_buffer *),
1957 			CIO2_MAX_BUFFERS, j);
1958 	}
1959 
1960 	/*
1961 	 * DMA clears the valid bit when accessing the buffer.
1962 	 * When stopping stream in suspend callback, some of the buffers
1963 	 * may be in invalid state. After resume, when DMA meets the invalid
1964 	 * buffer, it will halt and stop receiving new data.
1965 	 * To avoid DMA halting, set the valid bit for all buffers in FBPT.
1966 	 */
1967 	for (i = 0; i < CIO2_MAX_BUFFERS; i++)
1968 		cio2_fbpt_entry_enable(cio2, q->fbpt + i * CIO2_MAX_LOPS);
1969 }
1970 
1971 static int __maybe_unused cio2_suspend(struct device *dev)
1972 {
1973 	struct pci_dev *pci_dev = to_pci_dev(dev);
1974 	struct cio2_device *cio2 = pci_get_drvdata(pci_dev);
1975 	struct cio2_queue *q = cio2->cur_queue;
1976 	int r;
1977 
1978 	dev_dbg(dev, "cio2 suspend\n");
1979 	if (!cio2->streaming)
1980 		return 0;
1981 
1982 	/* Stop stream */
1983 	r = v4l2_subdev_call(q->sensor, video, s_stream, 0);
1984 	if (r) {
1985 		dev_err(dev, "failed to stop sensor streaming\n");
1986 		return r;
1987 	}
1988 
1989 	cio2_hw_exit(cio2, q);
1990 	synchronize_irq(pci_dev->irq);
1991 
1992 	pm_runtime_force_suspend(dev);
1993 
1994 	/*
1995 	 * Upon resume, hw starts to process the fbpt entries from beginning,
1996 	 * so relocate the queued buffs to the fbpt head before suspend.
1997 	 */
1998 	cio2_fbpt_rearrange(cio2, q);
1999 	q->bufs_first = 0;
2000 	q->bufs_next = 0;
2001 
2002 	return 0;
2003 }
2004 
2005 static int __maybe_unused cio2_resume(struct device *dev)
2006 {
2007 	struct cio2_device *cio2 = dev_get_drvdata(dev);
2008 	struct cio2_queue *q = cio2->cur_queue;
2009 	int r;
2010 
2011 	dev_dbg(dev, "cio2 resume\n");
2012 	if (!cio2->streaming)
2013 		return 0;
2014 	/* Start stream */
2015 	r = pm_runtime_force_resume(dev);
2016 	if (r < 0) {
2017 		dev_err(dev, "failed to set power %d\n", r);
2018 		return r;
2019 	}
2020 
2021 	r = cio2_hw_init(cio2, q);
2022 	if (r) {
2023 		dev_err(dev, "fail to init cio2 hw\n");
2024 		return r;
2025 	}
2026 
2027 	r = v4l2_subdev_call(q->sensor, video, s_stream, 1);
2028 	if (r) {
2029 		dev_err(dev, "fail to start sensor streaming\n");
2030 		cio2_hw_exit(cio2, q);
2031 	}
2032 
2033 	return r;
2034 }
2035 
2036 static const struct dev_pm_ops cio2_pm_ops = {
2037 	SET_RUNTIME_PM_OPS(&cio2_runtime_suspend, &cio2_runtime_resume, NULL)
2038 	SET_SYSTEM_SLEEP_PM_OPS(&cio2_suspend, &cio2_resume)
2039 };
2040 
2041 static const struct pci_device_id cio2_pci_id_table[] = {
2042 	{ PCI_DEVICE(PCI_VENDOR_ID_INTEL, CIO2_PCI_ID) },
2043 	{ }
2044 };
2045 
2046 MODULE_DEVICE_TABLE(pci, cio2_pci_id_table);
2047 
2048 static struct pci_driver cio2_pci_driver = {
2049 	.name = CIO2_NAME,
2050 	.id_table = cio2_pci_id_table,
2051 	.probe = cio2_pci_probe,
2052 	.remove = cio2_pci_remove,
2053 	.driver = {
2054 		.pm = &cio2_pm_ops,
2055 	},
2056 };
2057 
2058 module_pci_driver(cio2_pci_driver);
2059 
2060 MODULE_AUTHOR("Tuukka Toivonen <tuukka.toivonen@intel.com>");
2061 MODULE_AUTHOR("Tianshu Qiu <tian.shu.qiu@intel.com>");
2062 MODULE_AUTHOR("Jian Xu Zheng");
2063 MODULE_AUTHOR("Yuning Pu <yuning.pu@intel.com>");
2064 MODULE_AUTHOR("Yong Zhi <yong.zhi@intel.com>");
2065 MODULE_LICENSE("GPL v2");
2066 MODULE_DESCRIPTION("IPU3 CIO2 driver");
2067 MODULE_IMPORT_NS(INTEL_IPU_BRIDGE);
2068