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