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