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