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

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