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

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

DEFINITIONS
