Merge remote-tracking branch 'todor/release/qcomlt-4.4-camss-demo5' into release/qcomlt-4.4

21 files changed, 2797 insertions, 1325 deletions

diff --git a/Documentation/DocBook/media/v4l/pixfmt-srggb12p.xml b/Documentation/DocBook/media/v4l/pixfmt-srggb12p.xml
new file mode 100644
index 000000000000..12f0ac157fea
--- /dev/null
+++ b/Documentation/DocBook/media/v4l/pixfmt-srggb12p.xml
@@ -0,0 +1,103 @@
+    <refentry id="pixfmt-srggb12p">
+      <refmeta>
+	<refentrytitle>V4L2_PIX_FMT_SRGGB12P ('pRCC'),
+	 V4L2_PIX_FMT_SGRBG12P ('pgCC'),
+	 V4L2_PIX_FMT_SGBRG12P ('pGCC'),
+	 V4L2_PIX_FMT_SBGGR12P ('pBCC')
+	 </refentrytitle>
+	&manvol;
+      </refmeta>
+      <refnamediv>
+	<refname id="V4L2-PIX-FMT-SRGGB12P"><constant>V4L2_PIX_FMT_SRGGB12P</constant></refname>
+	<refname id="V4L2-PIX-FMT-SGRBG12P"><constant>V4L2_PIX_FMT_SGRBG12P</constant></refname>
+	<refname id="V4L2-PIX-FMT-SGBRG12P"><constant>V4L2_PIX_FMT_SGBRG12P</constant></refname>
+	<refname id="V4L2-PIX-FMT-SBGGR12P"><constant>V4L2_PIX_FMT_SBGGR12P</constant></refname>
+	<refpurpose>12-bit packed Bayer formats</refpurpose>
+      </refnamediv>
+      <refsect1>
+	<title>Description</title>
+
+	<para>These four pixel formats are packed raw sRGB / Bayer
+	formats with 12 bits per colour. Every four consecutive colour
+	components are packed into 6 bytes. Each of the first 4 bytes
+	contain the 8 high order bits of the pixels, and the fifth and
+	sixth bytes contains the four least significants bits of each
+	pixel, in the same order.</para>
+
+	<para>Each n-pixel row contains n/2 green samples and n/2 blue
+	or red samples, with alternating green-red and green-blue
+	rows. They are conventionally described as GRGR... BGBG...,
+	RGRG... GBGB..., etc. Below is an example of one of these
+	formats:</para>
+
+    <example>
+      <title><constant>V4L2_PIX_FMT_SBGGR12P</constant> 4 &times; 4
+      pixel image</title>
+
+      <formalpara>
+	<title>Byte Order.</title>
+	<para>Each cell is one byte.
+	  <informaltable frame="topbot" colsep="1" rowsep="1">
+	    <tgroup cols="6" align="center">
+	      <colspec align="left" colwidth="2*" />
+	      <tbody valign="top">
+		<row>
+		  <entry>start&nbsp;+&nbsp;0:</entry>
+		  <entry>B<subscript>00high</subscript></entry>
+		  <entry>G<subscript>01high</subscript></entry>
+		  <entry>G<subscript>01low</subscript>(bits 7--4)
+			 B<subscript>00low</subscript>(bits 3--0)
+		  </entry>
+		  <entry>B<subscript>02high</subscript></entry>
+		  <entry>G<subscript>03high</subscript></entry>
+		  <entry>G<subscript>03low</subscript>(bits 7--4)
+			 B<subscript>02low</subscript>(bits 3--0)
+		  </entry>
+		</row>
+		<row>
+		  <entry>start&nbsp;+&nbsp;6:</entry>
+		  <entry>G<subscript>10high</subscript></entry>
+		  <entry>R<subscript>11high</subscript></entry>
+		  <entry>R<subscript>11low</subscript>(bits 7--4)
+			 G<subscript>10low</subscript>(bits 3--0)
+		  </entry>
+		  <entry>G<subscript>12high</subscript></entry>
+		  <entry>R<subscript>13high</subscript></entry>
+		  <entry>R<subscript>13low</subscript>(bits 7--4)
+			 G<subscript>12low</subscript>(bits 3--0)
+		  </entry>
+		</row>
+		<row>
+		  <entry>start&nbsp;+&nbsp;12:</entry>
+		  <entry>B<subscript>20high</subscript></entry>
+		  <entry>G<subscript>21high</subscript></entry>
+		  <entry>G<subscript>21low</subscript>(bits 7--4)
+			 B<subscript>20low</subscript>(bits 3--0)
+		  </entry>
+		  <entry>B<subscript>22high</subscript></entry>
+		  <entry>G<subscript>23high</subscript></entry>
+		  <entry>G<subscript>23low</subscript>(bits 7--4)
+			 B<subscript>22low</subscript>(bits 3--0)
+		  </entry>
+		</row>
+		<row>
+		  <entry>start&nbsp;+&nbsp;18:</entry>
+		  <entry>G<subscript>30high</subscript></entry>
+		  <entry>R<subscript>31high</subscript></entry>
+		  <entry>R<subscript>31low</subscript>(bits 7--4)
+			 G<subscript>30low</subscript>(bits 3--0)
+		  </entry>
+		  <entry>G<subscript>32high</subscript></entry>
+		  <entry>R<subscript>33high</subscript></entry>
+		  <entry>R<subscript>33low</subscript>(bits 7--4)
+			 G<subscript>32low</subscript>(bits 3--0)
+		  </entry>
+		</row>
+	      </tbody>
+	    </tgroup>
+	  </informaltable>
+	</para>
+      </formalpara>
+    </example>
+  </refsect1>
+</refentry>
diff --git a/Documentation/DocBook/media/v4l/pixfmt.xml b/Documentation/DocBook/media/v4l/pixfmt.xml
index d871245d2973..7108853d31a2 100644
--- a/Documentation/DocBook/media/v4l/pixfmt.xml
+++ b/Documentation/DocBook/media/v4l/pixfmt.xml
@@ -1593,6 +1593,7 @@ access the palette, this must be done with ioctls of the Linux framebuffer API.<
     &sub-srggb10alaw8;
     &sub-srggb10dpcm8;
     &sub-srggb12;
+    &sub-srggb12p;
   </section>
 
   <section id="yuv-formats">
diff --git a/Documentation/devicetree/bindings/media/i2c/ov5645.txt b/Documentation/devicetree/bindings/media/i2c/ov5645.txt
index 468cf8306ce3..bcf6dba3342f 100644
--- a/Documentation/devicetree/bindings/media/i2c/ov5645.txt
+++ b/Documentation/devicetree/bindings/media/i2c/ov5645.txt
@@ -8,6 +8,7 @@ Required Properties:
 - compatible: Value should be "ovti,ov5645".
 - clocks: Reference to the xclk clock.
 - clock-names: Should be "xclk".
+- clock-frequency: Frequency of the xclk clock.
 - enable-gpios: Chip enable GPIO. Polarity is GPIO_ACTIVE_HIGH.
 - reset-gpios: Chip reset GPIO. Polarity is GPIO_ACTIVE_LOW.
 - vdddo-supply: Chip digital IO regulator.
@@ -34,6 +35,7 @@ Example:
 
 			clocks = <&clks 200>;
 			clock-names = "xclk";
+			clock-frequency = <23880000>;
 
 			vdddo-supply = <&camera_dovdd_1v8>;
 			vdda-supply = <&camera_avdd_2v8>;
diff --git a/arch/arm64/boot/dts/qcom/apq8016-sbc.dtsi b/arch/arm64/boot/dts/qcom/apq8016-sbc.dtsi
index aeed8161fc05..968d4f05a3b8 100644
--- a/arch/arm64/boot/dts/qcom/apq8016-sbc.dtsi
+++ b/arch/arm64/boot/dts/qcom/apq8016-sbc.dtsi
@@ -260,6 +260,30 @@
 				default-state = "off";
 			};
 		};
+
+		camera_vdddo_1v8: fixedregulator@0 {
+			compatible = "regulator-fixed";
+			regulator-name = "camera_vdddo";
+			regulator-min-microvolt = <1800000>;
+			regulator-max-microvolt = <1800000>;
+			regulator-always-on;
+		};
+
+		camera_vdda_2v8: fixedregulator@1 {
+			compatible = "regulator-fixed";
+			regulator-name = "camera_vdda";
+			regulator-min-microvolt = <2800000>;
+			regulator-max-microvolt = <2800000>;
+			regulator-always-on;
+		};
+
+		camera_vddd_1v5: fixedregulator@2 {
+			compatible = "regulator-fixed";
+			regulator-name = "camera_vddd";
+			regulator-min-microvolt = <1500000>;
+			regulator-max-microvolt = <1500000>;
+			regulator-always-on;
+		};
 	};
 
 	usb2513 {
@@ -293,6 +317,19 @@
 	};
 };
 
+&camss {
+	ports {
+		#address-cells = <1>;
+		#size-cells = <0>;
+		port@0 {
+			reg = <0>;
+		};
+		port@1 {
+			reg = <1>;
+		};
+	};
+};
+
 &spmi_pon {
 	// Overwrite RESETIN_N keyboard scan code
 	linux,code = <KEY_VOLUMEDOWN>;
@@ -386,6 +423,10 @@
 	};
 };
 
+&blsp_i2c6 {
+	status = "ok";
+};
+
 &mdss_dsi0 {
 	status = "ok";
 
diff --git a/arch/arm64/boot/dts/qcom/msm8916.dtsi b/arch/arm64/boot/dts/qcom/msm8916.dtsi
index f6091aafc984..2a04e76fc9e5 100644
--- a/arch/arm64/boot/dts/qcom/msm8916.dtsi
+++ b/arch/arm64/boot/dts/qcom/msm8916.dtsi
@@ -277,7 +277,7 @@
 			reg = <0x4ab000 0x4>;
 		};
 
-		camss@0 {
+		camss: camss@0 {
 			compatible = "qcom,msm-camss";
 
 			reg = <0x1b0ac00 0x200>,
@@ -370,21 +370,12 @@
 			qcom,msm-bus,num-paths = <1>;
 			qcom,msm-bus,vectors-KBps =
 					<29 512 0 0>,
-					<29 512 450000 900000>,
+					<29 512 11000000 11000000>,
 					<29 512 11000 11000>;
 
 			ports {
 				#address-cells = <1>;
 				#size-cells = <0>;
-				port@0 {
-					reg = <0>;
-					csiphy0_ep: endpoint {
-						clock-lanes = <1>;
-						data-lanes = <0 2>;
-						qcom,settle-cnt = <0xe>;
-						remote-endpoint = <&ov5645_ep>;
-					};
-				};
 			};
 		};
 
@@ -633,30 +624,6 @@
 			status = "disabled";
 		};
 
-		camera_vdddo_1v8: fixedregulator@0 {
-			compatible = "regulator-fixed";
-			regulator-name = "camera_vdddo";
-			regulator-min-microvolt = <1800000>;
-			regulator-max-microvolt = <1800000>;
-			regulator-always-on;
-		};
-
-		camera_vdda_2v8: fixedregulator@1 {
-			compatible = "regulator-fixed";
-			regulator-name = "camera_vdda";
-			regulator-min-microvolt = <2800000>;
-			regulator-max-microvolt = <2800000>;
-			regulator-always-on;
-		};
-
-		camera_vddd_1v5: fixedregulator@2 {
-			compatible = "regulator-fixed";
-			regulator-name = "camera_vddd";
-			regulator-min-microvolt = <1500000>;
-			regulator-max-microvolt = <1500000>;
-			regulator-always-on;
-		};
-
 		blsp_i2c6: i2c@78ba000 {
 			compatible = "qcom,i2c-qup-v2.2.1";
 			reg = <0x78ba000 0x1000>;
@@ -670,30 +637,6 @@
 			#address-cells = <1>;
 			#size-cells = <0>;
 			status = "disabled";
-			camera@78 {
-				compatible = "ovti,ov5645";
-				reg = <0x78>;
-
-				enable-gpios = <&msmgpio 34 0>;
-				reset-gpios = <&msmgpio 35 1>;
-				pinctrl-names = "default";
-				pinctrl-0 = <&camera_rear_default>;
-
-				clocks = <&gcc GCC_CAMSS_MCLK0_CLK>;
-				clock-names = "xclk";
-
-				vdddo-supply = <&camera_vdddo_1v8>;
-				vdda-supply = <&camera_vdda_2v8>;
-				vddd-supply = <&camera_vddd_1v5>;
-
-				port {
-					ov5645_ep: endpoint {
-						clock-lanes = <1>;
-						data-lanes = <0 2>;
-						remote-endpoint = <&csiphy0_ep>;
-					};
-				};
-			};
 		};
 
 		sdhc_1: sdhci@07824000 {
diff --git a/drivers/media/i2c/ov5645.c b/drivers/media/i2c/ov5645.c
index 4a7bf2448c16..0bfd672946ca 100644
--- a/drivers/media/i2c/ov5645.c
+++ b/drivers/media/i2c/ov5645.c
@@ -33,6 +33,7 @@
 #include <linux/i2c.h>
 #include <linux/init.h>
 #include <linux/module.h>
+#include <linux/mutex.h>
 #include <linux/of.h>
 #include <linux/of_graph.h>
 #include <linux/regulator/consumer.h>
@@ -42,6 +43,8 @@
 #include <media/v4l2-of.h>
 #include <media/v4l2-subdev.h>
 
+static DEFINE_MUTEX(ov5645_lock);
+
 /* HACKs here! */
 
 #include <../drivers/media/platform/msm/cci/msm_cci.h>
@@ -55,15 +58,13 @@
 #define OV5645_VOLTAGE_DIGITAL_CORE         1500000
 #define OV5645_VOLTAGE_DIGITAL_IO           1800000
 
-#define OV5645_XCLK	23880000
-
 #define OV5645_SYSTEM_CTRL0		0x3008
 #define		OV5645_SYSTEM_CTRL0_START	0x02
 #define		OV5645_SYSTEM_CTRL0_STOP	0x42
-#define OV5645_CHIP_ID_HIGH_REG		0x300A
-#define		OV5645_CHIP_ID_HIGH		0x56
-#define OV5645_CHIP_ID_LOW_REG		0x300B
-#define		OV5645_CHIP_ID_LOW		0x45
+#define OV5645_CHIP_ID_HIGH		0x300A
+#define		OV5645_CHIP_ID_HIGH_BYTE	0x56
+#define OV5645_CHIP_ID_LOW		0x300B
+#define		OV5645_CHIP_ID_LOW_BYTE		0x45
 #define OV5645_AWB_MANUAL_CONTROL	0x3406
 #define		OV5645_AWB_MANUAL_ENABLE	BIT(0)
 #define OV5645_AEC_PK_MANUAL		0x3503
@@ -111,6 +112,8 @@ struct ov5645 {
 	struct v4l2_mbus_framefmt fmt;
 	struct v4l2_rect crop;
 	struct clk *xclk;
+	/* External clock frequency currently supported is 23880000Hz */
+	u32 xclk_freq;
 
 	struct regulator *io_regulator;
 	struct regulator *core_regulator;
@@ -561,7 +564,7 @@ static int ov5645_regulators_enable(struct ov5645 *ov5645)
 	ret = regulator_enable(ov5645->io_regulator);
 	if (ret < 0) {
 		dev_err(ov5645->dev, "set io voltage failed\n");
-		goto exit;
+		return ret;
 	}
 
 	ret = regulator_enable(ov5645->core_regulator);
@@ -582,7 +585,7 @@ err_disable_core:
 	regulator_disable(ov5645->core_regulator);
 err_disable_io:
 	regulator_disable(ov5645->io_regulator);
-exit:
+
 	return ret;
 }
 
@@ -603,14 +606,29 @@ static void ov5645_regulators_disable(struct ov5645 *ov5645)
 		dev_err(ov5645->dev, "io regulator disable failed\n");
 }
 
+static int ov5645_write_reg_to(struct ov5645 *ov5645, u16 reg, u8 val, u16 i2c_addr)
+{
+	int ret;
+
+	ret = msm_cci_ctrl_write(i2c_addr, reg, &val, 1);
+	if (ret < 0)
+		dev_err(ov5645->dev,
+			"%s: write reg error %d on addr 0x%x: reg=0x%x, val=0x%x\n",
+			__func__, ret, i2c_addr, reg, val);
+
+	return ret;
+}
+
 static int ov5645_write_reg(struct ov5645 *ov5645, u16 reg, u8 val)
 {
 	int ret;
+	u16 i2c_addr = ov5645->i2c_client->addr;
 
-	ret = msm_cci_ctrl_write(reg, &val, 1);
+	ret = msm_cci_ctrl_write(i2c_addr, reg, &val, 1);
 	if (ret < 0)
-		dev_err(ov5645->dev, "%s: write reg error %d: reg=%x, val=%x\n",
-			__func__, ret, reg, val);
+		dev_err(ov5645->dev,
+			"%s: write reg error %d on addr 0x%x: reg=0x%x, val=0x%x\n",
+			__func__, ret, i2c_addr, reg, val);
 
 	return ret;
 }
@@ -619,11 +637,13 @@ static int ov5645_read_reg(struct ov5645 *ov5645, u16 reg, u8 *val)
 {
 	u8 tmpval;
 	int ret;
+	u16 i2c_addr = ov5645->i2c_client->addr;
 
-	ret = msm_cci_ctrl_read(reg, &tmpval, 1);
+	ret = msm_cci_ctrl_read(i2c_addr, reg, &tmpval, 1);
 	if (ret < 0) {
-		dev_err(ov5645->dev, "%s: read reg error %d: reg=%x\n",
-			__func__, ret, reg);
+		dev_err(ov5645->dev,
+			"%s: read reg error %d on addr 0x%x: reg=0x%x\n",
+			__func__, ret, i2c_addr, reg);
 		return ret;
 	}
 
@@ -646,8 +666,6 @@ static int ov5645_set_aec_mode(struct ov5645 *ov5645, u32 mode)
 	else /* V4L2_EXPOSURE_MANUAL */
 		val |= OV5645_AEC_MANUAL_ENABLE;
 
-	dev_dbg(ov5645->dev, "%s: mode = %d\n", __func__, mode);
-
 	return ov5645_write_reg(ov5645, OV5645_AEC_PK_MANUAL, val);
 }
 
@@ -665,8 +683,6 @@ static int ov5645_set_agc_mode(struct ov5645 *ov5645, u32 enable)
 	else
 		val |= OV5645_AGC_MANUAL_ENABLE;
 
-	dev_dbg(ov5645->dev, "%s: enable = %d\n", __func__, enable);
-
 	return ov5645_write_reg(ov5645, OV5645_AEC_PK_MANUAL, val);
 }
 
