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