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