@@ -677,7 +693,7 @@ static int ov5645_set_register_array(struct ov5645 *ov5645,
 	u16 reg;
 	u8 val;
 	u32 i;
-	int ret = 0;
+	int ret;
 
 	for (i = 0; i < num_settings; ++i, ++settings) {
 		reg = settings->reg;
@@ -685,10 +701,10 @@ static int ov5645_set_register_array(struct ov5645 *ov5645,
 
 		ret = ov5645_write_reg(ov5645, reg, val);
 		if (ret < 0)
-			goto err;
+			return ret;
 	}
-err:
-	return ret;
+
+	return 0;
 }
 
 static int ov5645_init(struct ov5645 *ov5645)
@@ -717,9 +733,7 @@ static int ov5645_set_power_on(struct ov5645 *ov5645)
 {
 	int ret;
 
-	dev_dbg(ov5645->dev, "%s: Enter\n", __func__);
-
-	clk_set_rate(ov5645->xclk, OV5645_XCLK);
+	clk_set_rate(ov5645->xclk, ov5645->xclk_freq);
 
 	ret = clk_prepare_enable(ov5645->xclk);
 	if (ret < 0) {
@@ -746,12 +760,8 @@ static int ov5645_set_power_on(struct ov5645 *ov5645)
 
 static void ov5645_set_power_off(struct ov5645 *ov5645)
 {
-	dev_dbg(ov5645->dev, "%s: Enter\n", __func__);
-
-	if (ov5645->rst_gpio)
-		gpiod_set_value_cansleep(ov5645->rst_gpio, 1);
-	if (ov5645->enable_gpio)
-		gpiod_set_value_cansleep(ov5645->enable_gpio, 0);
+	gpiod_set_value_cansleep(ov5645->rst_gpio, 1);
+	gpiod_set_value_cansleep(ov5645->enable_gpio, 0);
 	ov5645_regulators_disable(ov5645);
 	clk_disable_unprepare(ov5645->xclk);
 }
@@ -761,8 +771,6 @@ static int ov5645_s_power(struct v4l2_subdev *sd, int on)
 	struct ov5645 *ov5645 = to_ov5645(sd);
 	int ret = 0;
 
-	dev_dbg(ov5645->dev, "%s: on = %d\n", __func__, on);
-
 	mutex_lock(&ov5645->power_lock);
 
 	if (on) {
@@ -774,6 +782,8 @@ static int ov5645_s_power(struct v4l2_subdev *sd, int on)
 	if (ov5645->power == !on) {
 		/* Power state changes. */
 		if (on) {
+			mutex_lock(&ov5645_lock);
+
 			ret = ov5645_set_power_on(ov5645);
 			if (ret < 0) {
 				dev_err(ov5645->dev, "could not set power %s\n",
@@ -781,6 +791,18 @@ static int ov5645_s_power(struct v4l2_subdev *sd, int on)
 				goto exit;
 			}
 
+			ret = ov5645_write_reg_to(ov5645, 0x3100,
+					       ov5645->i2c_client->addr, 0x78);
+			if (ret < 0) {
+				dev_err(ov5645->dev,
+					"could not change i2c address\n");
+				ov5645_set_power_off(ov5645);
+				mutex_unlock(&ov5645_lock);
+				goto exit;
+			}
+
+			mutex_unlock(&ov5645_lock);
+
 			ret = ov5645_init(ov5645);
 			if (ret < 0) {
 				dev_err(ov5645->dev,
@@ -816,12 +838,13 @@ exit:
 static int ov5645_set_saturation(struct ov5645 *ov5645, s32 value)
 {
 	u32 reg_value = (value * 0x10) + 0x40;
-	int ret = 0;
+	int ret;
 
-	ret |= ov5645_write_reg(ov5645, OV5645_SDE_SAT_U, reg_value);
-	ret |= ov5645_write_reg(ov5645, OV5645_SDE_SAT_V, reg_value);
+	ret = ov5645_write_reg(ov5645, OV5645_SDE_SAT_U, reg_value);
+	if (ret < 0)
+		return ret;
 
-	dev_dbg(ov5645->dev, "%s: value = %d\n", __func__, value);
+	ret = ov5645_write_reg(ov5645, OV5645_SDE_SAT_V, reg_value);
 
 	return ret;
 }
@@ -840,8 +863,6 @@ static int ov5645_set_hflip(struct ov5645 *ov5645, s32 value)
 	else
 		val |= (OV5645_SENSOR_MIRROR);
 
-	dev_dbg(ov5645->dev, "%s: value = %d\n", __func__, value);
-
 	return ov5645_write_reg(ov5645, OV5645_TIMING_TC_REG21, val);
 }
 
@@ -859,8 +880,6 @@ static int ov5645_set_vflip(struct ov5645 *ov5645, s32 value)
 	else
 		val &= ~(OV5645_SENSOR_VFLIP | OV5645_ISP_VFLIP);
 
-	dev_dbg(ov5645->dev, "%s: value = %d\n", __func__, value);
-
 	return ov5645_write_reg(ov5645, OV5645_TIMING_TC_REG20, val);
 }
 
@@ -881,15 +900,13 @@ static int ov5645_set_test_pattern(struct ov5645 *ov5645, s32 value)
 		val &= ~OV5645_TEST_PATTERN_ENABLE;
 	}
 
-	dev_dbg(ov5645->dev, "%s: value = %d\n", __func__, value);
-
 	return ov5645_write_reg(ov5645, OV5645_PRE_ISP_TEST_SETTING_1, val);
 }
 
 static const char * const ov5645_test_pattern_menu[] = {
 	"Disabled",
 	"Vertical Color Bars",
-	"Random Data",
+	"Pseudo-Random Data",
 	"Color Square",
 	"Black Image",
 };
@@ -908,8 +925,6 @@ static int ov5645_set_awb(struct ov5645 *ov5645, s32 enable_auto)
 	else
 		val |= OV5645_AWB_MANUAL_ENABLE;
 
-	dev_dbg(ov5645->dev, "%s: enable_auto = %d\n", __func__, enable_auto);
-
 	return ov5645_write_reg(ov5645, OV5645_AWB_MANUAL_CONTROL, val);
 }
 
@@ -958,6 +973,24 @@ static struct v4l2_ctrl_ops ov5645_ctrl_ops = {
 	.s_ctrl = ov5645_s_ctrl,
 };
 
+static int ov5645_entity_init_cfg(struct v4l2_subdev *subdev,
+				  struct v4l2_subdev_pad_config *cfg)
+{
+	struct v4l2_subdev_format fmt = { 0 };
+	struct ov5645 *ov5645 = to_ov5645(subdev);
+
+	dev_err(ov5645->dev, "%s: Enter\n", __func__);
+
+
+	fmt.which = cfg ? V4L2_SUBDEV_FORMAT_TRY : V4L2_SUBDEV_FORMAT_ACTIVE;
+	fmt.format.width = 1920;
+	fmt.format.height = 1080;
+
+	v4l2_subdev_call(subdev, pad, set_fmt, cfg, &fmt);
+
+	return 0;
+}
+
 static int ov5645_enum_mbus_code(struct v4l2_subdev *sd,
 				 struct v4l2_subdev_pad_config *cfg,
 				 struct v4l2_subdev_mbus_code_enum *code)
@@ -976,7 +1009,7 @@ static int ov5645_enum_frame_size(struct v4l2_subdev *subdev,
 				  struct v4l2_subdev_pad_config *cfg,
 				  struct v4l2_subdev_frame_size_enum *fse)
 {
-	if (fse->index >= OV5645_MODE_MAX)
+	if (fse->index > OV5645_MODE_MAX)
 		return -EINVAL;
 
 	fse->min_width = ov5645_mode_info_data[fse->index].width;
@@ -1062,12 +1095,18 @@ static int ov5645_set_format(struct v4l2_subdev *sd,
 	__crop->width = ov5645_mode_info_data[new_mode].width;
 	__crop->height = ov5645_mode_info_data[new_mode].height;
 
-	ov5645->current_mode = new_mode;
+	if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+		ov5645->current_mode = new_mode;
 
 	__format = __ov5645_get_pad_format(ov5645, cfg, format->pad,
 			format->which);
 	__format->width = __crop->width;
 	__format->height = __crop->height;
+	__format->code = MEDIA_BUS_FMT_UYVY8_2X8;
+	__format->field = V4L2_FIELD_NONE;
+	__format->colorspace = V4L2_COLORSPACE_SRGB;
+
+	format->format = *__format;
 
 	return 0;
 }
@@ -1091,8 +1130,6 @@ static int ov5645_s_stream(struct v4l2_subdev *subdev, int enable)
 	struct ov5645 *ov5645 = to_ov5645(subdev);
 	int ret;
 
-	dev_dbg(ov5645->dev, "%s: enable = %d\n", __func__, enable);
-
 	if (enable) {
 		ret = ov5645_change_mode(ov5645, ov5645->current_mode);
 		if (ret < 0) {
@@ -1153,18 +1190,14 @@ static int ov5645_probe(struct i2c_client *client,
 	u8 chip_id_high, chip_id_low;
 	int ret;
 
+	dev_dbg(dev, "%s: Enter, i2c addr = 0x%x\n", __func__, client->addr);
+
 	ov5645 = devm_kzalloc(dev, sizeof(struct ov5645), GFP_KERNEL);
 	if (!ov5645)
 		return -ENOMEM;
 
 	ov5645->i2c_client = client;
 	ov5645->dev = dev;
-	ov5645->fmt.code = MEDIA_BUS_FMT_UYVY8_2X8;
-	ov5645->fmt.width = 1920;
-	ov5645->fmt.height = 1080;
-	ov5645->fmt.field = V4L2_FIELD_NONE;
-	ov5645->fmt.colorspace = V4L2_COLORSPACE_SRGB;
-	ov5645->current_mode = OV5645_MODE_1080P;
 
 	endpoint = of_graph_get_next_endpoint(dev->of_node, NULL);
 	if (!endpoint) {
@@ -1191,6 +1224,13 @@ static int ov5645_probe(struct i2c_client *client,
 		return PTR_ERR(ov5645->xclk);
 	}
 
+	ret = of_property_read_u32(dev->of_node, "clock-frequency",
+				    &ov5645->xclk_freq);
+	if (ret) {
+		dev_err(dev, "could not get xclk frequency\n");
+		return ret;
+	}
+
 	ov5645->io_regulator = devm_regulator_get(dev, "vdddo");
 	if (IS_ERR(ov5645->io_regulator)) {
 		dev_err(dev, "cannot get io regulator\n");
@@ -1299,14 +1339,14 @@ static int ov5645_probe(struct i2c_client *client,
 		goto unregister_subdev;
 	}
 
-	ret = ov5645_read_reg(ov5645, OV5645_CHIP_ID_HIGH_REG, &chip_id_high);
-	if (ret < 0 || chip_id_high != OV5645_CHIP_ID_HIGH) {
+	ret = ov5645_read_reg(ov5645, OV5645_CHIP_ID_HIGH, &chip_id_high);
+	if (ret < 0 || chip_id_high != OV5645_CHIP_ID_HIGH_BYTE) {
 		dev_err(dev, "could not read ID high\n");
 		ret = -ENODEV;
 		goto power_down;
 	}
-	ret = ov5645_read_reg(ov5645, OV5645_CHIP_ID_LOW_REG, &chip_id_low);
-	if (ret < 0 || chip_id_low != OV5645_CHIP_ID_LOW) {
+	ret = ov5645_read_reg(ov5645, OV5645_CHIP_ID_LOW, &chip_id_low);
+	if (ret < 0 || chip_id_low != OV5645_CHIP_ID_LOW_BYTE) {
 		dev_err(dev, "could not read ID low\n");
 		ret = -ENODEV;
 		goto power_down;
@@ -1316,6 +1356,8 @@ static int ov5645_probe(struct i2c_client *client,
 
 	ov5645_s_power(&ov5645->sd, false);
 
+	ov5645_entity_init_cfg(&ov5645->sd, NULL);
+
 	return 0;
 
 power_down:
@@ -1326,6 +1368,7 @@ free_entity:
 	media_entity_cleanup(&ov5645->sd.entity);
 free_ctrl:
 	v4l2_ctrl_handler_free(&ov5645->ctrls);
+	mutex_destroy(&ov5645->power_lock);
 
 	return ret;
 }
@@ -1339,6 +1382,7 @@ static int ov5645_remove(struct i2c_client *client)
 	v4l2_async_unregister_subdev(&ov5645->sd);
 	media_entity_cleanup(&ov5645->sd.entity);
 	v4l2_ctrl_handler_free(&ov5645->ctrls);
+	mutex_destroy(&ov5645->power_lock);
 
 	return 0;
 }
diff --git a/drivers/media/platform/msm/camss-8x16/camss.c b/drivers/media/platform/msm/camss-8x16/camss.c
index 0644bc62a1a9..671f7b01c765 100644
--- a/drivers/media/platform/msm/camss-8x16/camss.c
+++ b/drivers/media/platform/msm/camss-8x16/camss.c
@@ -27,9 +27,6 @@
 
 #include "camss.h"
 
-#define CAMSS_CSIPHY_NUM 2
-#define CAMSS_CSID_NUM 2
-
 static struct resources csiphy_res[] = {
 	/* CSIPHY0 */
 	{
@@ -76,18 +73,28 @@ static struct resources csid_res[] = {
 
 static struct resources_ispif ispif_res = {
 	/* ISPIF */
-	.clock = { "camss_ahb_src", "ispif_ahb_clk", "csi0_src_clk",
-		   "csi0_clk", "csi0_pix_clk", "csi0_rdi_clk",
-		   "csi1_src_clk", "csi1_clk", "csi1_pix_clk",
-		   "csi1_rdi_clk", "vfe_clk_src", "camss_vfe_vfe_clk",
-		   "camss_csi_vfe_clk"
-	},
-	.clock_for_reset = { 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 },
+	.clock = { "camss_ahb_src", "ispif_ahb_clk" },
+	.clock_for_reset = {
+		"csi0_src_clk", "csi0_clk", "csi0_pix_clk", "csi0_rdi_clk",
+		"csi1_src_clk", "csi1_clk", "csi1_pix_clk", "csi1_rdi_clk",
+		"vfe_clk_src", "camss_vfe_vfe_clk", "camss_csi_vfe_clk",
+		"camss_top_ahb_clk", "camss_ahb_clk" },
 	.reg = { "ispif", "csi_clk_mux" },
 	.interrupt = "ispif"
 
 };
 
+static struct resources vfe_res = {
+	/* VFE0 */
+	.regulator = { NULL },
+	.clock = { "camss_top_ahb_clk", "vfe_clk_src", "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, 0 },
+	.reg = { "vfe0", "vfe0_vbif" },
+	.interrupt = { "vfe0" }
+};
+
 /*
  * camss_pipeline_pm_use_count - Count the number of users of a pipeline
  * @entity: The entity
@@ -262,38 +269,10 @@ static int camss_pipeline_link_notify(struct media_link *link, u32 flags,
 	return 0;
 }
 
-static int camss_alloc(struct device *dev, struct camss **c)
-{
-	struct camss *camss;
-
-	*c = devm_kzalloc(dev, sizeof(**c), GFP_KERNEL);
-	if (!*c) {
-		dev_err(dev, "Failed to allocate memory\n");
-		return -ENOMEM;
-	}
-
-	camss = *c;
-	camss->csiphy_num = CAMSS_CSIPHY_NUM;
-	camss->csiphy = devm_kzalloc(dev,
-				     camss->csiphy_num * sizeof(*camss->csiphy),
-				     GFP_KERNEL);
-
-	camss->csid_num = CAMSS_CSID_NUM;
-	camss->csid = devm_kzalloc(dev,
-				   camss->csid_num * sizeof(*camss->csid),
-				   GFP_KERNEL);
-	if (!camss->csiphy || !camss->csid) {
-		dev_err(dev, "Failed to allocate memory\n");
-		return -ENOMEM;
-	}
-
-	return 0;
-}
-
 static int camss_of_parse_node(struct device *dev, struct device_node *node,
 			       struct camss_async_subdev *csd)
 {
-	struct camss_csiphy_lanes_cfg *lncfg = &csd->interface.csi2.lanecfg;
+	struct csiphy_lanes_cfg *lncfg = &csd->interface.csi2.lane_cfg;
 	int *settle_cnt = &csd->interface.csi2.settle_cnt;
 	struct v4l2_of_endpoint vep;
 	unsigned int i;
@@ -302,7 +281,7 @@ static int camss_of_parse_node(struct device *dev, struct device_node *node,
 
 	dev_dbg(dev, "parsing endpoint %s\n", node->full_name);
 
-	csd->interface.id = vep.base.port;
+	csd->interface.csiphy_id = vep.base.port;
 
 	lncfg->clk.pos = vep.bus.mipi_csi2.clock_lane;
 	lncfg->clk.pol = vep.bus.mipi_csi2.lane_polarities[0];
@@ -387,8 +366,8 @@ static int camss_init_subdevices(struct camss *camss)
 	int i;
 	int ret;
 
-	for (i = 0; i < camss->csiphy_num; i++) {
-		ret = msm_csiphy_subdev_init(&camss->csiphy[i], camss,
+	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,
@@ -397,8 +376,8 @@ static int camss_init_subdevices(struct camss *camss)
 		}
 	}
 
-	for (i = 0; i < camss->csid_num; i++) {
-		ret = msm_csid_subdev_init(&camss->csid[i], camss,
+	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,
@@ -407,15 +386,13 @@ static int camss_init_subdevices(struct camss *camss)
 		}
 	}
 
-	ret = msm_ispif_subdev_init(&camss->ispif, camss, &ispif_res);
+	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;
 	}
 
-	camss->vfe_init.num_cids = 1;
-	camss->vfe_init.cid[0] = -1;
-	ret = msm_vfe_subdev_init(&camss->vfe, camss, &camss->vfe_init);
+	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;
@@ -429,7 +406,7 @@ static int camss_register_entities(struct camss *camss)
 	int i, j;
 	int ret;
 
-	for (i = 0; i < camss->csiphy_num; i++) {
+	for (i = 0; i < ARRAY_SIZE(camss->csiphy); i++) {
 		ret = msm_csiphy_register_entities(&camss->csiphy[i],
 						   &camss->v4l2_dev);
 		if (ret < 0) {
@@ -439,7 +416,7 @@ static int camss_register_entities(struct camss *camss)
 		}
 	}
 
-	for (i = 0; i < camss->csiphy_num; i++) {
+	for (i = 0; i < ARRAY_SIZE(camss->csiphy); i++) {
 		ret = msm_csid_register_entities(&camss->csid[i],
 						 &camss->v4l2_dev);
 		if (ret < 0) {
@@ -461,8 +438,8 @@ static int camss_register_entities(struct camss *camss)
 		goto err_reg_vfe;
 	}
 
-	for (i = 0; i < camss->csiphy_num; i++) {
-		for (j = 0; j < camss->csid_num; j++) {
+	for (i = 0; i < ARRAY_SIZE(camss->csiphy); i++) {
+		for (j = 0; j < ARRAY_SIZE(camss->csid); j++) {
 			ret = media_entity_create_link(
 				&camss->csiphy[i].subdev.entity,
 				MSM_CSIPHY_PAD_SRC,
@@ -479,27 +456,40 @@ static int camss_register_entities(struct camss *camss)
 		}
 	}
 
-	for (i = 0; i < camss->csid_num; i++) {
-		ret = media_entity_create_link(
-			&camss->csid[i].subdev.entity, MSM_CSID_PAD_SRC,
-			&camss->ispif.subdev.entity, MSM_ISPIF_PAD_SINK, 0);
-		if (ret < 0) {
-			dev_err(camss->dev, "Fail to link %s->%s entities\n",
-				camss->csid[i].subdev.entity.name,
-				camss->ispif.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_entity_create_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,
+					"Fail to link %s->%s entities\n",
+					camss->csid[i].subdev.entity.name,
+					camss->ispif.line[j].subdev.entity.name);
+				goto err_link;
+			}
 		}
 	}
 
-	ret = media_entity_create_link(
-			&camss->ispif.subdev.entity, MSM_ISPIF_PAD_SRC,
-			&camss->vfe.subdev.entity, MSM_VFE_PAD_SINK,
-			MEDIA_LNK_FL_IMMUTABLE | MEDIA_LNK_FL_ENABLED);
-	if (ret < 0) {
-		dev_err(camss->dev, "Fail to link %s->%s entities\n",
-			camss->ispif.subdev.entity.name,
-			camss->vfe.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_entity_create_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,
+					"Fail 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;
@@ -510,13 +500,13 @@ err_reg_vfe:
 	msm_ispif_unregister_entities(&camss->ispif);
 err_reg_ispif:
 
-	i = camss->csid_num;
+	i = ARRAY_SIZE(camss->csid);
 err_reg_csid:
 	for (i--; i >= 0; i--) {
 		msm_csid_unregister_entities(&camss->csid[i]);
 	}
 
-	i = camss->csiphy_num;
+	i = ARRAY_SIZE(camss->csiphy);
 err_reg_csiphy:
 	for (i--; i >= 0; i--) {
 		msm_csiphy_unregister_entities(&camss->csiphy[i]);
@@ -529,10 +519,12 @@ static void camss_unregister_entities(struct camss *camss)
 {
 	int i;
 
-	for (i = camss->csiphy_num - 1; i >= 0; i--)
+	/* TODO: Remove links? */
+
+	for (i = ARRAY_SIZE(camss->csiphy) - 1; i >= 0; i--)
 		msm_csiphy_unregister_entities(&camss->csiphy[i]);
 
-	for (i = camss->csid_num - 1; i >= 0; i--)
+	for (i = ARRAY_SIZE(camss->csid) - 1; i >= 0; i--)
 		msm_csid_unregister_entities(&camss->csid[i]);
 
 	msm_ispif_unregister_entities(&camss->ispif);
@@ -547,7 +539,7 @@ static int camss_subdev_notifier_bound(struct v4l2_async_notifier *async,
 	struct camss *camss = container_of(async, struct camss, notifier);
 	struct camss_async_subdev *csd =
 		container_of(asd, struct camss_async_subdev, asd);
-	enum camss_csiphy id = csd->interface.id;
+	u8 id = csd->interface.csiphy_id;
 	struct csiphy_device *csiphy = &camss->csiphy[id];
 	struct media_entity *input = &csiphy->subdev.entity;
 	unsigned int flags = MEDIA_LNK_FL_IMMUTABLE | MEDIA_LNK_FL_ENABLED;
@@ -586,21 +578,26 @@ static int camss_subdev_notifier_complete(struct v4l2_async_notifier *async)
 
 static int camss_probe(struct platform_device *pdev)
 {
+	struct device *dev = &pdev->dev;
 	struct camss *camss;
 	int ret;
 
-	dev_dbg(&pdev->dev, "Enter\n");
+	dev_dbg(dev, "Enter\n");
 
-	ret = camss_alloc(&pdev->dev, &camss);
-	if (ret < 0)
-		return ret;
+	camss = devm_kzalloc(dev, sizeof(*camss), GFP_KERNEL);
+	if (!camss) {
+		dev_err(dev, "Failed to allocate memory\n");
+		return -ENOMEM;
+	}
 
-	camss->dev = &pdev->dev;
+	camss->dev = dev;
 	platform_set_drvdata(pdev, camss);
 
-	ret = camss_of_parse_nodes(&pdev->dev, &camss->notifier);
+	ret = camss_of_parse_nodes(dev, &camss->notifier);
 	if (ret < 0)
 		return ret;
+	else if (ret == 0)
+		return -ENODEV;
 
 	ret = camss_init_subdevices(camss);
 	if (ret < 0)
@@ -613,8 +610,7 @@ static int camss_probe(struct platform_device *pdev)
 	camss->media_dev.link_notify = camss_pipeline_link_notify;
 	ret = media_device_register(&camss->media_dev);
 	if (ret < 0) {
-		dev_err(&pdev->dev,
-			"%s: Media device registration failed (%d)\n",
+		dev_err(dev, "%s: Media device registration failed (%d)\n",
 			__func__, ret);
 		return ret;
 	}
@@ -622,8 +618,7 @@ static int camss_probe(struct platform_device *pdev)
 	camss->v4l2_dev.mdev = &camss->media_dev;
 	ret = v4l2_device_register(camss->dev, &camss->v4l2_dev);
 	if (ret < 0) {
-		dev_err(&pdev->dev,
-			"%s: V4L2 device registration failed (%d)\n",
+		dev_err(dev, "%s: V4L2 device registration failed (%d)\n",
 			__func__, ret);
 		goto err_register_v4l2;
 	}
@@ -639,7 +634,7 @@ static int camss_probe(struct platform_device *pdev)
 		ret = v4l2_async_notifier_register(&camss->v4l2_dev,
 						   &camss->notifier);
 		if (ret) {
-			dev_err(&pdev->dev,
+			dev_err(dev,
 				"%s: V4L2 async notifier registration failed (%d)\n",
 				__func__, ret);
 			goto err_register_subdevs;
@@ -647,14 +642,14 @@ static int camss_probe(struct platform_device *pdev)
 	} else {
 		ret = v4l2_device_register_subdev_nodes(&camss->v4l2_dev);
 		if (ret < 0) {
-			dev_err(&pdev->dev,
+			dev_err(dev,
 				"%s: V4L2 subdev nodes registration failed (%d)\n",
 				__func__, ret);
 			goto err_register_subdevs;
 		}
 	}
 
-	dev_dbg(&pdev->dev, "camss driver registered successfully!\n");
+	dev_dbg(dev, "camss driver registered successfully!\n");
 
 	return 0;
 
diff --git a/drivers/media/platform/msm/camss-8x16/camss.h b/drivers/media/platform/msm/camss-8x16/camss.h
index 9eb4f70aa809..a8adaa8db8eb 100644
--- a/drivers/media/platform/msm/camss-8x16/camss.h
+++ b/drivers/media/platform/msm/camss-8x16/camss.h
@@ -31,6 +31,25 @@
 
 #define CAMSS_VERSION KERNEL_VERSION(0, 1, 0)
 
+#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 {
@@ -43,7 +62,7 @@ struct resources {
 
 struct resources_ispif {
 	char *clock[CAMSS_RES_MAX];
-	u8 clock_for_reset[CAMSS_RES_MAX];
+	char *clock_for_reset[CAMSS_RES_MAX];
 	char *reg[CAMSS_RES_MAX];
 	char *interrupt;
 };
@@ -53,40 +72,16 @@ struct camss {
 	struct v4l2_async_notifier notifier;
 	struct media_device media_dev;
 	struct device *dev;
-	int csiphy_num;
-	struct csiphy_device *csiphy;
-	int csid_num;
-	struct csid_device *csid;
+	struct csiphy_device csiphy[CAMSS_CSIPHY_NUM];
+	struct csid_device csid[CAMSS_CSID_NUM];
 	struct ispif_device ispif;
 	struct vfe_device vfe;
-	struct vfe_init vfe_init;
 	struct device *iommu_dev;
 };
 
-enum camss_csiphy {
-	CAMSS_CSIPHY0 = 0,
-	CAMSS_CSIPHY1
-};
-
-struct camss_csiphy_lane {
-	u8 pos;
-	u8 pol;
-};
-
-struct camss_csiphy_lanes_cfg {
-	int num_data;
-	struct camss_csiphy_lane *data;
-	struct camss_csiphy_lane clk;
-};
-
-struct camss_csi2_cfg {
-	int settle_cnt;
-	struct camss_csiphy_lanes_cfg lanecfg;
-};
-
 struct camss_camera_interface {
-	enum camss_csiphy id;
-	struct camss_csi2_cfg csi2;
+	u8 csiphy_id;
+	struct csiphy_csi2_cfg csi2;
 };
 
 struct camss_async_subdev {
diff --git a/drivers/media/platform/msm/camss-8x16/csid.c b/drivers/media/platform/msm/camss-8x16/csid.c
index 7ca3640fb811..c69b728d5643 100644
--- a/drivers/media/platform/msm/camss-8x16/csid.c
+++ b/drivers/media/platform/msm/camss-8x16/csid.c
@@ -18,6 +18,7 @@
 #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>
@@ -47,6 +48,168 @@
 #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 DECODE_FORMAT_DPCM_10_8_10		0x5
+
+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
+	},
+	{
+		MEDIA_BUS_FMT_SBGGR10_DPCM8_1X8,
+		MEDIA_BUS_FMT_SBGGR10_1X10,
+		DATA_TYPE_RAW_10BIT,
+		DECODE_FORMAT_DPCM_10_8_10,
+		10
+	},
+	{
+		MEDIA_BUS_FMT_SGBRG10_DPCM8_1X8,
+		MEDIA_BUS_FMT_SGBRG10_1X10,
+		DATA_TYPE_RAW_10BIT,
+		DECODE_FORMAT_DPCM_10_8_10,
+		10
+	},
+	{
+		MEDIA_BUS_FMT_SGRBG10_DPCM8_1X8,
+		MEDIA_BUS_FMT_SGRBG10_1X10,
+		DATA_TYPE_RAW_10BIT,
+		DECODE_FORMAT_DPCM_10_8_10,
+		10
+	},
+	{
+		MEDIA_BUS_FMT_SRGGB10_DPCM8_1X8,
+		MEDIA_BUS_FMT_SRGGB10_1X10,
+		DATA_TYPE_RAW_10BIT,
+		DECODE_FORMAT_DPCM_10_8_10,
+		10
+	}
+};
+
 /*
  * csid_isr - CSID module interrupt handler
  * @irq: Interrupt line
@@ -59,8 +222,8 @@ static irqreturn_t csid_isr(int irq, void *dev)
 	struct csid_device *csid = dev;
 	u32 value;
 
-	value = readl(csid->base + CAMSS_CSID_IRQ_STATUS);
-	writel(value, csid->base + CAMSS_CSID_IRQ_CLEAR_CMD);
+	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);
@@ -71,7 +234,9 @@ static irqreturn_t csid_isr(int irq, void *dev)
 /*
  * csid_enable_clocks - Enable clocks for CSID module and
  * set clock rates where needed
- * @csid: CSID device
+ * @nclocks: Number of clocks in clock array
+ * @clock: Clock array
+ * @clock_rate: Clock rates array
  *
  * Return 0 on success or a negative error code otherwise
  */
@@ -113,7 +278,8 @@ error:
 
 /*
  * csid_disable_clocks - Disable clocks for CSID module
- * @csid: CSID device
+ * @nclocks: Number of clocks in clock array
+ * @clock: Clock array
  */
 static void csid_disable_clocks(int nclocks, struct clk **clock)
 {
@@ -135,7 +301,7 @@ static int csid_set_power(struct v4l2_subdev *sd, int on)
 	struct csid_device *csid = v4l2_get_subdevdata(sd);
 	int ret;
 
-	dev_err(csid->camss->dev, "%s: Enter, csid%d on = %d\n",
+	dev_dbg(to_device_index(csid, csid->id), "%s: Enter, csid%d on = %d\n",
 		__func__, csid->id, on);
 
 	if (on) {
@@ -152,8 +318,12 @@ static int csid_set_power(struct v4l2_subdev *sd, int on)
 
 		enable_irq(csid->irq);
 
-		hw_version = readl(csid->base + CAMSS_CSID_HW_VERSION);
-		dev_err(csid->camss->dev, "CSID HW Version = 0x%08x\n", hw_version);
+		/* Reset */
+		writel_relaxed(0x7fff, csid->base + CAMSS_CSID_RST_CMD);
+		wait_for_completion(&csid->reset_complete);
+
+		hw_version = readl_relaxed(csid->base + CAMSS_CSID_HW_VERSION);
+		dev_dbg(to_device_index(csid, csid->id), "CSID HW Version = 0x%08x\n", hw_version);
 	} else {
 		disable_irq(csid->irq);
 
@@ -164,31 +334,45 @@ static int csid_set_power(struct v4l2_subdev *sd, int on)
 			return ret;
 	}
 
-	dev_err(csid->camss->dev, "%s: Exit, csid%d on = %d\n",
+	dev_dbg(to_device_index(csid, csid->id), "%s: Exit, csid%d on = %d\n",
 		__func__, csid->id, on);
 
 	return 0;
 }
 
-#define DATA_TYPE_YUV422_8BIT 0x1e
+/*
+ * csid_get_uncompressed - map media bus format to uncompressed media bus format
+ * @fmt media bus format code
+ *
+ * Return uncompressed media bus format
+ */
+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 but format to data type
+ * csid_get_data_type - map media bus format to data type
  * @fmt media bus format code
  *
  * Return data type code
  */
-static u8 csid_get_data_type(u32 fmt)
+static u8 csid_get_data_type(u32 code)
 {
-	switch (fmt) {
-	case MEDIA_BUS_FMT_UYVY8_2X8:
-		return DATA_TYPE_YUV422_8BIT;
-	}
+	unsigned int i;
 
-	return 0;
-}
+	for (i = 0; i < ARRAY_SIZE(csid_input_fmts); i++)
+		if (code == csid_input_fmts[i].code)
+			break;
 
-#define DECODE_FORMAT_UNCOMPRESSED_8_BIT 0x1
+	return csid_input_fmts[i].data_type;
+}
 
 /*
  * csid_get_decode_format - map media but format to decode format
@@ -196,16 +380,35 @@ static u8 csid_get_data_type(u32 fmt)
  *
  * Return decode format code
  */
-static u8 csid_get_decode_format(u32 fmt)
+static u8 csid_get_decode_format(u32 code)
 {
-	switch (fmt) {
-	case MEDIA_BUS_FMT_UYVY8_2X8:
-		return DECODE_FORMAT_UNCOMPRESSED_8_BIT;
-	}
+	unsigned int i;
 
-	return 0;
+	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
+ * @fmt 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
@@ -220,7 +423,7 @@ 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;
 
-	dev_err(csid->camss->dev, "%s: Enter, csid%d enable = %d\n",
+	dev_dbg(to_device_index(csid, csid->id), "%s: Enter, csid%d enable = %d\n",
 		__func__, csid->id, enable);
 
 	if (enable) {
@@ -232,7 +435,7 @@ static int csid_set_stream(struct v4l2_subdev *sd, int enable)
 
 		ret = v4l2_ctrl_handler_setup(&csid->ctrls);
 		if (ret < 0) {
-			dev_err(csid->camss->dev,
+			dev_err(to_device_index(csid, csid->id),
 				"could not sync v4l2 controls\n");
 			return ret;
 		}
@@ -242,48 +445,50 @@ static int csid_set_stream(struct v4l2_subdev *sd, int enable)
 			return -ENOLINK;
 		}
 
-		/* Reset */
-		writel(0x7FFF, csid->base + CAMSS_CSID_RST_CMD);
-		wait_for_completion(&csid->reset_complete);
-
 		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 * 2;
+					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(val, csid->base + CAMSS_CSID_TG_VC_CFG);
+			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(val, csid->base + CAMSS_CSID_TG_DT_n_CGG_0(0));
+			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(val, csid->base + CAMSS_CSID_TG_DT_n_CGG_1(0));
+			writel_relaxed(val, csid->base +
+				       CAMSS_CSID_TG_DT_n_CGG_1(0));
 
 			/* 2:0 output random */
 			val = tg->payload_mode;
-			writel(val, csid->base + CAMSS_CSID_TG_DT_n_CGG_2(0));
+			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(val, csid->base + CAMSS_CSID_CORE_CTRL_0);
+			writel_relaxed(val,
+				       csid->base + CAMSS_CSID_CORE_CTRL_0);
 
 			val = phy->csiphy_id << 17;
 			val |= 0x9;
 
-			writel(val, csid->base + CAMSS_CSID_CORE_CTRL_1);
+			writel_relaxed(val,
+				       csid->base + CAMSS_CSID_CORE_CTRL_1);
 		}
 
 		/* Config LUT */
@@ -291,22 +496,22 @@ static int csid_set_stream(struct v4l2_subdev *sd, int enable)
 		dt_shift = (cid % 4) * 8;
 		df = csid_get_decode_format(csid->fmt[MSM_CSID_PAD_SINK].code);
 
-		val = readl(csid->base + CAMSS_CSID_CID_LUT_VC_n(vc));
+		val = readl_relaxed(csid->base + CAMSS_CSID_CID_LUT_VC_n(vc));
 		val &= ~(0xff << dt_shift);
 		val |= dt << dt_shift;
-		writel(val, csid->base + CAMSS_CSID_CID_LUT_VC_n(vc));
+		writel_relaxed(val, csid->base + CAMSS_CSID_CID_LUT_VC_n(vc));
 
 		val = (df << 4) | 0x3;
-		writel(val, csid->base + CAMSS_CSID_CID_n_CFG(cid));
+		writel_relaxed(val, csid->base + CAMSS_CSID_CID_n_CFG(cid));
 
 		if (tg->enabled) {
 			val = 0x00a06437;
-			writel(val, csid->base + CAMSS_CSID_TG_CTRL);
+			writel_relaxed(val, csid->base + CAMSS_CSID_TG_CTRL);
 		}
 	} else {
 		if (tg->enabled) {
 			u32 val = 0x00a06436;
-			writel(val, csid->base + CAMSS_CSID_TG_CTRL);
+			writel_relaxed(val, csid->base + CAMSS_CSID_TG_CTRL);
 		}
 	}
 
@@ -335,6 +540,151 @@ __csid_get_format(struct csid_device *csid,
 }
 
 /*
+ * 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 (fmt->code == csid_input_fmts[i].uncompressed)
+					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
@@ -373,60 +723,43 @@ static int csid_set_format(struct v4l2_subdev *sd,
 	struct csid_device *csid = v4l2_get_subdevdata(sd);
 	struct v4l2_mbus_framefmt *format;
 
-	if (fmt->pad == MSM_CSID_PAD_SINK) {
-		/* Set format on sink pad */
-		format = __csid_get_format(csid, cfg, fmt->pad,
-					   fmt->which);
-		if (format == NULL)
-			return -EINVAL;
-
-		if (fmt->format.field == V4L2_FIELD_ANY)
-			fmt->format.field = V4L2_FIELD_NONE;
+	format = __csid_get_format(csid, cfg, fmt->pad, fmt->which);
+	if (format == NULL)
+		return -EINVAL;
 
-		*format = fmt->format;
+	csid_try_format(csid, cfg, fmt->pad, &fmt->format, fmt->which);
+	*format = fmt->format;
 
-		/* Reset format on source pad */
+	/* 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);
-		if (format == NULL)
-			return -EINVAL;
 
 		*format = fmt->format;
-	} else {
-		if (media_entity_remote_pad(&csid->pads[MSM_CSID_PAD_SINK])) {
-			/* CSID is linked to CSIPHY */
-			/* Reset format on source pad to sink pad format */
-
-			format = __csid_get_format(csid, cfg, MSM_CSID_PAD_SINK,
-						   fmt->which);
-			if (format == NULL)
-				return -EINVAL;
-
-			fmt->format = *format;
-
-			format = __csid_get_format(csid, cfg, fmt->pad,
-						   fmt->which);
-			if (format == NULL)
-				return -EINVAL;
-
-			*format = fmt->format;
-		} else {
-			/* CSID is not linked to CSIPHY */
-			/* Set format on source pad to allow */
-			/* test generator usage */
+		csid_try_format(csid, cfg, MSM_CSID_PAD_SRC, format,
+				fmt->which);
+	}
 
-			format = __csid_get_format(csid, cfg, fmt->pad,
-						   fmt->which);
-			if (format == NULL)
-				return -EINVAL;
+	return 0;
+}
 
-			/* Accept only YUV422 format */
-			fmt->format.code = MEDIA_BUS_FMT_UYVY8_2X8;
-			fmt->format.field = V4L2_FIELD_NONE;
+/*
+ * csid_init_formats - Initialize formats on all pads
+ * @sd: CSID V4L2 subdevice
+ *
+ * Initialize all pad formats with default values.
+ */
+static int csid_init_formats(struct v4l2_subdev *sd)
+{
+	struct v4l2_subdev_format format;
 
-			*format = fmt->format;
-		}
-	}
+	memset(&format, 0, sizeof(format));
+	format.pad = MSM_CSID_PAD_SINK;
+	format.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+	format.format.code = MEDIA_BUS_FMT_UYVY8_2X8;
+	format.format.width = 1920;
+	format.format.height = 1080;
+	csid_set_format(sd, NULL, &format);
 
 	return 0;
 }
@@ -499,17 +832,15 @@ static struct v4l2_ctrl_ops csid_ctrl_ops = {
  *
  * Return 0 on success or a negative error code otherwise
  */
-int msm_csid_subdev_init(struct csid_device *csid, struct camss *camss,
+int msm_csid_subdev_init(struct csid_device *csid,
 			 struct resources *res, u8 id)
 {
-	struct device *dev = camss->dev;
+	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->camss = camss;
-
 	csid->id = id;
 
 	/* Memory */
@@ -539,9 +870,8 @@ int msm_csid_subdev_init(struct csid_device *csid, struct camss *camss,
 
 	/* Clocks */
 
-	i = 0;
 	csid->nclocks = 0;
-	while (res->clock[i++])
+	while (res->clock[csid->nclocks])
 		csid->nclocks++;
 
 	csid->clock = devm_kzalloc(dev, csid->nclocks * sizeof(*csid->clock),
@@ -578,19 +908,30 @@ int msm_csid_subdev_init(struct csid_device *csid, struct camss *camss,
 	return 0;
 }
 
+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 camss_csiphy_lanes_cfg *lanecfg)
+static u32 csid_get_lane_assign(struct csiphy_lanes_cfg *lane_cfg)
 {
 	u32 lane_assign = 0;
 	int i;
 
-	for (i = 0; i < lanecfg->num_data; i++)
-		lane_assign |= lanecfg->data[i].pos << (i * 4);
+	for (i = 0; i < lane_cfg->num_data; i++)
+		lane_assign |= lane_cfg->data[i].pos << (i * 4);
 
 	return lane_assign;
 }
@@ -613,7 +954,8 @@ static int csid_link_setup(struct media_entity *entity,
 		struct v4l2_subdev *sd;
 		struct csid_device *csid;
 		struct csiphy_device *csiphy;
-		struct camss_csiphy_lanes_cfg *lanecfg;
+		struct csiphy_lanes_cfg *lane_cfg;
+		struct v4l2_subdev_format format;
 
 		sd = container_of(entity, struct v4l2_subdev, entity);
 		csid = v4l2_get_subdevdata(sd);
@@ -633,9 +975,15 @@ static int csid_link_setup(struct media_entity *entity,
 
 		csid->phy.csiphy_id = csiphy->id;
 
-		lanecfg = &csiphy->cfg.csi2->lanecfg;
-		csid->phy.lane_cnt = lanecfg->num_data;
-		csid->phy.lane_assign = csid_get_lane_assign(lanecfg);
+		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;
@@ -650,6 +998,8 @@ static const struct v4l2_subdev_video_ops csid_video_ops = {
 };
 
 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,
 };
@@ -695,7 +1045,7 @@ int msm_csid_register_entities(struct csid_device *csid,
 				csid_test_pattern_menu);
 
 	if (csid->ctrls.error) {
-		dev_err(csid->camss->dev, "failed to init ctrl: %d\n",
+		dev_err(to_device_index(csid, csid->id), "failed to init ctrl: %d\n",
 			csid->ctrls.error);
 		ret = csid->ctrls.error;
 		goto free_ctrl;
@@ -703,19 +1053,21 @@ int msm_csid_register_entities(struct csid_device *csid,
 
 	csid->subdev.ctrl_handler = &csid->ctrls;
 
+	csid_init_formats(sd);
+
 	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_init(&sd->entity, MSM_CSID_PADS_NUM, pads, 0);
 	if (ret < 0) {
-		dev_err(csid->camss->dev, "failed to init media entity");
+		dev_err(to_device_index(csid, csid->id), "failed to init media entity");
 		goto free_ctrl;
 	}
 
 	ret = v4l2_device_register_subdev(v4l2_dev, sd);
 	if (ret < 0) {
-		dev_err(csid->camss->dev, "failed to register subdev");
+		dev_err(to_device_index(csid, csid->id), "failed to register subdev");
 		goto media_cleanup;
 	}
 
diff --git a/drivers/media/platform/msm/camss-8x16/csid.h b/drivers/media/platform/msm/camss-8x16/csid.h
index ac2022a2f071..d051c8298d3a 100644
--- a/drivers/media/platform/msm/camss-8x16/csid.h
+++ b/drivers/media/platform/msm/camss-8x16/csid.h
@@ -49,13 +49,10 @@ struct csid_phy_config {
 	u32 lane_assign;
 };
 
-struct camss;
-
 struct csid_device {
 	u8 id;
 	struct v4l2_subdev subdev;
 	struct media_pad pads[MSM_CSID_PADS_NUM];
-	struct camss *camss;
 	void __iomem *base;
 	u32 irq;
 	struct clk **clock;
@@ -72,7 +69,7 @@ struct csid_device {
 
 struct resources;
 
-int msm_csid_subdev_init(struct csid_device *csid, struct camss *camss,
+int msm_csid_subdev_init(struct csid_device *csid,
 			 struct resources *res, u8 id);
 
 int msm_csid_register_entities(struct csid_device *csid,
@@ -80,4 +77,6 @@ int msm_csid_register_entities(struct csid_device *csid,
 
 void msm_csid_unregister_entities(struct csid_device *csid);
 
+void msm_csid_get_csid_id(struct media_entity *entity, u8 *id);
+
 #endif /* QC_MSM_CAMSS_CSID_H */
diff --git a/drivers/media/platform/msm/camss-8x16/csiphy.c b/drivers/media/platform/msm/camss-8x16/csiphy.c
index 8f64491940a9..da4984f7f163 100644
--- a/drivers/media/platform/msm/camss-8x16/csiphy.c
+++ b/drivers/media/platform/msm/camss-8x16/csiphy.c
@@ -18,6 +18,7 @@
 #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>
@@ -43,6 +44,29 @@
 #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,
+	MEDIA_BUS_FMT_SBGGR10_DPCM8_1X8,
+	MEDIA_BUS_FMT_SGBRG10_DPCM8_1X8,
+	MEDIA_BUS_FMT_SGRBG10_DPCM8_1X8,
+	MEDIA_BUS_FMT_SRGGB10_DPCM8_1X8
+};
+
 /*
  * csiphy_isr - CSIPHY module interrupt handler
  * @irq: Interrupt line
@@ -84,7 +108,9 @@ static void csiphy_reset(struct csiphy_device *csiphy)
 /*
  * csiphy_enable_clocks - Enable clocks for CSIPHY module and
  * set clock rates where needed
- * @csiphy: CSIPHY device
+ * @nclocks: Number of clocks in clock array
+ * @clock: Clock array
+ * @clock_rate: Clock rates array
  *
  * Return 0 on success or a negative error code otherwise
  */
@@ -99,19 +125,22 @@ static int csiphy_enable_clocks(struct csiphy_device *csiphy)
 			clk_rate = clk_round_rate(csiphy->clock[i],
 						  csiphy->clock_rate[i]);
 			if (clk_rate < 0) {
-				dev_err(csiphy->camss->dev, "round failed\n");
+				dev_err(to_device_index(csiphy, csiphy->id),
+					"round failed\n");
 				ret = clk_rate;
 				goto error;
 			}
 			ret = clk_set_rate(csiphy->clock[i], clk_rate);
 			if (ret < 0) {
-				dev_err(csiphy->camss->dev, "set rate failed\n");
+				dev_err(to_device_index(csiphy, csiphy->id),
+					"set rate failed\n");
 				goto error;
 			}
 		}
 		ret = clk_prepare_enable(csiphy->clock[i]);
 		if (ret) {
-			dev_err(csiphy->camss->dev, "clk enable failed\n");
+			dev_err(to_device_index(csiphy, csiphy->id),
+				"clk enable failed\n");
 			goto error;
 		}
 	}
@@ -127,7 +156,8 @@ error:
 
 /*
  * csiphy_disable_clocks - Disable clocks for CSIPHY module
- * @csiphy: CSIPHY device
+ * @nclocks: Number of clocks in clock array
+ * @clock: Clock array
  */
 static void csiphy_disable_clocks(struct csiphy_device *csiphy)
 {
@@ -149,7 +179,8 @@ static int csiphy_set_power(struct v4l2_subdev *sd, int on)
 	struct csiphy_device *csiphy = v4l2_get_subdevdata(sd);
 	int ret;
 
-	dev_err(csiphy->camss->dev, "%s: Enter, csiphy%d on = %d\n",
+	dev_dbg(to_device_index(csiphy, csiphy->id),
+		"%s: Enter, csiphy%d on = %d\n",
 		__func__, csiphy->id, on);
 
 	if (on) {
@@ -163,8 +194,10 @@ static int csiphy_set_power(struct v4l2_subdev *sd, int on)
 
 		csiphy_reset(csiphy);
 
-		hw_version = readl(csiphy->base + CAMSS_CSI_PHY_HW_VERSION);
-		dev_err(csiphy->camss->dev, "CSIPHY HW Version = 0x%02x\n",
+		hw_version = readl_relaxed(csiphy->base +
+					   CAMSS_CSI_PHY_HW_VERSION);
+		dev_dbg(to_device_index(csiphy, csiphy->id),
+			"CSIPHY HW Version = 0x%02x\n",
 			hw_version);
 	} else {
 		disable_irq(csiphy->irq);
@@ -172,7 +205,8 @@ static int csiphy_set_power(struct v4l2_subdev *sd, int on)
 		csiphy_disable_clocks(csiphy);
 	}
 
-	dev_err(csiphy->camss->dev, "%s: Exit csiphy%d on = %d\n",
+	dev_dbg(to_device_index(csiphy, csiphy->id),
+		"%s: Exit csiphy%d on = %d\n",
 		__func__, csiphy->id, on);
 
 	return 0;
@@ -184,7 +218,7 @@ static int csiphy_set_power(struct v4l2_subdev *sd, int on)
  *
  * Return lane mask
  */
-static int csiphy_get_lane_mask(struct camss_csiphy_lanes_cfg *lane_cfg)
+static int csiphy_get_lane_mask(struct csiphy_lanes_cfg *lane_cfg)
 {
 	u16 lane_mask;
 	int i;
@@ -210,15 +244,16 @@ static int csiphy_set_stream(struct v4l2_subdev *sd, int enable)
 {
 	struct csiphy_device *csiphy = v4l2_get_subdevdata(sd);
 	struct csiphy_config *cfg = &csiphy->cfg;
-	u16 lane_mask = csiphy_get_lane_mask(&cfg->csi2->lanecfg);
+	u16 lane_mask = csiphy_get_lane_mask(&cfg->csi2->lane_cfg);
 	u8 i;
 	u8 val;
 
-	dev_err(csiphy->camss->dev, "%s: Enter, csiphy%d enable = %d\n",
+	dev_dbg(to_device_index(csiphy, csiphy->id),
+		"%s: Enter, csiphy%d enable = %d\n",
 		__func__, csiphy->id, enable);
 
 	if (enable) {
-		val = readl(csiphy->base_clk_mux);
+		val = readl_relaxed(csiphy->base_clk_mux);
 		if (cfg->combo_mode && (lane_mask & 0x18) == 0x18) {
 			val &= ~0xf0;
 			val |= cfg->csid_id << 4;
@@ -307,6 +342,123 @@ __csiphy_get_format(struct csiphy_device *csiphy,
 }
 
 /*
+ * 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;
+
+		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;
+	}
+
+	fmt->colorspace = V4L2_COLORSPACE_SRGB;
+}
+
+/*
+ * 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
@@ -345,39 +497,48 @@ static int csiphy_set_format(struct v4l2_subdev *sd,
 	struct csiphy_device *csiphy = v4l2_get_subdevdata(sd);
 	struct v4l2_mbus_framefmt *format;
 
-	if (fmt->pad == MSM_CSIPHY_PAD_SINK) {
-		/* Set format on sink pad */
-		format = __csiphy_get_format(csiphy, cfg, fmt->pad,
-					     fmt->which);
-		if (format == NULL)
-			return -EINVAL;
-
-		if (fmt->format.field == V4L2_FIELD_ANY)
-			fmt->format.field = V4L2_FIELD_NONE;
+	format = __csiphy_get_format(csiphy, cfg, fmt->pad, fmt->which);
+	if (format == NULL)
+		return -EINVAL;
 
-		*format = fmt->format;
+	csiphy_try_format(csiphy, cfg, fmt->pad, &fmt->format, fmt->which);
+	*format = fmt->format;
 
-		/* Reset format on source pad */
+	/* 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);
-		if (format == NULL)
-			return -EINVAL;
 
 		*format = fmt->format;
-	} else {
-		/* Source pad format must be same as sink pad format, */
-		/* so just return source pad format */
-		format = __csiphy_get_format(csiphy, cfg, fmt->pad, fmt->which);
-		if (format == NULL)
-			return -EINVAL;
-
-		fmt->format = *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
+ *
+ * Initialize all pad formats with default values.
+ */
+static int csiphy_init_formats(struct v4l2_subdev *sd)
+{
+	struct v4l2_subdev_format format;
+
+	memset(&format, 0, sizeof(format));
+	format.pad = MSM_CSIPHY_PAD_SINK;
+	format.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+	format.format.code = MEDIA_BUS_FMT_UYVY8_2X8;
+	format.format.width = 1920;
+	format.format.height = 1080;
+	csiphy_set_format(sd, NULL, &format);
+
+	return 0;
+}
+
+/*
  * msm_csiphy_subdev_init - Initialize CSIPHY device structure and resources
  * @csiphy: CSIPHY device
  * @camss: Camera sub-system structure
@@ -386,18 +547,16 @@ static int csiphy_set_format(struct v4l2_subdev *sd,
  *
  * Return 0 on success or a negative error code otherwise
  */
-int msm_csiphy_subdev_init(struct csiphy_device *csiphy, struct camss *camss,
+int msm_csiphy_subdev_init(struct csiphy_device *csiphy,
 			   struct resources *res, u8 id)
 {
-	struct device *dev = camss->dev;
+	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->camss = camss;
-
-	dev_err(csiphy->camss->dev, "%s: Enter\n", __func__);
+	dev_err(dev, "%s: Enter\n", __func__);
 
 	csiphy->id = id;
 
@@ -437,9 +596,8 @@ int msm_csiphy_subdev_init(struct csiphy_device *csiphy, struct camss *camss,
 
 	/* Clocks */
 
-	i = 0;
 	csiphy->nclocks = 0;
-	while (res->clock[i++])
+	while (res->clock[csiphy->nclocks])
 		csiphy->nclocks++;
 
 	csiphy->clock = devm_kzalloc(dev, csiphy->nclocks *
@@ -463,7 +621,7 @@ int msm_csiphy_subdev_init(struct csiphy_device *csiphy, struct camss *camss,
 		csiphy->clock_rate[i] = res->clock_rate[i];
 	}
 
-	dev_err(csiphy->camss->dev, "%s: Exit\n", __func__);
+	dev_err(dev, "%s: Exit\n", __func__);
 
 	return 0;
 }
@@ -508,6 +666,8 @@ static const struct v4l2_subdev_video_ops csiphy_video_ops = {
 };
 
 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,
 };
@@ -546,6 +706,8 @@ int msm_csiphy_register_entities(struct csiphy_device *csiphy,
 		 MSM_CSIPHY_NAME, csiphy->id);
 	v4l2_set_subdevdata(sd, csiphy);
 
+	csiphy_init_formats(sd);
+
 	pads[MSM_CSIPHY_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
 	pads[MSM_CSIPHY_PAD_SRC].flags = MEDIA_PAD_FL_SOURCE;
 
diff --git a/drivers/media/platform/msm/camss-8x16/csiphy.h b/drivers/media/platform/msm/camss-8x16/csiphy.h
index 672d5a3fcf06..9a9ba39628ec 100644
--- a/drivers/media/platform/msm/camss-8x16/csiphy.h
+++ b/drivers/media/platform/msm/camss-8x16/csiphy.h
@@ -28,21 +28,32 @@
 #define MSM_CSIPHY_PAD_SRC 1
 #define MSM_CSIPHY_PADS_NUM 2
 
-struct camss_csi2_cfg;
+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;
-	u32 csid_id;
-	struct camss_csi2_cfg *csi2;
+	u8 csid_id;
+	struct csiphy_csi2_cfg *csi2;
 };
 
-struct camss;
-
 struct csiphy_device {
 	u8 id;
 	struct v4l2_subdev subdev;
 	struct media_pad pads[MSM_CSIPHY_PADS_NUM];
-	struct camss *camss;
 	void __iomem *base;
 	void __iomem *base_clk_mux;
 	u32 irq;
@@ -55,7 +66,7 @@ struct csiphy_device {
 
 struct resources;
 
-int msm_csiphy_subdev_init(struct csiphy_device *csiphy, struct camss *camss,
+int msm_csiphy_subdev_init(struct csiphy_device *csiphy,
 			   struct resources *res, u8 id);
 
 int msm_csiphy_register_entities(struct csiphy_device *csiphy,
diff --git a/drivers/media/platform/msm/camss-8x16/ispif.c b/drivers/media/platform/msm/camss-8x16/ispif.c
index f53c534aa1d2..ba07872be4ab 100644
--- a/drivers/media/platform/msm/camss-8x16/ispif.c
+++ b/drivers/media/platform/msm/camss-8x16/ispif.c
@@ -19,6 +19,8 @@
 #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>
@@ -29,16 +31,29 @@
 
 #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_ENABLE   0x0a493249
+#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_ENABLE   0x02493249
+#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_ENABLE   0x00001249
+#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))
@@ -58,14 +73,6 @@
 #define ISPIF_TIMEOUT_SLEEP_US		1000
 #define ISPIF_TIMEOUT_ALL_US		1000000
 
-enum ispif_intf {
-	PIX0,
-	RDI0,
-	PIX1,
-	RDI1,
-	RDI2
-};
-
 enum ispif_intf_cmd {
 	CMD_DISABLE_FRAME_BOUNDARY = 0x0,
 	CMD_ENABLE_FRAME_BOUNDARY = 0x1,
@@ -74,6 +81,25 @@ enum ispif_intf_cmd {
 	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
@@ -86,17 +112,15 @@ static irqreturn_t ispif_isr(int irq, void *dev)
 	struct ispif_device *ispif = dev;
 	u32 value0, value1, value2;
 
-	value0 = readl(ispif->base + ISPIF_VFE_m_IRQ_STATUS_0(0));
-	value1 = readl(ispif->base + ISPIF_VFE_m_IRQ_STATUS_1(0));
-	value2 = readl(ispif->base + ISPIF_VFE_m_IRQ_STATUS_2(0));
+	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(value0, ispif->base + ISPIF_VFE_m_IRQ_CLEAR_0(0));
-	writel(value1, ispif->base + ISPIF_VFE_m_IRQ_CLEAR_1(0));
-	writel(value2, ispif->base + ISPIF_VFE_m_IRQ_CLEAR_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));
 
-	wmb();
 	writel(0x1, ispif->base + ISPIF_IRQ_GLOBAL_CLEAR_CMD);
-	wmb();
 
 	if ((value0 >> 27) & 0x1)
 		complete(&ispif->reset_complete);
@@ -106,22 +130,21 @@ static irqreturn_t ispif_isr(int irq, void *dev)
 
 /*
  * ispif_enable_clocks - Enable clocks for ISPIF module
+ * @nclocks: Number of clocks in clock array
+ * @clock: Clock array
  *
  * Return 0 on success or a negative error code otherwise
  */
-static int ispif_enable_clocks(int nclocks, struct clk **clock,
-			       u8 *clock_for_reset, u8 reset)
+static int ispif_enable_clocks(int nclocks, struct clk **clock)
 {
 	int ret;
 	int i;
 
 	for (i = 0; i < nclocks; i++) {
-		if (clock_for_reset[i] == reset) {
-			ret = clk_prepare_enable(clock[i]);
-			if (ret) {
-				pr_err("clock enable failed\n");
-				goto error;
-			}
+		ret = clk_prepare_enable(clock[i]);
+		if (ret) {
+			pr_err("clock enable failed\n");
+			goto error;
 		}
 	}
 
@@ -129,23 +152,40 @@ static int ispif_enable_clocks(int nclocks, struct clk **clock,
 
 error:
 	for (i--; i >= 0; i--)
-		if (clock_for_reset[i] == reset)
-			clk_disable_unprepare(clock[i]);
+		clk_disable_unprepare(clock[i]);
 
 	return ret;
 }
 
 /*
  * ispif_disable_clocks - Disable clocks for ISPIF module
+ * @nclocks: Number of clocks in clock array
+ * @clock: Clock array
  */
-static void ispif_disable_clocks(int nclocks, struct clk **clock,
-				 u8 *clock_for_reset, u8 reset)
+static void ispif_disable_clocks(int nclocks, struct clk **clock)
 {
 	int i;
 
 	for (i = nclocks - 1; i >= 0; i--)
-		if (clock_for_reset[i] == reset)
-			clk_disable_unprepare(clock[i]);
+		clk_disable_unprepare(clock[i]);
+}
+
+static int ispif_reset(struct ispif_device *ispif)
+{
+	int ret;
+
+	ret = ispif_enable_clocks(ispif->nclocks_for_reset,
+				  ispif->clock_for_reset);
+	if (ret < 0)
+		goto exit;
+
+	writel_relaxed(0x000f1fff, ispif->base + ISPIF_RST_CMD_0);
+	wait_for_completion(&ispif->reset_complete);
+
+	ispif_disable_clocks(ispif->nclocks_for_reset, ispif->clock_for_reset);
+
+exit:
+	return ret;
 }
 
 /*
@@ -157,81 +197,61 @@ static void ispif_disable_clocks(int nclocks, struct clk **clock,
  */
 static int ispif_set_power(struct v4l2_subdev *sd, int on)
 {
-	struct ispif_device *ispif = v4l2_get_subdevdata(sd);
+	struct ispif_line *line = v4l2_get_subdevdata(sd);
+	struct ispif_device *ispif = to_ispif(line);
 	int ret = 0;
 
-	dev_err(ispif->camss->dev, "%s: Enter, on = %d\n",
-		__func__, on);
+	dev_dbg(to_device(ispif), "%s: Enter, ispif%d on = %d\n",
+		__func__, line->id, on);
 
-	if (on)
-		ret = ispif_enable_clocks(ispif->nclocks, ispif->clock,
-					  ispif->clock_for_reset, 0);
-	else
-		ispif_disable_clocks(ispif->nclocks, ispif->clock,
-				     ispif->clock_for_reset, 0);
-
-	dev_err(ispif->camss->dev, "%s: Exit, on = %d\n",
-		__func__, on);
+	if (on) {
+		mutex_lock(&ispif->power_lock);
+		if (ispif->power_count) {
+			/* Power is already on */
+			ispif->power_count++;
+			goto exit;
+		}
 
-	return ret;
-}
+		ret = ispif_enable_clocks(ispif->nclocks, ispif->clock);
+		if (ret < 0)
+			goto exit;
 
-static int ispif_reset(struct ispif_device *ispif)
-{
-	int ret;
+		ret = ispif_reset(ispif);
+		if (ret < 0) {
+			ispif_disable_clocks(ispif->nclocks, ispif->clock);
+			goto exit;
+		}
 
-	ret = ispif_enable_clocks(ispif->nclocks, ispif->clock,
-				  ispif->clock_for_reset, 1);
-	if (ret < 0)
-		goto exit;
+		ispif->intf_cmd[0].cmd_0 = CMD_ALL_NO_CHANGE;
+		ispif->intf_cmd[0].cmd_1 = CMD_ALL_NO_CHANGE;
 
-	writel(0xfe0f1fff, ispif->base + ISPIF_RST_CMD_0);
-	wait_for_completion(&ispif->reset_complete);
+		ispif->power_count++;
+	} else {
+		mutex_lock(&ispif->power_lock);
+		if (ispif->power_count == 0) {
+			dev_err(to_device(ispif),
+				"%s: set_power off on power_count == 0 !!!\n",
+				__func__);
+			goto exit;
+		} else if (ispif->power_count == 1) {
+			ispif_disable_clocks(ispif->nclocks, ispif->clock);
+		}
 
-	ispif_disable_clocks(ispif->nclocks, ispif->clock,
-			     ispif->clock_for_reset, 1);
+		ispif->power_count--;
+	}
 
 exit:
-	return ret;
-}
+	mutex_unlock(&ispif->power_lock);
 
-static void ispif_reset_sw(struct ispif_device *ispif, u8 vfe)
-{
+	dev_dbg(to_device(ispif), "%s: Exit, ispif%d on = %d\n",
+		__func__, line->id, on);
 
-	writel_relaxed(ISPIF_VFE_m_CTRL_0_PIX0_LINE_BUF_EN,
-		       ispif->base + ISPIF_VFE_m_CTRL_0(vfe));
-	writel_relaxed(0, ispif->base + ISPIF_VFE_m_IRQ_MASK_0(vfe));
-	writel_relaxed(0, ispif->base + ISPIF_VFE_m_IRQ_MASK_1(vfe));
-	writel_relaxed(0, ispif->base + ISPIF_VFE_m_IRQ_MASK_2(vfe));
-	writel_relaxed(0xffffffff, ispif->base + ISPIF_VFE_m_IRQ_CLEAR_0(vfe));
-	writel_relaxed(0xffffffff, ispif->base + ISPIF_VFE_m_IRQ_CLEAR_1(vfe));
-	writel_relaxed(0xffffffff, ispif->base + ISPIF_VFE_m_IRQ_CLEAR_2(vfe));
-
-	writel_relaxed(0, ispif->base + ISPIF_VFE_m_INTF_INPUT_SEL(vfe));
-
-	writel_relaxed(CMD_ALL_NO_CHANGE,
-		       ispif->base + ISPIF_VFE_m_INTF_CMD_0(vfe));
-	writel_relaxed(CMD_ALL_NO_CHANGE,
-		       ispif->base + ISPIF_VFE_m_INTF_CMD_1(vfe));
-
-	writel_relaxed(0,
-		       ispif->base + ISPIF_VFE_m_PIX_INTF_n_CID_MASK(vfe, 0));
-	writel_relaxed(0,
-		       ispif->base + ISPIF_VFE_m_PIX_INTF_n_CID_MASK(vfe, 1));
-	writel_relaxed(0,
-		       ispif->base + ISPIF_VFE_m_RDI_INTF_n_CID_MASK(vfe, 0));
-	writel_relaxed(0,
-		       ispif->base + ISPIF_VFE_m_RDI_INTF_n_CID_MASK(vfe, 1));
-	writel_relaxed(0,
-		       ispif->base + ISPIF_VFE_m_RDI_INTF_n_CID_MASK(vfe, 2));
-
-	wmb();
-	writel_relaxed(0x1, ispif->base + ISPIF_IRQ_GLOBAL_CLEAR_CMD);
-	wmb();
+	return ret;
 }
 
 static void ispif_select_clk_mux(struct ispif_device *ispif,
-				 enum ispif_intf intf, u8 csid, u8 vfe)
+				 enum ispif_intf intf, u8 csid,
+				 u8 vfe, u8 enable)
 {
 	u32 val = 0;
 
@@ -239,35 +259,40 @@ static void ispif_select_clk_mux(struct ispif_device *ispif,
 	case PIX0:
 		val = readl_relaxed(ispif->base_clk_mux);
 		val &= ~(0xf << (vfe * 8));
-		val |= (csid << (vfe * 8));
+		if (enable)
+			val |= (csid << (vfe * 8));
 		writel_relaxed(val, ispif->base_clk_mux);
 		break;
 
 	case RDI0:
 		val = readl_relaxed(ispif->base_clk_mux + CSI_RDI_CLK_MUX_SEL);
 		val &= ~(0xf << (vfe * 12));
-		val |= (csid << (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);
 		val &= ~(0xf << (4 + (vfe * 8)));
-		val |= (csid << (4 + (vfe * 8)));
+		if (enable)
+			val |= (csid << (4 + (vfe * 8)));
 		writel_relaxed(val, ispif->base_clk_mux);
 		break;
 
 	case RDI1:
 		val = readl_relaxed(ispif->base_clk_mux + CSI_RDI_CLK_MUX_SEL);
 		val &= ~(0xf << (4 + (vfe * 12)));
-		val |= (csid << (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)));
-		val |= (csid << (8 + (vfe * 12)));
+		if (enable)
+			val |= (csid << (8 + (vfe * 12)));
 		writel_relaxed(val, ispif->base_clk_mux + CSI_RDI_CLK_MUX_SEL);
 		break;
 	}
@@ -279,7 +304,7 @@ static int ispif_validate_intf_status(struct ispif_device *ispif,
 				      enum ispif_intf intf, u8 vfe)
 {
 	int ret = 0;
-	u32 val;
+	u32 val = 0;
 
 	switch (intf) {
 	case PIX0:
@@ -304,14 +329,55 @@ static int ispif_validate_intf_status(struct ispif_device *ispif,
 		break;
 	}
 
-	if ((val & 0xf) != 0xf)
+	if ((val & 0xf) != 0xf) {
+		dev_err(to_device(ispif), "%s: ispif is busy: 0x%x\n",
+			__func__, val);
 		ret = -EBUSY;
+	}
 
 	return ret;
 }
 
-static void ispif_select_csid(struct ispif_device *ispif,
-			      enum ispif_intf intf, u8 csid, u8 vfe)
+static int ispif_wait_for_stop(struct ispif_device *ispif,
+			       enum ispif_intf intf, u8 vfe)
+{
+	int ret;
+	u32 addr = 0;
+	u32 stop_flag = 0;
+
+	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;
+}
+
+static void ispif_select_csid(struct ispif_device *ispif, enum ispif_intf intf,
+			      u8 csid, u8 vfe, u8 enable)
 {
 	u32 val;
 
@@ -319,41 +385,45 @@ static void ispif_select_csid(struct ispif_device *ispif,
 	switch (intf) {
 	case PIX0:
 		val &= ~(BIT(1) | BIT(0));
-		val |= csid;
+		if (enable)
+			val |= csid;
 		break;
 	case RDI0:
 		val &= ~(BIT(5) | BIT(4));
-		val |= (csid << 4);
+		if (enable)
+			val |= (csid << 4);
 		break;
 	case PIX1:
 		val &= ~(BIT(9) | BIT(8));
-		val |= (csid << 8);
+		if (enable)
+			val |= (csid << 8);
 		break;
 	case RDI1:
 		val &= ~(BIT(13) | BIT(12));
-		val |= (csid << 12);
+		if (enable)
+			val |= (csid << 12);
 		break;
 	case RDI2:
 		val &= ~(BIT(21) | BIT(20));
-		val |= (csid << 20);
+		if (enable)
+			val |= (csid << 20);
 		break;
 	}
 
-	wmb();
-	writel_relaxed(val, ispif->base + ISPIF_VFE_m_INTF_INPUT_SEL(vfe));
-	wmb();
+	writel(val, ispif->base + ISPIF_VFE_m_INTF_INPUT_SEL(vfe));
 }
 
 static void ispif_enable_cid(struct ispif_device *ispif, enum ispif_intf intf,
 			     u16 cid_mask, u8 vfe, u8 enable)
 {
-	u32 addr, val;
+	u32 addr = 0, val;
 
 	switch (intf) {
 	case PIX0:
 		addr = ISPIF_VFE_m_PIX_INTF_n_CID_MASK(vfe, 0);
 		break;
 	case RDI0:
+	default:
 		addr = ISPIF_VFE_m_RDI_INTF_n_CID_MASK(vfe, 0);
 		break;
 	case PIX1:
@@ -373,45 +443,83 @@ static void ispif_enable_cid(struct ispif_device *ispif, enum ispif_intf intf,
 	else
 		val &= ~cid_mask;
 
-	wmb();
-	writel_relaxed(val, ispif->base + addr);
-	wmb();
+	writel(val, ispif->base + addr);
 }
 
-static void ispif_config_irq(struct ispif_device *ispif, u8 vfe)
+static void ispif_config_irq(struct ispif_device *ispif, enum ispif_intf intf,
+			     u8 vfe, u8 enable)
 {
 	u32 val;
 
-	val = ISPIF_VFE_m_IRQ_MASK_0_ENABLE;
-	writel(val, ispif->base + ISPIF_VFE_m_IRQ_MASK_0(vfe));
-	writel(val, ispif->base + ISPIF_VFE_m_IRQ_CLEAR_0(vfe));
-	val = ISPIF_VFE_m_IRQ_MASK_1_ENABLE;
-	writel(val, ispif->base + ISPIF_VFE_m_IRQ_MASK_1(vfe));
-	writel(val, ispif->base + ISPIF_VFE_m_IRQ_CLEAR_1(vfe));
-	val = ISPIF_VFE_m_IRQ_MASK_2_ENABLE;
-	writel(val, ispif->base + ISPIF_VFE_m_IRQ_MASK_2(vfe));
-	writel(val, ispif->base + ISPIF_VFE_m_IRQ_CLEAR_2(vfe));
-	wmb();
+	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);
-	wmb();
 }
 
-static void ispif_intf_cmd(struct ispif_device *ispif, u8 cmd,
-			   enum ispif_intf intf, u8 vfe, u8 vc)
+static void ispif_set_intf_cmd(struct ispif_device *ispif, u8 cmd,
+			       enum ispif_intf intf, u8 vfe, u8 vc)
 {
-	u32 val = CMD_ALL_NO_CHANGE;
+	u32 *val;
 
 	if (intf == RDI2) {
-		val &= ~(0x3 << (vc * 2 + 8));
-		val |= (cmd << (vc * 2 + 8));
+		val = &ispif->intf_cmd[0].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));
+		writel_relaxed(*val, ispif->base + ISPIF_VFE_m_INTF_CMD_1(vfe));
 		wmb();
 	} else {
-		val &= ~(0x3 << (vc * 2 + intf * 8));
-		val |= (cmd << (vc * 2 + intf * 8));
+		val = &ispif->intf_cmd[0].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));
+		writel_relaxed(*val, ispif->base + ISPIF_VFE_m_INTF_CMD_0(vfe));
 		wmb();
 	}
 }
@@ -427,68 +535,280 @@ static void ispif_intf_cmd(struct ispif_device *ispif, u8 cmd,
  */
 static int ispif_set_stream(struct v4l2_subdev *sd, int enable)
 {
-	struct ispif_device *ispif = v4l2_get_subdevdata(sd);
-	enum ispif_intf ispif_intf = RDI0;
-	u8 vfe = 0;
+	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; /* TODO: How to get this from sensor? */
 	u8 cid = vc * 4;
 
 	int ret;
 
-	dev_err(ispif->camss->dev, "%s: Enter, enable = %d\n",
-		__func__, enable);
+	dev_dbg(to_device(ispif), "%s: Enter, ispif%d enable = %d\n",
+		__func__, line->id, enable);
 
 	if (enable) {
-		u8 csid = ispif->csid_id;
-
 		if (!media_entity_remote_pad(
-					&ispif->pads[MSM_ISPIF_PAD_SINK])) {
+					&line->pads[MSM_ISPIF_PAD_SINK])) {
 			return -ENOLINK;
 		}
 
-		/* Reset */
+		/* Config */
+
+		mutex_lock(&ispif->config_lock);
+		ispif_select_clk_mux(ispif, intf, csid, vfe, 1);
 
-		ret = ispif_reset(ispif);
+		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_enable_cid(ispif, intf, 1 << 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;
 
-		/* Config */
+		mutex_lock(&ispif->config_lock);
+		ispif_config_irq(ispif, intf, vfe, 0);
+		ispif_enable_cid(ispif, intf, 1 << 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_reset_sw(ispif, vfe);
+/*
+ * __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);
 
-		ispif_select_clk_mux(ispif, ispif_intf, csid, vfe);
+	return &line->fmt[pad];
+}
 
-		ret = ispif_validate_intf_status(ispif, ispif_intf, vfe);
-		if (ret < 0)
-			return ret;
+/*
+ * 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 */
 
-		ispif_select_csid(ispif, ispif_intf, csid, vfe);
+		for (i = 0; i < ARRAY_SIZE(ispif_formats); i++)
+			if (fmt->code == ispif_formats[i])
+				break;
 
-		ispif_enable_cid(ispif, ispif_intf, 1 << cid, vfe, 1);
+		/* If not found, use UYVY as default */
+		if (i >= ARRAY_SIZE(ispif_formats))
+			fmt->code = MEDIA_BUS_FMT_UYVY8_2X8;
 
-		ispif_config_irq(ispif, vfe);
+		fmt->width = clamp_t(u32, fmt->width, 1, 8191);
+		fmt->height = clamp_t(u32, fmt->height, 1, 8191);
 
-		ispif_intf_cmd(ispif, CMD_ENABLE_FRAME_BOUNDARY, ispif_intf, vfe, vc);
+		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 {
-		u32 stop_flag = 0;
+		if (code->index > 0)
+			return -EINVAL;
 
-		ispif_intf_cmd(ispif, CMD_DISABLE_FRAME_BOUNDARY, ispif_intf, vfe, vc);
+		format = __ispif_get_format(line, cfg, MSM_ISPIF_PAD_SINK,
+					    code->which);
 
-		ret = readl_poll_timeout(ispif->base + ISPIF_VFE_m_RDI_INTF_n_STATUS(vfe, 0),
-					 stop_flag,
-					 (stop_flag & 0xf) == 0xf,
-					 ISPIF_TIMEOUT_SLEEP_US,
-					 ISPIF_TIMEOUT_ALL_US);
-		if (ret < 0)
-			return ret;
+		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_enable_cid(ispif, ispif_intf, 1 << cid, vfe, 0);
+	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
+ *
+ * Initialize all pad formats with default values.
+ */
+static int ispif_init_formats(struct v4l2_subdev *sd)
+{
+	struct v4l2_subdev_format format;
+
+	memset(&format, 0, sizeof(format));
+	format.pad = MSM_ISPIF_PAD_SINK;
+	format.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+	format.format.code = MEDIA_BUS_FMT_UYVY8_2X8;
+	format.format.width = 1920;
+	format.format.height = 1080;
+	ispif_set_format(sd, NULL, &format);
+
+	return 0;
+}
+
+/*
  * msm_ispif_subdev_init - Initialize ISPIF device structure and resources
  * @ispif: ISPIF device
  * @camss: Camera sub-system structure
@@ -496,16 +816,22 @@ static int ispif_set_stream(struct v4l2_subdev *sd, int enable)
  *
  * Return 0 on success or a negative error code otherwise
  */
-int msm_ispif_subdev_init(struct ispif_device *ispif, struct camss *camss,
+int msm_ispif_subdev_init(struct ispif_device *ispif,
 			  struct resources_ispif *res)
 {
-	struct device *dev = camss->dev;
+	struct device *dev = to_device(ispif);
 	struct platform_device *pdev = container_of(dev, struct platform_device, dev);
 	struct resource *r;
 	int i;
 	int ret;
 
-	ispif->camss = camss;
+	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 */
 
@@ -539,9 +865,8 @@ int msm_ispif_subdev_init(struct ispif_device *ispif, struct camss *camss,
 
 	/* Clocks */
 
-	i = 0;
 	ispif->nclocks = 0;
-	while (res->clock[i++])
+	while (res->clock[ispif->nclocks])
 		ispif->nclocks++;
 
 	ispif->clock = devm_kzalloc(dev, ispif->nclocks * sizeof(*ispif->clock),
@@ -551,18 +876,29 @@ int msm_ispif_subdev_init(struct ispif_device *ispif, struct camss *camss,
 		return -ENOMEM;
 	}
 
-	ispif->clock_for_reset = devm_kzalloc(dev, ispif->nclocks *
+	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) {
 		dev_err(dev, "could not allocate memory\n");
 		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->clock_for_reset[i] = res->clock_for_reset[i];
+	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);
@@ -570,6 +906,20 @@ int msm_ispif_subdev_init(struct ispif_device *ispif, struct camss *camss,
 	return 0;
 }
 
+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;
+	}
+}
+
 static int ispif_link_setup(struct media_entity *entity,
 			   const struct media_pad *local,
 			   const struct media_pad *remote, u32 flags)
@@ -577,16 +927,24 @@ static int ispif_link_setup(struct media_entity *entity,
 	if ((local->flags & MEDIA_PAD_FL_SINK) &&
 	    (flags & MEDIA_LNK_FL_ENABLED)) {
 		struct v4l2_subdev *sd;
-		struct ispif_device *ispif;
-		struct csid_device *csid;
+		struct ispif_line *line;
 
 		sd = container_of(entity, struct v4l2_subdev, entity);
-		ispif = v4l2_get_subdevdata(sd);
+		line = v4l2_get_subdevdata(sd);
 
-		sd = container_of(remote->entity, struct v4l2_subdev, entity);
-		csid = v4l2_get_subdevdata(sd);
+		msm_csid_get_csid_id(remote->entity, &line->csid_id);
+	} else if ((local->flags & MEDIA_PAD_FL_SOURCE) &&
+		   (flags & MEDIA_LNK_FL_ENABLED)) {
+		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);
 
-		ispif->csid_id = csid->id;
+		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;
@@ -600,7 +958,12 @@ 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;
+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,
@@ -612,6 +975,7 @@ static const struct v4l2_subdev_internal_ops ispif_v4l2_internal_ops;
 
 static const struct media_entity_operations ispif_media_ops = {
 	.link_setup = ispif_link_setup,
+	.link_validate = v4l2_subdev_link_validate,
 };
 
 /*
@@ -624,29 +988,48 @@ static const struct media_entity_operations ispif_media_ops = {
 int msm_ispif_register_entities(struct ispif_device *ispif,
 				struct v4l2_device *v4l2_dev)
 {
-	struct v4l2_subdev *sd = &ispif->subdev;
-	struct media_pad *pads = ispif->pads;
 	int ret;
+	int i;
 
-	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), MSM_ISPIF_NAME);
-	v4l2_set_subdevdata(sd, ispif);
+	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;
 
-	pads[MSM_ISPIF_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
-	pads[MSM_ISPIF_PAD_SRC].flags = MEDIA_PAD_FL_SOURCE;
+		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]);
 
-	sd->entity.ops = &ispif_media_ops;
-	ret = media_entity_init(&sd->entity, MSM_ISPIF_PADS_NUM, pads, 0);
-	if (ret < 0) {
-		pr_err("Fail to init media entity");
-		return ret;
+		ispif_init_formats(sd);
+
+		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_init(&sd->entity, MSM_ISPIF_PADS_NUM,
+					pads, 0);
+		if (ret < 0) {
+			pr_err("Fail to init media entity");
+			goto error;
+		}
+
+		ret = v4l2_device_register_subdev(v4l2_dev, sd);
+		if (ret < 0) {
+			pr_err("Fail to register subdev");
+			media_entity_cleanup(&sd->entity);
+			goto error;
+		}
 	}
 
-	ret = v4l2_device_register_subdev(v4l2_dev, sd);
-	if (ret < 0) {
-		pr_err("Fail to register subdev");
+	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);
 	}
 
@@ -659,5 +1042,12 @@ int msm_ispif_register_entities(struct ispif_device *ispif,
  */
 void msm_ispif_unregister_entities(struct ispif_device *ispif)
 {
-	v4l2_device_unregister_subdev(&ispif->subdev);
+	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/msm/camss-8x16/ispif.h b/drivers/media/platform/msm/camss-8x16/ispif.h
index 01054031735c..ab6ba6ef84d0 100644
--- a/drivers/media/platform/msm/camss-8x16/ispif.h
+++ b/drivers/media/platform/msm/camss-8x16/ispif.h
@@ -23,29 +23,57 @@
 #include <media/v4l2-device.h>
 #include <media/v4l2-subdev.h>
 
+/* Number of ISPIF lines - same as number of CSID hardware modules */
+#define ISPIF_LINE_NUM 2
+
 #define MSM_ISPIF_PAD_SINK 0
 #define MSM_ISPIF_PAD_SRC 1
 #define MSM_ISPIF_PADS_NUM 2
 
-struct camss;
+#define MSM_ISPIF_VFE_NUM 1
 
-struct ispif_device {
+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 camss *camss;
+	struct v4l2_mbus_framefmt fmt[MSM_ISPIF_PADS_NUM];
+};
+
+struct ispif_device {
 	void __iomem *base;
 	void __iomem *base_clk_mux;
 	u32 irq;
 	struct clk **clock;
-	u8 *clock_for_reset;
 	int nclocks;
+	struct clk **clock_for_reset;
+	int nclocks_for_reset;
 	struct completion reset_complete;
-	u8 csid_id;
+	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[ISPIF_LINE_NUM];
 };
 
 struct resources_ispif;
 
-int msm_ispif_subdev_init(struct ispif_device *ispif, struct camss *camss,
+int msm_ispif_subdev_init(struct ispif_device *ispif,
 			  struct resources_ispif *res);
 
 int msm_ispif_register_entities(struct ispif_device *ispif,
diff --git a/drivers/media/platform/msm/camss-8x16/vfe.c b/drivers/media/platform/msm/camss-8x16/vfe.c
index e392e36b4aea..b7e0e402b266 100644
--- a/drivers/media/platform/msm/camss-8x16/vfe.c
+++ b/drivers/media/platform/msm/camss-8x16/vfe.c
@@ -17,6 +17,7 @@
  */
 #include <asm/dma-iommu.h>
 #include <linux/clk.h>
+#include <linux/completion.h>
 #include <linux/interrupt.h>
 #include <linux/msm-bus.h>
 #include <linux/mutex.h>
@@ -32,9 +33,15 @@
 #include "vfe.h"
 #include "camss.h"
 
-#define MSM_VFE_DRV_NAME "msm_vfe"
+#define MSM_VFE_NAME "msm_vfe"
 #define MSM_VFE_VIDEO_NAME "msm_vfe_video"
 
+#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
@@ -52,29 +59,19 @@
 #define VFE_0_IRQ_CMD_GLOBAL_CLEAR	(1 << 0)
 
 #define VFE_0_IRQ_MASK_0		0x028
-#define VFE_0_IRQ_MASK_0_IMAGE_MASTER_0_PING_PONG	(1 << 8)
-#define VFE_0_IRQ_MASK_0_IMAGE_MASTER_1_PING_PONG	(1 << 9)
-#define VFE_0_IRQ_MASK_0_IMAGE_MASTER_2_PING_PONG	(1 << 10)
-#define VFE_0_IRQ_MASK_0_IMAGE_MASTER_3_PING_PONG	(1 << 11)
-#define VFE_0_IRQ_MASK_0_IMAGE_MASTER_4_PING_PONG	(1 << 12)
-#define VFE_0_IRQ_MASK_0_IMAGE_MASTER_5_PING_PONG	(1 << 13)
-#define VFE_0_IRQ_MASK_0_IMAGE_MASTER_6_PING_PONG	(1 << 14)
+#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_0_BUS_OVERFLOW	(1 << 9)
-#define VFE_0_IRQ_MASK_1_IMAGE_MASTER_1_BUS_OVERFLOW	(1 << 10)
-#define VFE_0_IRQ_MASK_1_IMAGE_MASTER_2_BUS_OVERFLOW	(1 << 11)
+#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_0_ALL		0xffffffff
-
 #define VFE_0_IRQ_CLEAR_1		0x034
-#define VFE_0_IRQ_CLEAR_1_ALL		0xffffffff
 
 #define VFE_0_IRQ_STATUS_0		0x038
-#define VFE_0_IRQ_STATUS_0_IMAGE_MASTER_0_PING_PONG	(1 << 8)
+#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)
@@ -85,7 +82,7 @@
 
 #define VFE_0_BUS_CFG			0x050
 
-#define VFE_0_BUS_XBAR_CFG_x(x)		(0x58 + 0x4 * (x))
+#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
@@ -122,63 +119,64 @@
 
 #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_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_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_RDI0			(1 << 1)
+#define VFE_0_REG_UPDATE_RDIn(n)		(1 << (1 + (n)))
 #define VFE_0_REG_UPDATE_RDI1			(1 << 2)
 #define VFE_0_REG_UPDATE_RDI2			(1 << 3)
 
 #define VFE_0_CGC_OVERRIDE_1			0x974
-#define VFE_0_CGC_OVERRIDE_1_IMAGE_M0_CGC_OVERRIDE	1
+#define VFE_0_CGC_OVERRIDE_1_IMAGE_Mx_CGC_OVERRIDE(x)	(1 << (x))
 
 /* Vfe reset timeout */
-#define MSM_VFE_RESET_TIMEOUT_MS 50
+#define VFE_RESET_TIMEOUT_MS 50
 /* Vfe halt timeout */
-#define MSM_VFE_HALT_TIMEOUT_MS 100
+#define VFE_HALT_TIMEOUT_MS 100
 /* Max number of frame drop updates per frame */
-#define MSM_VFE_FRAME_DROP_UPDATES 5
+#define VFE_FRAME_DROP_UPDATES 5
 /* Frame drop value NOTE it VAL + UPDATES should not exceed 31 */
-#define MSM_VFE_FRAME_DROP_VAL 20
-
-
-static char *clocks[] = {
-	"camss_top_ahb_clk",
-	"vfe_clk_src",
-	"camss_vfe_vfe_clk",
-	"camss_csi_vfe_clk",
-	"iface_clk",
-	"bus_clk",
-	"camss_ahb_clk"
+#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 char *reg = "vfe0";
-static char *reg_vbif = "vfe0_vbif";
-
-static char *interrupt = "vfe0";
-
-static inline void msm_vfe_reg_clr(struct vfe_device *vfe,
-	u32 reg, u32 clr_bits)
+static inline void vfe_reg_clr(struct vfe_device *vfe, u32 reg, u32 clr_bits)
 {
 	u32 bits = readl(vfe->base + reg);
 
 	writel(bits & ~clr_bits, vfe->base + reg);
 }
 
-static inline void msm_vfe_reg_set(struct vfe_device *vfe,
-	u32 reg, u32 set_bits)
+static inline void vfe_reg_set(struct vfe_device *vfe, u32 reg, u32 set_bits)
 {
 	u32 bits = readl(vfe->base + reg);
 
 	writel(bits | set_bits, vfe->base + reg);
 }
 
-static void msm_vfe_global_reset(struct vfe_device *vfe)
+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	|
@@ -193,27 +191,25 @@ static void msm_vfe_global_reset(struct vfe_device *vfe)
 	writel(reset_bits, vfe->base + VFE_0_GLOBAL_RESET_CMD);
 }
 
-static void msm_vfe_wm_enable(struct vfe_device *vfe, u32 wm, u32 enable)
+static void vfe_wm_enable(struct vfe_device *vfe, u32 wm, u32 enable)
 {
 	if (enable)
-		msm_vfe_reg_set(vfe, VFE_0_BUS_IMAGE_MASTER_n_WR_CFG(wm), 1);
+		vfe_reg_set(vfe, VFE_0_BUS_IMAGE_MASTER_n_WR_CFG(wm), 1);
 	else
-		msm_vfe_reg_clr(vfe, VFE_0_BUS_IMAGE_MASTER_n_WR_CFG(wm), 1);
+		vfe_reg_clr(vfe, VFE_0_BUS_IMAGE_MASTER_n_WR_CFG(wm), 1);
 }
 
-static void msm_vfe_wm_frame_based(struct vfe_device *vfe, u32 wm, u32 enable)
+static void vfe_wm_frame_based(struct vfe_device *vfe, u32 wm, u32 enable)
 {
-	if (enable) {
-		msm_vfe_reg_set(vfe, VFE_0_BUS_IMAGE_MASTER_n_WR_CFG(wm),
+	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 {
-		msm_vfe_reg_clr(vfe, VFE_0_BUS_IMAGE_MASTER_n_WR_CFG(wm),
+	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 msm_vfe_wm_set_framedrop_period(struct vfe_device *vfe,
-					    u32 wm, u32 per)
+static void vfe_wm_set_framedrop_period(struct vfe_device *vfe, u32 wm, u32 per)
 {
 	u32 reg;
 
@@ -221,43 +217,45 @@ static void msm_vfe_wm_set_framedrop_period(struct vfe_device *vfe,
 
 	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;
+	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(reg, vfe->base + VFE_0_BUS_IMAGE_MASTER_n_WR_ADDR_CFG(wm));
 }
 
-static void msm_vfe_wm_set_framedrop_pattern(struct vfe_device *vfe,
-					     u32 wm, u32 pat)
+static void vfe_wm_set_framedrop_pattern(struct vfe_device *vfe, u32 wm,
+					 u32 pattern)
 {
-	writel(pat, vfe->base + VFE_0_BUS_IMAGE_MASTER_n_WR_FRAMEDROP_PATTERN(wm));
+	writel(pattern,
+	       vfe->base + VFE_0_BUS_IMAGE_MASTER_n_WR_FRAMEDROP_PATTERN(wm));
 }
 
-static void msm_vfe_wm_set_ub_cfg(struct vfe_device *vfe, u32 wm,
-				u16 offset, u16 depth)
+static void vfe_wm_set_ub_cfg(struct vfe_device *vfe, u32 wm, u16 offset,
+			      u16 depth)
 {
 	u32 reg;
 
-	reg = (offset << VFE_0_BUS_IMAGE_MASTER_n_WR_UB_CFG_OFFSET_SHIFT) | depth;
+	reg = (offset << VFE_0_BUS_IMAGE_MASTER_n_WR_UB_CFG_OFFSET_SHIFT) |
+		depth;
 	writel(reg, vfe->base + VFE_0_BUS_IMAGE_MASTER_n_WR_UB_CFG(wm));
 }
 
-static void msm_vfe_bus_reload_wm(struct vfe_device *vfe, u32 wm)
+static void vfe_bus_reload_wm(struct vfe_device *vfe, u32 wm)
 {
 	writel(VFE_0_BUS_CMD_Mx_RLD_CMD(wm), vfe->base + VFE_0_BUS_CMD);
 }
 
-static void msm_vfe_wm_set_ping_addr(struct vfe_device *vfe, u32 wm, u32 addr)
+static void vfe_wm_set_ping_addr(struct vfe_device *vfe, u32 wm, u32 addr)
 {
 	writel(addr, vfe->base + VFE_0_BUS_IMAGE_MASTER_n_WR_PING_ADDR(wm));
 }
 
-static void msm_vfe_wm_set_pong_addr(struct vfe_device *vfe, u32 wm, u32 addr)
+static void vfe_wm_set_pong_addr(struct vfe_device *vfe, u32 wm, u32 addr)
 {
 	writel(addr, vfe->base + VFE_0_BUS_IMAGE_MASTER_n_WR_PONG_ADDR(wm));
 }
 
-static int msm_vfe_wm_get_ping_pong_status(struct vfe_device *vfe, u32 wm)
+static int vfe_wm_get_ping_pong_status(struct vfe_device *vfe, u32 wm)
 {
 	u32 reg;
 
@@ -266,7 +264,7 @@ static int msm_vfe_wm_get_ping_pong_status(struct vfe_device *vfe, u32 wm)
 	return (reg >> wm) & 0x1;
 }
 
-static void msm_vfe_bus_enable_wr_if(struct vfe_device *vfe, u32 enable)
+static void vfe_bus_enable_wr_if(struct vfe_device *vfe, u32 enable)
 {
 	if (enable)
 		writel(0x10000009, vfe->base + VFE_0_BUS_CFG);
@@ -274,38 +272,44 @@ static void msm_vfe_bus_enable_wr_if(struct vfe_device *vfe, u32 enable)
 		writel(0, vfe->base + VFE_0_BUS_CFG);
 }
 
-static int msm_vfe_bus_connect_wm_to_rdi(struct vfe_device *vfe, u32 wm, u32 rdi)
+static int vfe_bus_connect_wm_to_rdi(struct vfe_device *vfe, u32 wm, enum vfe_line_id id)
 {
 	u32 reg;
 
+	if (id != VFE_LINE_RDI0 && id != VFE_LINE_RDI1 && id != VFE_LINE_RDI2)
+		return -EINVAL;
+
 	reg = VFE_0_RDI_CFG_x_MIPI_EN_BITS;
-	reg |= VFE_0_RDI_CFG_x_RDI_Mr_FRAME_BASED_EN(rdi);
-	msm_vfe_reg_set(vfe, VFE_0_RDI_CFG_x(0), reg);
+	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 |= (wm << VFE_0_RDI_CFG_x_RDI_STREAM_SEL_SHIFT) &
+	reg |= ((3 * id) << VFE_0_RDI_CFG_x_RDI_STREAM_SEL_SHIFT) &
 		VFE_0_RDI_CFG_x_RDI_STREAM_SEL_MASK;
-	msm_vfe_reg_set(vfe, VFE_0_RDI_CFG_x(rdi), reg);
+	vfe_reg_set(vfe, VFE_0_RDI_CFG_x(id), reg);
 
-	switch (rdi) {
-	case 0:
+	switch (id) {
+	case VFE_LINE_RDI0:
 		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 1:
+	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 2:
+	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;
 	default:
-		dev_err(vfe->camss->dev, "Invalid rdi %d\n", rdi);
+		dev_err(to_device(vfe), "Invalid rdi %d\n", id);
 		return -EINVAL;
 	}
 
-	writel(reg, vfe->base + VFE_0_BUS_XBAR_CFG_x(wm));
+	if (wm % 2 == 1)
+		reg <<= 16;
+
+	vfe_reg_set(vfe, VFE_0_BUS_XBAR_CFG_x(wm), reg);
 
 	writel(VFE_0_BUS_IMAGE_MASTER_n_WR_IRQ_SUBSAMPLE_PATTERN_DEF,
 	       vfe->base + VFE_0_BUS_IMAGE_MASTER_n_WR_IRQ_SUBSAMPLE_PATTERN(wm));
@@ -313,101 +317,113 @@ static int msm_vfe_bus_connect_wm_to_rdi(struct vfe_device *vfe, u32 wm, u32 rdi
 	return 0;
 }
 
-static int msm_vfe_bus_dicconnect_wm_from_rdi(struct vfe_device *vfe, u32 rdi)
+static int vfe_bus_disconnect_wm_from_rdi(struct vfe_device *vfe, u32 wm, enum vfe_line_id id)
 {
-	writel(0x0, vfe->base + VFE_0_RDI_CFG_x(rdi));
+	u32 reg;
 
-	return 0;
-}
+	if (id != VFE_LINE_RDI0 && id != VFE_LINE_RDI1 && id != VFE_LINE_RDI2)
+		return -EINVAL;
 
-static void msm_vfe_set_rdi_cid(struct vfe_device *vfe, u32 rdi_idx, u8 cid)
-{
-	msm_vfe_reg_clr(vfe,
-		VFE_0_RDI_CFG_x(rdi_idx),
-		VFE_0_RDI_CFG_x_RDI_M0_SEL_MASK);
+	reg = VFE_0_RDI_CFG_x_RDI_Mr_FRAME_BASED_EN(id);
+	vfe_reg_clr(vfe, VFE_0_RDI_CFG_x(0), reg);
 
-	msm_vfe_reg_set(vfe,
-		VFE_0_RDI_CFG_x(rdi_idx),
-		cid << VFE_0_RDI_CFG_x_RDI_M0_SEL_SHIFT);
-}
+	reg = VFE_0_RDI_CFG_x_RDI_EN_BIT;
+	vfe_reg_clr(vfe, VFE_0_RDI_CFG_x(id), reg);
 
-static int msm_vfe_reg_update(struct vfe_device *vfe, int rdi_idx)
-{
-	switch (rdi_idx) {
-	case 0:
-		writel(VFE_0_REG_UPDATE_RDI0, vfe->base + VFE_0_REG_UPDATE);
+	switch (id) {
+	case VFE_LINE_RDI0:
+		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 1:
-		writel(VFE_0_REG_UPDATE_RDI1, vfe->base + VFE_0_REG_UPDATE);
+	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 2:
-		writel(VFE_0_REG_UPDATE_RDI2, vfe->base + VFE_0_REG_UPDATE);
+	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;
 	default:
-		dev_err(vfe->camss->dev, "Invalid vfe interface %d\n", rdi_idx);
+		dev_err(to_device(vfe), "Invalid rdi %d\n", id);
 		return -EINVAL;
 	}
 
+	if (wm % 2 == 1)
+		reg <<= 16;
+
+	vfe_reg_clr(vfe, VFE_0_BUS_XBAR_CFG_x(wm), reg);
+
 	return 0;
 }
 
-static void msm_vfe_irq_clear(struct vfe_device *vfe, u32 clr_0, u32 clr_1)
+static int vfe_set_rdi_cid(struct vfe_device *vfe, enum vfe_line_id id, u8 cid)
 {
-	writel(clr_0, vfe->base + VFE_0_IRQ_CLEAR_0);
+	if (id != VFE_LINE_RDI0 && id != VFE_LINE_RDI1 && id != VFE_LINE_RDI2)
+		return -EINVAL;
 
-	writel(clr_1, vfe->base + VFE_0_IRQ_CLEAR_1);
+	vfe_reg_clr(vfe, VFE_0_RDI_CFG_x(id),
+		    VFE_0_RDI_CFG_x_RDI_M0_SEL_MASK);
 
-	wmb();
+	vfe_reg_set(vfe, VFE_0_RDI_CFG_x(id),
+		    cid << VFE_0_RDI_CFG_x_RDI_M0_SEL_SHIFT);
 
-	writel(VFE_0_IRQ_CMD_GLOBAL_CLEAR, vfe->base + VFE_0_IRQ_CMD);
+	return 0;
 }
 
-static void msm_vfe_enable_irq_0(struct vfe_device *vfe)
+static int vfe_reg_update(struct vfe_device *vfe, enum vfe_line_id line_id)
 {
-	u32 irq_en = VFE_0_IRQ_MASK_0_IMAGE_MASTER_0_PING_PONG |
-		     VFE_0_IRQ_MASK_0_IMAGE_MASTER_1_PING_PONG |
-		     VFE_0_IRQ_MASK_0_IMAGE_MASTER_2_PING_PONG |
-		     VFE_0_IRQ_MASK_0_IMAGE_MASTER_3_PING_PONG |
-		     VFE_0_IRQ_MASK_0_IMAGE_MASTER_4_PING_PONG |
-		     VFE_0_IRQ_MASK_0_IMAGE_MASTER_5_PING_PONG |
-		     VFE_0_IRQ_MASK_0_IMAGE_MASTER_6_PING_PONG |
-		     VFE_0_IRQ_MASK_0_RESET_ACK;
+	vfe->reg_update |= VFE_0_REG_UPDATE_RDIn(line_id);
+	writel(vfe->reg_update, vfe->base + VFE_0_REG_UPDATE);
 
-	writel(irq_en, vfe->base + VFE_0_IRQ_MASK_0);
+	return 0;
 }
 
-static void msm_vfe_enable_irq_1(struct vfe_device *vfe)
-{
-	u32 irq_en = VFE_0_IRQ_MASK_1_VIOLATION |
-		     VFE_0_IRQ_MASK_1_BUS_BDG_HALT_ACK |
-		     VFE_0_IRQ_MASK_1_IMAGE_MASTER_0_BUS_OVERFLOW |
-		     VFE_0_IRQ_MASK_1_IMAGE_MASTER_1_BUS_OVERFLOW |
-		     VFE_0_IRQ_MASK_1_IMAGE_MASTER_2_BUS_OVERFLOW;
+static void vfe_enable_irq_wm(struct vfe_device *vfe, u32 wm_idx, 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_idx);
+		irq_en1 |= VFE_0_IRQ_MASK_1_IMAGE_MASTER_n_BUS_OVERFLOW(wm_idx);
+	} else {
+		irq_en0 &= ~VFE_0_IRQ_MASK_0_IMAGE_MASTER_n_PING_PONG(wm_idx);
+		irq_en1 &= ~VFE_0_IRQ_MASK_1_IMAGE_MASTER_n_BUS_OVERFLOW(wm_idx);
+	}
 
-	writel(irq_en, vfe->base + VFE_0_IRQ_MASK_1);
+	writel_relaxed(irq_en0, vfe->base + VFE_0_IRQ_MASK_0);
+	writel_relaxed(irq_en1, vfe->base + VFE_0_IRQ_MASK_1);
 }
 
-static void msm_vfe_enable_irq_all(struct vfe_device *vfe)
+static void vfe_enable_irq_common(struct vfe_device *vfe)
 {
-	msm_vfe_irq_clear(vfe, VFE_0_IRQ_CLEAR_0_ALL, VFE_0_IRQ_CLEAR_1_ALL);
-	msm_vfe_enable_irq_0(vfe);
-	msm_vfe_enable_irq_1(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);
 }
 
-static void msm_vfe_disable_irq_all(struct vfe_device *vfe)
+static void vfe_isr_reg_update(struct vfe_device *vfe, enum vfe_line_id line_id)
 {
+	unsigned long flags;
 
-	writel(0x0, vfe->base + VFE_0_IRQ_MASK_0);
-	writel(0x0, vfe->base + VFE_0_IRQ_MASK_1);
-	msm_vfe_irq_clear(vfe, VFE_0_IRQ_CLEAR_0_ALL, VFE_0_IRQ_CLEAR_1_ALL);
+	spin_lock_irqsave(&vfe->output_lock, flags);
+	vfe->reg_update &= ~VFE_0_REG_UPDATE_RDIn(line_id);
+	spin_unlock_irqrestore(&vfe->output_lock, flags);
 }
 
-static void msm_vfe_isr_wm_done(struct vfe_device *vfe, u32 wm_idx);
+static void vfe_isr_wm_done(struct vfe_device *vfe, u32 wm_idx);
 
-static irqreturn_t msm_vfe_subdev_isr(int irq, void *dev)
+static irqreturn_t vfe_subdev_isr(int irq, void *dev)
 {
 	struct vfe_device *vfe = dev;
 	u32 value0, value1;
+	int i;
 
 	value0 = readl(vfe->base + VFE_0_IRQ_STATUS_0);
 	value1 = readl(vfe->base + VFE_0_IRQ_STATUS_1);
@@ -421,34 +437,41 @@ static irqreturn_t msm_vfe_subdev_isr(int irq, void *dev)
 	if (value0 & VFE_0_IRQ_STATUS_0_RESET_ACK)
 		complete_all(&vfe->reset_completion);
 
-	if (value1 & VFE_0_IRQ_STATUS_1_BUS_BDG_HALT_ACK)
+	if (value1 & VFE_0_IRQ_STATUS_1_BUS_BDG_HALT_ACK) {
 		complete_all(&vfe->halt_completion);
+		writel(0x0, vfe->base + VFE_0_BUS_BDG_CMD);
+	}
+
+	for (i = VFE_LINE_RDI0; i < VFE_LINE_RDI2 + 1; i++)
+		if (value0 & VFE_0_IRQ_STATUS_0_RDIn_REG_UPDATE(i))
+			vfe_isr_reg_update(vfe, i);
 
-	if (value0 & VFE_0_IRQ_STATUS_0_IMAGE_MASTER_0_PING_PONG)
-		msm_vfe_isr_wm_done(vfe, 0);
+	for (i = 0; i < 7; i++)
+		if (value0 & VFE_0_IRQ_STATUS_0_IMAGE_MASTER_n_PING_PONG(i))
+			vfe_isr_wm_done(vfe, i);
 
 	return IRQ_HANDLED;
 }
 
-static int msm_vfe_reset(struct vfe_device *vfe)
+static int vfe_reset(struct vfe_device *vfe)
 {
 	unsigned long time;
 
 	init_completion(&vfe->reset_completion);
 
-	msm_vfe_global_reset(vfe);
+	vfe_global_reset(vfe);
 
 	time = wait_for_completion_timeout(&vfe->reset_completion,
-		msecs_to_jiffies(MSM_VFE_RESET_TIMEOUT_MS));
+		msecs_to_jiffies(VFE_RESET_TIMEOUT_MS));
 	if (!time) {
-		dev_err(vfe->camss->dev, "Vfe reset timeout\n");
+		dev_err(to_device(vfe), "Vfe reset timeout\n");
 		return -EIO;
 	}
 
 	return 0;
 }
 
-static int msm_vfe_halt(struct vfe_device *vfe)
+static int vfe_halt(struct vfe_device *vfe)
 {
 	unsigned long time;
 
@@ -457,48 +480,43 @@ static int msm_vfe_halt(struct vfe_device *vfe)
 	writel(VFE_0_BUS_BDG_CMD_HALT_REQ, vfe->base + VFE_0_BUS_BDG_CMD);
 
 	time = wait_for_completion_timeout(&vfe->halt_completion,
-		msecs_to_jiffies(MSM_VFE_HALT_TIMEOUT_MS));
+		msecs_to_jiffies(VFE_HALT_TIMEOUT_MS));
 	if (!time) {
-		dev_err(vfe->camss->dev, "Vfe halt timeout\n");
+		dev_err(to_device(vfe), "Vfe halt timeout\n");
 		return -EIO;
 	}
 
 	return 0;
 }
 
-static void msm_vfe_init_outputs(struct vfe_device *vfe)
+static void vfe_init_outputs(struct vfe_device *vfe)
 {
 	int i;
 
-	for (i = 0; i < ARRAY_SIZE(vfe->output); i++) {
-		vfe->output[i].active_wm = 0;
-		vfe->output[i].state = MSM_VFE_OUTPUT_OFF;
-		vfe->output[i].buf[0] = NULL;
-		vfe->output[i].buf[1] = NULL;
-		INIT_LIST_HEAD(&vfe->output[i].pending_bufs);
+	for (i = 0; i < ARRAY_SIZE(vfe->line); i++) {
+		struct msm_vfe_output *output = &vfe->line[i].output;
+
+		output->state = MSM_VFE_OUTPUT_OFF;
+		output->buf[0] = NULL;
+		output->buf[1] = NULL;
+		INIT_LIST_HEAD(&output->pending_bufs);
 	}
 }
 
-static void msm_vfe_reset_output_maps(struct vfe_device *vfe)
+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->rdi_output_map); i++)
-		vfe->rdi_output_map[i] = -1;
-
 	for (i = 0; i < ARRAY_SIZE(vfe->wm_output_map); i++)
-		vfe->wm_output_map[i] = -1;
-
-	for (i = 0; i < ARRAY_SIZE(vfe->composite_output_map); i++)
-		vfe->composite_output_map[i] = -1;
+		vfe->wm_output_map[i] = VFE_LINE_NONE;
 
 	spin_unlock_irqrestore(&vfe->output_lock, flags);
 }
 
-static void msm_vfe_set_qos(struct vfe_device *vfe)
+static void vfe_set_qos(struct vfe_device *vfe)
 {
 	u32 val = 0xaaa5aaa5;
 	u32 val7 = 0x0001aaa5;
@@ -513,20 +531,24 @@ static void msm_vfe_set_qos(struct vfe_device *vfe)
 	writel(val7, vfe->base + VFE_0_BUS_BDG_QOS_CFG_7);
 }
 
-static void msm_vfe_im_cgc_override(struct vfe_device *vfe)
+static void vfe_set_cgc_override(struct vfe_device *vfe, u32 wm_idx, u8 enable)
 {
-	u32 val = VFE_0_CGC_OVERRIDE_1_IMAGE_M0_CGC_OVERRIDE;
+	u32 val = VFE_0_CGC_OVERRIDE_1_IMAGE_Mx_CGC_OVERRIDE(wm_idx);
+
+	if (enable)
+		vfe_reg_set(vfe, VFE_0_CGC_OVERRIDE_1, val);
+	else
+		vfe_reg_clr(vfe, VFE_0_CGC_OVERRIDE_1, val);
 
-	writel(val, vfe->base + VFE_0_CGC_OVERRIDE_1);
+	wmb();
 }
 
-static void msm_vfe_output_init_addrs(struct vfe_device *vfe,
-				      struct msm_vfe_output *output,
-				      int sync)
+static void vfe_output_init_addrs(struct vfe_device *vfe,
+				  struct msm_vfe_output *output,
+				  int sync)
 {
 	u32 ping_addr = 0;
 	u32 pong_addr = 0;
-	int i;
 
 	output->active_buf = 0;
 
@@ -538,94 +560,56 @@ static void msm_vfe_output_init_addrs(struct vfe_device *vfe,
 	else
 		pong_addr = ping_addr;
 
-	for (i = 0; i < output->active_wm; i++) {
-		dev_err(vfe->camss->dev, "init_addrs: wm[%d], ping = 0x%08x, pong = 0x%08x\n",
-			i, ping_addr, pong_addr);
-		msm_vfe_wm_set_ping_addr(vfe, output->wm[i].wm_idx, ping_addr);
-		msm_vfe_wm_set_pong_addr(vfe, output->wm[i].wm_idx, pong_addr);
-		if (sync)
-			msm_vfe_bus_reload_wm(vfe, output->wm[i].wm_idx);
-	}
+	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 msm_vfe_output_reset_addrs(struct vfe_device *vfe,
+static void vfe_output_reset_addrs(struct vfe_device *vfe,
 				       struct msm_vfe_output *output)
 {
-	int i;
-
-	for (i = 0; i < output->active_wm; i++) {
-		msm_vfe_wm_set_ping_addr(vfe, output->wm[i].wm_idx, 0x00);
-		msm_vfe_wm_set_pong_addr(vfe, output->wm[i].wm_idx, 0x00);
-	}
+	vfe_wm_set_ping_addr(vfe, output->wm_idx, 0x00);
+	vfe_wm_set_pong_addr(vfe, output->wm_idx, 0x00);
 }
 
-static void msm_vfe_output_update_ping_addr(struct vfe_device *vfe,
-					    struct msm_vfe_output *output,
-					    int sync)
+static void vfe_output_update_ping_addr(struct vfe_device *vfe,
+					struct msm_vfe_output *output,
+					int sync)
 {
 	u32 addr = 0;
-	int i;
 
 	if (output->buf[0])
 		addr = output->buf[0]->addr;
 
-	for (i = 0; i < output->active_wm; i++) {
-		msm_vfe_wm_set_ping_addr(vfe, output->wm[i].wm_idx, addr);
-		if (sync)
-			msm_vfe_bus_reload_wm(vfe, output->wm[i].wm_idx);
-	}
+	vfe_wm_set_ping_addr(vfe, output->wm_idx, addr);
+	if (sync)
+		vfe_bus_reload_wm(vfe, output->wm_idx);
 }
 
-static void msm_vfe_output_update_pong_addr(struct vfe_device *vfe,
-					    struct msm_vfe_output *output,
-					    int sync)
+static void vfe_output_update_pong_addr(struct vfe_device *vfe,
+					struct msm_vfe_output *output,
+					int sync)
 {
 	u32 addr = 0;
-	int i;
 
 	if (output->buf[1])
 		addr = output->buf[1]->addr;
 
-	for (i = 0; i < output->active_wm; i++) {
-		msm_vfe_wm_set_pong_addr(vfe, output->wm[i].wm_idx, addr);
-		if (sync)
-			msm_vfe_bus_reload_wm(vfe, output->wm[i].wm_idx);
-	}
-}
+	vfe_wm_set_pong_addr(vfe, output->wm_idx, addr);
+	if (sync)
+		vfe_bus_reload_wm(vfe, output->wm_idx);
 
-static int __msm_vfe_reserve_rdi(struct vfe_device *vfe, u32 output_idx)
-{
-	int ret = -EBUSY;
-	int i;
-
-	for (i = 0; i < ARRAY_SIZE(vfe->rdi_output_map); i++) {
-		if (vfe->rdi_output_map[i] < 0) {
-			vfe->rdi_output_map[i] = output_idx;
-			ret = i;
-			break;
-		}
-	}
-	return ret;
-}
-
-static int __msm_vfe_release_rdi(struct vfe_device *vfe, u32 rdi_idx)
-{
-	if (rdi_idx > ARRAY_SIZE(vfe->rdi_output_map))
-		return -EINVAL;
-
-	vfe->rdi_output_map[rdi_idx] = -1;
-
-	return 0;
 }
 
-static int __msm_vfe_reserve_wm(struct vfe_device *vfe, u32 output_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] < 0) {
-			vfe->wm_output_map[i] = output_idx;
+		if (vfe->wm_output_map[i] == VFE_LINE_NONE) {
+			vfe->wm_output_map[i] = line_id;
 			ret = i;
 			break;
 		}
@@ -634,19 +618,19 @@ static int __msm_vfe_reserve_wm(struct vfe_device *vfe, u32 output_idx)
 	return ret;
 }
 
-static int __msm_vfe_release_wm(struct vfe_device *vfe, u32 wm_idx)
+static int __vfe_release_wm(struct vfe_device *vfe, u8 wm_idx)
 {
 	if (wm_idx > ARRAY_SIZE(vfe->wm_output_map))
 		return -EINVAL;
 
-	vfe->wm_output_map[wm_idx] = -1;
+	vfe->wm_output_map[wm_idx] = VFE_LINE_NONE;
 
 	return 0;
 }
 
 /* Vfe hw buffer operations */
 static struct msm_video_buffer *
-__msm_vfe_get_next_output_buf(struct msm_vfe_output *output)
+__vfe_get_next_output_buf(struct msm_vfe_output *output)
 {
 	struct msm_video_buffer *buffer = NULL;
 
@@ -661,56 +645,52 @@ __msm_vfe_get_next_output_buf(struct msm_vfe_output *output)
 }
 
 /*
- * msm_vfe_output_frame_drop - Set frame drop pattern per given output
+ * vfe_output_frame_drop - Set frame drop pattern per given output
  * @vfe: Pointer to vfe device.
  * @output: Pointer to vfe output.
  * @drop_pattern: Kept (1) or dropped (0). The pattern starts from bit 0
  *   and progresses to bit 31.
  */
-static void msm_vfe_output_frame_drop(struct vfe_device *vfe,
-				      struct msm_vfe_output *output,
-				      u32 drop_pattern)
+static void vfe_output_frame_drop(struct vfe_device *vfe,
+				  struct msm_vfe_output *output,
+				  u32 drop_pattern)
 {
 	u32 drop_period;
-	int i;
 
 	/* We need to toggle update period to be valid on next frame */
 	output->drop_update_idx++;
-	output->drop_update_idx %= MSM_VFE_FRAME_DROP_UPDATES;
-	drop_period = MSM_VFE_FRAME_DROP_VAL + output->drop_update_idx;
-
-	for (i = 0; i < output->active_wm; i++) {
-		msm_vfe_wm_set_framedrop_period(vfe, output->wm[i].wm_idx,
-						drop_period);
-		wmb();
-		msm_vfe_wm_set_framedrop_pattern(vfe, output->wm[i].wm_idx,
-						drop_pattern);
-		wmb();
-		msm_vfe_reg_update(vfe, output->wm[i].rdi_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);
+	wmb();
+	vfe_wm_set_framedrop_pattern(vfe, output->wm_idx, drop_pattern);
+	wmb();
+	vfe_reg_update(vfe, container_of(output, struct vfe_line, output)->id);
+
 }
 
 /*
- * __msm_vfe_add_output_buf - Add output buffer to vfe output
+ * __vfe_add_output_buf - Add output buffer to vfe output
  * @output: Pointer to vfe output.
  * @buffer: Pointer to video buffer.
  *
  * NOTE: Should be called with vfe locked.
  */
-void __msm_vfe_add_output_buf(struct msm_vfe_output *output,
-	struct msm_video_buffer *buffer)
+static void __vfe_add_output_buf(struct msm_vfe_output *output,
+				 struct msm_video_buffer *buffer)
 {
 	INIT_LIST_HEAD(&buffer->dma_queue);
 	list_add_tail(&buffer->dma_queue, &output->pending_bufs);
 }
 
 /*
- * __msm_vfe_flush_output_bufs - Flush all pending out buffers.
+ * __vfe_flush_output_bufs - Flush all pending out buffers.
  * @output: Pointer to vfe output.
  *
  * NOTE: Should be called with vfe locked.
  */
-void __msm_vfe_flush_output_bufs(struct msm_vfe_output *output)
+static void __vfe_flush_output_bufs(struct msm_vfe_output *output)
 {
 	struct msm_video_buffer *buf;
 	struct msm_video_buffer *t;
@@ -721,15 +701,15 @@ void __msm_vfe_flush_output_bufs(struct msm_vfe_output *output)
 	}
 }
 
-static void __msm_vfe_update_wm_on_next_buf(struct vfe_device *vfe,
-					    struct msm_vfe_output *output)
+static void __vfe_update_wm_on_next_buf(struct vfe_device *vfe,
+					struct msm_vfe_output *output)
 {
 	switch (output->state) {
 	case MSM_VFE_OUTPUT_CONTINUOUS:
-		msm_vfe_output_frame_drop(vfe, output, 3);
+		vfe_output_frame_drop(vfe, output, 3);
 		break;
 	case MSM_VFE_OUTPUT_SINGLE:
-		dev_err_ratelimited(vfe->camss->dev,
+		dev_err_ratelimited(to_device(vfe),
 				    "Next buf in single state!\n");
 		break;
 	default:
@@ -737,30 +717,30 @@ static void __msm_vfe_update_wm_on_next_buf(struct vfe_device *vfe,
 	}
 }
 
-static void __msm_vfe_update_wm_on_last_buf(struct vfe_device *vfe,
-					    struct msm_vfe_output *output)
+static void __vfe_update_wm_on_last_buf(struct vfe_device *vfe,
+					struct msm_vfe_output *output)
 {
 	switch (output->state) {
 	case MSM_VFE_OUTPUT_CONTINUOUS:
 		output->state = MSM_VFE_OUTPUT_SINGLE;
-		msm_vfe_output_frame_drop(vfe, output, 1);
+		vfe_output_frame_drop(vfe, output, 1);
 		break;
 	case MSM_VFE_OUTPUT_SINGLE:
 		output->state = MSM_VFE_OUTPUT_IDLE;
-		msm_vfe_output_frame_drop(vfe, output, 0);
-		msm_vfe_output_reset_addrs(vfe, output);
+		vfe_output_frame_drop(vfe, output, 0);
+		vfe_output_reset_addrs(vfe, output);
 		break;
 	default:
-		dev_err_ratelimited(vfe->camss->dev,
+		dev_err_ratelimited(to_device(vfe),
 				    "Last buff in wrong state! %d\n",
 				    output->state);
 		return;
 	}
 }
 
-static void __msm_vfe_update_wm_on_new_buf(struct vfe_device *vfe,
-					   struct msm_vfe_output *output,
-					   struct msm_video_buffer *new_buf)
+static void __vfe_update_wm_on_new_buf(struct vfe_device *vfe,
+				       struct msm_vfe_output *output,
+				       struct msm_video_buffer *new_buf)
 {
 	int inactive_idx;
 
@@ -773,17 +753,15 @@ static void __msm_vfe_update_wm_on_new_buf(struct vfe_device *vfe,
 			output->buf[inactive_idx] = new_buf;
 
 			if (inactive_idx)
-				msm_vfe_output_update_pong_addr(vfe,
-								output, 0);
+				vfe_output_update_pong_addr(vfe, output, 0);
 			else
-				msm_vfe_output_update_ping_addr(vfe,
-								output, 0);
+				vfe_output_update_ping_addr(vfe, output, 0);
 
-			msm_vfe_output_frame_drop(vfe, output, 3);
+			vfe_output_frame_drop(vfe, output, 3);
 			output->state = MSM_VFE_OUTPUT_CONTINUOUS;
 		} else {
-			__msm_vfe_add_output_buf(output, new_buf);
-			dev_err_ratelimited(vfe->camss->dev,
+			__vfe_add_output_buf(output, new_buf);
+			dev_err_ratelimited(to_device(vfe),
 					    "Inactive buffer is busy\n");
 		}
 		break;
@@ -792,15 +770,15 @@ static void __msm_vfe_update_wm_on_new_buf(struct vfe_device *vfe,
 		if (!output->buf[0]) {
 			output->buf[0] = new_buf;
 
-			msm_vfe_output_init_addrs(vfe, output, 1);
+			vfe_output_init_addrs(vfe, output, 1);
 
 			/* After wm reload we can not skip second frame.
 			 * Capture only second frame to avoid iommu fault */
-			msm_vfe_output_frame_drop(vfe, output, 2);
+			vfe_output_frame_drop(vfe, output, 2);
 			output->state = MSM_VFE_OUTPUT_SINGLE;
 		} else {
-			__msm_vfe_add_output_buf(output, new_buf);
-			dev_err_ratelimited(vfe->camss->dev,
+			__vfe_add_output_buf(output, new_buf);
+			dev_err_ratelimited(to_device(vfe),
 					    "Output idle with buffer set!\n");
 		}
 		break;
@@ -808,258 +786,238 @@ static void __msm_vfe_update_wm_on_new_buf(struct vfe_device *vfe,
 	case MSM_VFE_OUTPUT_CONTINUOUS:
 
 	default:
-		__msm_vfe_add_output_buf(output, new_buf);
+		__vfe_add_output_buf(output, new_buf);
 		return;
 	}
 }
 
-static struct msm_vfe_output* msm_vfe_get_output(struct vfe_device *vfe,
-						 u32 output_idx)
+static int vfe_get_output(struct vfe_line *line)
 {
+	struct vfe_device *vfe = to_vfe(line);
 	struct msm_vfe_output *output;
 	unsigned long flags;
 	int wm_idx;
-	int rdi_idx;
-
-	if (output_idx > ARRAY_SIZE(vfe->output))
-		return ERR_PTR(-EINVAL);
 
 	spin_lock_irqsave(&vfe->output_lock, flags);
 
-	output = &vfe->output[output_idx];
+	output = &line->output;
 	if (output->state != MSM_VFE_OUTPUT_OFF) {
-		dev_err(vfe->camss->dev, "Output is running\n");
+		dev_err(to_device(vfe), "Output is running\n");
 		goto error;
 	}
 	output->state = MSM_VFE_OUTPUT_RESERVED;
 
 	output->active_buf = 0;
 
-	rdi_idx = __msm_vfe_reserve_rdi(vfe, output_idx);
-	if (rdi_idx < 0) {
-		dev_err(vfe->camss->dev, "Can not reserve rdi\n");
-		goto error_get_rdi;
-	}
-
 	/* We will use only one wm per output for now */
-	wm_idx = __msm_vfe_reserve_wm(vfe, output_idx);
+	wm_idx = __vfe_reserve_wm(vfe, line->id);
 	if (wm_idx < 0) {
-		dev_err(vfe->camss->dev, "Can not reserve wm\n");
+		dev_err(to_device(vfe), "Can not reserve wm\n");
 		goto error_get_wm;
 	}
-	output->active_wm = 1;
 	output->drop_update_idx = 0;
-	output->wm[0].wm_idx = wm_idx;
-	output->wm[0].rdi_idx = rdi_idx;
+	output->wm_idx = wm_idx;
 
 	spin_unlock_irqrestore(&vfe->output_lock, flags);
 
-	return output;
+	dev_dbg(to_device(vfe), "%s: RDI%d -> WM%d\n",
+		__func__, line->id, wm_idx);
+
+	return 0;
 
 error_get_wm:
-	__msm_vfe_release_rdi(vfe, rdi_idx);
-error_get_rdi:
 	output->state = MSM_VFE_OUTPUT_OFF;
 error:
 	spin_unlock_irqrestore(&vfe->output_lock, flags);
 
-	return ERR_PTR(-EINVAL);
+	return -EINVAL;
 }
 
-static int msm_vfe_put_output(struct vfe_device *vfe,
-			      struct msm_vfe_output *output)
+static int vfe_put_output(struct vfe_line *line)
 {
-	struct msm_vfe_wm *wm;
+	struct vfe_device *vfe = to_vfe(line);
+	struct msm_vfe_output *output = &line->output;
 	unsigned long flags;
 	int ret;
-	int i;
 
 	spin_lock_irqsave(&vfe->output_lock, flags);
 
-	for (i = 0; i < output->active_wm; i++) {
-		wm = &output->wm[i];
-
-		ret = __msm_vfe_release_wm(vfe, wm->wm_idx);
-		if (ret < 0)
-			goto out;
-
-		ret = __msm_vfe_release_rdi(vfe, wm->rdi_idx);
-		if (ret < 0)
-			goto out;
-	}
+	ret = __vfe_release_wm(vfe, output->wm_idx);
+	if (ret < 0)
+		goto out;
 
 	output->state = MSM_VFE_OUTPUT_OFF;
-	output->active_wm = 0;
 
 out:
 	spin_unlock_irqrestore(&vfe->output_lock, flags);
 	return ret;
 }
 
-static int msm_vfe_enable_output(struct vfe_device *vfe,
-				 struct msm_vfe_output *output,
-				 u32 ub_size)
+static int vfe_enable_output(struct vfe_line *line)
 {
-	struct msm_vfe_wm *wm;
+	struct vfe_device *vfe = to_vfe(line);
+	struct msm_vfe_output *output = &line->output;
 	unsigned long flags;
-	int i;
+	u32 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 != MSM_VFE_OUTPUT_RESERVED) {
-		dev_err(vfe->camss->dev, "Output is not in reserved state %d\n",
+		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 = MSM_VFE_OUTPUT_IDLE;
 
-	output->buf[0] = __msm_vfe_get_next_output_buf(output);
+	output->buf[0] = __vfe_get_next_output_buf(output);
 	if (output->buf[0])
 		output->state = MSM_VFE_OUTPUT_SINGLE;
 
-	output->buf[1] = __msm_vfe_get_next_output_buf(output);
+	output->buf[1] = __vfe_get_next_output_buf(output);
 	if (output->buf[1])
 		output->state = MSM_VFE_OUTPUT_CONTINUOUS;
 
-	msm_vfe_set_qos(vfe);
-
 	switch (output->state) {
 	case MSM_VFE_OUTPUT_SINGLE:
 		/* After wm reload we can not skip second frame.
 		 * Capture only second frame to avoid iommu fault */
 		/* Skip 4 bad frames from sensor TODO: get number from sensor */
-		msm_vfe_output_frame_drop(vfe, output, 2 << 4);
+		vfe_output_frame_drop(vfe, output, 2 << 4);
 		break;
 	case MSM_VFE_OUTPUT_CONTINUOUS:
 		/* Skip 4 bad frames from sensor TODO: get number from sensor */
-		msm_vfe_output_frame_drop(vfe, output, 3 << 4);
+		vfe_output_frame_drop(vfe, output, 3 << 4);
 		break;
 	default:
-		msm_vfe_output_frame_drop(vfe, output, 0);
+		vfe_output_frame_drop(vfe, output, 0);
 		break;
 	}
 
-	msm_vfe_output_init_addrs(vfe, output, 0);
+	vfe_output_init_addrs(vfe, output, 0);
 
-	for (i = 0; i < output->active_wm; i++) {
-		wm = &output->wm[i];
+	vfe_set_cgc_override(vfe, output->wm_idx, 1);
 
-		msm_vfe_bus_connect_wm_to_rdi(vfe, wm->wm_idx, wm->rdi_idx);
+	vfe_enable_irq_wm(vfe, output->wm_idx, 1);
 
-		msm_vfe_set_rdi_cid(vfe, wm->rdi_idx, wm->rdi_idx);
+	vfe_bus_connect_wm_to_rdi(vfe, output->wm_idx, line->id);
 
-		msm_vfe_wm_set_ub_cfg(vfe, wm->wm_idx,
-			(ub_size * wm->wm_idx), ub_size);
+	vfe_set_rdi_cid(vfe, line->id, 0);
 
-		msm_vfe_wm_frame_based(vfe, wm->wm_idx, 1);
-		msm_vfe_wm_enable(vfe, wm->wm_idx, 1);
+	vfe_wm_set_ub_cfg(vfe, output->wm_idx,
+			  (ub_size + 1) * output->wm_idx, ub_size);
 
-		msm_vfe_bus_reload_wm(vfe, output->wm[i].wm_idx);
+	vfe_wm_frame_based(vfe, output->wm_idx, 1);
+	vfe_wm_enable(vfe, output->wm_idx, 1);
 
-		msm_vfe_reg_update(vfe, wm->rdi_idx);
-	}
+	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 msm_vfe_disable_output(struct vfe_device *vfe,
-				  struct msm_vfe_output *output)
+static int vfe_disable_output(struct vfe_line *line)
 {
-	struct msm_vfe_wm *wm;
-	int i;
+	struct vfe_device *vfe = to_vfe(line);
+	struct msm_vfe_output *output = &line->output;
+	unsigned long flags;
 
-	for (i = 0; i <  output->active_wm; i++) {
-		wm = &output->wm[i];
-		msm_vfe_wm_enable(vfe, wm->wm_idx, 0);
-		msm_vfe_bus_dicconnect_wm_from_rdi(vfe, wm->rdi_idx);
-		msm_vfe_reg_update(vfe, wm->rdi_idx);
-	}
+	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;
 }
 
-static int msm_vfe_enable_all_outputs(struct vfe_device *vfe)
+static int vfe_enable(struct vfe_line *line)
 {
-	struct msm_vfe_output *output;
-	u32 ub_size;
+	struct vfe_device *vfe = to_vfe(line);
 	int ret;
-	int i;
 
-	mutex_lock(&vfe->mutex);
+	mutex_lock(&vfe->stream_lock);
 
-	if (!vfe->stream_cnt)
-		return -EINVAL;
+	if (!vfe->stream_count) {
+		vfe_enable_irq_common(vfe);
 
-	switch (vfe->hw_id) {
-	case 0:
-		ub_size = MSM_VFE_UB_MAX_SIZE_VFE0;
-		break;
-	case 1:
-		ub_size = MSM_VFE_UB_MAX_SIZE_VFE1;
-		break;
-	default:
-		return -EINVAL;
-	}
-	ub_size /= vfe->stream_cnt;
+		vfe_bus_enable_wr_if(vfe, 1);
 
-	msm_vfe_im_cgc_override(vfe);
-	wmb();
+		vfe_set_qos(vfe);
+	}
 
-	/* Bus interface should be enabled first */
-	msm_vfe_bus_enable_wr_if(vfe, 1);
+	vfe->stream_count++;
 
-	for (i = 0; i < vfe->stream_cnt; i++) {
-		output = msm_vfe_get_output(vfe, i);
-		if (IS_ERR_OR_NULL(output))
-			goto error;
+	mutex_unlock(&vfe->stream_lock);
 
-		ret = msm_vfe_enable_output(vfe, output, ub_size);
-		if (ret < 0)
-			goto error;
-	}
-	vfe->active_outputs = i;
+	ret = vfe_get_output(line);
+	if (ret < 0)
+		goto error_get_output;
 
-	mutex_unlock(&vfe->mutex);
+	ret = vfe_enable_output(line);
+	if (ret < 0)
+		goto error_enable_output;
 
 	return 0;
 
-error:
-	msm_vfe_bus_enable_wr_if(vfe, 0);
 
-	for (; i > 0; i--)
-		msm_vfe_put_output(vfe, &vfe->output[i - 1]);
+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->mutex);
+	mutex_unlock(&vfe->stream_lock);
 
 	return ret;
 }
 
-static int msm_vfe_disable_all_outputs(struct vfe_device *vfe)
+static int vfe_disable(struct vfe_line *line)
 {
-	int i;
+	struct vfe_device *vfe = to_vfe(line);
 
-	mutex_lock(&vfe->mutex);
+	mutex_lock(&vfe->stream_lock);
 
-	msm_vfe_bus_enable_wr_if(vfe, 0);
+	if (vfe->stream_count == 1) {
+		vfe_bus_enable_wr_if(vfe, 0);
 
-	for (i = 0; i < vfe->active_outputs; i++) {
-		msm_vfe_disable_output(vfe, &vfe->output[i]);
-		msm_vfe_put_output(vfe, &vfe->output[i]);
 	}
 
-	msm_vfe_halt(vfe);
+	vfe->stream_count--;
+
+	mutex_unlock(&vfe->stream_lock);
 
-	vfe->active_outputs = 0;
+	vfe_disable_output(line);
 
-	mutex_unlock(&vfe->mutex);
+	vfe_put_output(line);
 
 	return 0;
 }
 
-static void msm_vfe_isr_wm_done(struct vfe_device *vfe, u32 wm_idx)
+static void vfe_isr_wm_done(struct vfe_device *vfe, u32 wm_idx)
 {
 	struct msm_video_buffer *ready_buf;
 	struct msm_vfe_output *output;
@@ -1067,19 +1025,19 @@ static void msm_vfe_isr_wm_done(struct vfe_device *vfe, u32 wm_idx)
 	unsigned long flags;
 	u32 active_index;
 
-	active_index = msm_vfe_wm_get_ping_pong_status(vfe, wm_idx);
+	active_index = vfe_wm_get_ping_pong_status(vfe, wm_idx);
 
 	spin_lock_irqsave(&vfe->output_lock, flags);
 
-	if (vfe->wm_output_map[wm_idx] < 0) {
-		dev_err_ratelimited(vfe->camss->dev,
+	if (vfe->wm_output_map[wm_idx] == VFE_LINE_NONE) {
+		dev_err_ratelimited(to_device(vfe),
 				    "Received wm done for unmapped index\n");
 		goto out_unlock;
 	}
-	output = &vfe->output[vfe->wm_output_map[wm_idx]];
+	output = &vfe->line[vfe->wm_output_map[wm_idx]].output;
 
 	if (output->active_buf == active_index) {
-		dev_err_ratelimited(vfe->camss->dev,
+		dev_err_ratelimited(to_device(vfe),
 				    "Active buffer mismatch!\n");
 		goto out_unlock;
 	}
@@ -1087,33 +1045,33 @@ static void msm_vfe_isr_wm_done(struct vfe_device *vfe, u32 wm_idx)
 
 	ready_buf = output->buf[!active_index];
 	if (!ready_buf) {
-		dev_err_ratelimited(vfe->camss->dev,
+		dev_err_ratelimited(to_device(vfe),
 				    "Missing ready buf %d %d!\n",
 				    !active_index, output->state);
 		goto out_unlock;
 	}
 
 	/* Get next buffer */
-	output->buf[!active_index] = __msm_vfe_get_next_output_buf(output);
+	output->buf[!active_index] = __vfe_get_next_output_buf(output);
 	if (!output->buf[!active_index]) {
 		new_addr = 0;
-		__msm_vfe_update_wm_on_last_buf(vfe, output);
+		__vfe_update_wm_on_last_buf(vfe, output);
 	} else {
 		new_addr = output->buf[!active_index]->addr;
-		__msm_vfe_update_wm_on_next_buf(vfe, output);
+		__vfe_update_wm_on_next_buf(vfe, output);
 	}
 
 	if (active_index)
-		msm_vfe_wm_set_ping_addr(vfe, wm_idx, new_addr);
+		vfe_wm_set_ping_addr(vfe, wm_idx, new_addr);
 	else
-		msm_vfe_wm_set_pong_addr(vfe, wm_idx, new_addr);
+		vfe_wm_set_pong_addr(vfe, wm_idx, new_addr);
 
 	spin_unlock_irqrestore(&vfe->output_lock, flags);
 
 	if (ready_buf)
 		vb2_buffer_done(&ready_buf->vb, VB2_BUF_STATE_DONE);
 	else
-		dev_err_ratelimited(vfe->camss->dev,
+		dev_err_ratelimited(to_device(vfe),
 				    "Received wm without buffer\n");
 
 	return;
@@ -1122,26 +1080,26 @@ out_unlock:
 	spin_unlock_irqrestore(&vfe->output_lock, flags);
 }
 
-static int msm_vfe_bus_request(struct vfe_device *vfe)
+static int vfe_bus_request(struct vfe_device *vfe)
 {
 	int ret;
 
 	vfe->bus_client = msm_bus_scale_register_client(vfe->bus_scale_table);
 	if (!vfe->bus_client) {
-		dev_err(vfe->camss->dev, "Failed to register bus client\n");
+		dev_err(to_device(vfe), "Failed to register bus client\n");
 		return -ENOENT;
 	}
 
 	ret = msm_bus_scale_client_update_request(vfe->bus_client, 1);
 	if (ret < 0) {
-		dev_err(vfe->camss->dev, "Failed bus scale update %d\n", ret);
+		dev_err(to_device(vfe), "Failed bus scale update %d\n", ret);
 		return -EINVAL;
 	}
 
 	return 0;
 }
 
-static void msm_vfe_bus_release(struct vfe_device *vfe)
+static void vfe_bus_release(struct vfe_device *vfe)
 {
 	if (vfe->bus_client) {
 		msm_bus_scale_unregister_client(vfe->bus_client);
@@ -1149,36 +1107,38 @@ static void msm_vfe_bus_release(struct vfe_device *vfe)
 	}
 }
 
-static int msm_vfe_set_clock_rate(struct vfe_device *vfe)
+/*
+ * vfe_enable_clocks - Enable clocks for VFE module and
+ * set clock rates where needed
+ * @nclocks: Number of clocks in clock array
+ * @clock: Clock array
+ * @clock_rate: Clock rates array
+ *
+ * Return 0 on success or a negative error code otherwise
+ */
+static int vfe_enable_clocks(int nclocks, struct clk **clock, s32 *clock_rate)
 {
-	int ret;
 	long clk_rate;
-
-	// TODO
-	clk_rate = clk_round_rate(vfe->clocks[1].clk, 465000000);
-	if (clk_rate < 0) {
-		dev_err(vfe->camss->dev, "clk round failed\n");
-		return -EINVAL;
-	}
-	ret = clk_set_rate(vfe->clocks[1].clk, clk_rate);
-	if (ret < 0) {
-		dev_err(vfe->camss->dev, "clk set rate failed\n");
-		return -EINVAL;
-	}
-
-	return 0;
-}
-
-static int msm_vfe_enable_clocks(struct vfe_device *vfe)
-{
 	int i;
 	int ret;
 
-	for (i = 0; i < vfe->nclocks; i++) {
-		ret = clk_prepare_enable(vfe->clocks[i].clk);
-		if (ret < 0) {
-			dev_err(vfe->camss->dev,
-				"clock prepare_enable failed %d\n", i);
+	for (i = 0; i < nclocks; i++) {
+		if (clock_rate[i]) {
+			clk_rate = clk_round_rate(clock[i], clock_rate[i]);
+			if (clk_rate < 0) {
+				pr_err("clock round rate failed\n");
+				ret = clk_rate;
+				goto error;
+			}
+			ret = clk_set_rate(clock[i], clk_rate);
+			if (ret < 0) {
+				pr_err("clock set rate failed\n");
+				goto error;
+			}
+		}
+		ret = clk_prepare_enable(clock[i]);
+		if (ret) {
+			pr_err("clock enable failed\n");
 			goto error;
 		}
 	}
@@ -1186,115 +1146,137 @@ static int msm_vfe_enable_clocks(struct vfe_device *vfe)
 	return 0;
 
 error:
-	for (; i > 0; i--) {
-		clk_disable_unprepare(vfe->clocks[i - 1].clk);
-	}
+	for (i--; i >= 0; i--)
+		clk_disable_unprepare(clock[i]);
+
 	return ret;
 }
 
-static void msm_vfe_disable_clocks(struct vfe_device *vfe)
+/*
+ * vfe_disable_clocks - Disable clocks for VFE module
+ * @nclocks: Number of clocks in clock array
+ * @clock: Clock array
+ */
+static void vfe_disable_clocks(int nclocks, struct clk **clock)
 {
 	int i;
 
-	for (i = vfe->nclocks - 1; i >= 0; i--)
-		clk_disable_unprepare(vfe->clocks[i].clk);
+	for (i = nclocks - 1; i >= 0; i--)
+		clk_disable_unprepare(clock[i]);
 }
 
-static int msm_vfe_get(struct vfe_device *vfe)
+static int vfe_get(struct vfe_device *vfe)
 {
 	int ret;
 
-	mutex_lock(&vfe->mutex);
+	mutex_lock(&vfe->power_lock);
 
-	if (vfe->ref_count == 0) {
-		msm_vfe_reset_output_maps(vfe);
+	if (vfe->power_count == 0) {
+		vfe_reset_output_maps(vfe); /* TODO: Move? */
 
-		ret = msm_vfe_bus_request(vfe);
+		ret = vfe_bus_request(vfe);
 		if (ret < 0) {
-			dev_err(vfe->camss->dev, "Fail bus request\n");
+			dev_err(to_device(vfe), "Fail bus request\n");
 			goto error_clocks;
 		}
 
-		ret = msm_vfe_set_clock_rate(vfe);
+		ret = vfe_enable_clocks(vfe->nclocks, vfe->clock,
+					vfe->clock_rate);
 		if (ret < 0) {
-			dev_err(vfe->camss->dev, "Fail to set clocks rate\n");
+			dev_err(to_device(vfe), "Fail to enable clocks\n");
 			goto error_clocks;
 		}
 
-		ret = msm_vfe_enable_clocks(vfe);
+		ret = vfe_reset(vfe);
 		if (ret < 0) {
-			dev_err(vfe->camss->dev, "Fail to enable clocks\n");
-			goto error_clocks;
+			dev_err(to_device(vfe), "Fail to reset vfe\n");
+			goto error_reset;
 		}
 	}
-	vfe->ref_count++;
+	vfe->power_count++;
 
-	mutex_unlock(&vfe->mutex);
+	mutex_unlock(&vfe->power_lock);
 
 	return 0;
 
+error_reset:
+	vfe_disable_clocks(vfe->nclocks, vfe->clock);
+
 error_clocks:
-	mutex_unlock(&vfe->mutex);
+	mutex_unlock(&vfe->power_lock);
 	return ret;
 }
 
-static void msm_vfe_put(struct vfe_device *vfe)
+static void vfe_put(struct vfe_device *vfe)
 {
-	mutex_lock(&vfe->mutex);
-	BUG_ON(vfe->ref_count == 0);
+	mutex_lock(&vfe->power_lock);
+	BUG_ON(vfe->power_count == 0);
 
-	if (--vfe->ref_count == 0) {
-		msm_vfe_disable_irq_all(vfe);
-		msm_vfe_init_outputs(vfe);
-		msm_vfe_bus_release(vfe);
-		msm_vfe_disable_clocks(vfe);
+	if (--vfe->power_count == 0) {
+//		vfe_init_outputs(vfe); /* TODO */
+		vfe_halt(vfe);
+		vfe_bus_release(vfe);
+		vfe_disable_clocks(vfe->nclocks, vfe->clock);
 	}
-	mutex_unlock(&vfe->mutex);
+	mutex_unlock(&vfe->power_lock);
 }
 
-static int msm_vfe_queue_dmabuf(struct camss_video *vid,
-				struct msm_video_buffer *buf)
+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);
+}
+
+static int vfe_queue_dmabuf(struct camss_video *vid,
+			    struct msm_video_buffer *buf)
 {
 	struct vfe_device *vfe = &vid->camss->vfe;
+	struct vfe_line *line;
 	struct msm_vfe_output *output;
 	unsigned long flags;
-	int idx;
 
-	idx = 0; // TODO: msm_vfe_pad_to_output(vfe, vid->pad_idx);
-	if (idx < 0) {
-		dev_err(vfe->camss->dev,
-			"Can not queue dma buf invalid pad idx\n");
-		return idx;
+	line = vfe_video_pad_to_line(&vid->pad);
+	if (!line) {
+		dev_err(to_device(vfe), "Can not queue dma buf\n");
+		return -1;
 	}
-	output = &vfe->output[idx];
+	output = &line->output;
 
 	spin_lock_irqsave(&vfe->output_lock, flags);
 
-	__msm_vfe_update_wm_on_new_buf(vfe, output, buf);
+	__vfe_update_wm_on_new_buf(vfe, output, buf);
 
 	spin_unlock_irqrestore(&vfe->output_lock, flags);
 
 	return 0;
 }
 
-static int msm_vfe_flush_dmabufs(struct camss_video *vid)
+static int vfe_flush_dmabufs(struct camss_video *vid)
 {
 	struct vfe_device *vfe = &vid->camss->vfe;
+	struct vfe_line *line;
 	struct msm_vfe_output *output;
 	unsigned long flags;
-	int idx;
 
-	idx = 0; // TODO: msm_vfe_pad_to_output(vfe, vid->pad_idx);
-	if (idx < 0) {
-		dev_err(vfe->camss->dev,
-			"Can not flush dma buf invalid pad idx\n");
-		return idx;
+	line = vfe_video_pad_to_line(&vid->pad);
+	if (!line) {
+		dev_err(to_device(vfe),	"Can not flush dma buf\n");
+		return -1;
 	}
-	output = &vfe->output[idx];
+	output = &line->output;
 
 	spin_lock_irqsave(&vfe->output_lock, flags);
 
-	__msm_vfe_flush_output_bufs(output);
+	__vfe_flush_output_bufs(output);
 
 	if (output->buf[0])
 		vb2_buffer_done(&output->buf[0]->vb, VB2_BUF_STATE_ERROR);
@@ -1307,148 +1289,363 @@ static int msm_vfe_flush_dmabufs(struct camss_video *vid)
 	return 0;
 }
 
-static int msm_vfe_subdev_set_power(struct v4l2_subdev *sd, int on)
+static int vfe_set_power(struct v4l2_subdev *sd, int on)
 {
-	struct vfe_device *vfe = v4l2_get_subdevdata(sd);
+	struct vfe_line *line = v4l2_get_subdevdata(sd);
+	struct vfe_device *vfe = to_vfe(line);
 	int ret;
 
-	dev_err(vfe->camss->dev, "%s: Enter, on = %d\n",
-		__func__, on);
+	dev_dbg(to_device(vfe), "%s: Enter, rdi%d on = %d\n",
+		__func__, line->id, on);
 
 	if (on) {
 		u32 hw_version;
 
-		ret = msm_vfe_get(vfe);
+		ret = vfe_get(vfe);
 		if (ret < 0)
 			return ret;
 
-
 		hw_version = readl(vfe->base);
-		dev_err(vfe->camss->dev,
+		dev_dbg(to_device(vfe),
 			"VFE HW Version = 0x%08x\n", hw_version);
 	} else {
-		msm_vfe_put(vfe);
+		vfe_put(vfe);
 	}
 
-	dev_err(vfe->camss->dev, "%s: Exit, on = %d\n",
-		__func__, on);
+	dev_dbg(to_device(vfe), "%s: Exit, rdi%d on = %d\n",
+		__func__, line->id, on);
 
 	return 0;
 }
 
-static int msm_vfe_subdev_set_stream(struct v4l2_subdev *sd, int enable)
+static int vfe_set_stream(struct v4l2_subdev *sd, int enable)
 {
-	struct vfe_device *vfe = v4l2_get_subdevdata(sd);
+	struct vfe_line *line = v4l2_get_subdevdata(sd);
+	struct vfe_device *vfe = to_vfe(line);
 	int ret = 0;
 
-	dev_err(vfe->camss->dev, "%s: Enter, enable = %d\n",
-		__func__, enable);
+	dev_dbg(to_device(vfe), "%s: Enter, rdi%d enable = %d\n",
+		__func__, line->id, enable);
 
 	if (enable) {
-		mutex_lock(&vfe->mutex);
-
-		ret = msm_vfe_reset(vfe);
-		if (ret < 0) {
-			dev_err(vfe->camss->dev, "Fail to reset vfe\n");
-			return ret;
-		}
-
-		msm_vfe_enable_irq_all(vfe);
-
-		mutex_unlock(&vfe->mutex);
-
-		ret = msm_vfe_enable_all_outputs(vfe);
+		ret = vfe_enable(line);
 		if (ret < 0)
-			dev_err(vfe->camss->dev,
+			dev_err(to_device(vfe),
 				"Fail to enable vfe outputs\n");
 	} else {
-		ret = msm_vfe_disable_all_outputs(vfe);
+		ret = vfe_disable(line);
 		if (ret < 0)
-			dev_err(vfe->camss->dev,
+			dev_err(to_device(vfe),
 				"Fail to disable vfe outputs\n");
 	}
 
 	return 0;
 }
 
-int msm_vfe_subdev_init(struct vfe_device *vfe, struct camss *camss,
-			struct vfe_init *init)
+/*
+ * __vfe_get_format - Get pointer to format structure
+ * @vfe: 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
+ * @vfe: 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 device *dev = camss->dev;
+	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
+ *
+ * Initialize all pad formats with default values.
+ */
+static int vfe_init_formats(struct v4l2_subdev *sd)
+{
+	struct v4l2_subdev_format format;
+
+	memset(&format, 0, sizeof(format));
+	format.pad = MSM_VFE_PAD_SINK;
+	format.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+	format.format.code = MEDIA_BUS_FMT_UYVY8_2X8;
+	format.format.width = 1920;
+	format.format.height = 1080;
+	vfe_set_format(sd, NULL, &format);
+
+	return 0;
+}
+
+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 dma_iommu_mapping *mapping;
+	struct camss *camss = to_camss(vfe);
 	int i;
 	int ret;
 
-	mutex_init(&vfe->mutex);
-	spin_lock_init(&vfe->output_lock);
+	mutex_init(&vfe->power_lock);
+	vfe->power_count = 0;
 
-	vfe->hw_id = 0; // TODO
+	mutex_init(&vfe->stream_lock);
+	vfe->stream_count = 0;
 
-	vfe->camss = camss;
-	vfe->init = *init;
+	spin_lock_init(&vfe->output_lock);
 
-	vfe->video_out.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-	vfe->video_out.camss = camss;
+	vfe->id = 0;
+	vfe->reg_update = 0;
 
-	// Temp:
-#define FMT_WIDTH 1920
-#define FMT_HEIGHT 1080
-	vfe->stream_cnt = 1;
-	vfe->video_out.active_fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-	vfe->video_out.active_fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_UYVY;
-	vfe->video_out.active_fmt.fmt.pix.width = FMT_WIDTH;
-	vfe->video_out.active_fmt.fmt.pix.height = FMT_HEIGHT;
-	vfe->video_out.active_fmt.fmt.pix.bytesperline = FMT_WIDTH * 2;
-	vfe->video_out.active_fmt.fmt.pix.sizeimage = FMT_WIDTH * FMT_HEIGHT * 2;
-	vfe->video_out.active_fmt.fmt.pix.field = V4L2_FIELD_NONE;
-	vfe->video_out.active_fmt.fmt.pix.colorspace = V4L2_COLORSPACE_JPEG;
+	for (i = 0; i < ARRAY_SIZE(vfe->line); 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, reg);
+	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);
 	}
 
-	r = platform_get_resource_byname(pdev, IORESOURCE_MEM, reg_vbif);
-	vfe->base_vbif = devm_ioremap_resource(dev, r);
-	if (IS_ERR(vfe->base_vbif)) {
-		dev_err(dev, "could not map memory\n");
-		return PTR_ERR(vfe->base_vbif);
+	/* Interrupt */
+
+	r = platform_get_resource_byname(pdev, IORESOURCE_IRQ, res->interrupt[0]);
+	vfe->irq = r->start;
+	if (IS_ERR_VALUE(vfe->irq))
+		return vfe->irq;
+
+	ret = devm_request_irq(dev, vfe->irq, vfe_subdev_isr,
+			       IRQF_TRIGGER_RISING, "vfe", vfe);
+	if (ret < 0) {
+		dev_err(dev, "request_irq failed\n");
+		return ret;
 	}
 
 	/* Clocks */
 
-	vfe->nclocks = ARRAY_SIZE(clocks);
-	vfe->clocks = devm_kzalloc(dev, vfe->nclocks * sizeof(*vfe->clocks),
-				    GFP_KERNEL);
-	if (!vfe->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) {
 		dev_err(dev, "could not allocate memory\n");
 		return -ENOMEM;
 	}
 
-	for (i = 0; i < vfe->nclocks; i++) {
-		vfe->clocks[i].name = clocks[i];
+	vfe->clock_rate = devm_kzalloc(dev, vfe->nclocks *
+				       sizeof(*vfe->clock_rate), GFP_KERNEL);
+	if (!vfe->clock_rate) {
+		dev_err(dev, "could not allocate memory\n");
+		return -ENOMEM;
 	}
 
 	for (i = 0; i < vfe->nclocks; i++) {
-		vfe->clocks[i].clk = devm_clk_get(dev, vfe->clocks[i].name);
-		if (IS_ERR(vfe->clocks[i].clk))
-			return PTR_ERR(vfe->clocks[i].clk);
+		vfe->clock[i] = devm_clk_get(dev, res->clock[i]);
+		if (IS_ERR(vfe->clock[i]))
+			return PTR_ERR(vfe->clock[i]);
+		vfe->clock_rate[i] = res->clock_rate[i];
 	}
 
 	/* IOMMU */
 
-	vfe->camss->iommu_dev = msm_iommu_get_ctx("vfe");
-	if (IS_ERR(vfe->camss->iommu_dev)) {
+	camss->iommu_dev = msm_iommu_get_ctx("vfe");
+	if (IS_ERR(camss->iommu_dev)) {
 		dev_err(dev, "Cannot find iommu nonsecure ctx\n");
-		return PTR_ERR(vfe->camss->iommu_dev);
+		return PTR_ERR(camss->iommu_dev);
 	}
 
 	mapping = arm_iommu_create_mapping(&platform_bus_type,
@@ -1456,24 +1653,10 @@ int msm_vfe_subdev_init(struct vfe_device *vfe, struct camss *camss,
 	if (IS_ERR_OR_NULL(mapping))
 		return PTR_ERR(mapping) ?: -ENODEV;
 
-	ret = arm_iommu_attach_device(vfe->camss->iommu_dev, mapping);
+	ret = arm_iommu_attach_device(camss->iommu_dev, mapping);
 	if (ret)
 		return -1;
 
-	/* Interrupt */
-
-	r = platform_get_resource_byname(pdev, IORESOURCE_IRQ, interrupt);
-	vfe->irq = r->start;
-	if (IS_ERR_VALUE(vfe->irq))
-		return vfe->irq;
-
-	ret = devm_request_irq(dev, vfe->irq, msm_vfe_subdev_isr,
-			       IRQF_TRIGGER_RISING, "vfe", vfe);
-	if (ret < 0) {
-		dev_err(dev, "request_irq failed\n");
-		return ret;
-	}
-
 	/* MSM Bus */
 
 	vfe->bus_scale_table = msm_bus_cl_get_pdata(pdev);
@@ -1482,94 +1665,164 @@ int msm_vfe_subdev_init(struct vfe_device *vfe, struct camss *camss,
 		return -1;
 	}
 
-	msm_vfe_init_outputs(vfe);
+	vfe_init_outputs(vfe);
 
 	return 0;
 }
 
-static const struct v4l2_subdev_core_ops msm_vfe_core_ops = {
-	.s_power = msm_vfe_subdev_set_power,
+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;
+}
+
+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;
+}
+
+static const struct v4l2_subdev_core_ops vfe_core_ops = {
+	.s_power = vfe_set_power,
 };
 
-static const struct v4l2_subdev_video_ops msm_vfe_video_ops = {
-	.s_stream = msm_vfe_subdev_set_stream,
+static const struct v4l2_subdev_video_ops vfe_video_ops = {
+	.s_stream = vfe_set_stream,
 };
 
-static const struct v4l2_subdev_pad_ops msm_vfe_pad_ops;
+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 msm_vfe_ops = {
-	.core = &msm_vfe_core_ops,
-	.video = &msm_vfe_video_ops,
-	.pad = &msm_vfe_pad_ops,
+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 msm_vfe_internal_ops;
+static const struct v4l2_subdev_internal_ops vfe_v4l2_internal_ops;
 
-static struct msm_video_ops rdi_video_ops = {
-	.queue_dmabuf = msm_vfe_queue_dmabuf,
-	.flush_dmabufs = msm_vfe_flush_dmabufs,
+static const struct media_entity_operations vfe_media_ops = {
+	.link_validate = v4l2_subdev_link_validate,
 };
 
+static struct msm_video_ops rdi_video_ops = {
+	.queue_dmabuf = vfe_queue_dmabuf,
+	.flush_dmabufs = vfe_flush_dmabufs,
+};
 
 int msm_vfe_register_entities(struct vfe_device *vfe,
 			      struct v4l2_device *v4l2_dev)
 {
-	struct v4l2_subdev *sd = &vfe->subdev;
-	struct media_pad *pads = vfe->pads;
+	struct v4l2_subdev *sd;
+	struct media_pad *pads;
+	struct camss_video *video_out;
 	int ret;
+	int i;
 
-	v4l2_subdev_init(sd, &msm_vfe_ops);
-	sd->internal_ops = &msm_vfe_internal_ops;
-	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
-	snprintf(sd->name, ARRAY_SIZE(sd->name), MSM_VFE_DRV_NAME);
-	v4l2_set_subdevdata(sd, vfe);
+	for (i = 0; i < ARRAY_SIZE(vfe->line); i++) {
+		char name[32];
 
-	pads[MSM_VFE_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
-	pads[MSM_VFE_PAD_SRC].flags = MEDIA_PAD_FL_SOURCE;
+		sd = &vfe->line[i].subdev;
+		pads = vfe->line[i].pads;
+		video_out = &vfe->line[i].video_out;
 
-	ret = media_entity_init(&sd->entity, MSM_VFE_PADS_NUM, pads, 0);
-	if (ret < 0) {
-		pr_err("Fail to init media entity");
-		goto error_init_entity;
-	}
 
-	ret = v4l2_device_register_subdev(v4l2_dev, sd);
-	if (ret < 0) {
-		pr_err("Fail to register subdev");
-		goto error_reg_subdev;
-	}
+		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]);
 
-	vfe->video_out.ops = &rdi_video_ops;
-	ret = msm_video_register(&vfe->video_out, v4l2_dev, MSM_VFE_VIDEO_NAME);
-	if (ret < 0)
-		goto error_reg_video;
+		vfe_init_formats(sd);
 
-	ret = media_entity_create_link(
-			&vfe->subdev.entity, MSM_VFE_PAD_SRC,
-			&vfe->video_out.video.entity, 0,
-			MEDIA_LNK_FL_IMMUTABLE | MEDIA_LNK_FL_ENABLED);
-	if (ret < 0) {
-		pr_err("Fail to link %s->%s entities\n",
-			vfe->subdev.entity.name,
-			vfe->video_out.video.entity.name);
-		goto error_link;
+		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_init(&sd->entity, MSM_VFE_PADS_NUM, pads, 0);
+		if (ret < 0) {
+			pr_err("Fail to init media entity");
+			goto error_init_entity;
+		}
+
+		ret = v4l2_device_register_subdev(v4l2_dev, sd);
+		if (ret < 0) {
+			pr_err("Fail to register subdev");
+			goto error_reg_subdev;
+		}
+
+		video_out->ops = &rdi_video_ops;
+		snprintf(name, ARRAY_SIZE(name), "%s%d", MSM_VFE_VIDEO_NAME, i);
+		ret = msm_video_register(video_out, v4l2_dev, name);
+		if (ret < 0) {
+			pr_err("Failed to register video node");
+			goto error_reg_video;
+		}
+
+		ret = media_entity_create_link(
+				&sd->entity, MSM_VFE_PAD_SRC,
+				&video_out->vdev->entity, 0,
+				MEDIA_LNK_FL_IMMUTABLE | MEDIA_LNK_FL_ENABLED);
+		if (ret < 0) {
+			pr_err("Fail to link %s->%s entities\n",
+			       sd->entity.name, video_out->vdev->entity.name);
+			goto error_link;
+		}
 	}
 
 	return 0;
 
 error_link:
-	msm_video_unregister(&vfe->video_out);
+	msm_video_unregister(video_out);
+
 error_reg_video:
 	v4l2_device_unregister_subdev(sd);
+
 error_reg_subdev:
 	media_entity_cleanup(&sd->entity);
+
 error_init_entity:
+	for (i--; i >= 0; i--) {
+		sd = &vfe->line[i].subdev;
+		video_out = &vfe->line[i].video_out;
+
+		media_entity_remove_links(&sd->entity);
+		msm_video_unregister(video_out);
+		v4l2_device_unregister_subdev(sd);
+		media_entity_cleanup(&sd->entity);
+	}
 
 	return ret;
 }
 
 void msm_vfe_unregister_entities(struct vfe_device *vfe)
 {
-	v4l2_device_unregister_subdev(&vfe->subdev);
-	msm_video_unregister(&vfe->video_out);
+	int i;
+
+	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;
+
+		media_entity_remove_links(&sd->entity);
+		msm_video_unregister(video_out);
+		v4l2_device_unregister_subdev(sd);
+		media_entity_cleanup(&sd->entity);
+	}
 }
diff --git a/drivers/media/platform/msm/camss-8x16/vfe.h b/drivers/media/platform/msm/camss-8x16/vfe.h
index b29460d21259..a0658ebb19be 100644
--- a/drivers/media/platform/msm/camss-8x16/vfe.h
+++ b/drivers/media/platform/msm/camss-8x16/vfe.h
@@ -39,10 +39,11 @@
 /* Hw definitions */
 #define MSM_VFE_NUM_RDI 3
 #define MSM_VFE_IMAGE_MASTERS_NUM 7
-#define MSM_VFE_IMAGE_COMPOSITE_NUM 4
 
-#define MSM_VFE_UB_MAX_SIZE_VFE0 827
-#define MSM_VFE_UB_MAX_SIZE_VFE1 (1535)
+#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 msm_vfe_output_state {
 	MSM_VFE_OUTPUT_OFF,
@@ -52,15 +53,18 @@ enum msm_vfe_output_state {
 	MSM_VFE_OUTPUT_IDLE,
 };
 
-struct msm_vfe_wm {
-	u8 rdi_idx;
-	u8 wm_idx;
-	u32 bytesperline;
+enum vfe_line_id {
+	VFE_LINE_NONE = -1,
+	VFE_LINE_MIN = 0,
+	VFE_LINE_RDI0 = 0,
+	VFE_LINE_RDI1 = 1,
+	VFE_LINE_RDI2 = 2,
+	VFE_LINE_MAX = VFE_LINE_RDI2,
+	VFE_LINE_PIX /* TODO: implement */
 };
 
 struct msm_vfe_output {
-	u16 active_wm;
-	struct msm_vfe_wm wm[MSM_VFE_MAX_WM_PER_OUTPUT];
+	u8 wm_idx;
 
 	int active_buf;
 	struct msm_video_buffer *buf[2];
@@ -71,49 +75,46 @@ struct msm_vfe_output {
 	enum msm_vfe_output_state state;
 };
 
-struct vfe_init {
-	int num_cids;
-	unsigned int cid[MSM_VFE_MAX_CID_NUM];
-};
-
-struct clock_info {
-	const char *name;
-	struct clk *clk;
-};
-
-struct vfe_device {
-	int hw_id;
-	struct vfe_init init;
+struct vfe_line {
+	enum vfe_line_id id;
 	struct v4l2_subdev subdev;
 	struct media_pad pads[MSM_VFE_PADS_NUM];
-	struct camss *camss;
+	struct v4l2_mbus_framefmt fmt[MSM_VFE_PADS_NUM];
 	struct camss_video video_out;
+	struct msm_vfe_output output;
+};
+
+struct vfe_device {
+	u8 id;
 	void __iomem *base;
-	void __iomem *base_vbif;
 	u32 irq;
-	struct clock_info *clocks;
+	struct clk **clock;
+	s32 *clock_rate;
 	int nclocks;
 	struct completion reset_completion;
 	struct completion halt_completion;
-	struct mutex mutex;
-	int ref_count;
+	struct mutex power_lock;
+	int power_count;
+	struct mutex stream_lock;
+	int stream_count;
 	spinlock_t output_lock;
-	int rdi_output_map[MSM_VFE_NUM_RDI];
-	int wm_output_map[MSM_VFE_IMAGE_MASTERS_NUM];
-	int composite_output_map[MSM_VFE_IMAGE_COMPOSITE_NUM];
-	int stream_cnt;
-	int active_outputs;
-	struct msm_vfe_output output[MSM_VFE_MAX_OUTPUTS];
+	enum vfe_line_id wm_output_map[MSM_VFE_IMAGE_MASTERS_NUM];
 	struct msm_bus_scale_pdata *bus_scale_table;
 	uint32_t bus_client;
+	struct vfe_line line[VFE_LINE_MAX + 1];
+	u32 reg_update;
 };
 
-int msm_vfe_subdev_init(struct vfe_device *vfe, struct camss *camss,
-			struct vfe_init *init);
+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 */
diff --git a/drivers/media/platform/msm/camss-8x16/video.c b/drivers/media/platform/msm/camss-8x16/video.c
index 0c93483ff490..68fbee0a4c34 100644
--- a/drivers/media/platform/msm/camss-8x16/video.c
+++ b/drivers/media/platform/msm/camss-8x16/video.c
@@ -25,6 +25,25 @@
 #include "video.h"
 #include "camss.h"
 
+static struct format_info 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 }
+};
+
 static int video_queue_setup(struct vb2_queue *q, const void *parg,
 	unsigned int *num_buffers, unsigned int *num_planes,
 	unsigned int sizes[], void *alloc_ctxs[])
@@ -103,8 +122,8 @@ static int video_querycap(struct file *file, void *fh,
 {
 	struct camss_video *video = video_drvdata(file);
 
-	strlcpy(cap->driver, video->video.name, sizeof(cap->driver));
-	strlcpy(cap->card, video->video.name, sizeof(cap->card));
+	strlcpy(cap->driver, video->vdev->name, sizeof(cap->driver));
+	strlcpy(cap->card, video->vdev->name, sizeof(cap->card));
 	strlcpy(cap->bus_info, "media", sizeof(cap->bus_info));
 	cap->version = CAMSS_VERSION;
 	cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING |
@@ -114,55 +133,99 @@ static int video_querycap(struct file *file, void *fh,
 	return 0;
 }
 
-static int video_enum_fmt(struct file *file, void *fh, struct v4l2_fmtdesc *f)
+/*
+ * 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.
+ */
+static unsigned int video_mbus_to_pix(const struct v4l2_mbus_framefmt *mbus,
+				      struct v4l2_pix_format *pix)
 {
-	struct camss_video *video = video_drvdata(file);
+	unsigned int i;
 
-	if (f->type != video->type)
-		return -EINVAL;
+	memset(pix, 0, sizeof(*pix));
+	pix->width = mbus->width;
+	pix->height = mbus->height;
 
-	if (f->index)
+	for (i = 0; i < ARRAY_SIZE(formats); ++i) {
+		if (formats[i].code == mbus->code)
+			break;
+	}
+
+	if (WARN_ON(i == ARRAY_SIZE(formats)))
 		return -EINVAL;
 
-	f->pixelformat = video->active_fmt.fmt.pix.pixelformat;
+	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 int video_enum_framesizes(struct file *file, void *fh,
-				 struct v4l2_frmsizeenum *f)
+static struct v4l2_subdev *video_remote_subdev(struct camss_video *video,
+					       u32 *pad)
 {
-	struct camss_video *video = video_drvdata(file);
+	struct media_pad *remote;
 
-	if (f->pixel_format != video->active_fmt.fmt.pix.pixelformat)
-		return -EINVAL;
+	remote = media_entity_remote_pad(&video->pad);
 
-	if (f->index)
+	if (remote == NULL ||
+	    media_entity_type(remote->entity) != MEDIA_ENT_T_V4L2_SUBDEV)
+		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;
 
-	f->type = V4L2_FRMSIZE_TYPE_DISCRETE;
-	f->discrete.width = video->active_fmt.fmt.pix.width;
-	f->discrete.height = video->active_fmt.fmt.pix.height;
+	fmt.pad = pad;
+	fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
 
-	return 0;
+	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);
 }
 
-static int video_enum_frameintervals(struct file *file, void *fh,
-				     struct v4l2_frmivalenum *f)
+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->pixel_format != video->active_fmt.fmt.pix.pixelformat ||
-	    f->width != video->active_fmt.fmt.pix.width ||
-	    f->height != video->active_fmt.fmt.pix.height)
+	if (f->type != video->type)
 		return -EINVAL;
 
 	if (f->index)
 		return -EINVAL;
 
-	f->type = V4L2_FRMIVAL_TYPE_DISCRETE;
-	f->discrete.numerator = 1;
-	f->discrete.denominator = 30;
+	ret = video_get_subdev_format(video, &format);
+	if (ret < 0)
+		return ret;
+
+	f->pixelformat = format.fmt.pix.pixelformat;
 
 	return 0;
 }
@@ -182,11 +245,16 @@ static int video_g_fmt(struct file *file, void *fh, struct v4l2_format *f)
 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;
 
-	*f = video->active_fmt;
+	ret = video_get_subdev_format(video, f);
+	if (ret < 0)
+		return ret;
+
+	video->active_fmt = *f;
 
 	return 0;
 }
@@ -194,13 +262,14 @@ static int video_s_fmt(struct file *file, void *fh, struct v4l2_format *f)
 static int video_try_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;
 
-	*f = video->active_fmt;
+	ret = video_get_subdev_format(video, f);
 
-	return 0;
+	return ret;
 }
 
 static int video_reqbufs(struct file *file, void *fh,
@@ -245,9 +314,30 @@ static int video_dqbuf(struct file *file, void *fh, struct v4l2_buffer *b)
 	return ret;
 }
 
+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_streamon(struct file *file, void *fh, enum v4l2_buf_type type)
 {
-	struct video_device *video_dev = video_devdata(file);
+	struct video_device *vdev = video_devdata(file);
 	struct camss_video *video = video_drvdata(file);
 	struct media_entity *entity;
 	struct media_pad *pad;
@@ -257,15 +347,19 @@ static int video_streamon(struct file *file, void *fh, enum v4l2_buf_type type)
 	if (type != video->type)
 		return -EINVAL;
 
-	ret = media_entity_pipeline_start(&video->video.entity, &video->pipe);
+	ret = media_entity_pipeline_start(&vdev->entity, &video->pipe);
 	if (ret < 0)
 		return ret;
 
+	ret = video_check_format(video);
+	if (ret < 0)
+		goto pipeline_stop;
+
 	ret = vb2_streamon(&video->vb2_q, type);
 	if (ret < 0)
 		goto pipeline_stop;
 
-	entity = &video_dev->entity;
+	entity = &vdev->entity;
 	while (1) {
 		pad = &entity->pads[0];
 		if (!(pad->flags & MEDIA_PAD_FL_SINK))
@@ -287,7 +381,7 @@ static int video_streamon(struct file *file, void *fh, enum v4l2_buf_type type)
 	return 0;
 
 pipeline_stop:
-	media_entity_pipeline_stop(&video->video.entity);
+	media_entity_pipeline_stop(&vdev->entity);
 streamoff:
 	vb2_streamoff(&video->vb2_q, type);
 
@@ -296,17 +390,21 @@ streamoff:
 
 static int video_streamoff(struct file *file, void *fh, enum v4l2_buf_type type)
 {
-	struct video_device *video_dev = video_devdata(file);
+	struct video_device *vdev = video_devdata(file);
 	struct camss_video *video = video_drvdata(file);
 	struct media_entity *entity;
 	struct media_pad *pad;
 	struct v4l2_subdev *subdev;
+	struct v4l2_subdev *subdev_vfe = NULL;
 	int ret;
 
 	if (type != video->type)
 		return -EINVAL;
 
-	entity = &video_dev->entity;
+	if (!vb2_is_streaming(&video->vb2_q))
+		return 0;
+
+	entity = &vdev->entity;
 	while (1) {
 		pad = &entity->pads[0];
 		if (!(pad->flags & MEDIA_PAD_FL_SINK))
@@ -320,14 +418,21 @@ static int video_streamoff(struct file *file, void *fh, enum v4l2_buf_type type)
 		entity = pad->entity;
 		subdev = media_entity_to_v4l2_subdev(entity);
 
-		v4l2_subdev_call(subdev, video, s_stream, 0);
+		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);
+		}
 	}
 
 	ret = vb2_streamoff(&video->vb2_q, type);
 	if (ret)
 		return ret;
 
-	media_entity_pipeline_stop(&video->video.entity);
+	media_entity_pipeline_stop(&vdev->entity);
 
 	return 0;
 }
@@ -335,8 +440,6 @@ static int video_streamoff(struct file *file, void *fh, enum v4l2_buf_type type)
 static const struct v4l2_ioctl_ops msm_vid_ioctl_ops = {
 	.vidioc_querycap          = video_querycap,
 	.vidioc_enum_fmt_vid_cap  = video_enum_fmt,
-	.vidioc_enum_framesizes   = video_enum_framesizes,
-	.vidioc_enum_frameintervals = video_enum_frameintervals,
 	.vidioc_g_fmt_vid_cap     = video_g_fmt,
 	.vidioc_s_fmt_vid_cap     = video_s_fmt,
 	.vidioc_try_fmt_vid_cap   = video_try_fmt,
@@ -348,26 +451,45 @@ static const struct v4l2_ioctl_ops msm_vid_ioctl_ops = {
 	.vidioc_streamoff         = video_streamoff,
 };
 
+
+/*
+ * video_init_format - Initialize format
+ * @sd: VFE V4L2 subdevice
+ *
+ * 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;
+
+	video_s_fmt(file, fh, &format);
+
+	return 0;
+}
+
 static int video_open(struct file *file)
 {
-	struct video_device *video_dev = video_devdata(file);
+	struct video_device *vdev = video_devdata(file);
 	struct camss_video *video = video_drvdata(file);
 	struct vb2_queue *q;
 	int ret;
 
 	video->alloc_ctx = vb2_dma_contig_init_ctx(video->camss->iommu_dev);
 	if (IS_ERR(video->alloc_ctx)) {
-		dev_err(&video->video.dev, "Failed to init vb2 dma ctx\n");
+		dev_err(&vdev->dev, "Failed to init vb2 dma ctx\n");
 		return PTR_ERR(video->alloc_ctx);
 	}
 
-	v4l2_fh_init(&video->fh, video_dev);
+	v4l2_fh_init(&video->fh, vdev);
 	v4l2_fh_add(&video->fh);
 	file->private_data = &video->fh;
 
-	ret = msm_camss_pipeline_pm_use(&video_dev->entity, 1);
+	ret = msm_camss_pipeline_pm_use(&vdev->entity, 1);
 	if (ret < 0) {
-		dev_err(&video_dev->dev, "pipeline power-up failed\n");
+		dev_err(&vdev->dev, "pipeline power-up failed\n");
 		goto error;
 	}
 
@@ -375,16 +497,18 @@ static int video_open(struct file *file)
 	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; // TODO: MPLANE
+	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 msm_video_buffer);
 	ret = vb2_queue_init(q);
 	if (ret < 0) {
-		dev_err(&video_dev->dev, "vb2 queue init failed\n");
+		dev_err(&vdev->dev, "vb2 queue init failed\n");
 		goto error;
 	}
 
+	video_init_format(file, &video->fh);
+
 	return 0;
 
 error:
@@ -397,12 +521,14 @@ error:
 
 static int video_release(struct file *file)
 {
-	struct video_device *video_dev = video_devdata(file);
+	struct video_device *vdev = video_devdata(file);
 	struct camss_video *video = video_drvdata(file);
 
+	video_streamoff(file, &video->fh, video->type);
+
 	vb2_queue_release(&video->vb2_q);
 
-	msm_camss_pipeline_pm_use(&video_dev->entity, 0);
+	msm_camss_pipeline_pm_use(&vdev->entity, 0);
 
 	file->private_data = NULL;
 	v4l2_fh_del(&video->fh);
@@ -440,35 +566,43 @@ 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 *video_dev = &video->video;
+	struct video_device *vdev;
 	int ret;
 
+	vdev = video_device_alloc();
+	if (vdev == NULL) {
+		v4l2_err(v4l2_dev, "Failed to allocate video device\n");
+		return -ENOMEM;
+	}
+
+	video->vdev = vdev;
+
 	pad->flags = MEDIA_PAD_FL_SINK;
-	ret = media_entity_init(&video_dev->entity, 1, pad, 0);
+	ret = media_entity_init(&vdev->entity, 1, pad, 0);
 	if (ret < 0) {
 		v4l2_err(v4l2_dev, "Failed to init video entity\n");
 		return ret;
 	}
 
-	video_dev->fops = &msm_vid_fops;
-	video_dev->ioctl_ops = &msm_vid_ioctl_ops;
-	video_dev->release = video_device_release; // TODO: implement
-	video_dev->v4l2_dev = v4l2_dev;
-	video_dev->vfl_dir = VFL_DIR_RX;
-	strlcpy(video_dev->name, name, sizeof(video_dev->name));
+	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;
+	strlcpy(vdev->name, name, sizeof(vdev->name));
 
-	ret = video_register_device(video_dev, VFL_TYPE_GRABBER, -1);
+	ret = video_register_device(vdev, VFL_TYPE_GRABBER, -1);
 	if (ret < 0) {
 		v4l2_err(v4l2_dev, "Failed to register video device\n");
 		return ret;
 	}
 
-	video_set_drvdata(video_dev, video);
+	video_set_drvdata(vdev, video);
 
 	return 0;
 }
 
 void msm_video_unregister(struct camss_video *video)
 {
-	video_unregister_device(&video->video);
+	video_unregister_device(video->vdev);
 }
diff --git a/drivers/media/platform/msm/camss-8x16/video.h b/drivers/media/platform/msm/camss-8x16/video.h
index 4cd7f01c4fcc..c0b2ef4d32a7 100644
--- a/drivers/media/platform/msm/camss-8x16/video.h
+++ b/drivers/media/platform/msm/camss-8x16/video.h
@@ -23,8 +23,21 @@
 #include <media/v4l2-dev.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-fh.h>
+#include <media/v4l2-mediabus.h>
 #include <media/videobuf2-core.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
+ */
+struct format_info {
+	u32 code;
+	u32 pixelformat;
+	unsigned int bpp;
+};
+
 struct msm_video_buffer {
 	struct vb2_buffer vb;
 	unsigned long size;
@@ -37,7 +50,7 @@ struct camss_video {
 	struct camss *camss;
 	void *alloc_ctx;
 	struct vb2_queue vb2_q;
-	struct video_device video;
+	struct video_device *vdev;
 	struct media_pad pad;
 	struct v4l2_format active_fmt;
 	enum v4l2_buf_type type;
diff --git a/drivers/media/platform/msm/cci/msm_cci.c b/drivers/media/platform/msm/cci/msm_cci.c
index fbcd2b54f9ef..0a5e7bf12896 100644
--- a/drivers/media/platform/msm/cci/msm_cci.c
+++ b/drivers/media/platform/msm/cci/msm_cci.c
@@ -1161,7 +1161,7 @@ int32_t msm_cci_ctrl_release(void)
 	return rc;
 }
 
-int32_t msm_cci_ctrl_read(u16 addr, const char *buf, int count)
+int32_t msm_cci_ctrl_read(u16 i2c_addr, u16 addr, const char *buf, int count)
 {
 	struct v4l2_subdev *sd = msm_cci_get_subdev();
 	struct msm_camera_cci_ctrl cci_ctrl = { 0 };
@@ -1173,7 +1173,7 @@ int32_t msm_cci_ctrl_read(u16 addr, const char *buf, int count)
 
 	cci_ctrl.cci_info = &cci_info;
 	cci_ctrl.cci_info->cci_i2c_master = MASTER_0;
-	cci_ctrl.cci_info->sid = 0x78 >> 1;
+	cci_ctrl.cci_info->sid = i2c_addr >> 1;
 	cci_ctrl.cci_info->retries = 3;
 	cci_ctrl.cci_info->id_map = 0;
 	cci_ctrl.cci_info->i2c_freq_mode = I2C_STANDARD_MODE;
@@ -1193,7 +1193,7 @@ int32_t msm_cci_ctrl_read(u16 addr, const char *buf, int count)
 	return rc;
 }
 
-int32_t msm_cci_ctrl_write(u16 addr, const char *buf, int count)
+int32_t msm_cci_ctrl_write(u16 i2c_addr, u16 addr, const char *buf, int count)
 {
 	struct v4l2_subdev *sd = msm_cci_get_subdev();
 	struct msm_camera_cci_ctrl cci_ctrl = { 0 };
@@ -1206,7 +1206,7 @@ int32_t msm_cci_ctrl_write(u16 addr, const char *buf, int count)
 
 	cci_ctrl.cci_info = &cci_info;
 	cci_ctrl.cci_info->cci_i2c_master = MASTER_0;
-	cci_ctrl.cci_info->sid = 0x78 >> 1;
+	cci_ctrl.cci_info->sid = i2c_addr >> 1;
 	cci_ctrl.cci_info->retries = 3;
 	cci_ctrl.cci_info->id_map = 0;
 
diff --git a/drivers/media/platform/msm/cci/msm_cci.h b/drivers/media/platform/msm/cci/msm_cci.h
index 26b8c9459956..abc288fe3c58 100644
--- a/drivers/media/platform/msm/cci/msm_cci.h
+++ b/drivers/media/platform/msm/cci/msm_cci.h
@@ -197,8 +197,8 @@ struct v4l2_subdev *msm_cci_get_subdev(void);
 
 int32_t msm_cci_ctrl_init(void);
 int32_t msm_cci_ctrl_release(void);
-int32_t msm_cci_ctrl_read(u16 addr, const char *buf, int count);
-int32_t msm_cci_ctrl_write(u16 addr, const char *buf, int count);
+int32_t msm_cci_ctrl_read(u16 i2c_addr, u16 addr, const char *buf, int count);
+int32_t msm_cci_ctrl_write(u16 i2c_addr, u16 addr, const char *buf, int count);
 
 #else
 static inline struct v4l2_subdev *msm_cci_get_subdev(void) {
diff --git a/include/uapi/linux/videodev2.h b/include/uapi/linux/videodev2.h
index 421d27413731..eacf51408d1f 100644
--- a/include/uapi/linux/videodev2.h
+++ b/include/uapi/linux/videodev2.h
@@ -575,6 +575,11 @@ struct v4l2_pix_format {
 #define V4L2_PIX_FMT_SGBRG12 v4l2_fourcc('G', 'B', '1', '2') /* 12  GBGB.. RGRG.. */
 #define V4L2_PIX_FMT_SGRBG12 v4l2_fourcc('B', 'A', '1', '2') /* 12  GRGR.. BGBG.. */
 #define V4L2_PIX_FMT_SRGGB12 v4l2_fourcc('R', 'G', '1', '2') /* 12  RGRG.. GBGB.. */
+	/* 12bit raw bayer packed, 6 bytes for every 4 pixels */
+#define V4L2_PIX_FMT_SBGGR12P v4l2_fourcc('p', 'B', 'C', 'C')
+#define V4L2_PIX_FMT_SGBRG12P v4l2_fourcc('p', 'G', 'C', 'C')
+#define V4L2_PIX_FMT_SGRBG12P v4l2_fourcc('p', 'g', 'C', 'C')
+#define V4L2_PIX_FMT_SRGGB12P v4l2_fourcc('p', 'R', 'C', 'C')
 #define V4L2_PIX_FMT_SBGGR16 v4l2_fourcc('B', 'Y', 'R', '2') /* 16  BGBG.. GRGR.. */
 
 /* compressed formats */