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