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 irqreturn_t cio2_irq(int irq, void *cio2_ptr) 644 { 645 struct cio2_device *cio2 = cio2_ptr; 646 void __iomem *const base = cio2->base; 647 struct device *dev = &cio2->pci_dev->dev; 648 u32 int_status, int_clear; 649 650 int_status = readl(base + CIO2_REG_INT_STS); 651 int_clear = int_status; 652 653 if (!int_status) 654 return IRQ_NONE; 655 656 if (int_status & CIO2_INT_IOOE) { 657 /* 658 * Interrupt on Output Error: 659 * 1) SRAM is full and FS received, or 660 * 2) An invalid bit detected by DMA. 661 */ 662 u32 oe_status, oe_clear; 663 664 oe_clear = readl(base + CIO2_REG_INT_STS_EXT_OE); 665 oe_status = oe_clear; 666 667 if (oe_status & CIO2_INT_EXT_OE_DMAOE_MASK) { 668 dev_err(dev, "DMA output error: 0x%x\n", 669 (oe_status & CIO2_INT_EXT_OE_DMAOE_MASK) 670 >> CIO2_INT_EXT_OE_DMAOE_SHIFT); 671 oe_status &= ~CIO2_INT_EXT_OE_DMAOE_MASK; 672 } 673 if (oe_status & CIO2_INT_EXT_OE_OES_MASK) { 674 dev_err(dev, "DMA output error on CSI2 buses: 0x%x\n", 675 (oe_status & CIO2_INT_EXT_OE_OES_MASK) 676 >> CIO2_INT_EXT_OE_OES_SHIFT); 677 oe_status &= ~CIO2_INT_EXT_OE_OES_MASK; 678 } 679 writel(oe_clear, base + CIO2_REG_INT_STS_EXT_OE); 680 if (oe_status) 681 dev_warn(dev, "unknown interrupt 0x%x on OE\n", 682 oe_status); 683 int_status &= ~CIO2_INT_IOOE; 684 } 685 686 if (int_status & CIO2_INT_IOC_MASK) { 687 /* DMA IO done -- frame ready */ 688 u32 clr = 0; 689 unsigned int d; 690 691 for (d = 0; d < CIO2_NUM_DMA_CHAN; d++) 692 if (int_status & CIO2_INT_IOC(d)) { 693 clr |= CIO2_INT_IOC(d); 694 cio2_buffer_done(cio2, d); 695 } 696 int_status &= ~clr; 697 } 698 699 if (int_status & CIO2_INT_IOS_IOLN_MASK) { 700 /* DMA IO starts or reached specified line */ 701 u32 clr = 0; 702 unsigned int d; 703 704 for (d = 0; d < CIO2_NUM_DMA_CHAN; d++) 705 if (int_status & CIO2_INT_IOS_IOLN(d)) { 706 clr |= CIO2_INT_IOS_IOLN(d); 707 if (d == CIO2_DMA_CHAN) 708 cio2_queue_event_sof(cio2, 709 cio2->cur_queue); 710 } 711 int_status &= ~clr; 712 } 713 714 if (int_status & (CIO2_INT_IOIE | CIO2_INT_IOIRQ)) { 715 /* CSI2 receiver (error) interrupt */ 716 u32 ie_status, ie_clear; 717 unsigned int port; 718 719 ie_clear = readl(base + CIO2_REG_INT_STS_EXT_IE); 720 ie_status = ie_clear; 721 722 for (port = 0; port < CIO2_NUM_PORTS; port++) { 723 u32 port_status = (ie_status >> (port * 8)) & 0xff; 724 u32 err_mask = BIT_MASK(ARRAY_SIZE(cio2_port_errs)) - 1; 725 void __iomem *const csi_rx_base = 726 base + CIO2_REG_PIPE_BASE(port); 727 unsigned int i; 728 729 while (port_status & err_mask) { 730 i = ffs(port_status) - 1; 731 dev_err(dev, "port %i error %s\n", 732 port, cio2_port_errs[i]); 733 ie_status &= ~BIT(port * 8 + i); 734 port_status &= ~BIT(i); 735 } 736 737 if (ie_status & CIO2_INT_EXT_IE_IRQ(port)) { 738 u32 csi2_status, csi2_clear; 739 740 csi2_status = readl(csi_rx_base + 741 CIO2_REG_IRQCTRL_STATUS); 742 csi2_clear = csi2_status; 743 err_mask = 744 BIT_MASK(ARRAY_SIZE(cio2_irq_errs)) - 1; 745 746 while (csi2_status & err_mask) { 747 i = ffs(csi2_status) - 1; 748 dev_err(dev, 749 "CSI-2 receiver port %i: %s\n", 750 port, cio2_irq_errs[i]); 751 csi2_status &= ~BIT(i); 752 } 753 754 writel(csi2_clear, 755 csi_rx_base + CIO2_REG_IRQCTRL_CLEAR); 756 if (csi2_status) 757 dev_warn(dev, 758 "unknown CSI2 error 0x%x on port %i\n", 759 csi2_status, port); 760 761 ie_status &= ~CIO2_INT_EXT_IE_IRQ(port); 762 } 763 } 764 765 writel(ie_clear, base + CIO2_REG_INT_STS_EXT_IE); 766 if (ie_status) 767 dev_warn(dev, "unknown interrupt 0x%x on IE\n", 768 ie_status); 769 770 int_status &= ~(CIO2_INT_IOIE | CIO2_INT_IOIRQ); 771 } 772 773 writel(int_clear, base + CIO2_REG_INT_STS); 774 if (int_status) 775 dev_warn(dev, "unknown interrupt 0x%x on INT\n", int_status); 776 777 return IRQ_HANDLED; 778 } 779 780 /**************** Videobuf2 interface ****************/ 781 782 static void cio2_vb2_return_all_buffers(struct cio2_queue *q, 783 enum vb2_buffer_state state) 784 { 785 unsigned int i; 786 787 for (i = 0; i < CIO2_MAX_BUFFERS; i++) { 788 if (q->bufs[i]) { 789 atomic_dec(&q->bufs_queued); 790 vb2_buffer_done(&q->bufs[i]->vbb.vb2_buf, 791 state); 792 } 793 } 794 } 795 796 static int cio2_vb2_queue_setup(struct vb2_queue *vq, 797 unsigned int *num_buffers, 798 unsigned int *num_planes, 799 unsigned int sizes[], 800 struct device *alloc_devs[]) 801 { 802 struct cio2_device *cio2 = vb2_get_drv_priv(vq); 803 struct cio2_queue *q = vb2q_to_cio2_queue(vq); 804 unsigned int i; 805 806 *num_planes = q->format.num_planes; 807 808 for (i = 0; i < *num_planes; ++i) { 809 sizes[i] = q->format.plane_fmt[i].sizeimage; 810 alloc_devs[i] = &cio2->pci_dev->dev; 811 } 812 813 *num_buffers = clamp_val(*num_buffers, 1, CIO2_MAX_BUFFERS); 814 815 /* Initialize buffer queue */ 816 for (i = 0; i < CIO2_MAX_BUFFERS; i++) { 817 q->bufs[i] = NULL; 818 cio2_fbpt_entry_init_dummy(cio2, &q->fbpt[i * CIO2_MAX_LOPS]); 819 } 820 atomic_set(&q->bufs_queued, 0); 821 q->bufs_first = 0; 822 q->bufs_next = 0; 823 824 return 0; 825 } 826 827 /* Called after each buffer is allocated */ 828 static int cio2_vb2_buf_init(struct vb2_buffer *vb) 829 { 830 struct cio2_device *cio2 = vb2_get_drv_priv(vb->vb2_queue); 831 struct device *dev = &cio2->pci_dev->dev; 832 struct cio2_buffer *b = 833 container_of(vb, struct cio2_buffer, vbb.vb2_buf); 834 static const unsigned int entries_per_page = 835 CIO2_PAGE_SIZE / sizeof(u32); 836 unsigned int pages = DIV_ROUND_UP(vb->planes[0].length, CIO2_PAGE_SIZE); 837 unsigned int lops = DIV_ROUND_UP(pages + 1, entries_per_page); 838 struct sg_table *sg; 839 struct sg_page_iter sg_iter; 840 int i, j; 841 842 if (lops <= 0 || lops > CIO2_MAX_LOPS) { 843 dev_err(dev, "%s: bad buffer size (%i)\n", __func__, 844 vb->planes[0].length); 845 return -ENOSPC; /* Should never happen */ 846 } 847 848 memset(b->lop, 0, sizeof(b->lop)); 849 /* Allocate LOP table */ 850 for (i = 0; i < lops; i++) { 851 b->lop[i] = dma_alloc_coherent(dev, CIO2_PAGE_SIZE, 852 &b->lop_bus_addr[i], GFP_KERNEL); 853 if (!b->lop[i]) 854 goto fail; 855 } 856 857 /* Fill LOP */ 858 sg = vb2_dma_sg_plane_desc(vb, 0); 859 if (!sg) 860 return -ENOMEM; 861 862 if (sg->nents && sg->sgl) 863 b->offset = sg->sgl->offset; 864 865 i = j = 0; 866 for_each_sg_page(sg->sgl, &sg_iter, sg->nents, 0) { 867 if (!pages--) 868 break; 869 b->lop[i][j] = sg_page_iter_dma_address(&sg_iter) >> PAGE_SHIFT; 870 j++; 871 if (j == entries_per_page) { 872 i++; 873 j = 0; 874 } 875 } 876 877 b->lop[i][j] = cio2->dummy_page_bus_addr >> PAGE_SHIFT; 878 return 0; 879 fail: 880 for (i--; i >= 0; i--) 881 dma_free_coherent(dev, CIO2_PAGE_SIZE, 882 b->lop[i], b->lop_bus_addr[i]); 883 return -ENOMEM; 884 } 885 886 /* Transfer buffer ownership to cio2 */ 887 static void cio2_vb2_buf_queue(struct vb2_buffer *vb) 888 { 889 struct cio2_device *cio2 = vb2_get_drv_priv(vb->vb2_queue); 890 struct cio2_queue *q = 891 container_of(vb->vb2_queue, struct cio2_queue, vbq); 892 struct cio2_buffer *b = 893 container_of(vb, struct cio2_buffer, vbb.vb2_buf); 894 struct cio2_fbpt_entry *entry; 895 unsigned long flags; 896 unsigned int i, j, next = q->bufs_next; 897 int bufs_queued = atomic_inc_return(&q->bufs_queued); 898 u32 fbpt_rp; 899 900 dev_dbg(&cio2->pci_dev->dev, "queue buffer %d\n", vb->index); 901 902 /* 903 * This code queues the buffer to the CIO2 DMA engine, which starts 904 * running once streaming has started. It is possible that this code 905 * gets pre-empted due to increased CPU load. Upon this, the driver 906 * does not get an opportunity to queue new buffers to the CIO2 DMA 907 * engine. When the DMA engine encounters an FBPT entry without the 908 * VALID bit set, the DMA engine halts, which requires a restart of 909 * the DMA engine and sensor, to continue streaming. 910 * This is not desired and is highly unlikely given that there are 911 * 32 FBPT entries that the DMA engine needs to process, to run into 912 * an FBPT entry, without the VALID bit set. We try to mitigate this 913 * by disabling interrupts for the duration of this queueing. 914 */ 915 local_irq_save(flags); 916 917 fbpt_rp = (readl(cio2->base + CIO2_REG_CDMARI(CIO2_DMA_CHAN)) 918 >> CIO2_CDMARI_FBPT_RP_SHIFT) 919 & CIO2_CDMARI_FBPT_RP_MASK; 920 921 /* 922 * fbpt_rp is the fbpt entry that the dma is currently working 923 * on, but since it could jump to next entry at any time, 924 * assume that we might already be there. 925 */ 926 fbpt_rp = (fbpt_rp + 1) % CIO2_MAX_BUFFERS; 927 928 if (bufs_queued <= 1 || fbpt_rp == next) 929 /* Buffers were drained */ 930 next = (fbpt_rp + 1) % CIO2_MAX_BUFFERS; 931 932 for (i = 0; i < CIO2_MAX_BUFFERS; i++) { 933 /* 934 * We have allocated CIO2_MAX_BUFFERS circularly for the 935 * hw, the user has requested N buffer queue. The driver 936 * ensures N <= CIO2_MAX_BUFFERS and guarantees that whenever 937 * user queues a buffer, there necessarily is a free buffer. 938 */ 939 if (!q->bufs[next]) { 940 q->bufs[next] = b; 941 entry = &q->fbpt[next * CIO2_MAX_LOPS]; 942 cio2_fbpt_entry_init_buf(cio2, b, entry); 943 local_irq_restore(flags); 944 q->bufs_next = (next + 1) % CIO2_MAX_BUFFERS; 945 for (j = 0; j < vb->num_planes; j++) 946 vb2_set_plane_payload(vb, j, 947 q->format.plane_fmt[j].sizeimage); 948 return; 949 } 950 951 dev_dbg(&cio2->pci_dev->dev, "entry %i was full!\n", next); 952 next = (next + 1) % CIO2_MAX_BUFFERS; 953 } 954 955 local_irq_restore(flags); 956 dev_err(&cio2->pci_dev->dev, "error: all cio2 entries were full!\n"); 957 atomic_dec(&q->bufs_queued); 958 vb2_buffer_done(vb, VB2_BUF_STATE_ERROR); 959 } 960 961 /* Called when each buffer is freed */ 962 static void cio2_vb2_buf_cleanup(struct vb2_buffer *vb) 963 { 964 struct cio2_device *cio2 = vb2_get_drv_priv(vb->vb2_queue); 965 struct cio2_buffer *b = 966 container_of(vb, struct cio2_buffer, vbb.vb2_buf); 967 unsigned int i; 968 969 /* Free LOP table */ 970 for (i = 0; i < CIO2_MAX_LOPS; i++) { 971 if (b->lop[i]) 972 dma_free_coherent(&cio2->pci_dev->dev, CIO2_PAGE_SIZE, 973 b->lop[i], b->lop_bus_addr[i]); 974 } 975 } 976 977 static int cio2_vb2_start_streaming(struct vb2_queue *vq, unsigned int count) 978 { 979 struct cio2_queue *q = vb2q_to_cio2_queue(vq); 980 struct cio2_device *cio2 = vb2_get_drv_priv(vq); 981 int r; 982 983 cio2->cur_queue = q; 984 atomic_set(&q->frame_sequence, 0); 985 986 r = pm_runtime_get_sync(&cio2->pci_dev->dev); 987 if (r < 0) { 988 dev_info(&cio2->pci_dev->dev, "failed to set power %d\n", r); 989 pm_runtime_put_noidle(&cio2->pci_dev->dev); 990 return r; 991 } 992 993 r = media_pipeline_start(&q->vdev.entity, &q->pipe); 994 if (r) 995 goto fail_pipeline; 996 997 r = cio2_hw_init(cio2, q); 998 if (r) 999 goto fail_hw; 1000 1001 /* Start streaming on sensor */ 1002 r = v4l2_subdev_call(q->sensor, video, s_stream, 1); 1003 if (r) 1004 goto fail_csi2_subdev; 1005 1006 cio2->streaming = true; 1007 1008 return 0; 1009 1010 fail_csi2_subdev: 1011 cio2_hw_exit(cio2, q); 1012 fail_hw: 1013 media_pipeline_stop(&q->vdev.entity); 1014 fail_pipeline: 1015 dev_dbg(&cio2->pci_dev->dev, "failed to start streaming (%d)\n", r); 1016 cio2_vb2_return_all_buffers(q, VB2_BUF_STATE_QUEUED); 1017 pm_runtime_put(&cio2->pci_dev->dev); 1018 1019 return r; 1020 } 1021 1022 static void cio2_vb2_stop_streaming(struct vb2_queue *vq) 1023 { 1024 struct cio2_queue *q = vb2q_to_cio2_queue(vq); 1025 struct cio2_device *cio2 = vb2_get_drv_priv(vq); 1026 1027 if (v4l2_subdev_call(q->sensor, video, s_stream, 0)) 1028 dev_err(&cio2->pci_dev->dev, 1029 "failed to stop sensor streaming\n"); 1030 1031 cio2_hw_exit(cio2, q); 1032 synchronize_irq(cio2->pci_dev->irq); 1033 cio2_vb2_return_all_buffers(q, VB2_BUF_STATE_ERROR); 1034 media_pipeline_stop(&q->vdev.entity); 1035 pm_runtime_put(&cio2->pci_dev->dev); 1036 cio2->streaming = false; 1037 } 1038 1039 static const struct vb2_ops cio2_vb2_ops = { 1040 .buf_init = cio2_vb2_buf_init, 1041 .buf_queue = cio2_vb2_buf_queue, 1042 .buf_cleanup = cio2_vb2_buf_cleanup, 1043 .queue_setup = cio2_vb2_queue_setup, 1044 .start_streaming = cio2_vb2_start_streaming, 1045 .stop_streaming = cio2_vb2_stop_streaming, 1046 .wait_prepare = vb2_ops_wait_prepare, 1047 .wait_finish = vb2_ops_wait_finish, 1048 }; 1049 1050 /**************** V4L2 interface ****************/ 1051 1052 static int cio2_v4l2_querycap(struct file *file, void *fh, 1053 struct v4l2_capability *cap) 1054 { 1055 struct cio2_device *cio2 = video_drvdata(file); 1056 1057 strlcpy(cap->driver, CIO2_NAME, sizeof(cap->driver)); 1058 strlcpy(cap->card, CIO2_DEVICE_NAME, sizeof(cap->card)); 1059 snprintf(cap->bus_info, sizeof(cap->bus_info), 1060 "PCI:%s", pci_name(cio2->pci_dev)); 1061 1062 return 0; 1063 } 1064 1065 static int cio2_v4l2_enum_fmt(struct file *file, void *fh, 1066 struct v4l2_fmtdesc *f) 1067 { 1068 if (f->index >= ARRAY_SIZE(formats)) 1069 return -EINVAL; 1070 1071 f->pixelformat = formats[f->index].fourcc; 1072 1073 return 0; 1074 } 1075 1076 /* The format is validated in cio2_video_link_validate() */ 1077 static int cio2_v4l2_g_fmt(struct file *file, void *fh, struct v4l2_format *f) 1078 { 1079 struct cio2_queue *q = file_to_cio2_queue(file); 1080 1081 f->fmt.pix_mp = q->format; 1082 1083 return 0; 1084 } 1085 1086 static int cio2_v4l2_try_fmt(struct file *file, void *fh, struct v4l2_format *f) 1087 { 1088 const struct ipu3_cio2_fmt *fmt; 1089 struct v4l2_pix_format_mplane *mpix = &f->fmt.pix_mp; 1090 1091 fmt = cio2_find_format(&mpix->pixelformat, NULL); 1092 if (!fmt) 1093 fmt = &formats[0]; 1094 1095 /* Only supports up to 4224x3136 */ 1096 if (mpix->width > CIO2_IMAGE_MAX_WIDTH) 1097 mpix->width = CIO2_IMAGE_MAX_WIDTH; 1098 if (mpix->height > CIO2_IMAGE_MAX_LENGTH) 1099 mpix->height = CIO2_IMAGE_MAX_LENGTH; 1100 1101 mpix->num_planes = 1; 1102 mpix->pixelformat = fmt->fourcc; 1103 mpix->colorspace = V4L2_COLORSPACE_RAW; 1104 mpix->field = V4L2_FIELD_NONE; 1105 memset(mpix->reserved, 0, sizeof(mpix->reserved)); 1106 mpix->plane_fmt[0].bytesperline = cio2_bytesperline(mpix->width); 1107 mpix->plane_fmt[0].sizeimage = mpix->plane_fmt[0].bytesperline * 1108 mpix->height; 1109 memset(mpix->plane_fmt[0].reserved, 0, 1110 sizeof(mpix->plane_fmt[0].reserved)); 1111 1112 /* use default */ 1113 mpix->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT; 1114 mpix->quantization = V4L2_QUANTIZATION_DEFAULT; 1115 mpix->xfer_func = V4L2_XFER_FUNC_DEFAULT; 1116 1117 return 0; 1118 } 1119 1120 static int cio2_v4l2_s_fmt(struct file *file, void *fh, struct v4l2_format *f) 1121 { 1122 struct cio2_queue *q = file_to_cio2_queue(file); 1123 1124 cio2_v4l2_try_fmt(file, fh, f); 1125 q->format = f->fmt.pix_mp; 1126 1127 return 0; 1128 } 1129 1130 static int 1131 cio2_video_enum_input(struct file *file, void *fh, struct v4l2_input *input) 1132 { 1133 if (input->index > 0) 1134 return -EINVAL; 1135 1136 strlcpy(input->name, "camera", sizeof(input->name)); 1137 input->type = V4L2_INPUT_TYPE_CAMERA; 1138 1139 return 0; 1140 } 1141 1142 static int 1143 cio2_video_g_input(struct file *file, void *fh, unsigned int *input) 1144 { 1145 *input = 0; 1146 1147 return 0; 1148 } 1149 1150 static int 1151 cio2_video_s_input(struct file *file, void *fh, unsigned int input) 1152 { 1153 return input == 0 ? 0 : -EINVAL; 1154 } 1155 1156 static const struct v4l2_file_operations cio2_v4l2_fops = { 1157 .owner = THIS_MODULE, 1158 .unlocked_ioctl = video_ioctl2, 1159 .open = v4l2_fh_open, 1160 .release = vb2_fop_release, 1161 .poll = vb2_fop_poll, 1162 .mmap = vb2_fop_mmap, 1163 }; 1164 1165 static const struct v4l2_ioctl_ops cio2_v4l2_ioctl_ops = { 1166 .vidioc_querycap = cio2_v4l2_querycap, 1167 .vidioc_enum_fmt_vid_cap_mplane = cio2_v4l2_enum_fmt, 1168 .vidioc_g_fmt_vid_cap_mplane = cio2_v4l2_g_fmt, 1169 .vidioc_s_fmt_vid_cap_mplane = cio2_v4l2_s_fmt, 1170 .vidioc_try_fmt_vid_cap_mplane = cio2_v4l2_try_fmt, 1171 .vidioc_reqbufs = vb2_ioctl_reqbufs, 1172 .vidioc_create_bufs = vb2_ioctl_create_bufs, 1173 .vidioc_prepare_buf = vb2_ioctl_prepare_buf, 1174 .vidioc_querybuf = vb2_ioctl_querybuf, 1175 .vidioc_qbuf = vb2_ioctl_qbuf, 1176 .vidioc_dqbuf = vb2_ioctl_dqbuf, 1177 .vidioc_streamon = vb2_ioctl_streamon, 1178 .vidioc_streamoff = vb2_ioctl_streamoff, 1179 .vidioc_expbuf = vb2_ioctl_expbuf, 1180 .vidioc_enum_input = cio2_video_enum_input, 1181 .vidioc_g_input = cio2_video_g_input, 1182 .vidioc_s_input = cio2_video_s_input, 1183 }; 1184 1185 static int cio2_subdev_subscribe_event(struct v4l2_subdev *sd, 1186 struct v4l2_fh *fh, 1187 struct v4l2_event_subscription *sub) 1188 { 1189 if (sub->type != V4L2_EVENT_FRAME_SYNC) 1190 return -EINVAL; 1191 1192 /* Line number. For now only zero accepted. */ 1193 if (sub->id != 0) 1194 return -EINVAL; 1195 1196 return v4l2_event_subscribe(fh, sub, 0, NULL); 1197 } 1198 1199 static int cio2_subdev_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh) 1200 { 1201 struct v4l2_mbus_framefmt *format; 1202 const struct v4l2_mbus_framefmt fmt_default = { 1203 .width = 1936, 1204 .height = 1096, 1205 .code = formats[0].mbus_code, 1206 .field = V4L2_FIELD_NONE, 1207 .colorspace = V4L2_COLORSPACE_RAW, 1208 .ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT, 1209 .quantization = V4L2_QUANTIZATION_DEFAULT, 1210 .xfer_func = V4L2_XFER_FUNC_DEFAULT, 1211 }; 1212 1213 /* Initialize try_fmt */ 1214 format = v4l2_subdev_get_try_format(sd, fh->pad, CIO2_PAD_SINK); 1215 *format = fmt_default; 1216 1217 /* same as sink */ 1218 format = v4l2_subdev_get_try_format(sd, fh->pad, CIO2_PAD_SOURCE); 1219 *format = fmt_default; 1220 1221 return 0; 1222 } 1223 1224 /* 1225 * cio2_subdev_get_fmt - Handle get format by pads subdev method 1226 * @sd : pointer to v4l2 subdev structure 1227 * @cfg: V4L2 subdev pad config 1228 * @fmt: pointer to v4l2 subdev format structure 1229 * return -EINVAL or zero on success 1230 */ 1231 static int cio2_subdev_get_fmt(struct v4l2_subdev *sd, 1232 struct v4l2_subdev_pad_config *cfg, 1233 struct v4l2_subdev_format *fmt) 1234 { 1235 struct cio2_queue *q = container_of(sd, struct cio2_queue, subdev); 1236 struct v4l2_subdev_format format; 1237 int ret; 1238 1239 if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) { 1240 fmt->format = *v4l2_subdev_get_try_format(sd, cfg, fmt->pad); 1241 return 0; 1242 } 1243 1244 if (fmt->pad == CIO2_PAD_SINK) { 1245 format.which = V4L2_SUBDEV_FORMAT_ACTIVE; 1246 ret = v4l2_subdev_call(sd, pad, get_fmt, NULL, 1247 &format); 1248 1249 if (ret) 1250 return ret; 1251 /* update colorspace etc */ 1252 q->subdev_fmt.colorspace = format.format.colorspace; 1253 q->subdev_fmt.ycbcr_enc = format.format.ycbcr_enc; 1254 q->subdev_fmt.quantization = format.format.quantization; 1255 q->subdev_fmt.xfer_func = format.format.xfer_func; 1256 } 1257 1258 fmt->format = q->subdev_fmt; 1259 1260 return 0; 1261 } 1262 1263 /* 1264 * cio2_subdev_set_fmt - Handle set format by pads subdev method 1265 * @sd : pointer to v4l2 subdev structure 1266 * @cfg: V4L2 subdev pad config 1267 * @fmt: pointer to v4l2 subdev format structure 1268 * return -EINVAL or zero on success 1269 */ 1270 static int cio2_subdev_set_fmt(struct v4l2_subdev *sd, 1271 struct v4l2_subdev_pad_config *cfg, 1272 struct v4l2_subdev_format *fmt) 1273 { 1274 struct cio2_queue *q = container_of(sd, struct cio2_queue, subdev); 1275 1276 /* 1277 * Only allow setting sink pad format; 1278 * source always propagates from sink 1279 */ 1280 if (fmt->pad == CIO2_PAD_SOURCE) 1281 return cio2_subdev_get_fmt(sd, cfg, fmt); 1282 1283 if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) { 1284 *v4l2_subdev_get_try_format(sd, cfg, fmt->pad) = fmt->format; 1285 } else { 1286 /* It's the sink, allow changing frame size */ 1287 q->subdev_fmt.width = fmt->format.width; 1288 q->subdev_fmt.height = fmt->format.height; 1289 q->subdev_fmt.code = fmt->format.code; 1290 fmt->format = q->subdev_fmt; 1291 } 1292 1293 return 0; 1294 } 1295 1296 static int cio2_subdev_enum_mbus_code(struct v4l2_subdev *sd, 1297 struct v4l2_subdev_pad_config *cfg, 1298 struct v4l2_subdev_mbus_code_enum *code) 1299 { 1300 if (code->index >= ARRAY_SIZE(formats)) 1301 return -EINVAL; 1302 1303 code->code = formats[code->index].mbus_code; 1304 return 0; 1305 } 1306 1307 static int cio2_subdev_link_validate_get_format(struct media_pad *pad, 1308 struct v4l2_subdev_format *fmt) 1309 { 1310 if (is_media_entity_v4l2_subdev(pad->entity)) { 1311 struct v4l2_subdev *sd = 1312 media_entity_to_v4l2_subdev(pad->entity); 1313 1314 fmt->which = V4L2_SUBDEV_FORMAT_ACTIVE; 1315 fmt->pad = pad->index; 1316 return v4l2_subdev_call(sd, pad, get_fmt, NULL, fmt); 1317 } 1318 1319 return -EINVAL; 1320 } 1321 1322 static int cio2_video_link_validate(struct media_link *link) 1323 { 1324 struct video_device *vd = container_of(link->sink->entity, 1325 struct video_device, entity); 1326 struct cio2_queue *q = container_of(vd, struct cio2_queue, vdev); 1327 struct cio2_device *cio2 = video_get_drvdata(vd); 1328 struct v4l2_subdev_format source_fmt; 1329 int ret; 1330 1331 if (!media_entity_remote_pad(link->sink->entity->pads)) { 1332 dev_info(&cio2->pci_dev->dev, 1333 "video node %s pad not connected\n", vd->name); 1334 return -ENOTCONN; 1335 } 1336 1337 ret = cio2_subdev_link_validate_get_format(link->source, &source_fmt); 1338 if (ret < 0) 1339 return 0; 1340 1341 if (source_fmt.format.width != q->format.width || 1342 source_fmt.format.height != q->format.height) { 1343 dev_err(&cio2->pci_dev->dev, 1344 "Wrong width or height %ux%u (%ux%u expected)\n", 1345 q->format.width, q->format.height, 1346 source_fmt.format.width, source_fmt.format.height); 1347 return -EINVAL; 1348 } 1349 1350 if (!cio2_find_format(&q->format.pixelformat, &source_fmt.format.code)) 1351 return -EINVAL; 1352 1353 return 0; 1354 } 1355 1356 static const struct v4l2_subdev_core_ops cio2_subdev_core_ops = { 1357 .subscribe_event = cio2_subdev_subscribe_event, 1358 .unsubscribe_event = v4l2_event_subdev_unsubscribe, 1359 }; 1360 1361 static const struct v4l2_subdev_internal_ops cio2_subdev_internal_ops = { 1362 .open = cio2_subdev_open, 1363 }; 1364 1365 static const struct v4l2_subdev_pad_ops cio2_subdev_pad_ops = { 1366 .link_validate = v4l2_subdev_link_validate_default, 1367 .get_fmt = cio2_subdev_get_fmt, 1368 .set_fmt = cio2_subdev_set_fmt, 1369 .enum_mbus_code = cio2_subdev_enum_mbus_code, 1370 }; 1371 1372 static const struct v4l2_subdev_ops cio2_subdev_ops = { 1373 .core = &cio2_subdev_core_ops, 1374 .pad = &cio2_subdev_pad_ops, 1375 }; 1376 1377 /******* V4L2 sub-device asynchronous registration callbacks***********/ 1378 1379 struct sensor_async_subdev { 1380 struct v4l2_async_subdev asd; 1381 struct csi2_bus_info csi2; 1382 }; 1383 1384 /* The .bound() notifier callback when a match is found */ 1385 static int cio2_notifier_bound(struct v4l2_async_notifier *notifier, 1386 struct v4l2_subdev *sd, 1387 struct v4l2_async_subdev *asd) 1388 { 1389 struct cio2_device *cio2 = container_of(notifier, 1390 struct cio2_device, notifier); 1391 struct sensor_async_subdev *s_asd = container_of(asd, 1392 struct sensor_async_subdev, asd); 1393 struct cio2_queue *q; 1394 1395 if (cio2->queue[s_asd->csi2.port].sensor) 1396 return -EBUSY; 1397 1398 q = &cio2->queue[s_asd->csi2.port]; 1399 1400 q->csi2 = s_asd->csi2; 1401 q->sensor = sd; 1402 q->csi_rx_base = cio2->base + CIO2_REG_PIPE_BASE(q->csi2.port); 1403 1404 return 0; 1405 } 1406 1407 /* The .unbind callback */ 1408 static void cio2_notifier_unbind(struct v4l2_async_notifier *notifier, 1409 struct v4l2_subdev *sd, 1410 struct v4l2_async_subdev *asd) 1411 { 1412 struct cio2_device *cio2 = container_of(notifier, 1413 struct cio2_device, notifier); 1414 struct sensor_async_subdev *s_asd = container_of(asd, 1415 struct sensor_async_subdev, asd); 1416 1417 cio2->queue[s_asd->csi2.port].sensor = NULL; 1418 } 1419 1420 /* .complete() is called after all subdevices have been located */ 1421 static int cio2_notifier_complete(struct v4l2_async_notifier *notifier) 1422 { 1423 struct cio2_device *cio2 = container_of(notifier, struct cio2_device, 1424 notifier); 1425 struct sensor_async_subdev *s_asd; 1426 struct cio2_queue *q; 1427 unsigned int i, pad; 1428 int ret; 1429 1430 for (i = 0; i < notifier->num_subdevs; i++) { 1431 s_asd = container_of(cio2->notifier.subdevs[i], 1432 struct sensor_async_subdev, asd); 1433 q = &cio2->queue[s_asd->csi2.port]; 1434 1435 for (pad = 0; pad < q->sensor->entity.num_pads; pad++) 1436 if (q->sensor->entity.pads[pad].flags & 1437 MEDIA_PAD_FL_SOURCE) 1438 break; 1439 1440 if (pad == q->sensor->entity.num_pads) { 1441 dev_err(&cio2->pci_dev->dev, 1442 "failed to find src pad for %s\n", 1443 q->sensor->name); 1444 return -ENXIO; 1445 } 1446 1447 ret = media_create_pad_link( 1448 &q->sensor->entity, pad, 1449 &q->subdev.entity, CIO2_PAD_SINK, 1450 0); 1451 if (ret) { 1452 dev_err(&cio2->pci_dev->dev, 1453 "failed to create link for %s\n", 1454 cio2->queue[i].sensor->name); 1455 return ret; 1456 } 1457 } 1458 1459 return v4l2_device_register_subdev_nodes(&cio2->v4l2_dev); 1460 } 1461 1462 static const struct v4l2_async_notifier_operations cio2_async_ops = { 1463 .bound = cio2_notifier_bound, 1464 .unbind = cio2_notifier_unbind, 1465 .complete = cio2_notifier_complete, 1466 }; 1467 1468 static int cio2_fwnode_parse(struct device *dev, 1469 struct v4l2_fwnode_endpoint *vep, 1470 struct v4l2_async_subdev *asd) 1471 { 1472 struct sensor_async_subdev *s_asd = 1473 container_of(asd, struct sensor_async_subdev, asd); 1474 1475 if (vep->bus_type != V4L2_MBUS_CSI2) { 1476 dev_err(dev, "Only CSI2 bus type is currently supported\n"); 1477 return -EINVAL; 1478 } 1479 1480 s_asd->csi2.port = vep->base.port; 1481 s_asd->csi2.lanes = vep->bus.mipi_csi2.num_data_lanes; 1482 1483 return 0; 1484 } 1485 1486 static int cio2_notifier_init(struct cio2_device *cio2) 1487 { 1488 int ret; 1489 1490 ret = v4l2_async_notifier_parse_fwnode_endpoints( 1491 &cio2->pci_dev->dev, &cio2->notifier, 1492 sizeof(struct sensor_async_subdev), 1493 cio2_fwnode_parse); 1494 if (ret < 0) 1495 return ret; 1496 1497 if (!cio2->notifier.num_subdevs) 1498 return -ENODEV; /* no endpoint */ 1499 1500 cio2->notifier.ops = &cio2_async_ops; 1501 ret = v4l2_async_notifier_register(&cio2->v4l2_dev, &cio2->notifier); 1502 if (ret) { 1503 dev_err(&cio2->pci_dev->dev, 1504 "failed to register async notifier : %d\n", ret); 1505 v4l2_async_notifier_cleanup(&cio2->notifier); 1506 } 1507 1508 return ret; 1509 } 1510 1511 static void cio2_notifier_exit(struct cio2_device *cio2) 1512 { 1513 v4l2_async_notifier_unregister(&cio2->notifier); 1514 v4l2_async_notifier_cleanup(&cio2->notifier); 1515 } 1516 1517 /**************** Queue initialization ****************/ 1518 static const struct media_entity_operations cio2_media_ops = { 1519 .link_validate = v4l2_subdev_link_validate, 1520 }; 1521 1522 static const struct media_entity_operations cio2_video_entity_ops = { 1523 .link_validate = cio2_video_link_validate, 1524 }; 1525 1526 static int cio2_queue_init(struct cio2_device *cio2, struct cio2_queue *q) 1527 { 1528 static const u32 default_width = 1936; 1529 static const u32 default_height = 1096; 1530 const struct ipu3_cio2_fmt dflt_fmt = formats[0]; 1531 1532 struct video_device *vdev = &q->vdev; 1533 struct vb2_queue *vbq = &q->vbq; 1534 struct v4l2_subdev *subdev = &q->subdev; 1535 struct v4l2_mbus_framefmt *fmt; 1536 int r; 1537 1538 /* Initialize miscellaneous variables */ 1539 mutex_init(&q->lock); 1540 1541 /* Initialize formats to default values */ 1542 fmt = &q->subdev_fmt; 1543 fmt->width = default_width; 1544 fmt->height = default_height; 1545 fmt->code = dflt_fmt.mbus_code; 1546 fmt->field = V4L2_FIELD_NONE; 1547 1548 q->format.width = default_width; 1549 q->format.height = default_height; 1550 q->format.pixelformat = dflt_fmt.fourcc; 1551 q->format.colorspace = V4L2_COLORSPACE_RAW; 1552 q->format.field = V4L2_FIELD_NONE; 1553 q->format.num_planes = 1; 1554 q->format.plane_fmt[0].bytesperline = 1555 cio2_bytesperline(q->format.width); 1556 q->format.plane_fmt[0].sizeimage = q->format.plane_fmt[0].bytesperline * 1557 q->format.height; 1558 1559 /* Initialize fbpt */ 1560 r = cio2_fbpt_init(cio2, q); 1561 if (r) 1562 goto fail_fbpt; 1563 1564 /* Initialize media entities */ 1565 q->subdev_pads[CIO2_PAD_SINK].flags = MEDIA_PAD_FL_SINK | 1566 MEDIA_PAD_FL_MUST_CONNECT; 1567 q->subdev_pads[CIO2_PAD_SOURCE].flags = MEDIA_PAD_FL_SOURCE; 1568 subdev->entity.ops = &cio2_media_ops; 1569 subdev->internal_ops = &cio2_subdev_internal_ops; 1570 r = media_entity_pads_init(&subdev->entity, CIO2_PADS, q->subdev_pads); 1571 if (r) { 1572 dev_err(&cio2->pci_dev->dev, 1573 "failed initialize subdev media entity (%d)\n", r); 1574 goto fail_subdev_media_entity; 1575 } 1576 1577 q->vdev_pad.flags = MEDIA_PAD_FL_SINK | MEDIA_PAD_FL_MUST_CONNECT; 1578 vdev->entity.ops = &cio2_video_entity_ops; 1579 r = media_entity_pads_init(&vdev->entity, 1, &q->vdev_pad); 1580 if (r) { 1581 dev_err(&cio2->pci_dev->dev, 1582 "failed initialize videodev media entity (%d)\n", r); 1583 goto fail_vdev_media_entity; 1584 } 1585 1586 /* Initialize subdev */ 1587 v4l2_subdev_init(subdev, &cio2_subdev_ops); 1588 subdev->flags = V4L2_SUBDEV_FL_HAS_DEVNODE | V4L2_SUBDEV_FL_HAS_EVENTS; 1589 subdev->owner = THIS_MODULE; 1590 snprintf(subdev->name, sizeof(subdev->name), 1591 CIO2_ENTITY_NAME " %td", q - cio2->queue); 1592 v4l2_set_subdevdata(subdev, cio2); 1593 r = v4l2_device_register_subdev(&cio2->v4l2_dev, subdev); 1594 if (r) { 1595 dev_err(&cio2->pci_dev->dev, 1596 "failed initialize subdev (%d)\n", r); 1597 goto fail_subdev; 1598 } 1599 1600 /* Initialize vbq */ 1601 vbq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE; 1602 vbq->io_modes = VB2_USERPTR | VB2_MMAP | VB2_DMABUF; 1603 vbq->ops = &cio2_vb2_ops; 1604 vbq->mem_ops = &vb2_dma_sg_memops; 1605 vbq->buf_struct_size = sizeof(struct cio2_buffer); 1606 vbq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC; 1607 vbq->min_buffers_needed = 1; 1608 vbq->drv_priv = cio2; 1609 vbq->lock = &q->lock; 1610 r = vb2_queue_init(vbq); 1611 if (r) { 1612 dev_err(&cio2->pci_dev->dev, 1613 "failed to initialize videobuf2 queue (%d)\n", r); 1614 goto fail_vbq; 1615 } 1616 1617 /* Initialize vdev */ 1618 snprintf(vdev->name, sizeof(vdev->name), 1619 "%s %td", CIO2_NAME, q - cio2->queue); 1620 vdev->release = video_device_release_empty; 1621 vdev->fops = &cio2_v4l2_fops; 1622 vdev->ioctl_ops = &cio2_v4l2_ioctl_ops; 1623 vdev->lock = &cio2->lock; 1624 vdev->v4l2_dev = &cio2->v4l2_dev; 1625 vdev->queue = &q->vbq; 1626 vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE_MPLANE | V4L2_CAP_STREAMING; 1627 video_set_drvdata(vdev, cio2); 1628 r = video_register_device(vdev, VFL_TYPE_GRABBER, -1); 1629 if (r) { 1630 dev_err(&cio2->pci_dev->dev, 1631 "failed to register video device (%d)\n", r); 1632 goto fail_vdev; 1633 } 1634 1635 /* Create link from CIO2 subdev to output node */ 1636 r = media_create_pad_link( 1637 &subdev->entity, CIO2_PAD_SOURCE, &vdev->entity, 0, 1638 MEDIA_LNK_FL_ENABLED | MEDIA_LNK_FL_IMMUTABLE); 1639 if (r) 1640 goto fail_link; 1641 1642 return 0; 1643 1644 fail_link: 1645 video_unregister_device(&q->vdev); 1646 fail_vdev: 1647 vb2_queue_release(vbq); 1648 fail_vbq: 1649 v4l2_device_unregister_subdev(subdev); 1650 fail_subdev: 1651 media_entity_cleanup(&vdev->entity); 1652 fail_vdev_media_entity: 1653 media_entity_cleanup(&subdev->entity); 1654 fail_subdev_media_entity: 1655 cio2_fbpt_exit(q, &cio2->pci_dev->dev); 1656 fail_fbpt: 1657 mutex_destroy(&q->lock); 1658 1659 return r; 1660 } 1661 1662 static void cio2_queue_exit(struct cio2_device *cio2, struct cio2_queue *q) 1663 { 1664 video_unregister_device(&q->vdev); 1665 media_entity_cleanup(&q->vdev.entity); 1666 vb2_queue_release(&q->vbq); 1667 v4l2_device_unregister_subdev(&q->subdev); 1668 media_entity_cleanup(&q->subdev.entity); 1669 cio2_fbpt_exit(q, &cio2->pci_dev->dev); 1670 mutex_destroy(&q->lock); 1671 } 1672 1673 static int cio2_queues_init(struct cio2_device *cio2) 1674 { 1675 int i, r; 1676 1677 for (i = 0; i < CIO2_QUEUES; i++) { 1678 r = cio2_queue_init(cio2, &cio2->queue[i]); 1679 if (r) 1680 break; 1681 } 1682 1683 if (i == CIO2_QUEUES) 1684 return 0; 1685 1686 for (i--; i >= 0; i--) 1687 cio2_queue_exit(cio2, &cio2->queue[i]); 1688 1689 return r; 1690 } 1691 1692 static void cio2_queues_exit(struct cio2_device *cio2) 1693 { 1694 unsigned int i; 1695 1696 for (i = 0; i < CIO2_QUEUES; i++) 1697 cio2_queue_exit(cio2, &cio2->queue[i]); 1698 } 1699 1700 /**************** PCI interface ****************/ 1701 1702 static int cio2_pci_config_setup(struct pci_dev *dev) 1703 { 1704 u16 pci_command; 1705 int r = pci_enable_msi(dev); 1706 1707 if (r) { 1708 dev_err(&dev->dev, "failed to enable MSI (%d)\n", r); 1709 return r; 1710 } 1711 1712 pci_read_config_word(dev, PCI_COMMAND, &pci_command); 1713 pci_command |= PCI_COMMAND_MEMORY | PCI_COMMAND_MASTER | 1714 PCI_COMMAND_INTX_DISABLE; 1715 pci_write_config_word(dev, PCI_COMMAND, pci_command); 1716 1717 return 0; 1718 } 1719 1720 static int cio2_pci_probe(struct pci_dev *pci_dev, 1721 const struct pci_device_id *id) 1722 { 1723 struct cio2_device *cio2; 1724 void __iomem *const *iomap; 1725 int r; 1726 1727 cio2 = devm_kzalloc(&pci_dev->dev, sizeof(*cio2), GFP_KERNEL); 1728 if (!cio2) 1729 return -ENOMEM; 1730 cio2->pci_dev = pci_dev; 1731 1732 r = pcim_enable_device(pci_dev); 1733 if (r) { 1734 dev_err(&pci_dev->dev, "failed to enable device (%d)\n", r); 1735 return r; 1736 } 1737 1738 dev_info(&pci_dev->dev, "device 0x%x (rev: 0x%x)\n", 1739 pci_dev->device, pci_dev->revision); 1740 1741 r = pcim_iomap_regions(pci_dev, 1 << CIO2_PCI_BAR, pci_name(pci_dev)); 1742 if (r) { 1743 dev_err(&pci_dev->dev, "failed to remap I/O memory (%d)\n", r); 1744 return -ENODEV; 1745 } 1746 1747 iomap = pcim_iomap_table(pci_dev); 1748 if (!iomap) { 1749 dev_err(&pci_dev->dev, "failed to iomap table\n"); 1750 return -ENODEV; 1751 } 1752 1753 cio2->base = iomap[CIO2_PCI_BAR]; 1754 1755 pci_set_drvdata(pci_dev, cio2); 1756 1757 pci_set_master(pci_dev); 1758 1759 r = pci_set_dma_mask(pci_dev, CIO2_DMA_MASK); 1760 if (r) { 1761 dev_err(&pci_dev->dev, "failed to set DMA mask (%d)\n", r); 1762 return -ENODEV; 1763 } 1764 1765 r = cio2_pci_config_setup(pci_dev); 1766 if (r) 1767 return -ENODEV; 1768 1769 r = cio2_fbpt_init_dummy(cio2); 1770 if (r) 1771 return r; 1772 1773 mutex_init(&cio2->lock); 1774 1775 cio2->media_dev.dev = &cio2->pci_dev->dev; 1776 strlcpy(cio2->media_dev.model, CIO2_DEVICE_NAME, 1777 sizeof(cio2->media_dev.model)); 1778 snprintf(cio2->media_dev.bus_info, sizeof(cio2->media_dev.bus_info), 1779 "PCI:%s", pci_name(cio2->pci_dev)); 1780 cio2->media_dev.hw_revision = 0; 1781 1782 media_device_init(&cio2->media_dev); 1783 r = media_device_register(&cio2->media_dev); 1784 if (r < 0) 1785 goto fail_mutex_destroy; 1786 1787 cio2->v4l2_dev.mdev = &cio2->media_dev; 1788 r = v4l2_device_register(&pci_dev->dev, &cio2->v4l2_dev); 1789 if (r) { 1790 dev_err(&pci_dev->dev, 1791 "failed to register V4L2 device (%d)\n", r); 1792 goto fail_media_device_unregister; 1793 } 1794 1795 r = cio2_queues_init(cio2); 1796 if (r) 1797 goto fail_v4l2_device_unregister; 1798 1799 /* Register notifier for subdevices we care */ 1800 r = cio2_notifier_init(cio2); 1801 if (r) 1802 goto fail_cio2_queue_exit; 1803 1804 r = devm_request_irq(&pci_dev->dev, pci_dev->irq, cio2_irq, 1805 IRQF_SHARED, CIO2_NAME, cio2); 1806 if (r) { 1807 dev_err(&pci_dev->dev, "failed to request IRQ (%d)\n", r); 1808 goto fail; 1809 } 1810 1811 pm_runtime_put_noidle(&pci_dev->dev); 1812 pm_runtime_allow(&pci_dev->dev); 1813 1814 return 0; 1815 1816 fail: 1817 cio2_notifier_exit(cio2); 1818 fail_cio2_queue_exit: 1819 cio2_queues_exit(cio2); 1820 fail_v4l2_device_unregister: 1821 v4l2_device_unregister(&cio2->v4l2_dev); 1822 fail_media_device_unregister: 1823 media_device_unregister(&cio2->media_dev); 1824 media_device_cleanup(&cio2->media_dev); 1825 fail_mutex_destroy: 1826 mutex_destroy(&cio2->lock); 1827 cio2_fbpt_exit_dummy(cio2); 1828 1829 return r; 1830 } 1831 1832 static void cio2_pci_remove(struct pci_dev *pci_dev) 1833 { 1834 struct cio2_device *cio2 = pci_get_drvdata(pci_dev); 1835 unsigned int i; 1836 1837 cio2_notifier_exit(cio2); 1838 cio2_fbpt_exit_dummy(cio2); 1839 for (i = 0; i < CIO2_QUEUES; i++) 1840 cio2_queue_exit(cio2, &cio2->queue[i]); 1841 v4l2_device_unregister(&cio2->v4l2_dev); 1842 media_device_unregister(&cio2->media_dev); 1843 media_device_cleanup(&cio2->media_dev); 1844 mutex_destroy(&cio2->lock); 1845 } 1846 1847 static int __maybe_unused cio2_runtime_suspend(struct device *dev) 1848 { 1849 struct pci_dev *pci_dev = to_pci_dev(dev); 1850 struct cio2_device *cio2 = pci_get_drvdata(pci_dev); 1851 void __iomem *const base = cio2->base; 1852 u16 pm; 1853 1854 writel(CIO2_D0I3C_I3, base + CIO2_REG_D0I3C); 1855 dev_dbg(dev, "cio2 runtime suspend.\n"); 1856 1857 pci_read_config_word(pci_dev, pci_dev->pm_cap + CIO2_PMCSR_OFFSET, &pm); 1858 pm = (pm >> CIO2_PMCSR_D0D3_SHIFT) << CIO2_PMCSR_D0D3_SHIFT; 1859 pm |= CIO2_PMCSR_D3; 1860 pci_write_config_word(pci_dev, pci_dev->pm_cap + CIO2_PMCSR_OFFSET, pm); 1861 1862 return 0; 1863 } 1864 1865 static int __maybe_unused cio2_runtime_resume(struct device *dev) 1866 { 1867 struct pci_dev *pci_dev = to_pci_dev(dev); 1868 struct cio2_device *cio2 = pci_get_drvdata(pci_dev); 1869 void __iomem *const base = cio2->base; 1870 u16 pm; 1871 1872 writel(CIO2_D0I3C_RR, base + CIO2_REG_D0I3C); 1873 dev_dbg(dev, "cio2 runtime resume.\n"); 1874 1875 pci_read_config_word(pci_dev, pci_dev->pm_cap + CIO2_PMCSR_OFFSET, &pm); 1876 pm = (pm >> CIO2_PMCSR_D0D3_SHIFT) << CIO2_PMCSR_D0D3_SHIFT; 1877 pci_write_config_word(pci_dev, pci_dev->pm_cap + CIO2_PMCSR_OFFSET, pm); 1878 1879 return 0; 1880 } 1881 1882 /* 1883 * Helper function to advance all the elements of a circular buffer by "start" 1884 * positions 1885 */ 1886 static void arrange(void *ptr, size_t elem_size, size_t elems, size_t start) 1887 { 1888 struct { 1889 size_t begin, end; 1890 } arr[2] = { 1891 { 0, start - 1 }, 1892 { start, elems - 1 }, 1893 }; 1894 1895 #define CHUNK_SIZE(a) ((a)->end - (a)->begin + 1) 1896 1897 /* Loop as long as we have out-of-place entries */ 1898 while (CHUNK_SIZE(&arr[0]) && CHUNK_SIZE(&arr[1])) { 1899 size_t size0, i; 1900 1901 /* 1902 * Find the number of entries that can be arranged on this 1903 * iteration. 1904 */ 1905 size0 = min(CHUNK_SIZE(&arr[0]), CHUNK_SIZE(&arr[1])); 1906 1907 /* Swap the entries in two parts of the array. */ 1908 for (i = 0; i < size0; i++) { 1909 u8 *d = ptr + elem_size * (arr[1].begin + i); 1910 u8 *s = ptr + elem_size * (arr[0].begin + i); 1911 size_t j; 1912 1913 for (j = 0; j < elem_size; j++) 1914 swap(d[j], s[j]); 1915 } 1916 1917 if (CHUNK_SIZE(&arr[0]) > CHUNK_SIZE(&arr[1])) { 1918 /* The end of the first array remains unarranged. */ 1919 arr[0].begin += size0; 1920 } else { 1921 /* 1922 * The first array is fully arranged so we proceed 1923 * handling the next one. 1924 */ 1925 arr[0].begin = arr[1].begin; 1926 arr[0].end = arr[1].begin + size0 - 1; 1927 arr[1].begin += size0; 1928 } 1929 } 1930 } 1931 1932 static void cio2_fbpt_rearrange(struct cio2_device *cio2, struct cio2_queue *q) 1933 { 1934 unsigned int i, j; 1935 1936 for (i = 0, j = q->bufs_first; i < CIO2_MAX_BUFFERS; 1937 i++, j = (j + 1) % CIO2_MAX_BUFFERS) 1938 if (q->bufs[j]) 1939 break; 1940 1941 if (i == CIO2_MAX_BUFFERS) 1942 return; 1943 1944 if (j) { 1945 arrange(q->fbpt, sizeof(struct cio2_fbpt_entry) * CIO2_MAX_LOPS, 1946 CIO2_MAX_BUFFERS, j); 1947 arrange(q->bufs, sizeof(struct cio2_buffer *), 1948 CIO2_MAX_BUFFERS, j); 1949 } 1950 1951 /* 1952 * DMA clears the valid bit when accessing the buffer. 1953 * When stopping stream in suspend callback, some of the buffers 1954 * may be in invalid state. After resume, when DMA meets the invalid 1955 * buffer, it will halt and stop receiving new data. 1956 * To avoid DMA halting, set the valid bit for all buffers in FBPT. 1957 */ 1958 for (i = 0; i < CIO2_MAX_BUFFERS; i++) 1959 cio2_fbpt_entry_enable(cio2, q->fbpt + i * CIO2_MAX_LOPS); 1960 } 1961 1962 static int __maybe_unused cio2_suspend(struct device *dev) 1963 { 1964 struct pci_dev *pci_dev = to_pci_dev(dev); 1965 struct cio2_device *cio2 = pci_get_drvdata(pci_dev); 1966 struct cio2_queue *q = cio2->cur_queue; 1967 1968 dev_dbg(dev, "cio2 suspend\n"); 1969 if (!cio2->streaming) 1970 return 0; 1971 1972 /* Stop stream */ 1973 cio2_hw_exit(cio2, q); 1974 synchronize_irq(pci_dev->irq); 1975 1976 pm_runtime_force_suspend(dev); 1977 1978 /* 1979 * Upon resume, hw starts to process the fbpt entries from beginning, 1980 * so relocate the queued buffs to the fbpt head before suspend. 1981 */ 1982 cio2_fbpt_rearrange(cio2, q); 1983 q->bufs_first = 0; 1984 q->bufs_next = 0; 1985 1986 return 0; 1987 } 1988 1989 static int __maybe_unused cio2_resume(struct device *dev) 1990 { 1991 struct pci_dev *pci_dev = to_pci_dev(dev); 1992 struct cio2_device *cio2 = pci_get_drvdata(pci_dev); 1993 int r = 0; 1994 struct cio2_queue *q = cio2->cur_queue; 1995 1996 dev_dbg(dev, "cio2 resume\n"); 1997 if (!cio2->streaming) 1998 return 0; 1999 /* Start stream */ 2000 r = pm_runtime_force_resume(&cio2->pci_dev->dev); 2001 if (r < 0) { 2002 dev_err(&cio2->pci_dev->dev, 2003 "failed to set power %d\n", r); 2004 return r; 2005 } 2006 2007 r = cio2_hw_init(cio2, q); 2008 if (r) 2009 dev_err(dev, "fail to init cio2 hw\n"); 2010 2011 return r; 2012 } 2013 2014 static const struct dev_pm_ops cio2_pm_ops = { 2015 SET_RUNTIME_PM_OPS(&cio2_runtime_suspend, &cio2_runtime_resume, NULL) 2016 SET_SYSTEM_SLEEP_PM_OPS(&cio2_suspend, &cio2_resume) 2017 }; 2018 2019 static const struct pci_device_id cio2_pci_id_table[] = { 2020 { PCI_DEVICE(PCI_VENDOR_ID_INTEL, CIO2_PCI_ID) }, 2021 { 0 } 2022 }; 2023 2024 MODULE_DEVICE_TABLE(pci, cio2_pci_id_table); 2025 2026 static struct pci_driver cio2_pci_driver = { 2027 .name = CIO2_NAME, 2028 .id_table = cio2_pci_id_table, 2029 .probe = cio2_pci_probe, 2030 .remove = cio2_pci_remove, 2031 .driver = { 2032 .pm = &cio2_pm_ops, 2033 }, 2034 }; 2035 2036 module_pci_driver(cio2_pci_driver); 2037 2038 MODULE_AUTHOR("Tuukka Toivonen <tuukka.toivonen@intel.com>"); 2039 MODULE_AUTHOR("Tianshu Qiu <tian.shu.qiu@intel.com>"); 2040 MODULE_AUTHOR("Jian Xu Zheng <jian.xu.zheng@intel.com>"); 2041 MODULE_AUTHOR("Yuning Pu <yuning.pu@intel.com>"); 2042 MODULE_AUTHOR("Yong Zhi <yong.zhi@intel.com>"); 2043 MODULE_LICENSE("GPL v2"); 2044 MODULE_DESCRIPTION("IPU3 CIO2 driver"); 2045