media: intel-ipu3: cio2: add new MIPI-CSI2 driver

This patch adds CIO2 CSI-2 device driver for
Intel's IPU3 camera sub-system support.

Signed-off-by: Yong Zhi <yong.zhi@intel.com>
Signed-off-by: Hyungwoo Yang <hyungwoo.yang@intel.com>
Signed-off-by: Rajmohan Mani <rajmohan.mani@intel.com>
Signed-off-by: Vijaykumar Ramya <ramya.vijaykumar@intel.com>
Signed-off-by: Sakari Ailus <sakari.ailus@linux.intel.com>
Signed-off-by: Mauro Carvalho Chehab <mchehab@s-opensource.com>

5 files changed, 2522 insertions, 0 deletions

diff --git a/drivers/media/pci/intel/Makefile b/drivers/media/pci/intel/Makefile
new file mode 100644
index 000000000000..745c8b2a7819
--- /dev/null
+++ b/drivers/media/pci/intel/Makefile
@@ -0,0 +1,5 @@
+#
+# Makefile for the IPU3 cio2 and ImGU drivers
+#
+
+obj-y	+= ipu3/
diff --git a/drivers/media/pci/intel/ipu3/Kconfig b/drivers/media/pci/intel/ipu3/Kconfig
new file mode 100644
index 000000000000..a82d3fe277d2
--- /dev/null
+++ b/drivers/media/pci/intel/ipu3/Kconfig
@@ -0,0 +1,19 @@
+config VIDEO_IPU3_CIO2
+	tristate "Intel ipu3-cio2 driver"
+	depends on VIDEO_V4L2 && PCI
+	depends on VIDEO_V4L2_SUBDEV_API
+	depends on X86 || COMPILE_TEST
+	depends on MEDIA_CONTROLLER
+	depends on HAS_DMA
+	depends on ACPI
+	select V4L2_FWNODE
+	select VIDEOBUF2_DMA_SG
+
+	---help---
+	This is the Intel IPU3 CIO2 CSI-2 receiver unit, found in Intel
+	Skylake and Kaby Lake SoCs and used for capturing images and
+	video from a camera sensor.
+
+	Say Y or M here if you have a Skylake/Kaby Lake SoC with MIPI CSI-2
+	connected camera.
+	The module will be called ipu3-cio2.
diff --git a/drivers/media/pci/intel/ipu3/Makefile b/drivers/media/pci/intel/ipu3/Makefile
new file mode 100644
index 000000000000..20186e3ff2ae
--- /dev/null
+++ b/drivers/media/pci/intel/ipu3/Makefile
@@ -0,0 +1 @@
+obj-$(CONFIG_VIDEO_IPU3_CIO2) += ipu3-cio2.o
diff --git a/drivers/media/pci/intel/ipu3/ipu3-cio2.c b/drivers/media/pci/intel/ipu3/ipu3-cio2.c
new file mode 100644
index 000000000000..941caa987dab
--- /dev/null
+++ b/drivers/media/pci/intel/ipu3/ipu3-cio2.c
@@ -0,0 +1,2048 @@
+/*
+ * Copyright (c) 2017 Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * Based partially on Intel IPU4 driver written by
+ *  Sakari Ailus <sakari.ailus@linux.intel.com>
+ *  Samu Onkalo <samu.onkalo@intel.com>
+ *  Jouni Högander <jouni.hogander@intel.com>
+ *  Jouni Ukkonen <jouni.ukkonen@intel.com>
+ *  Antti Laakso <antti.laakso@intel.com>
+ * et al.
+ *
+ */
+
+#include <linux/delay.h>
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/pci.h>
+#include <linux/pm_runtime.h>
+#include <linux/property.h>
+#include <linux/vmalloc.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-ioctl.h>
+#include <media/videobuf2-dma-sg.h>
+
+#include "ipu3-cio2.h"
+
+struct ipu3_cio2_fmt {
+	u32 mbus_code;
+	u32 fourcc;
+	u8 mipicode;
+};
+
+/*
+ * These are raw formats used in Intel's third generation of
+ * Image Processing Unit known as IPU3.
+ * 10bit raw bayer packed, 32 bytes for every 25 pixels,
+ * last LSB 6 bits unused.
+ */
+static const struct ipu3_cio2_fmt formats[] = {
+	{	/* put default entry at beginning */
+		.mbus_code	= MEDIA_BUS_FMT_SGRBG10_1X10,
+		.fourcc		= V4L2_PIX_FMT_IPU3_SGRBG10,
+		.mipicode	= 0x2b,
+	}, {
+		.mbus_code	= MEDIA_BUS_FMT_SGBRG10_1X10,
+		.fourcc		= V4L2_PIX_FMT_IPU3_SGBRG10,
+		.mipicode	= 0x2b,
+	}, {
+		.mbus_code	= MEDIA_BUS_FMT_SBGGR10_1X10,
+		.fourcc		= V4L2_PIX_FMT_IPU3_SBGGR10,
+		.mipicode	= 0x2b,
+	}, {
+		.mbus_code	= MEDIA_BUS_FMT_SRGGB10_1X10,
+		.fourcc		= V4L2_PIX_FMT_IPU3_SRGGB10,
+		.mipicode	= 0x2b,
+	},
+};
+
+/*
+ * cio2_find_format - lookup color format by fourcc or/and media bus code
+ * @pixelformat: fourcc to match, ignored if null
+ * @mbus_code: media bus code to match, ignored if null
+ */
+static const struct ipu3_cio2_fmt *cio2_find_format(const u32 *pixelformat,
+						    const u32 *mbus_code)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(formats); i++) {
+		if (pixelformat && *pixelformat != formats[i].fourcc)
+			continue;
+		if (mbus_code && *mbus_code != formats[i].mbus_code)
+			continue;
+
+		return &formats[i];
+	}
+
+	return NULL;
+}
+
+static inline u32 cio2_bytesperline(const unsigned int width)
+{
+	/*
+	 * 64 bytes for every 50 pixels, the line length
+	 * in bytes is multiple of 64 (line end alignment).
+	 */
+	return DIV_ROUND_UP(width, 50) * 64;
+}
+
+/**************** FBPT operations ****************/
+
+static void cio2_fbpt_exit_dummy(struct cio2_device *cio2)
+{
+	if (cio2->dummy_lop) {
+		dma_free_coherent(&cio2->pci_dev->dev, CIO2_PAGE_SIZE,
+				  cio2->dummy_lop, cio2->dummy_lop_bus_addr);
+		cio2->dummy_lop = NULL;
+	}
+	if (cio2->dummy_page) {
+		dma_free_coherent(&cio2->pci_dev->dev, CIO2_PAGE_SIZE,
+				  cio2->dummy_page, cio2->dummy_page_bus_addr);
+		cio2->dummy_page = NULL;
+	}
+}
+
+static int cio2_fbpt_init_dummy(struct cio2_device *cio2)
+{
+	unsigned int i;
+
+	cio2->dummy_page = dma_alloc_coherent(&cio2->pci_dev->dev,
+					      CIO2_PAGE_SIZE,
+					      &cio2->dummy_page_bus_addr,
+					      GFP_KERNEL);
+	cio2->dummy_lop = dma_alloc_coherent(&cio2->pci_dev->dev,
+					     CIO2_PAGE_SIZE,
+					     &cio2->dummy_lop_bus_addr,
+					     GFP_KERNEL);
+	if (!cio2->dummy_page || !cio2->dummy_lop) {
+		cio2_fbpt_exit_dummy(cio2);
+		return -ENOMEM;
+	}
+	/*
+	 * List of Pointers(LOP) contains 1024x32b pointers to 4KB page each
+	 * Initialize each entry to dummy_page bus base address.
+	 */
+	for (i = 0; i < CIO2_PAGE_SIZE / sizeof(*cio2->dummy_lop); i++)
+		cio2->dummy_lop[i] = cio2->dummy_page_bus_addr >> PAGE_SHIFT;
+
+	return 0;
+}
+
+static void cio2_fbpt_entry_enable(struct cio2_device *cio2,
+				   struct cio2_fbpt_entry entry[CIO2_MAX_LOPS])
+{
+	/*
+	 * The CPU first initializes some fields in fbpt, then sets
+	 * the VALID bit, this barrier is to ensure that the DMA(device)
+	 * does not see the VALID bit enabled before other fields are
+	 * initialized; otherwise it could lead to havoc.
+	 */
+	dma_wmb();
+
+	/*
+	 * Request interrupts for start and completion
+	 * Valid bit is applicable only to 1st entry
+	 */
+	entry[0].first_entry.ctrl = CIO2_FBPT_CTRL_VALID |
+		CIO2_FBPT_CTRL_IOC | CIO2_FBPT_CTRL_IOS;
+}
+
+/* Initialize fpbt entries to point to dummy frame */
+static void cio2_fbpt_entry_init_dummy(struct cio2_device *cio2,
+				       struct cio2_fbpt_entry
+				       entry[CIO2_MAX_LOPS])
+{
+	unsigned int i;
+
+	entry[0].first_entry.first_page_offset = 0;
+	entry[1].second_entry.num_of_pages =
+		CIO2_PAGE_SIZE / sizeof(u32) * CIO2_MAX_LOPS;
+	entry[1].second_entry.last_page_available_bytes = CIO2_PAGE_SIZE - 1;
+
+	for (i = 0; i < CIO2_MAX_LOPS; i++)
+		entry[i].lop_page_addr = cio2->dummy_lop_bus_addr >> PAGE_SHIFT;
+
+	cio2_fbpt_entry_enable(cio2, entry);
+}
+
+/* Initialize fpbt entries to point to a given buffer */
+static void cio2_fbpt_entry_init_buf(struct cio2_device *cio2,
+				     struct cio2_buffer *b,
+				     struct cio2_fbpt_entry
+				     entry[CIO2_MAX_LOPS])
+{
+	struct vb2_buffer *vb = &b->vbb.vb2_buf;
+	unsigned int length = vb->planes[0].length;
+	int remaining, i;
+
+	entry[0].first_entry.first_page_offset = b->offset;
+	remaining = length + entry[0].first_entry.first_page_offset;
+	entry[1].second_entry.num_of_pages =
+		DIV_ROUND_UP(remaining, CIO2_PAGE_SIZE);
+	/*
+	 * last_page_available_bytes has the offset of the last byte in the
+	 * last page which is still accessible by DMA. DMA cannot access
+	 * beyond this point. Valid range for this is from 0 to 4095.
+	 * 0 indicates 1st byte in the page is DMA accessible.
+	 * 4095 (CIO2_PAGE_SIZE - 1) means every single byte in the last page
+	 * is available for DMA transfer.
+	 */
+	entry[1].second_entry.last_page_available_bytes =
+			(remaining & ~PAGE_MASK) ?
+				(remaining & ~PAGE_MASK) - 1 :
+				CIO2_PAGE_SIZE - 1;
+	/* Fill FBPT */
+	remaining = length;
+	i = 0;
+	while (remaining > 0) {
+		entry->lop_page_addr = b->lop_bus_addr[i] >> PAGE_SHIFT;
+		remaining -= CIO2_PAGE_SIZE / sizeof(u32) * CIO2_PAGE_SIZE;
+		entry++;
+		i++;
+	}
+
+	/*
+	 * The first not meaningful FBPT entry should point to a valid LOP
+	 */
+	entry->lop_page_addr = cio2->dummy_lop_bus_addr >> PAGE_SHIFT;
+
+	cio2_fbpt_entry_enable(cio2, entry);
+}
+
+static int cio2_fbpt_init(struct cio2_device *cio2, struct cio2_queue *q)
+{
+	struct device *dev = &cio2->pci_dev->dev;
+
+	q->fbpt = dma_alloc_coherent(dev, CIO2_FBPT_SIZE, &q->fbpt_bus_addr,
+				     GFP_KERNEL);
+	if (!q->fbpt)
+		return -ENOMEM;
+
+	memset(q->fbpt, 0, CIO2_FBPT_SIZE);
+
+	return 0;
+}
+
+static void cio2_fbpt_exit(struct cio2_queue *q, struct device *dev)
+{
+	dma_free_coherent(dev, CIO2_FBPT_SIZE, q->fbpt, q->fbpt_bus_addr);
+}
+
+/**************** CSI2 hardware setup ****************/
+
+/*
+ * The CSI2 receiver has several parameters affecting
+ * the receiver timings. These depend on the MIPI bus frequency
+ * F in Hz (sensor transmitter rate) as follows:
+ *     register value = (A/1e9 + B * UI) / COUNT_ACC
+ * where
+ *      UI = 1 / (2 * F) in seconds
+ *      COUNT_ACC = counter accuracy in seconds
+ *      For IPU3 COUNT_ACC = 0.0625
+ *
+ * A and B are coefficients from the table below,
+ * depending whether the register minimum or maximum value is
+ * calculated.
+ *                                     Minimum     Maximum
+ * Clock lane                          A     B     A     B
+ * reg_rx_csi_dly_cnt_termen_clane     0     0    38     0
+ * reg_rx_csi_dly_cnt_settle_clane    95    -8   300   -16
+ * Data lanes
+ * reg_rx_csi_dly_cnt_termen_dlane0    0     0    35     4
+ * reg_rx_csi_dly_cnt_settle_dlane0   85    -2   145    -6
+ * reg_rx_csi_dly_cnt_termen_dlane1    0     0    35     4
+ * reg_rx_csi_dly_cnt_settle_dlane1   85    -2   145    -6
+ * reg_rx_csi_dly_cnt_termen_dlane2    0     0    35     4
+ * reg_rx_csi_dly_cnt_settle_dlane2   85    -2   145    -6
+ * reg_rx_csi_dly_cnt_termen_dlane3    0     0    35     4
+ * reg_rx_csi_dly_cnt_settle_dlane3   85    -2   145    -6
+ *
+ * We use the minimum values of both A and B.
+ */
+
+/*
+ * shift for keeping value range suitable for 32-bit integer arithmetics
+ */
+#define LIMIT_SHIFT	8
+
+static s32 cio2_rx_timing(s32 a, s32 b, s64 freq, int def)
+{
+	const u32 accinv = 16; /* invert of counter resolution */
+	const u32 uiinv = 500000000; /* 1e9 / 2 */
+	s32 r;
+
+	freq >>= LIMIT_SHIFT;
+
+	if (WARN_ON(freq <= 0 || freq > S32_MAX))
+		return def;
+	/*
+	 * b could be 0, -2 or -8, so |accinv * b| is always
+	 * less than (1 << ds) and thus |r| < 500000000.
+	 */
+	r = accinv * b * (uiinv >> LIMIT_SHIFT);
+	r = r / (s32)freq;
+	/* max value of a is 95 */
+	r += accinv * a;
+
+	return r;
+};
+
+/* Calculate the the delay value for termination enable of clock lane HS Rx */
+static int cio2_csi2_calc_timing(struct cio2_device *cio2, struct cio2_queue *q,
+				 struct cio2_csi2_timing *timing)
+{
+	struct device *dev = &cio2->pci_dev->dev;
+	struct v4l2_querymenu qm = {.id = V4L2_CID_LINK_FREQ, };
+	struct v4l2_ctrl *link_freq;
+	s64 freq;
+	int r;
+
+	if (!q->sensor)
+		return -ENODEV;
+
+	link_freq = v4l2_ctrl_find(q->sensor->ctrl_handler, V4L2_CID_LINK_FREQ);
+	if (!link_freq) {
+		dev_err(dev, "failed to find LINK_FREQ\n");
+		return -EPIPE;
+	}
+
+	qm.index = v4l2_ctrl_g_ctrl(link_freq);
+	r = v4l2_querymenu(q->sensor->ctrl_handler, &qm);
+	if (r) {
+		dev_err(dev, "failed to get menu item\n");
+		return r;
+	}
+
+	if (!qm.value) {
+		dev_err(dev, "error invalid link_freq\n");
+		return -EINVAL;
+	}
+	freq = qm.value;
+
+	timing->clk_termen = cio2_rx_timing(CIO2_CSIRX_DLY_CNT_TERMEN_CLANE_A,
+					    CIO2_CSIRX_DLY_CNT_TERMEN_CLANE_B,
+					    freq,
+					    CIO2_CSIRX_DLY_CNT_TERMEN_DEFAULT);
+	timing->clk_settle = cio2_rx_timing(CIO2_CSIRX_DLY_CNT_SETTLE_CLANE_A,
+					    CIO2_CSIRX_DLY_CNT_SETTLE_CLANE_B,
+					    freq,
+					    CIO2_CSIRX_DLY_CNT_SETTLE_DEFAULT);
+	timing->dat_termen = cio2_rx_timing(CIO2_CSIRX_DLY_CNT_TERMEN_DLANE_A,
+					    CIO2_CSIRX_DLY_CNT_TERMEN_DLANE_B,
+					    freq,
+					    CIO2_CSIRX_DLY_CNT_TERMEN_DEFAULT);
+	timing->dat_settle = cio2_rx_timing(CIO2_CSIRX_DLY_CNT_SETTLE_DLANE_A,
+					    CIO2_CSIRX_DLY_CNT_SETTLE_DLANE_B,
+					    freq,
+					    CIO2_CSIRX_DLY_CNT_SETTLE_DEFAULT);
+
+	dev_dbg(dev, "freq ct value is %d\n", timing->clk_termen);
+	dev_dbg(dev, "freq cs value is %d\n", timing->clk_settle);
+	dev_dbg(dev, "freq dt value is %d\n", timing->dat_termen);
+	dev_dbg(dev, "freq ds value is %d\n", timing->dat_settle);
+
+	return 0;
+};
+
+static int cio2_hw_init(struct cio2_device *cio2, struct cio2_queue *q)
+{
+	static const int NUM_VCS = 4;
+	static const int SID;	/* Stream id */
+	static const int ENTRY;
+	static const int FBPT_WIDTH = DIV_ROUND_UP(CIO2_MAX_LOPS,
+					CIO2_FBPT_SUBENTRY_UNIT);
+	const u32 num_buffers1 = CIO2_MAX_BUFFERS - 1;
+	const struct ipu3_cio2_fmt *fmt;
+	void __iomem *const base = cio2->base;
+	u8 lanes, csi2bus = q->csi2.port;
+	u8 sensor_vc = SENSOR_VIR_CH_DFLT;
+	struct cio2_csi2_timing timing;
+	int i, r;
+
+	fmt = cio2_find_format(NULL, &q->subdev_fmt.code);
+	if (!fmt)
+		return -EINVAL;
+
+	lanes = q->csi2.lanes;
+
+	r = cio2_csi2_calc_timing(cio2, q, &timing);
+	if (r)
+		return r;
+
+	writel(timing.clk_termen, q->csi_rx_base +
+		CIO2_REG_CSIRX_DLY_CNT_TERMEN(CIO2_CSIRX_DLY_CNT_CLANE_IDX));
+	writel(timing.clk_settle, q->csi_rx_base +
+		CIO2_REG_CSIRX_DLY_CNT_SETTLE(CIO2_CSIRX_DLY_CNT_CLANE_IDX));
+
+	for (i = 0; i < lanes; i++) {
+		writel(timing.dat_termen, q->csi_rx_base +
+			CIO2_REG_CSIRX_DLY_CNT_TERMEN(i));
+		writel(timing.dat_settle, q->csi_rx_base +
+			CIO2_REG_CSIRX_DLY_CNT_SETTLE(i));
+	}
+
+	writel(CIO2_PBM_WMCTRL1_MIN_2CK |
+	       CIO2_PBM_WMCTRL1_MID1_2CK |
+	       CIO2_PBM_WMCTRL1_MID2_2CK, base + CIO2_REG_PBM_WMCTRL1);
+	writel(CIO2_PBM_WMCTRL2_HWM_2CK << CIO2_PBM_WMCTRL2_HWM_2CK_SHIFT |
+	       CIO2_PBM_WMCTRL2_LWM_2CK << CIO2_PBM_WMCTRL2_LWM_2CK_SHIFT |
+	       CIO2_PBM_WMCTRL2_OBFFWM_2CK <<
+	       CIO2_PBM_WMCTRL2_OBFFWM_2CK_SHIFT |
+	       CIO2_PBM_WMCTRL2_TRANSDYN << CIO2_PBM_WMCTRL2_TRANSDYN_SHIFT |
+	       CIO2_PBM_WMCTRL2_OBFF_MEM_EN, base + CIO2_REG_PBM_WMCTRL2);
+	writel(CIO2_PBM_ARB_CTRL_LANES_DIV <<
+	       CIO2_PBM_ARB_CTRL_LANES_DIV_SHIFT |
+	       CIO2_PBM_ARB_CTRL_LE_EN |
+	       CIO2_PBM_ARB_CTRL_PLL_POST_SHTDN <<
+	       CIO2_PBM_ARB_CTRL_PLL_POST_SHTDN_SHIFT |
+	       CIO2_PBM_ARB_CTRL_PLL_AHD_WK_UP <<
+	       CIO2_PBM_ARB_CTRL_PLL_AHD_WK_UP_SHIFT,
+	       base + CIO2_REG_PBM_ARB_CTRL);
+	writel(CIO2_CSIRX_STATUS_DLANE_HS_MASK,
+	       q->csi_rx_base + CIO2_REG_CSIRX_STATUS_DLANE_HS);
+	writel(CIO2_CSIRX_STATUS_DLANE_LP_MASK,
+	       q->csi_rx_base + CIO2_REG_CSIRX_STATUS_DLANE_LP);
+
+	writel(CIO2_FB_HPLL_FREQ, base + CIO2_REG_FB_HPLL_FREQ);
+	writel(CIO2_ISCLK_RATIO, base + CIO2_REG_ISCLK_RATIO);
+
+	/* Configure MIPI backend */
+	for (i = 0; i < NUM_VCS; i++)
+		writel(1, q->csi_rx_base + CIO2_REG_MIPIBE_SP_LUT_ENTRY(i));
+
+	/* There are 16 short packet LUT entry */
+	for (i = 0; i < 16; i++)
+		writel(CIO2_MIPIBE_LP_LUT_ENTRY_DISREGARD,
+		       q->csi_rx_base + CIO2_REG_MIPIBE_LP_LUT_ENTRY(i));
+	writel(CIO2_MIPIBE_GLOBAL_LUT_DISREGARD,
+	       q->csi_rx_base + CIO2_REG_MIPIBE_GLOBAL_LUT_DISREGARD);
+
+	writel(CIO2_INT_EN_EXT_IE_MASK, base + CIO2_REG_INT_EN_EXT_IE);
+	writel(CIO2_IRQCTRL_MASK, q->csi_rx_base + CIO2_REG_IRQCTRL_MASK);
+	writel(CIO2_IRQCTRL_MASK, q->csi_rx_base + CIO2_REG_IRQCTRL_ENABLE);
+	writel(0, q->csi_rx_base + CIO2_REG_IRQCTRL_EDGE);
+	writel(0, q->csi_rx_base + CIO2_REG_IRQCTRL_LEVEL_NOT_PULSE);
+	writel(CIO2_INT_EN_EXT_OE_MASK, base + CIO2_REG_INT_EN_EXT_OE);
+
+	writel(CIO2_REG_INT_EN_IRQ | CIO2_INT_IOC(CIO2_DMA_CHAN) |
+	       CIO2_REG_INT_EN_IOS(CIO2_DMA_CHAN),
+	       base + CIO2_REG_INT_EN);
+
+	writel((CIO2_PXM_PXF_FMT_CFG_BPP_10 | CIO2_PXM_PXF_FMT_CFG_PCK_64B)
+	       << CIO2_PXM_PXF_FMT_CFG_SID0_SHIFT,
+	       base + CIO2_REG_PXM_PXF_FMT_CFG0(csi2bus));
+	writel(SID << CIO2_MIPIBE_LP_LUT_ENTRY_SID_SHIFT |
+	       sensor_vc << CIO2_MIPIBE_LP_LUT_ENTRY_VC_SHIFT |
+	       fmt->mipicode << CIO2_MIPIBE_LP_LUT_ENTRY_FORMAT_TYPE_SHIFT,
+	       q->csi_rx_base + CIO2_REG_MIPIBE_LP_LUT_ENTRY(ENTRY));
+	writel(0, q->csi_rx_base + CIO2_REG_MIPIBE_COMP_FORMAT(sensor_vc));
+	writel(0, q->csi_rx_base + CIO2_REG_MIPIBE_FORCE_RAW8);
+	writel(0, base + CIO2_REG_PXM_SID2BID0(csi2bus));
+
+	writel(lanes, q->csi_rx_base + CIO2_REG_CSIRX_NOF_ENABLED_LANES);
+	writel(CIO2_CGC_PRIM_TGE |
+	       CIO2_CGC_SIDE_TGE |
+	       CIO2_CGC_XOSC_TGE |
+	       CIO2_CGC_D3I3_TGE |
+	       CIO2_CGC_CSI2_INTERFRAME_TGE |
+	       CIO2_CGC_CSI2_PORT_DCGE |
+	       CIO2_CGC_SIDE_DCGE |
+	       CIO2_CGC_PRIM_DCGE |
+	       CIO2_CGC_ROSC_DCGE |
+	       CIO2_CGC_XOSC_DCGE |
+	       CIO2_CGC_CLKGATE_HOLDOFF << CIO2_CGC_CLKGATE_HOLDOFF_SHIFT |
+	       CIO2_CGC_CSI_CLKGATE_HOLDOFF
+	       << CIO2_CGC_CSI_CLKGATE_HOLDOFF_SHIFT, base + CIO2_REG_CGC);
+	writel(CIO2_LTRCTRL_LTRDYNEN, base + CIO2_REG_LTRCTRL);
+	writel(CIO2_LTRVAL0_VAL << CIO2_LTRVAL02_VAL_SHIFT |
+	       CIO2_LTRVAL0_SCALE << CIO2_LTRVAL02_SCALE_SHIFT |
+	       CIO2_LTRVAL1_VAL << CIO2_LTRVAL13_VAL_SHIFT |
+	       CIO2_LTRVAL1_SCALE << CIO2_LTRVAL13_SCALE_SHIFT,
+	       base + CIO2_REG_LTRVAL01);
+	writel(CIO2_LTRVAL2_VAL << CIO2_LTRVAL02_VAL_SHIFT |
+	       CIO2_LTRVAL2_SCALE << CIO2_LTRVAL02_SCALE_SHIFT |
+	       CIO2_LTRVAL3_VAL << CIO2_LTRVAL13_VAL_SHIFT |
+	       CIO2_LTRVAL3_SCALE << CIO2_LTRVAL13_SCALE_SHIFT,
+	       base + CIO2_REG_LTRVAL23);
+
+	for (i = 0; i < CIO2_NUM_DMA_CHAN; i++) {
+		writel(0, base + CIO2_REG_CDMABA(i));
+		writel(0, base + CIO2_REG_CDMAC0(i));
+		writel(0, base + CIO2_REG_CDMAC1(i));
+	}
+
+	/* Enable DMA */
+	writel(q->fbpt_bus_addr >> PAGE_SHIFT,
+	       base + CIO2_REG_CDMABA(CIO2_DMA_CHAN));
+
+	writel(num_buffers1 << CIO2_CDMAC0_FBPT_LEN_SHIFT |
+	       FBPT_WIDTH << CIO2_CDMAC0_FBPT_WIDTH_SHIFT |
+	       CIO2_CDMAC0_DMA_INTR_ON_FE |
+	       CIO2_CDMAC0_FBPT_UPDATE_FIFO_FULL |
+	       CIO2_CDMAC0_DMA_EN |
+	       CIO2_CDMAC0_DMA_INTR_ON_FS |
+	       CIO2_CDMAC0_DMA_HALTED, base + CIO2_REG_CDMAC0(CIO2_DMA_CHAN));
+
+	writel(1 << CIO2_CDMAC1_LINENUMUPDATE_SHIFT,
+	       base + CIO2_REG_CDMAC1(CIO2_DMA_CHAN));
+
+	writel(0, base + CIO2_REG_PBM_FOPN_ABORT);
+
+	writel(CIO2_PXM_FRF_CFG_CRC_TH << CIO2_PXM_FRF_CFG_CRC_TH_SHIFT |
+	       CIO2_PXM_FRF_CFG_MSK_ECC_DPHY_NR |
+	       CIO2_PXM_FRF_CFG_MSK_ECC_RE |
+	       CIO2_PXM_FRF_CFG_MSK_ECC_DPHY_NE,
+	       base + CIO2_REG_PXM_FRF_CFG(q->csi2.port));
+
+	/* Clear interrupts */
+	writel(CIO2_IRQCTRL_MASK, q->csi_rx_base + CIO2_REG_IRQCTRL_CLEAR);
+	writel(~0, base + CIO2_REG_INT_STS_EXT_OE);
+	writel(~0, base + CIO2_REG_INT_STS_EXT_IE);
+	writel(~0, base + CIO2_REG_INT_STS);
+
+	/* Enable devices, starting from the last device in the pipe */
+	writel(1, q->csi_rx_base + CIO2_REG_MIPIBE_ENABLE);
+	writel(1, q->csi_rx_base + CIO2_REG_CSIRX_ENABLE);
+
+	return 0;
+}
+
+static void cio2_hw_exit(struct cio2_device *cio2, struct cio2_queue *q)
+{
+	void __iomem *base = cio2->base;
+	unsigned int i, maxloops = 1000;
+
+	/* Disable CSI receiver and MIPI backend devices */
+	writel(0, q->csi_rx_base + CIO2_REG_CSIRX_ENABLE);
+	writel(0, q->csi_rx_base + CIO2_REG_MIPIBE_ENABLE);
+
+	/* Halt DMA */
+	writel(0, base + CIO2_REG_CDMAC0(CIO2_DMA_CHAN));
+	do {
+		if (readl(base + CIO2_REG_CDMAC0(CIO2_DMA_CHAN)) &
+		    CIO2_CDMAC0_DMA_HALTED)
+			break;
+		usleep_range(1000, 2000);
+	} while (--maxloops);
+	if (!maxloops)
+		dev_err(&cio2->pci_dev->dev,
+			"DMA %i can not be halted\n", CIO2_DMA_CHAN);
+
+	for (i = 0; i < CIO2_NUM_PORTS; i++) {
+		writel(readl(base + CIO2_REG_PXM_FRF_CFG(i)) |
+		       CIO2_PXM_FRF_CFG_ABORT, base + CIO2_REG_PXM_FRF_CFG(i));
+		writel(readl(base + CIO2_REG_PBM_FOPN_ABORT) |
+		       CIO2_PBM_FOPN_ABORT(i), base + CIO2_REG_PBM_FOPN_ABORT);
+	}
+}
+
+static void cio2_buffer_done(struct cio2_device *cio2, unsigned int dma_chan)
+{
+	struct device *dev = &cio2->pci_dev->dev;
+	struct cio2_queue *q = cio2->cur_queue;
+	int buffers_found = 0;
+	u64 ns = ktime_get_ns();
+
+	if (dma_chan >= CIO2_QUEUES) {
+		dev_err(dev, "bad DMA channel %i\n", dma_chan);
+		return;
+	}
+
+	/* Find out which buffer(s) are ready */
+	do {
+		struct cio2_fbpt_entry *const entry =
+			&q->fbpt[q->bufs_first * CIO2_MAX_LOPS];
+		struct cio2_buffer *b;
+
+		if (entry->first_entry.ctrl & CIO2_FBPT_CTRL_VALID)
+			break;
+
+		b = q->bufs[q->bufs_first];
+		if (b) {
+			unsigned int bytes = entry[1].second_entry.num_of_bytes;
+
+			q->bufs[q->bufs_first] = NULL;
+			atomic_dec(&q->bufs_queued);
+			dev_dbg(&cio2->pci_dev->dev,
+				"buffer %i done\n", b->vbb.vb2_buf.index);
+
+			b->vbb.vb2_buf.timestamp = ns;
+			b->vbb.field = V4L2_FIELD_NONE;
+			b->vbb.sequence = atomic_read(&q->frame_sequence);
+			if (b->vbb.vb2_buf.planes[0].length != bytes)
+				dev_warn(dev, "buffer length is %d received %d\n",
+					 b->vbb.vb2_buf.planes[0].length,
+					 bytes);
+			vb2_buffer_done(&b->vbb.vb2_buf, VB2_BUF_STATE_DONE);
+		}
+		atomic_inc(&q->frame_sequence);
+		cio2_fbpt_entry_init_dummy(cio2, entry);
+		q->bufs_first = (q->bufs_first + 1) % CIO2_MAX_BUFFERS;
+		buffers_found++;
+	} while (1);
+
+	if (buffers_found == 0)
+		dev_warn(&cio2->pci_dev->dev,
+			 "no ready buffers found on DMA channel %u\n",
+			 dma_chan);
+}
+
+static void cio2_queue_event_sof(struct cio2_device *cio2, struct cio2_queue *q)
+{
+	/*
+	 * For the user space camera control algorithms it is essential
+	 * to know when the reception of a frame has begun. That's often
+	 * the best timing information to get from the hardware.
+	 */
+	struct v4l2_event event = {
+		.type = V4L2_EVENT_FRAME_SYNC,
+		.u.frame_sync.frame_sequence = atomic_read(&q->frame_sequence),
+	};
+
+	v4l2_event_queue(q->subdev.devnode, &event);
+}
+
+static const char *const cio2_irq_errs[] = {
+	"single packet header error corrected",
+	"multiple packet header errors detected",
+	"payload checksum (CRC) error",
+	"fifo overflow",
+	"reserved short packet data type detected",
+	"reserved long packet data type detected",
+	"incomplete long packet detected",
+	"frame sync error",
+	"line sync error",
+	"DPHY start of transmission error",
+	"DPHY synchronization error",
+	"escape mode error",
+	"escape mode trigger event",
+	"escape mode ultra-low power state for data lane(s)",
+	"escape mode ultra-low power state exit for clock lane",
+	"inter-frame short packet discarded",
+	"inter-frame long packet discarded",
+	"non-matching Long Packet stalled",
+};
+
+static const char *const cio2_port_errs[] = {
+	"ECC recoverable",
+	"DPHY not recoverable",
+	"ECC not recoverable",
+	"CRC error",
+	"INTERFRAMEDATA",
+	"PKT2SHORT",
+	"PKT2LONG",
+};
+
+static irqreturn_t cio2_irq(int irq, void *cio2_ptr)
+{
+	struct cio2_device *cio2 = cio2_ptr;
+	void __iomem *const base = cio2->base;
+	struct device *dev = &cio2->pci_dev->dev;
+	u32 int_status, int_clear;
+
+	int_status = readl(base + CIO2_REG_INT_STS);
+	int_clear = int_status;
+
+	if (!int_status)
+		return IRQ_NONE;
+
+	if (int_status & CIO2_INT_IOOE) {
+		/*
+		 * Interrupt on Output Error:
+		 * 1) SRAM is full and FS received, or
+		 * 2) An invalid bit detected by DMA.
+		 */
+		u32 oe_status, oe_clear;
+
+		oe_clear = readl(base + CIO2_REG_INT_STS_EXT_OE);
+		oe_status = oe_clear;
+
+		if (oe_status & CIO2_INT_EXT_OE_DMAOE_MASK) {
+			dev_err(dev, "DMA output error: 0x%x\n",
+				(oe_status & CIO2_INT_EXT_OE_DMAOE_MASK)
+				>> CIO2_INT_EXT_OE_DMAOE_SHIFT);
+			oe_status &= ~CIO2_INT_EXT_OE_DMAOE_MASK;
+		}
+		if (oe_status & CIO2_INT_EXT_OE_OES_MASK) {
+			dev_err(dev, "DMA output error on CSI2 buses: 0x%x\n",
+				(oe_status & CIO2_INT_EXT_OE_OES_MASK)
+				>> CIO2_INT_EXT_OE_OES_SHIFT);
+			oe_status &= ~CIO2_INT_EXT_OE_OES_MASK;
+		}
+		writel(oe_clear, base + CIO2_REG_INT_STS_EXT_OE);
+		if (oe_status)
+			dev_warn(dev, "unknown interrupt 0x%x on OE\n",
+				 oe_status);
+		int_status &= ~CIO2_INT_IOOE;
+	}
+
+	if (int_status & CIO2_INT_IOC_MASK) {
+		/* DMA IO done -- frame ready */
+		u32 clr = 0;
+		unsigned int d;
+
+		for (d = 0; d < CIO2_NUM_DMA_CHAN; d++)
+			if (int_status & CIO2_INT_IOC(d)) {
+				clr |= CIO2_INT_IOC(d);
+				cio2_buffer_done(cio2, d);
+			}
+		int_status &= ~clr;
+	}
+
+	if (int_status & CIO2_INT_IOS_IOLN_MASK) {
+		/* DMA IO starts or reached specified line */
+		u32 clr = 0;
+		unsigned int d;
+
+		for (d = 0; d < CIO2_NUM_DMA_CHAN; d++)
+			if (int_status & CIO2_INT_IOS_IOLN(d)) {
+				clr |= CIO2_INT_IOS_IOLN(d);
+				if (d == CIO2_DMA_CHAN)
+					cio2_queue_event_sof(cio2,
+							     cio2->cur_queue);
+			}
+		int_status &= ~clr;
+	}
+
+	if (int_status & (CIO2_INT_IOIE | CIO2_INT_IOIRQ)) {
+		/* CSI2 receiver (error) interrupt */
+		u32 ie_status, ie_clear;
+		unsigned int port;
+
+		ie_clear = readl(base + CIO2_REG_INT_STS_EXT_IE);
+		ie_status = ie_clear;
+
+		for (port = 0; port < CIO2_NUM_PORTS; port++) {
+			u32 port_status = (ie_status >> (port * 8)) & 0xff;
+			u32 err_mask = BIT_MASK(ARRAY_SIZE(cio2_port_errs)) - 1;
+			void __iomem *const csi_rx_base =
+						base + CIO2_REG_PIPE_BASE(port);
+			unsigned int i;
+
+			while (port_status & err_mask) {
+				i = ffs(port_status) - 1;
+				dev_err(dev, "port %i error %s\n",
+					port, cio2_port_errs[i]);
+				ie_status &= ~BIT(port * 8 + i);
+				port_status &= ~BIT(i);
+			}
+
+			if (ie_status & CIO2_INT_EXT_IE_IRQ(port)) {
+				u32 csi2_status, csi2_clear;
+
+				csi2_status = readl(csi_rx_base +
+						CIO2_REG_IRQCTRL_STATUS);
+				csi2_clear = csi2_status;
+				err_mask =
+					BIT_MASK(ARRAY_SIZE(cio2_irq_errs)) - 1;
+
+				while (csi2_status & err_mask) {
+					i = ffs(csi2_status) - 1;
+					dev_err(dev,
+						"CSI-2 receiver port %i: %s\n",
+							port, cio2_irq_errs[i]);
+					csi2_status &= ~BIT(i);
+				}
+
+				writel(csi2_clear,
+				       csi_rx_base + CIO2_REG_IRQCTRL_CLEAR);
+				if (csi2_status)
+					dev_warn(dev,
+						 "unknown CSI2 error 0x%x on port %i\n",
+						 csi2_status, port);
+
+				ie_status &= ~CIO2_INT_EXT_IE_IRQ(port);
+			}
+		}
+
+		writel(ie_clear, base + CIO2_REG_INT_STS_EXT_IE);
+		if (ie_status)
+			dev_warn(dev, "unknown interrupt 0x%x on IE\n",
+				 ie_status);
+
+		int_status &= ~(CIO2_INT_IOIE | CIO2_INT_IOIRQ);
+	}
+
+	writel(int_clear, base + CIO2_REG_INT_STS);
+	if (int_status)
+		dev_warn(dev, "unknown interrupt 0x%x on INT\n", int_status);
+
+	return IRQ_HANDLED;
+}
+
+/**************** Videobuf2 interface ****************/
+
+static void cio2_vb2_return_all_buffers(struct cio2_queue *q)
+{
+	unsigned int i;
+
+	for (i = 0; i < CIO2_MAX_BUFFERS; i++) {
+		if (q->bufs[i]) {
+			atomic_dec(&q->bufs_queued);
+			vb2_buffer_done(&q->bufs[i]->vbb.vb2_buf,
+					VB2_BUF_STATE_ERROR);
+		}
+	}
+}
+
+static int cio2_vb2_queue_setup(struct vb2_queue *vq,
+				unsigned int *num_buffers,
+				unsigned int *num_planes,
+				unsigned int sizes[],
+				struct device *alloc_devs[])
+{
+	struct cio2_device *cio2 = vb2_get_drv_priv(vq);
+	struct cio2_queue *q = vb2q_to_cio2_queue(vq);
+	unsigned int i;
+
+	*num_planes = q->format.num_planes;
+
+	for (i = 0; i < *num_planes; ++i) {
+		sizes[i] = q->format.plane_fmt[i].sizeimage;
+		alloc_devs[i] = &cio2->pci_dev->dev;
+	}
+
+	*num_buffers = clamp_val(*num_buffers, 1, CIO2_MAX_BUFFERS);
+
+	/* Initialize buffer queue */
+	for (i = 0; i < CIO2_MAX_BUFFERS; i++) {
+		q->bufs[i] = NULL;
+		cio2_fbpt_entry_init_dummy(cio2, &q->fbpt[i * CIO2_MAX_LOPS]);
+	}
+	atomic_set(&q->bufs_queued, 0);
+	q->bufs_first = 0;
+	q->bufs_next = 0;
+
+	return 0;
+}
+
+/* Called after each buffer is allocated */
+static int cio2_vb2_buf_init(struct vb2_buffer *vb)
+{
+	struct cio2_device *cio2 = vb2_get_drv_priv(vb->vb2_queue);
+	struct device *dev = &cio2->pci_dev->dev;
+	struct cio2_buffer *b =
+		container_of(vb, struct cio2_buffer, vbb.vb2_buf);
+	static const unsigned int entries_per_page =
+		CIO2_PAGE_SIZE / sizeof(u32);
+	unsigned int pages = DIV_ROUND_UP(vb->planes[0].length, CIO2_PAGE_SIZE);
+	unsigned int lops = DIV_ROUND_UP(pages + 1, entries_per_page);
+	struct sg_table *sg;
+	struct sg_page_iter sg_iter;
+	int i, j;
+
+	if (lops <= 0 || lops > CIO2_MAX_LOPS) {
+		dev_err(dev, "%s: bad buffer size (%i)\n", __func__,
+			vb->planes[0].length);
+		return -ENOSPC;		/* Should never happen */
+	}
+
+	memset(b->lop, 0, sizeof(b->lop));
+	/* Allocate LOP table */
+	for (i = 0; i < lops; i++) {
+		b->lop[i] = dma_alloc_coherent(dev, CIO2_PAGE_SIZE,
+					       &b->lop_bus_addr[i], GFP_KERNEL);
+		if (!b->lop[i])
+			goto fail;
+	}
+
+	/* Fill LOP */
+	sg = vb2_dma_sg_plane_desc(vb, 0);
+	if (!sg)
+		return -ENOMEM;
+
+	if (sg->nents && sg->sgl)
+		b->offset = sg->sgl->offset;
+
+	i = j = 0;
+	for_each_sg_page(sg->sgl, &sg_iter, sg->nents, 0) {
+		b->lop[i][j] = sg_page_iter_dma_address(&sg_iter) >> PAGE_SHIFT;
+		j++;
+		if (j == entries_per_page) {
+			i++;
+			j = 0;
+		}
+	}
+
+	b->lop[i][j] = cio2->dummy_page_bus_addr >> PAGE_SHIFT;
+	return 0;
+fail:
+	for (i--; i >= 0; i--)
+		dma_free_coherent(dev, CIO2_PAGE_SIZE,
+				  b->lop[i], b->lop_bus_addr[i]);
+	return -ENOMEM;
+}
+
+/* Transfer buffer ownership to cio2 */
+static void cio2_vb2_buf_queue(struct vb2_buffer *vb)
+{
+	struct cio2_device *cio2 = vb2_get_drv_priv(vb->vb2_queue);
+	struct cio2_queue *q =
+		container_of(vb->vb2_queue, struct cio2_queue, vbq);
+	struct cio2_buffer *b =
+		container_of(vb, struct cio2_buffer, vbb.vb2_buf);
+	struct cio2_fbpt_entry *entry;
+	unsigned long flags;
+	unsigned int i, j, next = q->bufs_next;
+	int bufs_queued = atomic_inc_return(&q->bufs_queued);
+	u32 fbpt_rp;
+
+	dev_dbg(&cio2->pci_dev->dev, "queue buffer %d\n", vb->index);
+
+	/*
+	 * This code queues the buffer to the CIO2 DMA engine, which starts
+	 * running once streaming has started. It is possible that this code
+	 * gets pre-empted due to increased CPU load. Upon this, the driver
+	 * does not get an opportunity to queue new buffers to the CIO2 DMA
+	 * engine. When the DMA engine encounters an FBPT entry without the
+	 * VALID bit set, the DMA engine halts, which requires a restart of
+	 * the DMA engine and sensor, to continue streaming.
+	 * This is not desired and is highly unlikely given that there are
+	 * 32 FBPT entries that the DMA engine needs to process, to run into
+	 * an FBPT entry, without the VALID bit set. We try to mitigate this
+	 * by disabling interrupts for the duration of this queueing.
+	 */
+	local_irq_save(flags);
+
+	fbpt_rp = (readl(cio2->base + CIO2_REG_CDMARI(CIO2_DMA_CHAN))
+		   >> CIO2_CDMARI_FBPT_RP_SHIFT)
+		   & CIO2_CDMARI_FBPT_RP_MASK;
+
+	/*
+	 * fbpt_rp is the fbpt entry that the dma is currently working
+	 * on, but since it could jump to next entry at any time,
+	 * assume that we might already be there.
+	 */
+	fbpt_rp = (fbpt_rp + 1) % CIO2_MAX_BUFFERS;
+
+	if (bufs_queued <= 1 || fbpt_rp == next)
+		/* Buffers were drained */
+		next = (fbpt_rp + 1) % CIO2_MAX_BUFFERS;
+
+	for (i = 0; i < CIO2_MAX_BUFFERS; i++) {
+		/*
+		 * We have allocated CIO2_MAX_BUFFERS circularly for the
+		 * hw, the user has requested N buffer queue. The driver
+		 * ensures N <= CIO2_MAX_BUFFERS and guarantees that whenever
+		 * user queues a buffer, there necessarily is a free buffer.
+		 */
+		if (!q->bufs[next]) {
+			q->bufs[next] = b;
+			entry = &q->fbpt[next * CIO2_MAX_LOPS];
+			cio2_fbpt_entry_init_buf(cio2, b, entry);
+			local_irq_restore(flags);
+			q->bufs_next = (next + 1) % CIO2_MAX_BUFFERS;
+			for (j = 0; j < vb->num_planes; j++)
+				vb2_set_plane_payload(vb, j,
+					q->format.plane_fmt[j].sizeimage);
+			return;
+		}
+
+		dev_dbg(&cio2->pci_dev->dev, "entry %i was full!\n", next);
+		next = (next + 1) % CIO2_MAX_BUFFERS;
+	}
+
+	local_irq_restore(flags);
+	dev_err(&cio2->pci_dev->dev, "error: all cio2 entries were full!\n");
+	atomic_dec(&q->bufs_queued);
+	vb2_buffer_done(vb, VB2_BUF_STATE_ERROR);
+}
+
+/* Called when each buffer is freed */
+static void cio2_vb2_buf_cleanup(struct vb2_buffer *vb)
+{
+	struct cio2_device *cio2 = vb2_get_drv_priv(vb->vb2_queue);
+	struct cio2_buffer *b =
+		container_of(vb, struct cio2_buffer, vbb.vb2_buf);
+	unsigned int i;
+
+	/* Free LOP table */
+	for (i = 0; i < CIO2_MAX_LOPS; i++) {
+		if (b->lop[i])
+			dma_free_coherent(&cio2->pci_dev->dev, CIO2_PAGE_SIZE,
+					  b->lop[i], b->lop_bus_addr[i]);
+	}
+}
+
+static int cio2_vb2_start_streaming(struct vb2_queue *vq, unsigned int count)
+{
+	struct cio2_queue *q = vb2q_to_cio2_queue(vq);
+	struct cio2_device *cio2 = vb2_get_drv_priv(vq);