1 // SPDX-License-Identifier: GPL-2.0 2 /* 3 * Copyright (C) 2017,2020 Intel Corporation 4 * 5 * Based partially on Intel IPU4 driver written by 6 * Sakari Ailus <sakari.ailus@linux.intel.com> 7 * Samu Onkalo <samu.onkalo@intel.com> 8 * Jouni Högander <jouni.hogander@intel.com> 9 * Jouni Ukkonen <jouni.ukkonen@intel.com> 10 * Antti Laakso <antti.laakso@intel.com> 11 * et al. 12 */ 13 14 #include <linux/bitops.h> 15 #include <linux/delay.h> 16 #include <linux/interrupt.h> 17 #include <linux/iopoll.h> 18 #include <linux/mm.h> 19 #include <linux/module.h> 20 #include <linux/pci.h> 21 #include <linux/pfn.h> 22 #include <linux/pm_runtime.h> 23 #include <linux/property.h> 24 #include <linux/vmalloc.h> 25 #include <media/v4l2-ctrls.h> 26 #include <media/v4l2-device.h> 27 #include <media/v4l2-event.h> 28 #include <media/v4l2-fwnode.h> 29 #include <media/v4l2-ioctl.h> 30 #include <media/videobuf2-dma-sg.h> 31 32 #include "../ipu-bridge.h" 33 #include "ipu3-cio2.h" 34 35 struct ipu3_cio2_fmt { 36 u32 mbus_code; 37 u32 fourcc; 38 u8 mipicode; 39 u8 bpp; 40 }; 41 42 /* 43 * These are raw formats used in Intel's third generation of 44 * Image Processing Unit known as IPU3. 45 * 10bit raw bayer packed, 32 bytes for every 25 pixels, 46 * last LSB 6 bits unused. 47 */ 48 static const struct ipu3_cio2_fmt formats[] = { 49 { /* put default entry at beginning */ 50 .mbus_code = MEDIA_BUS_FMT_SGRBG10_1X10, 51 .fourcc = V4L2_PIX_FMT_IPU3_SGRBG10, 52 .mipicode = 0x2b, 53 .bpp = 10, 54 }, { 55 .mbus_code = MEDIA_BUS_FMT_SGBRG10_1X10, 56 .fourcc = V4L2_PIX_FMT_IPU3_SGBRG10, 57 .mipicode = 0x2b, 58 .bpp = 10, 59 }, { 60 .mbus_code = MEDIA_BUS_FMT_SBGGR10_1X10, 61 .fourcc = V4L2_PIX_FMT_IPU3_SBGGR10, 62 .mipicode = 0x2b, 63 .bpp = 10, 64 }, { 65 .mbus_code = MEDIA_BUS_FMT_SRGGB10_1X10, 66 .fourcc = V4L2_PIX_FMT_IPU3_SRGGB10, 67 .mipicode = 0x2b, 68 .bpp = 10, 69 }, { 70 .mbus_code = MEDIA_BUS_FMT_Y10_1X10, 71 .fourcc = V4L2_PIX_FMT_IPU3_Y10, 72 .mipicode = 0x2b, 73 .bpp = 10, 74 }, 75 }; 76 77 /* 78 * cio2_find_format - lookup color format by fourcc or/and media bus code 79 * @pixelformat: fourcc to match, ignored if null 80 * @mbus_code: media bus code to match, ignored if null 81 */ 82 static const struct ipu3_cio2_fmt *cio2_find_format(const u32 *pixelformat, 83 const u32 *mbus_code) 84 { 85 unsigned int i; 86 87 for (i = 0; i < ARRAY_SIZE(formats); i++) { 88 if (pixelformat && *pixelformat != formats[i].fourcc) 89 continue; 90 if (mbus_code && *mbus_code != formats[i].mbus_code) 91 continue; 92 93 return &formats[i]; 94 } 95 96 return NULL; 97 } 98 99 static inline u32 cio2_bytesperline(const unsigned int width) 100 { 101 /* 102 * 64 bytes for every 50 pixels, the line length 103 * in bytes is multiple of 64 (line end alignment). 104 */ 105 return DIV_ROUND_UP(width, 50) * 64; 106 } 107 108 /**************** FBPT operations ****************/ 109 110 static void cio2_fbpt_exit_dummy(struct cio2_device *cio2) 111 { 112 struct device *dev = &cio2->pci_dev->dev; 113 114 if (cio2->dummy_lop) { 115 dma_free_coherent(dev, PAGE_SIZE, cio2->dummy_lop, 116 cio2->dummy_lop_bus_addr); 117 cio2->dummy_lop = NULL; 118 } 119 if (cio2->dummy_page) { 120 dma_free_coherent(dev, PAGE_SIZE, cio2->dummy_page, 121 cio2->dummy_page_bus_addr); 122 cio2->dummy_page = NULL; 123 } 124 } 125 126 static int cio2_fbpt_init_dummy(struct cio2_device *cio2) 127 { 128 struct device *dev = &cio2->pci_dev->dev; 129 unsigned int i; 130 131 cio2->dummy_page = dma_alloc_coherent(dev, PAGE_SIZE, 132 &cio2->dummy_page_bus_addr, 133 GFP_KERNEL); 134 cio2->dummy_lop = dma_alloc_coherent(dev, PAGE_SIZE, 135 &cio2->dummy_lop_bus_addr, 136 GFP_KERNEL); 137 if (!cio2->dummy_page || !cio2->dummy_lop) { 138 cio2_fbpt_exit_dummy(cio2); 139 return -ENOMEM; 140 } 141 /* 142 * List of Pointers(LOP) contains 1024x32b pointers to 4KB page each 143 * Initialize each entry to dummy_page bus base address. 144 */ 145 for (i = 0; i < CIO2_LOP_ENTRIES; i++) 146 cio2->dummy_lop[i] = PFN_DOWN(cio2->dummy_page_bus_addr); 147 148 return 0; 149 } 150 151 static void cio2_fbpt_entry_enable(struct cio2_device *cio2, 152 struct cio2_fbpt_entry entry[CIO2_MAX_LOPS]) 153 { 154 /* 155 * The CPU first initializes some fields in fbpt, then sets 156 * the VALID bit, this barrier is to ensure that the DMA(device) 157 * does not see the VALID bit enabled before other fields are 158 * initialized; otherwise it could lead to havoc. 159 */ 160 dma_wmb(); 161 162 /* 163 * Request interrupts for start and completion 164 * Valid bit is applicable only to 1st entry 165 */ 166 entry[0].first_entry.ctrl = CIO2_FBPT_CTRL_VALID | 167 CIO2_FBPT_CTRL_IOC | CIO2_FBPT_CTRL_IOS; 168 } 169 170 /* Initialize fpbt entries to point to dummy frame */ 171 static void cio2_fbpt_entry_init_dummy(struct cio2_device *cio2, 172 struct cio2_fbpt_entry 173 entry[CIO2_MAX_LOPS]) 174 { 175 unsigned int i; 176 177 entry[0].first_entry.first_page_offset = 0; 178 entry[1].second_entry.num_of_pages = CIO2_LOP_ENTRIES * CIO2_MAX_LOPS; 179 entry[1].second_entry.last_page_available_bytes = PAGE_SIZE - 1; 180 181 for (i = 0; i < CIO2_MAX_LOPS; i++) 182 entry[i].lop_page_addr = PFN_DOWN(cio2->dummy_lop_bus_addr); 183 184 cio2_fbpt_entry_enable(cio2, entry); 185 } 186 187 /* Initialize fpbt entries to point to a given buffer */ 188 static void cio2_fbpt_entry_init_buf(struct cio2_device *cio2, 189 struct cio2_buffer *b, 190 struct cio2_fbpt_entry 191 entry[CIO2_MAX_LOPS]) 192 { 193 struct vb2_buffer *vb = &b->vbb.vb2_buf; 194 unsigned int length = vb->planes[0].length; 195 int remaining, i; 196 197 entry[0].first_entry.first_page_offset = b->offset; 198 remaining = length + entry[0].first_entry.first_page_offset; 199 entry[1].second_entry.num_of_pages = PFN_UP(remaining); 200 /* 201 * last_page_available_bytes has the offset of the last byte in the 202 * last page which is still accessible by DMA. DMA cannot access 203 * beyond this point. Valid range for this is from 0 to 4095. 204 * 0 indicates 1st byte in the page is DMA accessible. 205 * 4095 (PAGE_SIZE - 1) means every single byte in the last page 206 * is available for DMA transfer. 207 */ 208 remaining = offset_in_page(remaining) ?: PAGE_SIZE; 209 entry[1].second_entry.last_page_available_bytes = remaining - 1; 210 /* Fill FBPT */ 211 remaining = length; 212 i = 0; 213 while (remaining > 0) { 214 entry->lop_page_addr = PFN_DOWN(b->lop_bus_addr[i]); 215 remaining -= CIO2_LOP_ENTRIES * PAGE_SIZE; 216 entry++; 217 i++; 218 } 219 220 /* 221 * The first not meaningful FBPT entry should point to a valid LOP 222 */ 223 entry->lop_page_addr = PFN_DOWN(cio2->dummy_lop_bus_addr); 224 225 cio2_fbpt_entry_enable(cio2, entry); 226 } 227 228 static int cio2_fbpt_init(struct cio2_device *cio2, struct cio2_queue *q) 229 { 230 struct device *dev = &cio2->pci_dev->dev; 231 232 q->fbpt = dma_alloc_coherent(dev, CIO2_FBPT_SIZE, &q->fbpt_bus_addr, 233 GFP_KERNEL); 234 if (!q->fbpt) 235 return -ENOMEM; 236 237 return 0; 238 } 239 240 static void cio2_fbpt_exit(struct cio2_queue *q, struct device *dev) 241 { 242 dma_free_coherent(dev, CIO2_FBPT_SIZE, q->fbpt, q->fbpt_bus_addr); 243 } 244 245 /**************** CSI2 hardware setup ****************/ 246 247 /* 248 * The CSI2 receiver has several parameters affecting 249 * the receiver timings. These depend on the MIPI bus frequency 250 * F in Hz (sensor transmitter rate) as follows: 251 * register value = (A/1e9 + B * UI) / COUNT_ACC 252 * where 253 * UI = 1 / (2 * F) in seconds 254 * COUNT_ACC = counter accuracy in seconds 255 * For IPU3 COUNT_ACC = 0.0625 256 * 257 * A and B are coefficients from the table below, 258 * depending whether the register minimum or maximum value is 259 * calculated. 260 * Minimum Maximum 261 * Clock lane A B A B 262 * reg_rx_csi_dly_cnt_termen_clane 0 0 38 0 263 * reg_rx_csi_dly_cnt_settle_clane 95 -8 300 -16 264 * Data lanes 265 * reg_rx_csi_dly_cnt_termen_dlane0 0 0 35 4 266 * reg_rx_csi_dly_cnt_settle_dlane0 85 -2 145 -6 267 * reg_rx_csi_dly_cnt_termen_dlane1 0 0 35 4 268 * reg_rx_csi_dly_cnt_settle_dlane1 85 -2 145 -6 269 * reg_rx_csi_dly_cnt_termen_dlane2 0 0 35 4 270 * reg_rx_csi_dly_cnt_settle_dlane2 85 -2 145 -6 271 * reg_rx_csi_dly_cnt_termen_dlane3 0 0 35 4 272 * reg_rx_csi_dly_cnt_settle_dlane3 85 -2 145 -6 273 * 274 * We use the minimum values of both A and B. 275 */ 276 277 /* 278 * shift for keeping value range suitable for 32-bit integer arithmetic 279 */ 280 #define LIMIT_SHIFT 8 281 282 static s32 cio2_rx_timing(s32 a, s32 b, s64 freq, int def) 283 { 284 const u32 accinv = 16; /* invert of counter resolution */ 285 const u32 uiinv = 500000000; /* 1e9 / 2 */ 286 s32 r; 287 288 freq >>= LIMIT_SHIFT; 289 290 if (WARN_ON(freq <= 0 || freq > S32_MAX)) 291 return def; 292 /* 293 * b could be 0, -2 or -8, so |accinv * b| is always 294 * less than (1 << ds) and thus |r| < 500000000. 295 */ 296 r = accinv * b * (uiinv >> LIMIT_SHIFT); 297 r = r / (s32)freq; 298 /* max value of a is 95 */ 299 r += accinv * a; 300 301 return r; 302 }; 303 304 /* Calculate the delay value for termination enable of clock lane HS Rx */ 305 static int cio2_csi2_calc_timing(struct cio2_device *cio2, struct cio2_queue *q, 306 struct cio2_csi2_timing *timing, 307 unsigned int bpp, unsigned int lanes) 308 { 309 struct device *dev = &cio2->pci_dev->dev; 310 s64 freq; 311 312 if (!q->sensor) 313 return -ENODEV; 314 315 freq = v4l2_get_link_freq(q->sensor->ctrl_handler, bpp, lanes * 2); 316 if (freq < 0) { 317 dev_err(dev, "error %lld, invalid link_freq\n", freq); 318 return freq; 319 } 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, fmt->bpp, lanes); 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 struct device *dev = &cio2->pci_dev->dev; 511 void __iomem *const base = cio2->base; 512 unsigned int i; 513 u32 value; 514 int ret; 515 516 /* Disable CSI receiver and MIPI backend devices */ 517 writel(0, q->csi_rx_base + CIO2_REG_IRQCTRL_MASK); 518 writel(0, q->csi_rx_base + CIO2_REG_IRQCTRL_ENABLE); 519 writel(0, q->csi_rx_base + CIO2_REG_CSIRX_ENABLE); 520 writel(0, q->csi_rx_base + CIO2_REG_MIPIBE_ENABLE); 521 522 /* Halt DMA */ 523 writel(0, base + CIO2_REG_CDMAC0(CIO2_DMA_CHAN)); 524 ret = readl_poll_timeout(base + CIO2_REG_CDMAC0(CIO2_DMA_CHAN), 525 value, value & CIO2_CDMAC0_DMA_HALTED, 526 4000, 2000000); 527 if (ret) 528 dev_err(dev, "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(dev, "no ready buffers found on DMA channel %u\n", 553 dma_chan); 554 return; 555 } 556 557 /* Find out which buffer(s) are ready */ 558 do { 559 struct cio2_buffer *b; 560 561 b = q->bufs[q->bufs_first]; 562 if (b) { 563 unsigned int received = entry[1].second_entry.num_of_bytes; 564 unsigned long payload = 565 vb2_get_plane_payload(&b->vbb.vb2_buf, 0); 566 567 q->bufs[q->bufs_first] = NULL; 568 atomic_dec(&q->bufs_queued); 569 dev_dbg(dev, "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 (payload != received) 575 dev_warn(dev, 576 "payload length is %lu, received %u\n", 577 payload, received); 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 void cio2_irq_log_irq_errs(struct device *dev, u8 port, u32 status) 624 { 625 unsigned long csi2_status = status; 626 unsigned int i; 627 628 for_each_set_bit(i, &csi2_status, ARRAY_SIZE(cio2_irq_errs)) 629 dev_err(dev, "CSI-2 receiver port %i: %s\n", 630 port, cio2_irq_errs[i]); 631 632 if (fls_long(csi2_status) >= ARRAY_SIZE(cio2_irq_errs)) 633 dev_warn(dev, "unknown CSI2 error 0x%lx on port %i\n", 634 csi2_status, port); 635 } 636 637 static const char *const cio2_port_errs[] = { 638 "ECC recoverable", 639 "DPHY not recoverable", 640 "ECC not recoverable", 641 "CRC error", 642 "INTERFRAMEDATA", 643 "PKT2SHORT", 644 "PKT2LONG", 645 }; 646 647 static void cio2_irq_log_port_errs(struct device *dev, u8 port, u32 status) 648 { 649 unsigned long port_status = status; 650 unsigned int i; 651 652 for_each_set_bit(i, &port_status, ARRAY_SIZE(cio2_port_errs)) 653 dev_err(dev, "port %i error %s\n", port, cio2_port_errs[i]); 654 } 655 656 static void cio2_irq_handle_once(struct cio2_device *cio2, u32 int_status) 657 { 658 struct device *dev = &cio2->pci_dev->dev; 659 void __iomem *const base = cio2->base; 660 661 if (int_status & CIO2_INT_IOOE) { 662 /* 663 * Interrupt on Output Error: 664 * 1) SRAM is full and FS received, or 665 * 2) An invalid bit detected by DMA. 666 */ 667 u32 oe_status, oe_clear; 668 669 oe_clear = readl(base + CIO2_REG_INT_STS_EXT_OE); 670 oe_status = oe_clear; 671 672 if (oe_status & CIO2_INT_EXT_OE_DMAOE_MASK) { 673 dev_err(dev, "DMA output error: 0x%x\n", 674 (oe_status & CIO2_INT_EXT_OE_DMAOE_MASK) 675 >> CIO2_INT_EXT_OE_DMAOE_SHIFT); 676 oe_status &= ~CIO2_INT_EXT_OE_DMAOE_MASK; 677 } 678 if (oe_status & CIO2_INT_EXT_OE_OES_MASK) { 679 dev_err(dev, "DMA output error on CSI2 buses: 0x%x\n", 680 (oe_status & CIO2_INT_EXT_OE_OES_MASK) 681 >> CIO2_INT_EXT_OE_OES_SHIFT); 682 oe_status &= ~CIO2_INT_EXT_OE_OES_MASK; 683 } 684 writel(oe_clear, base + CIO2_REG_INT_STS_EXT_OE); 685 if (oe_status) 686 dev_warn(dev, "unknown interrupt 0x%x on OE\n", 687 oe_status); 688 int_status &= ~CIO2_INT_IOOE; 689 } 690 691 if (int_status & CIO2_INT_IOC_MASK) { 692 /* DMA IO done -- frame ready */ 693 u32 clr = 0; 694 unsigned int d; 695 696 for (d = 0; d < CIO2_NUM_DMA_CHAN; d++) 697 if (int_status & CIO2_INT_IOC(d)) { 698 clr |= CIO2_INT_IOC(d); 699 cio2_buffer_done(cio2, d); 700 } 701 int_status &= ~clr; 702 } 703 704 if (int_status & CIO2_INT_IOS_IOLN_MASK) { 705 /* DMA IO starts or reached specified line */ 706 u32 clr = 0; 707 unsigned int d; 708 709 for (d = 0; d < CIO2_NUM_DMA_CHAN; d++) 710 if (int_status & CIO2_INT_IOS_IOLN(d)) { 711 clr |= CIO2_INT_IOS_IOLN(d); 712 if (d == CIO2_DMA_CHAN) 713 cio2_queue_event_sof(cio2, 714 cio2->cur_queue); 715 } 716 int_status &= ~clr; 717 } 718 719 if (int_status & (CIO2_INT_IOIE | CIO2_INT_IOIRQ)) { 720 /* CSI2 receiver (error) interrupt */ 721 unsigned int port; 722 u32 ie_status; 723 724 ie_status = readl(base + CIO2_REG_INT_STS_EXT_IE); 725 726 for (port = 0; port < CIO2_NUM_PORTS; port++) { 727 u32 port_status = (ie_status >> (port * 8)) & 0xff; 728 729 cio2_irq_log_port_errs(dev, port, port_status); 730 731 if (ie_status & CIO2_INT_EXT_IE_IRQ(port)) { 732 void __iomem *csi_rx_base = 733 base + CIO2_REG_PIPE_BASE(port); 734 u32 csi2_status; 735 736 csi2_status = readl(csi_rx_base + 737 CIO2_REG_IRQCTRL_STATUS); 738 739 cio2_irq_log_irq_errs(dev, port, csi2_status); 740 741 writel(csi2_status, 742 csi_rx_base + CIO2_REG_IRQCTRL_CLEAR); 743 } 744 } 745 746 writel(ie_status, base + CIO2_REG_INT_STS_EXT_IE); 747 748 int_status &= ~(CIO2_INT_IOIE | CIO2_INT_IOIRQ); 749 } 750 751 if (int_status) 752 dev_warn(dev, "unknown interrupt 0x%x on INT\n", int_status); 753 } 754 755 static irqreturn_t cio2_irq(int irq, void *cio2_ptr) 756 { 757 struct cio2_device *cio2 = cio2_ptr; 758 void __iomem *const base = cio2->base; 759 struct device *dev = &cio2->pci_dev->dev; 760 u32 int_status; 761 762 int_status = readl(base + CIO2_REG_INT_STS); 763 dev_dbg(dev, "isr enter - interrupt status 0x%x\n", int_status); 764 if (!int_status) 765 return IRQ_NONE; 766 767 do { 768 writel(int_status, base + CIO2_REG_INT_STS); 769 cio2_irq_handle_once(cio2, int_status); 770 int_status = readl(base + CIO2_REG_INT_STS); 771 if (int_status) 772 dev_dbg(dev, "pending status 0x%x\n", int_status); 773 } while (int_status); 774 775 return IRQ_HANDLED; 776 } 777 778 /**************** Videobuf2 interface ****************/ 779 780 static void cio2_vb2_return_all_buffers(struct cio2_queue *q, 781 enum vb2_buffer_state state) 782 { 783 unsigned int i; 784 785 for (i = 0; i < CIO2_MAX_BUFFERS; i++) { 786 if (q->bufs[i]) { 787 atomic_dec(&q->bufs_queued); 788 vb2_buffer_done(&q->bufs[i]->vbb.vb2_buf, 789 state); 790 q->bufs[i] = NULL; 791 } 792 } 793 } 794 795 static int cio2_vb2_queue_setup(struct vb2_queue *vq, 796 unsigned int *num_buffers, 797 unsigned int *num_planes, 798 unsigned int sizes[], 799 struct device *alloc_devs[]) 800 { 801 struct cio2_device *cio2 = vb2_get_drv_priv(vq); 802 struct device *dev = &cio2->pci_dev->dev; 803 struct cio2_queue *q = vb2q_to_cio2_queue(vq); 804 unsigned int i; 805 806 if (*num_planes && *num_planes < q->format.num_planes) 807 return -EINVAL; 808 809 for (i = 0; i < q->format.num_planes; ++i) { 810 if (*num_planes && sizes[i] < q->format.plane_fmt[i].sizeimage) 811 return -EINVAL; 812 sizes[i] = q->format.plane_fmt[i].sizeimage; 813 alloc_devs[i] = dev; 814 } 815 816 *num_planes = q->format.num_planes; 817 *num_buffers = clamp_val(*num_buffers, 1, CIO2_MAX_BUFFERS); 818 819 /* Initialize buffer queue */ 820 for (i = 0; i < CIO2_MAX_BUFFERS; i++) { 821 q->bufs[i] = NULL; 822 cio2_fbpt_entry_init_dummy(cio2, &q->fbpt[i * CIO2_MAX_LOPS]); 823 } 824 atomic_set(&q->bufs_queued, 0); 825 q->bufs_first = 0; 826 q->bufs_next = 0; 827 828 return 0; 829 } 830 831 /* Called after each buffer is allocated */ 832 static int cio2_vb2_buf_init(struct vb2_buffer *vb) 833 { 834 struct cio2_device *cio2 = vb2_get_drv_priv(vb->vb2_queue); 835 struct device *dev = &cio2->pci_dev->dev; 836 struct cio2_buffer *b = to_cio2_buffer(vb); 837 unsigned int pages = PFN_UP(vb->planes[0].length); 838 unsigned int lops = DIV_ROUND_UP(pages + 1, CIO2_LOP_ENTRIES); 839 struct sg_table *sg; 840 struct sg_dma_page_iter sg_iter; 841 unsigned int i, j; 842 843 if (lops <= 0 || lops > CIO2_MAX_LOPS) { 844 dev_err(dev, "%s: bad buffer size (%i)\n", __func__, 845 vb->planes[0].length); 846 return -ENOSPC; /* Should never happen */ 847 } 848 849 memset(b->lop, 0, sizeof(b->lop)); 850 /* Allocate LOP table */ 851 for (i = 0; i < lops; i++) { 852 b->lop[i] = dma_alloc_coherent(dev, PAGE_SIZE, 853 &b->lop_bus_addr[i], GFP_KERNEL); 854 if (!b->lop[i]) 855 goto fail; 856 } 857 858 /* Fill LOP */ 859 sg = vb2_dma_sg_plane_desc(vb, 0); 860 if (!sg) 861 return -ENOMEM; 862 863 if (sg->nents && sg->sgl) 864 b->offset = sg->sgl->offset; 865 866 i = j = 0; 867 for_each_sg_dma_page(sg->sgl, &sg_iter, sg->nents, 0) { 868 if (!pages--) 869 break; 870 b->lop[i][j] = PFN_DOWN(sg_page_iter_dma_address(&sg_iter)); 871 j++; 872 if (j == CIO2_LOP_ENTRIES) { 873 i++; 874 j = 0; 875 } 876 } 877 878 b->lop[i][j] = PFN_DOWN(cio2->dummy_page_bus_addr); 879 return 0; 880 fail: 881 while (i--) 882 dma_free_coherent(dev, PAGE_SIZE, 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 device *dev = &cio2->pci_dev->dev; 891 struct cio2_queue *q = 892 container_of(vb->vb2_queue, struct cio2_queue, vbq); 893 struct cio2_buffer *b = to_cio2_buffer(vb); 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(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(dev, "entry %i was full!\n", next); 952 next = (next + 1) % CIO2_MAX_BUFFERS; 953 } 954 955 local_irq_restore(flags); 956 dev_err(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 device *dev = &cio2->pci_dev->dev; 966 struct cio2_buffer *b = to_cio2_buffer(vb); 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(dev, 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 struct device *dev = &cio2->pci_dev->dev; 982 int r; 983 984 cio2->cur_queue = q; 985 atomic_set(&q->frame_sequence, 0); 986 987 r = pm_runtime_resume_and_get(dev); 988 if (r < 0) { 989 dev_info(dev, "failed to set power %d\n", r); 990 return r; 991 } 992 993 r = video_device_pipeline_start(&q->vdev, &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 video_device_pipeline_stop(&q->vdev); 1014 fail_pipeline: 1015 dev_dbg(dev, "failed to start streaming (%d)\n", r); 1016 cio2_vb2_return_all_buffers(q, VB2_BUF_STATE_QUEUED); 1017 pm_runtime_put(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 struct device *dev = &cio2->pci_dev->dev; 1027 1028 if (v4l2_subdev_call(q->sensor, video, s_stream, 0)) 1029 dev_err(dev, "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 video_device_pipeline_stop(&q->vdev); 1035 pm_runtime_put(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 strscpy(cap->driver, CIO2_NAME, sizeof(cap->driver)); 1056 strscpy(cap->card, CIO2_DEVICE_NAME, sizeof(cap->card)); 1057 1058 return 0; 1059 } 1060 1061 static int cio2_v4l2_enum_fmt(struct file *file, void *fh, 1062 struct v4l2_fmtdesc *f) 1063 { 1064 if (f->index >= ARRAY_SIZE(formats)) 1065 return -EINVAL; 1066 1067 f->pixelformat = formats[f->index].fourcc; 1068 1069 return 0; 1070 } 1071 1072 /* The format is validated in cio2_video_link_validate() */ 1073 static int cio2_v4l2_g_fmt(struct file *file, void *fh, struct v4l2_format *f) 1074 { 1075 struct cio2_queue *q = file_to_cio2_queue(file); 1076 1077 f->fmt.pix_mp = q->format; 1078 1079 return 0; 1080 } 1081 1082 static int cio2_v4l2_try_fmt(struct file *file, void *fh, struct v4l2_format *f) 1083 { 1084 const struct ipu3_cio2_fmt *fmt; 1085 struct v4l2_pix_format_mplane *mpix = &f->fmt.pix_mp; 1086 1087 fmt = cio2_find_format(&mpix->pixelformat, NULL); 1088 if (!fmt) 1089 fmt = &formats[0]; 1090 1091 /* Only supports up to 4224x3136 */ 1092 if (mpix->width > CIO2_IMAGE_MAX_WIDTH) 1093 mpix->width = CIO2_IMAGE_MAX_WIDTH; 1094 if (mpix->height > CIO2_IMAGE_MAX_HEIGHT) 1095 mpix->height = CIO2_IMAGE_MAX_HEIGHT; 1096 1097 mpix->num_planes = 1; 1098 mpix->pixelformat = fmt->fourcc; 1099 mpix->colorspace = V4L2_COLORSPACE_RAW; 1100 mpix->field = V4L2_FIELD_NONE; 1101 mpix->plane_fmt[0].bytesperline = cio2_bytesperline(mpix->width); 1102 mpix->plane_fmt[0].sizeimage = mpix->plane_fmt[0].bytesperline * 1103 mpix->height; 1104 1105 /* use default */ 1106 mpix->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT; 1107 mpix->quantization = V4L2_QUANTIZATION_DEFAULT; 1108 mpix->xfer_func = V4L2_XFER_FUNC_DEFAULT; 1109 1110 return 0; 1111 } 1112 1113 static int cio2_v4l2_s_fmt(struct file *file, void *fh, struct v4l2_format *f) 1114 { 1115 struct cio2_queue *q = file_to_cio2_queue(file); 1116 1117 cio2_v4l2_try_fmt(file, fh, f); 1118 q->format = f->fmt.pix_mp; 1119 1120 return 0; 1121 } 1122 1123 static int 1124 cio2_video_enum_input(struct file *file, void *fh, struct v4l2_input *input) 1125 { 1126 if (input->index > 0) 1127 return -EINVAL; 1128 1129 strscpy(input->name, "camera", sizeof(input->name)); 1130 input->type = V4L2_INPUT_TYPE_CAMERA; 1131 1132 return 0; 1133 } 1134 1135 static int 1136 cio2_video_g_input(struct file *file, void *fh, unsigned int *input) 1137 { 1138 *input = 0; 1139 1140 return 0; 1141 } 1142 1143 static int 1144 cio2_video_s_input(struct file *file, void *fh, unsigned int input) 1145 { 1146 return input == 0 ? 0 : -EINVAL; 1147 } 1148 1149 static const struct v4l2_file_operations cio2_v4l2_fops = { 1150 .owner = THIS_MODULE, 1151 .unlocked_ioctl = video_ioctl2, 1152 .open = v4l2_fh_open, 1153 .release = vb2_fop_release, 1154 .poll = vb2_fop_poll, 1155 .mmap = vb2_fop_mmap, 1156 }; 1157 1158 static const struct v4l2_ioctl_ops cio2_v4l2_ioctl_ops = { 1159 .vidioc_querycap = cio2_v4l2_querycap, 1160 .vidioc_enum_fmt_vid_cap = cio2_v4l2_enum_fmt, 1161 .vidioc_g_fmt_vid_cap_mplane = cio2_v4l2_g_fmt, 1162 .vidioc_s_fmt_vid_cap_mplane = cio2_v4l2_s_fmt, 1163 .vidioc_try_fmt_vid_cap_mplane = cio2_v4l2_try_fmt, 1164 .vidioc_reqbufs = vb2_ioctl_reqbufs, 1165 .vidioc_create_bufs = vb2_ioctl_create_bufs, 1166 .vidioc_prepare_buf = vb2_ioctl_prepare_buf, 1167 .vidioc_querybuf = vb2_ioctl_querybuf, 1168 .vidioc_qbuf = vb2_ioctl_qbuf, 1169 .vidioc_dqbuf = vb2_ioctl_dqbuf, 1170 .vidioc_streamon = vb2_ioctl_streamon, 1171 .vidioc_streamoff = vb2_ioctl_streamoff, 1172 .vidioc_expbuf = vb2_ioctl_expbuf, 1173 .vidioc_enum_input = cio2_video_enum_input, 1174 .vidioc_g_input = cio2_video_g_input, 1175 .vidioc_s_input = cio2_video_s_input, 1176 }; 1177 1178 static int cio2_subdev_subscribe_event(struct v4l2_subdev *sd, 1179 struct v4l2_fh *fh, 1180 struct v4l2_event_subscription *sub) 1181 { 1182 if (sub->type != V4L2_EVENT_FRAME_SYNC) 1183 return -EINVAL; 1184 1185 /* Line number. For now only zero accepted. */ 1186 if (sub->id != 0) 1187 return -EINVAL; 1188 1189 return v4l2_event_subscribe(fh, sub, 0, NULL); 1190 } 1191 1192 static int cio2_subdev_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh) 1193 { 1194 struct v4l2_mbus_framefmt *format; 1195 const struct v4l2_mbus_framefmt fmt_default = { 1196 .width = 1936, 1197 .height = 1096, 1198 .code = formats[0].mbus_code, 1199 .field = V4L2_FIELD_NONE, 1200 .colorspace = V4L2_COLORSPACE_RAW, 1201 .ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT, 1202 .quantization = V4L2_QUANTIZATION_DEFAULT, 1203 .xfer_func = V4L2_XFER_FUNC_DEFAULT, 1204 }; 1205 1206 /* Initialize try_fmt */ 1207 format = v4l2_subdev_get_try_format(sd, fh->state, CIO2_PAD_SINK); 1208 *format = fmt_default; 1209 1210 /* same as sink */ 1211 format = v4l2_subdev_get_try_format(sd, fh->state, CIO2_PAD_SOURCE); 1212 *format = fmt_default; 1213 1214 return 0; 1215 } 1216 1217 /* 1218 * cio2_subdev_get_fmt - Handle get format by pads subdev method 1219 * @sd : pointer to v4l2 subdev structure 1220 * @cfg: V4L2 subdev pad config 1221 * @fmt: pointer to v4l2 subdev format structure 1222 * return -EINVAL or zero on success 1223 */ 1224 static int cio2_subdev_get_fmt(struct v4l2_subdev *sd, 1225 struct v4l2_subdev_state *sd_state, 1226 struct v4l2_subdev_format *fmt) 1227 { 1228 struct cio2_queue *q = container_of(sd, struct cio2_queue, subdev); 1229 1230 mutex_lock(&q->subdev_lock); 1231 1232 if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) 1233 fmt->format = *v4l2_subdev_get_try_format(sd, sd_state, 1234 fmt->pad); 1235 else 1236 fmt->format = q->subdev_fmt; 1237 1238 mutex_unlock(&q->subdev_lock); 1239 1240 return 0; 1241 } 1242 1243 /* 1244 * cio2_subdev_set_fmt - Handle set format by pads subdev method 1245 * @sd : pointer to v4l2 subdev structure 1246 * @cfg: V4L2 subdev pad config 1247 * @fmt: pointer to v4l2 subdev format structure 1248 * return -EINVAL or zero on success 1249 */ 1250 static int cio2_subdev_set_fmt(struct v4l2_subdev *sd, 1251 struct v4l2_subdev_state *sd_state, 1252 struct v4l2_subdev_format *fmt) 1253 { 1254 struct cio2_queue *q = container_of(sd, struct cio2_queue, subdev); 1255 struct v4l2_mbus_framefmt *mbus; 1256 u32 mbus_code = fmt->format.code; 1257 unsigned int i; 1258 1259 /* 1260 * Only allow setting sink pad format; 1261 * source always propagates from sink 1262 */ 1263 if (fmt->pad == CIO2_PAD_SOURCE) 1264 return cio2_subdev_get_fmt(sd, sd_state, fmt); 1265 1266 if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) 1267 mbus = v4l2_subdev_get_try_format(sd, sd_state, fmt->pad); 1268 else 1269 mbus = &q->subdev_fmt; 1270 1271 fmt->format.code = formats[0].mbus_code; 1272 1273 for (i = 0; i < ARRAY_SIZE(formats); i++) { 1274 if (formats[i].mbus_code == mbus_code) { 1275 fmt->format.code = mbus_code; 1276 break; 1277 } 1278 } 1279 1280 fmt->format.width = min(fmt->format.width, CIO2_IMAGE_MAX_WIDTH); 1281 fmt->format.height = min(fmt->format.height, CIO2_IMAGE_MAX_HEIGHT); 1282 fmt->format.field = V4L2_FIELD_NONE; 1283 1284 mutex_lock(&q->subdev_lock); 1285 *mbus = fmt->format; 1286 mutex_unlock(&q->subdev_lock); 1287 1288 return 0; 1289 } 1290 1291 static int cio2_subdev_enum_mbus_code(struct v4l2_subdev *sd, 1292 struct v4l2_subdev_state *sd_state, 1293 struct v4l2_subdev_mbus_code_enum *code) 1294 { 1295 if (code->index >= ARRAY_SIZE(formats)) 1296 return -EINVAL; 1297 1298 code->code = formats[code->index].mbus_code; 1299 return 0; 1300 } 1301 1302 static int cio2_subdev_link_validate_get_format(struct media_pad *pad, 1303 struct v4l2_subdev_format *fmt) 1304 { 1305 if (is_media_entity_v4l2_subdev(pad->entity)) { 1306 struct v4l2_subdev *sd = 1307 media_entity_to_v4l2_subdev(pad->entity); 1308 1309 memset(fmt, 0, sizeof(*fmt)); 1310 fmt->which = V4L2_SUBDEV_FORMAT_ACTIVE; 1311 fmt->pad = pad->index; 1312 return v4l2_subdev_call(sd, pad, get_fmt, NULL, fmt); 1313 } 1314 1315 return -EINVAL; 1316 } 1317 1318 static int cio2_video_link_validate(struct media_link *link) 1319 { 1320 struct media_entity *entity = link->sink->entity; 1321 struct video_device *vd = media_entity_to_video_device(entity); 1322 struct cio2_queue *q = container_of(vd, struct cio2_queue, vdev); 1323 struct cio2_device *cio2 = video_get_drvdata(vd); 1324 struct device *dev = &cio2->pci_dev->dev; 1325 struct v4l2_subdev_format source_fmt; 1326 int ret; 1327 1328 if (!media_pad_remote_pad_first(entity->pads)) { 1329 dev_info(dev, "video node %s pad not connected\n", vd->name); 1330 return -ENOTCONN; 1331 } 1332 1333 ret = cio2_subdev_link_validate_get_format(link->source, &source_fmt); 1334 if (ret < 0) 1335 return 0; 1336 1337 if (source_fmt.format.width != q->format.width || 1338 source_fmt.format.height != q->format.height) { 1339 dev_err(dev, "Wrong width or height %ux%u (%ux%u expected)\n", 1340 q->format.width, q->format.height, 1341 source_fmt.format.width, source_fmt.format.height); 1342 return -EINVAL; 1343 } 1344 1345 if (!cio2_find_format(&q->format.pixelformat, &source_fmt.format.code)) 1346 return -EINVAL; 1347 1348 return 0; 1349 } 1350 1351 static const struct v4l2_subdev_core_ops cio2_subdev_core_ops = { 1352 .subscribe_event = cio2_subdev_subscribe_event, 1353 .unsubscribe_event = v4l2_event_subdev_unsubscribe, 1354 }; 1355 1356 static const struct v4l2_subdev_internal_ops cio2_subdev_internal_ops = { 1357 .open = cio2_subdev_open, 1358 }; 1359 1360 static const struct v4l2_subdev_pad_ops cio2_subdev_pad_ops = { 1361 .link_validate = v4l2_subdev_link_validate_default, 1362 .get_fmt = cio2_subdev_get_fmt, 1363 .set_fmt = cio2_subdev_set_fmt, 1364 .enum_mbus_code = cio2_subdev_enum_mbus_code, 1365 }; 1366 1367 static const struct v4l2_subdev_ops cio2_subdev_ops = { 1368 .core = &cio2_subdev_core_ops, 1369 .pad = &cio2_subdev_pad_ops, 1370 }; 1371 1372 /******* V4L2 sub-device asynchronous registration callbacks***********/ 1373 1374 struct sensor_async_subdev { 1375 struct v4l2_async_subdev asd; 1376 struct csi2_bus_info csi2; 1377 }; 1378 1379 #define to_sensor_asd(__asd) \ 1380 container_of_const(__asd, struct sensor_async_subdev, asd) 1381 1382 /* The .bound() notifier callback when a match is found */ 1383 static int cio2_notifier_bound(struct v4l2_async_notifier *notifier, 1384 struct v4l2_subdev *sd, 1385 struct v4l2_async_subdev *asd) 1386 { 1387 struct cio2_device *cio2 = to_cio2_device(notifier); 1388 struct sensor_async_subdev *s_asd = to_sensor_asd(asd); 1389 struct cio2_queue *q; 1390 1391 if (cio2->queue[s_asd->csi2.port].sensor) 1392 return -EBUSY; 1393 1394 q = &cio2->queue[s_asd->csi2.port]; 1395 1396 q->csi2 = s_asd->csi2; 1397 q->sensor = sd; 1398 q->csi_rx_base = cio2->base + CIO2_REG_PIPE_BASE(q->csi2.port); 1399 1400 return 0; 1401 } 1402 1403 /* The .unbind callback */ 1404 static void cio2_notifier_unbind(struct v4l2_async_notifier *notifier, 1405 struct v4l2_subdev *sd, 1406 struct v4l2_async_subdev *asd) 1407 { 1408 struct cio2_device *cio2 = to_cio2_device(notifier); 1409 struct sensor_async_subdev *s_asd = to_sensor_asd(asd); 1410 1411 cio2->queue[s_asd->csi2.port].sensor = NULL; 1412 } 1413 1414 /* .complete() is called after all subdevices have been located */ 1415 static int cio2_notifier_complete(struct v4l2_async_notifier *notifier) 1416 { 1417 struct cio2_device *cio2 = to_cio2_device(notifier); 1418 struct device *dev = &cio2->pci_dev->dev; 1419 struct sensor_async_subdev *s_asd; 1420 struct v4l2_async_subdev *asd; 1421 struct cio2_queue *q; 1422 int ret; 1423 1424 list_for_each_entry(asd, &cio2->notifier.asd_list, asd_list) { 1425 s_asd = to_sensor_asd(asd); 1426 q = &cio2->queue[s_asd->csi2.port]; 1427 1428 ret = media_entity_get_fwnode_pad(&q->sensor->entity, 1429 s_asd->asd.match.fwnode, 1430 MEDIA_PAD_FL_SOURCE); 1431 if (ret < 0) { 1432 dev_err(dev, "no pad for endpoint %pfw (%d)\n", 1433 s_asd->asd.match.fwnode, ret); 1434 return ret; 1435 } 1436 1437 ret = media_create_pad_link(&q->sensor->entity, ret, 1438 &q->subdev.entity, CIO2_PAD_SINK, 1439 0); 1440 if (ret) { 1441 dev_err(dev, "failed to create link for %s (endpoint %pfw, error %d)\n", 1442 q->sensor->name, s_asd->asd.match.fwnode, ret); 1443 return ret; 1444 } 1445 } 1446 1447 return v4l2_device_register_subdev_nodes(&cio2->v4l2_dev); 1448 } 1449 1450 static const struct v4l2_async_notifier_operations cio2_async_ops = { 1451 .bound = cio2_notifier_bound, 1452 .unbind = cio2_notifier_unbind, 1453 .complete = cio2_notifier_complete, 1454 }; 1455 1456 static int cio2_parse_firmware(struct cio2_device *cio2) 1457 { 1458 struct device *dev = &cio2->pci_dev->dev; 1459 unsigned int i; 1460 int ret; 1461 1462 for (i = 0; i < CIO2_NUM_PORTS; i++) { 1463 struct v4l2_fwnode_endpoint vep = { 1464 .bus_type = V4L2_MBUS_CSI2_DPHY 1465 }; 1466 struct sensor_async_subdev *s_asd; 1467 struct fwnode_handle *ep; 1468 1469 ep = fwnode_graph_get_endpoint_by_id(dev_fwnode(dev), i, 0, 1470 FWNODE_GRAPH_ENDPOINT_NEXT); 1471 if (!ep) 1472 continue; 1473 1474 ret = v4l2_fwnode_endpoint_parse(ep, &vep); 1475 if (ret) 1476 goto err_parse; 1477 1478 s_asd = v4l2_async_nf_add_fwnode_remote(&cio2->notifier, ep, 1479 struct 1480 sensor_async_subdev); 1481 if (IS_ERR(s_asd)) { 1482 ret = PTR_ERR(s_asd); 1483 goto err_parse; 1484 } 1485 1486 s_asd->csi2.port = vep.base.port; 1487 s_asd->csi2.lanes = vep.bus.mipi_csi2.num_data_lanes; 1488 1489 fwnode_handle_put(ep); 1490 1491 continue; 1492 1493 err_parse: 1494 fwnode_handle_put(ep); 1495 return ret; 1496 } 1497 1498 /* 1499 * Proceed even without sensors connected to allow the device to 1500 * suspend. 1501 */ 1502 cio2->notifier.ops = &cio2_async_ops; 1503 ret = v4l2_async_nf_register(&cio2->v4l2_dev, &cio2->notifier); 1504 if (ret) 1505 dev_err(dev, "failed to register async notifier : %d\n", ret); 1506 1507 return ret; 1508 } 1509 1510 /**************** Queue initialization ****************/ 1511 static const struct media_entity_operations cio2_media_ops = { 1512 .link_validate = v4l2_subdev_link_validate, 1513 }; 1514 1515 static const struct media_entity_operations cio2_video_entity_ops = { 1516 .link_validate = cio2_video_link_validate, 1517 }; 1518 1519 static int cio2_queue_init(struct cio2_device *cio2, struct cio2_queue *q) 1520 { 1521 static const u32 default_width = 1936; 1522 static const u32 default_height = 1096; 1523 const struct ipu3_cio2_fmt dflt_fmt = formats[0]; 1524 struct device *dev = &cio2->pci_dev->dev; 1525 struct video_device *vdev = &q->vdev; 1526 struct vb2_queue *vbq = &q->vbq; 1527 struct v4l2_subdev *subdev = &q->subdev; 1528 struct v4l2_mbus_framefmt *fmt; 1529 int r; 1530 1531 /* Initialize miscellaneous variables */ 1532 mutex_init(&q->lock); 1533 mutex_init(&q->subdev_lock); 1534 1535 /* Initialize formats to default values */ 1536 fmt = &q->subdev_fmt; 1537 fmt->width = default_width; 1538 fmt->height = default_height; 1539 fmt->code = dflt_fmt.mbus_code; 1540 fmt->field = V4L2_FIELD_NONE; 1541 1542 q->format.width = default_width; 1543 q->format.height = default_height; 1544 q->format.pixelformat = dflt_fmt.fourcc; 1545 q->format.colorspace = V4L2_COLORSPACE_RAW; 1546 q->format.field = V4L2_FIELD_NONE; 1547 q->format.num_planes = 1; 1548 q->format.plane_fmt[0].bytesperline = 1549 cio2_bytesperline(q->format.width); 1550 q->format.plane_fmt[0].sizeimage = q->format.plane_fmt[0].bytesperline * 1551 q->format.height; 1552 1553 /* Initialize fbpt */ 1554 r = cio2_fbpt_init(cio2, q); 1555 if (r) 1556 goto fail_fbpt; 1557 1558 /* Initialize media entities */ 1559 q->subdev_pads[CIO2_PAD_SINK].flags = MEDIA_PAD_FL_SINK | 1560 MEDIA_PAD_FL_MUST_CONNECT; 1561 q->subdev_pads[CIO2_PAD_SOURCE].flags = MEDIA_PAD_FL_SOURCE; 1562 subdev->entity.ops = &cio2_media_ops; 1563 subdev->internal_ops = &cio2_subdev_internal_ops; 1564 r = media_entity_pads_init(&subdev->entity, CIO2_PADS, q->subdev_pads); 1565 if (r) { 1566 dev_err(dev, "failed initialize subdev media entity (%d)\n", r); 1567 goto fail_subdev_media_entity; 1568 } 1569 1570 q->vdev_pad.flags = MEDIA_PAD_FL_SINK | MEDIA_PAD_FL_MUST_CONNECT; 1571 vdev->entity.ops = &cio2_video_entity_ops; 1572 r = media_entity_pads_init(&vdev->entity, 1, &q->vdev_pad); 1573 if (r) { 1574 dev_err(dev, "failed initialize videodev media entity (%d)\n", 1575 r); 1576 goto fail_vdev_media_entity; 1577 } 1578 1579 /* Initialize subdev */ 1580 v4l2_subdev_init(subdev, &cio2_subdev_ops); 1581 subdev->flags = V4L2_SUBDEV_FL_HAS_DEVNODE | V4L2_SUBDEV_FL_HAS_EVENTS; 1582 subdev->owner = THIS_MODULE; 1583 snprintf(subdev->name, sizeof(subdev->name), 1584 CIO2_ENTITY_NAME " %td", q - cio2->queue); 1585 subdev->entity.function = MEDIA_ENT_F_VID_IF_BRIDGE; 1586 v4l2_set_subdevdata(subdev, cio2); 1587 r = v4l2_device_register_subdev(&cio2->v4l2_dev, subdev); 1588 if (r) { 1589 dev_err(dev, "failed initialize subdev (%d)\n", r); 1590 goto fail_subdev; 1591 } 1592 1593 /* Initialize vbq */ 1594 vbq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE; 1595 vbq->io_modes = VB2_USERPTR | VB2_MMAP | VB2_DMABUF; 1596 vbq->ops = &cio2_vb2_ops; 1597 vbq->mem_ops = &vb2_dma_sg_memops; 1598 vbq->buf_struct_size = sizeof(struct cio2_buffer); 1599 vbq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC; 1600 vbq->min_buffers_needed = 1; 1601 vbq->drv_priv = cio2; 1602 vbq->lock = &q->lock; 1603 r = vb2_queue_init(vbq); 1604 if (r) { 1605 dev_err(dev, "failed to initialize videobuf2 queue (%d)\n", r); 1606 goto fail_subdev; 1607 } 1608 1609 /* Initialize vdev */ 1610 snprintf(vdev->name, sizeof(vdev->name), 1611 "%s %td", CIO2_NAME, q - cio2->queue); 1612 vdev->release = video_device_release_empty; 1613 vdev->fops = &cio2_v4l2_fops; 1614 vdev->ioctl_ops = &cio2_v4l2_ioctl_ops; 1615 vdev->lock = &cio2->lock; 1616 vdev->v4l2_dev = &cio2->v4l2_dev; 1617 vdev->queue = &q->vbq; 1618 vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE_MPLANE | V4L2_CAP_STREAMING; 1619 video_set_drvdata(vdev, cio2); 1620 r = video_register_device(vdev, VFL_TYPE_VIDEO, -1); 1621 if (r) { 1622 dev_err(dev, "failed to register video device (%d)\n", r); 1623 goto fail_vdev; 1624 } 1625 1626 /* Create link from CIO2 subdev to output node */ 1627 r = media_create_pad_link( 1628 &subdev->entity, CIO2_PAD_SOURCE, &vdev->entity, 0, 1629 MEDIA_LNK_FL_ENABLED | MEDIA_LNK_FL_IMMUTABLE); 1630 if (r) 1631 goto fail_link; 1632 1633 return 0; 1634 1635 fail_link: 1636 vb2_video_unregister_device(&q->vdev); 1637 fail_vdev: 1638 v4l2_device_unregister_subdev(subdev); 1639 fail_subdev: 1640 media_entity_cleanup(&vdev->entity); 1641 fail_vdev_media_entity: 1642 media_entity_cleanup(&subdev->entity); 1643 fail_subdev_media_entity: 1644 cio2_fbpt_exit(q, dev); 1645 fail_fbpt: 1646 mutex_destroy(&q->subdev_lock); 1647 mutex_destroy(&q->lock); 1648 1649 return r; 1650 } 1651 1652 static void cio2_queue_exit(struct cio2_device *cio2, struct cio2_queue *q) 1653 { 1654 vb2_video_unregister_device(&q->vdev); 1655 media_entity_cleanup(&q->vdev.entity); 1656 v4l2_device_unregister_subdev(&q->subdev); 1657 media_entity_cleanup(&q->subdev.entity); 1658 cio2_fbpt_exit(q, &cio2->pci_dev->dev); 1659 mutex_destroy(&q->subdev_lock); 1660 mutex_destroy(&q->lock); 1661 } 1662 1663 static int cio2_queues_init(struct cio2_device *cio2) 1664 { 1665 int i, r; 1666 1667 for (i = 0; i < CIO2_QUEUES; i++) { 1668 r = cio2_queue_init(cio2, &cio2->queue[i]); 1669 if (r) 1670 break; 1671 } 1672 1673 if (i == CIO2_QUEUES) 1674 return 0; 1675 1676 for (i--; i >= 0; i--) 1677 cio2_queue_exit(cio2, &cio2->queue[i]); 1678 1679 return r; 1680 } 1681 1682 static void cio2_queues_exit(struct cio2_device *cio2) 1683 { 1684 unsigned int i; 1685 1686 for (i = 0; i < CIO2_QUEUES; i++) 1687 cio2_queue_exit(cio2, &cio2->queue[i]); 1688 } 1689 1690 static int cio2_check_fwnode_graph(struct fwnode_handle *fwnode) 1691 { 1692 struct fwnode_handle *endpoint; 1693 1694 if (IS_ERR_OR_NULL(fwnode)) 1695 return -EINVAL; 1696 1697 endpoint = fwnode_graph_get_next_endpoint(fwnode, NULL); 1698 if (endpoint) { 1699 fwnode_handle_put(endpoint); 1700 return 0; 1701 } 1702 1703 return cio2_check_fwnode_graph(fwnode->secondary); 1704 } 1705 1706 /**************** PCI interface ****************/ 1707 1708 static int cio2_pci_probe(struct pci_dev *pci_dev, 1709 const struct pci_device_id *id) 1710 { 1711 struct device *dev = &pci_dev->dev; 1712 struct fwnode_handle *fwnode = dev_fwnode(dev); 1713 struct cio2_device *cio2; 1714 int r; 1715 1716 /* 1717 * On some platforms no connections to sensors are defined in firmware, 1718 * if the device has no endpoints then we can try to build those as 1719 * software_nodes parsed from SSDB. 1720 */ 1721 r = cio2_check_fwnode_graph(fwnode); 1722 if (r) { 1723 if (fwnode && !IS_ERR_OR_NULL(fwnode->secondary)) { 1724 dev_err(dev, "fwnode graph has no endpoints connected\n"); 1725 return -EINVAL; 1726 } 1727 1728 r = ipu_bridge_init(pci_dev); 1729 if (r) 1730 return r; 1731 } 1732 1733 cio2 = devm_kzalloc(dev, sizeof(*cio2), GFP_KERNEL); 1734 if (!cio2) 1735 return -ENOMEM; 1736 cio2->pci_dev = pci_dev; 1737 1738 r = pcim_enable_device(pci_dev); 1739 if (r) { 1740 dev_err(dev, "failed to enable device (%d)\n", r); 1741 return r; 1742 } 1743 1744 dev_info(dev, "device 0x%x (rev: 0x%x)\n", 1745 pci_dev->device, pci_dev->revision); 1746 1747 r = pcim_iomap_regions(pci_dev, 1 << CIO2_PCI_BAR, pci_name(pci_dev)); 1748 if (r) { 1749 dev_err(dev, "failed to remap I/O memory (%d)\n", r); 1750 return -ENODEV; 1751 } 1752 1753 cio2->base = pcim_iomap_table(pci_dev)[CIO2_PCI_BAR]; 1754 1755 pci_set_drvdata(pci_dev, cio2); 1756 1757 pci_set_master(pci_dev); 1758 1759 r = dma_set_mask(&pci_dev->dev, CIO2_DMA_MASK); 1760 if (r) { 1761 dev_err(dev, "failed to set DMA mask (%d)\n", r); 1762 return -ENODEV; 1763 } 1764 1765 r = pci_enable_msi(pci_dev); 1766 if (r) { 1767 dev_err(dev, "failed to enable MSI (%d)\n", r); 1768 return r; 1769 } 1770 1771 r = cio2_fbpt_init_dummy(cio2); 1772 if (r) 1773 return r; 1774 1775 mutex_init(&cio2->lock); 1776 1777 cio2->media_dev.dev = dev; 1778 strscpy(cio2->media_dev.model, CIO2_DEVICE_NAME, 1779 sizeof(cio2->media_dev.model)); 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(dev, &cio2->v4l2_dev); 1789 if (r) { 1790 dev_err(dev, "failed to register V4L2 device (%d)\n", r); 1791 goto fail_media_device_unregister; 1792 } 1793 1794 r = cio2_queues_init(cio2); 1795 if (r) 1796 goto fail_v4l2_device_unregister; 1797 1798 v4l2_async_nf_init(&cio2->notifier); 1799 1800 /* Register notifier for subdevices we care */ 1801 r = cio2_parse_firmware(cio2); 1802 if (r) 1803 goto fail_clean_notifier; 1804 1805 r = devm_request_irq(dev, pci_dev->irq, cio2_irq, IRQF_SHARED, 1806 CIO2_NAME, cio2); 1807 if (r) { 1808 dev_err(dev, "failed to request IRQ (%d)\n", r); 1809 goto fail_clean_notifier; 1810 } 1811 1812 pm_runtime_put_noidle(dev); 1813 pm_runtime_allow(dev); 1814 1815 return 0; 1816 1817 fail_clean_notifier: 1818 v4l2_async_nf_unregister(&cio2->notifier); 1819 v4l2_async_nf_cleanup(&cio2->notifier); 1820 cio2_queues_exit(cio2); 1821 fail_v4l2_device_unregister: 1822 v4l2_device_unregister(&cio2->v4l2_dev); 1823 fail_media_device_unregister: 1824 media_device_unregister(&cio2->media_dev); 1825 media_device_cleanup(&cio2->media_dev); 1826 fail_mutex_destroy: 1827 mutex_destroy(&cio2->lock); 1828 cio2_fbpt_exit_dummy(cio2); 1829 1830 return r; 1831 } 1832 1833 static void cio2_pci_remove(struct pci_dev *pci_dev) 1834 { 1835 struct cio2_device *cio2 = pci_get_drvdata(pci_dev); 1836 1837 media_device_unregister(&cio2->media_dev); 1838 v4l2_async_nf_unregister(&cio2->notifier); 1839 v4l2_async_nf_cleanup(&cio2->notifier); 1840 cio2_queues_exit(cio2); 1841 cio2_fbpt_exit_dummy(cio2); 1842 v4l2_device_unregister(&cio2->v4l2_dev); 1843 media_device_cleanup(&cio2->media_dev); 1844 mutex_destroy(&cio2->lock); 1845 1846 pm_runtime_forbid(&pci_dev->dev); 1847 pm_runtime_get_noresume(&pci_dev->dev); 1848 } 1849 1850 static int __maybe_unused cio2_runtime_suspend(struct device *dev) 1851 { 1852 struct pci_dev *pci_dev = to_pci_dev(dev); 1853 struct cio2_device *cio2 = pci_get_drvdata(pci_dev); 1854 void __iomem *const base = cio2->base; 1855 u16 pm; 1856 1857 writel(CIO2_D0I3C_I3, base + CIO2_REG_D0I3C); 1858 dev_dbg(dev, "cio2 runtime suspend.\n"); 1859 1860 pci_read_config_word(pci_dev, pci_dev->pm_cap + CIO2_PMCSR_OFFSET, &pm); 1861 pm = (pm >> CIO2_PMCSR_D0D3_SHIFT) << CIO2_PMCSR_D0D3_SHIFT; 1862 pm |= CIO2_PMCSR_D3; 1863 pci_write_config_word(pci_dev, pci_dev->pm_cap + CIO2_PMCSR_OFFSET, pm); 1864 1865 return 0; 1866 } 1867 1868 static int __maybe_unused cio2_runtime_resume(struct device *dev) 1869 { 1870 struct pci_dev *pci_dev = to_pci_dev(dev); 1871 struct cio2_device *cio2 = pci_get_drvdata(pci_dev); 1872 void __iomem *const base = cio2->base; 1873 u16 pm; 1874 1875 writel(CIO2_D0I3C_RR, base + CIO2_REG_D0I3C); 1876 dev_dbg(dev, "cio2 runtime resume.\n"); 1877 1878 pci_read_config_word(pci_dev, pci_dev->pm_cap + CIO2_PMCSR_OFFSET, &pm); 1879 pm = (pm >> CIO2_PMCSR_D0D3_SHIFT) << CIO2_PMCSR_D0D3_SHIFT; 1880 pci_write_config_word(pci_dev, pci_dev->pm_cap + CIO2_PMCSR_OFFSET, pm); 1881 1882 return 0; 1883 } 1884 1885 /* 1886 * Helper function to advance all the elements of a circular buffer by "start" 1887 * positions 1888 */ 1889 static void arrange(void *ptr, size_t elem_size, size_t elems, size_t start) 1890 { 1891 struct { 1892 size_t begin, end; 1893 } arr[2] = { 1894 { 0, start - 1 }, 1895 { start, elems - 1 }, 1896 }; 1897 1898 #define CHUNK_SIZE(a) ((a)->end - (a)->begin + 1) 1899 1900 /* Loop as long as we have out-of-place entries */ 1901 while (CHUNK_SIZE(&arr[0]) && CHUNK_SIZE(&arr[1])) { 1902 size_t size0, i; 1903 1904 /* 1905 * Find the number of entries that can be arranged on this 1906 * iteration. 1907 */ 1908 size0 = min(CHUNK_SIZE(&arr[0]), CHUNK_SIZE(&arr[1])); 1909 1910 /* Swap the entries in two parts of the array. */ 1911 for (i = 0; i < size0; i++) { 1912 u8 *d = ptr + elem_size * (arr[1].begin + i); 1913 u8 *s = ptr + elem_size * (arr[0].begin + i); 1914 size_t j; 1915 1916 for (j = 0; j < elem_size; j++) 1917 swap(d[j], s[j]); 1918 } 1919 1920 if (CHUNK_SIZE(&arr[0]) > CHUNK_SIZE(&arr[1])) { 1921 /* The end of the first array remains unarranged. */ 1922 arr[0].begin += size0; 1923 } else { 1924 /* 1925 * The first array is fully arranged so we proceed 1926 * handling the next one. 1927 */ 1928 arr[0].begin = arr[1].begin; 1929 arr[0].end = arr[1].begin + size0 - 1; 1930 arr[1].begin += size0; 1931 } 1932 } 1933 } 1934 1935 static void cio2_fbpt_rearrange(struct cio2_device *cio2, struct cio2_queue *q) 1936 { 1937 unsigned int i, j; 1938 1939 for (i = 0, j = q->bufs_first; i < CIO2_MAX_BUFFERS; 1940 i++, j = (j + 1) % CIO2_MAX_BUFFERS) 1941 if (q->bufs[j]) 1942 break; 1943 1944 if (i == CIO2_MAX_BUFFERS) 1945 return; 1946 1947 if (j) { 1948 arrange(q->fbpt, sizeof(struct cio2_fbpt_entry) * CIO2_MAX_LOPS, 1949 CIO2_MAX_BUFFERS, j); 1950 arrange(q->bufs, sizeof(struct cio2_buffer *), 1951 CIO2_MAX_BUFFERS, j); 1952 } 1953 1954 /* 1955 * DMA clears the valid bit when accessing the buffer. 1956 * When stopping stream in suspend callback, some of the buffers 1957 * may be in invalid state. After resume, when DMA meets the invalid 1958 * buffer, it will halt and stop receiving new data. 1959 * To avoid DMA halting, set the valid bit for all buffers in FBPT. 1960 */ 1961 for (i = 0; i < CIO2_MAX_BUFFERS; i++) 1962 cio2_fbpt_entry_enable(cio2, q->fbpt + i * CIO2_MAX_LOPS); 1963 } 1964 1965 static int __maybe_unused cio2_suspend(struct device *dev) 1966 { 1967 struct pci_dev *pci_dev = to_pci_dev(dev); 1968 struct cio2_device *cio2 = pci_get_drvdata(pci_dev); 1969 struct cio2_queue *q = cio2->cur_queue; 1970 int r; 1971 1972 dev_dbg(dev, "cio2 suspend\n"); 1973 if (!cio2->streaming) 1974 return 0; 1975 1976 /* Stop stream */ 1977 r = v4l2_subdev_call(q->sensor, video, s_stream, 0); 1978 if (r) { 1979 dev_err(dev, "failed to stop sensor streaming\n"); 1980 return r; 1981 } 1982 1983 cio2_hw_exit(cio2, q); 1984 synchronize_irq(pci_dev->irq); 1985 1986 pm_runtime_force_suspend(dev); 1987 1988 /* 1989 * Upon resume, hw starts to process the fbpt entries from beginning, 1990 * so relocate the queued buffs to the fbpt head before suspend. 1991 */ 1992 cio2_fbpt_rearrange(cio2, q); 1993 q->bufs_first = 0; 1994 q->bufs_next = 0; 1995 1996 return 0; 1997 } 1998 1999 static int __maybe_unused cio2_resume(struct device *dev) 2000 { 2001 struct cio2_device *cio2 = dev_get_drvdata(dev); 2002 struct cio2_queue *q = cio2->cur_queue; 2003 int r; 2004 2005 dev_dbg(dev, "cio2 resume\n"); 2006 if (!cio2->streaming) 2007 return 0; 2008 /* Start stream */ 2009 r = pm_runtime_force_resume(dev); 2010 if (r < 0) { 2011 dev_err(dev, "failed to set power %d\n", r); 2012 return r; 2013 } 2014 2015 r = cio2_hw_init(cio2, q); 2016 if (r) { 2017 dev_err(dev, "fail to init cio2 hw\n"); 2018 return r; 2019 } 2020 2021 r = v4l2_subdev_call(q->sensor, video, s_stream, 1); 2022 if (r) { 2023 dev_err(dev, "fail to start sensor streaming\n"); 2024 cio2_hw_exit(cio2, q); 2025 } 2026 2027 return r; 2028 } 2029 2030 static const struct dev_pm_ops cio2_pm_ops = { 2031 SET_RUNTIME_PM_OPS(&cio2_runtime_suspend, &cio2_runtime_resume, NULL) 2032 SET_SYSTEM_SLEEP_PM_OPS(&cio2_suspend, &cio2_resume) 2033 }; 2034 2035 static const struct pci_device_id cio2_pci_id_table[] = { 2036 { PCI_DEVICE(PCI_VENDOR_ID_INTEL, CIO2_PCI_ID) }, 2037 { } 2038 }; 2039 2040 MODULE_DEVICE_TABLE(pci, cio2_pci_id_table); 2041 2042 static struct pci_driver cio2_pci_driver = { 2043 .name = CIO2_NAME, 2044 .id_table = cio2_pci_id_table, 2045 .probe = cio2_pci_probe, 2046 .remove = cio2_pci_remove, 2047 .driver = { 2048 .pm = &cio2_pm_ops, 2049 }, 2050 }; 2051 2052 module_pci_driver(cio2_pci_driver); 2053 2054 MODULE_AUTHOR("Tuukka Toivonen <tuukka.toivonen@intel.com>"); 2055 MODULE_AUTHOR("Tianshu Qiu <tian.shu.qiu@intel.com>"); 2056 MODULE_AUTHOR("Jian Xu Zheng"); 2057 MODULE_AUTHOR("Yuning Pu <yuning.pu@intel.com>"); 2058 MODULE_AUTHOR("Yong Zhi <yong.zhi@intel.com>"); 2059 MODULE_LICENSE("GPL v2"); 2060 MODULE_DESCRIPTION("IPU3 CIO2 driver"); 2061 MODULE_IMPORT_NS(INTEL_IPU_BRIDGE); 2062