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