drivers/media/pci/intel/ipu3/ipu3-cio2.c

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