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