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