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