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