[PATCH 02/10] MAINTAINERS: Add Qualcomm Camera subsystem driver

[PATCH 02/10] MAINTAINERS: Add Qualcomm Camera subsystem driver

[Todor Tomov]

Add an entry for Qualcomm Camera subsystem driver.

Signed-off-by: Todor Tomov <todor.tomov@linaro.org>
---
 MAINTAINERS | 8 ++++++++
 1 file changed, 8 insertions(+)

diff --git a/MAINTAINERS b/MAINTAINERS
index 411e3b8..0740aee 100644
--- a/MAINTAINERS
+++ b/MAINTAINERS
@@ -9971,6 +9971,14 @@ T:	git git://git.kernel.org/pub/scm/linux/kernel/git/kvalo/ath.git
 S:	Supported
 F:	drivers/net/wireless/ath/ath10k/
 
+QUALCOMM CAMERA SUBSYSTEM DRIVER
+M:	Todor Tomov <todor.tomov@linaro.org>
+L:	linux-media@vger.kernel.org
+S:	Maintained
+F:	Documentation/devicetree/bindings/media/qcom,camss.txt
+F:	Documentation/media/v4l-drivers/qcom_camss.rst
+F:	drivers/media/platform/qcom/camss/
+
 QUALCOMM EMAC GIGABIT ETHERNET DRIVER
 M:	Timur Tabi <timur@codeaurora.org>
 L:	netdev@vger.kernel.org
-- 
1.9.1

[PATCH 03/10] doc: media/v4l-drivers: Add Qualcomm Camera Subsystem driver document

[Todor Tomov]

Add a document to describe Qualcomm Camera Subsystem driver.

Signed-off-by: Todor Tomov <todor.tomov@linaro.org>
---
 Documentation/media/v4l-drivers/index.rst      |   1 +
 Documentation/media/v4l-drivers/qcom_camss.rst | 124 +++++++++++++++++++++++++
 2 files changed, 125 insertions(+)
 create mode 100644 Documentation/media/v4l-drivers/qcom_camss.rst

diff --git a/Documentation/media/v4l-drivers/index.rst b/Documentation/media/v4l-drivers/index.rst
index aac566f..ba2aaeb 100644
--- a/Documentation/media/v4l-drivers/index.rst
+++ b/Documentation/media/v4l-drivers/index.rst
@@ -45,6 +45,7 @@ For more details see the file COPYING in the source distribution of Linux.
 	omap4_camera
 	pvrusb2
 	pxa_camera
+	qcom_camss
 	radiotrack
 	saa7134
 	sh_mobile_ceu_camera
diff --git a/Documentation/media/v4l-drivers/qcom_camss.rst b/Documentation/media/v4l-drivers/qcom_camss.rst
new file mode 100644
index 0000000..4707ea7
--- /dev/null
+++ b/Documentation/media/v4l-drivers/qcom_camss.rst
@@ -0,0 +1,124 @@
+.. include:: <isonum.txt>
+
+Qualcomm Camera Subsystem driver
+================================
+
+Introduction
+------------
+
+This file documents the Qualcomm Camera Subsystem driver located under
+drivers/media/platform/qcom/camss-8x16.
+
+The current version of the driver supports the Camera Subsystem found on
+Qualcomm MSM8916 and APQ8016 processors.
+
+The driver implements V4L2, Media controller and V4L2 subdev interfaces.
+Camera sensor using V4L2 subdev interface in the kernel is supported.
+
+The driver is implemented using as a reference the Qualcomm Camera Subsystem
+driver for Android as found in Code Aurora [#f1]_.
+
+
+Qualcomm Camera Subsystem hardware
+----------------------------------
+
+The Camera Subsystem hardware found on 8x16 processors and supported by the
+driver consists of:
+
+- 2 CSIPHY modules. They handle the Physical layer of the CSI2 receivers.
+  A separate camera sensor can be connected to each of the CSIPHY module;
+- 2 CSID (CSI Decoder) modules. They handle the Protocol and Application layer
+  of the CSI2 receivers. A CSID can decode data stream from any of the CSIPHY.
+  Each CSID also contains a TG (Test Generator) block which can generate
+  artificial input data for test purposes;
+- ISPIF (ISP Interface) module. Handles the routing of the data streams from
+  the CSIDs to the inputs of the VFE;
+- VFE (Video Front End) module. Contains a pipeline of image processing hardware
+  blocks. The VFE has different input interfaces. The PIX input interface feeds
+  the input data to the image processing pipeline. Three RDI input interfaces
+  bypass the image processing pipeline. The VFE also contains the AXI bus
+  interface which writes the output data to memory.
+
+
+Supported functionality
+-----------------------
+
+The current version of the driver supports:
+
+- input from camera sensor via CSIPHY;
+- generation of test input data by the TG in CSID;
+- raw dump of the input data to memory. RDI interface of VFE is supported.
+  PIX interface (ISP processing, statistics engines, resize/crop, format
+  conversion) is not supported in the current version;
+- concurrent and independent usage of two data inputs - could be camera sensors
+  and/or TG.
+
+
+Driver Architecture and Design
+------------------------------
+
+The driver implements the V4L2 subdev interface. With the goal to model the
+hardware links between the modules and to expose a clean, logical and usable
+interface, the driver is split into V4L2 sub-devices as follows:
+
+- 2 CSIPHY sub-devices - each CSIPHY is represented by a single sub-device;
+- 2 CSID sub-devices - each CSID is represented by a single sub-device;
+- 2 ISPIF sub-devices - ISPIF is represented by a number of sub-devices equal
+  to the number of CSID sub-devices;
+- 3 VFE sub-devices - VFE is represented by a number of sub-devices equal to
+  the number of RDI input interfaces.
+
+The considerations to split the driver in this particular way are as follows:
+
+- representing CSIPHY and CSID modules by a separate sub-device for each module
+  allows to model the hardware links between these modules;
+- representing VFE by a separate sub-devices for each RDI input interface allows
+  to use the three RDI interfaces concurently and independently as this is
+  supported by the hardware;
+- representing ISPIF by a number of sub-devices equal to the number of CSID
+  sub-devices allows to create linear media controller pipelines when using two
+  cameras simultaneously. This avoids branches in the pipelines which otherwise
+  will require a) userspace and b) media framework (e.g. power on/off
+  operations) to  make assumptions about the data flow from a sink pad to a
+  source pad on a single media entity.
+
+Each VFE sub-device is linked to a separate video device node.
+
+The complete list of the media entities (V4L2 sub-devices and video device
+nodes) is as follows:
+
+- msm_csiphy0
+- msm_csiphy1
+- msm_csid0
+- msm_csid1
+- msm_ispif0
+- msm_ispif1
+- msm_vfe0_rdi0
+- msm_vfe0_video0
+- msm_vfe0_rdi1
+- msm_vfe0_video1
+- msm_vfe0_rdi2
+- msm_vfe0_video2
+
+
+Implementation
+--------------
+
+Runtime configuration of the hardware (updating settings while streaming) is
+not required to implement the currently supported functionality. The complete
+configuration on each hardware module is applied on STREAMON ioctl based on
+the current active media links, formats and controls set.
+
+
+Documentation
+-------------
+
+APQ8016 Specification:
+https://developer.qualcomm.com/download/sd410/snapdragon-410-processor-device-specification.pdf
+Referenced 2016-11-24.
+
+
+References
+----------
+
+.. [#f1] https://source.codeaurora.org/quic/la/kernel/msm-3.10/
-- 
1.9.1

[PATCH 04/10] media: camss: Add CSIPHY files

[Todor Tomov]

These files control the CSIPHY modules which are responsible for the physical
layer of the CSI2 receivers.

Signed-off-by: Todor Tomov <todor.tomov@linaro.org>
---
 drivers/media/platform/qcom/camss-8x16/csiphy.c | 685 ++++++++++++++++++++++++
 drivers/media/platform/qcom/camss-8x16/csiphy.h |  77 +++
 2 files changed, 762 insertions(+)
 create mode 100644 drivers/media/platform/qcom/camss-8x16/csiphy.c
 create mode 100644 drivers/media/platform/qcom/camss-8x16/csiphy.h

diff --git a/drivers/media/platform/qcom/camss-8x16/csiphy.c b/drivers/media/platform/qcom/camss-8x16/csiphy.c
new file mode 100644
index 0000000..42ec7da
--- /dev/null
+++ b/drivers/media/platform/qcom/camss-8x16/csiphy.c
@@ -0,0 +1,685 @@
+/*
+ * csiphy.c
+ *
+ * Qualcomm MSM Camera Subsystem - CSIPHY Module
+ *
+ * Copyright (c) 2011-2015, The Linux Foundation. All rights reserved.
+ * Copyright (C) 2016 Linaro Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only 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.
+ */
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <media/media-entity.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-subdev.h>
+
+#include "csiphy.h"
+#include "camss.h"
+
+#define MSM_CSIPHY_NAME "msm_csiphy"
+
+#define CAMSS_CSI_PHY_LNn_CFG2(n)		(0x004 + 0x40 * (n))
+#define CAMSS_CSI_PHY_LNn_CFG3(n)		(0x008 + 0x40 * (n))
+#define CAMSS_CSI_PHY_GLBL_RESET		0x140
+#define CAMSS_CSI_PHY_GLBL_PWR_CFG		0x144
+#define CAMSS_CSI_PHY_GLBL_IRQ_CMD		0x164
+#define CAMSS_CSI_PHY_HW_VERSION		0x188
+#define CAMSS_CSI_PHY_INTERRUPT_STATUSn(n)	(0x18c + 0x4 * (n))
+#define CAMSS_CSI_PHY_INTERRUPT_MASKn(n)	(0x1ac + 0x4 * (n))
+#define CAMSS_CSI_PHY_INTERRUPT_CLEARn(n)	(0x1cc + 0x4 * (n))
+#define CAMSS_CSI_PHY_GLBL_T_INIT_CFG0		0x1ec
+#define CAMSS_CSI_PHY_T_WAKEUP_CFG0		0x1f4
+
+static const u32 csiphy_formats[] = {
+	MEDIA_BUS_FMT_UYVY8_2X8,
+	MEDIA_BUS_FMT_VYUY8_2X8,
+	MEDIA_BUS_FMT_YUYV8_2X8,
+	MEDIA_BUS_FMT_YVYU8_2X8,
+	MEDIA_BUS_FMT_SBGGR8_1X8,
+	MEDIA_BUS_FMT_SGBRG8_1X8,
+	MEDIA_BUS_FMT_SGRBG8_1X8,
+	MEDIA_BUS_FMT_SRGGB8_1X8,
+	MEDIA_BUS_FMT_SBGGR10_1X10,
+	MEDIA_BUS_FMT_SGBRG10_1X10,
+	MEDIA_BUS_FMT_SGRBG10_1X10,
+	MEDIA_BUS_FMT_SRGGB10_1X10,
+	MEDIA_BUS_FMT_SBGGR12_1X12,
+	MEDIA_BUS_FMT_SGBRG12_1X12,
+	MEDIA_BUS_FMT_SGRBG12_1X12,
+	MEDIA_BUS_FMT_SRGGB12_1X12,
+};
+
+/*
+ * csiphy_isr - CSIPHY module interrupt handler
+ * @irq: Interrupt line
+ * @dev: CSIPHY device
+ *
+ * Return IRQ_HANDLED on success
+ */
+static irqreturn_t csiphy_isr(int irq, void *dev)
+{
+	struct csiphy_device *csiphy = dev;
+	u8 i;
+
+	for (i = 0; i < 8; i++) {
+		u8 val = readl_relaxed(csiphy->base +
+				       CAMSS_CSI_PHY_INTERRUPT_STATUSn(i));
+		writel_relaxed(val, csiphy->base +
+			       CAMSS_CSI_PHY_INTERRUPT_CLEARn(i));
+		writel_relaxed(0x1, csiphy->base + CAMSS_CSI_PHY_GLBL_IRQ_CMD);
+		writel_relaxed(0x0, csiphy->base + CAMSS_CSI_PHY_GLBL_IRQ_CMD);
+		writel_relaxed(0x0, csiphy->base +
+			       CAMSS_CSI_PHY_INTERRUPT_CLEARn(i));
+	}
+
+	return IRQ_HANDLED;
+}
+
+/*
+ * csiphy_reset - Perform software reset on CSIPHY module
+ * @csiphy: CSIPHY device
+ */
+static void csiphy_reset(struct csiphy_device *csiphy)
+{
+	writel_relaxed(0x1, csiphy->base + CAMSS_CSI_PHY_GLBL_RESET);
+	usleep_range(5000, 8000);
+	writel_relaxed(0x0, csiphy->base + CAMSS_CSI_PHY_GLBL_RESET);
+}
+
+/*
+ * csiphy_set_power - Power on/off CSIPHY module
+ * @sd: CSIPHY V4L2 subdevice
+ * @on: Requested power state
+ *
+ * Return 0 on success or a negative error code otherwise
+ */
+static int csiphy_set_power(struct v4l2_subdev *sd, int on)
+{
+	struct csiphy_device *csiphy = v4l2_get_subdevdata(sd);
+	struct device *dev = to_device_index(csiphy, csiphy->id);
+	int ret;
+
+	if (on) {
+		u8 hw_version;
+
+		ret = camss_enable_clocks(csiphy->nclocks, csiphy->clock, dev);
+		if (ret < 0)
+			return ret;
+
+		enable_irq(csiphy->irq);
+
+		csiphy_reset(csiphy);
+
+		hw_version = readl_relaxed(csiphy->base +
+					   CAMSS_CSI_PHY_HW_VERSION);
+		dev_dbg(dev, "CSIPHY HW Version = 0x%02x\n", hw_version);
+	} else {
+		disable_irq(csiphy->irq);
+
+		camss_disable_clocks(csiphy->nclocks, csiphy->clock);
+	}
+
+	return 0;
+}
+
+/*
+ * csiphy_get_lane_mask - Calculate CSI2 lane mask configuration parameter
+ * @lane_cfg - CSI2 lane configuration
+ *
+ * Return lane mask
+ */
+static u8 csiphy_get_lane_mask(struct csiphy_lanes_cfg *lane_cfg)
+{
+	u8 lane_mask;
+	int i;
+
+	lane_mask = 1 << lane_cfg->clk.pos;
+
+	for (i = 0; i < lane_cfg->num_data; i++)
+		lane_mask |= 1 << lane_cfg->data[i].pos;
+
+	return lane_mask;
+}
+
+/*
+ * csiphy_stream_on - Enable streaming on CSIPHY module
+ * @csiphy: CSIPHY device
+ *
+ * Helper function to enable streaming on CSIPHY module.
+ * Main configuration of CSIPHY module is also done here.
+ */
+static void csiphy_stream_on(struct csiphy_device *csiphy)
+{
+	struct csiphy_config *cfg = &csiphy->cfg;
+	u8 lane_mask = csiphy_get_lane_mask(&cfg->csi2->lane_cfg);
+	u8 val;
+	int i = 0;
+
+	val = readl_relaxed(csiphy->base_clk_mux);
+	if (cfg->combo_mode && (lane_mask & 0x18) == 0x18) {
+		val &= ~0xf0;
+		val |= cfg->csid_id << 4;
+	} else {
+		val &= ~0xf;
+		val |= cfg->csid_id;
+	}
+	writel_relaxed(val, csiphy->base_clk_mux);
+
+	writel_relaxed(0x1, csiphy->base +
+		       CAMSS_CSI_PHY_GLBL_T_INIT_CFG0);
+	writel_relaxed(0x1, csiphy->base +
+		       CAMSS_CSI_PHY_T_WAKEUP_CFG0);
+
+	val = 0x1;
+	val |= lane_mask << 1;
+	writel_relaxed(val, csiphy->base + CAMSS_CSI_PHY_GLBL_PWR_CFG);
+
+	val = cfg->combo_mode << 4;
+	writel_relaxed(val, csiphy->base + CAMSS_CSI_PHY_GLBL_RESET);
+
+	while (lane_mask) {
+		if (lane_mask & 0x1) {
+			writel_relaxed(0x10, csiphy->base +
+				       CAMSS_CSI_PHY_LNn_CFG2(i));
+			writel_relaxed(cfg->csi2->settle_cnt, csiphy->base +
+				       CAMSS_CSI_PHY_LNn_CFG3(i));
+			writel_relaxed(0x3f, csiphy->base +
+				       CAMSS_CSI_PHY_INTERRUPT_MASKn(i));
+			writel_relaxed(0x3f, csiphy->base +
+				       CAMSS_CSI_PHY_INTERRUPT_CLEARn(i));
+		}
+
+		lane_mask >>= 1;
+		i++;
+	}
+}
+
+/*
+ * csiphy_stream_off - Disable streaming on CSIPHY module
+ * @csiphy: CSIPHY device
+ *
+ * Helper function to disable streaming on CSIPHY module
+ */
+static void csiphy_stream_off(struct csiphy_device *csiphy)
+{
+	u8 lane_mask = csiphy_get_lane_mask(&csiphy->cfg.csi2->lane_cfg);
+	int i = 0;
+
+	while (lane_mask) {
+		if (lane_mask & 0x1)
+			writel_relaxed(0x0, csiphy->base +
+				       CAMSS_CSI_PHY_LNn_CFG2(i));
+
+		lane_mask >>= 1;
+		i++;
+	}
+
+	writel_relaxed(0x0, csiphy->base + CAMSS_CSI_PHY_GLBL_PWR_CFG);
+}
+
+
+/*
+ * csiphy_set_stream - Enable/disable streaming on CSIPHY module
+ * @sd: CSIPHY V4L2 subdevice
+ * @enable: Requested streaming state
+ *
+ * Return 0 (awlays succeeds)
+ */
+static int csiphy_set_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct csiphy_device *csiphy = v4l2_get_subdevdata(sd);
+
+	if (enable)
+		csiphy_stream_on(csiphy);
+	else
+		csiphy_stream_off(csiphy);
+
+	return 0;
+}
+
+/*
+ * __csiphy_get_format - Get pointer to format structure
+ * @csiphy: CSIPHY device
+ * @cfg: V4L2 subdev pad configuration
+ * @pad: pad from which format is requested
+ * @which: TRY or ACTIVE format
+ *
+ * Return pointer to TRY or ACTIVE format structure
+ */
+static struct v4l2_mbus_framefmt *
+__csiphy_get_format(struct csiphy_device *csiphy,
+		    struct v4l2_subdev_pad_config *cfg,
+		    unsigned int pad,
+		    enum v4l2_subdev_format_whence which)
+{
+	if (which == V4L2_SUBDEV_FORMAT_TRY)
+		return v4l2_subdev_get_try_format(&csiphy->subdev, cfg, pad);
+
+	return &csiphy->fmt[pad];
+}
+
+/*
+ * csiphy_try_format - Handle try format by pad subdev method
+ * @csiphy: CSIPHY device
+ * @cfg: V4L2 subdev pad configuration
+ * @pad: pad on which format is requested
+ * @fmt: pointer to v4l2 format structure
+ * @which: wanted subdev format
+ */
+static void csiphy_try_format(struct csiphy_device *csiphy,
+			      struct v4l2_subdev_pad_config *cfg,
+			      unsigned int pad,
+			      struct v4l2_mbus_framefmt *fmt,
+			      enum v4l2_subdev_format_whence which)
+{
+	unsigned int i;
+
+	switch (pad) {
+	case MSM_CSIPHY_PAD_SINK:
+		/* Set format on sink pad */
+
+		for (i = 0; i < ARRAY_SIZE(csiphy_formats); i++)
+			if (fmt->code == csiphy_formats[i])
+				break;
+
+		/* If not found, use UYVY as default */
+		if (i >= ARRAY_SIZE(csiphy_formats))
+			fmt->code = MEDIA_BUS_FMT_UYVY8_2X8;
+
+		fmt->width = clamp_t(u32, fmt->width, 1, 8191);
+		fmt->height = clamp_t(u32, fmt->height, 1, 8191);
+
+		if (fmt->field == V4L2_FIELD_ANY)
+			fmt->field = V4L2_FIELD_NONE;
+
+		fmt->colorspace = V4L2_COLORSPACE_SRGB;
+
+		break;
+
+	case MSM_CSIPHY_PAD_SRC:
+		/* Set and return a format same as sink pad */
+
+		*fmt = *__csiphy_get_format(csiphy, cfg, MSM_CSID_PAD_SINK,
+					    which);
+
+		break;
+	}
+}
+
+/*
+ * csiphy_enum_mbus_code - Handle pixel format enumeration
+ * @sd: CSIPHY V4L2 subdevice
+ * @cfg: V4L2 subdev pad configuration
+ * @code: pointer to v4l2_subdev_mbus_code_enum structure
+ * return -EINVAL or zero on success
+ */
+static int csiphy_enum_mbus_code(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_pad_config *cfg,
+				 struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct csiphy_device *csiphy = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	if (code->pad == MSM_CSIPHY_PAD_SINK) {
+		if (code->index >= ARRAY_SIZE(csiphy_formats))
+			return -EINVAL;
+
+		code->code = csiphy_formats[code->index];
+	} else {
+		if (code->index > 0)
+			return -EINVAL;
+
+		format = __csiphy_get_format(csiphy, cfg, MSM_CSIPHY_PAD_SINK,
+					     code->which);
+
+		code->code = format->code;
+	}
+
+	return 0;
+}
+
+/*
+ * csiphy_enum_frame_size - Handle frame size enumeration
+ * @sd: CSIPHY V4L2 subdevice
+ * @cfg: V4L2 subdev pad configuration
+ * @fse: pointer to v4l2_subdev_frame_size_enum structure
+ * return -EINVAL or zero on success
+ */
+static int csiphy_enum_frame_size(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_pad_config *cfg,
+				  struct v4l2_subdev_frame_size_enum *fse)
+{
+	struct csiphy_device *csiphy = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt format;
+
+	if (fse->index != 0)
+		return -EINVAL;
+
+	format.code = fse->code;
+	format.width = 1;
+	format.height = 1;
+	csiphy_try_format(csiphy, cfg, fse->pad, &format, fse->which);
+	fse->min_width = format.width;
+	fse->min_height = format.height;
+
+	if (format.code != fse->code)
+		return -EINVAL;
+
+	format.code = fse->code;
+	format.width = -1;
+	format.height = -1;
+	csiphy_try_format(csiphy, cfg, fse->pad, &format, fse->which);
+	fse->max_width = format.width;
+	fse->max_height = format.height;
+
+	return 0;
+}
+
+/*
+ * csiphy_get_format - Handle get format by pads subdev method
+ * @sd: CSIPHY V4L2 subdevice
+ * @cfg: V4L2 subdev pad configuration
+ * @fmt: pointer to v4l2 subdev format structure
+ *
+ * Return -EINVAL or zero on success
+ */
+static int csiphy_get_format(struct v4l2_subdev *sd,
+			     struct v4l2_subdev_pad_config *cfg,
+			     struct v4l2_subdev_format *fmt)
+{
+	struct csiphy_device *csiphy = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	format = __csiphy_get_format(csiphy, cfg, fmt->pad, fmt->which);
+	if (format == NULL)
+		return -EINVAL;
+
+	fmt->format = *format;
+
+	return 0;
+}
+
+/*
+ * csiphy_set_format - Handle set format by pads subdev method
+ * @sd: CSIPHY V4L2 subdevice
+ * @cfg: V4L2 subdev pad configuration
+ * @fmt: pointer to v4l2 subdev format structure
+ *
+ * Return -EINVAL or zero on success
+ */
+static int csiphy_set_format(struct v4l2_subdev *sd,
+			     struct v4l2_subdev_pad_config *cfg,
+			     struct v4l2_subdev_format *fmt)
+{
+	struct csiphy_device *csiphy = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	format = __csiphy_get_format(csiphy, cfg, fmt->pad, fmt->which);
+	if (format == NULL)
+		return -EINVAL;
+
+	csiphy_try_format(csiphy, cfg, fmt->pad, &fmt->format, fmt->which);
+	*format = fmt->format;
+
+	/* Propagate the format from sink to source */
+	if (fmt->pad == MSM_CSIPHY_PAD_SINK) {
+		format = __csiphy_get_format(csiphy, cfg, MSM_CSIPHY_PAD_SRC,
+					     fmt->which);
+
+		*format = fmt->format;
+		csiphy_try_format(csiphy, cfg, MSM_CSIPHY_PAD_SRC, format,
+				  fmt->which);
+	}
+
+	return 0;
+}
+
+/*
+ * csiphy_init_formats - Initialize formats on all pads
+ * @sd: CSIPHY V4L2 subdevice
+ * @fh: V4L2 subdev file handle
+ *
+ * Initialize all pad formats with default values.
+ *
+ * Return 0 on success or a negative error code otherwise
+ */
+static int csiphy_init_formats(struct v4l2_subdev *sd,
+			       struct v4l2_subdev_fh *fh)
+{
+	struct v4l2_subdev_format format;
+
+	memset(&format, 0, sizeof(format));
+	format.pad = MSM_CSIPHY_PAD_SINK;
+	format.which = fh ? V4L2_SUBDEV_FORMAT_TRY : V4L2_SUBDEV_FORMAT_ACTIVE;
+	format.format.code = MEDIA_BUS_FMT_UYVY8_2X8;
+	format.format.width = 1920;
+	format.format.height = 1080;
+
+	return csiphy_set_format(sd, fh ? fh->pad : NULL, &format);
+}
+
+/*
+ * msm_csiphy_subdev_init - Initialize CSIPHY device structure and resources
+ * @csiphy: CSIPHY device
+ * @res: CSIPHY module resources table
+ * @id: CSIPHY module id
+ *
+ * Return 0 on success or a negative error code otherwise
+ */
+int msm_csiphy_subdev_init(struct csiphy_device *csiphy,
+			   struct resources *res, u8 id)
+{
+	struct device *dev = to_device_index(csiphy, id);
+	struct platform_device *pdev = container_of(dev,
+						struct platform_device, dev);
+	struct resource *r;
+	int i;
+	int ret;
+
+	csiphy->id = id;
+	csiphy->cfg.combo_mode = 0;
+
+	/* Memory */
+
+	r = platform_get_resource_byname(pdev, IORESOURCE_MEM, res->reg[0]);
+	csiphy->base = devm_ioremap_resource(dev, r);
+	if (IS_ERR(csiphy->base)) {
+		dev_err(dev, "could not map memory\n");
+		return PTR_ERR(csiphy->base);
+	}
+
+	r = platform_get_resource_byname(pdev, IORESOURCE_MEM, res->reg[1]);
+	csiphy->base_clk_mux = devm_ioremap_resource(dev, r);
+	if (IS_ERR(csiphy->base_clk_mux)) {
+		dev_err(dev, "could not map memory\n");
+		return PTR_ERR(csiphy->base_clk_mux);
+	}
+
+	/* Interrupt */
+
+	r = platform_get_resource_byname(pdev, IORESOURCE_IRQ,
+					 res->interrupt[0]);
+	if (!r) {
+		dev_err(dev, "missing IRQ\n");
+		return -EINVAL;
+	}
+
+	csiphy->irq = r->start;
+	snprintf(csiphy->irq_name, sizeof(csiphy->irq_name), "%s_%s%d",
+		 dev_name(dev), MSM_CSIPHY_NAME, csiphy->id);
+	ret = devm_request_irq(dev, csiphy->irq, csiphy_isr,
+			       IRQF_TRIGGER_RISING, csiphy->irq_name, csiphy);
+	if (ret < 0) {
+		dev_err(dev, "request_irq failed\n");
+		return ret;
+	}
+
+	disable_irq(csiphy->irq);
+
+	/* Clocks */
+
+	csiphy->nclocks = 0;
+	while (res->clock[csiphy->nclocks])
+		csiphy->nclocks++;
+
+	csiphy->clock = devm_kzalloc(dev, csiphy->nclocks *
+				     sizeof(*csiphy->clock), GFP_KERNEL);
+	if (!csiphy->clock)
+		return -ENOMEM;
+
+	for (i = 0; i < csiphy->nclocks; i++) {
+		csiphy->clock[i] = devm_clk_get(dev, res->clock[i]);
+		if (IS_ERR(csiphy->clock[i]))
+			return PTR_ERR(csiphy->clock[i]);
+
+		if (res->clock_rate[i]) {
+			long clk_rate = clk_round_rate(csiphy->clock[i],
+						       res->clock_rate[i]);
+			if (clk_rate < 0) {
+				dev_err(to_device_index(csiphy, csiphy->id),
+					"clk round rate failed\n");
+				return -EINVAL;
+			}
+			ret = clk_set_rate(csiphy->clock[i], clk_rate);
+			if (ret < 0) {
+				dev_err(to_device_index(csiphy, csiphy->id),
+					"clk set rate failed\n");
+				return ret;
+			}
+		}
+	}
+
+	return 0;
+}
+
+/*
+ * csiphy_link_setup - Setup CSIPHY connections
+ * @entity: Pointer to media entity structure
+ * @local: Pointer to local pad
+ * @remote: Pointer to remote pad
+ * @flags: Link flags
+ *
+ * Rreturn 0 on success
+ */
+static int csiphy_link_setup(struct media_entity *entity,
+			     const struct media_pad *local,
+			     const struct media_pad *remote, u32 flags)
+{
+	if ((local->flags & MEDIA_PAD_FL_SOURCE) &&
+	    (flags & MEDIA_LNK_FL_ENABLED)) {
+		struct v4l2_subdev *sd;
+		struct csiphy_device *csiphy;
+		struct csid_device *csid;
+
+		if (media_entity_remote_pad((struct media_pad *)local))
+			return -EBUSY;
+
+		sd = container_of(entity, struct v4l2_subdev, entity);
+		csiphy = v4l2_get_subdevdata(sd);
+
+		sd = container_of(remote->entity, struct v4l2_subdev, entity);
+		csid = v4l2_get_subdevdata(sd);
+
+		csiphy->cfg.csid_id = csid->id;
+	}
+
+	return 0;
+}
+
+static const struct v4l2_subdev_core_ops csiphy_core_ops = {
+	.s_power = csiphy_set_power,
+};
+
+static const struct v4l2_subdev_video_ops csiphy_video_ops = {
+	.s_stream = csiphy_set_stream,
+};
+
+static const struct v4l2_subdev_pad_ops csiphy_pad_ops = {
+	.enum_mbus_code = csiphy_enum_mbus_code,
+	.enum_frame_size = csiphy_enum_frame_size,
+	.get_fmt = csiphy_get_format,
+	.set_fmt = csiphy_set_format,
+};
+
+static const struct v4l2_subdev_ops csiphy_v4l2_ops = {
+	.core = &csiphy_core_ops,
+	.video = &csiphy_video_ops,
+	.pad = &csiphy_pad_ops,
+};
+
+static const struct v4l2_subdev_internal_ops csiphy_v4l2_internal_ops = {
+	.open = csiphy_init_formats,
+};
+
+static const struct media_entity_operations csiphy_media_ops = {
+	.link_setup = csiphy_link_setup,
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+/*
+ * msm_csiphy_register_entity - Register subdev node for CSIPHY module
+ * @csiphy: CSIPHY device
+ * @v4l2_dev: V4L2 device
+ *
+ * Return 0 on success or a negative error code otherwise
+ */
+int msm_csiphy_register_entity(struct csiphy_device *csiphy,
+			       struct v4l2_device *v4l2_dev)
+{
+	struct v4l2_subdev *sd = &csiphy->subdev;
+	struct media_pad *pads = csiphy->pads;
+	struct device *dev = to_device_index(csiphy, csiphy->id);
+	int ret;
+
+	v4l2_subdev_init(sd, &csiphy_v4l2_ops);
+	sd->internal_ops = &csiphy_v4l2_internal_ops;
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	snprintf(sd->name, ARRAY_SIZE(sd->name), "%s%d",
+		 MSM_CSIPHY_NAME, csiphy->id);
+	v4l2_set_subdevdata(sd, csiphy);
+
+	ret = csiphy_init_formats(sd, NULL);
+	if (ret < 0) {
+		dev_err(dev, "Failed to init format\n");
+		return ret;
+	}
+
+	pads[MSM_CSIPHY_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
+	pads[MSM_CSIPHY_PAD_SRC].flags = MEDIA_PAD_FL_SOURCE;
+
+	sd->entity.ops = &csiphy_media_ops;
+	ret = media_entity_pads_init(&sd->entity, MSM_CSIPHY_PADS_NUM, pads);
+	if (ret < 0) {
+		dev_err(dev, "Failed to init media entity\n");
+		return ret;
+	}
+
+	ret = v4l2_device_register_subdev(v4l2_dev, sd);
+	if (ret < 0) {
+		dev_err(dev, "Failed to register subdev\n");
+		media_entity_cleanup(&sd->entity);
+	}
+
+	return ret;
+}
+
+/*
+ * msm_csiphy_unregister_entity - Unregister CSIPHY module subdev node
+ * @csiphy: CSIPHY device
+ */
+void msm_csiphy_unregister_entity(struct csiphy_device *csiphy)
+{
+	v4l2_device_unregister_subdev(&csiphy->subdev);
+}
diff --git a/drivers/media/platform/qcom/camss-8x16/csiphy.h b/drivers/media/platform/qcom/camss-8x16/csiphy.h
new file mode 100644
index 0000000..60330a8
--- /dev/null
+++ b/drivers/media/platform/qcom/camss-8x16/csiphy.h
@@ -0,0 +1,77 @@
+/*
+ * csiphy.h
+ *
+ * Qualcomm MSM Camera Subsystem - CSIPHY Module
+ *
+ * Copyright (c) 2011-2015, The Linux Foundation. All rights reserved.
+ * Copyright (C) 2016 Linaro Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only 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.
+ */
+#ifndef QC_MSM_CAMSS_CSIPHY_H
+#define QC_MSM_CAMSS_CSIPHY_H
+
+#include <linux/clk.h>
+#include <media/media-entity.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-mediabus.h>
+#include <media/v4l2-subdev.h>
+
+#define MSM_CSIPHY_PAD_SINK 0
+#define MSM_CSIPHY_PAD_SRC 1
+#define MSM_CSIPHY_PADS_NUM 2
+
+struct csiphy_lane {
+	u8 pos;
+	u8 pol;
+};
+
+struct csiphy_lanes_cfg {
+	int num_data;
+	struct csiphy_lane *data;
+	struct csiphy_lane clk;
+};
+
+struct csiphy_csi2_cfg {
+	int settle_cnt;
+	struct csiphy_lanes_cfg lane_cfg;
+};
+
+struct csiphy_config {
+	u8 combo_mode;
+	u8 csid_id;
+	struct csiphy_csi2_cfg *csi2;
+};
+
+struct csiphy_device {
+	u8 id;
+	struct v4l2_subdev subdev;
+	struct media_pad pads[MSM_CSIPHY_PADS_NUM];
+	void __iomem *base;
+	void __iomem *base_clk_mux;
+	u32 irq;
+	char irq_name[30];
+	struct clk **clock;
+	int nclocks;
+	struct csiphy_config cfg;
+	struct v4l2_mbus_framefmt fmt[MSM_CSIPHY_PADS_NUM];
+};
+
+struct resources;
+
+int msm_csiphy_subdev_init(struct csiphy_device *csiphy,
+			   struct resources *res, u8 id);
+
+int msm_csiphy_register_entity(struct csiphy_device *csiphy,
+			       struct v4l2_device *v4l2_dev);
+
+void msm_csiphy_unregister_entity(struct csiphy_device *csiphy);
+
+#endif /* QC_MSM_CAMSS_CSIPHY_H */
-- 
1.9.1

[PATCH 05/10] media: camss: Add CSID files

[Todor Tomov]

These files control the CSID modules which handle the protocol and application
layer of the CSI2 receivers.

Signed-off-by: Todor Tomov <todor.tomov@linaro.org>
---
 drivers/media/platform/qcom/camss-8x16/csid.c | 1071 +++++++++++++++++++++++++
 drivers/media/platform/qcom/camss-8x16/csid.h |   82 ++
 2 files changed, 1153 insertions(+)
 create mode 100644 drivers/media/platform/qcom/camss-8x16/csid.c
 create mode 100644 drivers/media/platform/qcom/camss-8x16/csid.h

diff --git a/drivers/media/platform/qcom/camss-8x16/csid.c b/drivers/media/platform/qcom/camss-8x16/csid.c
new file mode 100644
index 0000000..c7b8b88
--- /dev/null
+++ b/drivers/media/platform/qcom/camss-8x16/csid.c
@@ -0,0 +1,1071 @@
+/*
+ * csid.c
+ *
+ * Qualcomm MSM Camera Subsystem - CSID Module
+ *
+ * Copyright (c) 2011-2015, The Linux Foundation. All rights reserved.
+ * Copyright (C) 2015-2016 Linaro Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only 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.
+ */
+#include <linux/clk.h>
+#include <linux/completion.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/consumer.h>
+#include <media/media-entity.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-subdev.h>
+
+#include "csid.h"
+#include "camss.h"
+
+#define MSM_CSID_NAME "msm_csid"
+
+#define CAMSS_CSID_HW_VERSION		0x0
+#define CAMSS_CSID_CORE_CTRL_0		0x004
+#define CAMSS_CSID_CORE_CTRL_1		0x008
+#define CAMSS_CSID_RST_CMD		0x00c
+#define CAMSS_CSID_CID_LUT_VC_n(n)	(0x010 + 0x4 * (n))
+#define CAMSS_CSID_CID_n_CFG(n)		(0x020 + 0x4 * (n))
+#define CAMSS_CSID_IRQ_CLEAR_CMD	0x060
+#define CAMSS_CSID_IRQ_MASK		0x064
+#define CAMSS_CSID_IRQ_STATUS		0x068
+#define CAMSS_CSID_TG_CTRL		0x0a0
+#define CAMSS_CSID_TG_VC_CFG		0x0a4
+#define CAMSS_CSID_TG_VC_CFG_H_BLANKING		0x3ff
+#define CAMSS_CSID_TG_VC_CFG_V_BLANKING		0x7f
+#define CAMSS_CSID_TG_DT_n_CGG_0(n)	(0x0ac + 0xc * (n))
+#define CAMSS_CSID_TG_DT_n_CGG_1(n)	(0x0b0 + 0xc * (n))
+#define CAMSS_CSID_TG_DT_n_CGG_2(n)	(0x0b4 + 0xc * (n))
+
+#define DATA_TYPE_EMBEDDED_DATA_8BIT	0x12
+#define DATA_TYPE_YUV422_8BIT		0x1e
+#define DATA_TYPE_RAW_6BIT		0x28
+#define DATA_TYPE_RAW_8BIT		0x2a
+#define DATA_TYPE_RAW_10BIT		0x2b
+#define DATA_TYPE_RAW_12BIT		0x2c
+
+#define DECODE_FORMAT_UNCOMPRESSED_6_BIT	0x0
+#define DECODE_FORMAT_UNCOMPRESSED_8_BIT	0x1
+#define DECODE_FORMAT_UNCOMPRESSED_10_BIT	0x2
+#define DECODE_FORMAT_UNCOMPRESSED_12_BIT	0x3
+
+#define CSID_RESET_TIMEOUT_MS 500
+
+static const struct {
+	u32 code;
+	u32 uncompressed;
+	u8 data_type;
+	u8 decode_format;
+	u8 uncompr_bpp;
+} csid_input_fmts[] = {
+	{
+		MEDIA_BUS_FMT_UYVY8_2X8,
+		MEDIA_BUS_FMT_UYVY8_2X8,
+		DATA_TYPE_YUV422_8BIT,
+		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
+		16
+	},
+	{
+		MEDIA_BUS_FMT_VYUY8_2X8,
+		MEDIA_BUS_FMT_VYUY8_2X8,
+		DATA_TYPE_YUV422_8BIT,
+		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
+		16
+	},
+	{
+		MEDIA_BUS_FMT_YUYV8_2X8,
+		MEDIA_BUS_FMT_YUYV8_2X8,
+		DATA_TYPE_YUV422_8BIT,
+		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
+		16
+	},
+	{
+		MEDIA_BUS_FMT_YVYU8_2X8,
+		MEDIA_BUS_FMT_YVYU8_2X8,
+		DATA_TYPE_YUV422_8BIT,
+		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
+		16
+	},
+	{
+		MEDIA_BUS_FMT_SBGGR8_1X8,
+		MEDIA_BUS_FMT_SBGGR8_1X8,
+		DATA_TYPE_RAW_8BIT,
+		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
+		8
+	},
+	{
+		MEDIA_BUS_FMT_SGBRG8_1X8,
+		MEDIA_BUS_FMT_SGBRG8_1X8,
+		DATA_TYPE_RAW_8BIT,
+		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
+		8
+	},
+	{
+		MEDIA_BUS_FMT_SGRBG8_1X8,
+		MEDIA_BUS_FMT_SGRBG8_1X8,
+		DATA_TYPE_RAW_8BIT,
+		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
+		8
+	},
+	{
+		MEDIA_BUS_FMT_SRGGB8_1X8,
+		MEDIA_BUS_FMT_SRGGB8_1X8,
+		DATA_TYPE_RAW_8BIT,
+		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
+		8
+	},
+	{
+		MEDIA_BUS_FMT_SBGGR10_1X10,
+		MEDIA_BUS_FMT_SBGGR10_1X10,
+		DATA_TYPE_RAW_10BIT,
+		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
+		10
+	},
+	{
+		MEDIA_BUS_FMT_SGBRG10_1X10,
+		MEDIA_BUS_FMT_SGBRG10_1X10,
+		DATA_TYPE_RAW_10BIT,
+		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
+		10
+	},
+	{
+		MEDIA_BUS_FMT_SGRBG10_1X10,
+		MEDIA_BUS_FMT_SGRBG10_1X10,
+		DATA_TYPE_RAW_10BIT,
+		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
+		10
+	},
+	{
+		MEDIA_BUS_FMT_SRGGB10_1X10,
+		MEDIA_BUS_FMT_SRGGB10_1X10,
+		DATA_TYPE_RAW_10BIT,
+		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
+		10
+	},
+	{
+		MEDIA_BUS_FMT_SBGGR12_1X12,
+		MEDIA_BUS_FMT_SBGGR12_1X12,
+		DATA_TYPE_RAW_12BIT,
+		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
+		12
+	},
+	{
+		MEDIA_BUS_FMT_SGBRG12_1X12,
+		MEDIA_BUS_FMT_SGBRG12_1X12,
+		DATA_TYPE_RAW_12BIT,
+		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
+		12
+	},
+	{
+		MEDIA_BUS_FMT_SGRBG12_1X12,
+		MEDIA_BUS_FMT_SGRBG12_1X12,
+		DATA_TYPE_RAW_12BIT,
+		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
+		12
+	},
+	{
+		MEDIA_BUS_FMT_SRGGB12_1X12,
+		MEDIA_BUS_FMT_SRGGB12_1X12,
+		DATA_TYPE_RAW_12BIT,
+		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
+		12
+	}
+};
+
+/*
+ * csid_isr - CSID module interrupt handler
+ * @irq: Interrupt line
+ * @dev: CSID device
+ *
+ * Return IRQ_HANDLED on success
+ */
+static irqreturn_t csid_isr(int irq, void *dev)
+{
+	struct csid_device *csid = dev;
+	u32 value;
+
+	value = readl_relaxed(csid->base + CAMSS_CSID_IRQ_STATUS);
+	writel_relaxed(value, csid->base + CAMSS_CSID_IRQ_CLEAR_CMD);
+
+	if ((value >> 11) & 0x1)
+		complete(&csid->reset_complete);
+
+	return IRQ_HANDLED;
+}
+
+/*
+ * csid_reset - Trigger reset on CSID module and wait to complete
+ * @csid: CSID device
+ *
+ * Return 0 on success or a negative error code otherwise
+ */
+static int csid_reset(struct csid_device *csid)
+{
+	unsigned long time;
+
+	reinit_completion(&csid->reset_complete);
+
+	writel_relaxed(0x7fff, csid->base + CAMSS_CSID_RST_CMD);
+
+	time = wait_for_completion_timeout(&csid->reset_complete,
+		msecs_to_jiffies(CSID_RESET_TIMEOUT_MS));
+	if (!time) {
+		dev_err(to_device_index(csid, csid->id),
+			"CSID reset timeout\n");
+		return -EIO;
+	}
+
+	return 0;
+}
+
+/*
+ * csid_set_power - Power on/off CSID module
+ * @sd: CSID V4L2 subdevice
+ * @on: Requested power state
+ *
+ * Return 0 on success or a negative error code otherwise
+ */
+static int csid_set_power(struct v4l2_subdev *sd, int on)
+{
+	struct csid_device *csid = v4l2_get_subdevdata(sd);
+	struct device *dev = to_device_index(csid, csid->id);
+	int ret;
+
+	if (on) {
+		u32 hw_version;
+
+		ret = regulator_enable(csid->vdda);
+		if (ret < 0)
+			return ret;
+
+		ret = camss_enable_clocks(csid->nclocks, csid->clock, dev);
+		if (ret < 0) {
+			regulator_disable(csid->vdda);
+			return ret;
+		}
+
+		enable_irq(csid->irq);
+
+		ret = csid_reset(csid);
+		if (ret < 0) {
+			disable_irq(csid->irq);
+			camss_disable_clocks(csid->nclocks, csid->clock);
+			regulator_disable(csid->vdda);
+			return ret;
+		}
+
+		hw_version = readl_relaxed(csid->base + CAMSS_CSID_HW_VERSION);
+		dev_dbg(dev, "CSID HW Version = 0x%08x\n", hw_version);
+	} else {
+		disable_irq(csid->irq);
+		camss_disable_clocks(csid->nclocks, csid->clock);
+		ret = regulator_disable(csid->vdda);
+	}
+
+	return ret;
+}
+
+/*
+ * csid_get_uncompressed - map media bus format to uncompressed media bus format
+ * @code: media bus format code
+ *
+ * Return uncompressed media bus format code
+ */
+static u32 csid_get_uncompressed(u32 code)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(csid_input_fmts); i++)
+		if (code == csid_input_fmts[i].code)
+			break;
+
+	return csid_input_fmts[i].uncompressed;
+}
+
+/*
+ * csid_get_data_type - map media bus format to data type
+ * @code: media bus format code
+ *
+ * Return data type code
+ */
+static u8 csid_get_data_type(u32 code)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(csid_input_fmts); i++)
+		if (code == csid_input_fmts[i].code)
+			break;
+
+	return csid_input_fmts[i].data_type;
+}
+
+/*
+ * csid_get_decode_format - map media bus format to decode format
+ * @code: media bus format code
+ *
+ * Return decode format code
+ */
+static u8 csid_get_decode_format(u32 code)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(csid_input_fmts); i++)
+		if (code == csid_input_fmts[i].code)
+			break;
+
+	return csid_input_fmts[i].decode_format;
+}
+
+/*
+ * csid_get_bpp - map media bus format to bits per pixel
+ * @code: media bus format code
+ *
+ * Return number of bits per pixel
+ */
+static u8 csid_get_bpp(u32 code)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(csid_input_fmts); i++)
+		if (code == csid_input_fmts[i].uncompressed)
+			break;
+
+	return csid_input_fmts[i].uncompr_bpp;
+}
+
+/*
+ * csid_set_stream - Enable/disable streaming on CSID module
+ * @sd: CSID V4L2 subdevice
+ * @enable: Requested streaming state
+ *
+ * Main configuration of CSID module is also done here.
+ *
+ * Return 0 on success or a negative error code otherwise
+ */
+static int csid_set_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct csid_device *csid = v4l2_get_subdevdata(sd);
+	struct csid_testgen_config *tg = &csid->testgen;
+	u32 val;
+
+	if (enable) {
+		u8 vc = 0; /* Virtual Channel 0 */
+		u8 cid = vc * 4;
+		u8 dt, dt_shift, df;
+		int ret;
+
+		ret = v4l2_ctrl_handler_setup(&csid->ctrls);
+		if (ret < 0) {
+			dev_err(to_device_index(csid, csid->id),
+				"could not sync v4l2 controls\n");
+			return ret;
+		}
+
+		if (!tg->enabled &&
+		    !media_entity_remote_pad(&csid->pads[MSM_CSID_PAD_SINK])) {
+			return -ENOLINK;
+		}
+
+		dt = csid_get_data_type(csid->fmt[MSM_CSID_PAD_SRC].code);
+
+		if (tg->enabled) {
+			/* Config Test Generator */
+			u8 bpp = csid_get_bpp(csid->fmt[MSM_CSID_PAD_SRC].code);
+			u32 num_bytes_per_line =
+				csid->fmt[MSM_CSID_PAD_SRC].width * bpp / 8;
+			u32 num_lines = csid->fmt[MSM_CSID_PAD_SRC].height;
+
+			/* 31:24 V blank, 23:13 H blank, 3:2 num of active DT */
+			/* 1:0 VC */
+			val = ((CAMSS_CSID_TG_VC_CFG_V_BLANKING & 0xff) << 24) |
+			      ((CAMSS_CSID_TG_VC_CFG_H_BLANKING & 0x7ff) << 13);
+			writel_relaxed(val, csid->base + CAMSS_CSID_TG_VC_CFG);
+
+			/* 28:16 bytes per lines, 12:0 num of lines */
+			val = ((num_bytes_per_line & 0x1fff) << 16) |
+			      (num_lines & 0x1fff);
+			writel_relaxed(val, csid->base +
+				       CAMSS_CSID_TG_DT_n_CGG_0(0));
+
+			/* 5:0 data type */
+			val = dt;
+			writel_relaxed(val, csid->base +
+				       CAMSS_CSID_TG_DT_n_CGG_1(0));
+
+			/* 2:0 output test pattern */
+			val = tg->payload_mode;
+			writel_relaxed(val, csid->base +
+				       CAMSS_CSID_TG_DT_n_CGG_2(0));
+		} else {
+			struct csid_phy_config *phy = &csid->phy;
+
+			val = phy->lane_cnt - 1;
+			val |= phy->lane_assign << 4;
+
+			writel_relaxed(val,
+				       csid->base + CAMSS_CSID_CORE_CTRL_0);
+
+			val = phy->csiphy_id << 17;
+			val |= 0x9;
+
+			writel_relaxed(val,
+				       csid->base + CAMSS_CSID_CORE_CTRL_1);
+		}
+
+		/* Config LUT */
+
+		dt_shift = (cid % 4) * 8;
+		df = csid_get_decode_format(csid->fmt[MSM_CSID_PAD_SINK].code);
+
+		val = readl_relaxed(csid->base + CAMSS_CSID_CID_LUT_VC_n(vc));
+		val &= ~(0xff << dt_shift);
+		val |= dt << dt_shift;
+		writel_relaxed(val, csid->base + CAMSS_CSID_CID_LUT_VC_n(vc));
+
+		val = (df << 4) | 0x3;
+		writel_relaxed(val, csid->base + CAMSS_CSID_CID_n_CFG(cid));
+
+		if (tg->enabled) {
+			val = 0x00a06437;
+			writel_relaxed(val, csid->base + CAMSS_CSID_TG_CTRL);
+		}
+	} else {
+		if (tg->enabled) {
+			val = 0x00a06436;
+			writel_relaxed(val, csid->base + CAMSS_CSID_TG_CTRL);
+		}
+	}
+
+	return 0;
+}
+
+/*
+ * __csid_get_format - Get pointer to format structure
+ * @csid: CSID device
+ * @cfg: V4L2 subdev pad configuration
+ * @pad: pad from which format is requested
+ * @which: TRY or ACTIVE format
+ *
+ * Return pointer to TRY or ACTIVE format structure
+ */
+static struct v4l2_mbus_framefmt *
+__csid_get_format(struct csid_device *csid,
+		  struct v4l2_subdev_pad_config *cfg,
+		  unsigned int pad,
+		  enum v4l2_subdev_format_whence which)
+{
+	if (which == V4L2_SUBDEV_FORMAT_TRY)
+		return v4l2_subdev_get_try_format(&csid->subdev, cfg, pad);
+
+	return &csid->fmt[pad];
+}
+
+/*
+ * csid_try_format - Handle try format by pad subdev method
+ * @csid: CSID device
+ * @cfg: V4L2 subdev pad configuration
+ * @pad: pad on which format is requested
+ * @fmt: pointer to v4l2 format structure
+ * @which: wanted subdev format
+ */
+static void csid_try_format(struct csid_device *csid,
+			    struct v4l2_subdev_pad_config *cfg,
+			    unsigned int pad,
+			    struct v4l2_mbus_framefmt *fmt,
+			    enum v4l2_subdev_format_whence which)
+{
+	unsigned int i;
+
+	switch (pad) {
+	case MSM_CSID_PAD_SINK:
+		/* Set format on sink pad */
+
+		for (i = 0; i < ARRAY_SIZE(csid_input_fmts); i++)
+			if (fmt->code == csid_input_fmts[i].code)
+				break;
+
+		/* If not found, use UYVY as default */
+		if (i >= ARRAY_SIZE(csid_input_fmts))
+			fmt->code = MEDIA_BUS_FMT_UYVY8_2X8;
+
+		fmt->width = clamp_t(u32, fmt->width, 1, 8191);
+		fmt->height = clamp_t(u32, fmt->height, 1, 8191);
+
+		if (fmt->field == V4L2_FIELD_ANY)
+			fmt->field = V4L2_FIELD_NONE;
+
+		break;
+
+	case MSM_CSID_PAD_SRC:
+		if (csid->testgen_mode->cur.val == 0) {
+			/* Test generator is disabled, keep pad formats */
+			/* in sync - set and return a format same as sink pad */
+			struct v4l2_mbus_framefmt format;
+
+			format = *__csid_get_format(csid, cfg,
+						    MSM_CSID_PAD_SINK, which);
+			format.code = csid_get_uncompressed(format.code);
+			*fmt = format;
+		} else {
+			/* Test generator is enabled, set format on source*/
+			/* pad to allow test generator usage */
+
+			for (i = 0; i < ARRAY_SIZE(csid_input_fmts); i++)
+				if (csid_input_fmts[i].uncompressed ==
+								fmt->code)
+					break;
+
+			/* If not found, use UYVY as default */
+			if (i >= ARRAY_SIZE(csid_input_fmts))
+				fmt->code = MEDIA_BUS_FMT_UYVY8_2X8;
+
+			fmt->width = clamp_t(u32, fmt->width, 1, 8191);
+			fmt->height = clamp_t(u32, fmt->height, 1, 8191);
+
+			fmt->field = V4L2_FIELD_NONE;
+		}
+		break;
+	}
+
+	fmt->colorspace = V4L2_COLORSPACE_SRGB;
+}
+
+/*
+ * csid_enum_mbus_code - Handle pixel format enumeration
+ * @sd: CSID V4L2 subdevice
+ * @cfg: V4L2 subdev pad configuration
+ * @code: pointer to v4l2_subdev_mbus_code_enum structure
+ * return -EINVAL or zero on success
+ */
+static int csid_enum_mbus_code(struct v4l2_subdev *sd,
+			       struct v4l2_subdev_pad_config *cfg,
+			       struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct csid_device *csid = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	if (code->pad == MSM_CSID_PAD_SINK) {
+		if (code->index >= ARRAY_SIZE(csid_input_fmts))
+			return -EINVAL;
+
+		code->code = csid_input_fmts[code->index].code;
+	} else {
+		if (csid->testgen_mode->cur.val == 0) {
+			if (code->index > 0)
+				return -EINVAL;
+
+			format = __csid_get_format(csid, cfg, MSM_CSID_PAD_SINK,
+						   code->which);
+
+			code->code = csid_get_uncompressed(format->code);
+		} else {
+			if (code->index >= ARRAY_SIZE(csid_input_fmts))
+				return -EINVAL;
+
+			code->code = csid_input_fmts[code->index].uncompressed;
+		}
+	}
+
+	return 0;
+}
+
+/*
+ * csid_enum_frame_size - Handle frame size enumeration
+ * @sd: CSID V4L2 subdevice
+ * @cfg: V4L2 subdev pad configuration
+ * @fse: pointer to v4l2_subdev_frame_size_enum structure
+ * return -EINVAL or zero on success
+ */
+static int csid_enum_frame_size(struct v4l2_subdev *sd,
+				struct v4l2_subdev_pad_config *cfg,
+				struct v4l2_subdev_frame_size_enum *fse)
+{
+	struct csid_device *csid = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt format;
+
+	if (fse->index != 0)
+		return -EINVAL;
+
+	format.code = fse->code;
+	format.width = 1;
+	format.height = 1;
+	csid_try_format(csid, cfg, fse->pad, &format, fse->which);
+	fse->min_width = format.width;
+	fse->min_height = format.height;
+
+	if (format.code != fse->code)
+		return -EINVAL;
+
+	format.code = fse->code;
+	format.width = -1;
+	format.height = -1;
+	csid_try_format(csid, cfg, fse->pad, &format, fse->which);
+	fse->max_width = format.width;
+	fse->max_height = format.height;
+
+	return 0;
+}
+
+/*
+ * csid_get_format - Handle get format by pads subdev method
+ * @sd: CSID V4L2 subdevice
+ * @cfg: V4L2 subdev pad configuration
+ * @fmt: pointer to v4l2 subdev format structure
+ *
+ * Return -EINVAL or zero on success
+ */
+static int csid_get_format(struct v4l2_subdev *sd,
+			   struct v4l2_subdev_pad_config *cfg,
+			   struct v4l2_subdev_format *fmt)
+{
+	struct csid_device *csid = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	format = __csid_get_format(csid, cfg, fmt->pad, fmt->which);
+	if (format == NULL)
+		return -EINVAL;
+
+	fmt->format = *format;
+
+	return 0;
+}
+
+/*
+ * csid_set_format - Handle set format by pads subdev method
+ * @sd: CSID V4L2 subdevice
+ * @cfg: V4L2 subdev pad configuration
+ * @fmt: pointer to v4l2 subdev format structure
+ *
+ * Return -EINVAL or zero on success
+ */
+static int csid_set_format(struct v4l2_subdev *sd,
+			   struct v4l2_subdev_pad_config *cfg,
+			   struct v4l2_subdev_format *fmt)
+{
+	struct csid_device *csid = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	format = __csid_get_format(csid, cfg, fmt->pad, fmt->which);
+	if (format == NULL)
+		return -EINVAL;
+
+	csid_try_format(csid, cfg, fmt->pad, &fmt->format, fmt->which);
+	*format = fmt->format;
+
+	/* Propagate the format from sink to source */
+	if (fmt->pad == MSM_CSID_PAD_SINK) {
+		format = __csid_get_format(csid, cfg, MSM_CSID_PAD_SRC,
+					   fmt->which);
+
+		*format = fmt->format;
+		csid_try_format(csid, cfg, MSM_CSID_PAD_SRC, format,
+				fmt->which);
+	}
+
+	return 0;
+}
+
+/*
+ * csid_init_formats - Initialize formats on all pads
+ * @sd: CSID V4L2 subdevice
+ * @fh: V4L2 subdev file handle
+ *
+ * Initialize all pad formats with default values.
+ *
+ * Return 0 on success or a negative error code otherwise
+ */
+static int csid_init_formats(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	struct v4l2_subdev_format format;
+
+	memset(&format, 0, sizeof(format));
+	format.pad = MSM_CSID_PAD_SINK;
+	format.which = fh ? V4L2_SUBDEV_FORMAT_TRY : V4L2_SUBDEV_FORMAT_ACTIVE;
+	format.format.code = MEDIA_BUS_FMT_UYVY8_2X8;
+	format.format.width = 1920;
+	format.format.height = 1080;
+
+	return csid_set_format(sd, fh ? fh->pad : NULL, &format);
+}
+
+static const char * const csid_test_pattern_menu[] = {
+	"Disabled",
+	"Incrementing",
+	"Alternating 55/AA",
+	"All Zeros",
+	"All Ones",
+	"Random Data",
+};
+
+/*
+ * csid_set_test_pattern - Set test generator's pattern mode
+ * @csid: CSID device
+ * @value: desired test pattern mode
+ *
+ * Return 0 on success or a negative error code otherwise
+ */
+static int csid_set_test_pattern(struct csid_device *csid, s32 value)
+{
+	struct csid_testgen_config *tg = &csid->testgen;
+
+	/* If CSID is linked to CSIPHY, do not allow to enable test generator */
+	if (value && media_entity_remote_pad(&csid->pads[MSM_CSID_PAD_SINK]))
+		return -EBUSY;
+
+	tg->enabled = !!value;
+
+	switch (value) {
+	case 1:
+		tg->payload_mode = CSID_PAYLOAD_MODE_INCREMENTING;
+		break;
+	case 2:
+		tg->payload_mode = CSID_PAYLOAD_MODE_ALTERNATING_55_AA;
+		break;
+	case 3:
+		tg->payload_mode = CSID_PAYLOAD_MODE_ALL_ZEROES;
+		break;
+	case 4:
+		tg->payload_mode = CSID_PAYLOAD_MODE_ALL_ONES;
+		break;
+	case 5:
+		tg->payload_mode = CSID_PAYLOAD_MODE_RANDOM;
+		break;
+	}
+
+	return 0;
+}
+
+/*
+ * csid_s_ctrl - Handle set control subdev method
+ * @ctrl: pointer to v4l2 control structure
+ *
+ * Return 0 on success or a negative error code otherwise
+ */
+static int csid_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct csid_device *csid = container_of(ctrl->handler,
+						struct csid_device, ctrls);
+	int ret = -EINVAL;
+
+	switch (ctrl->id) {
+	case V4L2_CID_TEST_PATTERN:
+		ret = csid_set_test_pattern(csid, ctrl->val);
+		break;
+	}
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops csid_ctrl_ops = {
+	.s_ctrl = csid_s_ctrl,
+};
+
+/*
+ * msm_csid_subdev_init - Initialize CSID device structure and resources
+ * @csid: CSID device
+ * @res: CSID module resources table
+ * @id: CSID module id
+ *
+ * Return 0 on success or a negative error code otherwise
+ */
+int msm_csid_subdev_init(struct csid_device *csid,
+			 struct resources *res, u8 id)
+{
+	struct device *dev = to_device_index(csid, id);
+	struct platform_device *pdev = container_of(dev, struct platform_device,
+						    dev);
+	struct resource *r;
+	int i;
+	int ret;
+
+	csid->id = id;
+
+	/* Memory */
+
+	r = platform_get_resource_byname(pdev, IORESOURCE_MEM, res->reg[0]);
+	csid->base = devm_ioremap_resource(dev, r);
+	if (IS_ERR(csid->base)) {
+		dev_err(dev, "could not map memory\n");
+		return PTR_ERR(csid->base);
+	}
+
+	/* Interrupt */
+
+	r = platform_get_resource_byname(pdev, IORESOURCE_IRQ,
+					 res->interrupt[0]);
+	if (!r) {
+		dev_err(dev, "missing IRQ\n");
+		return -EINVAL;
+	}
+
+	csid->irq = r->start;
+	snprintf(csid->irq_name, sizeof(csid->irq_name), "%s_%s%d",
+		 dev_name(dev), MSM_CSID_NAME, csid->id);
+	ret = devm_request_irq(dev, csid->irq, csid_isr,
+		IRQF_TRIGGER_RISING, csid->irq_name, csid);
+	if (ret < 0) {
+		dev_err(dev, "request_irq failed\n");
+		return ret;
+	}
+
+	disable_irq(csid->irq);
+
+	/* Clocks */
+
+	csid->nclocks = 0;
+	while (res->clock[csid->nclocks])
+		csid->nclocks++;
+
+	csid->clock = devm_kzalloc(dev, csid->nclocks * sizeof(*csid->clock),
+				    GFP_KERNEL);
+	if (!csid->clock)
+		return -ENOMEM;
+
+	for (i = 0; i < csid->nclocks; i++) {
+		csid->clock[i] = devm_clk_get(dev, res->clock[i]);
+		if (IS_ERR(csid->clock[i]))
+			return PTR_ERR(csid->clock[i]);
+
+		if (res->clock_rate[i]) {
+			long clk_rate = clk_round_rate(csid->clock[i],
+						       res->clock_rate[i]);
+			if (clk_rate < 0) {
+				dev_err(to_device_index(csid, csid->id),
+					"clk round rate failed\n");
+				return -EINVAL;
+			}
+			ret = clk_set_rate(csid->clock[i], clk_rate);
+			if (ret < 0) {
+				dev_err(to_device_index(csid, csid->id),
+					"clk set rate failed\n");
+				return ret;
+			}
+		}
+	}
+
+	/* Regulator */
+
+	csid->vdda = devm_regulator_get(dev, res->regulator[0]);
+	if (IS_ERR(csid->vdda)) {
+		dev_err(dev, "could not get regulator\n");
+		return PTR_ERR(csid->vdda);
+	}
+
+	init_completion(&csid->reset_complete);
+
+	return 0;
+}
+
+/*
+ * msm_csid_get_csid_id - Get CSID HW module id
+ * @entity: Pointer to CSID media entity structure
+ * @id: Return CSID HW module id here
+ */
+void msm_csid_get_csid_id(struct media_entity *entity, u8 *id)
+{
+	struct v4l2_subdev *sd;
+	struct csid_device *csid;
+
+	sd = container_of(entity, struct v4l2_subdev, entity);
+	csid = v4l2_get_subdevdata(sd);
+
+	*id = csid->id;
+}
+
+/*
+ * csid_get_lane_assign - Calculate CSI2 lane assign configuration parameter
+ * @lane_cfg - CSI2 lane configuration
+ *
+ * Return lane assign
+ */
+static u32 csid_get_lane_assign(struct csiphy_lanes_cfg *lane_cfg)
+{
+	u32 lane_assign = 0;
+	int i;
+
+	for (i = 0; i < lane_cfg->num_data; i++)
+		lane_assign |= lane_cfg->data[i].pos << (i * 4);
+
+	return lane_assign;
+}
+
+/*
+ * csid_link_setup - Setup CSID connections
+ * @entity: Pointer to media entity structure
+ * @local: Pointer to local pad
+ * @remote: Pointer to remote pad
+ * @flags: Link flags
+ *
+ * Return 0 on success
+ */
+static int csid_link_setup(struct media_entity *entity,
+			   const struct media_pad *local,
+			   const struct media_pad *remote, u32 flags)
+{
+	if (flags & MEDIA_LNK_FL_ENABLED)
+		if (media_entity_remote_pad((struct media_pad *)local))
+			return -EBUSY;
+
+	if ((local->flags & MEDIA_PAD_FL_SINK) &&
+	    (flags & MEDIA_LNK_FL_ENABLED)) {
+		struct v4l2_subdev *sd;
+		struct csid_device *csid;
+		struct csiphy_device *csiphy;
+		struct csiphy_lanes_cfg *lane_cfg;
+		struct v4l2_subdev_format format;
+
+		sd = container_of(entity, struct v4l2_subdev, entity);
+		csid = v4l2_get_subdevdata(sd);
+
+		/* If test generator is enabled */
+		/* do not allow a link from CSIPHY to CSID */
+		if (csid->testgen_mode->cur.val != 0)
+			return -EBUSY;
+
+		sd = container_of(remote->entity, struct v4l2_subdev, entity);
+		csiphy = v4l2_get_subdevdata(sd);
+
+		/* If a sensor is not linked to CSIPHY */
+		/* do no allow a link from CSIPHY to CSID */
+		if (!csiphy->cfg.csi2)
+			return -EPERM;
+
+		csid->phy.csiphy_id = csiphy->id;
+
+		lane_cfg = &csiphy->cfg.csi2->lane_cfg;
+		csid->phy.lane_cnt = lane_cfg->num_data;
+		csid->phy.lane_assign = csid_get_lane_assign(lane_cfg);
+
+		/* Reset format on source pad to sink pad format */
+		memset(&format, 0, sizeof(format));
+		format.pad = MSM_CSID_PAD_SRC;
+		format.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+		csid_set_format(&csid->subdev, NULL, &format);
+	}
+
+	return 0;
+}
+
+static const struct v4l2_subdev_core_ops csid_core_ops = {
+	.s_power = csid_set_power,
+};
+
+static const struct v4l2_subdev_video_ops csid_video_ops = {
+	.s_stream = csid_set_stream,
+};
+
+static const struct v4l2_subdev_pad_ops csid_pad_ops = {
+	.enum_mbus_code = csid_enum_mbus_code,
+	.enum_frame_size = csid_enum_frame_size,
+	.get_fmt = csid_get_format,
+	.set_fmt = csid_set_format,
+};
+
+static const struct v4l2_subdev_ops csid_v4l2_ops = {
+	.core = &csid_core_ops,
+	.video = &csid_video_ops,
+	.pad = &csid_pad_ops,
+};
+
+static const struct v4l2_subdev_internal_ops csid_v4l2_internal_ops = {
+	.open = csid_init_formats,
+};
+
+static const struct media_entity_operations csid_media_ops = {
+	.link_setup = csid_link_setup,
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+/*
+ * msm_csid_register_entity - Register subdev node for CSID module
+ * @csid: CSID device
+ * @v4l2_dev: V4L2 device
+ *
+ * Return 0 on success or a negative error code otherwise
+ */
+int msm_csid_register_entity(struct csid_device *csid,
+			     struct v4l2_device *v4l2_dev)
+{
+	struct v4l2_subdev *sd = &csid->subdev;
+	struct media_pad *pads = csid->pads;
+	struct device *dev = to_device_index(csid, csid->id);
+	int ret;
+
+	v4l2_subdev_init(sd, &csid_v4l2_ops);
+	sd->internal_ops = &csid_v4l2_internal_ops;
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	snprintf(sd->name, ARRAY_SIZE(sd->name), "%s%d",
+		 MSM_CSID_NAME, csid->id);
+	v4l2_set_subdevdata(sd, csid);
+
+	ret = v4l2_ctrl_handler_init(&csid->ctrls, 1);
+	if (ret < 0) {
+		dev_err(dev, "Failed to init ctrl handler\n");
+		return ret;
+	}
+
+	csid->testgen_mode = v4l2_ctrl_new_std_menu_items(&csid->ctrls,
+				&csid_ctrl_ops, V4L2_CID_TEST_PATTERN,
+				ARRAY_SIZE(csid_test_pattern_menu) - 1, 0, 0,
+				csid_test_pattern_menu);
+
+	if (csid->ctrls.error) {
+		dev_err(dev, "Failed to init ctrl: %d\n", csid->ctrls.error);
+		ret = csid->ctrls.error;
+		goto free_ctrl;
+	}
+
+	csid->subdev.ctrl_handler = &csid->ctrls;
+
+	ret = csid_init_formats(sd, NULL);
+	if (ret < 0) {
+		dev_err(dev, "Failed to init format\n");
+		goto free_ctrl;
+	}
+
+	pads[MSM_CSID_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
+	pads[MSM_CSID_PAD_SRC].flags = MEDIA_PAD_FL_SOURCE;
+
+	sd->entity.ops = &csid_media_ops;
+	ret = media_entity_pads_init(&sd->entity, MSM_CSID_PADS_NUM, pads);
+	if (ret < 0) {
+		dev_err(dev, "Failed to init media entity\n");
+		goto free_ctrl;
+	}
+
+	ret = v4l2_device_register_subdev(v4l2_dev, sd);
+	if (ret < 0) {
+		dev_err(dev, "Failed to register subdev\n");
+		goto media_cleanup;
+	}
+
+	return 0;
+
+media_cleanup:
+	media_entity_cleanup(&sd->entity);
+free_ctrl:
+	v4l2_ctrl_handler_free(&csid->ctrls);
+
+	return ret;
+}
+
+/*
+ * msm_csid_unregister_entity - Unregister CSID module subdev node
+ * @csid: CSID device
+ */
+void msm_csid_unregister_entity(struct csid_device *csid)
+{
+	v4l2_device_unregister_subdev(&csid->subdev);
+	v4l2_ctrl_handler_free(&csid->ctrls);
+}
diff --git a/drivers/media/platform/qcom/camss-8x16/csid.h b/drivers/media/platform/qcom/camss-8x16/csid.h
new file mode 100644
index 0000000..3875d5d
--- /dev/null
+++ b/drivers/media/platform/qcom/camss-8x16/csid.h
@@ -0,0 +1,82 @@
+/*
+ * csid.h
+ *
+ * Qualcomm MSM Camera Subsystem - CSID Module
+ *
+ * Copyright (c) 2011-2014, The Linux Foundation. All rights reserved.
+ * Copyright (C) 2015-2016 Linaro Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only 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.
+ */
+#ifndef QC_MSM_CAMSS_CSID_H
+#define QC_MSM_CAMSS_CSID_H
+
+#include <linux/clk.h>
+#include <media/media-entity.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-mediabus.h>
+#include <media/v4l2-subdev.h>
+
+#define MSM_CSID_PAD_SINK 0
+#define MSM_CSID_PAD_SRC 1
+#define MSM_CSID_PADS_NUM 2
+
+enum csid_payload_mode {
+	CSID_PAYLOAD_MODE_INCREMENTING = 0,
+	CSID_PAYLOAD_MODE_ALTERNATING_55_AA = 1,
+	CSID_PAYLOAD_MODE_ALL_ZEROES = 2,
+	CSID_PAYLOAD_MODE_ALL_ONES = 3,
+	CSID_PAYLOAD_MODE_RANDOM = 4,
+	CSID_PAYLOAD_MODE_USER_SPECIFIED = 5,
+};
+
+struct csid_testgen_config {
+	u8 enabled;
+	enum csid_payload_mode payload_mode;
+};
+
+struct csid_phy_config {
+	u8 csiphy_id;
+	u8 lane_cnt;
+	u32 lane_assign;
+};
+
+struct csid_device {
+	u8 id;
+	struct v4l2_subdev subdev;
+	struct media_pad pads[MSM_CSID_PADS_NUM];
+	void __iomem *base;
+	u32 irq;
+	char irq_name[30];
+	struct clk **clock;
+	int nclocks;
+	struct regulator *vdda;
+	struct completion reset_complete;
+	struct csid_testgen_config testgen;
+	struct csid_phy_config phy;
+	struct v4l2_mbus_framefmt fmt[MSM_CSID_PADS_NUM];
+	struct v4l2_ctrl_handler ctrls;
+	struct v4l2_ctrl *testgen_mode;
+};
+
+struct resources;
+
+int msm_csid_subdev_init(struct csid_device *csid,
+			 struct resources *res, u8 id);
+
+int msm_csid_register_entity(struct csid_device *csid,
+			     struct v4l2_device *v4l2_dev);
+
+void msm_csid_unregister_entity(struct csid_device *csid);
+
+void msm_csid_get_csid_id(struct media_entity *entity, u8 *id);
+
+#endif /* QC_MSM_CAMSS_CSID_H */
-- 
1.9.1

[PATCH 06/10] media: camss: Add ISPIF files

[Todor Tomov]

These files control the ISPIF module which handles the routing of the data
streams from the CSIDs to the inputs of the VFE.

Signed-off-by: Todor Tomov <todor.tomov@linaro.org>
---
 drivers/media/platform/qcom/camss-8x16/ispif.c | 1105 ++++++++++++++++++++++++
 drivers/media/platform/qcom/camss-8x16/ispif.h |   85 ++
 2 files changed, 1190 insertions(+)
 create mode 100644 drivers/media/platform/qcom/camss-8x16/ispif.c
 create mode 100644 drivers/media/platform/qcom/camss-8x16/ispif.h

diff --git a/drivers/media/platform/qcom/camss-8x16/ispif.c b/drivers/media/platform/qcom/camss-8x16/ispif.c
new file mode 100644
index 0000000..7d56b7d
--- /dev/null
+++ b/drivers/media/platform/qcom/camss-8x16/ispif.c
@@ -0,0 +1,1105 @@
+/*
+ * ispif.c
+ *
+ * Qualcomm MSM Camera Subsystem - ISPIF Module
+ *
+ * Copyright (c) 2013-2015, The Linux Foundation. All rights reserved.
+ * Copyright (C) 2015-2016 Linaro Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only 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.
+ */
+#include <linux/clk.h>
+#include <linux/completion.h>
+#include <linux/interrupt.h>
+#include <linux/iopoll.h>
+#include <linux/kernel.h>
+#include <linux/mutex.h>
+#include <linux/platform_device.h>
+#include <media/media-entity.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-subdev.h>
+
+#include "ispif.h"
+#include "camss.h"
+
+#define MSM_ISPIF_NAME "msm_ispif"
+
+#define ispif_line_array(ptr_line)	\
+	((const struct ispif_line (*)[]) &(ptr_line[-(ptr_line->id)]))
+
+#define to_ispif(ptr_line)	\
+	container_of(ispif_line_array(ptr_line), struct ispif_device, ptr_line)
+
+#define ISPIF_RST_CMD_0			0x008
+#define ISPIF_IRQ_GLOBAL_CLEAR_CMD	0x01c
+#define ISPIF_VFE_m_CTRL_0(m)		(0x200 + 0x200 * (m))
+#define ISPIF_VFE_m_CTRL_0_PIX0_LINE_BUF_EN	(1 << 6)
+#define ISPIF_VFE_m_IRQ_MASK_0(m)	(0x208 + 0x200 * (m))
+#define ISPIF_VFE_m_IRQ_MASK_0_PIX0_ENABLE	0x00001249
+#define ISPIF_VFE_m_IRQ_MASK_0_PIX0_MASK	0x00001fff
+#define ISPIF_VFE_m_IRQ_MASK_0_RDI0_ENABLE	0x02492000
+#define ISPIF_VFE_m_IRQ_MASK_0_RDI0_MASK	0x03ffe000
+#define ISPIF_VFE_m_IRQ_MASK_1(m)	(0x20c + 0x200 * (m))
+#define ISPIF_VFE_m_IRQ_MASK_1_PIX1_ENABLE	0x00001249
+#define ISPIF_VFE_m_IRQ_MASK_1_PIX1_MASK	0x00001fff
+#define ISPIF_VFE_m_IRQ_MASK_1_RDI1_ENABLE	0x02492000
+#define ISPIF_VFE_m_IRQ_MASK_1_RDI1_MASK	0x03ffe000
+#define ISPIF_VFE_m_IRQ_MASK_2(m)	(0x210 + 0x200 * (m))
+#define ISPIF_VFE_m_IRQ_MASK_2_RDI2_ENABLE	0x00001249
+#define ISPIF_VFE_m_IRQ_MASK_2_RDI2_MASK	0x00001fff
+#define ISPIF_VFE_m_IRQ_STATUS_0(m)	(0x21c + 0x200 * (m))
+#define ISPIF_VFE_m_IRQ_STATUS_1(m)	(0x220 + 0x200 * (m))
+#define ISPIF_VFE_m_IRQ_STATUS_2(m)	(0x224 + 0x200 * (m))
+#define ISPIF_VFE_m_IRQ_CLEAR_0(m)	(0x230 + 0x200 * (m))
+#define ISPIF_VFE_m_IRQ_CLEAR_1(m)	(0x234 + 0x200 * (m))
+#define ISPIF_VFE_m_IRQ_CLEAR_2(m)	(0x238 + 0x200 * (m))
+#define ISPIF_VFE_m_INTF_INPUT_SEL(m)	(0x244 + 0x200 * (m))
+#define ISPIF_VFE_m_INTF_CMD_0(m)	(0x248 + 0x200 * (m))
+#define ISPIF_VFE_m_INTF_CMD_1(m)	(0x24c + 0x200 * (m))
+#define ISPIF_VFE_m_PIX_INTF_n_CID_MASK(m, n)	\
+					(0x254 + 0x200 * (m) + 0x4 * (n))
+#define ISPIF_VFE_m_RDI_INTF_n_CID_MASK(m, n)	\
+					(0x264 + 0x200 * (m) + 0x4 * (n))
+#define ISPIF_VFE_m_PIX_INTF_n_STATUS(m, n)	\
+					(0x2c0 + 0x200 * (m) + 0x4 * (n))
+#define ISPIF_VFE_m_RDI_INTF_n_STATUS(m, n)	\
+					(0x2d0 + 0x200 * (m) + 0x4 * (n))
+
+#define CSI_PIX_CLK_MUX_SEL		0x000
+#define CSI_RDI_CLK_MUX_SEL		0x008
+
+#define ISPIF_TIMEOUT_SLEEP_US		1000
+#define ISPIF_TIMEOUT_ALL_US		1000000
+#define ISPIF_RESET_TIMEOUT_MS		500
+
+enum ispif_intf_cmd {
+	CMD_DISABLE_FRAME_BOUNDARY = 0x0,
+	CMD_ENABLE_FRAME_BOUNDARY = 0x1,
+	CMD_DISABLE_IMMEDIATELY = 0x2,
+	CMD_ALL_DISABLE_IMMEDIATELY = 0xaaaaaaaa,
+	CMD_ALL_NO_CHANGE = 0xffffffff,
+};
+
+static const u32 ispif_formats[] = {
+	MEDIA_BUS_FMT_UYVY8_2X8,
+	MEDIA_BUS_FMT_VYUY8_2X8,
+	MEDIA_BUS_FMT_YUYV8_2X8,
+	MEDIA_BUS_FMT_YVYU8_2X8,
+	MEDIA_BUS_FMT_SBGGR8_1X8,
+	MEDIA_BUS_FMT_SGBRG8_1X8,
+	MEDIA_BUS_FMT_SGRBG8_1X8,
+	MEDIA_BUS_FMT_SRGGB8_1X8,
+	MEDIA_BUS_FMT_SBGGR10_1X10,
+	MEDIA_BUS_FMT_SGBRG10_1X10,
+	MEDIA_BUS_FMT_SGRBG10_1X10,
+	MEDIA_BUS_FMT_SRGGB10_1X10,
+	MEDIA_BUS_FMT_SBGGR12_1X12,
+	MEDIA_BUS_FMT_SGBRG12_1X12,
+	MEDIA_BUS_FMT_SGRBG12_1X12,
+	MEDIA_BUS_FMT_SRGGB12_1X12,
+};
+
+/*
+ * ispif_isr - ISPIF module interrupt handler
+ * @irq: Interrupt line
+ * @dev: ISPIF device
+ *
+ * Return IRQ_HANDLED on success
+ */
+static irqreturn_t ispif_isr(int irq, void *dev)
+{
+	struct ispif_device *ispif = dev;
+	u32 value0, value1, value2;
+
+	value0 = readl_relaxed(ispif->base + ISPIF_VFE_m_IRQ_STATUS_0(0));
+	value1 = readl_relaxed(ispif->base + ISPIF_VFE_m_IRQ_STATUS_1(0));
+	value2 = readl_relaxed(ispif->base + ISPIF_VFE_m_IRQ_STATUS_2(0));
+
+	writel_relaxed(value0, ispif->base + ISPIF_VFE_m_IRQ_CLEAR_0(0));
+	writel_relaxed(value1, ispif->base + ISPIF_VFE_m_IRQ_CLEAR_1(0));
+	writel_relaxed(value2, ispif->base + ISPIF_VFE_m_IRQ_CLEAR_2(0));
+
+	writel(0x1, ispif->base + ISPIF_IRQ_GLOBAL_CLEAR_CMD);
+
+	if ((value0 >> 27) & 0x1)
+		complete(&ispif->reset_complete);
+
+	return IRQ_HANDLED;
+}
+
+/*
+ * ispif_reset - Trigger reset on ISPIF module and wait to complete
+ * @ispif: ISPIF device
+ *
+ * Return 0 on success or a negative error code otherwise
+ */
+static int ispif_reset(struct ispif_device *ispif)
+{
+	unsigned long time;
+	int ret;
+
+	ret = camss_enable_clocks(ispif->nclocks_for_reset,
+				  ispif->clock_for_reset,
+				  to_device(ispif));
+	if (ret < 0)
+		return ret;
+
+	reinit_completion(&ispif->reset_complete);
+
+	writel_relaxed(0x000f1fff, ispif->base + ISPIF_RST_CMD_0);
+
+	time = wait_for_completion_timeout(&ispif->reset_complete,
+		msecs_to_jiffies(ISPIF_RESET_TIMEOUT_MS));
+	if (!time) {
+		dev_err(to_device(ispif), "ISPIF reset timeout\n");
+		return -EIO;
+	}
+
+	camss_disable_clocks(ispif->nclocks_for_reset, ispif->clock_for_reset);
+
+	return 0;
+}
+
+/*
+ * ispif_set_power - Power on/off ISPIF module
+ * @sd: ISPIF V4L2 subdevice
+ * @on: Requested power state
+ *
+ * Return 0 on success or a negative error code otherwise
+ */
+static int ispif_set_power(struct v4l2_subdev *sd, int on)
+{
+	struct ispif_line *line = v4l2_get_subdevdata(sd);
+	struct ispif_device *ispif = to_ispif(line);
+	struct device *dev = to_device(ispif);
+	int ret = 0;
+
+	mutex_lock(&ispif->power_lock);
+
+	if (on) {
+		if (ispif->power_count) {
+			/* Power is already on */
+			ispif->power_count++;
+			goto exit;
+		}
+
+		ret = camss_enable_clocks(ispif->nclocks, ispif->clock, dev);
+		if (ret < 0)
+			goto exit;
+
+		ret = ispif_reset(ispif);
+		if (ret < 0) {
+			camss_disable_clocks(ispif->nclocks, ispif->clock);
+			goto exit;
+		}
+
+		ispif->intf_cmd[line->vfe_id].cmd_0 = CMD_ALL_NO_CHANGE;
+		ispif->intf_cmd[line->vfe_id].cmd_1 = CMD_ALL_NO_CHANGE;
+
+		ispif->power_count++;
+	} else {
+		if (ispif->power_count == 0) {
+			dev_err(dev, "ispif power off on power_count == 0\n");
+			goto exit;
+		} else if (ispif->power_count == 1) {
+			camss_disable_clocks(ispif->nclocks, ispif->clock);
+		}
+
+		ispif->power_count--;
+	}
+
+exit:
+	mutex_unlock(&ispif->power_lock);
+
+	return ret;
+}
+
+/*
+ * ispif_select_clk_mux - Select clock for PIX/RDI interface
+ * @ispif: ISPIF device
+ * @intf: VFE interface
+ * @csid: CSID HW module id
+ * @vfe: VFE HW module id
+ * @enable: enable or disable the selected clock
+ */
+static void ispif_select_clk_mux(struct ispif_device *ispif,
+				 enum ispif_intf intf, u8 csid,
+				 u8 vfe, u8 enable)
+{
+	u32 val;
+
+	switch (intf) {
+	case PIX0:
+		val = readl_relaxed(ispif->base_clk_mux + CSI_PIX_CLK_MUX_SEL);
+		val &= ~(0xf << (vfe * 8));
+		if (enable)
+			val |= (csid << (vfe * 8));
+		writel_relaxed(val, ispif->base_clk_mux + CSI_PIX_CLK_MUX_SEL);
+		break;
+
+	case RDI0:
+		val = readl_relaxed(ispif->base_clk_mux + CSI_RDI_CLK_MUX_SEL);
+		val &= ~(0xf << (vfe * 12));
+		if (enable)
+			val |= (csid << (vfe * 12));
+		writel_relaxed(val, ispif->base_clk_mux + CSI_RDI_CLK_MUX_SEL);
+		break;
+
+	case PIX1:
+		val = readl_relaxed(ispif->base_clk_mux + CSI_PIX_CLK_MUX_SEL);
+		val &= ~(0xf << (4 + (vfe * 8)));
+		if (enable)
+			val |= (csid << (4 + (vfe * 8)));
+		writel_relaxed(val, ispif->base_clk_mux + CSI_PIX_CLK_MUX_SEL);
+		break;
+
+	case RDI1:
+		val = readl_relaxed(ispif->base_clk_mux + CSI_RDI_CLK_MUX_SEL);
+		val &= ~(0xf << (4 + (vfe * 12)));
+		if (enable)
+			val |= (csid << (4 + (vfe * 12)));
+		writel_relaxed(val, ispif->base_clk_mux + CSI_RDI_CLK_MUX_SEL);
+		break;
+
+	case RDI2:
+		val = readl_relaxed(ispif->base_clk_mux + CSI_RDI_CLK_MUX_SEL);
+		val &= ~(0xf << (8 + (vfe * 12)));
+		if (enable)
+			val |= (csid << (8 + (vfe * 12)));
+		writel_relaxed(val, ispif->base_clk_mux + CSI_RDI_CLK_MUX_SEL);
+		break;
+	}
+
+	mb();
+}
+
+/*
+ * ispif_validate_intf_status - Validate current status of PIX/RDI interface
+ * @ispif: ISPIF device
+ * @intf: VFE interface
+ * @vfe: VFE HW module id
+ *
+ * Return 0 when interface is idle or -EBUSY otherwise
+ */
+static int ispif_validate_intf_status(struct ispif_device *ispif,
+				      enum ispif_intf intf, u8 vfe)
+{
+	int ret = 0;
+	u32 val = 0;
+
+	switch (intf) {
+	case PIX0:
+		val = readl_relaxed(ispif->base +
+			ISPIF_VFE_m_PIX_INTF_n_STATUS(vfe, 0));
+		break;
+	case RDI0:
+		val = readl_relaxed(ispif->base +
+			ISPIF_VFE_m_RDI_INTF_n_STATUS(vfe, 0));
+		break;
+	case PIX1:
+		val = readl_relaxed(ispif->base +
+			ISPIF_VFE_m_PIX_INTF_n_STATUS(vfe, 1));
+		break;
+	case RDI1:
+		val = readl_relaxed(ispif->base +
+			ISPIF_VFE_m_RDI_INTF_n_STATUS(vfe, 1));
+		break;
+	case RDI2:
+		val = readl_relaxed(ispif->base +
+			ISPIF_VFE_m_RDI_INTF_n_STATUS(vfe, 2));
+		break;
+	}
+
+	if ((val & 0xf) != 0xf) {
+		dev_err(to_device(ispif), "%s: ispif is busy: 0x%x\n",
+			__func__, val);
+		ret = -EBUSY;
+	}
+
+	return ret;
+}
+
+/*
+ * ispif_wait_for_stop - Wait for PIX/RDI interface to stop
+ * @ispif: ISPIF device
+ * @intf: VFE interface
+ * @vfe: VFE HW module id
+ *
+ * Return 0 on success or a negative error code otherwise
+ */
+static int ispif_wait_for_stop(struct ispif_device *ispif,
+			       enum ispif_intf intf, u8 vfe)
+{
+	u32 addr = 0;
+	u32 stop_flag = 0;
+	int ret;
+
+	switch (intf) {
+	case PIX0:
+		addr = ISPIF_VFE_m_PIX_INTF_n_STATUS(vfe, 0);
+		break;
+	case RDI0:
+		addr = ISPIF_VFE_m_RDI_INTF_n_STATUS(vfe, 0);
+		break;
+	case PIX1:
+		addr = ISPIF_VFE_m_PIX_INTF_n_STATUS(vfe, 1);
+		break;
+	case RDI1:
+		addr = ISPIF_VFE_m_RDI_INTF_n_STATUS(vfe, 1);
+		break;
+	case RDI2:
+		addr = ISPIF_VFE_m_RDI_INTF_n_STATUS(vfe, 2);
+		break;
+	}
+
+	ret = readl_poll_timeout(ispif->base + addr,
+				 stop_flag,
+				 (stop_flag & 0xf) == 0xf,
+				 ISPIF_TIMEOUT_SLEEP_US,
+				 ISPIF_TIMEOUT_ALL_US);
+	if (ret < 0)
+		dev_err(to_device(ispif), "%s: ispif stop timeout\n",
+			__func__);
+
+	return ret;
+}
+
+/*
+ * ispif_select_csid - Select CSID HW module for input from
+ * @ispif: ISPIF device
+ * @intf: VFE interface
+ * @csid: CSID HW module id
+ * @vfe: VFE HW module id
+ * @enable: enable or disable the selected input
+ */
+static void ispif_select_csid(struct ispif_device *ispif, enum ispif_intf intf,
+			      u8 csid, u8 vfe, u8 enable)
+{
+	u32 val;
+
+	val = readl_relaxed(ispif->base + ISPIF_VFE_m_INTF_INPUT_SEL(vfe));
+	switch (intf) {
+	case PIX0:
+		val &= ~(BIT(1) | BIT(0));
+		if (enable)
+			val |= csid;
+		break;
+	case RDI0:
+		val &= ~(BIT(5) | BIT(4));
+		if (enable)
+			val |= (csid << 4);
+		break;
+	case PIX1:
+		val &= ~(BIT(9) | BIT(8));
+		if (enable)
+			val |= (csid << 8);
+		break;
+	case RDI1:
+		val &= ~(BIT(13) | BIT(12));
+		if (enable)
+			val |= (csid << 12);
+		break;
+	case RDI2:
+		val &= ~(BIT(21) | BIT(20));
+		if (enable)
+			val |= (csid << 20);
+		break;
+	}
+
+	writel(val, ispif->base + ISPIF_VFE_m_INTF_INPUT_SEL(vfe));
+}
+
+/*
+ * ispif_select_cid - Enable/disable desired CID
+ * @ispif: ISPIF device
+ * @intf: VFE interface
+ * @cid: desired CID to enable/disable
+ * @vfe: VFE HW module id
+ * @enable: enable or disable the desired CID
+ */
+static void ispif_select_cid(struct ispif_device *ispif, enum ispif_intf intf,
+			     u8 cid, u8 vfe, u8 enable)
+{
+	u32 cid_mask = 1 << cid;
+	u32 addr = 0;
+	u32 val;
+
+	switch (intf) {
+	case PIX0:
+		addr = ISPIF_VFE_m_PIX_INTF_n_CID_MASK(vfe, 0);
+		break;
+	case RDI0:
+		addr = ISPIF_VFE_m_RDI_INTF_n_CID_MASK(vfe, 0);
+		break;
+	case PIX1:
+		addr = ISPIF_VFE_m_PIX_INTF_n_CID_MASK(vfe, 1);
+		break;
+	case RDI1:
+		addr = ISPIF_VFE_m_RDI_INTF_n_CID_MASK(vfe, 1);
+		break;
+	case RDI2:
+		addr = ISPIF_VFE_m_RDI_INTF_n_CID_MASK(vfe, 2);
+		break;
+	}
+
+	val = readl_relaxed(ispif->base + addr);
+	if (enable)
+		val |= cid_mask;
+	else
+		val &= ~cid_mask;
+
+	writel(val, ispif->base + addr);
+}
+
+/*
+ * ispif_config_irq - Enable/disable interrupts for PIX/RDI interface
+ * @ispif: ISPIF device
+ * @intf: VFE interface
+ * @vfe: VFE HW module id
+ * @enable: enable or disable
+ */
+static void ispif_config_irq(struct ispif_device *ispif, enum ispif_intf intf,
+			     u8 vfe, u8 enable)
+{
+	u32 val;
+
+	switch (intf) {
+	case PIX0:
+		val = readl_relaxed(ispif->base + ISPIF_VFE_m_IRQ_MASK_0(vfe));
+		val &= ~ISPIF_VFE_m_IRQ_MASK_0_PIX0_MASK;
+		if (enable)
+			val |= ISPIF_VFE_m_IRQ_MASK_0_PIX0_ENABLE;
+		writel_relaxed(val, ispif->base + ISPIF_VFE_m_IRQ_MASK_0(vfe));
+		writel_relaxed(ISPIF_VFE_m_IRQ_MASK_0_PIX0_ENABLE,
+			       ispif->base + ISPIF_VFE_m_IRQ_CLEAR_0(vfe));
+		break;
+	case RDI0:
+		val = readl_relaxed(ispif->base + ISPIF_VFE_m_IRQ_MASK_0(vfe));
+		val &= ~ISPIF_VFE_m_IRQ_MASK_0_RDI0_MASK;
+		if (enable)
+			val |= ISPIF_VFE_m_IRQ_MASK_0_RDI0_ENABLE;
+		writel_relaxed(val, ispif->base + ISPIF_VFE_m_IRQ_MASK_0(vfe));
+		writel_relaxed(ISPIF_VFE_m_IRQ_MASK_0_RDI0_ENABLE,
+			       ispif->base + ISPIF_VFE_m_IRQ_CLEAR_0(vfe));
+		break;
+	case PIX1:
+		val = readl_relaxed(ispif->base + ISPIF_VFE_m_IRQ_MASK_1(vfe));
+		val &= ~ISPIF_VFE_m_IRQ_MASK_1_PIX1_MASK;
+		if (enable)
+			val |= ISPIF_VFE_m_IRQ_MASK_1_PIX1_ENABLE;
+		writel_relaxed(val, ispif->base + ISPIF_VFE_m_IRQ_MASK_1(vfe));
+		writel_relaxed(ISPIF_VFE_m_IRQ_MASK_1_PIX1_ENABLE,
+			       ispif->base + ISPIF_VFE_m_IRQ_CLEAR_1(vfe));
+		break;
+	case RDI1:
+		val = readl_relaxed(ispif->base + ISPIF_VFE_m_IRQ_MASK_1(vfe));
+		val &= ~ISPIF_VFE_m_IRQ_MASK_1_RDI1_MASK;
+		if (enable)
+			val |= ISPIF_VFE_m_IRQ_MASK_1_RDI1_ENABLE;
+		writel_relaxed(val, ispif->base + ISPIF_VFE_m_IRQ_MASK_1(vfe));
+		writel_relaxed(ISPIF_VFE_m_IRQ_MASK_1_RDI1_ENABLE,
+			       ispif->base + ISPIF_VFE_m_IRQ_CLEAR_1(vfe));
+		break;
+	case RDI2:
+		val = readl_relaxed(ispif->base + ISPIF_VFE_m_IRQ_MASK_2(vfe));
+		val &= ~ISPIF_VFE_m_IRQ_MASK_2_RDI2_MASK;
+		if (enable)
+			val |= ISPIF_VFE_m_IRQ_MASK_2_RDI2_ENABLE;
+		writel_relaxed(val, ispif->base + ISPIF_VFE_m_IRQ_MASK_2(vfe));
+		writel_relaxed(ISPIF_VFE_m_IRQ_MASK_2_RDI2_ENABLE,
+			       ispif->base + ISPIF_VFE_m_IRQ_CLEAR_2(vfe));
+		break;
+	}
+
+	writel(0x1, ispif->base + ISPIF_IRQ_GLOBAL_CLEAR_CMD);
+}
+
+/*
+ * ispif_set_intf_cmd - Set command to enable/disable interface
+ * @ispif: ISPIF device
+ * @cmd: interface command
+ * @intf: VFE interface
+ * @vfe: VFE HW module id
+ * @vc: virtual channel
+ */
+static void ispif_set_intf_cmd(struct ispif_device *ispif, u8 cmd,
+			       enum ispif_intf intf, u8 vfe, u8 vc)
+{
+	u32 *val;
+
+	if (intf == RDI2) {
+		val = &ispif->intf_cmd[vfe].cmd_1;
+		*val &= ~(0x3 << (vc * 2 + 8));
+		*val |= (cmd << (vc * 2 + 8));
+		wmb();
+		writel_relaxed(*val, ispif->base + ISPIF_VFE_m_INTF_CMD_1(vfe));
+		wmb();
+	} else {
+		val = &ispif->intf_cmd[vfe].cmd_0;
+		*val &= ~(0x3 << (vc * 2 + intf * 8));
+		*val |= (cmd << (vc * 2 + intf * 8));
+		wmb();
+		writel_relaxed(*val, ispif->base + ISPIF_VFE_m_INTF_CMD_0(vfe));
+		wmb();
+	}
+}
+
+/*
+ * ispif_set_stream - Enable/disable streaming on ISPIF module
+ * @sd: ISPIF V4L2 subdevice
+ * @enable: Requested streaming state
+ *
+ * Main configuration of ISPIF module is also done here.
+ *
+ * Return 0 on success or a negative error code otherwise
+ */
+static int ispif_set_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct ispif_line *line = v4l2_get_subdevdata(sd);
+	struct ispif_device *ispif = to_ispif(line);
+	enum ispif_intf intf = line->interface;
+	u8 csid = line->csid_id;
+	u8 vfe = line->vfe_id;
+	u8 vc = 0; /* Virtual Channel 0 */
+	u8 cid = vc * 4;
+	int ret;
+
+	if (enable) {
+		if (!media_entity_remote_pad(
+					&line->pads[MSM_ISPIF_PAD_SINK])) {
+			return -ENOLINK;
+		}
+
+		/* Config */
+
+		mutex_lock(&ispif->config_lock);
+		ispif_select_clk_mux(ispif, intf, csid, vfe, 1);
+
+		ret = ispif_validate_intf_status(ispif, intf, vfe);
+		if (ret < 0) {
+			mutex_unlock(&ispif->config_lock);
+			return ret;
+		}
+
+		ispif_select_csid(ispif, intf, csid, vfe, 1);
+		ispif_select_cid(ispif, intf, cid, vfe, 1);
+		ispif_config_irq(ispif, intf, vfe, 1);
+		ispif_set_intf_cmd(ispif, CMD_ENABLE_FRAME_BOUNDARY,
+				   intf, vfe, vc);
+	} else {
+		mutex_lock(&ispif->config_lock);
+		ispif_set_intf_cmd(ispif, CMD_DISABLE_FRAME_BOUNDARY,
+				   intf, vfe, vc);
+		mutex_unlock(&ispif->config_lock);
+
+		ret = ispif_wait_for_stop(ispif, intf, vfe);
+		if (ret < 0)
+			return ret;
+
+		mutex_lock(&ispif->config_lock);
+		ispif_config_irq(ispif, intf, vfe, 0);
+		ispif_select_cid(ispif, intf, cid, vfe, 0);
+		ispif_select_csid(ispif, intf, csid, vfe, 0);
+		ispif_select_clk_mux(ispif, intf, csid, vfe, 0);
+	}
+
+	mutex_unlock(&ispif->config_lock);
+
+	return 0;
+}
+
+/*
+ * __ispif_get_format - Get pointer to format structure
+ * @ispif: ISPIF line
+ * @cfg: V4L2 subdev pad configuration
+ * @pad: pad from which format is requested
+ * @which: TRY or ACTIVE format
+ *
+ * Return pointer to TRY or ACTIVE format structure
+ */
+static struct v4l2_mbus_framefmt *
+__ispif_get_format(struct ispif_line *line,
+		   struct v4l2_subdev_pad_config *cfg,
+		   unsigned int pad,
+		   enum v4l2_subdev_format_whence which)
+{
+	if (which == V4L2_SUBDEV_FORMAT_TRY)
+		return v4l2_subdev_get_try_format(&line->subdev, cfg, pad);
+
+	return &line->fmt[pad];
+}
+
+/*
+ * ispif_try_format - Handle try format by pad subdev method
+ * @ispif: ISPIF line
+ * @cfg: V4L2 subdev pad configuration
+ * @pad: pad on which format is requested
+ * @fmt: pointer to v4l2 format structure
+ * @which: wanted subdev format
+ */
+static void ispif_try_format(struct ispif_line *line,
+			     struct v4l2_subdev_pad_config *cfg,
+			     unsigned int pad,
+			     struct v4l2_mbus_framefmt *fmt,
+			     enum v4l2_subdev_format_whence which)
+{
+	unsigned int i;
+
+	switch (pad) {
+	case MSM_ISPIF_PAD_SINK:
+		/* Set format on sink pad */
+
+		for (i = 0; i < ARRAY_SIZE(ispif_formats); i++)
+			if (fmt->code == ispif_formats[i])
+				break;
+
+		/* If not found, use UYVY as default */
+		if (i >= ARRAY_SIZE(ispif_formats))
+			fmt->code = MEDIA_BUS_FMT_UYVY8_2X8;
+
+		fmt->width = clamp_t(u32, fmt->width, 1, 8191);
+		fmt->height = clamp_t(u32, fmt->height, 1, 8191);
+
+		if (fmt->field == V4L2_FIELD_ANY)
+			fmt->field = V4L2_FIELD_NONE;
+
+		break;
+
+	case MSM_ISPIF_PAD_SRC:
+		/* Set and return a format same as sink pad */
+
+		*fmt = *__ispif_get_format(line, cfg, MSM_ISPIF_PAD_SINK,
+					   which);
+
+		break;
+	}
+
+	fmt->colorspace = V4L2_COLORSPACE_SRGB;
+}
+
+/*
+ * ispif_enum_mbus_code - Handle pixel format enumeration
+ * @sd: ISPIF V4L2 subdevice
+ * @cfg: V4L2 subdev pad configuration
+ * @code: pointer to v4l2_subdev_mbus_code_enum structure
+ * return -EINVAL or zero on success
+ */
+static int ispif_enum_mbus_code(struct v4l2_subdev *sd,
+				struct v4l2_subdev_pad_config *cfg,
+				struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct ispif_line *line = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	if (code->pad == MSM_ISPIF_PAD_SINK) {
+		if (code->index >= ARRAY_SIZE(ispif_formats))
+			return -EINVAL;
+
+		code->code = ispif_formats[code->index];
+	} else {
+		if (code->index > 0)
+			return -EINVAL;
+
+		format = __ispif_get_format(line, cfg, MSM_ISPIF_PAD_SINK,
+					    code->which);
+
+		code->code = format->code;
+	}
+
+	return 0;
+}
+
+/*
+ * ispif_enum_frame_size - Handle frame size enumeration
+ * @sd: ISPIF V4L2 subdevice
+ * @cfg: V4L2 subdev pad configuration
+ * @fse: pointer to v4l2_subdev_frame_size_enum structure
+ * return -EINVAL or zero on success
+ */
+static int ispif_enum_frame_size(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_pad_config *cfg,
+				 struct v4l2_subdev_frame_size_enum *fse)
+{
+	struct ispif_line *line = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt format;
+
+	if (fse->index != 0)
+		return -EINVAL;
+
+	format.code = fse->code;
+	format.width = 1;
+	format.height = 1;
+	ispif_try_format(line, cfg, fse->pad, &format, fse->which);
+	fse->min_width = format.width;
+	fse->min_height = format.height;
+
+	if (format.code != fse->code)
+		return -EINVAL;
+
+	format.code = fse->code;
+	format.width = -1;
+	format.height = -1;
+	ispif_try_format(line, cfg, fse->pad, &format, fse->which);
+	fse->max_width = format.width;
+	fse->max_height = format.height;
+
+	return 0;
+}
+
+/*
+ * ispif_get_format - Handle get format by pads subdev method
+ * @sd: ISPIF V4L2 subdevice
+ * @cfg: V4L2 subdev pad configuration
+ * @fmt: pointer to v4l2 subdev format structure
+ *
+ * Return -EINVAL or zero on success
+ */
+static int ispif_get_format(struct v4l2_subdev *sd,
+			    struct v4l2_subdev_pad_config *cfg,
+			    struct v4l2_subdev_format *fmt)
+{
+	struct ispif_line *line = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	format = __ispif_get_format(line, cfg, fmt->pad, fmt->which);
+	if (format == NULL)
+		return -EINVAL;
+
+	fmt->format = *format;
+
+	return 0;
+}
+
+/*
+ * ispif_set_format - Handle set format by pads subdev method
+ * @sd: ISPIF V4L2 subdevice
+ * @cfg: V4L2 subdev pad configuration
+ * @fmt: pointer to v4l2 subdev format structure
+ *
+ * Return -EINVAL or zero on success
+ */
+static int ispif_set_format(struct v4l2_subdev *sd,
+			    struct v4l2_subdev_pad_config *cfg,
+			    struct v4l2_subdev_format *fmt)
+{
+	struct ispif_line *line = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	format = __ispif_get_format(line, cfg, fmt->pad, fmt->which);
+	if (format == NULL)
+		return -EINVAL;
+
+	ispif_try_format(line, cfg, fmt->pad, &fmt->format, fmt->which);
+	*format = fmt->format;
+
+	/* Propagate the format from sink to source */
+	if (fmt->pad == MSM_ISPIF_PAD_SINK) {
+		format = __ispif_get_format(line, cfg, MSM_ISPIF_PAD_SRC,
+					    fmt->which);
+
+		*format = fmt->format;
+		ispif_try_format(line, cfg, MSM_ISPIF_PAD_SRC, format,
+				 fmt->which);
+	}
+
+	return 0;
+}
+
+/*
+ * ispif_init_formats - Initialize formats on all pads
+ * @sd: ISPIF V4L2 subdevice
+ * @fh: V4L2 subdev file handle
+ *
+ * Initialize all pad formats with default values.
+ *
+ * Return 0 on success or a negative error code otherwise
+ */
+static int ispif_init_formats(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	struct v4l2_subdev_format format;
+
+	memset(&format, 0, sizeof(format));
+	format.pad = MSM_ISPIF_PAD_SINK;
+	format.which = fh ? V4L2_SUBDEV_FORMAT_TRY : V4L2_SUBDEV_FORMAT_ACTIVE;
+	format.format.code = MEDIA_BUS_FMT_UYVY8_2X8;
+	format.format.width = 1920;
+	format.format.height = 1080;
+
+	return ispif_set_format(sd, fh ? fh->pad : NULL, &format);
+}
+
+/*
+ * msm_ispif_subdev_init - Initialize ISPIF device structure and resources
+ * @ispif: ISPIF device
+ * @res: ISPIF module resources table
+ *
+ * Return 0 on success or a negative error code otherwise
+ */
+int msm_ispif_subdev_init(struct ispif_device *ispif,
+			  struct resources_ispif *res)
+{
+	struct device *dev = to_device(ispif);
+	struct platform_device *pdev = container_of(dev, struct platform_device,
+						    dev);
+	struct resource *r;
+	int i;
+	int ret;
+
+	for (i = 0; i < ARRAY_SIZE(ispif->line); i++)
+		ispif->line[i].id = i;
+
+	mutex_init(&ispif->power_lock);
+	ispif->power_count = 0;
+
+	mutex_init(&ispif->config_lock);
+
+	/* Memory */
+
+	r = platform_get_resource_byname(pdev, IORESOURCE_MEM, res->reg[0]);
+	ispif->base = devm_ioremap_resource(dev, r);
+	if (IS_ERR(ispif->base)) {
+		dev_err(dev, "could not map memory\n");
+		return PTR_ERR(ispif->base);
+	}
+
+	r = platform_get_resource_byname(pdev, IORESOURCE_MEM, res->reg[1]);
+	ispif->base_clk_mux = devm_ioremap_resource(dev, r);
+	if (IS_ERR(ispif->base_clk_mux)) {
+		dev_err(dev, "could not map memory\n");
+		return PTR_ERR(ispif->base_clk_mux);
+	}
+
+	/* Interrupt */
+
+	r = platform_get_resource_byname(pdev, IORESOURCE_IRQ, res->interrupt);
+
+	if (!r) {
+		dev_err(dev, "missing IRQ\n");
+		return -EINVAL;
+	}
+
+	ispif->irq = r->start;
+	snprintf(ispif->irq_name, sizeof(ispif->irq_name), "%s_%s",
+		 dev_name(dev), MSM_ISPIF_NAME);
+	ret = devm_request_irq(dev, ispif->irq, ispif_isr,
+			       IRQF_TRIGGER_RISING, ispif->irq_name, ispif);
+	if (ret < 0) {
+		dev_err(dev, "request_irq failed\n");
+		return ret;
+	}
+
+	/* Clocks */
+
+	ispif->nclocks = 0;
+	while (res->clock[ispif->nclocks])
+		ispif->nclocks++;
+
+	ispif->clock = devm_kzalloc(dev, ispif->nclocks * sizeof(*ispif->clock),
+				    GFP_KERNEL);
+	if (!ispif->clock)
+		return -ENOMEM;
+
+	for (i = 0; i < ispif->nclocks; i++) {
+		ispif->clock[i] = devm_clk_get(dev, res->clock[i]);
+		if (IS_ERR(ispif->clock[i]))
+			return PTR_ERR(ispif->clock[i]);
+	}
+
+	ispif->nclocks_for_reset = 0;
+	while (res->clock_for_reset[ispif->nclocks_for_reset])
+		ispif->nclocks_for_reset++;
+
+	ispif->clock_for_reset = devm_kzalloc(dev, ispif->nclocks_for_reset *
+			sizeof(*ispif->clock_for_reset), GFP_KERNEL);
+	if (!ispif->clock_for_reset)
+		return -ENOMEM;
+
+	for (i = 0; i < ispif->nclocks_for_reset; i++) {
+		ispif->clock_for_reset[i] = devm_clk_get(dev,
+						res->clock_for_reset[i]);
+		if (IS_ERR(ispif->clock_for_reset[i]))
+			return PTR_ERR(ispif->clock_for_reset[i]);
+	}
+
+	init_completion(&ispif->reset_complete);
+
+	return 0;
+}
+
+/*
+ * ispif_get_intf - Get ISPIF interface to use by VFE line id
+ * @line_id: VFE line id that the ISPIF line is connected to
+ *
+ * Return ISPIF interface to use
+ */
+static enum ispif_intf ispif_get_intf(enum vfe_line_id line_id)
+{
+	switch (line_id) {
+	case (VFE_LINE_RDI0):
+		return RDI0;
+	case (VFE_LINE_RDI1):
+		return RDI1;
+	case (VFE_LINE_RDI2):
+		return RDI2;
+	default:
+		return RDI0;
+	}
+}
+
+/*
+ * ispif_link_setup - Setup ISPIF connections
+ * @entity: Pointer to media entity structure
+ * @local: Pointer to local pad
+ * @remote: Pointer to remote pad
+ * @flags: Link flags
+ *
+ * Return 0 on success
+ */
+static int ispif_link_setup(struct media_entity *entity,
+			    const struct media_pad *local,
+			    const struct media_pad *remote, u32 flags)
+{
+	if (flags & MEDIA_LNK_FL_ENABLED) {
+		if (media_entity_remote_pad((struct media_pad *)local))
+			return -EBUSY;
+
+		if (local->flags & MEDIA_PAD_FL_SINK) {
+			struct v4l2_subdev *sd;
+			struct ispif_line *line;
+
+			sd = container_of(entity, struct v4l2_subdev, entity);
+			line = v4l2_get_subdevdata(sd);
+
+			msm_csid_get_csid_id(remote->entity, &line->csid_id);
+		} else { /* MEDIA_PAD_FL_SOURCE */
+			struct v4l2_subdev *sd;
+			struct ispif_line *line;
+			enum vfe_line_id id;
+
+			sd = container_of(entity, struct v4l2_subdev, entity);
+			line = v4l2_get_subdevdata(sd);
+
+			msm_vfe_get_vfe_id(remote->entity, &line->vfe_id);
+			msm_vfe_get_vfe_line_id(remote->entity, &id);
+			line->interface = ispif_get_intf(id);
+		}
+	}
+
+	return 0;
+}
+
+static const struct v4l2_subdev_core_ops ispif_core_ops = {
+	.s_power = ispif_set_power,
+};
+
+static const struct v4l2_subdev_video_ops ispif_video_ops = {
+	.s_stream = ispif_set_stream,
+};
+
+static const struct v4l2_subdev_pad_ops ispif_pad_ops = {
+	.enum_mbus_code = ispif_enum_mbus_code,
+	.enum_frame_size = ispif_enum_frame_size,
+	.get_fmt = ispif_get_format,
+	.set_fmt = ispif_set_format,
+};
+
+static const struct v4l2_subdev_ops ispif_v4l2_ops = {
+	.core = &ispif_core_ops,
+	.video = &ispif_video_ops,
+	.pad = &ispif_pad_ops,
+};
+
+static const struct v4l2_subdev_internal_ops ispif_v4l2_internal_ops = {
+	.open = ispif_init_formats,
+};
+
+static const struct media_entity_operations ispif_media_ops = {
+	.link_setup = ispif_link_setup,
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+/*
+ * msm_ispif_register_entities - Register subdev node for ISPIF module
+ * @ispif: ISPIF device
+ * @v4l2_dev: V4L2 device
+ *
+ * Return 0 on success or a negative error code otherwise
+ */
+int msm_ispif_register_entities(struct ispif_device *ispif,
+				struct v4l2_device *v4l2_dev)
+{
+	struct device *dev = to_device(ispif);
+	int ret;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(ispif->line); i++) {
+		struct v4l2_subdev *sd = &ispif->line[i].subdev;
+		struct media_pad *pads = ispif->line[i].pads;
+
+		v4l2_subdev_init(sd, &ispif_v4l2_ops);
+		sd->internal_ops = &ispif_v4l2_internal_ops;
+		sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+		snprintf(sd->name, ARRAY_SIZE(sd->name), "%s%d",
+			 MSM_ISPIF_NAME, i);
+		v4l2_set_subdevdata(sd, &ispif->line[i]);
+
+		ret = ispif_init_formats(sd, NULL);
+		if (ret < 0) {
+			dev_err(dev, "Failed to init format\n");
+			goto error;
+		}
+
+		pads[MSM_ISPIF_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
+		pads[MSM_ISPIF_PAD_SRC].flags = MEDIA_PAD_FL_SOURCE;
+
+		sd->entity.ops = &ispif_media_ops;
+		ret = media_entity_pads_init(&sd->entity, MSM_ISPIF_PADS_NUM,
+					     pads);
+		if (ret < 0) {
+			dev_err(dev, "Failed to init media entity\n");
+			goto error;
+		}
+
+		ret = v4l2_device_register_subdev(v4l2_dev, sd);
+		if (ret < 0) {
+			dev_err(dev, "Failed to register subdev\n");
+			media_entity_cleanup(&sd->entity);
+			goto error;
+		}
+	}
+
+	return 0;
+
+error:
+	for (i--; i >= 0; i--) {
+		struct v4l2_subdev *sd = &ispif->line[i].subdev;
+
+		v4l2_device_unregister_subdev(sd);
+		media_entity_cleanup(&sd->entity);
+	}
+
+	return ret;
+}
+
+/*
+ * msm_ispif_unregister_entities - Unregister ISPIF module subdev node
+ * @ispif: ISPIF device
+ */
+void msm_ispif_unregister_entities(struct ispif_device *ispif)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(ispif->line); i++) {
+		struct v4l2_subdev *sd = &ispif->line[i].subdev;
+
+		v4l2_device_unregister_subdev(sd);
+		media_entity_cleanup(&sd->entity);
+	}
+}
diff --git a/drivers/media/platform/qcom/camss-8x16/ispif.h b/drivers/media/platform/qcom/camss-8x16/ispif.h
new file mode 100644
index 0000000..935987f
--- /dev/null
+++ b/drivers/media/platform/qcom/camss-8x16/ispif.h
@@ -0,0 +1,85 @@
+/*
+ * ispif.h
+ *
+ * Qualcomm MSM Camera Subsystem - ISPIF Module
+ *
+ * Copyright (c) 2013-2014, The Linux Foundation. All rights reserved.
+ * Copyright (C) 2015-2016 Linaro Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only 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.
+ */
+#ifndef QC_MSM_CAMSS_ISPIF_H
+#define QC_MSM_CAMSS_ISPIF_H
+
+#include <linux/clk.h>
+#include <media/media-entity.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-subdev.h>
+
+/* Number of ISPIF lines - same as number of CSID hardware modules */
+#define MSM_ISPIF_LINE_NUM 2
+
+#define MSM_ISPIF_PAD_SINK 0
+#define MSM_ISPIF_PAD_SRC 1
+#define MSM_ISPIF_PADS_NUM 2
+
+#define MSM_ISPIF_VFE_NUM 1
+
+enum ispif_intf {
+	PIX0,
+	RDI0,
+	PIX1,
+	RDI1,
+	RDI2
+};
+
+struct ispif_intf_cmd_reg {
+	u32 cmd_0;
+	u32 cmd_1;
+};
+
+struct ispif_line {
+	u8 id;
+	u8 csid_id;
+	u8 vfe_id;
+	enum ispif_intf interface;
+	struct v4l2_subdev subdev;
+	struct media_pad pads[MSM_ISPIF_PADS_NUM];
+	struct v4l2_mbus_framefmt fmt[MSM_ISPIF_PADS_NUM];
+};
+
+struct ispif_device {
+	void __iomem *base;
+	void __iomem *base_clk_mux;
+	u32 irq;
+	char irq_name[30];
+	struct clk **clock;
+	int nclocks;
+	struct clk **clock_for_reset;
+	int nclocks_for_reset;
+	struct completion reset_complete;
+	int power_count;
+	struct mutex power_lock;
+	struct ispif_intf_cmd_reg intf_cmd[MSM_ISPIF_VFE_NUM];
+	struct mutex config_lock;
+	struct ispif_line line[MSM_ISPIF_LINE_NUM];
+};
+
+struct resources_ispif;
+
+int msm_ispif_subdev_init(struct ispif_device *ispif,
+			  struct resources_ispif *res);
+
+int msm_ispif_register_entities(struct ispif_device *ispif,
+				struct v4l2_device *v4l2_dev);
+
+void msm_ispif_unregister_entities(struct ispif_device *ispif);
+
+#endif /* QC_MSM_CAMSS_ISPIF_H */
-- 
1.9.1

[PATCH 07/10] media: camss: Add VFE files

[Todor Tomov]

These files control the VFE module. The VFE has different input interfaces.
The PIX input interface feeds the input data to an image processing pipeline.
Three RDI input interfaces bypass the image processing pipeline. The VFE also
contains the AXI bus interface which writes the output data to memory.

RDI interfaces are supported in this version. PIX interface is not supported.

Signed-off-by: Todor Tomov <todor.tomov@linaro.org>
---
 drivers/media/platform/qcom/camss-8x16/vfe.c | 1877 ++++++++++++++++++++++++++
 drivers/media/platform/qcom/camss-8x16/vfe.h |  112 ++
 2 files changed, 1989 insertions(+)
 create mode 100644 drivers/media/platform/qcom/camss-8x16/vfe.c
 create mode 100644 drivers/media/platform/qcom/camss-8x16/vfe.h

diff --git a/drivers/media/platform/qcom/camss-8x16/vfe.c b/drivers/media/platform/qcom/camss-8x16/vfe.c
new file mode 100644
index 0000000..86fdf47
--- /dev/null
+++ b/drivers/media/platform/qcom/camss-8x16/vfe.c
@@ -0,0 +1,1877 @@
+/*
+ * vfe.c
+ *
+ * Qualcomm MSM Camera Subsystem - VFE Module
+ *
+ * Copyright (c) 2013-2015, The Linux Foundation. All rights reserved.
+ * Copyright (C) 2015-2016 Linaro Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only 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.
+ */
+#include <linux/clk.h>
+#include <linux/completion.h>
+#include <linux/interrupt.h>
+#include <linux/iommu.h>
+#include <linux/mutex.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/spinlock_types.h>
+#include <linux/spinlock.h>
+#include <media/media-entity.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-subdev.h>
+
+#include "vfe.h"
+#include "camss.h"
+
+#define MSM_VFE_NAME "msm_vfe"
+
+#define vfe_line_array(ptr_line)	\
+	((const struct vfe_line (*)[]) &(ptr_line[-(ptr_line->id)]))
+
+#define to_vfe(ptr_line)	\
+	container_of(vfe_line_array(ptr_line), struct vfe_device, ptr_line)
+
+#define VFE_0_HW_VERSION		0x000
+
+#define VFE_0_GLOBAL_RESET_CMD		0x00c
+#define VFE_0_GLOBAL_RESET_CMD_CORE	(1 << 0)
+#define VFE_0_GLOBAL_RESET_CMD_CAMIF	(1 << 1)
+#define VFE_0_GLOBAL_RESET_CMD_BUS	(1 << 2)
+#define VFE_0_GLOBAL_RESET_CMD_BUS_BDG	(1 << 3)
+#define VFE_0_GLOBAL_RESET_CMD_REGISTER	(1 << 4)
+#define VFE_0_GLOBAL_RESET_CMD_TIMER	(1 << 5)
+#define VFE_0_GLOBAL_RESET_CMD_PM	(1 << 6)
+#define VFE_0_GLOBAL_RESET_CMD_BUS_MISR	(1 << 7)
+#define VFE_0_GLOBAL_RESET_CMD_TESTGEN	(1 << 8)
+
+#define VFE_0_IRQ_CMD			0x024
+#define VFE_0_IRQ_CMD_GLOBAL_CLEAR	(1 << 0)
+
+#define VFE_0_IRQ_MASK_0		0x028
+#define VFE_0_IRQ_MASK_0_RDIn_REG_UPDATE(n)		(1 << ((n) + 5))
+#define VFE_0_IRQ_MASK_0_IMAGE_MASTER_n_PING_PONG(n)	(1 << ((n) + 8))
+#define VFE_0_IRQ_MASK_0_RESET_ACK			(1 << 31)
+#define VFE_0_IRQ_MASK_1		0x02c
+#define VFE_0_IRQ_MASK_1_VIOLATION			(1 << 7)
+#define VFE_0_IRQ_MASK_1_BUS_BDG_HALT_ACK		(1 << 8)
+#define VFE_0_IRQ_MASK_1_IMAGE_MASTER_n_BUS_OVERFLOW(n)	(1 << ((n) + 9))
+
+#define VFE_0_IRQ_CLEAR_0		0x030
+#define VFE_0_IRQ_CLEAR_1		0x034
+
+#define VFE_0_IRQ_STATUS_0		0x038
+#define VFE_0_IRQ_STATUS_0_RDIn_REG_UPDATE(n)		(1 << ((n) + 5))
+#define VFE_0_IRQ_STATUS_0_IMAGE_MASTER_n_PING_PONG(n)	(1 << ((n) + 8))
+#define VFE_0_IRQ_STATUS_0_RESET_ACK			(1 << 31)
+#define VFE_0_IRQ_STATUS_1		0x03c
+#define VFE_0_IRQ_STATUS_1_BUS_BDG_HALT_ACK		(1 << 8)
+
+#define VFE_0_BUS_CMD			0x4c
+#define VFE_0_BUS_CMD_Mx_RLD_CMD(x)	(1 << (x))
+
+#define VFE_0_BUS_CFG			0x050
+
+#define VFE_0_BUS_XBAR_CFG_x(x)		(0x58 + 0x4 * ((x) / 2))
+#define VFE_0_BUS_XBAR_CFG_x_M0_SINGLE_STREAM_SEL_SHIFT		8
+#define VFE_0_BUS_XBAR_CFG_x_M_SINGLE_STREAM_SEL_VAL_RDI0	5
+#define VFE_0_BUS_XBAR_CFG_x_M_SINGLE_STREAM_SEL_VAL_RDI1	6
+#define VFE_0_BUS_XBAR_CFG_x_M_SINGLE_STREAM_SEL_VAL_RDI2	7
+
+#define VFE_0_BUS_IMAGE_MASTER_n_WR_CFG(n)		(0x06c + 0x24 * (n))
+#define VFE_0_BUS_IMAGE_MASTER_n_WR_CFG_WR_PATH_SHIFT	0
+#define VFE_0_BUS_IMAGE_MASTER_n_WR_CFG_FRM_BASED_SHIFT	1
+#define VFE_0_BUS_IMAGE_MASTER_n_WR_PING_ADDR(n)	(0x070 + 0x24 * (n))
+#define VFE_0_BUS_IMAGE_MASTER_n_WR_PONG_ADDR(n)	(0x074 + 0x24 * (n))
+#define VFE_0_BUS_IMAGE_MASTER_n_WR_ADDR_CFG(n)		(0x078 + 0x24 * (n))
+#define VFE_0_BUS_IMAGE_MASTER_n_WR_ADDR_CFG_FRM_DROP_PER_SHIFT	2
+#define VFE_0_BUS_IMAGE_MASTER_n_WR_ADDR_CFG_FRM_DROP_PER_MASK	(0x1F << 2)
+
+#define VFE_0_BUS_IMAGE_MASTER_n_WR_UB_CFG(n)		(0x07c + 0x24 * (n))
+#define VFE_0_BUS_IMAGE_MASTER_n_WR_UB_CFG_OFFSET_SHIFT	16
+#define VFE_0_BUS_IMAGE_MASTER_n_WR_FRAMEDROP_PATTERN(n)	\
+							(0x088 + 0x24 * (n))
+#define VFE_0_BUS_IMAGE_MASTER_n_WR_IRQ_SUBSAMPLE_PATTERN(n)	\
+							(0x08c + 0x24 * (n))
+#define VFE_0_BUS_IMAGE_MASTER_n_WR_IRQ_SUBSAMPLE_PATTERN_DEF	0xffffffff
+
+#define VFE_0_BUS_PING_PONG_STATUS	0x268
+
+#define VFE_0_BUS_BDG_CMD		0x2c0
+#define VFE_0_BUS_BDG_CMD_HALT_REQ	1
+
+#define VFE_0_BUS_BDG_QOS_CFG_0		0x2c4
+#define VFE_0_BUS_BDG_QOS_CFG_1		0x2c8
+#define VFE_0_BUS_BDG_QOS_CFG_2		0x2cc
+#define VFE_0_BUS_BDG_QOS_CFG_3		0x2d0
+#define VFE_0_BUS_BDG_QOS_CFG_4		0x2d4
+#define VFE_0_BUS_BDG_QOS_CFG_5		0x2d8
+#define VFE_0_BUS_BDG_QOS_CFG_6		0x2dc
+#define VFE_0_BUS_BDG_QOS_CFG_7		0x2e0
+
+#define VFE_0_RDI_CFG_x(x)		(0x2e8 + (0x4 * (x)))
+#define VFE_0_RDI_CFG_x_RDI_STREAM_SEL_SHIFT	28
+#define VFE_0_RDI_CFG_x_RDI_STREAM_SEL_MASK	(0xf << 28)
+#define VFE_0_RDI_CFG_x_RDI_M0_SEL_SHIFT	4
+#define VFE_0_RDI_CFG_x_RDI_M0_SEL_MASK		(0xf << 4)
+#define VFE_0_RDI_CFG_x_RDI_EN_BIT		(1 << 2)
+#define VFE_0_RDI_CFG_x_MIPI_EN_BITS		0x3
+#define VFE_0_RDI_CFG_x_RDI_Mr_FRAME_BASED_EN(r)	(1 << (16 + (r)))
+
+#define VFE_0_REG_UPDATE			0x378
+#define VFE_0_REG_UPDATE_RDIn(n)		(1 << (1 + (n)))
+
+#define VFE_0_CGC_OVERRIDE_1			0x974
+#define VFE_0_CGC_OVERRIDE_1_IMAGE_Mx_CGC_OVERRIDE(x)	(1 << (x))
+
+/* VFE reset timeout */
+#define VFE_RESET_TIMEOUT_MS 50
+/* VFE halt timeout */
+#define VFE_HALT_TIMEOUT_MS 100
+/* Max number of frame drop updates per frame */
+#define VFE_FRAME_DROP_UPDATES 5
+/* Frame drop value. NOTE: VAL + UPDATES should not exceed 31 */
+#define VFE_FRAME_DROP_VAL 20
+
+static const u32 vfe_formats[] = {
+	MEDIA_BUS_FMT_UYVY8_2X8,
+	MEDIA_BUS_FMT_VYUY8_2X8,
+	MEDIA_BUS_FMT_YUYV8_2X8,
+	MEDIA_BUS_FMT_YVYU8_2X8,
+	MEDIA_BUS_FMT_SBGGR8_1X8,
+	MEDIA_BUS_FMT_SGBRG8_1X8,
+	MEDIA_BUS_FMT_SGRBG8_1X8,
+	MEDIA_BUS_FMT_SRGGB8_1X8,
+	MEDIA_BUS_FMT_SBGGR10_1X10,
+	MEDIA_BUS_FMT_SGBRG10_1X10,
+	MEDIA_BUS_FMT_SGRBG10_1X10,
+	MEDIA_BUS_FMT_SRGGB10_1X10,
+	MEDIA_BUS_FMT_SBGGR12_1X12,
+	MEDIA_BUS_FMT_SGBRG12_1X12,
+	MEDIA_BUS_FMT_SGRBG12_1X12,
+	MEDIA_BUS_FMT_SRGGB12_1X12,
+};
+
+static inline void vfe_reg_clr(struct vfe_device *vfe, u32 reg, u32 clr_bits)
+{
+	u32 bits = readl_relaxed(vfe->base + reg);
+
+	writel_relaxed(bits & ~clr_bits, vfe->base + reg);
+}
+
+static inline void vfe_reg_set(struct vfe_device *vfe, u32 reg, u32 set_bits)
+{
+	u32 bits = readl_relaxed(vfe->base + reg);
+
+	writel_relaxed(bits | set_bits, vfe->base + reg);
+}
+
+static void vfe_global_reset(struct vfe_device *vfe)
+{
+	u32 reset_bits = VFE_0_GLOBAL_RESET_CMD_TESTGEN		|
+			 VFE_0_GLOBAL_RESET_CMD_BUS_MISR	|
+			 VFE_0_GLOBAL_RESET_CMD_PM		|
+			 VFE_0_GLOBAL_RESET_CMD_TIMER		|
+			 VFE_0_GLOBAL_RESET_CMD_REGISTER	|
+			 VFE_0_GLOBAL_RESET_CMD_BUS_BDG		|
+			 VFE_0_GLOBAL_RESET_CMD_BUS		|
+			 VFE_0_GLOBAL_RESET_CMD_CAMIF		|
+			 VFE_0_GLOBAL_RESET_CMD_CORE;
+
+	writel_relaxed(reset_bits, vfe->base + VFE_0_GLOBAL_RESET_CMD);
+}
+
+static void vfe_wm_enable(struct vfe_device *vfe, u8 wm, u8 enable)
+{
+	if (enable)
+		vfe_reg_set(vfe, VFE_0_BUS_IMAGE_MASTER_n_WR_CFG(wm),
+			    1 << VFE_0_BUS_IMAGE_MASTER_n_WR_CFG_WR_PATH_SHIFT);
+	else
+		vfe_reg_clr(vfe, VFE_0_BUS_IMAGE_MASTER_n_WR_CFG(wm),
+			    1 << VFE_0_BUS_IMAGE_MASTER_n_WR_CFG_WR_PATH_SHIFT);
+}
+
+static void vfe_wm_frame_based(struct vfe_device *vfe, u8 wm, u8 enable)
+{
+	if (enable)
+		vfe_reg_set(vfe, VFE_0_BUS_IMAGE_MASTER_n_WR_CFG(wm),
+			1 << VFE_0_BUS_IMAGE_MASTER_n_WR_CFG_FRM_BASED_SHIFT);
+	else
+		vfe_reg_clr(vfe, VFE_0_BUS_IMAGE_MASTER_n_WR_CFG(wm),
+			1 << VFE_0_BUS_IMAGE_MASTER_n_WR_CFG_FRM_BASED_SHIFT);
+}
+
+static void vfe_wm_set_framedrop_period(struct vfe_device *vfe, u8 wm, u8 per)
+{
+	u32 reg;
+
+	reg = readl_relaxed(vfe->base +
+			    VFE_0_BUS_IMAGE_MASTER_n_WR_ADDR_CFG(wm));
+
+	reg &= ~(VFE_0_BUS_IMAGE_MASTER_n_WR_ADDR_CFG_FRM_DROP_PER_MASK);
+
+	reg |= (per << VFE_0_BUS_IMAGE_MASTER_n_WR_ADDR_CFG_FRM_DROP_PER_SHIFT)
+		& VFE_0_BUS_IMAGE_MASTER_n_WR_ADDR_CFG_FRM_DROP_PER_MASK;
+
+	writel_relaxed(reg,
+		       vfe->base + VFE_0_BUS_IMAGE_MASTER_n_WR_ADDR_CFG(wm));
+}
+
+static void vfe_wm_set_framedrop_pattern(struct vfe_device *vfe, u8 wm,
+					 u32 pattern)
+{
+	writel_relaxed(pattern,
+	       vfe->base + VFE_0_BUS_IMAGE_MASTER_n_WR_FRAMEDROP_PATTERN(wm));
+}
+
+static void vfe_wm_set_ub_cfg(struct vfe_device *vfe, u8 wm, u16 offset,
+			      u16 depth)
+{
+	u32 reg;
+
+	reg = (offset << VFE_0_BUS_IMAGE_MASTER_n_WR_UB_CFG_OFFSET_SHIFT) |
+		depth;
+	writel_relaxed(reg, vfe->base + VFE_0_BUS_IMAGE_MASTER_n_WR_UB_CFG(wm));
+}
+
+static void vfe_bus_reload_wm(struct vfe_device *vfe, u8 wm)
+{
+	wmb();
+	writel_relaxed(VFE_0_BUS_CMD_Mx_RLD_CMD(wm), vfe->base + VFE_0_BUS_CMD);
+	wmb();
+}
+
+static void vfe_wm_set_ping_addr(struct vfe_device *vfe, u8 wm, u32 addr)
+{
+	writel_relaxed(addr,
+		       vfe->base + VFE_0_BUS_IMAGE_MASTER_n_WR_PING_ADDR(wm));
+}
+
+static void vfe_wm_set_pong_addr(struct vfe_device *vfe, u8 wm, u32 addr)
+{
+	writel_relaxed(addr,
+		       vfe->base + VFE_0_BUS_IMAGE_MASTER_n_WR_PONG_ADDR(wm));
+}
+
+static int vfe_wm_get_ping_pong_status(struct vfe_device *vfe, u8 wm)
+{
+	u32 reg;
+
+	reg = readl_relaxed(vfe->base + VFE_0_BUS_PING_PONG_STATUS);
+
+	return (reg >> wm) & 0x1;
+}
+
+static void vfe_bus_enable_wr_if(struct vfe_device *vfe, u8 enable)
+{
+	if (enable)
+		writel_relaxed(0x10000009, vfe->base + VFE_0_BUS_CFG);
+	else
+		writel_relaxed(0, vfe->base + VFE_0_BUS_CFG);
+}
+
+static void vfe_bus_connect_wm_to_rdi(struct vfe_device *vfe, u8 wm,
+				      enum vfe_line_id id)
+{
+	u32 reg;
+
+	reg = VFE_0_RDI_CFG_x_MIPI_EN_BITS;
+	reg |= VFE_0_RDI_CFG_x_RDI_Mr_FRAME_BASED_EN(id);
+	vfe_reg_set(vfe, VFE_0_RDI_CFG_x(0), reg);
+
+	reg = VFE_0_RDI_CFG_x_RDI_EN_BIT;
+	reg |= ((3 * id) << VFE_0_RDI_CFG_x_RDI_STREAM_SEL_SHIFT) &
+		VFE_0_RDI_CFG_x_RDI_STREAM_SEL_MASK;
+	vfe_reg_set(vfe, VFE_0_RDI_CFG_x(id), reg);
+
+	switch (id) {
+	case VFE_LINE_RDI0:
+	default:
+		reg = VFE_0_BUS_XBAR_CFG_x_M_SINGLE_STREAM_SEL_VAL_RDI0 <<
+		      VFE_0_BUS_XBAR_CFG_x_M0_SINGLE_STREAM_SEL_SHIFT;
+		break;
+	case VFE_LINE_RDI1:
+		reg = VFE_0_BUS_XBAR_CFG_x_M_SINGLE_STREAM_SEL_VAL_RDI1 <<
+		      VFE_0_BUS_XBAR_CFG_x_M0_SINGLE_STREAM_SEL_SHIFT;
+		break;
+	case VFE_LINE_RDI2:
+		reg = VFE_0_BUS_XBAR_CFG_x_M_SINGLE_STREAM_SEL_VAL_RDI2 <<
+		      VFE_0_BUS_XBAR_CFG_x_M0_SINGLE_STREAM_SEL_SHIFT;
+		break;
+	}
+
+	if (wm % 2 == 1)
+		reg <<= 16;
+
+	vfe_reg_set(vfe, VFE_0_BUS_XBAR_CFG_x(wm), reg);
+
+	writel_relaxed(VFE_0_BUS_IMAGE_MASTER_n_WR_IRQ_SUBSAMPLE_PATTERN_DEF,
+	       vfe->base +
+	       VFE_0_BUS_IMAGE_MASTER_n_WR_IRQ_SUBSAMPLE_PATTERN(wm));
+}
+
+static void vfe_bus_disconnect_wm_from_rdi(struct vfe_device *vfe, u8 wm,
+					   enum vfe_line_id id)
+{
+	u32 reg;
+
+	reg = VFE_0_RDI_CFG_x_RDI_Mr_FRAME_BASED_EN(id);
+	vfe_reg_clr(vfe, VFE_0_RDI_CFG_x(0), reg);
+
+	reg = VFE_0_RDI_CFG_x_RDI_EN_BIT;
+	vfe_reg_clr(vfe, VFE_0_RDI_CFG_x(id), reg);
+
+	switch (id) {
+	case VFE_LINE_RDI0:
+	default:
+		reg = VFE_0_BUS_XBAR_CFG_x_M_SINGLE_STREAM_SEL_VAL_RDI0 <<
+		      VFE_0_BUS_XBAR_CFG_x_M0_SINGLE_STREAM_SEL_SHIFT;
+		break;
+	case VFE_LINE_RDI1:
+		reg = VFE_0_BUS_XBAR_CFG_x_M_SINGLE_STREAM_SEL_VAL_RDI1 <<
+		      VFE_0_BUS_XBAR_CFG_x_M0_SINGLE_STREAM_SEL_SHIFT;
+		break;
+	case VFE_LINE_RDI2:
+		reg = VFE_0_BUS_XBAR_CFG_x_M_SINGLE_STREAM_SEL_VAL_RDI2 <<
+		      VFE_0_BUS_XBAR_CFG_x_M0_SINGLE_STREAM_SEL_SHIFT;
+		break;
+	}
+
+	if (wm % 2 == 1)
+		reg <<= 16;
+
+	vfe_reg_clr(vfe, VFE_0_BUS_XBAR_CFG_x(wm), reg);
+}
+
+static void vfe_set_rdi_cid(struct vfe_device *vfe, enum vfe_line_id id, u8 cid)
+{
+	vfe_reg_clr(vfe, VFE_0_RDI_CFG_x(id),
+		    VFE_0_RDI_CFG_x_RDI_M0_SEL_MASK);
+
+	vfe_reg_set(vfe, VFE_0_RDI_CFG_x(id),
+		    cid << VFE_0_RDI_CFG_x_RDI_M0_SEL_SHIFT);
+}
+
+static void vfe_reg_update(struct vfe_device *vfe, enum vfe_line_id line_id)
+{
+	vfe->reg_update |= VFE_0_REG_UPDATE_RDIn(line_id);
+	wmb();
+	writel_relaxed(vfe->reg_update, vfe->base + VFE_0_REG_UPDATE);
+	wmb();
+}
+
+static void vfe_enable_irq_wm_line(struct vfe_device *vfe, u8 wm,
+				   enum vfe_line_id line_id, u8 enable) {
+	u32 irq_en0 = readl_relaxed(vfe->base + VFE_0_IRQ_MASK_0);
+	u32 irq_en1 = readl_relaxed(vfe->base + VFE_0_IRQ_MASK_1);
+
+	if (enable) {
+		irq_en0 |= VFE_0_IRQ_MASK_0_IMAGE_MASTER_n_PING_PONG(wm);
+		irq_en0 |= VFE_0_IRQ_MASK_0_RDIn_REG_UPDATE(line_id);
+		irq_en1 |= VFE_0_IRQ_MASK_1_IMAGE_MASTER_n_BUS_OVERFLOW(wm);
+	} else {
+		irq_en0 &= ~VFE_0_IRQ_MASK_0_IMAGE_MASTER_n_PING_PONG(wm);
+		irq_en0 &= ~VFE_0_IRQ_MASK_0_RDIn_REG_UPDATE(line_id);
+		irq_en1 &= ~VFE_0_IRQ_MASK_1_IMAGE_MASTER_n_BUS_OVERFLOW(wm);
+	}
+
+	writel_relaxed(irq_en0, vfe->base + VFE_0_IRQ_MASK_0);
+	writel_relaxed(irq_en1, vfe->base + VFE_0_IRQ_MASK_1);
+}
+
+static void vfe_enable_irq_common(struct vfe_device *vfe)
+{
+	u32 irq_en0 = readl_relaxed(vfe->base + VFE_0_IRQ_MASK_0);
+	u32 irq_en1 = readl_relaxed(vfe->base + VFE_0_IRQ_MASK_1);
+
+	irq_en0 |= VFE_0_IRQ_MASK_0_RESET_ACK;
+
+	irq_en1 |= VFE_0_IRQ_MASK_1_VIOLATION;
+	irq_en1 |= VFE_0_IRQ_MASK_1_BUS_BDG_HALT_ACK;
+
+	writel(irq_en0, vfe->base + VFE_0_IRQ_MASK_0);
+	writel(irq_en1, vfe->base + VFE_0_IRQ_MASK_1);
+}
+
+/*
+ * vfe_reset - Trigger reset on VFE module and wait to complete
+ * @vfe: VFE device
+ *
+ * Return 0 on success or a negative error code otherwise
+ */
+static int vfe_reset(struct vfe_device *vfe)
+{
+	unsigned long time;
+
+	reinit_completion(&vfe->reset_complete);
+
+	vfe_global_reset(vfe);
+
+	time = wait_for_completion_timeout(&vfe->reset_complete,
+		msecs_to_jiffies(VFE_RESET_TIMEOUT_MS));
+	if (!time) {
+		dev_err(to_device(vfe), "VFE reset timeout\n");
+		return -EIO;
+	}
+
+	return 0;
+}
+
+/*
+ * vfe_halt - Trigger halt on VFE module and wait to complete
+ * @vfe: VFE device
+ *
+ * Return 0 on success or a negative error code otherwise
+ */
+static int vfe_halt(struct vfe_device *vfe)
+{
+	unsigned long time;
+
+	reinit_completion(&vfe->halt_complete);
+
+	writel_relaxed(VFE_0_BUS_BDG_CMD_HALT_REQ,
+		       vfe->base + VFE_0_BUS_BDG_CMD);
+
+	time = wait_for_completion_timeout(&vfe->halt_complete,
+		msecs_to_jiffies(VFE_HALT_TIMEOUT_MS));
+	if (!time) {
+		dev_err(to_device(vfe), "VFE halt timeout\n");
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static void vfe_init_outputs(struct vfe_device *vfe)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(vfe->line); i++) {
+		struct vfe_output *output = &vfe->line[i].output;
+
+		output->state = VFE_OUTPUT_OFF;
+		output->buf[0] = NULL;
+		output->buf[1] = NULL;
+		INIT_LIST_HEAD(&output->pending_bufs);
+	}
+}
+
+static void vfe_reset_output_maps(struct vfe_device *vfe)
+{
+	unsigned long flags;
+	int i;
+
+	spin_lock_irqsave(&vfe->output_lock, flags);
+
+	for (i = 0; i < ARRAY_SIZE(vfe->wm_output_map); i++)
+		vfe->wm_output_map[i] = VFE_LINE_NONE;
+
+	spin_unlock_irqrestore(&vfe->output_lock, flags);
+}
+
+static void vfe_set_qos(struct vfe_device *vfe)
+{
+	u32 val = 0xaaa5aaa5;
+	u32 val7 = 0x0001aaa5;
+
+	writel_relaxed(val, vfe->base + VFE_0_BUS_BDG_QOS_CFG_0);
+	writel_relaxed(val, vfe->base + VFE_0_BUS_BDG_QOS_CFG_1);
+	writel_relaxed(val, vfe->base + VFE_0_BUS_BDG_QOS_CFG_2);
+	writel_relaxed(val, vfe->base + VFE_0_BUS_BDG_QOS_CFG_3);
+	writel_relaxed(val, vfe->base + VFE_0_BUS_BDG_QOS_CFG_4);
+	writel_relaxed(val, vfe->base + VFE_0_BUS_BDG_QOS_CFG_5);
+	writel_relaxed(val, vfe->base + VFE_0_BUS_BDG_QOS_CFG_6);
+	writel_relaxed(val7, vfe->base + VFE_0_BUS_BDG_QOS_CFG_7);
+}
+
+static void vfe_set_cgc_override(struct vfe_device *vfe, u8 wm, u8 enable)
+{
+	u32 val = VFE_0_CGC_OVERRIDE_1_IMAGE_Mx_CGC_OVERRIDE(wm);
+
+	if (enable)
+		vfe_reg_set(vfe, VFE_0_CGC_OVERRIDE_1, val);
+	else
+		vfe_reg_clr(vfe, VFE_0_CGC_OVERRIDE_1, val);
+
+	wmb();
+}
+
+static void vfe_output_init_addrs(struct vfe_device *vfe,
+				  struct vfe_output *output, u8 sync)
+{
+	u32 ping_addr = 0;
+	u32 pong_addr = 0;
+
+	output->active_buf = 0;
+
+	if (output->buf[0])
+		ping_addr = output->buf[0]->addr;
+
+	if (output->buf[1])
+		pong_addr = output->buf[1]->addr;
+	else
+		pong_addr = ping_addr;
+
+	vfe_wm_set_ping_addr(vfe, output->wm_idx, ping_addr);
+	vfe_wm_set_pong_addr(vfe, output->wm_idx, pong_addr);
+	if (sync)
+		vfe_bus_reload_wm(vfe, output->wm_idx);
+}
+
+static void vfe_output_update_ping_addr(struct vfe_device *vfe,
+					struct vfe_output *output, u8 sync)
+{
+	u32 addr = 0;
+
+	if (output->buf[0])
+		addr = output->buf[0]->addr;
+
+	vfe_wm_set_ping_addr(vfe, output->wm_idx, addr);
+	if (sync)
+		vfe_bus_reload_wm(vfe, output->wm_idx);
+}
+
+static void vfe_output_update_pong_addr(struct vfe_device *vfe,
+					struct vfe_output *output, u8 sync)
+{
+	u32 addr = 0;
+
+	if (output->buf[1])
+		addr = output->buf[1]->addr;
+
+	vfe_wm_set_pong_addr(vfe, output->wm_idx, addr);
+	if (sync)
+		vfe_bus_reload_wm(vfe, output->wm_idx);
+
+}
+
+static int vfe_reserve_wm(struct vfe_device *vfe, enum vfe_line_id line_id)
+{
+	int ret = -EBUSY;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(vfe->wm_output_map); i++) {
+		if (vfe->wm_output_map[i] == VFE_LINE_NONE) {
+			vfe->wm_output_map[i] = line_id;
+			ret = i;
+			break;
+		}
+	}
+
+	return ret;
+}
+
+static int vfe_release_wm(struct vfe_device *vfe, u8 wm)
+{
+	if (wm > ARRAY_SIZE(vfe->wm_output_map))
+		return -EINVAL;
+
+	vfe->wm_output_map[wm] = VFE_LINE_NONE;
+
+	return 0;
+}
+
+static void vfe_output_frame_drop(struct vfe_device *vfe,
+				  struct vfe_output *output,
+				  u32 drop_pattern)
+{
+	u8 drop_period;
+
+	/* We need to toggle update period to be valid on next frame */
+	output->drop_update_idx++;
+	output->drop_update_idx %= VFE_FRAME_DROP_UPDATES;
+	drop_period = VFE_FRAME_DROP_VAL + output->drop_update_idx;
+
+	vfe_wm_set_framedrop_period(vfe, output->wm_idx, drop_period);
+	vfe_wm_set_framedrop_pattern(vfe, output->wm_idx, drop_pattern);
+	vfe_reg_update(vfe, container_of(output, struct vfe_line, output)->id);
+
+}
+
+static struct camss_buffer *vfe_buf_get_pending(struct vfe_output *output)
+{
+	struct camss_buffer *buffer = NULL;
+
+	if (!list_empty(&output->pending_bufs)) {
+		buffer = list_first_entry(&output->pending_bufs,
+					  struct camss_buffer,
+					  queue);
+		list_del(&buffer->queue);
+	}
+
+	return buffer;
+}
+
+/*
+ * vfe_buf_add_pending - Add output buffer to list of pending
+ * @output: VFE output
+ * @buffer: Video buffer
+ */
+static void vfe_buf_add_pending(struct vfe_output *output,
+				struct camss_buffer *buffer)
+{
+	INIT_LIST_HEAD(&buffer->queue);
+	list_add_tail(&buffer->queue, &output->pending_bufs);
+}
+
+/*
+ * vfe_buf_flush_pending - Flush all pending buffers.
+ * @output: VFE output
+ */
+static void vfe_buf_flush_pending(struct vfe_output *output)
+{
+	struct camss_buffer *buf;
+	struct camss_buffer *t;
+
+	list_for_each_entry_safe(buf, t, &output->pending_bufs, queue) {
+		vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
+		list_del(&buf->queue);
+	}
+}
+
+static void vfe_buf_update_wm_on_next(struct vfe_device *vfe,
+				      struct vfe_output *output)
+{
+	switch (output->state) {
+	case VFE_OUTPUT_CONTINUOUS:
+		vfe_output_frame_drop(vfe, output, 3);
+		break;
+	case VFE_OUTPUT_SINGLE:
+	default:
+		dev_err_ratelimited(to_device(vfe),
+				    "Next buf in wrong state! %d\n",
+				    output->state);
+		break;
+	}
+}
+
+static void vfe_buf_update_wm_on_last(struct vfe_device *vfe,
+				      struct vfe_output *output)
+{
+	switch (output->state) {
+	case VFE_OUTPUT_CONTINUOUS:
+		output->state = VFE_OUTPUT_SINGLE;
+		vfe_output_frame_drop(vfe, output, 1);
+		break;
+	case VFE_OUTPUT_SINGLE:
+		output->state = VFE_OUTPUT_STOPPING;
+		vfe_output_frame_drop(vfe, output, 0);
+		break;
+	default:
+		dev_err_ratelimited(to_device(vfe),
+				    "Last buff in wrong state! %d\n",
+				    output->state);
+		break;
+	}
+}
+
+static void vfe_buf_update_wm_on_new(struct vfe_device *vfe,
+				     struct vfe_output *output,
+				     struct camss_buffer *new_buf)
+{
+	int inactive_idx;
+
+	switch (output->state) {
+	case VFE_OUTPUT_SINGLE:
+		inactive_idx = !output->active_buf;
+
+		if (!output->buf[inactive_idx]) {
+			output->buf[inactive_idx] = new_buf;
+
+			if (inactive_idx)
+				vfe_output_update_pong_addr(vfe, output, 0);
+			else
+				vfe_output_update_ping_addr(vfe, output, 0);
+
+			vfe_output_frame_drop(vfe, output, 3);
+			output->state = VFE_OUTPUT_CONTINUOUS;
+		} else {
+			vfe_buf_add_pending(output, new_buf);
+			dev_err_ratelimited(to_device(vfe),
+					    "Inactive buffer is busy\n");
+		}
+		break;
+
+	case VFE_OUTPUT_IDLE:
+		if (!output->buf[0]) {
+			output->buf[0] = new_buf;
+
+			vfe_output_init_addrs(vfe, output, 1);
+
+			vfe_output_frame_drop(vfe, output, 1);
+			output->state = VFE_OUTPUT_SINGLE;
+		} else {
+			vfe_buf_add_pending(output, new_buf);
+			dev_err_ratelimited(to_device(vfe),
+					    "Output idle with buffer set!\n");
+		}
+		break;
+
+	case VFE_OUTPUT_CONTINUOUS:
+	default:
+		vfe_buf_add_pending(output, new_buf);
+		break;
+	}
+}
+
+static int vfe_get_output(struct vfe_line *line)
+{
+	struct vfe_device *vfe = to_vfe(line);
+	struct vfe_output *output;
+	unsigned long flags;
+	int wm_idx;
+
+	spin_lock_irqsave(&vfe->output_lock, flags);
+
+	output = &line->output;
+	if (output->state != VFE_OUTPUT_OFF) {
+		dev_err(to_device(vfe), "Output is running\n");
+		goto error;
+	}
+	output->state = VFE_OUTPUT_RESERVED;
+
+	output->active_buf = 0;
+
+	/* We will use only one wm per output for now */
+	wm_idx = vfe_reserve_wm(vfe, line->id);
+	if (wm_idx < 0) {
+		dev_err(to_device(vfe), "Can not reserve wm\n");
+		goto error_get_wm;
+	}
+	output->drop_update_idx = 0;
+	output->wm_idx = wm_idx;
+
+	spin_unlock_irqrestore(&vfe->output_lock, flags);
+
+	return 0;
+
+error_get_wm:
+	output->state = VFE_OUTPUT_OFF;
+error:
+	spin_unlock_irqrestore(&vfe->output_lock, flags);
+
+	return -EINVAL;
+}
+
+static int vfe_put_output(struct vfe_line *line)
+{
+	struct vfe_device *vfe = to_vfe(line);
+	struct vfe_output *output = &line->output;
+	unsigned long flags;
+	int ret;
+
+	spin_lock_irqsave(&vfe->output_lock, flags);
+
+	ret = vfe_release_wm(vfe, output->wm_idx);
+	if (ret < 0)
+		goto out;
+
+	output->state = VFE_OUTPUT_OFF;
+
+out:
+	spin_unlock_irqrestore(&vfe->output_lock, flags);
+	return ret;
+}
+
+static int vfe_enable_output(struct vfe_line *line)
+{
+	struct vfe_device *vfe = to_vfe(line);
+	struct vfe_output *output = &line->output;
+	unsigned long flags;
+	u16 ub_size;
+
+	switch (vfe->id) {
+	case 0:
+		ub_size = MSM_VFE_VFE0_UB_SIZE_RDI;
+		break;
+	case 1:
+		ub_size = MSM_VFE_VFE1_UB_SIZE_RDI;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	spin_lock_irqsave(&vfe->output_lock, flags);
+
+	vfe->reg_update &= ~VFE_0_REG_UPDATE_RDIn(line->id);
+
+	if (output->state != VFE_OUTPUT_RESERVED) {
+		dev_err(to_device(vfe), "Output is not in reserved state %d\n",
+			output->state);
+		spin_unlock_irqrestore(&vfe->output_lock, flags);
+		return -EINVAL;
+	}
+	output->state = VFE_OUTPUT_IDLE;
+
+	output->buf[0] = vfe_buf_get_pending(output);
+	if (output->buf[0])
+		output->state = VFE_OUTPUT_SINGLE;
+
+	output->buf[1] = vfe_buf_get_pending(output);
+	if (output->buf[1])
+		output->state = VFE_OUTPUT_CONTINUOUS;
+
+	switch (output->state) {
+	case VFE_OUTPUT_SINGLE:
+		vfe_output_frame_drop(vfe, output, 1);
+		break;
+	case VFE_OUTPUT_CONTINUOUS:
+		vfe_output_frame_drop(vfe, output, 3);
+		break;
+	default:
+		vfe_output_frame_drop(vfe, output, 0);
+		break;
+	}
+
+	output->sequence = 0;
+
+	vfe_output_init_addrs(vfe, output, 0);
+
+	vfe_set_cgc_override(vfe, output->wm_idx, 1);
+
+	vfe_enable_irq_wm_line(vfe, output->wm_idx, line->id, 1);
+
+	vfe_bus_connect_wm_to_rdi(vfe, output->wm_idx, line->id);
+
+	vfe_set_rdi_cid(vfe, line->id, 0);
+
+	vfe_wm_set_ub_cfg(vfe, output->wm_idx,
+			  (ub_size + 1) * output->wm_idx, ub_size);
+
+	vfe_wm_frame_based(vfe, output->wm_idx, 1);
+	vfe_wm_enable(vfe, output->wm_idx, 1);
+
+	vfe_bus_reload_wm(vfe, output->wm_idx);
+
+	vfe_reg_update(vfe, line->id);
+
+	spin_unlock_irqrestore(&vfe->output_lock, flags);
+
+	return 0;
+}
+
+static int vfe_disable_output(struct vfe_line *line)
+{
+	struct vfe_device *vfe = to_vfe(line);
+	struct vfe_output *output = &line->output;
+	unsigned long flags;
+
+	spin_lock_irqsave(&vfe->output_lock, flags);
+
+	vfe_wm_enable(vfe, output->wm_idx, 0);
+	vfe_bus_disconnect_wm_from_rdi(vfe, output->wm_idx, line->id);
+	vfe_reg_update(vfe, line->id);
+
+	spin_unlock_irqrestore(&vfe->output_lock, flags);
+
+	return 0;
+}
+
+/*
+ * vfe_enable - Enable streaming on VFE line
+ * @line: VFE line
+ *
+ * Return 0 on success or a negative error code otherwise
+ */
+static int vfe_enable(struct vfe_line *line)
+{
+	struct vfe_device *vfe = to_vfe(line);
+	int ret;
+
+	mutex_lock(&vfe->stream_lock);
+
+	if (!vfe->stream_count) {
+		vfe_enable_irq_common(vfe);
+
+		vfe_bus_enable_wr_if(vfe, 1);
+
+		vfe_set_qos(vfe);
+	}
+
+	vfe->stream_count++;
+
+	mutex_unlock(&vfe->stream_lock);
+
+	ret = vfe_get_output(line);
+	if (ret < 0)
+		goto error_get_output;
+
+	ret = vfe_enable_output(line);
+	if (ret < 0)
+		goto error_enable_output;
+
+	vfe->was_streaming = 1;
+
+	return 0;
+
+
+error_enable_output:
+	vfe_put_output(line);
+
+error_get_output:
+	mutex_lock(&vfe->stream_lock);
+
+	if (vfe->stream_count == 1)
+		vfe_bus_enable_wr_if(vfe, 0);
+
+	vfe->stream_count--;
+
+	mutex_unlock(&vfe->stream_lock);
+
+	return ret;
+}
+
+/*
+ * vfe_disable - Disable streaming on VFE line
+ * @line: VFE line
+ *
+ * Return 0 on success or a negative error code otherwise
+ */
+static int vfe_disable(struct vfe_line *line)
+{
+	struct vfe_device *vfe = to_vfe(line);
+
+	mutex_lock(&vfe->stream_lock);
+
+	if (vfe->stream_count == 1)
+		vfe_bus_enable_wr_if(vfe, 0);
+
+	vfe->stream_count--;
+
+	mutex_unlock(&vfe->stream_lock);
+
+	vfe_disable_output(line);
+
+	vfe_put_output(line);
+
+	return 0;
+}
+
+/*
+ * vfe_isr_reg_update - Process reg update interrupt
+ * @vfe: VFE Device
+ * @line_id: VFE line
+ */
+static void vfe_isr_reg_update(struct vfe_device *vfe, enum vfe_line_id line_id)
+{
+	struct vfe_output *output;
+	unsigned long flags;
+
+	spin_lock_irqsave(&vfe->output_lock, flags);
+	vfe->reg_update &= ~VFE_0_REG_UPDATE_RDIn(line_id);
+
+	output = &vfe->line[line_id].output;
+	if (output->state == VFE_OUTPUT_STOPPING) {
+		/* Release last buffer when hw is idle */
+		if (output->last_buffer) {
+			vb2_buffer_done(&output->last_buffer->vb.vb2_buf,
+					VB2_BUF_STATE_DONE);
+			output->last_buffer = NULL;
+		}
+		output->state = VFE_OUTPUT_IDLE;
+
+		/* Buffers received in stopping state are queued in */
+		/* dma pending queue, start next capture here */
+
+		output->buf[0] = vfe_buf_get_pending(output);
+		if (output->buf[0])
+			output->state = VFE_OUTPUT_SINGLE;
+
+		output->buf[1] = vfe_buf_get_pending(output);
+		if (output->buf[1])
+			output->state = VFE_OUTPUT_CONTINUOUS;
+
+		switch (output->state) {
+		case VFE_OUTPUT_SINGLE:
+			vfe_output_frame_drop(vfe, output, 2);
+			break;
+		case VFE_OUTPUT_CONTINUOUS:
+			vfe_output_frame_drop(vfe, output, 3);
+			break;
+		default:
+			vfe_output_frame_drop(vfe, output, 0);
+			break;
+		}
+
+		vfe_output_init_addrs(vfe, output, 1);
+	}
+
+	spin_unlock_irqrestore(&vfe->output_lock, flags);
+}
+
+/*
+ * vfe_isr_wm_done - Process write master done interrupt
+ * @vfe: VFE Device
+ * @wm: Write master id
+ */
+static void vfe_isr_wm_done(struct vfe_device *vfe, u8 wm)
+{
+	struct camss_buffer *ready_buf;
+	struct vfe_output *output;
+	dma_addr_t new_addr;
+	unsigned long flags;
+	u32 active_index;
+	u64 ts = ktime_get_ns();
+
+	active_index = vfe_wm_get_ping_pong_status(vfe, wm);
+
+	spin_lock_irqsave(&vfe->output_lock, flags);
+
+	if (vfe->wm_output_map[wm] == VFE_LINE_NONE) {
+		dev_err_ratelimited(to_device(vfe),
+				    "Received wm done for unmapped index\n");
+		goto out_unlock;
+	}
+	output = &vfe->line[vfe->wm_output_map[wm]].output;
+
+	if (output->active_buf == active_index) {
+		dev_err_ratelimited(to_device(vfe),
+				    "Active buffer mismatch!\n");
+		goto out_unlock;
+	}
+	output->active_buf = active_index;
+
+	ready_buf = output->buf[!active_index];
+	if (!ready_buf) {
+		dev_err_ratelimited(to_device(vfe),
+				    "Missing ready buf %d %d!\n",
+				    !active_index, output->state);
+		goto out_unlock;
+	}
+
+	ready_buf->vb.vb2_buf.timestamp = ts;
+	ready_buf->vb.sequence = output->sequence++;
+
+	/* Get next buffer */
+	output->buf[!active_index] = vfe_buf_get_pending(output);
+	if (!output->buf[!active_index]) {
+		/* No next buffer - set same address */
+		new_addr = ready_buf->addr;
+		vfe_buf_update_wm_on_last(vfe, output);
+	} else {
+		new_addr = output->buf[!active_index]->addr;
+		vfe_buf_update_wm_on_next(vfe, output);
+	}
+
+	if (active_index)
+		vfe_wm_set_ping_addr(vfe, wm, new_addr);
+	else
+		vfe_wm_set_pong_addr(vfe, wm, new_addr);
+
+	spin_unlock_irqrestore(&vfe->output_lock, flags);
+
+	if (output->state == VFE_OUTPUT_STOPPING)
+		output->last_buffer = ready_buf;
+	else
+		vb2_buffer_done(&ready_buf->vb.vb2_buf, VB2_BUF_STATE_DONE);
+
+	return;
+
+out_unlock:
+	spin_unlock_irqrestore(&vfe->output_lock, flags);
+}
+
+/*
+ * vfe_isr - ISPIF module interrupt handler
+ * @irq: Interrupt line
+ * @dev: VFE device
+ *
+ * Return IRQ_HANDLED on success
+ */
+static irqreturn_t vfe_isr(int irq, void *dev)
+{
+	struct vfe_device *vfe = dev;
+	u32 value0, value1;
+	int i;
+
+	value0 = readl_relaxed(vfe->base + VFE_0_IRQ_STATUS_0);
+	value1 = readl_relaxed(vfe->base + VFE_0_IRQ_STATUS_1);
+
+	writel_relaxed(value0, vfe->base + VFE_0_IRQ_CLEAR_0);
+	writel_relaxed(value1, vfe->base + VFE_0_IRQ_CLEAR_1);
+
+	wmb();
+	writel_relaxed(VFE_0_IRQ_CMD_GLOBAL_CLEAR, vfe->base + VFE_0_IRQ_CMD);
+
+	if (value0 & VFE_0_IRQ_STATUS_0_RESET_ACK)
+		complete(&vfe->reset_complete);
+
+	if (value1 & VFE_0_IRQ_STATUS_1_BUS_BDG_HALT_ACK) {
+		complete(&vfe->halt_complete);
+		writel_relaxed(0x0, vfe->base + VFE_0_BUS_BDG_CMD);
+	}
+
+	for (i = VFE_LINE_RDI0; i <= VFE_LINE_RDI2; i++)
+		if (value0 & VFE_0_IRQ_STATUS_0_RDIn_REG_UPDATE(i))
+			vfe_isr_reg_update(vfe, i);
+
+	for (i = 0; i < MSM_VFE_IMAGE_MASTERS_NUM; i++)
+		if (value0 & VFE_0_IRQ_STATUS_0_IMAGE_MASTER_n_PING_PONG(i))
+			vfe_isr_wm_done(vfe, i);
+
+	return IRQ_HANDLED;
+}
+
+/*
+ * vfe_get - Power up and reset VFE module
+ * @vfe: VFE Device
+ *
+ * Return 0 on success or a negative error code otherwise
+ */
+static int vfe_get(struct vfe_device *vfe)
+{
+	int ret;
+
+	mutex_lock(&vfe->power_lock);
+
+	if (vfe->power_count == 0) {
+		ret = camss_enable_clocks(vfe->nclocks, vfe->clock,
+					  to_device(vfe));
+		if (ret < 0)
+			goto error_clocks;
+
+		ret = vfe_reset(vfe);
+		if (ret < 0)
+			goto error_reset;
+
+		vfe_reset_output_maps(vfe);
+
+		vfe_init_outputs(vfe);
+	}
+	vfe->power_count++;
+
+	mutex_unlock(&vfe->power_lock);
+
+	return 0;
+
+error_reset:
+	camss_disable_clocks(vfe->nclocks, vfe->clock);
+
+error_clocks:
+	mutex_unlock(&vfe->power_lock);
+
+	return ret;
+}
+
+/*
+ * vfe_put - Power down VFE module
+ * @vfe: VFE Device
+ */
+static void vfe_put(struct vfe_device *vfe)
+{
+	mutex_lock(&vfe->power_lock);
+
+	if (vfe->power_count == 0) {
+		dev_err(to_device(vfe), "vfe power off on power_count == 0\n");
+		goto exit;
+	} else if (vfe->power_count == 1) {
+		if (vfe->was_streaming) {
+			vfe->was_streaming = 0;
+			vfe_halt(vfe);
+		}
+		camss_disable_clocks(vfe->nclocks, vfe->clock);
+	}
+
+	vfe->power_count--;
+
+exit:
+	mutex_unlock(&vfe->power_lock);
+}
+
+/*
+ * vfe_video_pad_to_line - Get pointer to VFE line by media pad
+ * @pad: Media pad
+ *
+ * Return pointer to vfe line structure
+ */
+static struct vfe_line *vfe_video_pad_to_line(struct media_pad *pad)
+{
+	struct media_pad *vfe_pad;
+	struct v4l2_subdev *subdev;
+
+	vfe_pad = media_entity_remote_pad(pad);
+	if (pad == NULL)
+		return NULL;
+
+	subdev = media_entity_to_v4l2_subdev(vfe_pad->entity);
+
+	return container_of(subdev, struct vfe_line, subdev);
+}
+
+/*
+ * vfe_queue_buffer - Add empty buffer
+ * @vid: Video device structure
+ * @buf: Buffer to be enqueued
+ *
+ * Add an empty buffer - depending on the current number of buffers it will be
+ * put in pending buffer queue or directly given to the hardware to be filled.
+ *
+ * Return 0 on success or a negative error code otherwise
+ */
+static int vfe_queue_buffer(struct camss_video *vid,
+			    struct camss_buffer *buf)
+{
+	struct vfe_device *vfe = &vid->camss->vfe;
+	struct vfe_line *line;
+	struct vfe_output *output;
+	unsigned long flags;
+
+	line = vfe_video_pad_to_line(&vid->pad);
+	if (!line) {
+		dev_err(to_device(vfe), "Can not queue buffer\n");
+		return -1;
+	}
+	output = &line->output;
+
+	spin_lock_irqsave(&vfe->output_lock, flags);
+
+	vfe_buf_update_wm_on_new(vfe, output, buf);
+
+	spin_unlock_irqrestore(&vfe->output_lock, flags);
+
+	return 0;
+}
+
+/*
+ * vfe_flush_buffers - Return all vb2 buffers
+ * @vid: Video device structure
+ *
+ * Return all buffers to vb2. This includes queued pending buffers (still
+ * unused) and any buffers given to the hardware but again still not used.
+ *
+ * Return 0 on success or a negative error code otherwise
+ */
+static int vfe_flush_buffers(struct camss_video *vid)
+{
+	struct vfe_device *vfe = &vid->camss->vfe;
+	struct vfe_line *line;
+	struct vfe_output *output;
+	unsigned long flags;
+
+	line = vfe_video_pad_to_line(&vid->pad);
+	if (!line) {
+		dev_err(to_device(vfe),	"Can not flush buffers\n");
+		return -1;
+	}
+	output = &line->output;
+
+	spin_lock_irqsave(&vfe->output_lock, flags);
+
+	vfe_buf_flush_pending(output);
+
+	if (output->buf[0])
+		vb2_buffer_done(&output->buf[0]->vb.vb2_buf,
+				VB2_BUF_STATE_ERROR);
+
+	if (output->buf[1])
+		vb2_buffer_done(&output->buf[1]->vb.vb2_buf,
+				VB2_BUF_STATE_ERROR);
+
+	if (output->last_buffer) {
+		vb2_buffer_done(&output->last_buffer->vb.vb2_buf,
+				VB2_BUF_STATE_ERROR);
+		output->last_buffer = NULL;
+	}
+
+	spin_unlock_irqrestore(&vfe->output_lock, flags);
+
+	return 0;
+}
+
+/*
+ * vfe_set_power - Power on/off VFE module
+ * @sd: VFE V4L2 subdevice
+ * @on: Requested power state
+ *
+ * Return 0 on success or a negative error code otherwise
+ */
+static int vfe_set_power(struct v4l2_subdev *sd, int on)
+{
+	struct vfe_line *line = v4l2_get_subdevdata(sd);
+	struct vfe_device *vfe = to_vfe(line);
+	int ret;
+
+	if (on) {
+		u32 hw_version;
+
+		ret = vfe_get(vfe);
+		if (ret < 0)
+			return ret;
+
+		hw_version = readl_relaxed(vfe->base + VFE_0_HW_VERSION);
+		dev_dbg(to_device(vfe),
+			"VFE HW Version = 0x%08x\n", hw_version);
+	} else {
+		vfe_put(vfe);
+	}
+
+	return 0;
+}
+
+/*
+ * vfe_set_stream - Enable/disable streaming on VFE module
+ * @sd: VFE V4L2 subdevice
+ * @enable: Requested streaming state
+ *
+ * Main configuration of VFE module is triggered here.
+ *
+ * Return 0 on success or a negative error code otherwise
+ */
+static int vfe_set_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct vfe_line *line = v4l2_get_subdevdata(sd);
+	struct vfe_device *vfe = to_vfe(line);
+	int ret;
+
+	if (enable) {
+		ret = vfe_enable(line);
+		if (ret < 0)
+			dev_err(to_device(vfe),
+				"Failed to enable vfe outputs\n");
+	} else {
+		ret = vfe_disable(line);
+		if (ret < 0)
+			dev_err(to_device(vfe),
+				"Failed to disable vfe outputs\n");
+	}
+
+	return ret;
+}
+
+/*
+ * __vfe_get_format - Get pointer to format structure
+ * @line: VFE line
+ * @cfg: V4L2 subdev pad configuration
+ * @pad: pad from which format is requested
+ * @which: TRY or ACTIVE format
+ *
+ * Return pointer to TRY or ACTIVE format structure
+ */
+static struct v4l2_mbus_framefmt *
+__vfe_get_format(struct vfe_line *line,
+		 struct v4l2_subdev_pad_config *cfg,
+		 unsigned int pad,
+		 enum v4l2_subdev_format_whence which)
+{
+	if (which == V4L2_SUBDEV_FORMAT_TRY)
+		return v4l2_subdev_get_try_format(&line->subdev, cfg, pad);
+
+	return &line->fmt[pad];
+}
+
+
+/*
+ * vfe_try_format - Handle try format by pad subdev method
+ * @line: VFE line
+ * @cfg: V4L2 subdev pad configuration
+ * @pad: pad on which format is requested
+ * @fmt: pointer to v4l2 format structure
+ * @which: wanted subdev format
+ */
+static void vfe_try_format(struct vfe_line *line,
+			   struct v4l2_subdev_pad_config *cfg,
+			   unsigned int pad,
+			   struct v4l2_mbus_framefmt *fmt,
+			   enum v4l2_subdev_format_whence which)
+{
+	unsigned int i;
+
+	switch (pad) {
+	case MSM_VFE_PAD_SINK:
+		/* Set format on sink pad */
+
+		for (i = 0; i < ARRAY_SIZE(vfe_formats); i++)
+			if (fmt->code == vfe_formats[i])
+				break;
+
+		/* If not found, use UYVY as default */
+		if (i >= ARRAY_SIZE(vfe_formats))
+			fmt->code = MEDIA_BUS_FMT_UYVY8_2X8;
+
+		fmt->width = clamp_t(u32, fmt->width, 1, 8191);
+		fmt->height = clamp_t(u32, fmt->height, 1, 8191);
+
+		if (fmt->field == V4L2_FIELD_ANY)
+			fmt->field = V4L2_FIELD_NONE;
+
+		break;
+
+	case MSM_VFE_PAD_SRC:
+		/* Set and return a format same as sink pad */
+
+		*fmt = *__vfe_get_format(line, cfg, MSM_VFE_PAD_SINK,
+					 which);
+
+		break;
+	}
+
+	fmt->colorspace = V4L2_COLORSPACE_SRGB;
+}
+
+/*
+ * vfe_enum_mbus_code - Handle pixel format enumeration
+ * @sd: VFE V4L2 subdevice
+ * @cfg: V4L2 subdev pad configuration
+ * @code: pointer to v4l2_subdev_mbus_code_enum structure
+ *
+ * return -EINVAL or zero on success
+ */
+static int vfe_enum_mbus_code(struct v4l2_subdev *sd,
+			      struct v4l2_subdev_pad_config *cfg,
+			      struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct vfe_line *line = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	if (code->pad == MSM_VFE_PAD_SINK) {
+		if (code->index >= ARRAY_SIZE(vfe_formats))
+			return -EINVAL;
+
+		code->code = vfe_formats[code->index];
+	} else {
+		if (code->index > 0)
+			return -EINVAL;
+
+		format = __vfe_get_format(line, cfg, MSM_VFE_PAD_SINK,
+					  code->which);
+
+		code->code = format->code;
+	}
+
+	return 0;
+}
+
+/*
+ * vfe_enum_frame_size - Handle frame size enumeration
+ * @sd: VFE V4L2 subdevice
+ * @cfg: V4L2 subdev pad configuration
+ * @fse: pointer to v4l2_subdev_frame_size_enum structure
+ *
+ * Return -EINVAL or zero on success
+ */
+static int vfe_enum_frame_size(struct v4l2_subdev *sd,
+			       struct v4l2_subdev_pad_config *cfg,
+			       struct v4l2_subdev_frame_size_enum *fse)
+{
+	struct vfe_line *line = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt format;
+
+	if (fse->index != 0)
+		return -EINVAL;
+
+	format.code = fse->code;
+	format.width = 1;
+	format.height = 1;
+	vfe_try_format(line, cfg, fse->pad, &format, fse->which);
+	fse->min_width = format.width;
+	fse->min_height = format.height;
+
+	if (format.code != fse->code)
+		return -EINVAL;
+
+	format.code = fse->code;
+	format.width = -1;
+	format.height = -1;
+	vfe_try_format(line, cfg, fse->pad, &format, fse->which);
+	fse->max_width = format.width;
+	fse->max_height = format.height;
+
+	return 0;
+}
+
+/*
+ * vfe_get_format - Handle get format by pads subdev method
+ * @sd: VFE V4L2 subdevice
+ * @cfg: V4L2 subdev pad configuration
+ * @fmt: pointer to v4l2 subdev format structure
+ *
+ * Return -EINVAL or zero on success
+ */
+static int vfe_get_format(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_pad_config *cfg,
+			  struct v4l2_subdev_format *fmt)
+{
+	struct vfe_line *line = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	format = __vfe_get_format(line, cfg, fmt->pad, fmt->which);
+	if (format == NULL)
+		return -EINVAL;
+
+	fmt->format = *format;
+
+	return 0;
+}
+
+/*
+ * vfe_set_format - Handle set format by pads subdev method
+ * @sd: VFE V4L2 subdevice
+ * @cfg: V4L2 subdev pad configuration
+ * @fmt: pointer to v4l2 subdev format structure
+ *
+ * Return -EINVAL or zero on success
+ */
+static int vfe_set_format(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_pad_config *cfg,
+			  struct v4l2_subdev_format *fmt)
+{
+	struct vfe_line *line = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	format = __vfe_get_format(line, cfg, fmt->pad, fmt->which);
+	if (format == NULL)
+		return -EINVAL;
+
+	vfe_try_format(line, cfg, fmt->pad, &fmt->format, fmt->which);
+	*format = fmt->format;
+
+	/* Propagate the format from sink to source */
+	if (fmt->pad == MSM_VFE_PAD_SINK) {
+		format = __vfe_get_format(line, cfg, MSM_VFE_PAD_SRC,
+					  fmt->which);
+
+		*format = fmt->format;
+		vfe_try_format(line, cfg, MSM_VFE_PAD_SRC, format,
+			       fmt->which);
+	}
+
+	return 0;
+}
+
+/*
+ * vfe_init_formats - Initialize formats on all pads
+ * @sd: VFE V4L2 subdevice
+ * @fh: V4L2 subdev file handle
+ *
+ * Initialize all pad formats with default values.
+ *
+ * Return 0 on success or a negative error code otherwise
+ */
+static int vfe_init_formats(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	struct v4l2_subdev_format format;
+
+	memset(&format, 0, sizeof(format));
+	format.pad = MSM_VFE_PAD_SINK;
+	format.which = fh ? V4L2_SUBDEV_FORMAT_TRY : V4L2_SUBDEV_FORMAT_ACTIVE;
+	format.format.code = MEDIA_BUS_FMT_UYVY8_2X8;
+	format.format.width = 1920;
+	format.format.height = 1080;
+
+	return vfe_set_format(sd, fh ? fh->pad : NULL, &format);
+}
+
+/*
+ * msm_vfe_subdev_init - Initialize VFE device structure and resources
+ * @vfe: VFE device
+ * @res: VFE module resources table
+ *
+ * Return 0 on success or a negative error code otherwise
+ */
+int msm_vfe_subdev_init(struct vfe_device *vfe, struct resources *res)
+{
+	struct device *dev = to_device(vfe);
+	struct platform_device *pdev = container_of(dev, struct platform_device,
+						    dev);
+	struct resource *r;
+	struct camss *camss = to_camss(vfe);
+
+	int i;
+	int ret;
+
+	mutex_init(&vfe->power_lock);
+	vfe->power_count = 0;
+
+	mutex_init(&vfe->stream_lock);
+	vfe->stream_count = 0;
+
+	spin_lock_init(&vfe->output_lock);
+
+	vfe->id = 0;
+	vfe->reg_update = 0;
+
+	for (i = VFE_LINE_RDI0; i <= VFE_LINE_RDI2; i++) {
+		vfe->line[i].video_out.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+		vfe->line[i].video_out.camss = camss;
+		vfe->line[i].id = i;
+	}
+
+	/* Memory */
+
+	r = platform_get_resource_byname(pdev, IORESOURCE_MEM, res->reg[0]);
+	vfe->base = devm_ioremap_resource(dev, r);
+	if (IS_ERR(vfe->base)) {
+		dev_err(dev, "could not map memory\n");
+		return PTR_ERR(vfe->base);
+	}
+
+	/* Interrupt */
+
+	r = platform_get_resource_byname(pdev, IORESOURCE_IRQ,
+					 res->interrupt[0]);
+	if (!r) {
+		dev_err(dev, "missing IRQ\n");
+		return -EINVAL;
+	}
+
+	vfe->irq = r->start;
+	snprintf(vfe->irq_name, sizeof(vfe->irq_name), "%s_%s%d",
+		 dev_name(dev), MSM_VFE_NAME, vfe->id);
+	ret = devm_request_irq(dev, vfe->irq, vfe_isr,
+			       IRQF_TRIGGER_RISING, vfe->irq_name, vfe);
+	if (ret < 0) {
+		dev_err(dev, "request_irq failed\n");
+		return ret;
+	}
+
+	/* Clocks */
+
+	vfe->nclocks = 0;
+	while (res->clock[vfe->nclocks])
+		vfe->nclocks++;
+
+	vfe->clock = devm_kzalloc(dev, vfe->nclocks * sizeof(*vfe->clock),
+				  GFP_KERNEL);
+	if (!vfe->clock)
+		return -ENOMEM;
+
+	for (i = 0; i < vfe->nclocks; i++) {
+		vfe->clock[i] = devm_clk_get(dev, res->clock[i]);
+		if (IS_ERR(vfe->clock[i]))
+			return PTR_ERR(vfe->clock[i]);
+
+		if (res->clock_rate[i]) {
+			long clk_rate = clk_round_rate(vfe->clock[i],
+						       res->clock_rate[i]);
+			if (clk_rate < 0) {
+				dev_err(dev, "clk round rate failed\n");
+				return -EINVAL;
+			}
+			ret = clk_set_rate(vfe->clock[i], clk_rate);
+			if (ret < 0) {
+				dev_err(dev, "clk set rate failed\n");
+				return ret;
+			}
+		}
+	}
+
+	init_completion(&vfe->reset_complete);
+	init_completion(&vfe->halt_complete);
+
+	return 0;
+}
+
+/*
+ * msm_vfe_get_vfe_id - Get VFE HW module id
+ * @entity: Pointer to VFE media entity structure
+ * @id: Return CSID HW module id here
+ */
+void msm_vfe_get_vfe_id(struct media_entity *entity, u8 *id)
+{
+	struct v4l2_subdev *sd;
+	struct vfe_line *line;
+	struct vfe_device *vfe;
+
+	sd = container_of(entity, struct v4l2_subdev, entity);
+	line = v4l2_get_subdevdata(sd);
+	vfe = to_vfe(line);
+
+	*id = vfe->id;
+}
+
+/*
+ * msm_vfe_get_vfe_line_id - Get VFE line id by media entity
+ * @entity: Pointer to VFE media entity structure
+ * @id: Return VFE line id here
+ */
+void msm_vfe_get_vfe_line_id(struct media_entity *entity, enum vfe_line_id *id)
+{
+	struct v4l2_subdev *sd;
+	struct vfe_line *line;
+
+	sd = container_of(entity, struct v4l2_subdev, entity);
+	line = v4l2_get_subdevdata(sd);
+
+	*id = line->id;
+}
+
+/*
+ * vfe_link_setup - Setup VFE connections
+ * @entity: Pointer to media entity structure
+ * @local: Pointer to local pad
+ * @remote: Pointer to remote pad
+ * @flags: Link flags
+ *
+ * Return 0 on success
+ */
+static int vfe_link_setup(struct media_entity *entity,
+			  const struct media_pad *local,
+			  const struct media_pad *remote, u32 flags)
+{
+	if (flags & MEDIA_LNK_FL_ENABLED)
+		if (media_entity_remote_pad((struct media_pad *)local))
+			return -EBUSY;
+
+	return 0;
+}
+
+static const struct v4l2_subdev_core_ops vfe_core_ops = {
+	.s_power = vfe_set_power,
+};
+
+static const struct v4l2_subdev_video_ops vfe_video_ops = {
+	.s_stream = vfe_set_stream,
+};
+
+static const struct v4l2_subdev_pad_ops vfe_pad_ops = {
+	.enum_mbus_code = vfe_enum_mbus_code,
+	.enum_frame_size = vfe_enum_frame_size,
+	.get_fmt = vfe_get_format,
+	.set_fmt = vfe_set_format,
+};
+
+static const struct v4l2_subdev_ops vfe_v4l2_ops = {
+	.core = &vfe_core_ops,
+	.video = &vfe_video_ops,
+	.pad = &vfe_pad_ops,
+};
+
+static const struct v4l2_subdev_internal_ops vfe_v4l2_internal_ops = {
+	.open = vfe_init_formats,
+};
+
+static const struct media_entity_operations vfe_media_ops = {
+	.link_setup = vfe_link_setup,
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+static struct camss_video_ops camss_vfe_video_ops = {
+	.queue_buffer = vfe_queue_buffer,
+	.flush_buffers = vfe_flush_buffers,
+};
+
+/*
+ * msm_vfe_register_entities - Register subdev node for VFE module
+ * @vfe: VFE device
+ * @v4l2_dev: V4L2 device
+ *
+ * Return 0 on success or a negative error code otherwise
+ */
+int msm_vfe_register_entities(struct vfe_device *vfe,
+			      struct v4l2_device *v4l2_dev)
+{
+	struct device *dev = to_device(vfe);
+	struct v4l2_subdev *sd;
+	struct media_pad *pads;
+	struct camss_video *video_out;
+	int ret;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(vfe->line); i++) {
+		char name[32];
+
+		sd = &vfe->line[i].subdev;
+		pads = vfe->line[i].pads;
+		video_out = &vfe->line[i].video_out;
+
+		v4l2_subdev_init(sd, &vfe_v4l2_ops);
+		sd->internal_ops = &vfe_v4l2_internal_ops;
+		sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+		snprintf(sd->name, ARRAY_SIZE(sd->name), "%s%d_%s%d",
+			 MSM_VFE_NAME, vfe->id, "rdi", i);
+		v4l2_set_subdevdata(sd, &vfe->line[i]);
+
+		ret = vfe_init_formats(sd, NULL);
+		if (ret < 0) {
+			dev_err(dev, "Failed to init format\n");
+			goto error_init;
+		}
+
+		pads[MSM_VFE_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
+		pads[MSM_VFE_PAD_SRC].flags = MEDIA_PAD_FL_SOURCE;
+
+		sd->entity.ops = &vfe_media_ops;
+		ret = media_entity_pads_init(&sd->entity, MSM_VFE_PADS_NUM,
+					     pads);
+		if (ret < 0) {
+			dev_err(dev, "Failed to init media entity\n");
+			goto error_init;
+		}
+
+		ret = v4l2_device_register_subdev(v4l2_dev, sd);
+		if (ret < 0) {
+			dev_err(dev, "Failed to register subdev\n");
+			goto error_reg_subdev;
+		}
+
+		video_out->ops = &camss_vfe_video_ops;
+		snprintf(name, ARRAY_SIZE(name), "%s%d_%s%d",
+			 MSM_VFE_NAME, vfe->id, "video", i);
+		ret = msm_video_register(video_out, v4l2_dev, name);
+		if (ret < 0) {
+			dev_err(dev, "Failed to register video node\n");
+			goto error_reg_video;
+		}
+
+		ret = media_create_pad_link(
+				&sd->entity, MSM_VFE_PAD_SRC,
+				&video_out->vdev->entity, 0,
+				MEDIA_LNK_FL_IMMUTABLE | MEDIA_LNK_FL_ENABLED);
+		if (ret < 0) {
+			dev_err(dev, "Failed to link %s->%s entities\n",
+			       sd->entity.name, video_out->vdev->entity.name);
+			goto error_link;
+		}
+	}
+
+	return 0;
+
+error_link:
+	msm_video_unregister(video_out);
+
+error_reg_video:
+	v4l2_device_unregister_subdev(sd);
+
+error_reg_subdev:
+	media_entity_cleanup(&sd->entity);
+
+error_init:
+	for (i--; i >= 0; i--) {
+		sd = &vfe->line[i].subdev;
+		video_out = &vfe->line[i].video_out;
+
+		msm_video_unregister(video_out);
+		v4l2_device_unregister_subdev(sd);
+		media_entity_cleanup(&sd->entity);
+	}
+
+	return ret;
+}
+
+/*
+ * msm_vfe_unregister_entities - Unregister VFE module subdev node
+ * @vfe: VFE device
+ */
+void msm_vfe_unregister_entities(struct vfe_device *vfe)
+{
+	int i;
+
+	mutex_destroy(&vfe->power_lock);
+	mutex_destroy(&vfe->stream_lock);
+
+	for (i = 0; i < ARRAY_SIZE(vfe->line); i++) {
+		struct v4l2_subdev *sd = &vfe->line[i].subdev;
+		struct camss_video *video_out = &vfe->line[i].video_out;
+
+		msm_video_unregister(video_out);
+		v4l2_device_unregister_subdev(sd);
+		media_entity_cleanup(&sd->entity);
+	}
+}
diff --git a/drivers/media/platform/qcom/camss-8x16/vfe.h b/drivers/media/platform/qcom/camss-8x16/vfe.h
new file mode 100644
index 0000000..96c6494
--- /dev/null
+++ b/drivers/media/platform/qcom/camss-8x16/vfe.h
@@ -0,0 +1,112 @@
+/*
+ * vfe.h
+ *
+ * Qualcomm MSM Camera Subsystem - VFE Module
+ *
+ * Copyright (c) 2013-2015, The Linux Foundation. All rights reserved.
+ * Copyright (C) 2015-2016 Linaro Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only 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.
+ */
+#ifndef QC_MSM_CAMSS_VFE_H
+#define QC_MSM_CAMSS_VFE_H
+
+#include <linux/clk.h>
+#include <linux/spinlock_types.h>
+#include <media/media-entity.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-subdev.h>
+
+#include "video.h"
+
+#define MSM_VFE_PAD_SINK 0
+#define MSM_VFE_PAD_SRC 1
+#define MSM_VFE_PADS_NUM 2
+
+#define MSM_VFE_LINE_NUM 3
+#define MSM_VFE_IMAGE_MASTERS_NUM 7
+
+#define MSM_VFE_VFE0_UB_SIZE 1023
+#define MSM_VFE_VFE0_UB_SIZE_RDI (MSM_VFE_VFE0_UB_SIZE / 3)
+#define MSM_VFE_VFE1_UB_SIZE 1535
+#define MSM_VFE_VFE1_UB_SIZE_RDI (MSM_VFE_VFE1_UB_SIZE / 3)
+
+enum vfe_output_state {
+	VFE_OUTPUT_OFF,
+	VFE_OUTPUT_RESERVED,
+	VFE_OUTPUT_SINGLE,
+	VFE_OUTPUT_CONTINUOUS,
+	VFE_OUTPUT_IDLE,
+	VFE_OUTPUT_STOPPING
+};
+
+enum vfe_line_id {
+	VFE_LINE_NONE = -1,
+	VFE_LINE_RDI0 = 0,
+	VFE_LINE_RDI1 = 1,
+	VFE_LINE_RDI2 = 2
+};
+
+struct vfe_output {
+	u8 wm_idx;
+
+	int active_buf;
+	struct camss_buffer *buf[2];
+	struct camss_buffer *last_buffer;
+	struct list_head pending_bufs;
+
+	unsigned int drop_update_idx;
+
+	enum vfe_output_state state;
+	unsigned int sequence;
+};
+
+struct vfe_line {
+	enum vfe_line_id id;
+	struct v4l2_subdev subdev;
+	struct media_pad pads[MSM_VFE_PADS_NUM];
+	struct v4l2_mbus_framefmt fmt[MSM_VFE_PADS_NUM];
+	struct camss_video video_out;
+	struct vfe_output output;
+};
+
+struct vfe_device {
+	u8 id;
+	void __iomem *base;
+	u32 irq;
+	char irq_name[30];
+	struct clk **clock;
+	int nclocks;
+	struct completion reset_complete;
+	struct completion halt_complete;
+	struct mutex power_lock;
+	int power_count;
+	struct mutex stream_lock;
+	int stream_count;
+	spinlock_t output_lock;
+	enum vfe_line_id wm_output_map[MSM_VFE_IMAGE_MASTERS_NUM];
+	struct vfe_line line[MSM_VFE_LINE_NUM];
+	u32 reg_update;
+	u8 was_streaming;
+};
+
+struct resources;
+
+int msm_vfe_subdev_init(struct vfe_device *vfe, struct resources *res);
+
+int msm_vfe_register_entities(struct vfe_device *vfe,
+			      struct v4l2_device *v4l2_dev);
+
+void msm_vfe_unregister_entities(struct vfe_device *vfe);
+
+void msm_vfe_get_vfe_id(struct media_entity *entity, u8 *id);
+void msm_vfe_get_vfe_line_id(struct media_entity *entity, enum vfe_line_id *id);
+
+#endif /* QC_MSM_CAMSS_VFE_H */
-- 
1.9.1

[PATCH 08/10] media: camss: Add files which handle the video device nodes

[Todor Tomov]

These files handle the video device nodes of the camss driver.

Signed-off-by: Todor Tomov <todor.tomov@linaro.org>
---
 drivers/media/platform/qcom/camss-8x16/video.c | 597 +++++++++++++++++++++++++
 drivers/media/platform/qcom/camss-8x16/video.h |  67 +++
 2 files changed, 664 insertions(+)
 create mode 100644 drivers/media/platform/qcom/camss-8x16/video.c
 create mode 100644 drivers/media/platform/qcom/camss-8x16/video.h

diff --git a/drivers/media/platform/qcom/camss-8x16/video.c b/drivers/media/platform/qcom/camss-8x16/video.c
new file mode 100644
index 0000000..0bf8ea9
--- /dev/null
+++ b/drivers/media/platform/qcom/camss-8x16/video.c
@@ -0,0 +1,597 @@
+/*
+ * video.c
+ *
+ * Qualcomm MSM Camera Subsystem - V4L2 device node
+ *
+ * Copyright (c) 2013-2015, The Linux Foundation. All rights reserved.
+ * Copyright (C) 2015-2016 Linaro Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only 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.
+ */
+#include <media/media-entity.h>
+#include <media/v4l2-dev.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-mc.h>
+#include <media/videobuf-core.h>
+#include <media/videobuf2-dma-contig.h>
+
+#include "video.h"
+#include "camss.h"
+
+/*
+ * struct format_info - ISP media bus format information
+ * @code: V4L2 media bus format code
+ * @pixelformat: V4L2 pixel format FCC identifier
+ * @bpp: Bits per pixel when stored in memory
+ */
+static const struct format_info {
+	u32 code;
+	u32 pixelformat;
+	unsigned int bpp;
+} formats[] = {
+	{ MEDIA_BUS_FMT_UYVY8_2X8, V4L2_PIX_FMT_UYVY, 16 },
+	{ MEDIA_BUS_FMT_VYUY8_2X8, V4L2_PIX_FMT_VYUY, 16 },
+	{ MEDIA_BUS_FMT_YUYV8_2X8, V4L2_PIX_FMT_YUYV, 16 },
+	{ MEDIA_BUS_FMT_YVYU8_2X8, V4L2_PIX_FMT_YVYU, 16 },
+	{ MEDIA_BUS_FMT_SBGGR8_1X8, V4L2_PIX_FMT_SBGGR8, 8 },
+	{ MEDIA_BUS_FMT_SGBRG8_1X8, V4L2_PIX_FMT_SGBRG8, 8 },
+	{ MEDIA_BUS_FMT_SGRBG8_1X8, V4L2_PIX_FMT_SGRBG8, 8 },
+	{ MEDIA_BUS_FMT_SRGGB8_1X8, V4L2_PIX_FMT_SRGGB8, 8 },
+	{ MEDIA_BUS_FMT_SBGGR10_1X10, V4L2_PIX_FMT_SBGGR10P, 10 },
+	{ MEDIA_BUS_FMT_SGBRG10_1X10, V4L2_PIX_FMT_SGBRG10P, 10 },
+	{ MEDIA_BUS_FMT_SGRBG10_1X10, V4L2_PIX_FMT_SGRBG10P, 10 },
+	{ MEDIA_BUS_FMT_SRGGB10_1X10, V4L2_PIX_FMT_SRGGB10P, 10 },
+	{ MEDIA_BUS_FMT_SBGGR12_1X12, V4L2_PIX_FMT_SRGGB12P, 12 },
+	{ MEDIA_BUS_FMT_SGBRG12_1X12, V4L2_PIX_FMT_SGBRG12P, 12 },
+	{ MEDIA_BUS_FMT_SGRBG12_1X12, V4L2_PIX_FMT_SGRBG12P, 12 },
+	{ MEDIA_BUS_FMT_SRGGB12_1X12, V4L2_PIX_FMT_SRGGB12P, 12 }
+};
+
+/* -----------------------------------------------------------------------------
+ * Helper functions
+ */
+
+/*
+ * video_mbus_to_pix - Convert v4l2_mbus_framefmt to v4l2_pix_format
+ * @mbus: v4l2_mbus_framefmt format (input)
+ * @pix: v4l2_pix_format format (output)
+ *
+ * Fill the output pix structure with information from the input mbus format.
+ *
+ * Return 0 on success or a negative error code otherwise
+ */
+static unsigned int video_mbus_to_pix(const struct v4l2_mbus_framefmt *mbus,
+				      struct v4l2_pix_format *pix)
+{
+	unsigned int i;
+
+	memset(pix, 0, sizeof(*pix));
+	pix->width = mbus->width;
+	pix->height = mbus->height;
+
+	for (i = 0; i < ARRAY_SIZE(formats); ++i) {
+		if (formats[i].code == mbus->code)
+			break;
+	}
+
+	if (WARN_ON(i == ARRAY_SIZE(formats)))
+		return -EINVAL;
+
+	pix->pixelformat = formats[i].pixelformat;
+	pix->bytesperline = pix->width * formats[i].bpp / 8;
+	pix->bytesperline = ALIGN(pix->bytesperline, 8);
+	pix->sizeimage = pix->bytesperline * pix->height;
+	pix->colorspace = mbus->colorspace;
+	pix->field = mbus->field;
+
+	return 0;
+}
+
+static struct v4l2_subdev *video_remote_subdev(struct camss_video *video,
+					       u32 *pad)
+{
+	struct media_pad *remote;
+
+	remote = media_entity_remote_pad(&video->pad);
+
+	if (!remote || !is_media_entity_v4l2_subdev(remote->entity))
+		return NULL;
+
+	if (pad)
+		*pad = remote->index;
+
+	return media_entity_to_v4l2_subdev(remote->entity);
+}
+
+static int video_get_subdev_format(struct camss_video *video,
+				   struct v4l2_format *format)
+{
+	struct v4l2_subdev_format fmt;
+	struct v4l2_subdev *subdev;
+	u32 pad;
+	int ret;
+
+	subdev = video_remote_subdev(video, &pad);
+	if (subdev == NULL)
+		return -EINVAL;
+
+	fmt.pad = pad;
+	fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+
+	ret = v4l2_subdev_call(subdev, pad, get_fmt, NULL, &fmt);
+	if (ret)
+		return ret;
+
+	format->type = video->type;
+	return video_mbus_to_pix(&fmt.format, &format->fmt.pix);
+}
+
+/* -----------------------------------------------------------------------------
+ * Video queue operations
+ */
+
+static int video_queue_setup(struct vb2_queue *q,
+	unsigned int *num_buffers, unsigned int *num_planes,
+	unsigned int sizes[], struct device *alloc_devs[])
+{
+	struct camss_video *video = vb2_get_drv_priv(q);
+
+	if (*num_planes) {
+		if (*num_planes != 1)
+			return -EINVAL;
+
+		if (sizes[0] < video->active_fmt.fmt.pix.sizeimage)
+			return -EINVAL;
+
+		return 0;
+	}
+
+	*num_planes = 1;
+
+	sizes[0] = video->active_fmt.fmt.pix.sizeimage;
+
+	return 0;
+}
+
+static int video_buf_prepare(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct camss_video *video = vb2_get_drv_priv(vb->vb2_queue);
+	struct camss_buffer *buffer = container_of(vbuf, struct camss_buffer,
+						   vb);
+
+	vb2_set_plane_payload(vb, 0, video->active_fmt.fmt.pix.sizeimage);
+	if (vb2_get_plane_payload(vb, 0) > vb2_plane_size(vb, 0))
+		return -EINVAL;
+
+	vbuf->field = V4L2_FIELD_NONE;
+
+	buffer->addr = vb2_dma_contig_plane_dma_addr(vb, 0);
+
+	return 0;
+}
+
+static void video_buf_queue(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct camss_video *video = vb2_get_drv_priv(vb->vb2_queue);
+	struct camss_buffer *buffer = container_of(vbuf, struct camss_buffer,
+						   vb);
+
+	camss_video_call(video, queue_buffer, buffer);
+}
+
+
+static int video_check_format(struct camss_video *video)
+{
+	struct v4l2_pix_format *pix = &video->active_fmt.fmt.pix;
+	struct v4l2_format format;
+	int ret;
+
+	ret = video_get_subdev_format(video, &format);
+	if (ret < 0)
+		return ret;
+
+	if (pix->pixelformat != format.fmt.pix.pixelformat ||
+	    pix->height != format.fmt.pix.height ||
+	    pix->width != format.fmt.pix.width ||
+	    pix->bytesperline != format.fmt.pix.bytesperline ||
+	    pix->sizeimage != format.fmt.pix.sizeimage ||
+	    pix->field != format.fmt.pix.field)
+		return -EINVAL;
+
+	return 0;
+}
+
+static int video_start_streaming(struct vb2_queue *q, unsigned int count)
+{
+	struct camss_video *video = vb2_get_drv_priv(q);
+	struct video_device *vdev = video->vdev;
+	struct media_entity *entity;
+	struct media_pad *pad;
+	struct v4l2_subdev *subdev;
+	int ret;
+
+	ret = media_entity_pipeline_start(&vdev->entity, &video->pipe);
+	if (ret < 0)
+		return ret;
+
+	ret = video_check_format(video);
+	if (ret < 0)
+		goto error;
+
+	entity = &vdev->entity;
+	while (1) {
+		pad = &entity->pads[0];
+		if (!(pad->flags & MEDIA_PAD_FL_SINK))
+			break;
+
+		pad = media_entity_remote_pad(pad);
+		if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
+			break;
+
+		entity = pad->entity;
+		subdev = media_entity_to_v4l2_subdev(entity);
+
+		ret = v4l2_subdev_call(subdev, video, s_stream, 1);
+		if (ret < 0 && ret != -ENOIOCTLCMD)
+			goto error;
+	}
+
+	return 0;
+
+error:
+	media_entity_pipeline_stop(&vdev->entity);
+
+	return ret;
+}
+
+static void video_stop_streaming(struct vb2_queue *q)
+{
+	struct camss_video *video = vb2_get_drv_priv(q);
+	struct video_device *vdev = video->vdev;
+	struct media_entity *entity;
+	struct media_pad *pad;
+	struct v4l2_subdev *subdev;
+	struct v4l2_subdev *subdev_vfe;
+
+	entity = &vdev->entity;
+	while (1) {
+		pad = &entity->pads[0];
+		if (!(pad->flags & MEDIA_PAD_FL_SINK))
+			break;
+
+		pad = media_entity_remote_pad(pad);
+		if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
+			break;
+
+		entity = pad->entity;
+		subdev = media_entity_to_v4l2_subdev(entity);
+
+		if (strstr(subdev->name, "vfe")) {
+			subdev_vfe = subdev;
+		} else if (strstr(subdev->name, "ispif")) {
+			v4l2_subdev_call(subdev, video, s_stream, 0);
+			v4l2_subdev_call(subdev_vfe, video, s_stream, 0);
+		} else {
+			v4l2_subdev_call(subdev, video, s_stream, 0);
+		}
+	}
+
+	media_entity_pipeline_stop(&vdev->entity);
+
+	camss_video_call(video, flush_buffers);
+}
+
+static struct vb2_ops msm_video_vb2_q_ops = {
+	.queue_setup     = video_queue_setup,
+	.buf_prepare     = video_buf_prepare,
+	.buf_queue       = video_buf_queue,
+	.start_streaming = video_start_streaming,
+	.stop_streaming  = video_stop_streaming,
+};
+
+/* -----------------------------------------------------------------------------
+ * V4L2 ioctls
+ */
+
+static int video_querycap(struct file *file, void *fh,
+			  struct v4l2_capability *cap)
+{
+	strlcpy(cap->driver, "qcom-camss", sizeof(cap->driver));
+	strlcpy(cap->card, "Qualcomm Camera Subsystem", sizeof(cap->card));
+	strlcpy(cap->bus_info, "platform:qcom-camss", sizeof(cap->bus_info));
+	cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING |
+							V4L2_CAP_DEVICE_CAPS;
+	cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
+
+	return 0;
+}
+
+static int video_enum_fmt(struct file *file, void *fh, struct v4l2_fmtdesc *f)
+{
+	struct camss_video *video = video_drvdata(file);
+	struct v4l2_format format;
+	int ret;
+
+	if (f->type != video->type)
+		return -EINVAL;
+
+	if (f->index)
+		return -EINVAL;
+
+	ret = video_get_subdev_format(video, &format);
+	if (ret < 0)
+		return ret;
+
+	f->pixelformat = format.fmt.pix.pixelformat;
+
+	return 0;
+}
+
+static int video_g_fmt(struct file *file, void *fh, struct v4l2_format *f)
+{
+	struct camss_video *video = video_drvdata(file);
+
+	if (f->type != video->type)
+		return -EINVAL;
+
+	*f = video->active_fmt;
+
+	return 0;
+}
+
+static int video_s_fmt(struct file *file, void *fh, struct v4l2_format *f)
+{
+	struct camss_video *video = video_drvdata(file);
+	int ret;
+
+	if (f->type != video->type)
+		return -EINVAL;
+
+	ret = video_get_subdev_format(video, f);
+	if (ret < 0)
+		return ret;
+
+	video->active_fmt = *f;
+
+	return 0;
+}
+
+static int video_try_fmt(struct file *file, void *fh, struct v4l2_format *f)
+{
+	struct camss_video *video = video_drvdata(file);
+
+	if (f->type != video->type)
+		return -EINVAL;
+
+	return video_get_subdev_format(video, f);
+}
+
+static int video_enum_input(struct file *file, void *fh,
+			    struct v4l2_input *input)
+{
+	if (input->index > 0)
+		return -EINVAL;
+
+	strlcpy(input->name, "camera", sizeof(input->name));
+	input->type = V4L2_INPUT_TYPE_CAMERA;
+
+	return 0;
+}
+
+static int video_g_input(struct file *file, void *fh, unsigned int *input)
+{
+	*input = 0;
+
+	return 0;
+}
+
+static int video_s_input(struct file *file, void *fh, unsigned int input)
+{
+	return input == 0 ? 0 : -EINVAL;
+}
+
+static const struct v4l2_ioctl_ops msm_vid_ioctl_ops = {
+	.vidioc_querycap          = video_querycap,
+	.vidioc_enum_fmt_vid_cap  = video_enum_fmt,
+	.vidioc_g_fmt_vid_cap     = video_g_fmt,
+	.vidioc_s_fmt_vid_cap     = video_s_fmt,
+	.vidioc_try_fmt_vid_cap   = video_try_fmt,
+	.vidioc_reqbufs           = vb2_ioctl_reqbufs,
+	.vidioc_querybuf          = vb2_ioctl_querybuf,
+	.vidioc_qbuf              = vb2_ioctl_qbuf,
+	.vidioc_dqbuf             = vb2_ioctl_dqbuf,
+	.vidioc_create_bufs       = vb2_ioctl_create_bufs,
+	.vidioc_streamon          = vb2_ioctl_streamon,
+	.vidioc_streamoff         = vb2_ioctl_streamoff,
+	.vidioc_enum_input        = video_enum_input,
+	.vidioc_g_input           = video_g_input,
+	.vidioc_s_input           = video_s_input,
+};
+
+/* -----------------------------------------------------------------------------
+ * V4L2 file operations
+ */
+
+/*
+ * video_init_format - Helper function to initialize format
+ *
+ * Initialize all pad formats with default values.
+ */
+static int video_init_format(struct file *file, void *fh)
+{
+	struct v4l2_format format;
+
+	memset(&format, 0, sizeof(format));
+	format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+	return video_s_fmt(file, fh, &format);
+}
+
+static int video_open(struct file *file)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct camss_video *video = video_drvdata(file);
+	struct camss_video_fh *handle;
+	int ret;
+
+	handle = kzalloc(sizeof(*handle), GFP_KERNEL);
+	if (handle == NULL)
+		return -ENOMEM;
+
+	v4l2_fh_init(&handle->vfh, video->vdev);
+	v4l2_fh_add(&handle->vfh);
+
+	handle->video = video;
+	file->private_data = &handle->vfh;
+
+	ret = v4l2_pipeline_pm_use(&vdev->entity, 1);
+	if (ret < 0) {
+		dev_err(video->camss->dev, "Failed to power up pipeline\n");
+		goto error_pm_use;
+	}
+
+	ret = video_init_format(file, &handle->vfh);
+	if (ret < 0) {
+		dev_err(video->camss->dev, "Failed to init format\n");
+		goto error_init_format;
+	}
+
+	return 0;
+
+error_init_format:
+	v4l2_pipeline_pm_use(&vdev->entity, 0);
+
+error_pm_use:
+	v4l2_fh_del(&handle->vfh);
+	kfree(handle);
+
+	return ret;
+}
+
+static int video_release(struct file *file)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct camss_video *video = video_drvdata(file);
+	struct v4l2_fh *vfh = file->private_data;
+	struct camss_video_fh *handle = container_of(vfh, struct camss_video_fh,
+						     vfh);
+
+	vb2_ioctl_streamoff(file, vfh, video->type);
+
+	v4l2_pipeline_pm_use(&vdev->entity, 0);
+
+	v4l2_fh_del(vfh);
+	kfree(handle);
+	file->private_data = NULL;
+
+	return 0;
+}
+
+static unsigned int video_poll(struct file *file,
+				   struct poll_table_struct *wait)
+{
+	struct camss_video *video = video_drvdata(file);
+
+	return vb2_poll(&video->vb2_q, file, wait);
+}
+
+static int video_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	struct camss_video *video = video_drvdata(file);
+
+	return vb2_mmap(&video->vb2_q, vma);
+}
+
+static const struct v4l2_file_operations msm_vid_fops = {
+	.owner          = THIS_MODULE,
+	.unlocked_ioctl = video_ioctl2,
+	.open           = video_open,
+	.release        = video_release,
+	.poll           = video_poll,
+	.mmap		= video_mmap,
+};
+
+/* -----------------------------------------------------------------------------
+ * CAMSS video core
+ */
+
+int msm_video_register(struct camss_video *video, struct v4l2_device *v4l2_dev,
+		       const char *name)
+{
+	struct media_pad *pad = &video->pad;
+	struct video_device *vdev;
+	struct vb2_queue *q;
+	int ret;
+
+	vdev = video_device_alloc();
+	if (vdev == NULL) {
+		dev_err(v4l2_dev->dev, "Failed to allocate video device\n");
+		return -ENOMEM;
+	}
+
+	video->vdev = vdev;
+
+	q = &video->vb2_q;
+	q->drv_priv = video;
+	q->mem_ops = &vb2_dma_contig_memops;
+	q->ops = &msm_video_vb2_q_ops;
+	q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	q->io_modes = VB2_MMAP;
+	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	q->buf_struct_size = sizeof(struct camss_buffer);
+	q->dev = video->camss->dev;
+	ret = vb2_queue_init(q);
+	if (ret < 0) {
+		dev_err(v4l2_dev->dev, "Failed to init vb2 queue\n");
+		return ret;
+	}
+
+	pad->flags = MEDIA_PAD_FL_SINK;
+	ret = media_entity_pads_init(&vdev->entity, 1, pad);
+	if (ret < 0) {
+		dev_err(v4l2_dev->dev, "Failed to init video entity\n");
+		goto error_media_init;
+	}
+
+	vdev->fops = &msm_vid_fops;
+	vdev->ioctl_ops = &msm_vid_ioctl_ops;
+	vdev->release = video_device_release;
+	vdev->v4l2_dev = v4l2_dev;
+	vdev->vfl_dir = VFL_DIR_RX;
+	vdev->queue = &video->vb2_q;
+	strlcpy(vdev->name, name, sizeof(vdev->name));
+
+	ret = video_register_device(vdev, VFL_TYPE_GRABBER, -1);
+	if (ret < 0) {
+		dev_err(v4l2_dev->dev, "Failed to register video device\n");
+		goto error_video_register;
+	}
+
+	video_set_drvdata(vdev, video);
+
+	return 0;
+
+error_video_register:
+	media_entity_cleanup(&vdev->entity);
+error_media_init:
+	vb2_queue_release(&video->vb2_q);
+
+	return ret;
+}
+
+void msm_video_unregister(struct camss_video *video)
+{
+	video_unregister_device(video->vdev);
+	media_entity_cleanup(&video->vdev->entity);
+	vb2_queue_release(&video->vb2_q);
+}
diff --git a/drivers/media/platform/qcom/camss-8x16/video.h b/drivers/media/platform/qcom/camss-8x16/video.h
new file mode 100644
index 0000000..5ab5929d
--- /dev/null
+++ b/drivers/media/platform/qcom/camss-8x16/video.h
@@ -0,0 +1,67 @@
+/*
+ * video.h
+ *
+ * Qualcomm MSM Camera Subsystem - V4L2 device node
+ *
+ * Copyright (c) 2013-2015, The Linux Foundation. All rights reserved.
+ * Copyright (C) 2015-2016 Linaro Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only 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.
+ */
+#ifndef QC_MSM_CAMSS_VIDEO_H
+#define QC_MSM_CAMSS_VIDEO_H
+
+#include <linux/videodev2.h>
+#include <media/media-entity.h>
+#include <media/v4l2-dev.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-fh.h>
+#include <media/v4l2-mediabus.h>
+#include <media/videobuf2-v4l2.h>
+
+#define camss_video_call(f, op, args...)			\
+	(!(f) ? -ENODEV : (((f)->ops && (f)->ops->op) ? \
+			    (f)->ops->op((f), ##args) : -ENOIOCTLCMD))
+
+struct camss_buffer {
+	struct vb2_v4l2_buffer vb;
+	dma_addr_t addr;
+	struct list_head queue;
+};
+
+struct camss_video;
+
+struct camss_video_ops {
+	int (*queue_buffer)(struct camss_video *vid, struct camss_buffer *buf);
+	int (*flush_buffers)(struct camss_video *vid);
+};
+
+struct camss_video {
+	struct camss *camss;
+	struct vb2_queue vb2_q;
+	struct video_device *vdev;
+	struct media_pad pad;
+	struct v4l2_format active_fmt;
+	enum v4l2_buf_type type;
+	struct media_pipeline pipe;
+	struct camss_video_ops *ops;
+};
+
+struct camss_video_fh {
+	struct v4l2_fh vfh;
+	struct camss_video *video;
+};
+
+int msm_video_register(struct camss_video *video, struct v4l2_device *v4l2_dev,
+		       const char *name);
+
+void msm_video_unregister(struct camss_video *video);
+
+#endif /* QC_MSM_CAMSS_VIDEO_H */
-- 
1.9.1

Re: [PATCH 08/10] media: camss: Add files which handle the video device nodes

[Hans Verkuil]

A few comments below:

On 11/25/2016 03:57 PM, Todor Tomov wrote:
> These files handle the video device nodes of the camss driver.
> 
> Signed-off-by: Todor Tomov <todor.tomov@linaro.org>
> ---
>  drivers/media/platform/qcom/camss-8x16/video.c | 597 +++++++++++++++++++++++++
>  drivers/media/platform/qcom/camss-8x16/video.h |  67 +++
>  2 files changed, 664 insertions(+)
>  create mode 100644 drivers/media/platform/qcom/camss-8x16/video.c
>  create mode 100644 drivers/media/platform/qcom/camss-8x16/video.h
> 
> diff --git a/drivers/media/platform/qcom/camss-8x16/video.c b/drivers/media/platform/qcom/camss-8x16/video.c
> new file mode 100644
> index 0000000..0bf8ea9
> --- /dev/null
> +++ b/drivers/media/platform/qcom/camss-8x16/video.c
> @@ -0,0 +1,597 @@

<snip>

> +static int video_start_streaming(struct vb2_queue *q, unsigned int count)
> +{
> +	struct camss_video *video = vb2_get_drv_priv(q);
> +	struct video_device *vdev = video->vdev;
> +	struct media_entity *entity;
> +	struct media_pad *pad;
> +	struct v4l2_subdev *subdev;
> +	int ret;
> +
> +	ret = media_entity_pipeline_start(&vdev->entity, &video->pipe);
> +	if (ret < 0)
> +		return ret;
> +
> +	ret = video_check_format(video);
> +	if (ret < 0)
> +		goto error;
> +
> +	entity = &vdev->entity;
> +	while (1) {
> +		pad = &entity->pads[0];
> +		if (!(pad->flags & MEDIA_PAD_FL_SINK))
> +			break;
> +
> +		pad = media_entity_remote_pad(pad);
> +		if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
> +			break;
> +
> +		entity = pad->entity;
> +		subdev = media_entity_to_v4l2_subdev(entity);
> +
> +		ret = v4l2_subdev_call(subdev, video, s_stream, 1);
> +		if (ret < 0 && ret != -ENOIOCTLCMD)
> +			goto error;
> +	}
> +
> +	return 0;
> +
> +error:
> +	media_entity_pipeline_stop(&vdev->entity);
> +

On error all queued buffers must be returned with state VB2_STATE_QUEUED.

Basically the same as 'camss_video_call(video, flush_buffers);', just with
a different state.

> +	return ret;
> +}

<snip>

> +static int video_querycap(struct file *file, void *fh,
> +			  struct v4l2_capability *cap)
> +{
> +	strlcpy(cap->driver, "qcom-camss", sizeof(cap->driver));
> +	strlcpy(cap->card, "Qualcomm Camera Subsystem", sizeof(cap->card));
> +	strlcpy(cap->bus_info, "platform:qcom-camss", sizeof(cap->bus_info));
> +	cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING |
> +							V4L2_CAP_DEVICE_CAPS;
> +	cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;

Don't set capabilities and device_caps here. Instead fill in the struct video_device
device_caps field and the v4l2 core will take care of cap->capabilities and
cap->device_caps.

> +
> +	return 0;
> +}
> +
> +static int video_enum_fmt(struct file *file, void *fh, struct v4l2_fmtdesc *f)
> +{
> +	struct camss_video *video = video_drvdata(file);
> +	struct v4l2_format format;
> +	int ret;
> +
> +	if (f->type != video->type)
> +		return -EINVAL;
> +
> +	if (f->index)
> +		return -EINVAL;
> +
> +	ret = video_get_subdev_format(video, &format);
> +	if (ret < 0)
> +		return ret;
> +
> +	f->pixelformat = format.fmt.pix.pixelformat;
> +
> +	return 0;
> +}
> +
> +static int video_g_fmt(struct file *file, void *fh, struct v4l2_format *f)
> +{
> +	struct camss_video *video = video_drvdata(file);
> +
> +	if (f->type != video->type)
> +		return -EINVAL;
> +
> +	*f = video->active_fmt;
> +
> +	return 0;
> +}
> +
> +static int video_s_fmt(struct file *file, void *fh, struct v4l2_format *f)
> +{
> +	struct camss_video *video = video_drvdata(file);
> +	int ret;
> +
> +	if (f->type != video->type)
> +		return -EINVAL;
> +
> +	ret = video_get_subdev_format(video, f);
> +	if (ret < 0)
> +		return ret;
> +
> +	video->active_fmt = *f;
> +
> +	return 0;
> +}
> +
> +static int video_try_fmt(struct file *file, void *fh, struct v4l2_format *f)
> +{
> +	struct camss_video *video = video_drvdata(file);
> +
> +	if (f->type != video->type)
> +		return -EINVAL;
> +
> +	return video_get_subdev_format(video, f);
> +}
> +
> +static int video_enum_input(struct file *file, void *fh,
> +			    struct v4l2_input *input)
> +{
> +	if (input->index > 0)
> +		return -EINVAL;
> +
> +	strlcpy(input->name, "camera", sizeof(input->name));
> +	input->type = V4L2_INPUT_TYPE_CAMERA;
> +
> +	return 0;
> +}
> +
> +static int video_g_input(struct file *file, void *fh, unsigned int *input)
> +{
> +	*input = 0;
> +
> +	return 0;
> +}
> +
> +static int video_s_input(struct file *file, void *fh, unsigned int input)
> +{
> +	return input == 0 ? 0 : -EINVAL;
> +}
> +
> +static const struct v4l2_ioctl_ops msm_vid_ioctl_ops = {
> +	.vidioc_querycap          = video_querycap,
> +	.vidioc_enum_fmt_vid_cap  = video_enum_fmt,
> +	.vidioc_g_fmt_vid_cap     = video_g_fmt,
> +	.vidioc_s_fmt_vid_cap     = video_s_fmt,
> +	.vidioc_try_fmt_vid_cap   = video_try_fmt,
> +	.vidioc_reqbufs           = vb2_ioctl_reqbufs,
> +	.vidioc_querybuf          = vb2_ioctl_querybuf,
> +	.vidioc_qbuf              = vb2_ioctl_qbuf,
> +	.vidioc_dqbuf             = vb2_ioctl_dqbuf,
> +	.vidioc_create_bufs       = vb2_ioctl_create_bufs,
> +	.vidioc_streamon          = vb2_ioctl_streamon,
> +	.vidioc_streamoff         = vb2_ioctl_streamoff,
> +	.vidioc_enum_input        = video_enum_input,
> +	.vidioc_g_input           = video_g_input,
> +	.vidioc_s_input           = video_s_input,
> +};
> +
> +/* -----------------------------------------------------------------------------
> + * V4L2 file operations
> + */
> +
> +/*
> + * video_init_format - Helper function to initialize format
> + *
> + * Initialize all pad formats with default values.
> + */
> +static int video_init_format(struct file *file, void *fh)
> +{
> +	struct v4l2_format format;
> +
> +	memset(&format, 0, sizeof(format));
> +	format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
> +
> +	return video_s_fmt(file, fh, &format);
> +}
> +
> +static int video_open(struct file *file)
> +{
> +	struct video_device *vdev = video_devdata(file);
> +	struct camss_video *video = video_drvdata(file);
> +	struct camss_video_fh *handle;
> +	int ret;
> +
> +	handle = kzalloc(sizeof(*handle), GFP_KERNEL);
> +	if (handle == NULL)
> +		return -ENOMEM;
> +
> +	v4l2_fh_init(&handle->vfh, video->vdev);
> +	v4l2_fh_add(&handle->vfh);
> +
> +	handle->video = video;
> +	file->private_data = &handle->vfh;
> +
> +	ret = v4l2_pipeline_pm_use(&vdev->entity, 1);
> +	if (ret < 0) {
> +		dev_err(video->camss->dev, "Failed to power up pipeline\n");
> +		goto error_pm_use;
> +	}
> +
> +	ret = video_init_format(file, &handle->vfh);
> +	if (ret < 0) {
> +		dev_err(video->camss->dev, "Failed to init format\n");
> +		goto error_init_format;
> +	}
> +
> +	return 0;
> +
> +error_init_format:
> +	v4l2_pipeline_pm_use(&vdev->entity, 0);
> +
> +error_pm_use:
> +	v4l2_fh_del(&handle->vfh);

You're missing a call to v4l2_fh_exit().

> +	kfree(handle);

But I would recommend replacing v4l2_fh_del, v4l2_fh_exit and kfree by a single
call to v4l2_fh_release().

> +
> +	return ret;
> +}
> +
> +static int video_release(struct file *file)
> +{
> +	struct video_device *vdev = video_devdata(file);
> +	struct camss_video *video = video_drvdata(file);
> +	struct v4l2_fh *vfh = file->private_data;
> +	struct camss_video_fh *handle = container_of(vfh, struct camss_video_fh,
> +						     vfh);
> +
> +	vb2_ioctl_streamoff(file, vfh, video->type);

Don't call this function, instead call vb2_fop_release().

> +
> +	v4l2_pipeline_pm_use(&vdev->entity, 0);
> +
> +	v4l2_fh_del(vfh);
> +	kfree(handle);

These two lines can be dropped as well when you use vb2_fop_release.

> +	file->private_data = NULL;
> +
> +	return 0;
> +}
> +
> +static unsigned int video_poll(struct file *file,
> +				   struct poll_table_struct *wait)
> +{
> +	struct camss_video *video = video_drvdata(file);
> +
> +	return vb2_poll(&video->vb2_q, file, wait);
> +}
> +
> +static int video_mmap(struct file *file, struct vm_area_struct *vma)
> +{
> +	struct camss_video *video = video_drvdata(file);
> +
> +	return vb2_mmap(&video->vb2_q, vma);
> +}
> +
> +static const struct v4l2_file_operations msm_vid_fops = {
> +	.owner          = THIS_MODULE,
> +	.unlocked_ioctl = video_ioctl2,
> +	.open           = video_open,
> +	.release        = video_release,
> +	.poll           = video_poll,
> +	.mmap		= video_mmap,

You should be able to use vb2_fop_poll/mmap here instead of rolling your own.
I recommend adding vb2_fop_read as well.

> +};
> +
> +/* -----------------------------------------------------------------------------
> + * CAMSS video core
> + */
> +
> +int msm_video_register(struct camss_video *video, struct v4l2_device *v4l2_dev,
> +		       const char *name)
> +{
> +	struct media_pad *pad = &video->pad;
> +	struct video_device *vdev;
> +	struct vb2_queue *q;
> +	int ret;
> +
> +	vdev = video_device_alloc();
> +	if (vdev == NULL) {
> +		dev_err(v4l2_dev->dev, "Failed to allocate video device\n");
> +		return -ENOMEM;
> +	}
> +
> +	video->vdev = vdev;
> +
> +	q = &video->vb2_q;
> +	q->drv_priv = video;
> +	q->mem_ops = &vb2_dma_contig_memops;
> +	q->ops = &msm_video_vb2_q_ops;
> +	q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
> +	q->io_modes = VB2_MMAP;

Add modes VB2_DMABUF and VB2_READ. These are for free, so why not? Especially DMABUF
is of course very desirable to have.

> +	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
> +	q->buf_struct_size = sizeof(struct camss_buffer);
> +	q->dev = video->camss->dev;
> +	ret = vb2_queue_init(q);
> +	if (ret < 0) {
> +		dev_err(v4l2_dev->dev, "Failed to init vb2 queue\n");
> +		return ret;
> +	}
> +
> +	pad->flags = MEDIA_PAD_FL_SINK;
> +	ret = media_entity_pads_init(&vdev->entity, 1, pad);
> +	if (ret < 0) {
> +		dev_err(v4l2_dev->dev, "Failed to init video entity\n");
> +		goto error_media_init;
> +	}
> +
> +	vdev->fops = &msm_vid_fops;
> +	vdev->ioctl_ops = &msm_vid_ioctl_ops;
> +	vdev->release = video_device_release;
> +	vdev->v4l2_dev = v4l2_dev;
> +	vdev->vfl_dir = VFL_DIR_RX;
> +	vdev->queue = &video->vb2_q;

As mentioned in querycap: set vdev->device_caps here.

> +	strlcpy(vdev->name, name, sizeof(vdev->name));
> +
> +	ret = video_register_device(vdev, VFL_TYPE_GRABBER, -1);
> +	if (ret < 0) {
> +		dev_err(v4l2_dev->dev, "Failed to register video device\n");
> +		goto error_video_register;
> +	}
> +
> +	video_set_drvdata(vdev, video);
> +
> +	return 0;
> +
> +error_video_register:
> +	media_entity_cleanup(&vdev->entity);
> +error_media_init:
> +	vb2_queue_release(&video->vb2_q);
> +
> +	return ret;
> +}
> +
> +void msm_video_unregister(struct camss_video *video)
> +{
> +	video_unregister_device(video->vdev);
> +	media_entity_cleanup(&video->vdev->entity);
> +	vb2_queue_release(&video->vb2_q);
> +}
> diff --git a/drivers/media/platform/qcom/camss-8x16/video.h b/drivers/media/platform/qcom/camss-8x16/video.h
> new file mode 100644
> index 0000000..5ab5929d
> --- /dev/null
> +++ b/drivers/media/platform/qcom/camss-8x16/video.h
> @@ -0,0 +1,67 @@
> +/*
> + * video.h
> + *
> + * Qualcomm MSM Camera Subsystem - V4L2 device node
> + *
> + * Copyright (c) 2013-2015, The Linux Foundation. All rights reserved.
> + * Copyright (C) 2015-2016 Linaro Ltd.
> + *
> + * This program is free software; you can redistribute it and/or modify
> + * it under the terms of the GNU General Public License version 2 and
> + * only 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.
> + */
> +#ifndef QC_MSM_CAMSS_VIDEO_H
> +#define QC_MSM_CAMSS_VIDEO_H
> +
> +#include <linux/videodev2.h>
> +#include <media/media-entity.h>
> +#include <media/v4l2-dev.h>
> +#include <media/v4l2-device.h>
> +#include <media/v4l2-fh.h>
> +#include <media/v4l2-mediabus.h>
> +#include <media/videobuf2-v4l2.h>
> +
> +#define camss_video_call(f, op, args...)			\
> +	(!(f) ? -ENODEV : (((f)->ops && (f)->ops->op) ? \
> +			    (f)->ops->op((f), ##args) : -ENOIOCTLCMD))
> +
> +struct camss_buffer {
> +	struct vb2_v4l2_buffer vb;
> +	dma_addr_t addr;
> +	struct list_head queue;
> +};
> +
> +struct camss_video;
> +
> +struct camss_video_ops {
> +	int (*queue_buffer)(struct camss_video *vid, struct camss_buffer *buf);
> +	int (*flush_buffers)(struct camss_video *vid);
> +};
> +
> +struct camss_video {
> +	struct camss *camss;
> +	struct vb2_queue vb2_q;
> +	struct video_device *vdev;
> +	struct media_pad pad;
> +	struct v4l2_format active_fmt;
> +	enum v4l2_buf_type type;
> +	struct media_pipeline pipe;
> +	struct camss_video_ops *ops;
> +};
> +
> +struct camss_video_fh {
> +	struct v4l2_fh vfh;
> +	struct camss_video *video;
> +};
> +
> +int msm_video_register(struct camss_video *video, struct v4l2_device *v4l2_dev,
> +		       const char *name);
> +
> +void msm_video_unregister(struct camss_video *video);
> +
> +#endif /* QC_MSM_CAMSS_VIDEO_H */
> 

Regards,

	Hans

Re: [PATCH 08/10] media: camss: Add files which handle the video device nodes

[Laurent Pinchart]

Hi Hans,

On Monday 05 Dec 2016 14:44:55 Hans Verkuil wrote:
> > +static int video_querycap(struct file *file, void *fh,
> > +                       struct v4l2_capability *cap)
> > +{
> > +     strlcpy(cap->driver, "qcom-camss", sizeof(cap->driver));
> > +     strlcpy(cap->card, "Qualcomm Camera Subsystem", sizeof(cap->card));
> > +     strlcpy(cap->bus_info, "platform:qcom-camss",
> > sizeof(cap->bus_info));
> > +     cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING |
> > +                                                     V4L2_CAP_DEVICE_CAPS
> > ;
> > +     cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
> 
> Don't set capabilities and device_caps here. Instead fill in the struct
> video_device device_caps field and the v4l2 core will take care of
> cap->capabilities and cap->device_caps.

Time to add this to checkpatch.pl ? :-)

> > +
> > +     return 0;
> > +}

-- 
Regards,

Laurent Pinchart

Re: [PATCH 08/10] media: camss: Add files which handle the video device nodes

[Laurent Pinchart]

Hello,

On Monday 05 Dec 2016 14:44:55 Hans Verkuil wrote:
> On 11/25/2016 03:57 PM, Todor Tomov wrote:
> > These files handle the video device nodes of the camss driver.
> >
> > Signed-off-by: Todor Tomov <todor.tomov@linaro.org>
> > ---
> >
> >  drivers/media/platform/qcom/camss-8x16/video.c | 597 ++++++++++++++++++++
> >  drivers/media/platform/qcom/camss-8x16/video.h |  67 +++
> >  2 files changed, 664 insertions(+)
> >  create mode 100644 drivers/media/platform/qcom/camss-8x16/video.c
> >  create mode 100644 drivers/media/platform/qcom/camss-8x16/video.h

[snip]

> > +int msm_video_register(struct camss_video *video, struct v4l2_device
> > *v4l2_dev,
> > +                    const char *name)
> > +{
> > +     struct media_pad *pad = &video->pad;
> > +     struct video_device *vdev;
> > +     struct vb2_queue *q;
> > +     int ret;
> > +
> > +     vdev = video_device_alloc();
> > +     if (vdev == NULL) {
> > +             dev_err(v4l2_dev->dev, "Failed to allocate video device\n");
> > +             return -ENOMEM;
> > +     }
> > +
> > +     video->vdev = vdev;
> > +
> > +     q = &video->vb2_q;
> > +     q->drv_priv = video;
> > +     q->mem_ops = &vb2_dma_contig_memops;
> > +     q->ops = &msm_video_vb2_q_ops;
> > +     q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
> > +     q->io_modes = VB2_MMAP;
> 
> Add modes VB2_DMABUF and VB2_READ. These are for free, so why not?
> Especially DMABUF is of course very desirable to have.

I certainly agree with VB2_DMABUF, but I wouldn't expose VB2_READ. read() for 
this kind of device is inefficient and we should encourage userspace 
application to move away from it (and certainly make it very clear that new 
applications should not use read() with this device).

> > +     q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
> > +     q->buf_struct_size = sizeof(struct camss_buffer);
> > +     q->dev = video->camss->dev;
> > +     ret = vb2_queue_init(q);
> > +     if (ret < 0) {
> > +             dev_err(v4l2_dev->dev, "Failed to init vb2 queue\n");
> > +             return ret;
> > +     }
> > +
> > +     pad->flags = MEDIA_PAD_FL_SINK;
> > +     ret = media_entity_pads_init(&vdev->entity, 1, pad);
> > +     if (ret < 0) {
> > +             dev_err(v4l2_dev->dev, "Failed to init video entity\n");
> > +             goto error_media_init;
> > +     }
> > +
> > +     vdev->fops = &msm_vid_fops;
> > +     vdev->ioctl_ops = &msm_vid_ioctl_ops;
> > +     vdev->release = video_device_release;
> > +     vdev->v4l2_dev = v4l2_dev;
> > +     vdev->vfl_dir = VFL_DIR_RX;
> > +     vdev->queue = &video->vb2_q;
> 
> As mentioned in querycap: set vdev->device_caps here.
> 
> > +     strlcpy(vdev->name, name, sizeof(vdev->name));
> > +
> > +     ret = video_register_device(vdev, VFL_TYPE_GRABBER, -1);
> > +     if (ret < 0) {
> > +             dev_err(v4l2_dev->dev, "Failed to register video device\n");
> > +             goto error_video_register;
> > +     }
> > +
> > +     video_set_drvdata(vdev, video);
> > +
> > +     return 0;
> > +
> > +error_video_register:
> > +     media_entity_cleanup(&vdev->entity);
> > +error_media_init:
> > +     vb2_queue_release(&video->vb2_q);
> > +
> > +     return ret;
> > +}

-- 
Regards,

Laurent Pinchart

Re: [PATCH 08/10] media: camss: Add files which handle the video device nodes

[Hans Verkuil]

On 12/05/2016 03:45 PM, Laurent Pinchart wrote:
> Hello,
> 
> On Monday 05 Dec 2016 14:44:55 Hans Verkuil wrote:
>> On 11/25/2016 03:57 PM, Todor Tomov wrote:
>>> These files handle the video device nodes of the camss driver.
>>>
>>> Signed-off-by: Todor Tomov <todor.tomov@linaro.org>
>>> ---
>>>
>>>  drivers/media/platform/qcom/camss-8x16/video.c | 597 ++++++++++++++++++++
>>>  drivers/media/platform/qcom/camss-8x16/video.h |  67 +++
>>>  2 files changed, 664 insertions(+)
>>>  create mode 100644 drivers/media/platform/qcom/camss-8x16/video.c
>>>  create mode 100644 drivers/media/platform/qcom/camss-8x16/video.h
> 
> [snip]
> 
>>> +int msm_video_register(struct camss_video *video, struct v4l2_device
>>> *v4l2_dev,
>>> +                    const char *name)
>>> +{
>>> +     struct media_pad *pad = &video->pad;
>>> +     struct video_device *vdev;
>>> +     struct vb2_queue *q;
>>> +     int ret;
>>> +
>>> +     vdev = video_device_alloc();
>>> +     if (vdev == NULL) {
>>> +             dev_err(v4l2_dev->dev, "Failed to allocate video device\n");
>>> +             return -ENOMEM;
>>> +     }
>>> +
>>> +     video->vdev = vdev;
>>> +
>>> +     q = &video->vb2_q;
>>> +     q->drv_priv = video;
>>> +     q->mem_ops = &vb2_dma_contig_memops;
>>> +     q->ops = &msm_video_vb2_q_ops;
>>> +     q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
>>> +     q->io_modes = VB2_MMAP;
>>
>> Add modes VB2_DMABUF and VB2_READ. These are for free, so why not?
>> Especially DMABUF is of course very desirable to have.
> 
> I certainly agree with VB2_DMABUF, but I wouldn't expose VB2_READ. read() for 
> this kind of device is inefficient and we should encourage userspace 
> application to move away from it (and certainly make it very clear that new 
> applications should not use read() with this device).

VB2_READ is very nice when debugging (no need to program a stream I/O application first)
and useful when you want to pipe the video.

Nobody uses read() in 'regular' applications since it is obviously inefficient, but
I don't see that as a reason not to offer VB2_READ.

I don't think this is something anyone will ever abuse, and it is a nice feature
to have (and for free as well).

Regards,

	Hans

Re: [PATCH 08/10] media: camss: Add files which handle the video device nodes

[Laurent Pinchart]

Hi Hans,

On Monday 05 Dec 2016 16:02:55 Hans Verkuil wrote:
> On 12/05/2016 03:45 PM, Laurent Pinchart wrote:
> > On Monday 05 Dec 2016 14:44:55 Hans Verkuil wrote:
> >> On 11/25/2016 03:57 PM, Todor Tomov wrote:
> >>> These files handle the video device nodes of the camss driver.
> >>> 
> >>> Signed-off-by: Todor Tomov <todor.tomov@linaro.org>
> >>> ---
> >>> 
> >>>  drivers/media/platform/qcom/camss-8x16/video.c | 597 +++++++++++++++++
> >>>  drivers/media/platform/qcom/camss-8x16/video.h |  67 +++
> >>>  2 files changed, 664 insertions(+)
> >>>  create mode 100644 drivers/media/platform/qcom/camss-8x16/video.c
> >>>  create mode 100644 drivers/media/platform/qcom/camss-8x16/video.h
> > 
> > [snip]
> > 
> >>> +int msm_video_register(struct camss_video *video, struct v4l2_device
> >>> *v4l2_dev,
> >>> +                    const char *name)
> >>> +{
> >>> +     struct media_pad *pad = &video->pad;
> >>> +     struct video_device *vdev;
> >>> +     struct vb2_queue *q;
> >>> +     int ret;
> >>> +
> >>> +     vdev = video_device_alloc();
> >>> +     if (vdev == NULL) {
> >>> +             dev_err(v4l2_dev->dev, "Failed to allocate video
> >>> device\n");
> >>> +             return -ENOMEM;
> >>> +     }
> >>> +
> >>> +     video->vdev = vdev;
> >>> +
> >>> +     q = &video->vb2_q;
> >>> +     q->drv_priv = video;
> >>> +     q->mem_ops = &vb2_dma_contig_memops;
> >>> +     q->ops = &msm_video_vb2_q_ops;
> >>> +     q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
> >>> +     q->io_modes = VB2_MMAP;
> >> 
> >> Add modes VB2_DMABUF and VB2_READ. These are for free, so why not?
> >> Especially DMABUF is of course very desirable to have.
> > 
> > I certainly agree with VB2_DMABUF, but I wouldn't expose VB2_READ. read()
> > for this kind of device is inefficient and we should encourage userspace
> > application to move away from it (and certainly make it very clear that
> > new applications should not use read() with this device).
> 
> VB2_READ is very nice when debugging (no need to program a stream I/O
> application first)

There's at least v4l2-ctl and yavta that can (and should) be used for 
debugging ;-)

> and useful when you want to pipe the video.

Except that you lose frame boundaries. It's really a poor man's solution that 
should never be used in any "real" application. I'd rather add an option to 
v4l2-ctl to output the video stream to stdout (and/or stderr).

> Nobody uses read() in 'regular' applications since it is obviously
> inefficient, but I don't see that as a reason not to offer VB2_READ.
> 
> I don't think this is something anyone will ever abuse,

Famous last words ;-)

> and it is a nice feature to have (and for free as well).

-- 
Regards,

Laurent Pinchart

Re: [PATCH 08/10] media: camss: Add files which handle the video device nodes

[Todor Tomov]

Hi Laurent, Hans,

On 12/05/2016 05:25 PM, Laurent Pinchart wrote:
> Hi Hans,
> 
> On Monday 05 Dec 2016 16:02:55 Hans Verkuil wrote:
>> On 12/05/2016 03:45 PM, Laurent Pinchart wrote:
>>> On Monday 05 Dec 2016 14:44:55 Hans Verkuil wrote:
>>>> On 11/25/2016 03:57 PM, Todor Tomov wrote:
>>>>> These files handle the video device nodes of the camss driver.
>>>>>
>>>>> Signed-off-by: Todor Tomov <todor.tomov@linaro.org>
>>>>> ---
>>>>>
>>>>>  drivers/media/platform/qcom/camss-8x16/video.c | 597 +++++++++++++++++
>>>>>  drivers/media/platform/qcom/camss-8x16/video.h |  67 +++
>>>>>  2 files changed, 664 insertions(+)
>>>>>  create mode 100644 drivers/media/platform/qcom/camss-8x16/video.c
>>>>>  create mode 100644 drivers/media/platform/qcom/camss-8x16/video.h
>>>
>>> [snip]
>>>
>>>>> +int msm_video_register(struct camss_video *video, struct v4l2_device
>>>>> *v4l2_dev,
>>>>> +                    const char *name)
>>>>> +{
>>>>> +     struct media_pad *pad = &video->pad;
>>>>> +     struct video_device *vdev;
>>>>> +     struct vb2_queue *q;
>>>>> +     int ret;
>>>>> +
>>>>> +     vdev = video_device_alloc();
>>>>> +     if (vdev == NULL) {
>>>>> +             dev_err(v4l2_dev->dev, "Failed to allocate video
>>>>> device\n");
>>>>> +             return -ENOMEM;
>>>>> +     }
>>>>> +
>>>>> +     video->vdev = vdev;
>>>>> +
>>>>> +     q = &video->vb2_q;
>>>>> +     q->drv_priv = video;
>>>>> +     q->mem_ops = &vb2_dma_contig_memops;
>>>>> +     q->ops = &msm_video_vb2_q_ops;
>>>>> +     q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
>>>>> +     q->io_modes = VB2_MMAP;
>>>>
>>>> Add modes VB2_DMABUF and VB2_READ. These are for free, so why not?
>>>> Especially DMABUF is of course very desirable to have.
>>>
>>> I certainly agree with VB2_DMABUF, but I wouldn't expose VB2_READ. read()
>>> for this kind of device is inefficient and we should encourage userspace
>>> application to move away from it (and certainly make it very clear that
>>> new applications should not use read() with this device).
>>
>> VB2_READ is very nice when debugging (no need to program a stream I/O
>> application first)
> 
> There's at least v4l2-ctl and yavta that can (and should) be used for 
> debugging ;-)
> 
>> and useful when you want to pipe the video.
> 
> Except that you lose frame boundaries. It's really a poor man's solution that 
> should never be used in any "real" application. I'd rather add an option to 
> v4l2-ctl to output the video stream to stdout (and/or stderr).
> 
>> Nobody uses read() in 'regular' applications since it is obviously
>> inefficient, but I don't see that as a reason not to offer VB2_READ.
>>
>> I don't think this is something anyone will ever abuse,
> 
> Famous last words ;-)
> 
>> and it is a nice feature to have (and for free as well).
> 

Thank you for the discussion over this. Both of you have reasonable arguments
about the read i/o.
I'd say that you cannot always save a person from himself. I'll add VB2_READ.

-- 
Best regards,
Todor Tomov

Re: [PATCH 08/10] media: camss: Add files which handle the video device nodes

[Todor Tomov]

Hi Hans,

Thank you for the extensive review.

On 12/05/2016 03:44 PM, Hans Verkuil wrote:
> A few comments below:
> 
> On 11/25/2016 03:57 PM, Todor Tomov wrote:
>> These files handle the video device nodes of the camss driver.
>>
>> Signed-off-by: Todor Tomov <todor.tomov@linaro.org>
>> ---
>>  drivers/media/platform/qcom/camss-8x16/video.c | 597 +++++++++++++++++++++++++
>>  drivers/media/platform/qcom/camss-8x16/video.h |  67 +++
>>  2 files changed, 664 insertions(+)
>>  create mode 100644 drivers/media/platform/qcom/camss-8x16/video.c
>>  create mode 100644 drivers/media/platform/qcom/camss-8x16/video.h
>>
>> diff --git a/drivers/media/platform/qcom/camss-8x16/video.c b/drivers/media/platform/qcom/camss-8x16/video.c
>> new file mode 100644
>> index 0000000..0bf8ea9
>> --- /dev/null
>> +++ b/drivers/media/platform/qcom/camss-8x16/video.c
>> @@ -0,0 +1,597 @@
> 
> <snip>
> 
>> +static int video_start_streaming(struct vb2_queue *q, unsigned int count)
>> +{
>> +	struct camss_video *video = vb2_get_drv_priv(q);
>> +	struct video_device *vdev = video->vdev;
>> +	struct media_entity *entity;
>> +	struct media_pad *pad;
>> +	struct v4l2_subdev *subdev;
>> +	int ret;
>> +
>> +	ret = media_entity_pipeline_start(&vdev->entity, &video->pipe);
>> +	if (ret < 0)
>> +		return ret;
>> +
>> +	ret = video_check_format(video);
>> +	if (ret < 0)
>> +		goto error;
>> +
>> +	entity = &vdev->entity;
>> +	while (1) {
>> +		pad = &entity->pads[0];
>> +		if (!(pad->flags & MEDIA_PAD_FL_SINK))
>> +			break;
>> +
>> +		pad = media_entity_remote_pad(pad);
>> +		if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
>> +			break;
>> +
>> +		entity = pad->entity;
>> +		subdev = media_entity_to_v4l2_subdev(entity);
>> +
>> +		ret = v4l2_subdev_call(subdev, video, s_stream, 1);
>> +		if (ret < 0 && ret != -ENOIOCTLCMD)
>> +			goto error;
>> +	}
>> +
>> +	return 0;
>> +
>> +error:
>> +	media_entity_pipeline_stop(&vdev->entity);
>> +
> 
> On error all queued buffers must be returned with state VB2_STATE_QUEUED.
> 
> Basically the same as 'camss_video_call(video, flush_buffers);', just with
> a different state.

Ok, I'll add this.

> 
>> +	return ret;
>> +}
> 
> <snip>
> 
>> +static int video_querycap(struct file *file, void *fh,
>> +			  struct v4l2_capability *cap)
>> +{
>> +	strlcpy(cap->driver, "qcom-camss", sizeof(cap->driver));
>> +	strlcpy(cap->card, "Qualcomm Camera Subsystem", sizeof(cap->card));
>> +	strlcpy(cap->bus_info, "platform:qcom-camss", sizeof(cap->bus_info));
>> +	cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING |
>> +							V4L2_CAP_DEVICE_CAPS;
>> +	cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
> 
> Don't set capabilities and device_caps here. Instead fill in the struct video_device
> device_caps field and the v4l2 core will take care of cap->capabilities and
> cap->device_caps.

Ok.

> 
>> +
>> +	return 0;
>> +}
>> +
>> +static int video_enum_fmt(struct file *file, void *fh, struct v4l2_fmtdesc *f)
>> +{
>> +	struct camss_video *video = video_drvdata(file);
>> +	struct v4l2_format format;
>> +	int ret;
>> +
>> +	if (f->type != video->type)
>> +		return -EINVAL;
>> +
>> +	if (f->index)
>> +		return -EINVAL;
>> +
>> +	ret = video_get_subdev_format(video, &format);
>> +	if (ret < 0)
>> +		return ret;
>> +
>> +	f->pixelformat = format.fmt.pix.pixelformat;
>> +
>> +	return 0;
>> +}
>> +
>> +static int video_g_fmt(struct file *file, void *fh, struct v4l2_format *f)
>> +{
>> +	struct camss_video *video = video_drvdata(file);
>> +
>> +	if (f->type != video->type)
>> +		return -EINVAL;
>> +
>> +	*f = video->active_fmt;
>> +
>> +	return 0;
>> +}
>> +
>> +static int video_s_fmt(struct file *file, void *fh, struct v4l2_format *f)
>> +{
>> +	struct camss_video *video = video_drvdata(file);
>> +	int ret;
>> +
>> +	if (f->type != video->type)
>> +		return -EINVAL;
>> +
>> +	ret = video_get_subdev_format(video, f);
>> +	if (ret < 0)
>> +		return ret;
>> +
>> +	video->active_fmt = *f;
>> +
>> +	return 0;
>> +}
>> +
>> +static int video_try_fmt(struct file *file, void *fh, struct v4l2_format *f)
>> +{
>> +	struct camss_video *video = video_drvdata(file);
>> +
>> +	if (f->type != video->type)
>> +		return -EINVAL;
>> +
>> +	return video_get_subdev_format(video, f);
>> +}
>> +
>> +static int video_enum_input(struct file *file, void *fh,
>> +			    struct v4l2_input *input)
>> +{
>> +	if (input->index > 0)
>> +		return -EINVAL;
>> +
>> +	strlcpy(input->name, "camera", sizeof(input->name));
>> +	input->type = V4L2_INPUT_TYPE_CAMERA;
>> +
>> +	return 0;
>> +}
>> +
>> +static int video_g_input(struct file *file, void *fh, unsigned int *input)
>> +{
>> +	*input = 0;
>> +
>> +	return 0;
>> +}
>> +
>> +static int video_s_input(struct file *file, void *fh, unsigned int input)
>> +{
>> +	return input == 0 ? 0 : -EINVAL;
>> +}
>> +
>> +static const struct v4l2_ioctl_ops msm_vid_ioctl_ops = {
>> +	.vidioc_querycap          = video_querycap,
>> +	.vidioc_enum_fmt_vid_cap  = video_enum_fmt,
>> +	.vidioc_g_fmt_vid_cap     = video_g_fmt,
>> +	.vidioc_s_fmt_vid_cap     = video_s_fmt,
>> +	.vidioc_try_fmt_vid_cap   = video_try_fmt,
>> +	.vidioc_reqbufs           = vb2_ioctl_reqbufs,
>> +	.vidioc_querybuf          = vb2_ioctl_querybuf,
>> +	.vidioc_qbuf              = vb2_ioctl_qbuf,
>> +	.vidioc_dqbuf             = vb2_ioctl_dqbuf,
>> +	.vidioc_create_bufs       = vb2_ioctl_create_bufs,
>> +	.vidioc_streamon          = vb2_ioctl_streamon,
>> +	.vidioc_streamoff         = vb2_ioctl_streamoff,
>> +	.vidioc_enum_input        = video_enum_input,
>> +	.vidioc_g_input           = video_g_input,
>> +	.vidioc_s_input           = video_s_input,
>> +};
>> +
>> +/* -----------------------------------------------------------------------------
>> + * V4L2 file operations
>> + */
>> +
>> +/*
>> + * video_init_format - Helper function to initialize format
>> + *
>> + * Initialize all pad formats with default values.
>> + */
>> +static int video_init_format(struct file *file, void *fh)
>> +{
>> +	struct v4l2_format format;
>> +
>> +	memset(&format, 0, sizeof(format));
>> +	format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
>> +
>> +	return video_s_fmt(file, fh, &format);
>> +}
>> +
>> +static int video_open(struct file *file)
>> +{
>> +	struct video_device *vdev = video_devdata(file);
>> +	struct camss_video *video = video_drvdata(file);
>> +	struct camss_video_fh *handle;
>> +	int ret;
>> +
>> +	handle = kzalloc(sizeof(*handle), GFP_KERNEL);
>> +	if (handle == NULL)
>> +		return -ENOMEM;
>> +
>> +	v4l2_fh_init(&handle->vfh, video->vdev);
>> +	v4l2_fh_add(&handle->vfh);
>> +
>> +	handle->video = video;
>> +	file->private_data = &handle->vfh;
>> +
>> +	ret = v4l2_pipeline_pm_use(&vdev->entity, 1);
>> +	if (ret < 0) {
>> +		dev_err(video->camss->dev, "Failed to power up pipeline\n");
>> +		goto error_pm_use;
>> +	}
>> +
>> +	ret = video_init_format(file, &handle->vfh);
>> +	if (ret < 0) {
>> +		dev_err(video->camss->dev, "Failed to init format\n");
>> +		goto error_init_format;
>> +	}
>> +
>> +	return 0;
>> +
>> +error_init_format:
>> +	v4l2_pipeline_pm_use(&vdev->entity, 0);
>> +
>> +error_pm_use:
>> +	v4l2_fh_del(&handle->vfh);
> 
> You're missing a call to v4l2_fh_exit().
> 
>> +	kfree(handle);
> 
> But I would recommend replacing v4l2_fh_del, v4l2_fh_exit and kfree by a single
> call to v4l2_fh_release().

Ok, I'll switch to v4l2_fh_release().

> 
>> +
>> +	return ret;
>> +}
>> +
>> +static int video_release(struct file *file)
>> +{
>> +	struct video_device *vdev = video_devdata(file);
>> +	struct camss_video *video = video_drvdata(file);
>> +	struct v4l2_fh *vfh = file->private_data;
>> +	struct camss_video_fh *handle = container_of(vfh, struct camss_video_fh,
>> +						     vfh);
>> +
>> +	vb2_ioctl_streamoff(file, vfh, video->type);
> 
> Don't call this function, instead call vb2_fop_release().

Ok.

> 
>> +
>> +	v4l2_pipeline_pm_use(&vdev->entity, 0);
>> +
>> +	v4l2_fh_del(vfh);
>> +	kfree(handle);
> 
> These two lines can be dropped as well when you use vb2_fop_release.

Ok.

> 
>> +	file->private_data = NULL;
>> +
>> +	return 0;
>> +}
>> +
>> +static unsigned int video_poll(struct file *file,
>> +				   struct poll_table_struct *wait)
>> +{
>> +	struct camss_video *video = video_drvdata(file);
>> +
>> +	return vb2_poll(&video->vb2_q, file, wait);
>> +}
>> +
>> +static int video_mmap(struct file *file, struct vm_area_struct *vma)
>> +{
>> +	struct camss_video *video = video_drvdata(file);
>> +
>> +	return vb2_mmap(&video->vb2_q, vma);
>> +}
>> +
>> +static const struct v4l2_file_operations msm_vid_fops = {
>> +	.owner          = THIS_MODULE,
>> +	.unlocked_ioctl = video_ioctl2,
>> +	.open           = video_open,
>> +	.release        = video_release,
>> +	.poll           = video_poll,
>> +	.mmap		= video_mmap,
> 
> You should be able to use vb2_fop_poll/mmap here instead of rolling your own.
> I recommend adding vb2_fop_read as well.

Ok. Trying to use vb2_fop_poll makes me realize that I probably have to initialize
video_device->lock before video_register_device() and maybe vb2_queue->lock too.
I'll try with these.

<snip>


-- 
Best regards,
Todor Tomov

Re: [PATCH 08/10] media: camss: Add files which handle the video device nodes

[Laurent Pinchart]

Hi Todor,

Thank you for the patch.

On Friday 25 Nov 2016 16:57:20 Todor Tomov wrote:
> These files handle the video device nodes of the camss driver.

camss is a quite generic, I'm a bit concerned about claiming that acronym in 
the global kernel namespace. Would it be too long if we prefixed symbols with 
msm_camss instead ?

> Signed-off-by: Todor Tomov <todor.tomov@linaro.org>
> ---
>  drivers/media/platform/qcom/camss-8x16/video.c | 597 ++++++++++++++++++++++
>  drivers/media/platform/qcom/camss-8x16/video.h |  67 +++
>  2 files changed, 664 insertions(+)
>  create mode 100644 drivers/media/platform/qcom/camss-8x16/video.c
>  create mode 100644 drivers/media/platform/qcom/camss-8x16/video.h
> 
> diff --git a/drivers/media/platform/qcom/camss-8x16/video.c
> b/drivers/media/platform/qcom/camss-8x16/video.c new file mode 100644
> index 0000000..0bf8ea9
> --- /dev/null
> +++ b/drivers/media/platform/qcom/camss-8x16/video.c
> @@ -0,0 +1,597 @@
> +/*
> + * video.c
> + *
> + * Qualcomm MSM Camera Subsystem - V4L2 device node
> + *
> + * Copyright (c) 2013-2015, The Linux Foundation. All rights reserved.
> + * Copyright (C) 2015-2016 Linaro Ltd.
> + *
> + * This program is free software; you can redistribute it and/or modify
> + * it under the terms of the GNU General Public License version 2 and
> + * only 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.
> + */
> +#include <media/media-entity.h>
> +#include <media/v4l2-dev.h>
> +#include <media/v4l2-device.h>
> +#include <media/v4l2-ioctl.h>
> +#include <media/v4l2-mc.h>
> +#include <media/videobuf-core.h>
> +#include <media/videobuf2-dma-contig.h>
> +
> +#include "video.h"
> +#include "camss.h"
> +
> +/*
> + * struct format_info - ISP media bus format information
> + * @code: V4L2 media bus format code
> + * @pixelformat: V4L2 pixel format FCC identifier
> + * @bpp: Bits per pixel when stored in memory
> + */
> +static const struct format_info {
> +	u32 code;
> +	u32 pixelformat;
> +	unsigned int bpp;
> +} formats[] = {
> +	{ MEDIA_BUS_FMT_UYVY8_2X8, V4L2_PIX_FMT_UYVY, 16 },
> +	{ MEDIA_BUS_FMT_VYUY8_2X8, V4L2_PIX_FMT_VYUY, 16 },
> +	{ MEDIA_BUS_FMT_YUYV8_2X8, V4L2_PIX_FMT_YUYV, 16 },
> +	{ MEDIA_BUS_FMT_YVYU8_2X8, V4L2_PIX_FMT_YVYU, 16 },
> +	{ MEDIA_BUS_FMT_SBGGR8_1X8, V4L2_PIX_FMT_SBGGR8, 8 },
> +	{ MEDIA_BUS_FMT_SGBRG8_1X8, V4L2_PIX_FMT_SGBRG8, 8 },
> +	{ MEDIA_BUS_FMT_SGRBG8_1X8, V4L2_PIX_FMT_SGRBG8, 8 },
> +	{ MEDIA_BUS_FMT_SRGGB8_1X8, V4L2_PIX_FMT_SRGGB8, 8 },
> +	{ MEDIA_BUS_FMT_SBGGR10_1X10, V4L2_PIX_FMT_SBGGR10P, 10 },
> +	{ MEDIA_BUS_FMT_SGBRG10_1X10, V4L2_PIX_FMT_SGBRG10P, 10 },
> +	{ MEDIA_BUS_FMT_SGRBG10_1X10, V4L2_PIX_FMT_SGRBG10P, 10 },
> +	{ MEDIA_BUS_FMT_SRGGB10_1X10, V4L2_PIX_FMT_SRGGB10P, 10 },
> +	{ MEDIA_BUS_FMT_SBGGR12_1X12, V4L2_PIX_FMT_SRGGB12P, 12 },
> +	{ MEDIA_BUS_FMT_SGBRG12_1X12, V4L2_PIX_FMT_SGBRG12P, 12 },
> +	{ MEDIA_BUS_FMT_SGRBG12_1X12, V4L2_PIX_FMT_SGRBG12P, 12 },
> +	{ MEDIA_BUS_FMT_SRGGB12_1X12, V4L2_PIX_FMT_SRGGB12P, 12 }
> +};
> +
> +/*
> ---------------------------------------------------------------------------
> -- + * Helper functions
> + */
> +
> +/*
> + * video_mbus_to_pix - Convert v4l2_mbus_framefmt to v4l2_pix_format
> + * @mbus: v4l2_mbus_framefmt format (input)
> + * @pix: v4l2_pix_format format (output)
> + *
> + * Fill the output pix structure with information from the input mbus
> format.
> + *
> + * Return 0 on success or a negative error code otherwise
> + */
> +static unsigned int video_mbus_to_pix(const struct v4l2_mbus_framefmt
> *mbus,
> +				      struct v4l2_pix_format *pix)
> +{
> +	unsigned int i;
> +
> +	memset(pix, 0, sizeof(*pix));
> +	pix->width = mbus->width;
> +	pix->height = mbus->height;
> +
> +	for (i = 0; i < ARRAY_SIZE(formats); ++i) {
> +		if (formats[i].code == mbus->code)
> +			break;
> +	}
> +
> +	if (WARN_ON(i == ARRAY_SIZE(formats)))
> +		return -EINVAL;
> +
> +	pix->pixelformat = formats[i].pixelformat;
> +	pix->bytesperline = pix->width * formats[i].bpp / 8;
> +	pix->bytesperline = ALIGN(pix->bytesperline, 8);

Does the hardware support padding at the end of lines ? If so it would be 
useful to use the maximum of the computed and requested by userspace values 
(up to the maximum size of the padding supported by the hardware).

> +	pix->sizeimage = pix->bytesperline * pix->height;

Similarly, should we support padding at the end of the image ?

> +	pix->colorspace = mbus->colorspace;
> +	pix->field = mbus->field;
> +
> +	return 0;
> +}
> +
> +static struct v4l2_subdev *video_remote_subdev(struct camss_video *video,
> +					       u32 *pad)
> +{
> +	struct media_pad *remote;
> +
> +	remote = media_entity_remote_pad(&video->pad);
> +
> +	if (!remote || !is_media_entity_v4l2_subdev(remote->entity))
> +		return NULL;
> +
> +	if (pad)
> +		*pad = remote->index;
> +
> +	return media_entity_to_v4l2_subdev(remote->entity);
> +}
> +
> +static int video_get_subdev_format(struct camss_video *video,
> +				   struct v4l2_format *format)
> +{
> +	struct v4l2_subdev_format fmt;
> +	struct v4l2_subdev *subdev;
> +	u32 pad;
> +	int ret;
> +
> +	subdev = video_remote_subdev(video, &pad);
> +	if (subdev == NULL)
> +		return -EINVAL;
> +
> +	fmt.pad = pad;
> +	fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
> +
> +	ret = v4l2_subdev_call(subdev, pad, get_fmt, NULL, &fmt);
> +	if (ret)
> +		return ret;
> +
> +	format->type = video->type;
> +	return video_mbus_to_pix(&fmt.format, &format->fmt.pix);
> +}
> +
> +/* ------------------------------------------------------------------------
> + * Video queue operations
> + */
> +
> +static int video_queue_setup(struct vb2_queue *q,
> +	unsigned int *num_buffers, unsigned int *num_planes,
> +	unsigned int sizes[], struct device *alloc_devs[])
> +{
> +	struct camss_video *video = vb2_get_drv_priv(q);
> +
> +	if (*num_planes) {
> +		if (*num_planes != 1)
> +			return -EINVAL;
> +
> +		if (sizes[0] < video->active_fmt.fmt.pix.sizeimage)
> +			return -EINVAL;
> +
> +		return 0;
> +	}
> +
> +	*num_planes = 1;
> +
> +	sizes[0] = video->active_fmt.fmt.pix.sizeimage;
> +
> +	return 0;
> +}
> +
> +static int video_buf_prepare(struct vb2_buffer *vb)
> +{
> +	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
> +	struct camss_video *video = vb2_get_drv_priv(vb->vb2_queue);
> +	struct camss_buffer *buffer = container_of(vbuf, struct camss_buffer,
> +						   vb);

You can define a static inline function to wrap this container_of() as it's 
used in multiple places in this file.

> +
> +	vb2_set_plane_payload(vb, 0, video->active_fmt.fmt.pix.sizeimage);
> +	if (vb2_get_plane_payload(vb, 0) > vb2_plane_size(vb, 0))

Wouldn't it be more efficient to compare video->active_fmt.fmt.pix.sizeimage 
instead of vb2_get_plane_payload(vb, 0) ? The two should be identical as 
you've just called vb2_set_plane_payload(). I would also move the 
vb2_set_plane_payload() call after the error check.

> +		return -EINVAL;
> +
> +	vbuf->field = V4L2_FIELD_NONE;
> +
> +	buffer->addr = vb2_dma_contig_plane_dma_addr(vb, 0);
> +
> +	return 0;
> +}
> +
> +static void video_buf_queue(struct vb2_buffer *vb)
> +{
> +	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
> +	struct camss_video *video = vb2_get_drv_priv(vb->vb2_queue);
> +	struct camss_buffer *buffer = container_of(vbuf, struct camss_buffer,
> +						   vb);
> +
> +	camss_video_call(video, queue_buffer, buffer);

Should I assume that this abstraction means you'll add support for more than 
the RDI in the future ? ;-)

I'd remove the camss_video_call() macro and use

	video->ops->queue_buffer(video, buffer);

directly as it's easier to follow, and we don't need to test video->ops && 
video->ops->op as all operations should always be provided.

> +}
> +
> +

A single blank line is enough.

> +static int video_check_format(struct camss_video *video)
> +{
> +	struct v4l2_pix_format *pix = &video->active_fmt.fmt.pix;
> +	struct v4l2_format format;
> +	int ret;
> +
> +	ret = video_get_subdev_format(video, &format);
> +	if (ret < 0)
> +		return ret;
> +
> +	if (pix->pixelformat != format.fmt.pix.pixelformat ||
> +	    pix->height != format.fmt.pix.height ||
> +	    pix->width != format.fmt.pix.width ||
> +	    pix->bytesperline != format.fmt.pix.bytesperline ||
> +	    pix->sizeimage != format.fmt.pix.sizeimage ||
> +	    pix->field != format.fmt.pix.field)
> +		return -EINVAL;
> +
> +	return 0;
> +}
> +
> +static int video_start_streaming(struct vb2_queue *q, unsigned int count)
> +{
> +	struct camss_video *video = vb2_get_drv_priv(q);
> +	struct video_device *vdev = video->vdev;
> +	struct media_entity *entity;
> +	struct media_pad *pad;
> +	struct v4l2_subdev *subdev;
> +	int ret;
> +
> +	ret = media_entity_pipeline_start(&vdev->entity, &video->pipe);
> +	if (ret < 0)
> +		return ret;
> +
> +	ret = video_check_format(video);
> +	if (ret < 0)
> +		goto error;
> +
> +	entity = &vdev->entity;
> +	while (1) {
> +		pad = &entity->pads[0];
> +		if (!(pad->flags & MEDIA_PAD_FL_SINK))
> +			break;
> +
> +		pad = media_entity_remote_pad(pad);
> +		if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
> +			break;
> +
> +		entity = pad->entity;
> +		subdev = media_entity_to_v4l2_subdev(entity);
> +
> +		ret = v4l2_subdev_call(subdev, video, s_stream, 1);
> +		if (ret < 0 && ret != -ENOIOCTLCMD)
> +			goto error;
> +	}
> +
> +	return 0;
> +
> +error:
> +	media_entity_pipeline_stop(&vdev->entity);
> +
> +	return ret;
> +}
> +
> +static void video_stop_streaming(struct vb2_queue *q)
> +{
> +	struct camss_video *video = vb2_get_drv_priv(q);
> +	struct video_device *vdev = video->vdev;
> +	struct media_entity *entity;
> +	struct media_pad *pad;
> +	struct v4l2_subdev *subdev;
> +	struct v4l2_subdev *subdev_vfe;
> +
> +	entity = &vdev->entity;
> +	while (1) {
> +		pad = &entity->pads[0];
> +		if (!(pad->flags & MEDIA_PAD_FL_SINK))
> +			break;
> +
> +		pad = media_entity_remote_pad(pad);
> +		if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
> +			break;
> +
> +		entity = pad->entity;
> +		subdev = media_entity_to_v4l2_subdev(entity);
> +
> +		if (strstr(subdev->name, "vfe")) {
> +			subdev_vfe = subdev;
> +		} else if (strstr(subdev->name, "ispif")) {
> +			v4l2_subdev_call(subdev, video, s_stream, 0);
> +			v4l2_subdev_call(subdev_vfe, video, s_stream, 0);
> +		} else {
> +			v4l2_subdev_call(subdev, video, s_stream, 0);
> +		}
> +	}
> +
> +	media_entity_pipeline_stop(&vdev->entity);
> +
> +	camss_video_call(video, flush_buffers);
> +}
> +
> +static struct vb2_ops msm_video_vb2_q_ops = {

You can make this static const.

> +	.queue_setup     = video_queue_setup,
> +	.buf_prepare     = video_buf_prepare,
> +	.buf_queue       = video_buf_queue,
> +	.start_streaming = video_start_streaming,
> +	.stop_streaming  = video_stop_streaming,
> +};
> +
> +/* ------------------------------------------------------------------------
> + * V4L2 ioctls
> + */
> +
> +static int video_querycap(struct file *file, void *fh,
> +			  struct v4l2_capability *cap)
> +{
> +	strlcpy(cap->driver, "qcom-camss", sizeof(cap->driver));
> +	strlcpy(cap->card, "Qualcomm Camera Subsystem", sizeof(cap->card));
> +	strlcpy(cap->bus_info, "platform:qcom-camss", sizeof(cap->bus_info));

	snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s",
		 dev_name(...));

would be better.

> +	cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING |
> +							V4L2_CAP_DEVICE_CAPS;
> +	cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
> +
> +	return 0;
> +}
> +
> +static int video_enum_fmt(struct file *file, void *fh, struct v4l2_fmtdesc
> *f)
> +{
> +	struct camss_video *video = video_drvdata(file);
> +	struct v4l2_format format;
> +	int ret;
> +
> +	if (f->type != video->type)
> +		return -EINVAL;
> +
> +	if (f->index)
> +		return -EINVAL;
> +
> +	ret = video_get_subdev_format(video, &format);
> +	if (ret < 0)
> +		return ret;
> +
> +	f->pixelformat = format.fmt.pix.pixelformat;
> +
> +	return 0;
> +}
> +
> +static int video_g_fmt(struct file *file, void *fh, struct v4l2_format *f)
> +{
> +	struct camss_video *video = video_drvdata(file);
> +
> +	if (f->type != video->type)
> +		return -EINVAL;
> +
> +	*f = video->active_fmt;
> +
> +	return 0;
> +}
> +
> +static int video_s_fmt(struct file *file, void *fh, struct v4l2_format *f)
> +{
> +	struct camss_video *video = video_drvdata(file);
> +	int ret;
> +
> +	if (f->type != video->type)
> +		return -EINVAL;
> +
> +	ret = video_get_subdev_format(video, f);
> +	if (ret < 0)
> +		return ret;
> +
> +	video->active_fmt = *f;
> +
> +	return 0;
> +}
> +
> +static int video_try_fmt(struct file *file, void *fh, struct v4l2_format
> *f) +{
> +	struct camss_video *video = video_drvdata(file);
> +
> +	if (f->type != video->type)
> +		return -EINVAL;
> +
> +	return video_get_subdev_format(video, f);
> +}
> +
> +static int video_enum_input(struct file *file, void *fh,
> +			    struct v4l2_input *input)
> +{
> +	if (input->index > 0)
> +		return -EINVAL;
> +
> +	strlcpy(input->name, "camera", sizeof(input->name));
> +	input->type = V4L2_INPUT_TYPE_CAMERA;
> +
> +	return 0;
> +}
> +
> +static int video_g_input(struct file *file, void *fh, unsigned int *input)
> +{
> +	*input = 0;
> +
> +	return 0;
> +}
> +
> +static int video_s_input(struct file *file, void *fh, unsigned int input)
> +{
> +	return input == 0 ? 0 : -EINVAL;
> +}
> +
> +static const struct v4l2_ioctl_ops msm_vid_ioctl_ops = {
> +	.vidioc_querycap          = video_querycap,
> +	.vidioc_enum_fmt_vid_cap  = video_enum_fmt,
> +	.vidioc_g_fmt_vid_cap     = video_g_fmt,
> +	.vidioc_s_fmt_vid_cap     = video_s_fmt,
> +	.vidioc_try_fmt_vid_cap   = video_try_fmt,
> +	.vidioc_reqbufs           = vb2_ioctl_reqbufs,
> +	.vidioc_querybuf          = vb2_ioctl_querybuf,
> +	.vidioc_qbuf              = vb2_ioctl_qbuf,
> +	.vidioc_dqbuf             = vb2_ioctl_dqbuf,
> +	.vidioc_create_bufs       = vb2_ioctl_create_bufs,
> +	.vidioc_streamon          = vb2_ioctl_streamon,
> +	.vidioc_streamoff         = vb2_ioctl_streamoff,
> +	.vidioc_enum_input        = video_enum_input,
> +	.vidioc_g_input           = video_g_input,
> +	.vidioc_s_input           = video_s_input,
> +};
> +
> +/* ------------------------------------------------------------------------
> + * V4L2 file operations
> + */
> +
> +/*
> + * video_init_format - Helper function to initialize format
> + *
> + * Initialize all pad formats with default values.
> + */
> +static int video_init_format(struct file *file, void *fh)
> +{
> +	struct v4l2_format format;
> +
> +	memset(&format, 0, sizeof(format));
> +	format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
> +
> +	return video_s_fmt(file, fh, &format);

This will set the active format every time you open the device node, I don't 
think that's what you want.

> +}
> +
> +static int video_open(struct file *file)
> +{
> +	struct video_device *vdev = video_devdata(file);
> +	struct camss_video *video = video_drvdata(file);
> +	struct camss_video_fh *handle;
> +	int ret;
> +
> +	handle = kzalloc(sizeof(*handle), GFP_KERNEL);
> +	if (handle == NULL)
> +		return -ENOMEM;
> +
> +	v4l2_fh_init(&handle->vfh, video->vdev);
> +	v4l2_fh_add(&handle->vfh);
> +
> +	handle->video = video;
> +	file->private_data = &handle->vfh;
> +
> +	ret = v4l2_pipeline_pm_use(&vdev->entity, 1);
> +	if (ret < 0) {
> +		dev_err(video->camss->dev, "Failed to power up pipeline\n");
> +		goto error_pm_use;
> +	}
> +
> +	ret = video_init_format(file, &handle->vfh);
> +	if (ret < 0) {
> +		dev_err(video->camss->dev, "Failed to init format\n");
> +		goto error_init_format;
> +	}
> +
> +	return 0;
> +
> +error_init_format:
> +	v4l2_pipeline_pm_use(&vdev->entity, 0);
> +
> +error_pm_use:
> +	v4l2_fh_del(&handle->vfh);
> +	kfree(handle);
> +
> +	return ret;
> +}
> +
> +static int video_release(struct file *file)
> +{
> +	struct video_device *vdev = video_devdata(file);
> +	struct camss_video *video = video_drvdata(file);
> +	struct v4l2_fh *vfh = file->private_data;
> +	struct camss_video_fh *handle = container_of(vfh, struct 
camss_video_fh,
> +						     vfh);
> +
> +	vb2_ioctl_streamoff(file, vfh, video->type);
> +
> +	v4l2_pipeline_pm_use(&vdev->entity, 0);
> +
> +	v4l2_fh_del(vfh);
> +	kfree(handle);
> +	file->private_data = NULL;
> +
> +	return 0;
> +}
> +
> +static unsigned int video_poll(struct file *file,
> +				   struct poll_table_struct *wait)
> +{
> +	struct camss_video *video = video_drvdata(file);
> +
> +	return vb2_poll(&video->vb2_q, file, wait);
> +}
> +
> +static int video_mmap(struct file *file, struct vm_area_struct *vma)
> +{
> +	struct camss_video *video = video_drvdata(file);
> +
> +	return vb2_mmap(&video->vb2_q, vma);
> +}
> +
> +static const struct v4l2_file_operations msm_vid_fops = {
> +	.owner          = THIS_MODULE,
> +	.unlocked_ioctl = video_ioctl2,
> +	.open           = video_open,
> +	.release        = video_release,
> +	.poll           = video_poll,
> +	.mmap		= video_mmap,
> +};
> +
> +/* ------------------------------------------------------------------------
> + * CAMSS video core
> + */
> +
> +int msm_video_register(struct camss_video *video, struct v4l2_device
> *v4l2_dev,
> +		       const char *name)
> +{
> +	struct media_pad *pad = &video->pad;
> +	struct video_device *vdev;
> +	struct vb2_queue *q;
> +	int ret;
> +
> +	vdev = video_device_alloc();
> +	if (vdev == NULL) {
> +		dev_err(v4l2_dev->dev, "Failed to allocate video device\n");
> +		return -ENOMEM;
> +	}
> +
> +	video->vdev = vdev;
> +
> +	q = &video->vb2_q;
> +	q->drv_priv = video;
> +	q->mem_ops = &vb2_dma_contig_memops;
> +	q->ops = &msm_video_vb2_q_ops;
> +	q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
> +	q->io_modes = VB2_MMAP;
> +	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
> +	q->buf_struct_size = sizeof(struct camss_buffer);
> +	q->dev = video->camss->dev;
> +	ret = vb2_queue_init(q);
> +	if (ret < 0) {
> +		dev_err(v4l2_dev->dev, "Failed to init vb2 queue\n");
> +		return ret;
> +	}
> +
> +	pad->flags = MEDIA_PAD_FL_SINK;
> +	ret = media_entity_pads_init(&vdev->entity, 1, pad);
> +	if (ret < 0) {
> +		dev_err(v4l2_dev->dev, "Failed to init video entity\n");
> +		goto error_media_init;
> +	}
> +
> +	vdev->fops = &msm_vid_fops;
> +	vdev->ioctl_ops = &msm_vid_ioctl_ops;
> +	vdev->release = video_device_release;

This will result in a race condition between device unbind and userspace 
access to the video node. You should instead refcount the top-level camss data 
structure, and provide a custom release function here that decrements the 
refcount. If you do that you can embed your struct video_device in struct 
camss_video instead of allocating it dynamically. The media_entity_cleanup() 
call will have to move from msm_video_unregister() to the release operation 
for your top-level structure.

To test this race condition, perform the following steps:

- Configure the pipeline
- Open the video device node, configure it and start streaming
- Unbind the device through the sysfs unbind attribute of the driver
- Stop streaming

You'll likely get an oops with you current version.

There are unfortunately very few drivers implementing this correctly 
(including drivers I wrote :-/). A good (even if slightly complex) example is 
uvcvideo.

> +	vdev->v4l2_dev = v4l2_dev;
> +	vdev->vfl_dir = VFL_DIR_RX;
> +	vdev->queue = &video->vb2_q;
> +	strlcpy(vdev->name, name, sizeof(vdev->name));
> +
> +	ret = video_register_device(vdev, VFL_TYPE_GRABBER, -1);
> +	if (ret < 0) {
> +		dev_err(v4l2_dev->dev, "Failed to register video device\n");
> +		goto error_video_register;
> +	}
> +
> +	video_set_drvdata(vdev, video);
> +
> +	return 0;
> +
> +error_video_register:
> +	media_entity_cleanup(&vdev->entity);
> +error_media_init:
> +	vb2_queue_release(&video->vb2_q);
> +
> +	return ret;
> +}
> +
> +void msm_video_unregister(struct camss_video *video)
> +{
> +	video_unregister_device(video->vdev);
> +	media_entity_cleanup(&video->vdev->entity);
> +	vb2_queue_release(&video->vb2_q);

If you use vb2_fop_release() as Hans proposed you can remove the 
vb2_queue_release() call here.

> +}
> diff --git a/drivers/media/platform/qcom/camss-8x16/video.h
> b/drivers/media/platform/qcom/camss-8x16/video.h new file mode 100644
> index 0000000..5ab5929d
> --- /dev/null
> +++ b/drivers/media/platform/qcom/camss-8x16/video.h
> @@ -0,0 +1,67 @@
> +/*
> + * video.h
> + *
> + * Qualcomm MSM Camera Subsystem - V4L2 device node
> + *
> + * Copyright (c) 2013-2015, The Linux Foundation. All rights reserved.
> + * Copyright (C) 2015-2016 Linaro Ltd.
> + *
> + * This program is free software; you can redistribute it and/or modify
> + * it under the terms of the GNU General Public License version 2 and
> + * only 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.
> + */
> +#ifndef QC_MSM_CAMSS_VIDEO_H
> +#define QC_MSM_CAMSS_VIDEO_H
> +
> +#include <linux/videodev2.h>
> +#include <media/media-entity.h>
> +#include <media/v4l2-dev.h>
> +#include <media/v4l2-device.h>
> +#include <media/v4l2-fh.h>
> +#include <media/v4l2-mediabus.h>
> +#include <media/videobuf2-v4l2.h>
> +
> +#define camss_video_call(f, op, args...)			\
> +	(!(f) ? -ENODEV : (((f)->ops && (f)->ops->op) ? \
> +			    (f)->ops->op((f), ##args) : -ENOIOCTLCMD))
> +
> +struct camss_buffer {
> +	struct vb2_v4l2_buffer vb;
> +	dma_addr_t addr;
> +	struct list_head queue;
> +};
> +
> +struct camss_video;
> +
> +struct camss_video_ops {
> +	int (*queue_buffer)(struct camss_video *vid, struct camss_buffer 
*buf);
> +	int (*flush_buffers)(struct camss_video *vid);
> +};
> +
> +struct camss_video {
> +	struct camss *camss;
> +	struct vb2_queue vb2_q;
> +	struct video_device *vdev;
> +	struct media_pad pad;
> +	struct v4l2_format active_fmt;
> +	enum v4l2_buf_type type;
> +	struct media_pipeline pipe;
> +	struct camss_video_ops *ops;

You can make this const.

> +};
> +
> +struct camss_video_fh {
> +	struct v4l2_fh vfh;
> +	struct camss_video *video;
> +};

If there's nothing else to be stored here you don't need a custom struct 
camss_video_fh. You can use struct v4l2_fh and retrieve the camss_video 
instance with a container_of on vfh->vdev (provided you embed the video_device 
instance in struct camss_video as I proposed above).

> +
> +int msm_video_register(struct camss_video *video, struct v4l2_device
> *v4l2_dev,
> +		       const char *name);
> +
> +void msm_video_unregister(struct camss_video *video);
> +
> +#endif /* QC_MSM_CAMSS_VIDEO_H */

-- 
Regards,

Laurent Pinchart

Re: [PATCH 08/10] media: camss: Add files which handle the video device nodes

[Todor Tomov]

Hi Laurent,

Thank you for the detailed review.

On 12/05/2016 05:22 PM, Laurent Pinchart wrote:
> Hi Todor,
> 
> Thank you for the patch.
> 
> On Friday 25 Nov 2016 16:57:20 Todor Tomov wrote:
>> These files handle the video device nodes of the camss driver.
> 
> camss is a quite generic, I'm a bit concerned about claiming that acronym in 
> the global kernel namespace. Would it be too long if we prefixed symbols with 
> msm_camss instead ?

Ok. Are you concerned about camss_enable_clocks() and camss_disable_clocks() or
you have something else in mind too?

> 
>> Signed-off-by: Todor Tomov <todor.tomov@linaro.org>
>> ---
>>  drivers/media/platform/qcom/camss-8x16/video.c | 597 ++++++++++++++++++++++
>>  drivers/media/platform/qcom/camss-8x16/video.h |  67 +++
>>  2 files changed, 664 insertions(+)
>>  create mode 100644 drivers/media/platform/qcom/camss-8x16/video.c
>>  create mode 100644 drivers/media/platform/qcom/camss-8x16/video.h
>>
>> diff --git a/drivers/media/platform/qcom/camss-8x16/video.c
>> b/drivers/media/platform/qcom/camss-8x16/video.c new file mode 100644
>> index 0000000..0bf8ea9
>> --- /dev/null
>> +++ b/drivers/media/platform/qcom/camss-8x16/video.c
>> @@ -0,0 +1,597 @@
>> +/*
>> + * video.c
>> + *
>> + * Qualcomm MSM Camera Subsystem - V4L2 device node
>> + *
>> + * Copyright (c) 2013-2015, The Linux Foundation. All rights reserved.
>> + * Copyright (C) 2015-2016 Linaro Ltd.
>> + *
>> + * This program is free software; you can redistribute it and/or modify
>> + * it under the terms of the GNU General Public License version 2 and
>> + * only 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.
>> + */
>> +#include <media/media-entity.h>
>> +#include <media/v4l2-dev.h>
>> +#include <media/v4l2-device.h>
>> +#include <media/v4l2-ioctl.h>
>> +#include <media/v4l2-mc.h>
>> +#include <media/videobuf-core.h>
>> +#include <media/videobuf2-dma-contig.h>
>> +
>> +#include "video.h"
>> +#include "camss.h"
>> +
>> +/*
>> + * struct format_info - ISP media bus format information
>> + * @code: V4L2 media bus format code
>> + * @pixelformat: V4L2 pixel format FCC identifier
>> + * @bpp: Bits per pixel when stored in memory
>> + */
>> +static const struct format_info {
>> +	u32 code;
>> +	u32 pixelformat;
>> +	unsigned int bpp;
>> +} formats[] = {
>> +	{ MEDIA_BUS_FMT_UYVY8_2X8, V4L2_PIX_FMT_UYVY, 16 },
>> +	{ MEDIA_BUS_FMT_VYUY8_2X8, V4L2_PIX_FMT_VYUY, 16 },
>> +	{ MEDIA_BUS_FMT_YUYV8_2X8, V4L2_PIX_FMT_YUYV, 16 },
>> +	{ MEDIA_BUS_FMT_YVYU8_2X8, V4L2_PIX_FMT_YVYU, 16 },
>> +	{ MEDIA_BUS_FMT_SBGGR8_1X8, V4L2_PIX_FMT_SBGGR8, 8 },
>> +	{ MEDIA_BUS_FMT_SGBRG8_1X8, V4L2_PIX_FMT_SGBRG8, 8 },
>> +	{ MEDIA_BUS_FMT_SGRBG8_1X8, V4L2_PIX_FMT_SGRBG8, 8 },
>> +	{ MEDIA_BUS_FMT_SRGGB8_1X8, V4L2_PIX_FMT_SRGGB8, 8 },
>> +	{ MEDIA_BUS_FMT_SBGGR10_1X10, V4L2_PIX_FMT_SBGGR10P, 10 },
>> +	{ MEDIA_BUS_FMT_SGBRG10_1X10, V4L2_PIX_FMT_SGBRG10P, 10 },
>> +	{ MEDIA_BUS_FMT_SGRBG10_1X10, V4L2_PIX_FMT_SGRBG10P, 10 },
>> +	{ MEDIA_BUS_FMT_SRGGB10_1X10, V4L2_PIX_FMT_SRGGB10P, 10 },
>> +	{ MEDIA_BUS_FMT_SBGGR12_1X12, V4L2_PIX_FMT_SRGGB12P, 12 },
>> +	{ MEDIA_BUS_FMT_SGBRG12_1X12, V4L2_PIX_FMT_SGBRG12P, 12 },
>> +	{ MEDIA_BUS_FMT_SGRBG12_1X12, V4L2_PIX_FMT_SGRBG12P, 12 },
>> +	{ MEDIA_BUS_FMT_SRGGB12_1X12, V4L2_PIX_FMT_SRGGB12P, 12 }
>> +};
>> +
>> +/*
>> ---------------------------------------------------------------------------
>> -- + * Helper functions
>> + */
>> +
>> +/*
>> + * video_mbus_to_pix - Convert v4l2_mbus_framefmt to v4l2_pix_format
>> + * @mbus: v4l2_mbus_framefmt format (input)
>> + * @pix: v4l2_pix_format format (output)
>> + *
>> + * Fill the output pix structure with information from the input mbus
>> format.
>> + *
>> + * Return 0 on success or a negative error code otherwise
>> + */
>> +static unsigned int video_mbus_to_pix(const struct v4l2_mbus_framefmt
>> *mbus,
>> +				      struct v4l2_pix_format *pix)
>> +{
>> +	unsigned int i;
>> +
>> +	memset(pix, 0, sizeof(*pix));
>> +	pix->width = mbus->width;
>> +	pix->height = mbus->height;
>> +
>> +	for (i = 0; i < ARRAY_SIZE(formats); ++i) {
>> +		if (formats[i].code == mbus->code)
>> +			break;
>> +	}
>> +
>> +	if (WARN_ON(i == ARRAY_SIZE(formats)))
>> +		return -EINVAL;
>> +
>> +	pix->pixelformat = formats[i].pixelformat;
>> +	pix->bytesperline = pix->width * formats[i].bpp / 8;
>> +	pix->bytesperline = ALIGN(pix->bytesperline, 8);
> 
> Does the hardware support padding at the end of lines ? If so it would be 
> useful to use the maximum of the computed and requested by userspace values 
> (up to the maximum size of the padding supported by the hardware).
> 
>> +	pix->sizeimage = pix->bytesperline * pix->height;
> 
> Similarly, should we support padding at the end of the image ?

Yes, the hardware supports horizontal and vertical padding. More advanced
configuration is needed for this however. I'd prefer to keep the driver
simple for now. Features like this I can add later.

> 
>> +	pix->colorspace = mbus->colorspace;
>> +	pix->field = mbus->field;
>> +
>> +	return 0;
>> +}
>> +
>> +static struct v4l2_subdev *video_remote_subdev(struct camss_video *video,
>> +					       u32 *pad)
>> +{
>> +	struct media_pad *remote;
>> +
>> +	remote = media_entity_remote_pad(&video->pad);
>> +
>> +	if (!remote || !is_media_entity_v4l2_subdev(remote->entity))
>> +		return NULL;
>> +
>> +	if (pad)
>> +		*pad = remote->index;
>> +
>> +	return media_entity_to_v4l2_subdev(remote->entity);
>> +}
>> +
>> +static int video_get_subdev_format(struct camss_video *video,
>> +				   struct v4l2_format *format)
>> +{
>> +	struct v4l2_subdev_format fmt;
>> +	struct v4l2_subdev *subdev;
>> +	u32 pad;
>> +	int ret;
>> +
>> +	subdev = video_remote_subdev(video, &pad);
>> +	if (subdev == NULL)
>> +		return -EINVAL;
>> +
>> +	fmt.pad = pad;
>> +	fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
>> +
>> +	ret = v4l2_subdev_call(subdev, pad, get_fmt, NULL, &fmt);
>> +	if (ret)
>> +		return ret;
>> +
>> +	format->type = video->type;
>> +	return video_mbus_to_pix(&fmt.format, &format->fmt.pix);
>> +}
>> +
>> +/* ------------------------------------------------------------------------
>> + * Video queue operations
>> + */
>> +
>> +static int video_queue_setup(struct vb2_queue *q,
>> +	unsigned int *num_buffers, unsigned int *num_planes,
>> +	unsigned int sizes[], struct device *alloc_devs[])
>> +{
>> +	struct camss_video *video = vb2_get_drv_priv(q);
>> +
>> +	if (*num_planes) {
>> +		if (*num_planes != 1)
>> +			return -EINVAL;
>> +
>> +		if (sizes[0] < video->active_fmt.fmt.pix.sizeimage)
>> +			return -EINVAL;
>> +
>> +		return 0;
>> +	}
>> +
>> +	*num_planes = 1;
>> +
>> +	sizes[0] = video->active_fmt.fmt.pix.sizeimage;
>> +
>> +	return 0;
>> +}
>> +
>> +static int video_buf_prepare(struct vb2_buffer *vb)
>> +{
>> +	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
>> +	struct camss_video *video = vb2_get_drv_priv(vb->vb2_queue);
>> +	struct camss_buffer *buffer = container_of(vbuf, struct camss_buffer,
>> +						   vb);
> 
> You can define a static inline function to wrap this container_of() as it's 
> used in multiple places in this file.

I can see it on two places only so it might be too early for an inline function.
If more occurrences are added I'll add a function.

> 
>> +
>> +	vb2_set_plane_payload(vb, 0, video->active_fmt.fmt.pix.sizeimage);
>> +	if (vb2_get_plane_payload(vb, 0) > vb2_plane_size(vb, 0))
> 
> Wouldn't it be more efficient to compare video->active_fmt.fmt.pix.sizeimage 
> instead of vb2_get_plane_payload(vb, 0) ? The two should be identical as 
> you've just called vb2_set_plane_payload(). I would also move the 
> vb2_set_plane_payload() call after the error check.

Yes, I'll do these.

> 
>> +		return -EINVAL;
>> +
>> +	vbuf->field = V4L2_FIELD_NONE;
>> +
>> +	buffer->addr = vb2_dma_contig_plane_dma_addr(vb, 0);
>> +
>> +	return 0;
>> +}
>> +
>> +static void video_buf_queue(struct vb2_buffer *vb)
>> +{
>> +	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
>> +	struct camss_video *video = vb2_get_drv_priv(vb->vb2_queue);
>> +	struct camss_buffer *buffer = container_of(vbuf, struct camss_buffer,
>> +						   vb);
>> +
>> +	camss_video_call(video, queue_buffer, buffer);
> 
> Should I assume that this abstraction means you'll add support for more than 
> the RDI in the future ? ;-)

There might be more but it is not connected with this abstraction :)

> 
> I'd remove the camss_video_call() macro and use
> 
> 	video->ops->queue_buffer(video, buffer);
> 
> directly as it's easier to follow, and we don't need to test video->ops && 
> video->ops->op as all operations should always be provided.

Ok.

> 
>> +}
>> +
>> +
> 
> A single blank line is enough.

Yes.

> 
>> +static int video_check_format(struct camss_video *video)
>> +{
>> +	struct v4l2_pix_format *pix = &video->active_fmt.fmt.pix;
>> +	struct v4l2_format format;
>> +	int ret;
>> +
>> +	ret = video_get_subdev_format(video, &format);
>> +	if (ret < 0)
>> +		return ret;
>> +
>> +	if (pix->pixelformat != format.fmt.pix.pixelformat ||
>> +	    pix->height != format.fmt.pix.height ||
>> +	    pix->width != format.fmt.pix.width ||
>> +	    pix->bytesperline != format.fmt.pix.bytesperline ||
>> +	    pix->sizeimage != format.fmt.pix.sizeimage ||
>> +	    pix->field != format.fmt.pix.field)
>> +		return -EINVAL;
>> +
>> +	return 0;
>> +}
>> +
>> +static int video_start_streaming(struct vb2_queue *q, unsigned int count)
>> +{
>> +	struct camss_video *video = vb2_get_drv_priv(q);
>> +	struct video_device *vdev = video->vdev;
>> +	struct media_entity *entity;
>> +	struct media_pad *pad;
>> +	struct v4l2_subdev *subdev;
>> +	int ret;
>> +
>> +	ret = media_entity_pipeline_start(&vdev->entity, &video->pipe);
>> +	if (ret < 0)
>> +		return ret;
>> +
>> +	ret = video_check_format(video);
>> +	if (ret < 0)
>> +		goto error;
>> +
>> +	entity = &vdev->entity;
>> +	while (1) {
>> +		pad = &entity->pads[0];
>> +		if (!(pad->flags & MEDIA_PAD_FL_SINK))
>> +			break;
>> +
>> +		pad = media_entity_remote_pad(pad);
>> +		if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
>> +			break;
>> +
>> +		entity = pad->entity;
>> +		subdev = media_entity_to_v4l2_subdev(entity);
>> +
>> +		ret = v4l2_subdev_call(subdev, video, s_stream, 1);
>> +		if (ret < 0 && ret != -ENOIOCTLCMD)
>> +			goto error;
>> +	}
>> +
>> +	return 0;
>> +
>> +error:
>> +	media_entity_pipeline_stop(&vdev->entity);
>> +
>> +	return ret;
>> +}
>> +
>> +static void video_stop_streaming(struct vb2_queue *q)
>> +{
>> +	struct camss_video *video = vb2_get_drv_priv(q);
>> +	struct video_device *vdev = video->vdev;
>> +	struct media_entity *entity;
>> +	struct media_pad *pad;
>> +	struct v4l2_subdev *subdev;
>> +	struct v4l2_subdev *subdev_vfe;
>> +
>> +	entity = &vdev->entity;
>> +	while (1) {
>> +		pad = &entity->pads[0];
>> +		if (!(pad->flags & MEDIA_PAD_FL_SINK))
>> +			break;
>> +
>> +		pad = media_entity_remote_pad(pad);
>> +		if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
>> +			break;
>> +
>> +		entity = pad->entity;
>> +		subdev = media_entity_to_v4l2_subdev(entity);
>> +
>> +		if (strstr(subdev->name, "vfe")) {
>> +			subdev_vfe = subdev;
>> +		} else if (strstr(subdev->name, "ispif")) {
>> +			v4l2_subdev_call(subdev, video, s_stream, 0);
>> +			v4l2_subdev_call(subdev_vfe, video, s_stream, 0);
>> +		} else {
>> +			v4l2_subdev_call(subdev, video, s_stream, 0);
>> +		}
>> +	}
>> +
>> +	media_entity_pipeline_stop(&vdev->entity);
>> +
>> +	camss_video_call(video, flush_buffers);
>> +}
>> +
>> +static struct vb2_ops msm_video_vb2_q_ops = {
> 
> You can make this static const.

Ok.

> 
>> +	.queue_setup     = video_queue_setup,
>> +	.buf_prepare     = video_buf_prepare,
>> +	.buf_queue       = video_buf_queue,
>> +	.start_streaming = video_start_streaming,
>> +	.stop_streaming  = video_stop_streaming,
>> +};
>> +
>> +/* ------------------------------------------------------------------------
>> + * V4L2 ioctls
>> + */
>> +
>> +static int video_querycap(struct file *file, void *fh,
>> +			  struct v4l2_capability *cap)
>> +{
>> +	strlcpy(cap->driver, "qcom-camss", sizeof(cap->driver));
>> +	strlcpy(cap->card, "Qualcomm Camera Subsystem", sizeof(cap->card));
>> +	strlcpy(cap->bus_info, "platform:qcom-camss", sizeof(cap->bus_info));
> 
> 	snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s",
> 		 dev_name(...));
> 
> would be better.

Ok.

> 
>> +	cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING |
>> +							V4L2_CAP_DEVICE_CAPS;
>> +	cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
>> +
>> +	return 0;
>> +}
>> +
>> +static int video_enum_fmt(struct file *file, void *fh, struct v4l2_fmtdesc
>> *f)
>> +{
>> +	struct camss_video *video = video_drvdata(file);
>> +	struct v4l2_format format;
>> +	int ret;
>> +
>> +	if (f->type != video->type)
>> +		return -EINVAL;
>> +
>> +	if (f->index)
>> +		return -EINVAL;
>> +
>> +	ret = video_get_subdev_format(video, &format);
>> +	if (ret < 0)
>> +		return ret;
>> +
>> +	f->pixelformat = format.fmt.pix.pixelformat;
>> +
>> +	return 0;
>> +}
>> +
>> +static int video_g_fmt(struct file *file, void *fh, struct v4l2_format *f)
>> +{
>> +	struct camss_video *video = video_drvdata(file);
>> +
>> +	if (f->type != video->type)
>> +		return -EINVAL;
>> +
>> +	*f = video->active_fmt;
>> +
>> +	return 0;
>> +}
>> +
>> +static int video_s_fmt(struct file *file, void *fh, struct v4l2_format *f)
>> +{
>> +	struct camss_video *video = video_drvdata(file);
>> +	int ret;
>> +
>> +	if (f->type != video->type)
>> +		return -EINVAL;
>> +
>> +	ret = video_get_subdev_format(video, f);
>> +	if (ret < 0)
>> +		return ret;
>> +
>> +	video->active_fmt = *f;
>> +
>> +	return 0;
>> +}
>> +
>> +static int video_try_fmt(struct file *file, void *fh, struct v4l2_format
>> *f) +{
>> +	struct camss_video *video = video_drvdata(file);
>> +
>> +	if (f->type != video->type)
>> +		return -EINVAL;
>> +
>> +	return video_get_subdev_format(video, f);
>> +}
>> +
>> +static int video_enum_input(struct file *file, void *fh,
>> +			    struct v4l2_input *input)
>> +{
>> +	if (input->index > 0)
>> +		return -EINVAL;
>> +
>> +	strlcpy(input->name, "camera", sizeof(input->name));
>> +	input->type = V4L2_INPUT_TYPE_CAMERA;
>> +
>> +	return 0;
>> +}
>> +
>> +static int video_g_input(struct file *file, void *fh, unsigned int *input)
>> +{
>> +	*input = 0;
>> +
>> +	return 0;
>> +}
>> +
>> +static int video_s_input(struct file *file, void *fh, unsigned int input)
>> +{
>> +	return input == 0 ? 0 : -EINVAL;
>> +}
>> +
>> +static const struct v4l2_ioctl_ops msm_vid_ioctl_ops = {
>> +	.vidioc_querycap          = video_querycap,
>> +	.vidioc_enum_fmt_vid_cap  = video_enum_fmt,
>> +	.vidioc_g_fmt_vid_cap     = video_g_fmt,
>> +	.vidioc_s_fmt_vid_cap     = video_s_fmt,
>> +	.vidioc_try_fmt_vid_cap   = video_try_fmt,
>> +	.vidioc_reqbufs           = vb2_ioctl_reqbufs,
>> +	.vidioc_querybuf          = vb2_ioctl_querybuf,
>> +	.vidioc_qbuf              = vb2_ioctl_qbuf,
>> +	.vidioc_dqbuf             = vb2_ioctl_dqbuf,
>> +	.vidioc_create_bufs       = vb2_ioctl_create_bufs,
>> +	.vidioc_streamon          = vb2_ioctl_streamon,
>> +	.vidioc_streamoff         = vb2_ioctl_streamoff,
>> +	.vidioc_enum_input        = video_enum_input,
>> +	.vidioc_g_input           = video_g_input,
>> +	.vidioc_s_input           = video_s_input,
>> +};
>> +
>> +/* ------------------------------------------------------------------------
>> + * V4L2 file operations
>> + */
>> +
>> +/*
>> + * video_init_format - Helper function to initialize format
>> + *
>> + * Initialize all pad formats with default values.
>> + */
>> +static int video_init_format(struct file *file, void *fh)
>> +{
>> +	struct v4l2_format format;
>> +
>> +	memset(&format, 0, sizeof(format));
>> +	format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
>> +
>> +	return video_s_fmt(file, fh, &format);
> 
> This will set the active format every time you open the device node, I don't 
> think that's what you want.

Well, actually this is what I wanted. I wanted to keep in sync the pixel format
on the video node and the media bus format on the subdev node (i.e. the pixel
format will be always correct for the current media bus format). For the current
version there is a direct correspondence between the pixel format and the media
format so this will work I think. For the future there might be multiple pixel
formats for one media bus format and a second open of the video node could reset
the pixel format to unwanted value so this will need a change. I'm wondering about
(and still not able to find) a good moment/event when to perform the initialization
of the format on the video node. As it gets the current format from the subdev
node, the moment of the registration will be too early as the media link is still
not created. But after that I couldn't find a suitable callback/event where to do
it. If you can share any idea about this, please do :)

> 
>> +}
>> +
>> +static int video_open(struct file *file)
>> +{
>> +	struct video_device *vdev = video_devdata(file);
>> +	struct camss_video *video = video_drvdata(file);
>> +	struct camss_video_fh *handle;
>> +	int ret;
>> +
>> +	handle = kzalloc(sizeof(*handle), GFP_KERNEL);
>> +	if (handle == NULL)
>> +		return -ENOMEM;
>> +
>> +	v4l2_fh_init(&handle->vfh, video->vdev);
>> +	v4l2_fh_add(&handle->vfh);
>> +
>> +	handle->video = video;
>> +	file->private_data = &handle->vfh;
>> +
>> +	ret = v4l2_pipeline_pm_use(&vdev->entity, 1);
>> +	if (ret < 0) {
>> +		dev_err(video->camss->dev, "Failed to power up pipeline\n");
>> +		goto error_pm_use;
>> +	}
>> +
>> +	ret = video_init_format(file, &handle->vfh);
>> +	if (ret < 0) {
>> +		dev_err(video->camss->dev, "Failed to init format\n");
>> +		goto error_init_format;
>> +	}
>> +
>> +	return 0;
>> +
>> +error_init_format:
>> +	v4l2_pipeline_pm_use(&vdev->entity, 0);
>> +
>> +error_pm_use:
>> +	v4l2_fh_del(&handle->vfh);
>> +	kfree(handle);
>> +
>> +	return ret;
>> +}
>> +
>> +static int video_release(struct file *file)
>> +{
>> +	struct video_device *vdev = video_devdata(file);
>> +	struct camss_video *video = video_drvdata(file);
>> +	struct v4l2_fh *vfh = file->private_data;
>> +	struct camss_video_fh *handle = container_of(vfh, struct 
> camss_video_fh,
>> +						     vfh);
>> +
>> +	vb2_ioctl_streamoff(file, vfh, video->type);
>> +
>> +	v4l2_pipeline_pm_use(&vdev->entity, 0);
>> +
>> +	v4l2_fh_del(vfh);
>> +	kfree(handle);
>> +	file->private_data = NULL;
>> +
>> +	return 0;
>> +}
>> +
>> +static unsigned int video_poll(struct file *file,
>> +				   struct poll_table_struct *wait)
>> +{
>> +	struct camss_video *video = video_drvdata(file);
>> +
>> +	return vb2_poll(&video->vb2_q, file, wait);
>> +}
>> +
>> +static int video_mmap(struct file *file, struct vm_area_struct *vma)
>> +{
>> +	struct camss_video *video = video_drvdata(file);
>> +
>> +	return vb2_mmap(&video->vb2_q, vma);
>> +}
>> +
>> +static const struct v4l2_file_operations msm_vid_fops = {
>> +	.owner          = THIS_MODULE,
>> +	.unlocked_ioctl = video_ioctl2,
>> +	.open           = video_open,
>> +	.release        = video_release,
>> +	.poll           = video_poll,
>> +	.mmap		= video_mmap,
>> +};
>> +
>> +/* ------------------------------------------------------------------------
>> + * CAMSS video core
>> + */
>> +
>> +int msm_video_register(struct camss_video *video, struct v4l2_device
>> *v4l2_dev,
>> +		       const char *name)
>> +{
>> +	struct media_pad *pad = &video->pad;
>> +	struct video_device *vdev;
>> +	struct vb2_queue *q;
>> +	int ret;
>> +
>> +	vdev = video_device_alloc();
>> +	if (vdev == NULL) {
>> +		dev_err(v4l2_dev->dev, "Failed to allocate video device\n");
>> +		return -ENOMEM;
>> +	}
>> +
>> +	video->vdev = vdev;
>> +
>> +	q = &video->vb2_q;
>> +	q->drv_priv = video;
>> +	q->mem_ops = &vb2_dma_contig_memops;
>> +	q->ops = &msm_video_vb2_q_ops;
>> +	q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
>> +	q->io_modes = VB2_MMAP;
>> +	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
>> +	q->buf_struct_size = sizeof(struct camss_buffer);
>> +	q->dev = video->camss->dev;
>> +	ret = vb2_queue_init(q);
>> +	if (ret < 0) {
>> +		dev_err(v4l2_dev->dev, "Failed to init vb2 queue\n");
>> +		return ret;
>> +	}
>> +
>> +	pad->flags = MEDIA_PAD_FL_SINK;
>> +	ret = media_entity_pads_init(&vdev->entity, 1, pad);
>> +	if (ret < 0) {
>> +		dev_err(v4l2_dev->dev, "Failed to init video entity\n");
>> +		goto error_media_init;
>> +	}
>> +
>> +	vdev->fops = &msm_vid_fops;
>> +	vdev->ioctl_ops = &msm_vid_ioctl_ops;
>> +	vdev->release = video_device_release;
> 
> This will result in a race condition between device unbind and userspace 
> access to the video node. You should instead refcount the top-level camss data 
> structure, and provide a custom release function here that decrements the 
> refcount. If you do that you can embed your struct video_device in struct 
> camss_video instead of allocating it dynamically. The media_entity_cleanup() 
> call will have to move from msm_video_unregister() to the release operation 
> for your top-level structure.
> 
> To test this race condition, perform the following steps:
> 
> - Configure the pipeline
> - Open the video device node, configure it and start streaming
> - Unbind the device through the sysfs unbind attribute of the driver
> - Stop streaming
> 
> You'll likely get an oops with you current version.
> 
> There are unfortunately very few drivers implementing this correctly 
> (including drivers I wrote :-/). A good (even if slightly complex) example is 
> uvcvideo.

Yes, I have tested with these steps and I have an oops - the camss data struct
is freed on unbind and on stop streaming it is used again.

I have tried to implement it the way you suggested and also refering to the
uvcvideo driver. I'm reference-counting the camss structure and freeing it last
(in the example above - on stop streaming) - this is ok.

However I have to find a way to trigger vb2_ops->stop_streaming on unbind to
stop the hardware and return the vb2 buffers. For this I have added
if (vb2_is_streaming())
	vb2_queue_release()

triggered from platform_driver->remove()

This seems ok to me, what do you think? (Or you can review the code after
I send the new version if it will make more sense)

> 
>> +	vdev->v4l2_dev = v4l2_dev;
>> +	vdev->vfl_dir = VFL_DIR_RX;
>> +	vdev->queue = &video->vb2_q;
>> +	strlcpy(vdev->name, name, sizeof(vdev->name));
>> +
>> +	ret = video_register_device(vdev, VFL_TYPE_GRABBER, -1);
>> +	if (ret < 0) {
>> +		dev_err(v4l2_dev->dev, "Failed to register video device\n");
>> +		goto error_video_register;
>> +	}
>> +
>> +	video_set_drvdata(vdev, video);
>> +
>> +	return 0;
>> +
>> +error_video_register:
>> +	media_entity_cleanup(&vdev->entity);
>> +error_media_init:
>> +	vb2_queue_release(&video->vb2_q);
>> +
>> +	return ret;
>> +}
>> +
>> +void msm_video_unregister(struct camss_video *video)
>> +{
>> +	video_unregister_device(video->vdev);
>> +	media_entity_cleanup(&video->vdev->entity);
>> +	vb2_queue_release(&video->vb2_q);
> 
> If you use vb2_fop_release() as Hans proposed you can remove the 
> vb2_queue_release() call here.

Ok.

> 
>> +}
>> diff --git a/drivers/media/platform/qcom/camss-8x16/video.h
>> b/drivers/media/platform/qcom/camss-8x16/video.h new file mode 100644
>> index 0000000..5ab5929d
>> --- /dev/null
>> +++ b/drivers/media/platform/qcom/camss-8x16/video.h
>> @@ -0,0 +1,67 @@
>> +/*
>> + * video.h
>> + *
>> + * Qualcomm MSM Camera Subsystem - V4L2 device node
>> + *
>> + * Copyright (c) 2013-2015, The Linux Foundation. All rights reserved.
>> + * Copyright (C) 2015-2016 Linaro Ltd.
>> + *
>> + * This program is free software; you can redistribute it and/or modify
>> + * it under the terms of the GNU General Public License version 2 and
>> + * only 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.
>> + */
>> +#ifndef QC_MSM_CAMSS_VIDEO_H
>> +#define QC_MSM_CAMSS_VIDEO_H
>> +
>> +#include <linux/videodev2.h>
>> +#include <media/media-entity.h>
>> +#include <media/v4l2-dev.h>
>> +#include <media/v4l2-device.h>
>> +#include <media/v4l2-fh.h>
>> +#include <media/v4l2-mediabus.h>
>> +#include <media/videobuf2-v4l2.h>
>> +
>> +#define camss_video_call(f, op, args...)			\
>> +	(!(f) ? -ENODEV : (((f)->ops && (f)->ops->op) ? \
>> +			    (f)->ops->op((f), ##args) : -ENOIOCTLCMD))
>> +
>> +struct camss_buffer {
>> +	struct vb2_v4l2_buffer vb;
>> +	dma_addr_t addr;
>> +	struct list_head queue;
>> +};
>> +
>> +struct camss_video;
>> +
>> +struct camss_video_ops {
>> +	int (*queue_buffer)(struct camss_video *vid, struct camss_buffer 
> *buf);
>> +	int (*flush_buffers)(struct camss_video *vid);
>> +};
>> +
>> +struct camss_video {
>> +	struct camss *camss;
>> +	struct vb2_queue vb2_q;
>> +	struct video_device *vdev;
>> +	struct media_pad pad;
>> +	struct v4l2_format active_fmt;
>> +	enum v4l2_buf_type type;
>> +	struct media_pipeline pipe;
>> +	struct camss_video_ops *ops;
> 
> You can make this const.

Ok.

> 
>> +};
>> +
>> +struct camss_video_fh {
>> +	struct v4l2_fh vfh;
>> +	struct camss_video *video;
>> +};
> 
> If there's nothing else to be stored here you don't need a custom struct 
> camss_video_fh. You can use struct v4l2_fh and retrieve the camss_video 
> instance with a container_of on vfh->vdev (provided you embed the video_device 
> instance in struct camss_video as I proposed above).

Ok.

> 
>> +
>> +int msm_video_register(struct camss_video *video, struct v4l2_device
>> *v4l2_dev,
>> +		       const char *name);
>> +
>> +void msm_video_unregister(struct camss_video *video);
>> +
>> +#endif /* QC_MSM_CAMSS_VIDEO_H */
> 

-- 
Best regards,
Todor Tomov

Re: [PATCH 08/10] media: camss: Add files which handle the video device nodes

[Todor Tomov]

Hi Laurent,

On 01/19/2017 12:43 PM, Todor Tomov wrote:
> Hi Laurent,
> 
> Thank you for the detailed review.
> 
> On 12/05/2016 05:22 PM, Laurent Pinchart wrote:
>> Hi Todor,
>>
>> Thank you for the patch.
>>
>> On Friday 25 Nov 2016 16:57:20 Todor Tomov wrote:
>>> These files handle the video device nodes of the camss driver.
>>
>> camss is a quite generic, I'm a bit concerned about claiming that acronym in 
>> the global kernel namespace. Would it be too long if we prefixed symbols with 
>> msm_camss instead ?
> 
> Ok. Are you concerned about camss_enable_clocks() and camss_disable_clocks() or
> you have something else in mind too?

Could you please add more details about this?

> 
>>
>>> Signed-off-by: Todor Tomov <todor.tomov@linaro.org>
>>> ---
>>>  drivers/media/platform/qcom/camss-8x16/video.c | 597 ++++++++++++++++++++++
>>>  drivers/media/platform/qcom/camss-8x16/video.h |  67 +++
>>>  2 files changed, 664 insertions(+)
>>>  create mode 100644 drivers/media/platform/qcom/camss-8x16/video.c
>>>  create mode 100644 drivers/media/platform/qcom/camss-8x16/video.h
>>>
>>> diff --git a/drivers/media/platform/qcom/camss-8x16/video.c
>>> b/drivers/media/platform/qcom/camss-8x16/video.c new file mode 100644
>>> index 0000000..0bf8ea9
>>> --- /dev/null
>>> +++ b/drivers/media/platform/qcom/camss-8x16/video.c

<snip>

>>> +/* ------------------------------------------------------------------------
>>> + * V4L2 file operations
>>> + */
>>> +
>>> +/*
>>> + * video_init_format - Helper function to initialize format
>>> + *
>>> + * Initialize all pad formats with default values.
>>> + */
>>> +static int video_init_format(struct file *file, void *fh)
>>> +{
>>> +	struct v4l2_format format;
>>> +
>>> +	memset(&format, 0, sizeof(format));
>>> +	format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
>>> +
>>> +	return video_s_fmt(file, fh, &format);
>>
>> This will set the active format every time you open the device node, I don't 
>> think that's what you want.
> 
> Well, actually this is what I wanted. I wanted to keep in sync the pixel format
> on the video node and the media bus format on the subdev node (i.e. the pixel
> format will be always correct for the current media bus format). For the current
> version there is a direct correspondence between the pixel format and the media
> format so this will work I think. For the future there might be multiple pixel
> formats for one media bus format and a second open of the video node could reset
> the pixel format to unwanted value so this will need a change. I'm wondering about
> (and still not able to find) a good moment/event when to perform the initialization
> of the format on the video node. As it gets the current format from the subdev
> node, the moment of the registration will be too early as the media link is still
> not created. But after that I couldn't find a suitable callback/event where to do
> it. If you can share any idea about this, please do :)

I still haven't found a better solution for this. If you have something in mind,
please share.

> 
>>
>>> +}
>>> +
>>> +static int video_open(struct file *file)
>>> +{
>>> +	struct video_device *vdev = video_devdata(file);
>>> +	struct camss_video *video = video_drvdata(file);
>>> +	struct camss_video_fh *handle;
>>> +	int ret;
>>> +
>>> +	handle = kzalloc(sizeof(*handle), GFP_KERNEL);
>>> +	if (handle == NULL)
>>> +		return -ENOMEM;
>>> +
>>> +	v4l2_fh_init(&handle->vfh, video->vdev);
>>> +	v4l2_fh_add(&handle->vfh);
>>> +
>>> +	handle->video = video;
>>> +	file->private_data = &handle->vfh;
>>> +
>>> +	ret = v4l2_pipeline_pm_use(&vdev->entity, 1);
>>> +	if (ret < 0) {
>>> +		dev_err(video->camss->dev, "Failed to power up pipeline\n");
>>> +		goto error_pm_use;
>>> +	}
>>> +
>>> +	ret = video_init_format(file, &handle->vfh);
>>> +	if (ret < 0) {
>>> +		dev_err(video->camss->dev, "Failed to init format\n");
>>> +		goto error_init_format;
>>> +	}
>>> +
>>> +	return 0;
>>> +
>>> +error_init_format:
>>> +	v4l2_pipeline_pm_use(&vdev->entity, 0);
>>> +
>>> +error_pm_use:
>>> +	v4l2_fh_del(&handle->vfh);
>>> +	kfree(handle);
>>> +
>>> +	return ret;
>>> +}

-- 
Best regards,
Todor Tomov

[PATCH 09/10] media: camms: Add core files

[Todor Tomov]

These files implement the platform driver code.

Signed-off-by: Todor Tomov <todor.tomov@linaro.org>
---
 drivers/media/platform/qcom/camss-8x16/camss.c | 603 +++++++++++++++++++++++++
 drivers/media/platform/qcom/camss-8x16/camss.h |  93 ++++
 2 files changed, 696 insertions(+)
 create mode 100644 drivers/media/platform/qcom/camss-8x16/camss.c
 create mode 100644 drivers/media/platform/qcom/camss-8x16/camss.h

diff --git a/drivers/media/platform/qcom/camss-8x16/camss.c b/drivers/media/platform/qcom/camss-8x16/camss.c
new file mode 100644
index 0000000..6d9ef1f
--- /dev/null
+++ b/drivers/media/platform/qcom/camss-8x16/camss.c
@@ -0,0 +1,603 @@
+/*
+ * camss.c
+ *
+ * Qualcomm MSM Camera Subsystem - Core
+ *
+ * Copyright (c) 2015, The Linux Foundation. All rights reserved.
+ * Copyright (C) 2015-2016 Linaro Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only 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.
+ */
+#include <linux/clk.h>
+#include <linux/media-bus-format.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/of.h>
+
+#include <media/media-device.h>
+#include <media/v4l2-async.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-mc.h>
+#include <media/v4l2-of.h>
+
+#include "camss.h"
+
+static struct resources csiphy_res[] = {
+	/* CSIPHY0 */
+	{
+		.regulator = { NULL },
+		.clock = { "camss_top_ahb_clk", "ispif_ahb_clk",
+			   "camss_ahb_clk", "csiphy0_timer_clk" },
+		.clock_rate = { 0, 0, 0, 200000000 },
+		.reg = { "csiphy0", "csiphy0_clk_mux" },
+		.interrupt = { "csiphy0" }
+	},
+
+	/* CSIPHY1 */
+	{
+		.regulator = { NULL },
+		.clock = { "camss_top_ahb_clk", "ispif_ahb_clk",
+			   "camss_ahb_clk", "csiphy1_timer_clk" },
+		.clock_rate = { 0, 0, 0, 200000000 },
+		.reg = { "csiphy1", "csiphy1_clk_mux" },
+		.interrupt = { "csiphy1" }
+	}
+};
+
+static struct resources csid_res[] = {
+	/* CSID0 */
+	{
+		.regulator = { "vdda" },
+		.clock = { "camss_top_ahb_clk", "ispif_ahb_clk",
+			   "csi0_ahb_clk", "camss_ahb_clk",
+			   "csi0_clk", "csi0_phy_clk",
+			   "csi0_pix_clk", "csi0_rdi_clk" },
+		.clock_rate = { 0, 0, 0, 0, 200000000, 0, 0, 0 },
+		.reg = { "csid0" },
+		.interrupt = { "csid0" }
+	},
+
+	/* CSID1 */
+	{
+		.regulator = { "vdda" },
+		.clock = { "camss_top_ahb_clk", "ispif_ahb_clk",
+			   "csi1_ahb_clk", "camss_ahb_clk",
+			   "csi1_clk", "csi1_phy_clk",
+			   "csi1_pix_clk", "csi1_rdi_clk" },
+		.clock_rate = { 0, 0, 0, 0, 200000000, 0, 0, 0 },
+		.reg = { "csid1" },
+		.interrupt = { "csid1" }
+	},
+};
+
+static struct resources_ispif ispif_res = {
+	/* ISPIF */
+	.clock = { "camss_top_ahb_clk", "camss_ahb_clk", "ispif_ahb_clk",
+		   "csi0_clk", "csi0_pix_clk", "csi0_rdi_clk",
+		   "csi1_clk", "csi1_pix_clk", "csi1_rdi_clk" },
+	.clock_for_reset = { "camss_vfe_vfe_clk", "camss_csi_vfe_clk" },
+	.reg = { "ispif", "csi_clk_mux" },
+	.interrupt = "ispif"
+
+};
+
+static struct resources vfe_res = {
+	/* VFE0 */
+	.regulator = { NULL },
+	.clock = { "camss_top_ahb_clk", "camss_vfe_vfe_clk",
+		   "camss_csi_vfe_clk", "iface_clk",
+		   "bus_clk", "camss_ahb_clk" },
+	.clock_rate = { 0, 320000000, 0, 0, 0, 0, 0, 0 },
+	.reg = { "vfe0" },
+	.interrupt = { "vfe0" }
+};
+
+/*
+ * camss_enable_clocks - Enable multiple clocks
+ * @nclocks: Number of clocks in clock array
+ * @clock: Clock array
+ * @dev: Device
+ *
+ * Return 0 on success or a negative error code otherwise
+ */
+int camss_enable_clocks(int nclocks, struct clk **clock, struct device *dev)
+{
+	int ret;
+	int i;
+
+	for (i = 0; i < nclocks; i++) {
+		ret = clk_prepare_enable(clock[i]);
+		if (ret) {
+			dev_err(dev, "clock enable failed\n");
+			goto error;
+		}
+	}
+
+	return 0;
+
+error:
+	for (i--; i >= 0; i--)
+		clk_disable_unprepare(clock[i]);
+
+	return ret;
+}
+
+/*
+ * camss_disable_clocks - Disable multiple clocks
+ * @nclocks: Number of clocks in clock array
+ * @clock: Clock array
+ */
+void camss_disable_clocks(int nclocks, struct clk **clock)
+{
+	int i;
+
+	for (i = nclocks - 1; i >= 0; i--)
+		clk_disable_unprepare(clock[i]);
+}
+
+/*
+ * camss_of_parse_endpoint_node - Parse port endpoint node
+ * @dev: Device
+ * @node: Device node to be parsed
+ * @csd: Parsed data from port endpoint node
+ *
+ * Return 0 on success or a negative error code on failure
+ */
+static int camss_of_parse_endpoint_node(struct device *dev,
+					struct device_node *node,
+					struct camss_async_subdev *csd)
+{
+	struct csiphy_lanes_cfg *lncfg = &csd->interface.csi2.lane_cfg;
+	int *settle_cnt = &csd->interface.csi2.settle_cnt;
+	struct v4l2_of_bus_mipi_csi2 *mipi_csi2;
+	struct v4l2_of_endpoint vep;
+	unsigned int i;
+
+	v4l2_of_parse_endpoint(node, &vep);
+
+	csd->interface.csiphy_id = vep.base.port;
+
+	mipi_csi2 = &vep.bus.mipi_csi2;
+	lncfg->clk.pos = mipi_csi2->clock_lane;
+	lncfg->clk.pol = mipi_csi2->lane_polarities[0];
+	lncfg->num_data = mipi_csi2->num_data_lanes;
+
+	lncfg->data = devm_kzalloc(dev, lncfg->num_data * sizeof(*lncfg->data),
+				   GFP_KERNEL);
+	if (!lncfg->data)
+		return -ENOMEM;
+
+	for (i = 0; i < lncfg->num_data; i++) {
+		lncfg->data[i].pos = mipi_csi2->data_lanes[i];
+		lncfg->data[i].pol = mipi_csi2->lane_polarities[i + 1];
+	}
+
+	of_property_read_u32(node, "qcom,settle-cnt", settle_cnt);
+
+	return 0;
+}
+
+/*
+ * camss_of_parse_ports - Parse ports node
+ * @dev: Device
+ * @notifier: v4l2_device notifier data
+ *
+ * Return number of "port" nodes found in "ports" node
+ */
+static int camss_of_parse_ports(struct device *dev,
+				struct v4l2_async_notifier *notifier)
+{
+	struct device_node *node = NULL;
+	unsigned int size, i;
+	int ret;
+
+	while ((node = of_graph_get_next_endpoint(dev->of_node, node)))
+		notifier->num_subdevs++;
+
+	size = sizeof(*notifier->subdevs) * notifier->num_subdevs;
+	notifier->subdevs = devm_kzalloc(dev, size, GFP_KERNEL);
+	if (!notifier->subdevs) {
+		dev_err(dev, "Failed to allocate memory\n");
+		return -ENOMEM;
+	}
+
+	i = 0;
+	while ((node = of_graph_get_next_endpoint(dev->of_node, node))) {
+		struct camss_async_subdev *csd;
+
+		csd = devm_kzalloc(dev, sizeof(*csd), GFP_KERNEL);
+		if (!csd) {
+			of_node_put(node);
+			dev_err(dev, "Failed to allocate memory\n");
+			return -ENOMEM;
+		}
+
+		notifier->subdevs[i++] = &csd->asd;
+
+		ret = camss_of_parse_endpoint_node(dev, node, csd);
+		if (ret < 0) {
+			of_node_put(node);
+			return ret;
+		}
+
+		csd->asd.match.of.node = of_graph_get_remote_port_parent(node);
+		of_node_put(node);
+		if (!csd->asd.match.of.node) {
+			dev_err(dev, "Bad remote port parent\n");
+			return -EINVAL;
+		}
+
+		csd->asd.match_type = V4L2_ASYNC_MATCH_OF;
+	}
+
+	return notifier->num_subdevs;
+}
+
+/*
+ * camss_init_subdevices - Initialize subdev structures and resources
+ * @camss: CAMSS device
+ *
+ * Return 0 on success or a negative error code on failure
+ */
+static int camss_init_subdevices(struct camss *camss)
+{
+	unsigned int i;
+	int ret;
+
+	for (i = 0; i < ARRAY_SIZE(camss->csiphy); i++) {
+		ret = msm_csiphy_subdev_init(&camss->csiphy[i],
+					     &csiphy_res[i], i);
+		if (ret < 0) {
+			dev_err(camss->dev,
+				"Failed to init csiphy%d sub-device\n", i);
+			return ret;
+		}
+	}
+
+	for (i = 0; i < ARRAY_SIZE(camss->csid); i++) {
+		ret = msm_csid_subdev_init(&camss->csid[i],
+					   &csid_res[i], i);
+		if (ret < 0) {
+			dev_err(camss->dev,
+				"Failed to init csid%d sub-device\n", i);
+			return ret;
+		}
+	}
+
+	ret = msm_ispif_subdev_init(&camss->ispif, &ispif_res);
+	if (ret < 0) {
+		dev_err(camss->dev, "Failed to init ispif sub-device\n");
+		return ret;
+	}
+
+	ret = msm_vfe_subdev_init(&camss->vfe, &vfe_res);
+	if (ret < 0) {
+		dev_err(camss->dev, "Fail to init vfe sub-device\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+/*
+ * camss_register_entities - Register subdev nodes and create links
+ * @camss: CAMSS device
+ *
+ * Return 0 on success or a negative error code on failure
+ */
+static int camss_register_entities(struct camss *camss)
+{
+	unsigned int i, j;
+	int ret;
+
+	for (i = 0; i < ARRAY_SIZE(camss->csiphy); i++) {
+		ret = msm_csiphy_register_entity(&camss->csiphy[i],
+						 &camss->v4l2_dev);
+		if (ret < 0) {
+			dev_err(camss->dev,
+				"Failed to register csiphy%d entity\n", i);
+			goto err_reg_csiphy;
+		}
+	}
+
+	for (i = 0; i < ARRAY_SIZE(camss->csid); i++) {
+		ret = msm_csid_register_entity(&camss->csid[i],
+					       &camss->v4l2_dev);
+		if (ret < 0) {
+			dev_err(camss->dev,
+				"Failed to register csid%d entity\n", i);
+			goto err_reg_csid;
+		}
+	}
+
+	ret = msm_ispif_register_entities(&camss->ispif, &camss->v4l2_dev);
+	if (ret < 0) {
+		dev_err(camss->dev, "Failed to register ispif entities\n");
+		goto err_reg_ispif;
+	}
+
+	ret = msm_vfe_register_entities(&camss->vfe, &camss->v4l2_dev);
+	if (ret < 0) {
+		dev_err(camss->dev, "Failed to register vfe entities\n");
+		goto err_reg_vfe;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(camss->csiphy); i++) {
+		for (j = 0; j < ARRAY_SIZE(camss->csid); j++) {
+			ret = media_create_pad_link(
+				&camss->csiphy[i].subdev.entity,
+				MSM_CSIPHY_PAD_SRC,
+				&camss->csid[j].subdev.entity,
+				MSM_CSID_PAD_SINK,
+				0);
+			if (ret < 0) {
+				dev_err(camss->dev,
+					"Failed to link %s->%s entities\n",
+					camss->csiphy[i].subdev.entity.name,
+					camss->csid[j].subdev.entity.name);
+				goto err_link;
+			}
+		}
+	}
+
+	for (i = 0; i < ARRAY_SIZE(camss->csid); i++) {
+		for (j = 0; j < ARRAY_SIZE(camss->ispif.line); j++) {
+			ret = media_create_pad_link(
+				&camss->csid[i].subdev.entity,
+				MSM_CSID_PAD_SRC,
+				&camss->ispif.line[j].subdev.entity,
+				MSM_ISPIF_PAD_SINK,
+				0);
+			if (ret < 0) {
+				dev_err(camss->dev,
+					"Failed to link %s->%s entities\n",
+					camss->csid[i].subdev.entity.name,
+					camss->ispif.line[j].subdev.entity.name
+					);
+				goto err_link;
+			}
+		}
+	}
+
+	for (i = 0; i < ARRAY_SIZE(camss->ispif.line); i++) {
+		for (j = 0; j < ARRAY_SIZE(camss->vfe.line); j++) {
+			ret = media_create_pad_link(
+				&camss->ispif.line[i].subdev.entity,
+				MSM_ISPIF_PAD_SRC,
+				&camss->vfe.line[j].subdev.entity,
+				MSM_VFE_PAD_SINK,
+				0);
+			if (ret < 0) {
+				dev_err(camss->dev,
+					"Failed to link %s->%s entities\n",
+					camss->ispif.line[i].subdev.entity.name,
+					camss->vfe.line[j].subdev.entity.name);
+				goto err_link;
+			}
+		}
+	}
+
+	return 0;
+
+err_link:
+	msm_vfe_unregister_entities(&camss->vfe);
+err_reg_vfe:
+	msm_ispif_unregister_entities(&camss->ispif);
+err_reg_ispif:
+
+	i = ARRAY_SIZE(camss->csid);
+err_reg_csid:
+	for (i--; i >= 0; i--)
+		msm_csid_unregister_entity(&camss->csid[i]);
+
+	i = ARRAY_SIZE(camss->csiphy);
+err_reg_csiphy:
+	for (i--; i >= 0; i--)
+		msm_csiphy_unregister_entity(&camss->csiphy[i]);
+
+	return ret;
+}
+
+/*
+ * camss_unregister_entities - Unregister subdev nodes
+ * @camss: CAMSS device
+ *
+ * Return 0 on success or a negative error code on failure
+ */
+static void camss_unregister_entities(struct camss *camss)
+{
+	unsigned int i;
+
+	for (i = ARRAY_SIZE(camss->csiphy) - 1; i >= 0; i--)
+		msm_csiphy_unregister_entity(&camss->csiphy[i]);
+
+	for (i = ARRAY_SIZE(camss->csid) - 1; i >= 0; i--)
+		msm_csid_unregister_entity(&camss->csid[i]);
+
+	msm_ispif_unregister_entities(&camss->ispif);
+	msm_vfe_unregister_entities(&camss->vfe);
+}
+
+static int camss_subdev_notifier_bound(struct v4l2_async_notifier *async,
+				       struct v4l2_subdev *subdev,
+				       struct v4l2_async_subdev *asd)
+{
+	struct camss *camss = container_of(async, struct camss, notifier);
+	struct camss_async_subdev *csd =
+		container_of(asd, struct camss_async_subdev, asd);
+	u8 id = csd->interface.csiphy_id;
+	struct csiphy_device *csiphy = &camss->csiphy[id];
+
+	csiphy->cfg.csi2 = &csd->interface.csi2;
+	subdev->host_priv = csiphy;
+
+	return 0;
+}
+
+static int camss_subdev_notifier_complete(struct v4l2_async_notifier *async)
+{
+	struct camss *camss = container_of(async, struct camss, notifier);
+	struct v4l2_device *v4l2_dev = &camss->v4l2_dev;
+	struct v4l2_subdev *sd;
+	int ret;
+
+	list_for_each_entry(sd, &v4l2_dev->subdevs, list) {
+		if (sd->host_priv) {
+			struct media_entity *sensor = &sd->entity;
+			struct csiphy_device *csiphy =
+					(struct csiphy_device *) sd->host_priv;
+			struct media_entity *input = &csiphy->subdev.entity;
+			unsigned int i;
+
+			for (i = 0; i < sensor->num_pads; i++) {
+				if (sensor->pads[i].flags & MEDIA_PAD_FL_SOURCE)
+					break;
+			}
+			if (i == sensor->num_pads) {
+				dev_err(camss->dev,
+					"No source pad in external entity\n");
+				return -EINVAL;
+			}
+
+			ret = media_create_pad_link(sensor, i,
+				input, MSM_CSIPHY_PAD_SINK,
+				MEDIA_LNK_FL_IMMUTABLE | MEDIA_LNK_FL_ENABLED);
+			if (ret < 0) {
+				dev_err(camss->dev,
+					"Failed to link %s->%s entities\n",
+					sensor->name, input->name);
+				return ret;
+			}
+		}
+	}
+
+	ret = v4l2_device_register_subdev_nodes(&camss->v4l2_dev);
+	if (ret < 0)
+		return ret;
+
+	return media_device_register(&camss->media_dev);
+}
+
+static const struct media_device_ops camss_media_ops = {
+	.link_notify = v4l2_pipeline_link_notify,
+};
+
+/*
+ * camss_probe - Probe CAMSS platform device
+ * @pdev: Pointer to CAMSS platform device
+ *
+ * Return 0 on success or a negative error code on failure
+ */
+static int camss_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct camss *camss;
+	int ret;
+
+	camss = devm_kzalloc(dev, sizeof(*camss), GFP_KERNEL);
+	if (!camss)
+		return -ENOMEM;
+
+	camss->dev = dev;
+	platform_set_drvdata(pdev, camss);
+
+	ret = camss_of_parse_ports(dev, &camss->notifier);
+	if (ret < 0)
+		return ret;
+
+	ret = camss_init_subdevices(camss);
+	if (ret < 0)
+		return ret;
+
+	camss->media_dev.dev = camss->dev;
+	strlcpy(camss->media_dev.model, "Qualcomm Camera Subsystem",
+		sizeof(camss->media_dev.model));
+	camss->media_dev.ops = &camss_media_ops;
+	media_device_init(&camss->media_dev);
+
+	camss->v4l2_dev.mdev = &camss->media_dev;
+	ret = v4l2_device_register(camss->dev, &camss->v4l2_dev);
+	if (ret < 0) {
+		dev_err(dev, "Failed to register V4L2 device\n");
+		return ret;
+	}
+
+	ret = camss_register_entities(camss);
+	if (ret < 0)
+		goto err_register_entities;
+
+	if (camss->notifier.num_subdevs) {
+		camss->notifier.bound = camss_subdev_notifier_bound;
+		camss->notifier.complete = camss_subdev_notifier_complete;
+
+		ret = v4l2_async_notifier_register(&camss->v4l2_dev,
+						   &camss->notifier);
+		if (ret) {
+			dev_err(dev, "Failed to register async subdev nodes");
+			goto err_register_subdevs;
+		}
+	} else {
+		ret = v4l2_device_register_subdev_nodes(&camss->v4l2_dev);
+		if (ret < 0) {
+			dev_err(dev, "Failed to register subdev nodes");
+			goto err_register_subdevs;
+		}
+	}
+
+	return 0;
+
+err_register_subdevs:
+	camss_unregister_entities(camss);
+err_register_entities:
+	v4l2_device_unregister(&camss->v4l2_dev);
+
+	return ret;
+}
+
+/*
+ * camss_remove - Remove CAMSS platform device
+ * @pdev: Pointer to CAMSS platform device
+ *
+ * Always returns 0.
+ */
+static int camss_remove(struct platform_device *pdev)
+{
+	struct camss *camss = platform_get_drvdata(pdev);
+
+	v4l2_async_notifier_unregister(&camss->notifier);
+	camss_unregister_entities(camss);
+	v4l2_device_unregister(&camss->v4l2_dev);
+	media_device_unregister(&camss->media_dev);
+	media_device_cleanup(&camss->media_dev);
+
+	return 0;
+}
+
+static const struct of_device_id camss_dt_match[] = {
+	{ .compatible = "qcom,8x16-camss" },
+	{ }
+};
+
+MODULE_DEVICE_TABLE(of, camss_dt_match);
+
+static struct platform_driver qcom_camss_driver = {
+	.probe = camss_probe,
+	.remove = camss_remove,
+	.driver = {
+		.name = "qcom-camss",
+		.of_match_table = camss_dt_match,
+	},
+};
+
+module_platform_driver(qcom_camss_driver);
+
+MODULE_ALIAS("platform:qcom-camss");
+MODULE_DESCRIPTION("Qualcomm Camera Subsystem driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/platform/qcom/camss-8x16/camss.h b/drivers/media/platform/qcom/camss-8x16/camss.h
new file mode 100644
index 0000000..c513926
--- /dev/null
+++ b/drivers/media/platform/qcom/camss-8x16/camss.h
@@ -0,0 +1,93 @@
+/*
+ * camss.h
+ *
+ * Qualcomm MSM Camera Subsystem - Core
+ *
+ * Copyright (c) 2015, The Linux Foundation. All rights reserved.
+ * Copyright (C) 2015-2016 Linaro Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only 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.
+ */
+#ifndef QC_MSM_CAMSS_H
+#define QC_MSM_CAMSS_H
+
+#include <media/v4l2-async.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-subdev.h>
+#include <media/media-device.h>
+#include <media/media-entity.h>
+#include <linux/device.h>
+
+#include "csid.h"
+#include "csiphy.h"
+#include "ispif.h"
+#include "vfe.h"
+
+#define CAMSS_CSID_NUM 2
+#define CAMSS_CSIPHY_NUM 2
+
+#define to_camss(ptr_module)	\
+	container_of(ptr_module, struct camss, ptr_module)
+
+#define to_device(ptr_module)	\
+	(to_camss(ptr_module)->dev)
+
+#define module_pointer(ptr_module, index)	\
+	((const struct ptr_module##_device (*)[]) &(ptr_module[-(index)]))
+
+#define to_camss_index(ptr_module, index)	\
+	container_of(module_pointer(ptr_module, index),	\
+		     struct camss, ptr_module)
+
+#define to_device_index(ptr_module, index)	\
+	(to_camss_index(ptr_module, index)->dev)
+
+#define CAMSS_RES_MAX 15
+
+struct resources {
+	char *regulator[CAMSS_RES_MAX];
+	char *clock[CAMSS_RES_MAX];
+	s32 clock_rate[CAMSS_RES_MAX];
+	char *reg[CAMSS_RES_MAX];
+	char *interrupt[CAMSS_RES_MAX];
+};
+
+struct resources_ispif {
+	char *clock[CAMSS_RES_MAX];
+	char *clock_for_reset[CAMSS_RES_MAX];
+	char *reg[CAMSS_RES_MAX];
+	char *interrupt;
+};
+
+struct camss {
+	struct v4l2_device v4l2_dev;
+	struct v4l2_async_notifier notifier;
+	struct media_device media_dev;
+	struct device *dev;
+	struct csiphy_device csiphy[CAMSS_CSIPHY_NUM];
+	struct csid_device csid[CAMSS_CSID_NUM];
+	struct ispif_device ispif;
+	struct vfe_device vfe;
+};
+
+struct camss_camera_interface {
+	u8 csiphy_id;
+	struct csiphy_csi2_cfg csi2;
+};
+
+struct camss_async_subdev {
+	struct camss_camera_interface interface;
+	struct v4l2_async_subdev asd;
+};
+
+int camss_enable_clocks(int nclocks, struct clk **clock, struct device *dev);
+void camss_disable_clocks(int nclocks, struct clk **clock);
+
+#endif /* QC_MSM_CAMSS_H */
-- 
1.9.1