media/starfive: add starfive v4l2 driver framework

1. Add starfive video v4l2 driver framework 2. Support DVP sensor and MIPI csi sensor, eg: imx219, ov4689, dvp ov5640, sc2235 Signed-off-by: sw.multimedia <sw.multimedia@starfivetech.com> Signed-off-by: david.li <david.li@starfivetech.com> Signed-off-by: jack.zhu <jack.zhu@starfivetech.com> Signed-off-by: keithzhao <keith.zhao@starfivetech.com> Signed-off-by: andy.hu <andy.hu@starfivetech.com> Signed-off-by: Curry Zhang <curry.zhang@starfivetech.com>
author: Hal Feng <hal.feng@starfivetech.com> 2021-12-22 11:21:21 +0300
committer: MichaelZhuxx <michael.zhu@starfivetech.com> 2022-01-04 14:47:09 +0300
commit: 016794a1040c8e1ab002f3aa41659589385eb63b (patch)
tree: 8573c4cfb423ac4e799438a11aa07a8403a36c6b
parent: 32f645aaf3b1fb8c47ae9dfb50a6ab642b6fa5b4 (diff)
download: linux-016794a1040c8e1ab002f3aa41659589385eb63b.tar.xz
61 files changed, 28765 insertions, 1 deletions
diff --git a/arch/riscv/boot/dts/starfive/jh7100-common.dtsi b/arch/riscv/boot/dts/starfive/jh7100-common.dtsi
index ff3e6b746132..a3cad6db9bf6 100644
--- a/arch/riscv/boot/dts/starfive/jh7100-common.dtsi
+++ b/arch/riscv/boot/dts/starfive/jh7100-common.dtsi
@@ -9,6 +9,7 @@
 #include <dt-bindings/gpio/gpio.h>
 #include <dt-bindings/leds/common.h>
 #include <dt-bindings/pinctrl/pinctrl-starfive.h>
+#include <dt-bindings/starfive_fb.h>
 
 / {
 	aliases {
@@ -441,6 +442,29 @@
 	pinctrl-0 = <&i2c0_pins>;
 	status = "okay";
 
+	imx219@10 {
+		compatible = "imx219";
+		reg = <0x10>;
+		clocks = <&clk_ext_camera>;
+		clock-names = "xclk";
+		// powerdown-gpio = <&gpio 21 GPIO_ACTIVE_HIGH>;
+		reset-gpio = <&gpio 58 GPIO_ACTIVE_HIGH>;
+		//DOVDD-supply = <&v2v8>;
+		rotation = <0>;
+		orientation = <1>; //CAMERA_ORIENTATION_BACK
+
+		port {
+			/* CSI2 bus endpoint */
+			imx219_to_csi2rx0: endpoint {
+				remote-endpoint = <&csi2rx0_from_imx219>;
+				bus-type = <4>;      /* MIPI CSI-2 D-PHY */
+				clock-lanes = <0>;
+				data-lanes = <1 2>;
+				link-frequencies = /bits/ 64 <456000000>;
+			};
+		};
+	};
+
 	pmic@5e {
 		compatible = "ti,tps65086";
 		reg = <0x5e>;
@@ -478,6 +502,56 @@
 		#sound-dai-cells = <0>;
 		wlf,shared-lrclk;
 	};
+
+	seeed_plane_i2c@45 {
+		compatible = "seeed_panel";
+		reg = <0x45>;
+	};
+
+	/* TODO: Used for EVB board, should comment here for starlight board, remove it later*/
+	ov5640: ov5640@3c {
+		compatible = "ovti,ov5640";
+		reg = <0x3c>;
+		clocks = <&clk_ext_camera>;
+		clock-names = "xclk";
+		powerdown-gpios = <&gpio 27 GPIO_ACTIVE_HIGH>;
+		reset-gpios = <&gpio 28 GPIO_ACTIVE_HIGH>;
+		//DOVDD-supply = <&v2v8>;
+		rotation = <180>;
+		port {
+			// Parallel bus endpoint
+			ov5640_to_parallel: endpoint {
+				remote-endpoint = <&parallel_from_ov5640>;
+				bus-type = <5>;      // Parallel
+				bus-width = <8>;
+				data-shift = <2>; // lines 9:2 are used
+				hsync-active = <0>;
+				vsync-active = <1>;
+				pclk-sample = <1>;
+			};
+		};
+	};
+	sc2235@30 {
+		compatible = "sc2235";
+		reg = <0x30>;
+		clocks = <&clk_ext_camera>;
+		clock-names = "xclk";
+		powerdown-gpios = <&gpio 27 GPIO_ACTIVE_HIGH>;
+		reset-gpios = <&gpio 28 GPIO_ACTIVE_HIGH>;
+		//DOVDD-supply = <&v2v8>;
+		port {
+			// Parallel bus endpoint
+			sc2235_to_parallel: endpoint {
+				remote-endpoint = <&parallel_from_sc2235>;
+				bus-type = <5>;      // Parallel
+				bus-width = <8>;
+				data-shift = <2>; // lines 13:6 are used
+				hsync-active = <1>;
+				vsync-active = <1>;
+				pclk-sample = <1>;
+			};
+		};
+	};	
 };
 
 &i2c2 {
@@ -488,6 +562,40 @@
 	pinctrl-names = "default";
 	pinctrl-0 = <&i2c2_pins>;
 	status = "okay";
+
+	seeed_plane_i2c@45 {
+		compatible = "seeed_panel";
+		reg = <0x45>;
+
+		port {
+			panel_dsi_port: endpoint {
+				remote-endpoint = <&dsi_out_port>;
+			};
+		};
+	};
+
+	imx219sub@10 {
+		compatible = "imx219";
+		reg = <0x10>;
+		clocks = <&clk_ext_camera>;
+		clock-names = "xclk";
+		// powerdown-gpio = <&gpio 21 GPIO_ACTIVE_HIGH>;
+		reset-gpio = <&gpio 57 GPIO_ACTIVE_HIGH>;
+		//DOVDD-supply = <&v2v8>;
+		rotation = <0>;
+		orientation = <0>; //CAMERA_ORIENTATION_FRONT
+
+		port {
+			/* CSI2 bus endpoint */
+			imx219_to_csi2rx1: endpoint {
+				remote-endpoint = <&csi2rx1_from_imx219>;
+				bus-type = <4>;      /* MIPI CSI-2 D-PHY */
+				clock-lanes = <0>;
+				data-lanes = <1 2>;
+				link-frequencies = /bits/ 64 <456000000>;
+			};
+		};
+	};
 };
 
 &osc_sys {
@@ -570,6 +678,238 @@
 	};
 };
 
+&sfivefb {
+	status = "okay";
+
+	pp1 {
+		pp-id = <1>;
+		fifo-out;
+		src-format = <COLOR_YUV420_NV21>;
+		src-width = <1920>;
+		src-height = <1080>;
+		dst-format = <COLOR_RGB888_ARGB>;
+		dst-width = <1920>;
+		dst-height = <1080>;
+	};
+
+	tda_998x_1080p {
+		compatible = "starfive,display-dev";
+		panel_name = "tda_998x_1080p";
+		panel_lcd_id = <22>;  /* 1080p */
+		interface_info = "rgb_interface";
+		refresh_en = <1>;
+		bits-per-pixel = <16>;
+		physical-width = <62>;
+		physical-height = <114>;
+		panel-width = <1920>;
+		panel-height = <1080>;
+		pixel-clock = <78000000>;
+		/*dyn_fps;*/		/*dynamic frame rate support*/
+
+		/*.flags = PREFER_CMD_SEND_MONOLITHIC | CE_CMD_SEND_MONOLITHIC | RESUME_WITH_PREFER | RESUME_WITH_CE*/
+		/*gamma-command-monolithic;*/
+		/*ce-command-monolithic;*/
+		/*resume-with-gamma;*/
+		/*resume-with-ce;*/
+
+		/*mipi info*/
+		mipi-byte-clock = <78000>;
+		mipi-escape-clock = <13000>;
+		lane-no = <4>;
+		display_mode = "video_mode";		/*video_mode, command_mode*/
+
+		/*
+		auto_stop_clklane_en;
+		im_pin_val;*/
+
+		color_bits = <COLOR_CODE_24BIT>;
+		/*is_18bit_loosely;*/
+
+		/*video mode info*/
+		h-pulse-width = <44>;
+		h-back-porch = <148>;
+		h-front-porch = <88>;
+		v-pulse-width = <5>;
+		v-back-porch = <36>;
+		v-front-porch = <4>;
+		status = "okay";
+		sync_pol = "vsync_high_act";		/*vsync_high_act, hsync_high_act*/
+		lp_cmd_en;
+		/*lp_hfp_en;*/
+		/*lp_hbp_en;*/
+		/*lp_vact_en;*/
+		lp_vfp_en;
+		lp_vbp_en;
+		lp_vsa_en;
+		traffic-mode = "burst_with_sync_pulses";	/*non_burst_with_sync_pulses, non_burst_with_sync_events*/
+
+		/*phy info*/
+		data_tprepare = /bits/ 8 <0>;
+		data_hs_zero = /bits/ 8 <0>;
+		data_hs_exit = /bits/ 8 <0>;
+		data_hs_trail = /bits/ 8 <0>;
+
+		/*te info*/
+		te_source = "external_pin";			/*external_pin, dsi_te_trigger*/
+		te_trigger_mode = "rising_edge";		/*rising_edge, high_1000us*/
+		te_enable = <0>;
+		cm_te_effect_sync_enable = <0>;			/*used in command mode*/
+		te_count_per_sec = <64>;				/*used in esd*/
+
+		/*ext info*/
+		/*
+		crc_rx_en;
+		ecc_rx_en;
+		eotp_rx_en;
+		*/
+		eotp_tx_en;
+
+		dev_read_time = <0x7fff>;
+		/*type cmd return_count return_code*/
+		/*id_read_cmd_info = [];*/
+		/*pre_id_cmd = [];*/
+		/*esd_read_cmd_info = [DCS_CMD 0A 01 9C];*/
+		/*pre_esd_cmd = [];*/
+		/*panel-on-command = [];*/
+		/*panel-off-command = [];*/
+		/*reset-sequence = <1 5>, <0 10>, <1 30>;*/
+		/*
+		panel-gamma-warm-command = [
+
+		];
+		panel-gamma-nature-command = [
+
+		];
+		panel-gamma-cool-command = [
+
+		];
+
+		panel-ce-std-command = [
+
+		];
+		panel-ce-vivid-command = [
+
+		];
+		*/
+	};
+
+	seeed_5_inch {
+		compatible = "starfive,display-dev";
+		panel_name = "seeed_5_inch";
+		panel_lcd_id = <22>;  /* 480p */
+		interface_info = "mipi_interface";
+		refresh_en = <1>;
+		bits-per-pixel = <24>;
+		physical-width = <62>;
+		physical-height = <114>;
+		panel-width = <800>;
+		panel-height = <480>;
+		pixel-clock = <27500000>;
+		/*dyn_fps;*/		/*dynamic frame rate support*/
+		fps = <50>;
+		/*.flags = PREFER_CMD_SEND_MONOLITHIC | CE_CMD_SEND_MONOLITHIC | RESUME_WITH_PREFER | RESUME_WITH_CE*/
+		/*gamma-command-monolithic;*/
+		/*ce-command-monolithic;*/
+		/*resume-with-gamma;*/
+		/*resume-with-ce;*/
+
+		/*mipi info*/
+		mipi-byte-clock = <78000>;
+		mipi-escape-clock = <13000>;
+		lane-no = <1>;
+		display_mode = "video_mode";		/*video_mode, command_mode*/
+
+		/*
+		auto_stop_clklane_en;
+		im_pin_val;
+		*/
+
+		color_bits = <COLOR_CODE_24BIT>;
+		/*is_18bit_loosely;*/
+
+		/*video mode info*/
+		h-pulse-width = <10>;
+		h-back-porch = <20>;
+		h-front-porch = <50>;
+		v-pulse-width = <5>;
+		v-back-porch = <5>;
+		v-front-porch = <135>;
+
+		/*seeed panel mode info*/
+		dphy_bps   = <700000000>;
+		dsi_burst_mode = <0>;
+		dsi_sync_pulse = <1>;
+		// bytes
+		dsi_hsa   = <30>;
+		dsi_hbp   = <211>;
+		dsi_hfp   = <159>;
+		// lines
+		dsi_vsa   = <5>;
+		dsi_vbp   = <5>;
+		dsi_vfp   = <134>;
+
+		status = "okay";
+		sync_pol = "vsync_high_act";		/*vsync_high_act, hsync_high_act*/
+		lp_cmd_en;
+		/*lp_hfp_en;*/
+		/*lp_hbp_en;*/
+		/*lp_vact_en;*/
+		lp_vfp_en;
+		lp_vbp_en;
+		lp_vsa_en;
+		traffic-mode = "burst_with_sync_pulses";	/*non_burst_with_sync_pulses, non_burst_with_sync_events*/
+
+		/*phy info*/
+		data_tprepare = /bits/ 8 <0>;
+		data_hs_zero = /bits/ 8 <0>;
+		data_hs_exit = /bits/ 8 <0>;
+		data_hs_trail = /bits/ 8 <0>;
+
+		/*te info*/
+		te_source = "external_pin";			/*external_pin, dsi_te_trigger*/
+		te_trigger_mode = "rising_edge";		/*rising_edge, high_1000us*/
+		te_enable = <0>;
+		cm_te_effect_sync_enable = <0>;			/*used in command mode*/
+		te_count_per_sec = <64>;				/*used in esd*/
+
+		/*ext info*/
+		/*
+		crc_rx_en;
+		ecc_rx_en;
+		eotp_rx_en;
+		*/
+		eotp_tx_en;
+
+		dev_read_time = <0x7fff>;
+		/*type cmd return_count return_code*/
+		/*id_read_cmd_info = [];*/
+		/*pre_id_cmd = [];*/
+		/*esd_read_cmd_info = [DCS_CMD 0A 01 9C];*/
+		/*pre_esd_cmd = [];*/
+		/*panel-on-command = [];*/
+		/*panel-off-command = [];*/
+		/*reset-sequence = <1 5>, <0 10>, <1 30>;*/
+		/*
+		panel-gamma-warm-command = [
+
+		];
+		panel-gamma-nature-command = [
+
+		];
+		panel-gamma-cool-command = [
+
+		];
+
+		panel-ce-std-command = [
+
+		];
+		panel-ce-vivid-command = [
+
+		];
+		*/
+	};
+};
+
 &spi2 {
 	pinctrl-names = "default";
 	pinctrl-0 = <&spi2_pins>;
diff --git a/arch/riscv/boot/dts/starfive/jh7100.dtsi b/arch/riscv/boot/dts/starfive/jh7100.dtsi
index 48f87141f00e..5b463e89b16b 100644
--- a/arch/riscv/boot/dts/starfive/jh7100.dtsi
+++ b/arch/riscv/boot/dts/starfive/jh7100.dtsi
@@ -5,10 +5,12 @@
  */
 
 /dts-v1/;
+#include "starfive_vic7100_clk.dtsi"
 #include <dt-bindings/clock/starfive-jh7100.h>
 #include <dt-bindings/clock/starfive-jh7100-audio.h>
 #include <dt-bindings/reset/starfive-jh7100.h>
 #include <dt-bindings/reset/starfive-jh7100-audio.h>
+#include <dt-bindings/starfive_fb.h>
 
 / {
 	compatible = "starfive,jh7100";
@@ -395,6 +397,195 @@
 			interrupts = <98>;
 		};
 
+		vin_sysctl: vin_sysctl@19800000 {
+			compatible = "starfive,stf-vin";
+			reg = <0x0 0x19800000 0x0 0x10000>,
+			      <0x0 0x19810000 0x0 0x10000>,
+			      <0x0 0x19820000 0x0 0x10000>,
+			      <0x0 0x19830000 0x0 0x10000>,
+			      <0x0 0x19840000 0x0 0x10000>,
+			      <0x0 0x19870000 0x0 0x30000>,
+			      <0x0 0x198a0000 0x0 0x30000>,
+			      <0x0 0x11800000 0x0 0x10000>,
+			      <0x0 0x11840000 0x0 0x10000>,
+			      <0x0 0x11858000 0x0 0x10000>;
+			reg-names = "mipi0", "vclk", "vrst", "mipi1", "sctrl",
+			            "isp0", "isp1", "tclk", "trst", "iopad";
+			interrupts = <119 109 112>;
+			memory-region = <&vin_reserved>;
+
+			clocks =  <&clkgen JH7100_CLK_VIN_SRC>,
+				 <&clkgen JH7100_CLK_ISP0_AXI>,
+				 <&clkgen JH7100_CLK_ISP0NOC_AXI>,
+				 <&clkgen JH7100_CLK_ISPSLV_AXI>,
+				 <&clkgen JH7100_CLK_ISP1_AXI>,
+				 <&clkgen JH7100_CLK_ISP1NOC_AXI>,
+				 <&clkgen JH7100_CLK_VIN_AXI>,
+				 <&clkgen JH7100_CLK_VINNOC_AXI>;
+			//	 <&clkgen JH7100_CLK_CSI2RX_APB>,
+			// 	 <&clkgen JH7100_CLK_MIPI_RX0_PXL_0>,
+			// 	 <&clkgen JH7100_CLK_MIPI_RX0_PXL_1>,
+			// 	 <&clkgen JH7100_CLK_MIPI_RX0_PXL_2>,
+			// 	 <&clkgen JH7100_CLK_MIPI_RX0_PXL_3>,
+			// 	 <&clkgen JH7100_CLK_MIPI_RX0_SYS>,
+			// 	 <&clkgen JH7100_CLK_MIPI_RX1_PXL_0>,
+			// 	 <&clkgen JH7100_CLK_MIPI_RX1_PXL_1>,
+			// 	 <&clkgen JH7100_CLK_MIPI_RX1_PXL_2>,
+			// 	 <&clkgen JH7100_CLK_MIPI_RX1_PXL_3>,
+			// 	 <&clkgen JH7100_CLK_MIPI_RX1_SYS>,
+			// 	 <&clkgen JH7100_CLK_DPHY_CFGCLK>,
+			// 	 <&clkgen JH7100_CLK_DPHY_REFCLK>,
+			// 	 <&clkgen JH7100_CLK_DPHY_TXCLKESC>,
+			// 	 <&clkgen JH7100_CLK_ISP0_CTRL>,
+			// 	 <&clkgen JH7100_CLK_ISP0_2X_CTRL>,
+			// 	 <&clkgen JH7100_CLK_ISP0_MIPI_CTRL>,
+			// 	 <&clkgen JH7100_CLK_ISP1_CTRL>,
+			// 	 <&clkgen JH7100_CLK_ISP1_2X_CTRL>,
+			// 	 <&clkgen JH7100_CLK_ISP1_MIPI_CTRL>;
+
+			clock-names = "vin_src",
+				      "isp0_axi",
+				      "isp0noc_axi",
+				      "ispslv_axi",
+				      "isp1_axi",
+				      "isp1noc_axi",
+				      "vin_axi",
+				      "vinnoc_axi";
+				// 	"csi2rx_apb_clk",
+				// 	"mipirx0_pixel0",
+				// 	"mipirx0_pixel1",
+				// 	"mipirx0_pixel2",
+				// 	"mipirx0_pixel3",
+				// 	"mipirx0_sys",
+				// 	"mipirx1_pixel0",
+				// 	"mipirx1_pixel1",
+				// 	"mipirx1_pixel2",
+				// 	"mipirx1_pixel3",
+				// 	"mipirx1_sys",
+				// 	"csidphy_cfgclk",
+				// 	"csidphy_regclk",
+				// 	"csidphy_txclkesc",
+				// 	"isp0_ctrl",
+				// 	"isp0_2x_ctrl",
+				// 	"isp0_mipi_ctrl",
+				// 	"isp1_ctrl",
+				// 	"isp1_2x_ctrl",
+				// 	"isp1_mipi_ctrl";
+
+			ports {
+				#address-cells = <1>;
+				#size-cells = <0>;
+
+				port@0 {
+					reg = <0>;
+
+					dphy0_out_csi2rx: endpoint {
+						remote-endpoint = <&csi2rx0_in_dphy>;
+						clock-lanes = <0>;
+						data-lanes = <1 2>;
+						status = "failed";
+					};
+				};
+
+				//port@1 {
+				//	reg = <1>;
+
+				//	dphy1_out_csi2rx: endpoint {
+				//		remote-endpoint = <&csi2rx1_in_dphy>;
+				//		clock-lanes = <0>;
+				//		data-lanes = <1 2>;
+				//		status = "failed";
+				//	};
+				//};
+
+				/* TODO: Used for EVB board, comment here for starlight board, remove it later*/
+				port@2 {
+					reg = <2>; // dvp sensor
+
+					// Parallel bus endpoint
+					parallel_from_ov5640: endpoint {
+						remote-endpoint = <&ov5640_to_parallel>;
+						bus-type = <5>;      // Parallel
+						bus-width = <8>;
+						data-shift = <2>; // lines 9:2 are used
+						hsync-active = <1>;
+						vsync-active = <0>;
+						pclk-sample = <1>;
+						status = "failed";
+					};
+				};
+
+				port@3 {
+					reg = <2>; //dvp sensor
+
+					// Parallel bus endpoint
+					parallel_from_sc2235: endpoint {
+						remote-endpoint = <&sc2235_to_parallel>;
+						bus-type = <5>;      // Parallel
+						bus-width = <8>;
+						data-shift = <2>; // lines 9:2 are used
+						hsync-active = <1>;
+						vsync-active = <1>;
+						pclk-sample = <1>;
+						status = "failed";
+					};
+				};
+
+				port@4 {
+					reg = <3>; //csi2rx0 sensor
+
+					/* CSI2 bus endpoint */
+					csi2rx0_from_imx219: endpoint {
+						remote-endpoint = <&imx219_to_csi2rx0>;
+						bus-type = <4>;      /* MIPI CSI-2 D-PHY */
+						clock-lanes = <0>;
+						data-lanes = <1 2>;
+						status = "okay";
+					};
+				};
+
+				port@5 {
+					reg = <4>; // csi2rx1 sensor
+
+					/* CSI2 bus endpoint */
+					csi2rx1_from_imx219: endpoint {
+						remote-endpoint = <&imx219_to_csi2rx1>;
+						bus-type = <4>;      /* MIPI CSI-2 D-PHY */
+						clock-lanes = <5>;
+						data-lanes = <3 4>;
+						lane-polarities = <0 1 0>;
+						status = "okay";
+					};
+				};
+			};
+		};
+
+		csi2rx: csi-bridge@19800000 {
+			compatible = "cdns,csi2rx";
+			reg = <0x0 0x19800000 0x0 0x10000>;
+			clocks = <&byteclock>, <&byteclock>,
+				 <&coreclock>, <&coreclock>,
+				 <&coreclock>, <&coreclock>;
+			clock-names = "sys_clk", "p_clk",
+				      "pixel_if0_clk", "pixel_if1_clk",
+				      "pixel_if2_clk", "pixel_if3_clk";
+
+			ports {
+				#address-cells = <1>;
+				#size-cells = <0>;
+
+				port@0 {
+					reg = <0>;
+
+					csi2rx0_in_dphy: endpoint {
+						remote-endpoint = <&dphy0_out_csi2rx>;
+						clock-lanes = <0>;
+						data-lanes = <1 2>;
+					};
+				};
+			};
+		};
+
 		vpu_enc: vpu_enc@118e0000 {
 			compatible = "cm,cm521-vpu";
 			reg = <0x0 0x118e0000 0x0 0x4000>;
@@ -445,6 +636,29 @@
 			status = "okay";
 		};
 
+		sfivefb: sfivefb@12000000 {
+			compatible = "starfive,vpp-lcdc";
+			interrupts = <101>, <103>;
+			interrupt-names = "lcdc_irq", "vpp1_irq";
+			reg = <0x0 0x12000000 0x0 0x10000>,
+			      <0x0 0x12100000 0x0 0x10000>,
+			      <0x0 0x12040000 0x0 0x10000>,
+			      <0x0 0x12080000 0x0 0x10000>,
+			      <0x0 0x120c0000 0x0 0x10000>,
+			      <0x0 0x12240000 0x0 0x10000>,
+			      <0x0 0x12250000 0x0 0x10000>,
+			      <0x0 0x12260000 0x0 0x10000>;
+			reg-names = "lcdc", "dsitx", "vpp0", "vpp1", "vpp2", "clk", "rst", "sys";
+			memory-region = <&sffb_reserved>;
+#if 0 // FIXME uart clocks can't be right for lcdc
+			clocks = <&clkgen JH7100_CLK_UART>,
+				 <&clkgen JH7100_CLK_APB2>;
+#endif
+			clock-names = "baudclk", "apb_pclk";
+			ddr-format = <WIN_FMT_RGB565>;/*LCDC win_format*/
+			status = "disabled";
+		};
+
 		display: display-subsystem {
 			compatible = "starfive,display-subsystem";
 			dma-coherent;
@@ -489,6 +703,35 @@
 			};
 		};
 
+		mipi_dphy: mipi-dphy@12260000{
+				compatible = "starfive,jh7100-mipi-dphy-tx";
+				//reg = <0x0 0x12260000 0x0 0x10000>;
+				clocks = <&uartclk>, <&apb2clk>;
+				clock-names = "baudclk", "apb_pclk";
+				#phy-cells = <0>;
+		};
+
+		mipi_dsi: mipi@12100000 {
+			compatible = "cdns,dsi";
+			clocks = <&apb1clk>, <&apb2clk>;
+			clock-names = "dsi_p_clk", "dsi_sys_clk";
+			phys = <&mipi_dphy>;
+			phy-names = "dphy";
+			status = "okay";
+			reg = <0x0 0x12100000 0x0 0x10000>;
+			reg-names = "dsi";
+
+			port {
+				dsi_out_port: endpoint {
+					remote-endpoint = <&panel_dsi_port>;
+				};
+			};
+			mipi_panel: panel@0 {
+				//compatible = "";
+				status = "okay";
+			};
+		};
+
 		spi2: spi@12410000 {
 			compatible = "snps,dw-apb-ssi";
 			reg = <0x0 0x12410000 0x0 0x10000>;
diff --git a/arch/riscv/boot/dts/starfive/starfive_vic7100_clk.dtsi b/arch/riscv/boot/dts/starfive/starfive_vic7100_clk.dtsi
new file mode 100644
index 000000000000..6fe645c884e0
--- /dev/null
+++ b/arch/riscv/boot/dts/starfive/starfive_vic7100_clk.dtsi
@@ -0,0 +1,113 @@
+/ {
+	hfclk: hfclk {
+		#clock-cells = <0>;
+		compatible = "fixed-clock";
+		clock-frequency = <25000000>;
+		clock-output-names = "hfclk";
+	};
+	rtcclk: rtcclk {
+		#clock-cells = <0>;
+		compatible = "fixed-clock";
+		clock-frequency = <6250000>;
+		clock-output-names = "rtcclk";
+	};
+	axiclk: axiclk {
+		#clock-cells = <0>;
+		compatible = "fixed-clock";
+		clock-frequency = <500000000>;
+		clock-output-names = "axiclk";
+	};
+
+	ahb0clk: ahb0clk {
+		#clock-cells = <0>;
+		compatible = "fixed-clock";
+		clock-frequency = <250000000>;
+	};
+	ahb2clk: ahb2clk {
+		#clock-cells = <0>;
+		compatible = "fixed-clock";
+		clock-frequency = <125000000>;
+	};
+	apb1clk: apb1clk {
+		#clock-cells = <0>;
+		compatible = "fixed-clock";
+		clock-frequency = <125000000>;
+	};
+	apb2clk: apb2clk {
+		#clock-cells = <0>;
+		compatible = "fixed-clock";
+		clock-frequency = <125000000>;
+	};
+	jpuclk: jpuclk {
+		#clock-cells = <0>;
+		compatible = "fixed-clock";
+		clock-frequency = <333333333>;
+	};
+	vpuclk: vpuclk {
+		#clock-cells = <0>;
+		compatible = "fixed-clock";
+		clock-frequency = <400000000>;
+	};
+	gmacclk: gmacclk {
+		#clock-cells = <0>;
+		compatible = "fixed-clock";
+		clock-frequency = <25000000>;
+	};
+	qspi_clk: qspi-clk@0 {
+		#clock-cells = <0>;
+		compatible = "fixed-clock";
+		clock-frequency = <50000000>;
+	};
+	uartclk: uartclk {
+		#clock-cells = <0>;
+		compatible = "fixed-clock";
+		clock-frequency = <100000000>;
+	};
+	hs_uartclk: hs_uartclk {
+		#clock-cells = <0>;
+		compatible = "fixed-clock";
+		clock-frequency = <74250000>;
+	};
+	dwmmc_biuclk: dwmmc_biuclk {
+		#clock-cells = <0>;
+		compatible = "fixed-clock";
+		clock-frequency = <100000000>;
+	};
+/*
+	dwmmc_ciuclk: dwmmc_ciuclk {
+		#clock-cells = <0>;
+		compatible = "fixed-clock";
+		clock-frequency = <100000000>;
+	};
+*/
+	pwmclk: pwmclk {
+            #clock-cells = <0>;
+            compatible = "fixed-clock";
+            clock-frequency = <125000000>;
+        };
+	spiclk: spiclk {
+	    #clock-cells = <0>;
+            compatible = "fixed-clock";
+            clock-frequency = <50000000>;
+	};
+	audioclk: audioclk {
+            #clock-cells = <0>;
+            compatible = "fixed-clock";
+            clock-frequency = <12288000>;
+        };
+	clk_ext_camera: clk-ext-camera {
+		#clock-cells = <0>;
+		compatible = "fixed-clock";
+		clock-frequency = <24000000>;
+	};
+	byteclock: byteclock {
+		#clock-cells = <0>;
+		compatible = "fixed-clock";
+		clock-frequency = <24000000>;
+	};
+	coreclock: coreclock {
+		#clock-cells = <0>;
+		compatible = "fixed-clock";
+		clock-frequency = <24000000>;
+	};
+};
diff --git a/drivers/media/platform/Kconfig b/drivers/media/platform/Kconfig
index 80321e03809a..47a27f626621 100644
--- a/drivers/media/platform/Kconfig
+++ b/drivers/media/platform/Kconfig
@@ -171,6 +171,7 @@ source "drivers/media/platform/xilinx/Kconfig"
 source "drivers/media/platform/rcar-vin/Kconfig"
 source "drivers/media/platform/atmel/Kconfig"
 source "drivers/media/platform/sunxi/Kconfig"
+source "drivers/media/platform/starfive/Kconfig"
 
 config VIDEO_TI_CAL
 	tristate "TI CAL (Camera Adaptation Layer) driver"
diff --git a/drivers/media/platform/Makefile b/drivers/media/platform/Makefile
index 73ce083c2fc6..57b4202a15ce 100644
--- a/drivers/media/platform/Makefile
+++ b/drivers/media/platform/Makefile
@@ -43,6 +43,8 @@ obj-$(CONFIG_VIDEO_STI_DELTA)		+= sti/delta/
 
 obj-y					+= stm32/
 
+obj-y					+= starfive/
+
 obj-y					+= davinci/
 
 obj-$(CONFIG_VIDEO_SH_VOU)		+= sh_vou.o
diff --git a/drivers/media/platform/starfive/Kconfig b/drivers/media/platform/starfive/Kconfig
new file mode 100755
index 000000000000..49e4b09637bd
--- /dev/null
+++ b/drivers/media/platform/starfive/Kconfig
@@ -0,0 +1,51 @@
+# SPDX-License-Identifier: GPL-2.0
+# Copyright (C) 2021 StarFive Technology Co., Ltd.
+#
+# VIN sensor driver configuration
+#
+config VIDEO_STARFIVE_VIN
+	tristate "starfive VIC video input support"
+	depends on VIDEO_V4L2 && OF
+	select MEDIA_CONTROLLER
+	select VIDEOBUF2_DMA_CONTIG
+	select VIDEO_V4L2_SUBDEV_API
+	select V4L2_FWNODE
+	help
+	  To compile this driver as a module, choose M here: the module
+	  will be called stf-vin.
+
+choice
+	prompt "Image Sensor for VIC board"
+	default VIN_SENSOR_IMX219
+	depends on VIDEO_STARFIVE_VIN
+	optional
+
+config VIN_SENSOR_OV5640
+	bool "VIN SENSOR support OV5640"
+	depends on VIDEO_STARFIVE_VIN
+	select V4L2_FWNODE
+	help
+	  Say Y here if you want to have support for VIN sensor OV5640
+
+config VIN_SENSOR_SC2235
+	bool "VIN SENSOR support SC2235"
+	depends on VIDEO_STARFIVE_VIN
+	select V4L2_FWNODE
+	help
+	  Say Y here if you want to have support for VIN sensor SC2235
+
+config VIN_SENSOR_OV4689
+	bool "VIN SENSOR support OV4689"
+	depends on VIDEO_STARFIVE_VIN
+	select V4L2_FWNODE
+	help
+	  Say Y here if you want to have support for VIN sensor OV4689
+
+config VIN_SENSOR_IMX219
+	bool "VIN SENSOR support imx219"
+	depends on VIDEO_STARFIVE_VIN
+	select V4L2_FWNODE
+	help
+	  Say Y here if you want to have support for VIN sensor IMX219
+
+endchoice
diff --git a/drivers/media/platform/starfive/Makefile b/drivers/media/platform/starfive/Makefile
new file mode 100755
index 000000000000..5353d3a7de49
--- /dev/null
+++ b/drivers/media/platform/starfive/Makefile
@@ -0,0 +1,23 @@
+# SPDX-License-Identifier: GPL-2.0
+#
+# Copyright (C) 2021 StarFive Technology Co., Ltd.
+#
+# Makefile for starfive v4l2 driver framework
+#
+obj-$(CONFIG_VIN_SENSOR_OV5640) += ov5640.o
+obj-$(CONFIG_VIN_SENSOR_SC2235) += sc2235.o
+obj-$(CONFIG_VIN_SENSOR_OV4689) += ov4689_mipi.o
+obj-$(CONFIG_VIN_SENSOR_IMX219) += imx219_mipi.o
+obj-$(CONFIG_VIDEO_STARFIVE_VIN) += stfcamss.o \
+								stf_event.o \
+								stf_dvp.o \
+								stf_csi.o \
+								stf_csiphy.o \
+								stf_isp.o \
+								stf_video.o \
+								stf_vin.o \
+								stf_vin_hw_ops.o \
+								stf_csi_hw_ops.o \
+								stf_csiphy_hw_ops.o \
+								stf_isp_hw_ops.o \
+								stf_dvp_hw_ops.o
diff --git a/drivers/media/platform/starfive/Readme.txt b/drivers/media/platform/starfive/Readme.txt
new file mode 100644
index 000000000000..f2cad538c371
--- /dev/null
+++ b/drivers/media/platform/starfive/Readme.txt
@@ -0,0 +1,98 @@
+
+
+/dev/video0 sensor配置为ov5640的设备节点
+/dev/video1 sensor配置为ov4689(i2c0)的设备节点
+/dev/video2 sensor配置为sc2235/ov4689(i2c2)的设备节点
+
+确认conf/sdk_210209_defconfig
+CONFIG_VIN_SENSOR_OV5640=y
+CONFIG_VIN_SENSOR_SC2235=y
+CONFIG_VIN_SENSOR_OV4689=y
+
+
+只支持DPHY的lane0/lane5做clk通道，lane1/2/3/4做数据通道。
+
+sensor port 设为okay, 硬件需要接入对应的sensor，否则驱动不能使用。
+
+1. ov5640 config dts:
+	parallel_from_ov5640 port status 设置为okay, sc2235 port status 设为failed.
+		port@2 {
+			reg = <2>; // dvp sensor
+
+			/* Parallel bus endpoint */
+			parallel_from_ov5640: endpoint {
+				remote-endpoint = <&ov5640_to_parallel>;
+				bus-type = <5>;      /* Parallel */
+				bus-width = <8>;
+				data-shift = <2>; /* lines 9:2 are used */
+				hsync-active = <1>;
+				vsync-active = <0>;
+				pclk-sample = <1>;
+				sensor-type = <0>; //0:SENSOR_VIN 1:SENSOR_ISP0 2:SENSOR_ISP1
+				status = "okay";
+			};
+		};
+
+2. SC2235 config dts:
+	stf_isp_hw_ops.c:
+		stf_isp_set_format函数里面注释掉：
+			// isp_settings = isp_1920_1080_settings;
+
+	parallel_from_sc2235 port status 设置为okay, ov5640/ov4689(i2c2) port status设为failed.
+		port@3 {
+			reg = <2>; // dvp sensor
+
+			/* Parallel bus endpoint */
+			parallel_from_sc2235: endpoint {
+				remote-endpoint = <&sc2235_to_parallel>;
+				bus-type = <5>;      /* Parallel */
+				bus-width = <8>;
+				data-shift = <2>; /* lines 9:2 are used */
+				hsync-active = <1>;
+				vsync-active = <1>;
+				pclk-sample = <1>;
+				sensor-type = <2>; //0:SENSOR_VIN 1:SENSOR_ISP0 2:SENSOR_ISP1
+				status = "okay";
+			};
+		};
+
+3. i2c0 ov4689 config dts:
+	csi2rx0_from_ov4689 port status 设置为okay.
+		port@4 {
+			reg = <3>; // csi2rx0 sensor
+
+			/* CSI2 bus endpoint */
+			csi2rx0_from_ov4689: endpoint {
+				remote-endpoint = <&ov4689_to_csi2rx0>;
+				bus-type = <4>;      /* MIPI CSI-2 D-PHY */
+				clock-lanes = <0>;
+				data-lanes = <1 2>;
+				sensor-type = <1>; //0:SENSOR_VIN 1:SENSOR_ISP0 2:SENSOR_ISP1
+				csi-dt = <0x2b>;
+				status = "okay";
+			};
+		};
+
+4. i2c2 ov4689 config dts:
+
+	stf_isp_hw_ops.c:
+		stf_isp_set_format函数里面346行不要注释掉：
+			isp_settings = isp_1920_1080_settings;
+
+	csi2rx1_from_ov4689 port status 设置为okay, sc2235 port status 设为failed.
+		port@5 {
+			reg = <4>; // csi2rx1 sensor
+
+			/* CSI2 bus endpoint */
+			csi2rx1_from_ov4689: endpoint {
+				remote-endpoint = <&ov4689_to_csi2rx1>;
+				bus-type = <4>;      /* MIPI CSI-2 D-PHY */
+				clock-lanes = <5>;
+				data-lanes = <4 3>;
+				lane-polarities = <1 1 1>;
+				sensor-type = <2>; //0:SENSOR_VIN 1:SENSOR_ISP0 2:SENSOR_ISP1
+				csi-dt = <0x2b>;
+				status = "okay";
+			};
+		};
+
diff --git a/drivers/media/platform/starfive/imx219_mipi.c b/drivers/media/platform/starfive/imx219_mipi.c
new file mode 100755
index 000000000000..71acafdb6a0c
--- /dev/null
+++ b/drivers/media/platform/starfive/imx219_mipi.c
@@ -0,0 +1,1612 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * A V4L2 driver for Sony IMX219 cameras.
+ * Copyright (C) 2019, Raspberry Pi (Trading) Ltd
+ *
+ * Based on Sony imx258 camera driver
+ * Copyright (C) 2018 Intel Corporation
+ *
+ * DT / fwnode changes, and regulator / GPIO control taken from imx214 driver
+ * Copyright 2018 Qtechnology A/S
+ *
+ * Flip handling taken from the Sony IMX319 driver.
+ * Copyright (C) 2018 Intel Corporation
+ *
+ * Copyright (C) 2021 StarFive Technology Co., Ltd.
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/gpio/consumer.h>
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/pm_runtime.h>
+#include <linux/regulator/consumer.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-mediabus.h>
+#include <asm/unaligned.h>
+
+#define IMX219_REG_VALUE_08BIT		1
+#define IMX219_REG_VALUE_16BIT		2
+
+#define IMX219_REG_MODE_SELECT		0x0100
+#define IMX219_MODE_STANDBY		0x00
+#define IMX219_MODE_STREAMING		0x01
+
+/* Chip ID */
+#define IMX219_REG_CHIP_ID		0x0000
+#define IMX219_CHIP_ID			0x0219
+
+/* External clock frequency is 24.0M */
+#define IMX219_XCLK_FREQ		24000000
+
+/* Pixel rate is fixed at 182.4M for all the modes */
+#define IMX219_PIXEL_RATE		182400000
+
+#define IMX219_DEFAULT_LINK_FREQ	456000000
+
+/* V_TIMING internal */
+#define IMX219_REG_VTS			0x0160
+#define IMX219_VTS_15FPS		0x0dc6
+#define IMX219_VTS_30FPS_1080P		0x06e3
+#define IMX219_VTS_30FPS_BINNED		0x06e3
+#define IMX219_VTS_30FPS_640x480	0x06e3
+#define IMX219_VTS_MAX			0xffff
+
+#define IMX219_VBLANK_MIN		4
+
+/*Frame Length Line*/
+#define IMX219_FLL_MIN			0x08a6
+#define IMX219_FLL_MAX			0xffff
+#define IMX219_FLL_STEP			1
+#define IMX219_FLL_DEFAULT		0x0c98
+
+/* HBLANK control - read only */
+#define IMX219_PPL_DEFAULT		3448
+
+/* Exposure control */
+#define IMX219_REG_EXPOSURE		0x015a
+#define IMX219_EXPOSURE_MIN		4
+#define IMX219_EXPOSURE_STEP		1
+#define IMX219_EXPOSURE_DEFAULT		0x640
+#define IMX219_EXPOSURE_MAX		65535
+
+/* Analog gain control */
+#define IMX219_REG_ANALOG_GAIN		0x0157
+#define IMX219_ANA_GAIN_MIN		0
+#define IMX219_ANA_GAIN_MAX		232
+#define IMX219_ANA_GAIN_STEP		1
+#define IMX219_ANA_GAIN_DEFAULT		0xd0
+
+/* Digital gain control */
+#define IMX219_REG_DIGITAL_GAIN		0x0158
+#define IMX219_DGTL_GAIN_MIN		0x0100
+#define IMX219_DGTL_GAIN_MAX		0x0fff
+#define IMX219_DGTL_GAIN_DEFAULT	0x0100
+#define IMX219_DGTL_GAIN_STEP		1
+
+#define IMX219_REG_ORIENTATION		0x0172
+
+/* Test Pattern Control */
+#define IMX219_REG_TEST_PATTERN		0x0600
+#define IMX219_TEST_PATTERN_DISABLE	0
+#define IMX219_TEST_PATTERN_SOLID_COLOR	1
+#define IMX219_TEST_PATTERN_COLOR_BARS	2
+#define IMX219_TEST_PATTERN_GREY_COLOR	3
+#define IMX219_TEST_PATTERN_PN9		4
+
+/* Test pattern colour components */
+#define IMX219_REG_TESTP_RED		0x0602
+#define IMX219_REG_TESTP_GREENR		0x0604
+#define IMX219_REG_TESTP_BLUE		0x0606
+#define IMX219_REG_TESTP_GREENB		0x0608
+#define IMX219_TESTP_COLOUR_MIN		0
+#define IMX219_TESTP_COLOUR_MAX		0x03ff
+#define IMX219_TESTP_COLOUR_STEP	1
+#define IMX219_TESTP_RED_DEFAULT	IMX219_TESTP_COLOUR_MAX
+#define IMX219_TESTP_GREENR_DEFAULT	0
+#define IMX219_TESTP_BLUE_DEFAULT	0
+#define IMX219_TESTP_GREENB_DEFAULT	0
+
+/* IMX219 native and active pixel array size. */
+#define IMX219_NATIVE_WIDTH		3296U
+#define IMX219_NATIVE_HEIGHT		2480U
+#define IMX219_PIXEL_ARRAY_LEFT		8U
+#define IMX219_PIXEL_ARRAY_TOP		8U
+#define IMX219_PIXEL_ARRAY_WIDTH	3280U
+#define IMX219_PIXEL_ARRAY_HEIGHT	2464U
+
+struct imx219_reg {
+	u16 address;
+	u8 val;
+};
+
+struct imx219_reg_list {
+	unsigned int num_of_regs;
+	const struct imx219_reg *regs;
+};
+
+/* Mode : resolution and related config&values */
+struct imx219_mode {
+	/* Frame width */
+	unsigned int width;
+	/* Frame height */
+	unsigned int height;
+
+	/* Analog crop rectangle. */
+	struct v4l2_rect crop;
+
+	/* V-timing */
+	unsigned int vts_def;
+
+	/* Default register values */
+	struct imx219_reg_list reg_list;
+};
+
+/*
+ * Register sets lifted off the i2C interface from the Raspberry Pi firmware
+ * driver.
+ * 3280x2464 = mode 2, 1920x1080 = mode 1, 1640x1232 = mode 4, 640x480 = mode 7.
+ */
+static const struct imx219_reg mode_3280x2464_regs[] = {
+	{0x0100, 0x00},
+	{0x30eb, 0x0c},
+	{0x30eb, 0x05},
+	{0x300a, 0xff},
+	{0x300b, 0xff},
+	{0x30eb, 0x05},
+	{0x30eb, 0x09},
+	{0x0114, 0x01},
+	{0x0128, 0x00},
+	{0x012a, 0x18},
+	{0x012b, 0x00},
+	{0x0164, 0x00},
+	{0x0165, 0x00},
+	{0x0166, 0x0c},
+	{0x0167, 0xcf},
+	{0x0168, 0x00},
+	{0x0169, 0x00},
+	{0x016a, 0x09},
+	{0x016b, 0x9f},
+	{0x016c, 0x0c},
+	{0x016d, 0xd0},
+	{0x016e, 0x09},
+	{0x016f, 0xa0},
+	{0x0170, 0x01},
+	{0x0171, 0x01},
+	{0x0174, 0x00},
+	{0x0175, 0x00},
+	{0x0301, 0x05},
+	{0x0303, 0x01},
+	{0x0304, 0x03},
+	{0x0305, 0x03},
+	{0x0306, 0x00},
+	{0x0307, 0x39},
+	{0x030b, 0x01},
+	{0x030c, 0x00},
+	{0x030d, 0x72},
+	{0x0624, 0x0c},
+	{0x0625, 0xd0},
+	{0x0626, 0x09},
+	{0x0627, 0xa0},
+	{0x455e, 0x00},
+	{0x471e, 0x4b},
+	{0x4767, 0x0f},
+	{0x4750, 0x14},
+	{0x4540, 0x00},
+	{0x47b4, 0x14},
+	{0x4713, 0x30},
+	{0x478b, 0x10},
+	{0x478f, 0x10},
+	{0x4793, 0x10},
+	{0x4797, 0x0e},
+	{0x479b, 0x0e},
+	{0x0162, 0x0d},
+	{0x0163, 0x78},
+};
+
+static const struct imx219_reg mode_1920_1080_regs[] = {
+	{0x0100, 0x00},
+	{0x30eb, 0x05},
+	{0x30eb, 0x0c},
+	{0x300a, 0xff},
+	{0x300b, 0xff},
+	{0x30eb, 0x05},
+	{0x30eb, 0x09},
+	{0x0114, 0x01},
+	{0x0128, 0x00},
+	{0x012a, 0x18},
+	{0x012b, 0x00},
+	{0x0162, 0x0d},
+	{0x0163, 0x78},
+	{0x0164, 0x02},
+	{0x0165, 0xa8},
+	{0x0166, 0x0a},
+	{0x0167, 0x27},
+	{0x0168, 0x02},
+	{0x0169, 0xb4},
+	{0x016a, 0x06},
+	{0x016b, 0xeb},
+	{0x016c, 0x07},
+	{0x016d, 0x80},
+	{0x016e, 0x04},
+	{0x016f, 0x38},
+	{0x0170, 0x01},
+	{0x0171, 0x01},
+	{0x0174, 0x00},
+	{0x0175, 0x00},
+	{0x0301, 0x05},
+	{0x0303, 0x01},
+	{0x0304, 0x03},
+	{0x0305, 0x03},
+	{0x0306, 0x00},
+	{0x0307, 0x39},
+	{0x030b, 0x01},
+	{0x030c, 0x00},
+	{0x030d, 0x72},
+	{0x0624, 0x07},
+	{0x0625, 0x80},
+	{0x0626, 0x04},
+	{0x0627, 0x38},
+	{0x455e, 0x00},
+	{0x471e, 0x4b},
+	{0x4767, 0x0f},
+	{0x4750, 0x14},
+	{0x4540, 0x00},
+	{0x47b4, 0x14},
+	{0x4713, 0x30},
+	{0x478b, 0x10},
+	{0x478f, 0x10},
+	{0x4793, 0x10},
+	{0x4797, 0x0e},
+	{0x479b, 0x0e},
+};
+
+static const struct imx219_reg mode_1640_1232_regs[] = {
+	{0x0100, 0x00},
+	{0x30eb, 0x0c},
+	{0x30eb, 0x05},
+	{0x300a, 0xff},
+	{0x300b, 0xff},
+	{0x30eb, 0x05},
+	{0x30eb, 0x09},
+	{0x0114, 0x01},
+	{0x0128, 0x00},
+	{0x012a, 0x18},
+	{0x012b, 0x00},
+	{0x0164, 0x00},
+	{0x0165, 0x00},
+	{0x0166, 0x0c},
+	{0x0167, 0xcf},
+	{0x0168, 0x00},
+	{0x0169, 0x00},
+	{0x016a, 0x09},
+	{0x016b, 0x9f},
+	{0x016c, 0x06},
+	{0x016d, 0x68},
+	{0x016e, 0x04},
+	{0x016f, 0xd0},
+	{0x0170, 0x01},
+	{0x0171, 0x01},
+	{0x0174, 0x01},
+	{0x0175, 0x01},
+	{0x0301, 0x05},
+	{0x0303, 0x01},
+	{0x0304, 0x03},
+	{0x0305, 0x03},
+	{0x0306, 0x00},
+	{0x0307, 0x39},
+	{0x030b, 0x01},
+	{0x030c, 0x00},
+	{0x030d, 0x72},
+	{0x0624, 0x06},
+	{0x0625, 0x68},
+	{0x0626, 0x04},
+	{0x0627, 0xd0},
+	{0x455e, 0x00},
+	{0x471e, 0x4b},
+	{0x4767, 0x0f},
+	{0x4750, 0x14},
+	{0x4540, 0x00},
+	{0x47b4, 0x14},
+	{0x4713, 0x30},
+	{0x478b, 0x10},
+	{0x478f, 0x10},
+	{0x4793, 0x10},
+	{0x4797, 0x0e},
+	{0x479b, 0x0e},
+	{0x0162, 0x0d},
+	{0x0163, 0x78},
+};
+
+static const struct imx219_reg mode_640_480_regs[] = {
+	{0x0100, 0x00},
+	{0x30eb, 0x05},
+	{0x30eb, 0x0c},
+	{0x300a, 0xff},
+	{0x300b, 0xff},
+	{0x30eb, 0x05},
+	{0x30eb, 0x09},
+	{0x0114, 0x01},
+	{0x0128, 0x00},
+	{0x012a, 0x18},
+	{0x012b, 0x00},
+	{0x0162, 0x0d},
+	{0x0163, 0x78},
+	{0x0164, 0x03},
+	{0x0165, 0xe8},
+	{0x0166, 0x08},
+	{0x0167, 0xe7},
+	{0x0168, 0x02},
+	{0x0169, 0xf0},
+	{0x016a, 0x06},
+	{0x016b, 0xaf},
+	{0x016c, 0x02},
+	{0x016d, 0x80},
+	{0x016e, 0x01},
+	{0x016f, 0xe0},
+	{0x0170, 0x01},
+	{0x0171, 0x01},
+	{0x0174, 0x03},
+	{0x0175, 0x03},
+	{0x0301, 0x05},
+	{0x0303, 0x01},
+	{0x0304, 0x03},
+	{0x0305, 0x03},
+	{0x0306, 0x00},
+	{0x0307, 0x39},
+	{0x030b, 0x01},
+	{0x030c, 0x00},
+	{0x030d, 0x72},
+	{0x0624, 0x06},
+	{0x0625, 0x68},
+	{0x0626, 0x04},
+	{0x0627, 0xd0},
+	{0x455e, 0x00},
+	{0x471e, 0x4b},
+	{0x4767, 0x0f},
+	{0x4750, 0x14},
+	{0x4540, 0x00},
+	{0x47b4, 0x14},
+	{0x4713, 0x30},
+	{0x478b, 0x10},
+	{0x478f, 0x10},
+	{0x4793, 0x10},
+	{0x4797, 0x0e},
+	{0x479b, 0x0e},
+};
+
+static const struct imx219_reg raw8_framefmt_regs[] = {
+	{0x018c, 0x08},
+	{0x018d, 0x08},
+	{0x0309, 0x08},
+};
+
+static const struct imx219_reg raw10_framefmt_regs[] = {
+	{0x018c, 0x0a},
+	{0x018d, 0x0a},
+	{0x0309, 0x0a},
+};
+
+static const s64 imx219_link_freq_menu[] = {
+	IMX219_DEFAULT_LINK_FREQ,
+};
+
+static const char * const imx219_test_pattern_menu[] = {
+	"Disabled",
+	"Color Bars",
+	"Solid Color",
+	"Grey Color Bars",
+	"PN9"
+};
+
+static const int imx219_test_pattern_val[] = {
+	IMX219_TEST_PATTERN_DISABLE,
+	IMX219_TEST_PATTERN_COLOR_BARS,
+	IMX219_TEST_PATTERN_SOLID_COLOR,
+	IMX219_TEST_PATTERN_GREY_COLOR,
+	IMX219_TEST_PATTERN_PN9,
+};
+
+/* regulator supplies */
+static const char * const imx219_supply_name[] = {
+	/* Supplies can be enabled in any order */
+	"VANA",  /* Analog (2.8V) supply */
+	"VDIG",  /* Digital Core (1.8V) supply */
+	"VDDL",  /* IF (1.2V) supply */
+};
+
+#define IMX219_NUM_SUPPLIES ARRAY_SIZE(imx219_supply_name)
+
+/*
+ * The supported formats.
+ * This table MUST contain 4 entries per format, to cover the various flip
+ * combinations in the order
+ * - no flip
+ * - h flip
+ * - v flip
+ * - h&v flips
+ */
+static const u32 codes[] = {
+	MEDIA_BUS_FMT_SRGGB10_1X10,
+	MEDIA_BUS_FMT_SGRBG10_1X10,
+	MEDIA_BUS_FMT_SGBRG10_1X10,
+	MEDIA_BUS_FMT_SBGGR10_1X10,
+
+	MEDIA_BUS_FMT_SRGGB8_1X8,
+	MEDIA_BUS_FMT_SGRBG8_1X8,
+	MEDIA_BUS_FMT_SGBRG8_1X8,
+	MEDIA_BUS_FMT_SBGGR8_1X8,
+};
+
+/*
+ * Initialisation delay between XCLR low->high and the moment when the sensor
+ * can start capture (i.e. can leave software stanby) must be not less than:
+ *   t4 + max(t5, t6 + <time to initialize the sensor register over I2C>)
+ * where
+ *   t4 is fixed, and is max 200uS,
+ *   t5 is fixed, and is 6000uS,
+ *   t6 depends on the sensor external clock, and is max 32000 clock periods.
+ * As per sensor datasheet, the external clock must be from 6MHz to 27MHz.
+ * So for any acceptable external clock t6 is always within the range of
+ * 1185 to 5333 uS, and is always less than t5.
+ * For this reason this is always safe to wait (t4 + t5) = 6200 uS, then
+ * initialize the sensor over I2C, and then exit the software standby.
+ *
+ * This start-up time can be optimized a bit more, if we start the writes
+ * over I2C after (t4+t6), but before (t4+t5) expires. But then sensor
+ * initialization over I2C may complete before (t4+t5) expires, and we must
+ * ensure that capture is not started before (t4+t5).
+ *
+ * This delay doesn't account for the power supply startup time. If needed,
+ * this should be taken care of via the regulator framework. E.g. in the
+ * case of DT for regulator-fixed one should define the startup-delay-us
+ * property.
+ */
+#define IMX219_XCLR_MIN_DELAY_US	6200
+#define IMX219_XCLR_DELAY_RANGE_US	1000
+
+/* Mode configs */
+static const struct imx219_mode supported_modes[] = {
+	{
+		/* 8MPix 15fps mode */
+		.width = 3280,
+		.height = 2464,
+		.crop = {
+			.left = IMX219_PIXEL_ARRAY_LEFT,
+			.top = IMX219_PIXEL_ARRAY_TOP,
+			.width = 3280,
+			.height = 2464
+		},
+		.vts_def = IMX219_VTS_15FPS,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_3280x2464_regs),
+			.regs = mode_3280x2464_regs,
+		},
+	},
+	{
+		/* 1080P 30fps cropped */
+		.width = 1920,
+		.height = 1080,
+		.crop = {
+			.left = 688,
+			.top = 700,
+			.width = 1920,
+			.height = 1080
+		},
+		.vts_def = IMX219_VTS_30FPS_1080P,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_1920_1080_regs),
+			.regs = mode_1920_1080_regs,
+		},
+	},
+	{
+		/* 2x2 binned 30fps mode */
+		.width = 1640,
+		.height = 1232,
+		.crop = {
+			.left = IMX219_PIXEL_ARRAY_LEFT,
+			.top = IMX219_PIXEL_ARRAY_TOP,
+			.width = 3280,
+			.height = 2464
+		},
+		.vts_def = IMX219_VTS_30FPS_BINNED,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_1640_1232_regs),
+			.regs = mode_1640_1232_regs,
+		},
+	},
+	{
+		/* 640x480 30fps mode */
+		.width = 640,
+		.height = 480,
+		.crop = {
+			.left = 1008,
+			.top = 760,
+			.width = 1280,
+			.height = 960
+		},
+		.vts_def = IMX219_VTS_30FPS_640x480,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_640_480_regs),
+			.regs = mode_640_480_regs,
+		},
+	},
+};
+#define MODE_COUNT_MAX	ARRAY_SIZE(supported_modes)
+static int imx219_fps[MODE_COUNT_MAX] = {15, 30, 30, 30};
+
+struct imx219 {
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+	//struct v4l2_fwnode_endpoint ep; /* the parsed DT endpoint info */
+
+	struct v4l2_mbus_framefmt fmt;
+
+	struct clk *xclk; /* system clock to IMX219 */
+	u32 xclk_freq;
+
+	struct gpio_desc *reset_gpio;
+	struct regulator_bulk_data supplies[IMX219_NUM_SUPPLIES];
+
+	struct v4l2_ctrl_handler ctrl_handler;
+	/* V4L2 Controls */
+	struct v4l2_ctrl *pixel_rate;
+	struct v4l2_ctrl *link_freq;
+	struct v4l2_ctrl *exposure;
+	struct v4l2_ctrl *vflip;
+	struct v4l2_ctrl *hflip;
+	struct v4l2_ctrl *vblank;
+	struct v4l2_ctrl *hblank;
+
+	/* Current mode */
+	const struct imx219_mode *mode;
+
+	/*
+	 * Mutex for serialized access:
+	 * Protect sensor module set pad format and start/stop streaming safely.
+	 */
+	struct mutex mutex;
+
+	/* Streaming on/off */
+	bool streaming;
+};
+
+static inline struct imx219 *to_imx219(struct v4l2_subdev *_sd)
+{
+	return container_of(_sd, struct imx219, sd);
+}
+
+/* Read registers up to 2 at a time */
+static int imx219_read_reg(struct imx219 *imx219, u16 reg, u32 len, u32 *val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
+	struct i2c_msg msgs[2];
+	u8 addr_buf[2] = { reg >> 8, reg & 0xff };
+	u8 data_buf[4] = { 0, };
+	int ret;
+
+	if (len > 4)
+		return -EINVAL;
+
+	/* Write register address */
+	msgs[0].addr = client->addr;
+	msgs[0].flags = 0;
+	msgs[0].len = ARRAY_SIZE(addr_buf);
+	msgs[0].buf = addr_buf;
+
+	/* Read data from register */
+	msgs[1].addr = client->addr;
+	msgs[1].flags = I2C_M_RD;
+	msgs[1].len = len;
+	msgs[1].buf = &data_buf[4 - len];
+
+	ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
+	if (ret != ARRAY_SIZE(msgs))
+		return -EIO;
+
+	*val = get_unaligned_be32(data_buf);
+
+	return 0;
+}
+
+/* Write registers up to 2 at a time */
+static int imx219_write_reg(struct imx219 *imx219, u16 reg, u32 len, u32 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
+	u8 buf[6];
+
+	if (len > 4)
+		return -EINVAL;
+
+	put_unaligned_be16(reg, buf);
+	put_unaligned_be32(val << (8 * (4 - len)), buf + 2);
+	if (i2c_master_send(client, buf, len + 2) != len + 2)
+		return -EIO;
+
+	return 0;
+}
+
+/* Write a list of registers */
+static int imx219_write_regs(struct imx219 *imx219,
+			     const struct imx219_reg *regs, u32 len)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
+	unsigned int i;
+	int ret;
+
+	for (i = 0; i < len; i++) {
+		ret = imx219_write_reg(imx219, regs[i].address, 1, regs[i].val);
+		if (ret) {
+			dev_err_ratelimited(&client->dev,
+					    "Failed to write reg 0x%4.4x. error = %d\n",
+					    regs[i].address, ret);
+
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+/* Get bayer order based on flip setting. */
+static u32 imx219_get_format_code(struct imx219 *imx219, u32 code)
+{
+	unsigned int i;
+
+	lockdep_assert_held(&imx219->mutex);
+
+	for (i = 0; i < ARRAY_SIZE(codes); i++)
+		if (codes[i] == code)
+			break;
+
+	if (i >= ARRAY_SIZE(codes))
+		i = 0;
+
+	i = (i & ~3) | (imx219->vflip->val ? 2 : 0) |
+	    (imx219->hflip->val ? 1 : 0);
+
+	return codes[i];
+}
+
+static void imx219_set_default_format(struct imx219 *imx219)
+{
+	struct v4l2_mbus_framefmt *fmt;
+
+	fmt = &imx219->fmt;
+	fmt->code = MEDIA_BUS_FMT_SRGGB10_1X10;
+	fmt->colorspace = V4L2_COLORSPACE_SRGB;
+	fmt->ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(fmt->colorspace);
+	fmt->quantization = V4L2_MAP_QUANTIZATION_DEFAULT(true,
+		fmt->colorspace,fmt->ycbcr_enc);
+	fmt->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(fmt->colorspace);
+	fmt->width = supported_modes[0].width;
+	fmt->height = supported_modes[0].height;
+	fmt->field = V4L2_FIELD_NONE;
+}
+
+static int imx219_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	struct imx219 *imx219 = to_imx219(sd);
+	struct v4l2_mbus_framefmt *try_fmt =
+		v4l2_subdev_get_try_format(sd, fh->state, 0);
+	struct v4l2_rect *try_crop;
+
+	mutex_lock(&imx219->mutex);
+
+	/* Initialize try_fmt */
+	try_fmt->width = supported_modes[0].width;
+	try_fmt->height = supported_modes[0].height;
+	try_fmt->code = imx219_get_format_code(imx219, MEDIA_BUS_FMT_SRGGB10_1X10);
+	try_fmt->field = V4L2_FIELD_NONE;
+
+	/* Initialize try_crop rectangle. */
+	try_crop = v4l2_subdev_get_try_crop(sd, fh->state, 0);
+	try_crop->top = IMX219_PIXEL_ARRAY_TOP;
+	try_crop->left = IMX219_PIXEL_ARRAY_LEFT;
+	try_crop->width = IMX219_PIXEL_ARRAY_WIDTH;
+	try_crop->height = IMX219_PIXEL_ARRAY_HEIGHT;
+
+	mutex_unlock(&imx219->mutex);
+
+	return 0;
+}
+
+static int imx219_set_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct imx219 *imx219 =
+		container_of(ctrl->handler, struct imx219, ctrl_handler);
+	struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
+	int ret;
+
+	if (ctrl->id == V4L2_CID_VBLANK) {
+		int exposure_max, exposure_def;
+
+		/* Update max exposure while meeting expected vblanking */
+		exposure_max = imx219->mode->height + ctrl->val - 4;
+		exposure_def = (exposure_max < IMX219_EXPOSURE_DEFAULT) ?
+			exposure_max : IMX219_EXPOSURE_DEFAULT;
+		__v4l2_ctrl_modify_range(imx219->exposure, imx219->exposure->minimum,
+				exposure_max, imx219->exposure->step, exposure_def);
+	}
+
+	/*
+	 * Applying V4L2 control value only happens
+	 * when power is up for streaming
+	 */
+	if (pm_runtime_get_if_in_use(&client->dev) == 0)
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_ANALOGUE_GAIN:
+		ret = imx219_write_reg(imx219, IMX219_REG_ANALOG_GAIN,
+				IMX219_REG_VALUE_08BIT, ctrl->val);
+		break;
+	case V4L2_CID_EXPOSURE:
+		ret = imx219_write_reg(imx219, IMX219_REG_EXPOSURE,
+				IMX219_REG_VALUE_16BIT, ctrl->val);
+		break;
+	case V4L2_CID_DIGITAL_GAIN:
+		ret = imx219_write_reg(imx219, IMX219_REG_DIGITAL_GAIN,
+				IMX219_REG_VALUE_16BIT, ctrl->val);
+		break;
+	case V4L2_CID_TEST_PATTERN:
+		ret = imx219_write_reg(imx219, IMX219_REG_TEST_PATTERN,
+				IMX219_REG_VALUE_16BIT,imx219_test_pattern_val[ctrl->val]);
+		break;
+	case V4L2_CID_HFLIP:
+	case V4L2_CID_VFLIP:
+		ret = imx219_write_reg(imx219, IMX219_REG_ORIENTATION, 1,
+			imx219->hflip->val | imx219->vflip->val << 1);
+		break;
+	case V4L2_CID_VBLANK:
+		ret = imx219_write_reg(imx219, IMX219_REG_VTS, IMX219_REG_VALUE_16BIT,
+				imx219->mode->height + ctrl->val);
+		break;
+	case V4L2_CID_TEST_PATTERN_RED:
+		ret = imx219_write_reg(imx219, IMX219_REG_TESTP_RED,
+				IMX219_REG_VALUE_16BIT, ctrl->val);
+		break;
+	case V4L2_CID_TEST_PATTERN_GREENR:
+		ret = imx219_write_reg(imx219, IMX219_REG_TESTP_GREENR,
+				IMX219_REG_VALUE_16BIT, ctrl->val);
+		break;
+	case V4L2_CID_TEST_PATTERN_BLUE:
+		ret = imx219_write_reg(imx219, IMX219_REG_TESTP_BLUE,
+				IMX219_REG_VALUE_16BIT, ctrl->val);
+		break;
+	case V4L2_CID_TEST_PATTERN_GREENB:
+		ret = imx219_write_reg(imx219, IMX219_REG_TESTP_GREENB,
+				IMX219_REG_VALUE_16BIT, ctrl->val);
+		break;
+	default:
+		dev_info(&client->dev,
+			 "ctrl(id:0x%x,val:0x%x) is not handled\n", ctrl->id, ctrl->val);
+		ret = -EINVAL;
+		break;
+	}
+
+	pm_runtime_put(&client->dev);
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops imx219_ctrl_ops = {
+	.s_ctrl = imx219_set_ctrl,
+};
+
+static int imx219_enum_mbus_code(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *state,
+				 struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct imx219 *imx219 = to_imx219(sd);
+
+	if (code->index >= (ARRAY_SIZE(codes) / 4))
+		return -EINVAL;
+
+	mutex_lock(&imx219->mutex);
+	code->code = imx219_get_format_code(imx219, codes[code->index * 4]);
+	mutex_unlock(&imx219->mutex);
+
+	return 0;
+}
+
+static int imx219_enum_frame_size(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *state,
+				  struct v4l2_subdev_frame_size_enum *fse)
+{
+	struct imx219 *imx219 = to_imx219(sd);
+	u32 code;
+
+	if (fse->index >= ARRAY_SIZE(supported_modes))
+		return -EINVAL;
+
+	mutex_lock(&imx219->mutex);
+	code = imx219_get_format_code(imx219, fse->code);
+	mutex_unlock(&imx219->mutex);
+	if (fse->code != code)
+		return -EINVAL;
+
+	fse->min_width = supported_modes[fse->index].width;
+	fse->max_width = fse->min_width;
+	fse->min_height = supported_modes[fse->index].height;
+	fse->max_height = fse->min_height;
+
+	return 0;
+}
+static int imx219_enum_frame_interval(struct v4l2_subdev *sd,
+			struct v4l2_subdev_state *state,
+			struct v4l2_subdev_frame_interval_enum *fie)
+{
+	struct imx219 *imx219 = to_imx219(sd);
+	u32 code;
+	int i = 0;
+
+	if (fie->index >= ARRAY_SIZE(supported_modes) || fie->index)
+		return -EINVAL;
+
+	mutex_lock(&imx219->mutex);
+	code = imx219_get_format_code(imx219, fie->code);
+	mutex_unlock(&imx219->mutex);
+	if (fie->code != code)
+		return -EINVAL;
+
+	pr_debug("fie->width = %d, fie->height = %d \n", fie->width, fie->height);
+	for (i = 0; i < MODE_COUNT_MAX; i++)
+	{
+		if (fie->width == supported_modes[i].width && fie->height == supported_modes[i].height)
+			break;
+	}
+	if (i == MODE_COUNT_MAX)
+		return -ENOTTY;
+
+	fie->interval.denominator = imx219_fps[i];
+	fie->interval.numerator = 1;
+	fie->code = code;
+	fie->width = supported_modes[i].width;
+	fie->height = supported_modes[i].height;
+
+	return 0;
+}
+
+static void imx219_reset_colorspace(struct v4l2_mbus_framefmt *fmt)
+{
+	fmt->colorspace = V4L2_COLORSPACE_SRGB;
+	fmt->ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(fmt->colorspace);
+	fmt->quantization = V4L2_MAP_QUANTIZATION_DEFAULT(true,
+			fmt->colorspace, fmt->ycbcr_enc);
+	fmt->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(fmt->colorspace);
+}
+
+static void imx219_update_pad_format(struct imx219 *imx219,
+				     const struct imx219_mode *mode,
+				     struct v4l2_subdev_format *fmt)
+{
+	fmt->format.width = mode->width;
+	fmt->format.height = mode->height;
+	fmt->format.field = V4L2_FIELD_NONE;
+	imx219_reset_colorspace(&fmt->format);
+}
+
+static int __imx219_get_pad_format(struct imx219 *imx219,
+				   struct v4l2_subdev_state *state,
+				   struct v4l2_subdev_format *fmt)
+{
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		struct v4l2_mbus_framefmt *try_fmt =
+			v4l2_subdev_get_try_format(&imx219->sd, state, fmt->pad);
+		/* update the code which could change due to vflip or hflip: */
+		try_fmt->code = imx219_get_format_code(imx219, try_fmt->code);
+		fmt->format = *try_fmt;
+	} else {
+		imx219_update_pad_format(imx219, imx219->mode, fmt);
+		fmt->format.code = imx219_get_format_code(imx219,imx219->fmt.code);
+	}
+
+	return 0;
+}
+
+static int imx219_get_pad_format(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *state,
+				 struct v4l2_subdev_format *fmt)
+{
+	struct imx219 *imx219 = to_imx219(sd);
+	int ret;
+
+	mutex_lock(&imx219->mutex);
+	ret = __imx219_get_pad_format(imx219, state, fmt);
+	mutex_unlock(&imx219->mutex);
+
+	return ret;
+}
+
+static int imx219_set_pad_format(struct v4l2_subdev *sd,
+				struct v4l2_subdev_state *state,
+				struct v4l2_subdev_format *fmt)
+{
+	struct imx219 *imx219 = to_imx219(sd);
+	const struct imx219_mode *mode;
+	struct v4l2_mbus_framefmt *framefmt;
+	int exposure_max, exposure_def, hblank;
+	unsigned int i;
+
+	mutex_lock(&imx219->mutex);
+
+	for (i = 0; i < ARRAY_SIZE(codes); i++)
+		if (codes[i] == fmt->format.code)
+			break;
+	if (i >= ARRAY_SIZE(codes))
+		i = 0;
+
+	/* Bayer order varies with flips */
+	fmt->format.code = imx219_get_format_code(imx219, codes[i]);
+
+	mode = v4l2_find_nearest_size(supported_modes, ARRAY_SIZE(supported_modes),
+			width, height, fmt->format.width, fmt->format.height);
+	imx219_update_pad_format(imx219, mode, fmt);
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		framefmt = v4l2_subdev_get_try_format(sd, state, fmt->pad);
+		*framefmt = fmt->format;
+	} else if (imx219->mode != mode ||
+		   imx219->fmt.code != fmt->format.code) {
+		imx219->fmt = fmt->format;
+		imx219->mode = mode;
+		/* Update limits and set FPS to default */
+		__v4l2_ctrl_modify_range(imx219->vblank, IMX219_VBLANK_MIN,
+				IMX219_VTS_MAX - mode->height, 1,
+				mode->vts_def - mode->height);
+		__v4l2_ctrl_s_ctrl(imx219->vblank,mode->vts_def - mode->height);
+		/* Update max exposure while meeting expected vblanking */
+		exposure_max = mode->vts_def - 4;
+		exposure_def = (exposure_max < IMX219_EXPOSURE_DEFAULT) ?
+			exposure_max : IMX219_EXPOSURE_DEFAULT;
+		__v4l2_ctrl_modify_range(imx219->exposure, imx219->exposure->minimum,
+				exposure_max, imx219->exposure->step, exposure_def);
+		/*
+		 * Currently PPL is fixed to IMX219_PPL_DEFAULT, so hblank
+		 * depends on mode->width only, and is not changeble in any
+		 * way other than changing the mode.
+		 */
+		hblank = IMX219_PPL_DEFAULT - mode->width;
+		__v4l2_ctrl_modify_range(imx219->hblank, hblank, hblank, 1, hblank);
+	}
+
+	mutex_unlock(&imx219->mutex);
+
+	return 0;
+}
+
+static int imx219_set_framefmt(struct imx219 *imx219)
+{
+	switch (imx219->fmt.code) {
+	case MEDIA_BUS_FMT_SRGGB8_1X8:
+	case MEDIA_BUS_FMT_SGRBG8_1X8:
+	case MEDIA_BUS_FMT_SGBRG8_1X8:
+	case MEDIA_BUS_FMT_SBGGR8_1X8:
+		return imx219_write_regs(imx219, raw8_framefmt_regs,
+				ARRAY_SIZE(raw8_framefmt_regs));
+
+	case MEDIA_BUS_FMT_SRGGB10_1X10:
+	case MEDIA_BUS_FMT_SGRBG10_1X10:
+	case MEDIA_BUS_FMT_SGBRG10_1X10:
+	case MEDIA_BUS_FMT_SBGGR10_1X10:
+		return imx219_write_regs(imx219, raw10_framefmt_regs,
+				ARRAY_SIZE(raw10_framefmt_regs));
+	}
+
+	return -EINVAL;
+}
+
+static const struct v4l2_rect *
+__imx219_get_pad_crop(struct imx219 *imx219, struct v4l2_subdev_state *state,
+				unsigned int pad, enum v4l2_subdev_format_whence which)
+{
+	switch (which) {
+	case V4L2_SUBDEV_FORMAT_TRY:
+		return v4l2_subdev_get_try_crop(&imx219->sd, state, pad);
+	case V4L2_SUBDEV_FORMAT_ACTIVE:
+		return &imx219->mode->crop;
+	}
+
+	return NULL;
+}
+
+static int imx219_get_selection(struct v4l2_subdev *sd,
+					struct v4l2_subdev_state *state,
+					struct v4l2_subdev_selection *sel)
+{
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP: {
+		struct imx219 *imx219 = to_imx219(sd);
+
+		mutex_lock(&imx219->mutex);
+		sel->r = *__imx219_get_pad_crop(imx219, state, sel->pad, sel->which);
+		mutex_unlock(&imx219->mutex);
+		return 0;
+	}
+
+	case V4L2_SEL_TGT_NATIVE_SIZE:
+		sel->r.top = 0;
+		sel->r.left = 0;
+		sel->r.width = IMX219_NATIVE_WIDTH;
+		sel->r.height = IMX219_NATIVE_HEIGHT;
+		return 0;
+
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		sel->r.top = IMX219_PIXEL_ARRAY_TOP;
+		sel->r.left = IMX219_PIXEL_ARRAY_LEFT;
+		sel->r.width = IMX219_PIXEL_ARRAY_WIDTH;
+		sel->r.height = IMX219_PIXEL_ARRAY_HEIGHT;
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+static int imx219_start_streaming(struct imx219 *imx219)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
+	const struct imx219_reg_list *reg_list;
+	int ret;
+
+	ret = pm_runtime_get_sync(&client->dev);
+	if (ret < 0) {
+		pm_runtime_put_noidle(&client->dev);
+		return ret;
+	}
+
+	/* Apply default values of current mode */
+	reg_list = &imx219->mode->reg_list;
+	ret = imx219_write_regs(imx219, reg_list->regs, reg_list->num_of_regs);
+	if (ret) {
+		dev_err(&client->dev, "%s failed to set mode\n", __func__);
+		goto err_rpm_put;
+	}
+
+	ret = imx219_set_framefmt(imx219);
+	if (ret) {
+		dev_err(&client->dev, "%s failed to set frame format: %d\n",
+			__func__, ret);
+		goto err_rpm_put;
+	}
+
+	/* Apply customized values from user */
+	ret =  __v4l2_ctrl_handler_setup(imx219->sd.ctrl_handler);
+	if (ret)
+		goto err_rpm_put;
+
+	/* set stream on register */
+	ret = imx219_write_reg(imx219, IMX219_REG_MODE_SELECT,
+			       IMX219_REG_VALUE_08BIT, IMX219_MODE_STREAMING);
+	if (ret)
+		goto err_rpm_put;
+
+	/* vflip and hflip cannot change during streaming */
+	__v4l2_ctrl_grab(imx219->vflip, true);
+	__v4l2_ctrl_grab(imx219->hflip, true);
+
+	return 0;
+
+err_rpm_put:
+	pm_runtime_put(&client->dev);
+	return ret;
+}
+
+static void imx219_stop_streaming(struct imx219 *imx219)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
+	int ret;
+
+	/* set stream off register */
+	ret = imx219_write_reg(imx219, IMX219_REG_MODE_SELECT,
+			IMX219_REG_VALUE_08BIT, IMX219_MODE_STANDBY);
+	if (ret)
+		dev_err(&client->dev, "%s failed to set stream\n", __func__);
+
+	__v4l2_ctrl_grab(imx219->vflip, false);
+	__v4l2_ctrl_grab(imx219->hflip, false);
+
+	pm_runtime_put(&client->dev);
+}
+
+static int imx219_set_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct imx219 *imx219 = to_imx219(sd);
+	int ret = 0;
+
+	mutex_lock(&imx219->mutex);
+	if (imx219->streaming == enable) {
+		mutex_unlock(&imx219->mutex);
+		return 0;
+	}
+
+	if (enable) {
+		/*
+		 * Apply default & customized values
+		 * and then start streaming.
+		 */
+		ret = imx219_start_streaming(imx219);
+		if (ret)
+			goto err_unlock;
+	} else {
+		imx219_stop_streaming(imx219);
+	}
+
+	imx219->streaming = enable;
+
+	mutex_unlock(&imx219->mutex);
+
+	return ret;
+
+err_unlock:
+	mutex_unlock(&imx219->mutex);
+
+	return ret;
+}
+
+/* Power/clock management functions */
+static int imx219_power_on(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct imx219 *imx219 = to_imx219(sd);
+	int ret;
+
+	ret = regulator_bulk_enable(IMX219_NUM_SUPPLIES, imx219->supplies);
+	if (ret) {
+		dev_err(dev, "%s: failed to enable regulators\n",
+			__func__);
+		return ret;
+	}
+
+	ret = clk_prepare_enable(imx219->xclk);
+	if (ret) {
+		dev_err(dev, "%s: failed to enable clock\n", __func__);
+		goto reg_off;
+	}
+
+	gpiod_set_value_cansleep(imx219->reset_gpio, 1);
+	usleep_range(IMX219_XCLR_MIN_DELAY_US,
+			IMX219_XCLR_MIN_DELAY_US + IMX219_XCLR_DELAY_RANGE_US);
+
+	return 0;
+
+reg_off:
+	regulator_bulk_disable(IMX219_NUM_SUPPLIES, imx219->supplies);
+
+	return ret;
+}
+
+static int imx219_power_off(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct imx219 *imx219 = to_imx219(sd);
+
+	gpiod_set_value_cansleep(imx219->reset_gpio, 0);
+	regulator_bulk_disable(IMX219_NUM_SUPPLIES, imx219->supplies);
+	clk_disable_unprepare(imx219->xclk);
+
+	return 0;
+}
+
+static int __maybe_unused imx219_suspend(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct imx219 *imx219 = to_imx219(sd);
+
+	if (imx219->streaming)
+		imx219_stop_streaming(imx219);
+
+	return 0;
+}
+
+static int __maybe_unused imx219_resume(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct imx219 *imx219 = to_imx219(sd);
+	int ret;
+
+	if (imx219->streaming) {
+		ret = imx219_start_streaming(imx219);
+		if (ret)
+			goto error;
+	}
+
+	return 0;
+
+error:
+	imx219_stop_streaming(imx219);
+	imx219->streaming = false;
+
+	return ret;
+}
+
+static int imx219_get_regulators(struct imx219 *imx219)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
+	unsigned int i;
+
+	for (i = 0; i < IMX219_NUM_SUPPLIES; i++)
+		imx219->supplies[i].supply = imx219_supply_name[i];
+
+	return devm_regulator_bulk_get(&client->dev,
+			IMX219_NUM_SUPPLIES, imx219->supplies);
+}
+
+/* Verify chip ID */
+static int imx219_identify_module(struct imx219 *imx219)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
+	int ret;
+	u32 val;
+
+	ret = imx219_read_reg(imx219, IMX219_REG_CHIP_ID,
+			      IMX219_REG_VALUE_16BIT, &val);
+	if (ret) {
+		dev_err(&client->dev, "failed to read chip id %x\n",
+			IMX219_CHIP_ID);
+		return ret;
+	}
+
+	if (val != IMX219_CHIP_ID) {
+		dev_err(&client->dev, "chip id mismatch: %x!=%x\n",
+			IMX219_CHIP_ID, val);
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static const struct v4l2_subdev_core_ops imx219_core_ops = {
+	.subscribe_event = v4l2_ctrl_subdev_subscribe_event,
+	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
+};
+
+static const struct v4l2_subdev_video_ops imx219_video_ops = {
+	.s_stream = imx219_set_stream,
+};
+
+static const struct v4l2_subdev_pad_ops imx219_pad_ops = {
+	.enum_mbus_code = imx219_enum_mbus_code,
+	.get_fmt = imx219_get_pad_format,
+	.set_fmt = imx219_set_pad_format,
+	.get_selection = imx219_get_selection,
+	.enum_frame_size = imx219_enum_frame_size,
+	.enum_frame_interval = imx219_enum_frame_interval,
+};
+
+static const struct v4l2_subdev_ops imx219_subdev_ops = {
+	.core = &imx219_core_ops,
+	.video = &imx219_video_ops,
+	.pad = &imx219_pad_ops,
+};
+
+static const struct v4l2_subdev_internal_ops imx219_internal_ops = {
+	.open = imx219_open,
+};
+
+/* Initialize control handlers */
+static int imx219_init_controls(struct imx219 *imx219)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
+	struct v4l2_ctrl_handler *ctrl_hdlr;
+	unsigned int height = imx219->mode->height;
+	struct v4l2_fwnode_device_properties props;
+	int exposure_max, exposure_def, hblank;
+	int i, ret;
+
+	ctrl_hdlr = &imx219->ctrl_handler;
+	ret = v4l2_ctrl_handler_init(ctrl_hdlr, 12);
+	if (ret)
+		return ret;
+
+	mutex_init(&imx219->mutex);
+	ctrl_hdlr->lock = &imx219->mutex;
+
+	/* By default, PIXEL_RATE is read only */
+	imx219->pixel_rate = v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops,
+			V4L2_CID_PIXEL_RATE, IMX219_PIXEL_RATE,
+			IMX219_PIXEL_RATE, 1, IMX219_PIXEL_RATE);
+
+	imx219->link_freq =
+		v4l2_ctrl_new_int_menu(ctrl_hdlr, &imx219_ctrl_ops, V4L2_CID_LINK_FREQ,
+			ARRAY_SIZE(imx219_link_freq_menu) - 1, 0, imx219_link_freq_menu);
+	if (imx219->link_freq)
+		imx219->link_freq->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	/* Initial vblank/hblank/exposure parameters based on current mode */
+	imx219->vblank = v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops,
+					V4L2_CID_VBLANK,IMX219_VBLANK_MIN,
+					IMX219_VTS_MAX - height, 1,
+					imx219->mode->vts_def - height);
+	hblank = IMX219_PPL_DEFAULT - imx219->mode->width;
+	imx219->hblank = v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops,
+					V4L2_CID_HBLANK,hblank, hblank, 1, hblank);
+	if (imx219->hblank)
+		imx219->hblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+	exposure_max = imx219->mode->vts_def - 4;
+	exposure_def = (exposure_max < IMX219_EXPOSURE_DEFAULT) ?
+		exposure_max : IMX219_EXPOSURE_DEFAULT;
+	imx219->exposure = v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops,
+						V4L2_CID_EXPOSURE, IMX219_EXPOSURE_MIN, exposure_max,
+						IMX219_EXPOSURE_STEP, exposure_def);
+
+	v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops, V4L2_CID_ANALOGUE_GAIN,
+			IMX219_ANA_GAIN_MIN, IMX219_ANA_GAIN_MAX,
+			IMX219_ANA_GAIN_STEP, IMX219_ANA_GAIN_DEFAULT);
+
+	v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops, V4L2_CID_DIGITAL_GAIN,
+			IMX219_DGTL_GAIN_MIN, IMX219_DGTL_GAIN_MAX,
+			IMX219_DGTL_GAIN_STEP, IMX219_DGTL_GAIN_DEFAULT);
+
+	imx219->hflip = v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops,
+					  V4L2_CID_HFLIP, 0, 1, 1, 0);
+	if (imx219->hflip)
+		imx219->hflip->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
+
+	imx219->vflip = v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops,
+					  V4L2_CID_VFLIP, 0, 1, 1, 0);
+	if (imx219->vflip)
+		imx219->vflip->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
+
+	v4l2_ctrl_new_std_menu_items(ctrl_hdlr, &imx219_ctrl_ops, V4L2_CID_TEST_PATTERN,
+			ARRAY_SIZE(imx219_test_pattern_menu) - 1,
+			0, 0, imx219_test_pattern_menu);
+	for (i = 0; i < 4; i++) {
+		/*
+		 * The assumption is that
+		 * V4L2_CID_TEST_PATTERN_GREENR == V4L2_CID_TEST_PATTERN_RED + 1
+		 * V4L2_CID_TEST_PATTERN_BLUE   == V4L2_CID_TEST_PATTERN_RED + 2
+		 * V4L2_CID_TEST_PATTERN_GREENB == V4L2_CID_TEST_PATTERN_RED + 3
+		 */
+		v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops,
+				V4L2_CID_TEST_PATTERN_RED + i,
+				IMX219_TESTP_COLOUR_MIN,
+				IMX219_TESTP_COLOUR_MAX,
+				IMX219_TESTP_COLOUR_STEP,
+				IMX219_TESTP_COLOUR_MAX);
+		/* The "Solid color" pattern is white by default */
+	}
+
+	if (ctrl_hdlr->error) {
+		ret = ctrl_hdlr->error;
+		dev_err(&client->dev, "%s control init failed (%d)\n",
+			__func__, ret);
+		goto error;
+	}
+
+	ret = v4l2_fwnode_device_parse(&client->dev, &props);
+	if (ret)
+		goto error;
+
+	ret = v4l2_ctrl_new_fwnode_properties(ctrl_hdlr, &imx219_ctrl_ops, &props);
+	if (ret)
+		goto error;
+
+	imx219->sd.ctrl_handler = ctrl_hdlr;
+
+	return 0;
+
+error:
+	v4l2_ctrl_handler_free(ctrl_hdlr);
+	mutex_destroy(&imx219->mutex);
+
+	return ret;
+}
+
+static void imx219_free_controls(struct imx219 *imx219)
+{
+	v4l2_ctrl_handler_free(imx219->sd.ctrl_handler);
+	mutex_destroy(&imx219->mutex);
+}
+
+static int imx219_check_hwcfg(struct device *dev)
+{
+	struct fwnode_handle *endpoint;
+	struct v4l2_fwnode_endpoint ep_cfg = {
+		.bus_type = V4L2_MBUS_CSI2_DPHY
+	};
+	int ret = -EINVAL;
+
+	endpoint = fwnode_graph_get_next_endpoint(dev_fwnode(dev), NULL);
+	if (!endpoint) {
+		dev_err(dev, "endpoint node not found\n");
+		return -EINVAL;
+	}
+
+	if (v4l2_fwnode_endpoint_alloc_parse(endpoint, &ep_cfg)) {
+		dev_err(dev, "could not parse endpoint\n");
+		goto error_out;
+	}
+
+	/* Check the number of MIPI CSI2 data lanes */
+	if (ep_cfg.bus.mipi_csi2.num_data_lanes != 2) {
+		dev_err(dev, "only 2 data lanes are currently supported\n");
+		goto error_out;
+	}
+
+	/* Check the link frequency set in device tree */
+	if (!ep_cfg.nr_of_link_frequencies) {
+		dev_err(dev, "link-frequency property not found in DT\n");
+		goto error_out;
+	}
+
+	if (ep_cfg.nr_of_link_frequencies != 1 ||
+	    ep_cfg.link_frequencies[0] != IMX219_DEFAULT_LINK_FREQ) {
+		dev_err(dev, "Link frequency not supported: %lld\n",
+			ep_cfg.link_frequencies[0]);
+		goto error_out;
+	}
+
+	ret = 0;
+
+error_out:
+	v4l2_fwnode_endpoint_free(&ep_cfg);
+	fwnode_handle_put(endpoint);
+
+	return ret;
+}
+
+static int imx219_probe(struct i2c_client *client)
+{
+	struct device *dev = &client->dev;
+	struct imx219 *imx219;
+	int ret;
+
+	imx219 = devm_kzalloc(&client->dev, sizeof(*imx219), GFP_KERNEL);
+	if (!imx219)
+		return -ENOMEM;
+
+	v4l2_i2c_subdev_init(&imx219->sd, client, &imx219_subdev_ops);
+
+	/* Check the hardware configuration in device tree */
+	if (imx219_check_hwcfg(dev))
+		return -EINVAL;
+
+	/* Get system clock (xclk) */
+	imx219->xclk = devm_clk_get(dev, NULL);
+	if (IS_ERR(imx219->xclk)) {
+		dev_err(dev, "failed to get xclk\n");
+		return PTR_ERR(imx219->xclk);
+	}
+
+	imx219->xclk_freq = clk_get_rate(imx219->xclk);
+	if (imx219->xclk_freq != IMX219_XCLK_FREQ) {
+		dev_err(dev, "xclk frequency not supported: %d Hz\n",
+			imx219->xclk_freq);
+		return -EINVAL;
+	}
+
+	ret = imx219_get_regulators(imx219);
+	if (ret) {
+		dev_err(dev, "failed to get regulators\n");
+		return ret;
+	}
+
+	/* Request optional enable pin */
+	imx219->reset_gpio = devm_gpiod_get_optional(dev, "reset", GPIOD_OUT_HIGH);
+
+	/*
+	 * The sensor must be powered for imx219_identify_module()
+	 * to be able to read the CHIP_ID register
+	 */
+	ret = imx219_power_on(dev);
+	if (ret)
+		return ret;
+
+	ret = imx219_identify_module(imx219);
+	if (ret)
+		goto error_power_off;
+
+	/* Set default mode to max resolution */
+	imx219->mode = &supported_modes[0];
+
+	/* sensor doesn't enter LP-11 state upon power up until and unless
+	 * streaming is started, so upon power up switch the modes to:
+	 * streaming -> standby
+	 */
+	ret = imx219_write_reg(imx219, IMX219_REG_MODE_SELECT,
+			IMX219_REG_VALUE_08BIT, IMX219_MODE_STREAMING);
+	if (ret < 0)
+		goto error_power_off;
+	usleep_range(100, 110);
+
+	/* put sensor back to standby mode */
+	ret = imx219_write_reg(imx219, IMX219_REG_MODE_SELECT,
+			IMX219_REG_VALUE_08BIT, IMX219_MODE_STANDBY);
+	if (ret < 0)
+		goto error_power_off;
+	usleep_range(100, 110);
+
+	ret = imx219_init_controls(imx219);
+	if (ret)
+		goto error_power_off;
+
+	/* Initialize subdev */
+	imx219->sd.internal_ops = &imx219_internal_ops;
+	imx219->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE | V4L2_SUBDEV_FL_HAS_EVENTS;
+	imx219->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+
+	/* Initialize source pad */
+	imx219->pad.flags = MEDIA_PAD_FL_SOURCE;
+
+	/* Initialize default format */
+	imx219_set_default_format(imx219);
+
+	ret = media_entity_pads_init(&imx219->sd.entity, 1, &imx219->pad);
+	if (ret) {
+		dev_err(dev, "failed to init entity pads: %d\n", ret);
+		goto error_handler_free;
+	}
+
+	ret = v4l2_async_register_subdev_sensor(&imx219->sd);
+	if (ret < 0) {
+		dev_err(dev, "failed to register sensor sub-device: %d\n", ret);
+		goto error_media_entity;
+	}
+
+	/* Enable runtime PM and turn off the device */
+	pm_runtime_set_active(dev);
+	pm_runtime_enable(dev);
+	pm_runtime_idle(dev);
+
+	return 0;
+
+error_media_entity:
+	media_entity_cleanup(&imx219->sd.entity);
+
+error_handler_free:
+	imx219_free_controls(imx219);
+
+error_power_off:
+	imx219_power_off(dev);
+
+	return ret;
+}
+
+static int imx219_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct imx219 *imx219 = to_imx219(sd);
+
+	v4l2_async_unregister_subdev(sd);
+	media_entity_cleanup(&sd->entity);
+	imx219_free_controls(imx219);
+
+	pm_runtime_disable(&client->dev);
+	if (!pm_runtime_status_suspended(&client->dev))
+		imx219_power_off(&client->dev);
+	pm_runtime_set_suspended(&client->dev);
+
+	return 0;
+}
+
+static const struct of_device_id imx219_dt_ids[] = {
+	{ .compatible = "imx219" },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, imx219_dt_ids);
+
+static const struct dev_pm_ops imx219_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(imx219_suspend, imx219_resume)
+	SET_RUNTIME_PM_OPS(imx219_power_off, imx219_power_on, NULL)
+};
+
+static struct i2c_driver imx219_i2c_driver = {
+	.driver = {
+		.name = "imx219",
+		.of_match_table	= imx219_dt_ids,
+		.pm = &imx219_pm_ops,
+	},
+	.probe_new = imx219_probe,
+	.remove = imx219_remove,
+};
+
+module_i2c_driver(imx219_i2c_driver);
+
+MODULE_AUTHOR("David.li");
+MODULE_DESCRIPTION("Sony IMX219 sensor driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/platform/starfive/ov4689_mipi.c b/drivers/media/platform/starfive/ov4689_mipi.c
new file mode 100755
index 000000000000..64d850dbd136
--- /dev/null
+++ b/drivers/media/platform/starfive/ov4689_mipi.c
@@ -0,0 +1,2735 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Copyright (C) 2011-2013 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright (C) 2014-2017 Mentor Graphics Inc.
+ * Copyright (C) 2021 StarFive Technology Co., Ltd.
+ */
+
+#include <linux/clk.h>
+#include <linux/clk-provider.h>
+#include <linux/clkdev.h>
+#include <linux/ctype.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/gpio/consumer.h>
+#include <linux/i2c.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/of_device.h>
+#include <linux/regulator/consumer.h>
+#include <linux/slab.h>
+#include <linux/types.h>
+#include <media/v4l2-async.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-subdev.h>
+#include "stfcamss.h"
+
+#define OV4689_LANES    2
+
+#define OV4689_LINK_FREQ_500MHZ         500000000LL
+
+/* min/typical/max system clock (xclk) frequencies */
+#define OV4689_XCLK_MIN  6000000
+#define OV4689_XCLK_MAX 54000000
+
+#define OV4689_CHIP_ID	(0x4688)
+
+#define OV4689_CHIP_ID_HIGH_BYTE        0x300a   // max should be 0x46
+#define OV4689_CHIP_ID_LOW_BYTE         0x300b   // max should be 0x88
+#define OV4689_REG_CHIP_ID              0x300a
+
+#define OV4689_REG_H_OUTPUT_SIZE	0x3808
+#define OV4689_REG_V_OUTPUT_SIZE	0x380a
+#define OV4689_REG_TIMING_HTS		0x380c
+#define OV4689_REG_TIMING_VTS		0x380e
+
+#define OV4689_REG_EXPOSURE_HI          0x3500
+#define OV4689_REG_EXPOSURE_MED         0x3501
+#define OV4689_REG_EXPOSURE_LO          0x3502
+#define OV4689_REG_GAIN_H               0x3507
+#define OV4689_REG_GAIN_M               0x3508
+#define OV4689_REG_GAIN_L               0x3509
+#define OV4689_REG_TEST_PATTERN         0x5040
+#define OV4689_REG_TIMING_TC_REG20      0x3820
+#define OV4689_REG_TIMING_TC_REG21      0x3821
+
+#define OV4689_REG_AWB_R_GAIN           0x500C
+#define OV4689_REG_AWB_B_GAIN           0x5010
+
+enum ov4689_mode_id {
+	OV4689_MODE_720P_1280_720 = 0,
+	OV4689_MODE_1080P_1920_1080,
+	OV4689_MODE_4M_2688_1520,
+	OV4689_NUM_MODES,
+};
+
+enum ov4689_frame_rate {
+	OV4689_15_FPS = 0,
+	OV4689_30_FPS,
+	OV4689_45_FPS,
+	OV4689_60_FPS,
+	OV4689_90_FPS,
+	OV4689_120_FPS,
+	OV4689_150_FPS,
+	OV4689_180_FPS,
+	OV4689_330_FPS,
+	OV4689_NUM_FRAMERATES,
+};
+
+enum ov4689_format_mux {
+	OV4689_FMT_MUX_RAW,
+};
+
+static const int ov4689_framerates[] = {
+	[OV4689_15_FPS] = 15,
+	[OV4689_30_FPS] = 30,
+	[OV4689_45_FPS] = 45,
+	[OV4689_60_FPS] = 60,
+	[OV4689_90_FPS] = 90,
+	[OV4689_120_FPS] = 120,
+	[OV4689_150_FPS] = 150,
+	[OV4689_180_FPS] = 180,
+	[OV4689_330_FPS] = 330,
+};
+
+/* regulator supplies */
+static const char * const ov4689_supply_name[] = {
+	"DOVDD", /* Digital I/O (1.8V) supply */
+	"AVDD",  /* Analog (2.8V) supply */
+	"DVDD",  /* Digital Core (1.5V) supply */
+};
+
+#define OV4689_NUM_SUPPLIES ARRAY_SIZE(ov4689_supply_name)
+
+/*
+ * Image size under 1280 * 960 are SUBSAMPLING
+ * Image size upper 1280 * 960 are SCALING
+ */
+enum ov4689_downsize_mode {
+	SUBSAMPLING,
+	SCALING,
+};
+
+struct reg_value {
+	u16 reg_addr;
+	u8 val;
+	u8 mask;
+	u32 delay_ms;
+};
+
+struct ov4689_mode_info {
+	enum ov4689_mode_id id;
+	enum ov4689_downsize_mode dn_mode;
+	u32 hact;
+	u32 htot;
+	u32 vact;
+	u32 vtot;
+	const struct reg_value *reg_data;
+	u32 reg_data_size;
+	u32 max_fps;
+};
+
+struct ov4689_ctrls {
+	struct v4l2_ctrl_handler handler;
+	struct v4l2_ctrl *pixel_rate;
+	struct {
+		struct v4l2_ctrl *exposure;
+	};
+	struct {
+		struct v4l2_ctrl *auto_wb;
+		struct v4l2_ctrl *blue_balance;
+		struct v4l2_ctrl *red_balance;
+	};
+	struct {
+		struct v4l2_ctrl *anal_gain;
+	};
+	struct v4l2_ctrl *brightness;
+	struct v4l2_ctrl *light_freq;
+	struct v4l2_ctrl *link_freq;
+	struct v4l2_ctrl *saturation;
+	struct v4l2_ctrl *contrast;
+	struct v4l2_ctrl *hue;
+	struct v4l2_ctrl *test_pattern;
+	struct v4l2_ctrl *hflip;
+	struct v4l2_ctrl *vflip;
+};
+
+struct ov4689_dev {
+	struct i2c_client *i2c_client;
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+	struct v4l2_fwnode_endpoint ep; /* the parsed DT endpoint info */
+	struct clk *xclk; /* system clock to OV4689 */
+	u32 xclk_freq;
+
+	struct regulator_bulk_data supplies[OV4689_NUM_SUPPLIES];
+	struct gpio_desc *reset_gpio;
+	struct gpio_desc *pwdn_gpio;
+	bool   upside_down;
+
+	/* lock to protect all members below */
+	struct mutex lock;
+
+	int power_count;
+
+	struct v4l2_mbus_framefmt fmt;
+
+	const struct ov4689_mode_info *current_mode;
+	const struct ov4689_mode_info *last_mode;
+	enum ov4689_frame_rate current_fr;
+	struct v4l2_fract frame_interval;
+
+	struct ov4689_ctrls ctrls;
+
+	u32 prev_sysclk, prev_hts;
+	u32 ae_low, ae_high, ae_target;
+
+	bool pending_mode_change;
+	bool streaming;
+};
+
+static inline struct ov4689_dev *to_ov4689_dev(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct ov4689_dev, sd);
+}
+
+static inline struct v4l2_subdev *ctrl_to_sd(struct v4l2_ctrl *ctrl)
+{
+	return &container_of(ctrl->handler, struct ov4689_dev,
+			ctrls.handler)->sd;
+}
+
+/* ov4689 initial register */
+static const struct reg_value ov4689_init_setting_30fps_1080P[] = {
+
+};
+
+static const struct reg_value ov4689_setting_VGA_640_480[] = {
+	//@@ RES_640x480_2x_Bin_330fps_816Mbps
+	//OV4689_AM01B_640x480_24M_2lane_816Mbps_330fps_20140210.txt
+	{0x0103, 0x01, 0, 0},
+	{0x3638, 0x00, 0, 0},
+	{0x0300, 0x00, 0, 0}, // 00
+	{0x0302, 0x22, 0, 0}, // 816Mbps 5a ; 64 ; 5a ; 78  ; 78 ; 2a
+	{0x0303, 0x00, 0, 0}, // 03 ; 01 ; 02 ;
+	{0x0304, 0x03, 0, 0},
+	{0x030b, 0x00, 0, 0},
+	{0x030d, 0x1e, 0, 0},
+	{0x030e, 0x04, 0, 0},
+	{0x030f, 0x01, 0, 0},
+	{0x0312, 0x01, 0, 0},
+	{0x031e, 0x00, 0, 0},
+	{0x3000, 0x20, 0, 0},
+	{0x3002, 0x00, 0, 0},
+	{0x3018, 0x32, 0, 0}, // 32/72 2lane/4lane
+	{0x3019, 0x0c, 0, 0}, // 0c/00 2lane/4lane
+	{0x3020, 0x93, 0, 0},
+	{0x3021, 0x03, 0, 0},
+	{0x3022, 0x01, 0, 0},
+	{0x3031, 0x0a, 0, 0},
+	{0x303f, 0x0c, 0, 0},
+	{0x3305, 0xf1, 0, 0},
+	{0x3307, 0x04, 0, 0},
+	{0x3309, 0x29, 0, 0},
+	{0x3500, 0x00, 0, 0},
+	{0x3501, 0x4c, 0, 0},
+	{0x3502, 0x00, 0, 0},
+	{0x3503, 0x04, 0, 0},
+	{0x3504, 0x00, 0, 0},
+	{0x3505, 0x00, 0, 0},
+	{0x3506, 0x00, 0, 0},
+	{0x3507, 0x00, 0, 0},
+	{0x3508, 0x00, 0, 0},
+	{0x3509, 0x80, 0, 0}, // 8X
+	{0x350a, 0x00, 0, 0},
+	{0x350b, 0x00, 0, 0},
+	{0x350c, 0x00, 0, 0},
+	{0x350d, 0x00, 0, 0},
+	{0x350e, 0x00, 0, 0},
+	{0x350f, 0x80, 0, 0},
+	{0x3510, 0x00, 0, 0},
+	{0x3511, 0x00, 0, 0},
+	{0x3512, 0x00, 0, 0},
+	{0x3513, 0x00, 0, 0},
+	{0x3514, 0x00, 0, 0},
+	{0x3515, 0x80, 0, 0},
+	{0x3516, 0x00, 0, 0},
+	{0x3517, 0x00, 0, 0},
+	{0x3518, 0x00, 0, 0},
+	{0x3519, 0x00, 0, 0},
+	{0x351a, 0x00, 0, 0},
+	{0x351b, 0x80, 0, 0},
+	{0x351c, 0x00, 0, 0},
+	{0x351d, 0x00, 0, 0},
+	{0x351e, 0x00, 0, 0},
+	{0x351f, 0x00, 0, 0},
+	{0x3520, 0x00, 0, 0},
+	{0x3521, 0x80, 0, 0},
+	{0x3522, 0x08, 0, 0},
+	{0x3524, 0x08, 0, 0},
+	{0x3526, 0x08, 0, 0},
+	{0x3528, 0x08, 0, 0},
+	{0x352a, 0x08, 0, 0},
+	{0x3602, 0x00, 0, 0},
+	{0x3603, 0x40, 0, 0},
+	{0x3604, 0x02, 0, 0},
+	{0x3605, 0x00, 0, 0},
+	{0x3606, 0x00, 0, 0},
+	{0x3607, 0x00, 0, 0},
+	{0x3609, 0x12, 0, 0},
+	{0x360a, 0x40, 0, 0},
+	{0x360c, 0x08, 0, 0},
+	{0x360f, 0xe5, 0, 0},
+	{0x3608, 0x8f, 0, 0},
+	{0x3611, 0x00, 0, 0},
+	{0x3613, 0xf7, 0, 0},
+	{0x3616, 0x58, 0, 0},
+	{0x3619, 0x99, 0, 0},
+	{0x361b, 0x60, 0, 0},
+	{0x361c, 0x7a, 0, 0},
+	{0x361e, 0x79, 0, 0},
+	{0x361f, 0x02, 0, 0},
+	{0x3632, 0x05, 0, 0},
+	{0x3633, 0x10, 0, 0},
+	{0x3634, 0x10, 0, 0},
+	{0x3635, 0x10, 0, 0},
+	{0x3636, 0x15, 0, 0},
+	{0x3646, 0x86, 0, 0},
+	{0x364a, 0x0b, 0, 0},
+	{0x3700, 0x17, 0, 0},
+	{0x3701, 0x22, 0, 0},
+	{0x3703, 0x10, 0, 0},
+	{0x370a, 0x37, 0, 0},
+	{0x3705, 0x00, 0, 0},
+	{0x3706, 0x63, 0, 0},
+	{0x3709, 0x3c, 0, 0},
+	{0x370b, 0x01, 0, 0},
+	{0x370c, 0x30, 0, 0},
+	{0x3710, 0x24, 0, 0},
+	{0x3711, 0x0c, 0, 0},
+	{0x3716, 0x00, 0, 0},
+	{0x3720, 0x28, 0, 0},
+	{0x3729, 0x7b, 0, 0},
+	{0x372a, 0x84, 0, 0},
+	{0x372b, 0xbd, 0, 0},
+	{0x372c, 0xbc, 0, 0},
+	{0x372e, 0x52, 0, 0},
+	{0x373c, 0x0e, 0, 0},
+	{0x373e, 0x33, 0, 0},
+	{0x3743, 0x10, 0, 0},
+	{0x3744, 0x88, 0, 0},
+	{0x3745, 0xc0, 0, 0},
+	{0x374a, 0x43, 0, 0},
+	{0x374c, 0x00, 0, 0},
+	{0x374e, 0x23, 0, 0},
+	{0x3751, 0x7b, 0, 0},
+	{0x3752, 0x84, 0, 0},
+	{0x3753, 0xbd, 0, 0},
+	{0x3754, 0xbc, 0, 0},
+	{0x3756, 0x52, 0, 0},
+	{0x375c, 0x00, 0, 0},
+	{0x3760, 0x00, 0, 0},
+	{0x3761, 0x00, 0, 0},
+	{0x3762, 0x00, 0, 0},
+	{0x3763, 0x00, 0, 0},
+	{0x3764, 0x00, 0, 0},
+	{0x3767, 0x04, 0, 0},
+	{0x3768, 0x04, 0, 0},
+	{0x3769, 0x08, 0, 0},
+	{0x376a, 0x08, 0, 0},
+	{0x376b, 0x40, 0, 0},
+	{0x376c, 0x00, 0, 0},
+	{0x376d, 0x00, 0, 0},
+	{0x376e, 0x00, 0, 0},
+	{0x3773, 0x00, 0, 0},
+	{0x3774, 0x51, 0, 0},
+	{0x3776, 0xbd, 0, 0},
+	{0x3777, 0xbd, 0, 0},
+	{0x3781, 0x18, 0, 0},
+	{0x3783, 0x25, 0, 0},
+	{0x3798, 0x1b, 0, 0},
+	{0x3800, 0x00, 0, 0},
+	{0x3801, 0x48, 0, 0},
+	{0x3802, 0x00, 0, 0},
+	{0x3803, 0x2C, 0, 0},
+	{0x3804, 0x0a, 0, 0},
+	{0x3805, 0x57, 0, 0},
+	{0x3806, 0x05, 0, 0},
+	{0x3807, 0xD3, 0, 0},
+	{0x3808, 0x02, 0, 0},
+	{0x3809, 0x80, 0, 0},
+	{0x380a, 0x01, 0, 0},
+	{0x380b, 0xe0, 0, 0},
+	{0x380c, 0x02, 0, 0},
+	{0x380d, 0x04, 0, 0},
+	{0x380e, 0x03, 0, 0},
+	{0x380f, 0x05, 0, 0},
+	{0x3810, 0x00, 0, 0},
+	{0x3811, 0x04, 0, 0},
+	{0x3812, 0x00, 0, 0},
+	{0x3813, 0x02, 0, 0},
+	{0x3814, 0x03, 0, 0},
+	{0x3815, 0x01, 0, 0},
+	{0x3819, 0x01, 0, 0},
+	{0x3820, 0x06, 0, 0},
+	{0x3821, 0x00, 0, 0},
+	{0x3829, 0x00, 0, 0},
+	{0x382a, 0x03, 0, 0},
+	{0x382b, 0x01, 0, 0},
+	{0x382d, 0x7f, 0, 0},
+	{0x3830, 0x08, 0, 0},
+	{0x3836, 0x02, 0, 0},
+	{0x3837, 0x00, 0, 0},
+	{0x3841, 0x02, 0, 0},
+	{0x3846, 0x08, 0, 0},
+	{0x3847, 0x07, 0, 0},
+	{0x3d85, 0x36, 0, 0},
+	{0x3d8c, 0x71, 0, 0},
+	{0x3d8d, 0xcb, 0, 0},
+	{0x3f0a, 0x00, 0, 0},
+	{0x4000, 0x71, 0, 0},
+	{0x4001, 0x50, 0, 0},
+	{0x4002, 0x04, 0, 0},
+	{0x4003, 0x14, 0, 0},
+	{0x400e, 0x00, 0, 0},
+	{0x4011, 0x00, 0, 0},
+	{0x401a, 0x00, 0, 0},
+	{0x401b, 0x00, 0, 0},
+	{0x401c, 0x00, 0, 0},
+	{0x401d, 0x00, 0, 0},
+	{0x401f, 0x00, 0, 0},
+	{0x4020, 0x00, 0, 0},
+	{0x4021, 0x10, 0, 0},
+	{0x4022, 0x03, 0, 0},
+	{0x4023, 0x93, 0, 0},
+	{0x4024, 0x04, 0, 0},
+	{0x4025, 0xC0, 0, 0},
+	{0x4026, 0x04, 0, 0},
+	{0x4027, 0xD0, 0, 0},
+	{0x4028, 0x00, 0, 0},
+	{0x4029, 0x02, 0, 0},
+	{0x402a, 0x06, 0, 0},
+	{0x402b, 0x04, 0, 0},
+	{0x402c, 0x02, 0, 0},
+	{0x402d, 0x02, 0, 0},
+	{0x402e, 0x0e, 0, 0},
+	{0x402f, 0x04, 0, 0},
+	{0x4302, 0xff, 0, 0},
+	{0x4303, 0xff, 0, 0},
+	{0x4304, 0x00, 0, 0},
+	{0x4305, 0x00, 0, 0},
+	{0x4306, 0x00, 0, 0},
+	{0x4308, 0x02, 0, 0},
+	{0x4500, 0x6c, 0, 0},
+	{0x4501, 0xc4, 0, 0},
+	{0x4502, 0x44, 0, 0},
+	{0x4503, 0x01, 0, 0},
+	{0x4600, 0x00, 0, 0},
+	{0x4601, 0x4F, 0, 0},
+	{0x4800, 0x04, 0, 0},
+	{0x4813, 0x08, 0, 0},
+	{0x481f, 0x40, 0, 0},
+	{0x4829, 0x78, 0, 0},
+	{0x4837, 0x10, 0, 0},
+	{0x4b00, 0x2a, 0, 0},
+	{0x4b0d, 0x00, 0, 0},
+	{0x4d00, 0x04, 0, 0},
+	{0x4d01, 0x42, 0, 0},
+	{0x4d02, 0xd1, 0, 0},
+	{0x4d03, 0x93, 0, 0},
+	{0x4d04, 0xf5, 0, 0},
+	{0x4d05, 0xc1, 0, 0},
+	{0x5000, 0xf3, 0, 0},
+	{0x5001, 0x11, 0, 0},
+	{0x5004, 0x00, 0, 0},
+	{0x500a, 0x00, 0, 0},
+	{0x500b, 0x00, 0, 0},
+	{0x5032, 0x00, 0, 0},
+	{0x5040, 0x00, 0, 0},
+	{0x5050, 0x3c, 0, 0},
+	{0x5500, 0x00, 0, 0},
+	{0x5501, 0x10, 0, 0},
+	{0x5502, 0x01, 0, 0},
+	{0x5503, 0x0f, 0, 0},
+	{0x8000, 0x00, 0, 0},
+	{0x8001, 0x00, 0, 0},
+	{0x8002, 0x00, 0, 0},
+	{0x8003, 0x00, 0, 0},
+	{0x8004, 0x00, 0, 0},
+	{0x8005, 0x00, 0, 0},
+	{0x8006, 0x00, 0, 0},
+	{0x8007, 0x00, 0, 0},
+	{0x8008, 0x00, 0, 0},
+	{0x3638, 0x00, 0, 0},
+};
+
+static const struct reg_value ov4689_setting_720P_1280_720[] = {
+	//@@ RES_1280x720_2x_Bin_150fps_816Mbps
+	//OV4689_AM01B_1280x720_24M_2lane_816Mbps_150fps_20140210.txt
+	{0x0103, 0x01, 0, 0},
+	{0x3638, 0x00, 0, 0},
+	{0x0300, 0x00, 0, 0},
+	{0x0302, 0x22, 0, 0},
+	{0x0303, 0x00, 0, 0},
+	{0x0304, 0x03, 0, 0},
+	{0x030b, 0x00, 0, 0},
+	{0x030d, 0x1e, 0, 0},
+	{0x030e, 0x04, 0, 0},
+	{0x030f, 0x01, 0, 0},
+	{0x0312, 0x01, 0, 0},
+	{0x031e, 0x00, 0, 0},
+	{0x3000, 0x20, 0, 0},
+	{0x3002, 0x00, 0, 0},
+	{0x3018, 0x32, 0, 0}, // 32/72 2lane/4lane
+	{0x3019, 0x0c, 0, 0}, // 0c/00 2lane/4lane
+	{0x3020, 0x93, 0, 0},
+	{0x3021, 0x03, 0, 0},
+	{0x3022, 0x01, 0, 0},
+	{0x3031, 0x0a, 0, 0},
+	{0x303f, 0x0c, 0, 0},
+	{0x3305, 0xf1, 0, 0},
+	{0x3307, 0x04, 0, 0},
+	{0x3309, 0x29, 0, 0},
+	{0x3500, 0x00, 0, 0},
+	{0x3501, 0x30, 0, 0},
+	{0x3502, 0x00, 0, 0},
+	{0x3503, 0x04, 0, 0},
+	{0x3504, 0x00, 0, 0},
+	{0x3505, 0x00, 0, 0},
+	{0x3506, 0x00, 0, 0},
+	{0x3507, 0x00, 0, 0},
+	{0x3508, 0x07, 0, 0},
+	{0x3509, 0x78, 0, 0},
+	{0x350a, 0x00, 0, 0},
+	{0x350b, 0x00, 0, 0},
+	{0x350c, 0x00, 0, 0},
+	{0x350d, 0x00, 0, 0},
+	{0x350e, 0x00, 0, 0},
+	{0x350f, 0x80, 0, 0},
+	{0x3510, 0x00, 0, 0},
+	{0x3511, 0x00, 0, 0},
+	{0x3512, 0x00, 0, 0},
+	{0x3513, 0x00, 0, 0},
+	{0x3514, 0x00, 0, 0},
+	{0x3515, 0x80, 0, 0},
+	{0x3516, 0x00, 0, 0},
+	{0x3517, 0x00, 0, 0},
+	{0x3518, 0x00, 0, 0},
+	{0x3519, 0x00, 0, 0},
+	{0x351a, 0x00, 0, 0},
+	{0x351b, 0x80, 0, 0},
+	{0x351c, 0x00, 0, 0},
+	{0x351d, 0x00, 0, 0},
+	{0x351e, 0x00, 0, 0},
+	{0x351f, 0x00, 0, 0},
+	{0x3520, 0x00, 0, 0},
+	{0x3521, 0x80, 0, 0},
+	{0x3522, 0x08, 0, 0},
+	{0x3524, 0x08, 0, 0},
+	{0x3526, 0x08, 0, 0},
+	{0x3528, 0x08, 0, 0},
+	{0x352a, 0x08, 0, 0},
+	{0x3602, 0x00, 0, 0},
+	{0x3603, 0x40, 0, 0},
+	{0x3604, 0x02, 0, 0},
+	{0x3605, 0x00, 0, 0},
+	{0x3606, 0x00, 0, 0},
+	{0x3607, 0x00, 0, 0},
+	{0x3609, 0x12, 0, 0},
+	{0x360a, 0x40, 0, 0},
+	{0x360c, 0x08, 0, 0},
+	{0x360f, 0xe5, 0, 0},
+	{0x3608, 0x8f, 0, 0},
+	{0x3611, 0x00, 0, 0},
+	{0x3613, 0xf7, 0, 0},
+	{0x3616, 0x58, 0, 0},
+	{0x3619, 0x99, 0, 0},
+	{0x361b, 0x60, 0, 0},
+	{0x361c, 0x7a, 0, 0},
+	{0x361e, 0x79, 0, 0},
+	{0x361f, 0x02, 0, 0},
+	{0x3632, 0x05, 0, 0},
+	{0x3633, 0x10, 0, 0},
+	{0x3634, 0x10, 0, 0},
+	{0x3635, 0x10, 0, 0},
+	{0x3636, 0x15, 0, 0},
+	{0x3646, 0x86, 0, 0},
+	{0x364a, 0x0b, 0, 0},
+	{0x3700, 0x17, 0, 0},
+	{0x3701, 0x22, 0, 0},
+	{0x3703, 0x10, 0, 0},
+	{0x370a, 0x37, 0, 0},
+	{0x3705, 0x00, 0, 0},
+	{0x3706, 0x63, 0, 0},
+	{0x3709, 0x3c, 0, 0},
+	{0x370b, 0x01, 0, 0},
+	{0x370c, 0x30, 0, 0},
+	{0x3710, 0x24, 0, 0},
+	{0x3711, 0x0c, 0, 0},
+	{0x3716, 0x00, 0, 0},
+	{0x3720, 0x28, 0, 0},
+	{0x3729, 0x7b, 0, 0},
+	{0x372a, 0x84, 0, 0},
+	{0x372b, 0xbd, 0, 0},
+	{0x372c, 0xbc, 0, 0},
+	{0x372e, 0x52, 0, 0},
+	{0x373c, 0x0e, 0, 0},
+	{0x373e, 0x33, 0, 0},
+	{0x3743, 0x10, 0, 0},
+	{0x3744, 0x88, 0, 0},
+	{0x3745, 0xc0, 0, 0},
+	{0x374a, 0x43, 0, 0},
+	{0x374c, 0x00, 0, 0},
+	{0x374e, 0x23, 0, 0},
+	{0x3751, 0x7b, 0, 0},
+	{0x3752, 0x84, 0, 0},
+	{0x3753, 0xbd, 0, 0},
+	{0x3754, 0xbc, 0, 0},
+	{0x3756, 0x52, 0, 0},
+	{0x375c, 0x00, 0, 0},
+	{0x3760, 0x00, 0, 0},
+	{0x3761, 0x00, 0, 0},
+	{0x3762, 0x00, 0, 0},
+	{0x3763, 0x00, 0, 0},
+	{0x3764, 0x00, 0, 0},
+	{0x3767, 0x04, 0, 0},
+	{0x3768, 0x04, 0, 0},
+	{0x3769, 0x08, 0, 0},
+	{0x376a, 0x08, 0, 0},
+	{0x376b, 0x40, 0, 0},
+	{0x376c, 0x00, 0, 0},
+	{0x376d, 0x00, 0, 0},
+	{0x376e, 0x00, 0, 0},
+	{0x3773, 0x00, 0, 0},
+	{0x3774, 0x51, 0, 0},
+	{0x3776, 0xbd, 0, 0},
+	{0x3777, 0xbd, 0, 0},
+	{0x3781, 0x18, 0, 0},
+	{0x3783, 0x25, 0, 0},
+	{0x3798, 0x1b, 0, 0},
+	{0x3800, 0x00, 0, 0},
+	{0x3801, 0x48, 0, 0},
+	{0x3802, 0x00, 0, 0},
+	{0x3803, 0x2C, 0, 0},
+	{0x3804, 0x0a, 0, 0},
+	{0x3805, 0x57, 0, 0},
+	{0x3806, 0x05, 0, 0},
+	{0x3807, 0xD3, 0, 0},
+	{0x3808, 0x05, 0, 0},
+	{0x3809, 0x00, 0, 0},
+	{0x380a, 0x02, 0, 0},
+	{0x380b, 0xD0, 0, 0},
+#if 1
+	{0x380c, 0x04, 0, 0}, // 0a ; 03
+	{0x380d, 0x08, 0, 0}, // 1c ; 5C
+#else
+	{0x380c, 0x05, 0, 0}, // 120fps
+	{0x380d, 0x0A, 0, 0},
+#endif
+	{0x380e, 0x03, 0, 0},
+	{0x380f, 0x05, 0, 0},
+	{0x3810, 0x00, 0, 0},
+	{0x3811, 0x04, 0, 0},
+	{0x3812, 0x00, 0, 0},
+	{0x3813, 0x02, 0, 0},
+	{0x3814, 0x03, 0, 0},
+	{0x3815, 0x01, 0, 0},
+	{0x3819, 0x01, 0, 0},
+	{0x3820, 0x06, 0, 0},
+	{0x3821, 0x00, 0, 0},
+	{0x3829, 0x00, 0, 0},
+	{0x382a, 0x03, 0, 0},
+	{0x382b, 0x01, 0, 0},
+	{0x382d, 0x7f, 0, 0},
+	{0x3830, 0x08, 0, 0},
+	{0x3836, 0x02, 0, 0},
+	{0x3837, 0x00, 0, 0},
+	{0x3841, 0x02, 0, 0},
+	{0x3846, 0x08, 0, 0},
+	{0x3847, 0x07, 0, 0},
+	{0x3d85, 0x36, 0, 0},
+	{0x3d8c, 0x71, 0, 0},
+	{0x3d8d, 0xcb, 0, 0},
+	{0x3f0a, 0x00, 0, 0},
+	{0x4000, 0x71, 0, 0},
+	{0x4001, 0x50, 0, 0},
+	{0x4002, 0x04, 0, 0},
+	{0x4003, 0x14, 0, 0},
+	{0x400e, 0x00, 0, 0},
+	{0x4011, 0x00, 0, 0},
+	{0x401a, 0x00, 0, 0},
+	{0x401b, 0x00, 0, 0},
+	{0x401c, 0x00, 0, 0},
+	{0x401d, 0x00, 0, 0},
+	{0x401f, 0x00, 0, 0},
+	{0x4020, 0x00, 0, 0},
+	{0x4021, 0x10, 0, 0},
+	{0x4022, 0x03, 0, 0},
+	{0x4023, 0x93, 0, 0},
+	{0x4024, 0x04, 0, 0},
+	{0x4025, 0xC0, 0, 0},
+	{0x4026, 0x04, 0, 0},
+	{0x4027, 0xD0, 0, 0},
+	{0x4028, 0x00, 0, 0},
+	{0x4029, 0x02, 0, 0},
+	{0x402a, 0x06, 0, 0},
+	{0x402b, 0x04, 0, 0},
+	{0x402c, 0x02, 0, 0},
+	{0x402d, 0x02, 0, 0},
+	{0x402e, 0x0e, 0, 0},
+	{0x402f, 0x04, 0, 0},
+	{0x4302, 0xff, 0, 0},
+	{0x4303, 0xff, 0, 0},
+	{0x4304, 0x00, 0, 0},
+	{0x4305, 0x00, 0, 0},
+	{0x4306, 0x00, 0, 0},
+	{0x4308, 0x02, 0, 0},
+	{0x4500, 0x6c, 0, 0},
+	{0x4501, 0xc4, 0, 0},
+	{0x4502, 0x44, 0, 0},
+	{0x4503, 0x01, 0, 0},
+	{0x4600, 0x00, 0, 0},
+	{0x4601, 0x4F, 0, 0},
+	{0x4800, 0x04, 0, 0},
+	{0x4813, 0x08, 0, 0},
+	{0x481f, 0x40, 0, 0},
+	{0x4829, 0x78, 0, 0},
+	{0x4837, 0x10, 0, 0},
+	{0x4b00, 0x2a, 0, 0},
+	{0x4b0d, 0x00, 0, 0},
+	{0x4d00, 0x04, 0, 0},
+	{0x4d01, 0x42, 0, 0},
+	{0x4d02, 0xd1, 0, 0},
+	{0x4d03, 0x93, 0, 0},
+	{0x4d04, 0xf5, 0, 0},
+	{0x4d05, 0xc1, 0, 0},
+	{0x5000, 0xf3, 0, 0},
+	{0x5001, 0x11, 0, 0},
+	{0x5004, 0x00, 0, 0},
+	{0x500a, 0x00, 0, 0},
+	{0x500b, 0x00, 0, 0},
+	{0x5032, 0x00, 0, 0},
+	{0x5040, 0x00, 0, 0},
+	{0x5050, 0x3c, 0, 0},
+	{0x5500, 0x00, 0, 0},
+	{0x5501, 0x10, 0, 0},
+	{0x5502, 0x01, 0, 0},
+	{0x5503, 0x0f, 0, 0},
+	{0x8000, 0x00, 0, 0},
+	{0x8001, 0x00, 0, 0},
+	{0x8002, 0x00, 0, 0},
+	{0x8003, 0x00, 0, 0},
+	{0x8004, 0x00, 0, 0},
+	{0x8005, 0x00, 0, 0},
+	{0x8006, 0x00, 0, 0},
+	{0x8007, 0x00, 0, 0},
+	{0x8008, 0x00, 0, 0},
+	{0x3638, 0x00, 0, 0},
+};
+
+static const struct reg_value ov4689_setting_1080P_1920_1080[] = {
+	//@@ RES_1920x1080_60fps_816Mbps 2lanes
+	{0x0103, 0x01, 0, 0},
+	{0x3638, 0x00, 0, 0},
+	{0x0300, 0x00, 0, 0},
+	{0x0302, 0x22, 0, 0},
+	{0x0303, 0x00, 0, 0},
+	{0x0304, 0x03, 0, 0},
+	{0x030b, 0x00, 0, 0},
+	{0x030d, 0x1e, 0, 0},
+	{0x030e, 0x04, 0, 0},
+	{0x030f, 0x01, 0, 0},
+	{0x0312, 0x01, 0, 0},
+	{0x031e, 0x00, 0, 0},
+	{0x3000, 0x20, 0, 0},
+	{0x3002, 0x00, 0, 0},
+	{0x3018, 0x32, 0, 0},
+	{0x3019, 0x0c, 0, 0},
+	{0x3020, 0x93, 0, 0},
+	{0x3021, 0x03, 0, 0},
+	{0x3022, 0x01, 0, 0},
+	{0x3031, 0x0a, 0, 0},
+	{0x303f, 0x0c, 0, 0},
+	{0x3305, 0xf1, 0, 0},
+	{0x3307, 0x04, 0, 0},
+	{0x3309, 0x29, 0, 0},
+	{0x3500, 0x00, 0, 0},  // AEC
+	{0x3501, 0x4c, 0, 0},
+	{0x3502, 0x00, 0, 0},
+	{0x3503, 0x04, 0, 0},
+	{0x3504, 0x00, 0, 0},
+	{0x3505, 0x00, 0, 0},
+	{0x3506, 0x00, 0, 0},
+	{0x3507, 0x00, 0, 0},
+	{0x3508, 0x00, 0, 0},
+	{0x3509, 0x80, 0, 0},
+	{0x350a, 0x00, 0, 0},
+	{0x350b, 0x00, 0, 0},
+	{0x350c, 0x00, 0, 0},
+	{0x350d, 0x00, 0, 0},
+	{0x350e, 0x00, 0, 0},
+	{0x350f, 0x80, 0, 0},
+	{0x3510, 0x00, 0, 0},
+	{0x3511, 0x00, 0, 0},
+	{0x3512, 0x00, 0, 0},
+	{0x3513, 0x00, 0, 0},
+	{0x3514, 0x00, 0, 0},
+	{0x3515, 0x80, 0, 0},
+	{0x3516, 0x00, 0, 0},
+	{0x3517, 0x00, 0, 0},
+	{0x3518, 0x00, 0, 0},
+	{0x3519, 0x00, 0, 0},
+	{0x351a, 0x00, 0, 0},
+	{0x351b, 0x80, 0, 0},
+	{0x351c, 0x00, 0, 0},
+	{0x351d, 0x00, 0, 0},
+	{0x351e, 0x00, 0, 0},
+	{0x351f, 0x00, 0, 0},
+	{0x3520, 0x00, 0, 0},
+	{0x3521, 0x80, 0, 0},
+	{0x3522, 0x08, 0, 0},
+	{0x3524, 0x08, 0, 0},
+	{0x3526, 0x08, 0, 0},
+	{0x3528, 0x08, 0, 0},
+	{0x352a, 0x08, 0, 0},
+	{0x3602, 0x00, 0, 0},
+	{0x3603, 0x40, 0, 0},
+	{0x3604, 0x02, 0, 0},
+	{0x3605, 0x00, 0, 0},
+	{0x3606, 0x00, 0, 0},
+	{0x3607, 0x00, 0, 0},
+	{0x3609, 0x12, 0, 0},
+	{0x360a, 0x40, 0, 0},
+	{0x360c, 0x08, 0, 0},
+	{0x360f, 0xe5, 0, 0},
+	{0x3608, 0x8f, 0, 0},
+	{0x3611, 0x00, 0, 0},
+	{0x3613, 0xf7, 0, 0},
+	{0x3616, 0x58, 0, 0},
+	{0x3619, 0x99, 0, 0},
+	{0x361b, 0x60, 0, 0},
+	{0x361c, 0x7a, 0, 0},
+	{0x361e, 0x79, 0, 0},
+	{0x361f, 0x02, 0, 0},
+	{0x3632, 0x00, 0, 0},
+	{0x3633, 0x10, 0, 0},
+	{0x3634, 0x10, 0, 0},
+	{0x3635, 0x10, 0, 0},
+	{0x3636, 0x15, 0, 0},
+	{0x3646, 0x86, 0, 0},
+	{0x364a, 0x0b, 0, 0},
+	{0x3700, 0x17, 0, 0},
+	{0x3701, 0x22, 0, 0},
+	{0x3703, 0x10, 0, 0},
+	{0x370a, 0x37, 0, 0},
+	{0x3705, 0x00, 0, 0},
+	{0x3706, 0x63, 0, 0},
+	{0x3709, 0x3c, 0, 0},
+	{0x370b, 0x01, 0, 0},
+	{0x370c, 0x30, 0, 0},
+	{0x3710, 0x24, 0, 0},
+	{0x3711, 0x0c, 0, 0},
+	{0x3716, 0x00, 0, 0},
+	{0x3720, 0x28, 0, 0},
+	{0x3729, 0x7b, 0, 0},
+	{0x372a, 0x84, 0, 0},
+	{0x372b, 0xbd, 0, 0},
+	{0x372c, 0xbc, 0, 0},
+	{0x372e, 0x52, 0, 0},
+	{0x373c, 0x0e, 0, 0},
+	{0x373e, 0x33, 0, 0},
+	{0x3743, 0x10, 0, 0},
+	{0x3744, 0x88, 0, 0},
+	{0x3745, 0xc0, 0, 0},
+	{0x374a, 0x43, 0, 0},
+	{0x374c, 0x00, 0, 0},
+	{0x374e, 0x23, 0, 0},
+	{0x3751, 0x7b, 0, 0},
+	{0x3752, 0x84, 0, 0},
+	{0x3753, 0xbd, 0, 0},
+	{0x3754, 0xbc, 0, 0},
+	{0x3756, 0x52, 0, 0},
+	{0x375c, 0x00, 0, 0},
+	{0x3760, 0x00, 0, 0},
+	{0x3761, 0x00, 0, 0},
+	{0x3762, 0x00, 0, 0},
+	{0x3763, 0x00, 0, 0},
+	{0x3764, 0x00, 0, 0},
+	{0x3767, 0x04, 0, 0},
+	{0x3768, 0x04, 0, 0},
+	{0x3769, 0x08, 0, 0},
+	{0x376a, 0x08, 0, 0},
+	{0x376b, 0x20, 0, 0},
+	{0x376c, 0x00, 0, 0},
+	{0x376d, 0x00, 0, 0},
+	{0x376e, 0x00, 0, 0},
+	{0x3773, 0x00, 0, 0},
+	{0x3774, 0x51, 0, 0},
+	{0x3776, 0xbd, 0, 0},
+	{0x3777, 0xbd, 0, 0},
+	{0x3781, 0x18, 0, 0},
+	{0x3783, 0x25, 0, 0},
+	{0x3798, 0x1b, 0, 0},
+	{0x3800, 0x01, 0, 0},   // timings
+	{0x3801, 0x88, 0, 0},
+	{0x3802, 0x00, 0, 0},
+	{0x3803, 0xe0, 0, 0},
+	{0x3804, 0x09, 0, 0},
+	{0x3805, 0x17, 0, 0},
+	{0x3806, 0x05, 0, 0},
+	{0x3807, 0x1f, 0, 0},
+	{0x3808, 0x07, 0, 0},
+	{0x3809, 0x80, 0, 0},
+	{0x380a, 0x04, 0, 0},
+	{0x380b, 0x38, 0, 0},
+	{0x380c, 0x06, 0, 0},
+	{0x380d, 0xe0, 0, 0},
+	{0x380e, 0x04, 0, 0},
+	{0x380f, 0x70, 0, 0},
+	{0x3810, 0x00, 0, 0},
+	{0x3811, 0x08, 0, 0},
+	{0x3812, 0x00, 0, 0},
+	{0x3813, 0x04, 0, 0},
+	{0x3814, 0x01, 0, 0},
+	{0x3815, 0x01, 0, 0},
+	{0x3819, 0x01, 0, 0},
+	{0x3820, 0x06, 0, 0},
+	{0x3821, 0x00, 0, 0},
+	{0x3829, 0x00, 0, 0},
+	{0x382a, 0x01, 0, 0},
+	{0x382b, 0x01, 0, 0},
+	{0x382d, 0x7f, 0, 0},
+	{0x3830, 0x04, 0, 0},
+	{0x3836, 0x01, 0, 0},
+	{0x3837, 0x00, 0, 0},
+	{0x3841, 0x02, 0, 0},
+	{0x3846, 0x08, 0, 0},
+	{0x3847, 0x07, 0, 0},
+	{0x3d85, 0x36, 0, 0},
+	{0x3d8c, 0x71, 0, 0},
+	{0x3d8d, 0xcb, 0, 0},
+	{0x3f0a, 0x00, 0, 0},
+	{0x4000, 0xf1, 0, 0},
+	{0x4001, 0x40, 0, 0},
+	{0x4002, 0x04, 0, 0},
+	{0x4003, 0x14, 0, 0},
+	{0x400e, 0x00, 0, 0},
+	{0x4011, 0x00, 0, 0},
+	{0x401a, 0x00, 0, 0},
+	{0x401b, 0x00, 0, 0},
+	{0x401c, 0x00, 0, 0},
+	{0x401d, 0x00, 0, 0},
+	{0x401f, 0x00, 0, 0},
+	{0x4020, 0x00, 0, 0},
+	{0x4021, 0x10, 0, 0},
+	{0x4022, 0x06, 0, 0},
+	{0x4023, 0x13, 0, 0},
+	{0x4024, 0x07, 0, 0},
+	{0x4025, 0x40, 0, 0},
+	{0x4026, 0x07, 0, 0},
+	{0x4027, 0x50, 0, 0},
+	{0x4028, 0x00, 0, 0},
+	{0x4029, 0x02, 0, 0},
+	{0x402a, 0x06, 0, 0},
+	{0x402b, 0x04, 0, 0},
+	{0x402c, 0x02, 0, 0},
+	{0x402d, 0x02, 0, 0},
+	{0x402e, 0x0e, 0, 0},
+	{0x402f, 0x04, 0, 0},
+	{0x4302, 0xff, 0, 0},
+	{0x4303, 0xff, 0, 0},
+	{0x4304, 0x00, 0, 0},
+	{0x4305, 0x00, 0, 0},
+	{0x4306, 0x00, 0, 0},
+	{0x4308, 0x02, 0, 0},
+	{0x4500, 0x6c, 0, 0},
+	{0x4501, 0xc4, 0, 0},
+	{0x4502, 0x40, 0, 0},
+	{0x4503, 0x01, 0, 0},
+	{0x4601, 0x77, 0, 0},
+	{0x4800, 0x04, 0, 0},
+	{0x4813, 0x08, 0, 0},
+	{0x481f, 0x40, 0, 0},
+	{0x4829, 0x78, 0, 0},
+	{0x4837, 0x10, 0, 0},
+	{0x4b00, 0x2a, 0, 0},
+	{0x4b0d, 0x00, 0, 0},
+	{0x4d00, 0x04, 0, 0},
+	{0x4d01, 0x42, 0, 0},
+	{0x4d02, 0xd1, 0, 0},
+	{0x4d03, 0x93, 0, 0},
+	{0x4d04, 0xf5, 0, 0},
+	{0x4d05, 0xc1, 0, 0},
+	{0x5000, 0xf3, 0, 0},
+	{0x5001, 0x11, 0, 0},
+	{0x5004, 0x00, 0, 0},
+	{0x500a, 0x00, 0, 0},
+	{0x500b, 0x00, 0, 0},
+	{0x5032, 0x00, 0, 0},
+	{0x5040, 0x00, 0, 0},
+	{0x5050, 0x0c, 0, 0},
+	{0x5500, 0x00, 0, 0},
+	{0x5501, 0x10, 0, 0},
+	{0x5502, 0x01, 0, 0},
+	{0x5503, 0x0f, 0, 0},
+	{0x8000, 0x00, 0, 0},
+	{0x8001, 0x00, 0, 0},
+	{0x8002, 0x00, 0, 0},
+	{0x8003, 0x00, 0, 0},
+	{0x8004, 0x00, 0, 0},
+	{0x8005, 0x00, 0, 0},
+	{0x8006, 0x00, 0, 0},
+	{0x8007, 0x00, 0, 0},
+	{0x8008, 0x00, 0, 0},
+	{0x3638, 0x00, 0, 0},
+};
+
+static const struct reg_value ov4689_setting_4M_2688_1520[] = {
+	//@@ 0 10 RES_2688x1520_default(60fps)
+	//102 2630 960
+	{0x0103, 0x01, 0, 0},
+	{0x3638, 0x00, 0, 0},
+	{0x0300, 0x00, 0, 0},
+	{0x0302, 0x22, 0, 0}, // 2a ;1008Mbps,23 ;; 840Mbps
+	{0x0304, 0x03, 0, 0},
+	{0x030b, 0x00, 0, 0},
+	{0x030d, 0x1e, 0, 0},
+	{0x030e, 0x04, 0, 0},
+	{0x030f, 0x01, 0, 0},
+	{0x0312, 0x01, 0, 0},
+	{0x031e, 0x00, 0, 0},
+	{0x3000, 0x20, 0, 0},
+	{0x3002, 0x00, 0, 0},
+	{0x3018, 0x32, 0, 0},
+	{0x3019, 0x0C, 0, 0},
+	{0x3020, 0x93, 0, 0},
+	{0x3021, 0x03, 0, 0},
+	{0x3022, 0x01, 0, 0},
+	{0x3031, 0x0a, 0, 0},
+	{0x303f, 0x0c, 0, 0},
+	{0x3305, 0xf1, 0, 0},
+	{0x3307, 0x04, 0, 0},
+	{0x3309, 0x29, 0, 0},
+	{0x3500, 0x00, 0, 0},
+	{0x3501, 0x60, 0, 0},
+	{0x3502, 0x00, 0, 0},
+	{0x3503, 0x04, 0, 0},
+	{0x3504, 0x00, 0, 0},
+	{0x3505, 0x00, 0, 0},
+	{0x3506, 0x00, 0, 0},
+	{0x3507, 0x00, 0, 0},
+	{0x3508, 0x00, 0, 0},
+	{0x3509, 0x80, 0, 0},
+	{0x350a, 0x00, 0, 0},
+	{0x350b, 0x00, 0, 0},
+	{0x350c, 0x00, 0, 0},
+	{0x350d, 0x00, 0, 0},
+	{0x350e, 0x00, 0, 0},
+	{0x350f, 0x80, 0, 0},
+	{0x3510, 0x00, 0, 0},
+	{0x3511, 0x00, 0, 0},
+	{0x3512, 0x00, 0, 0},
+	{0x3513, 0x00, 0, 0},
+	{0x3514, 0x00, 0, 0},
+	{0x3515, 0x80, 0, 0},
+	{0x3516, 0x00, 0, 0},
+	{0x3517, 0x00, 0, 0},
+	{0x3518, 0x00, 0, 0},
+	{0x3519, 0x00, 0, 0},
+	{0x351a, 0x00, 0, 0},
+	{0x351b, 0x80, 0, 0},
+	{0x351c, 0x00, 0, 0},
+	{0x351d, 0x00, 0, 0},
+	{0x351e, 0x00, 0, 0},
+	{0x351f, 0x00, 0, 0},
+	{0x3520, 0x00, 0, 0},
+	{0x3521, 0x80, 0, 0},
+	{0x3522, 0x08, 0, 0},
+	{0x3524, 0x08, 0, 0},
+	{0x3526, 0x08, 0, 0},
+	{0x3528, 0x08, 0, 0},
+	{0x352a, 0x08, 0, 0},
+	{0x3602, 0x00, 0, 0},
+	{0x3603, 0x40, 0, 0},
+	{0x3604, 0x02, 0, 0},
+	{0x3605, 0x00, 0, 0},
+	{0x3606, 0x00, 0, 0},
+	{0x3607, 0x00, 0, 0},
+	{0x3609, 0x12, 0, 0},
+	{0x360a, 0x40, 0, 0},
+	{0x360c, 0x08, 0, 0},
+	{0x360f, 0xe5, 0, 0},
+	{0x3608, 0x8f, 0, 0},
+	{0x3611, 0x00, 0, 0},
+	{0x3613, 0xf7, 0, 0},
+	{0x3616, 0x58, 0, 0},
+	{0x3619, 0x99, 0, 0},
+	{0x361b, 0x60, 0, 0},
+	{0x361c, 0x7a, 0, 0},
+	{0x361e, 0x79, 0, 0},
+	{0x361f, 0x02, 0, 0},
+	{0x3632, 0x00, 0, 0},
+	{0x3633, 0x10, 0, 0},
+	{0x3634, 0x10, 0, 0},
+	{0x3635, 0x10, 0, 0},
+	{0x3636, 0x15, 0, 0},
+	{0x3646, 0x86, 0, 0},
+	{0x364a, 0x0b, 0, 0},
+	{0x3700, 0x17, 0, 0},
+	{0x3701, 0x22, 0, 0},
+	{0x3703, 0x10, 0, 0},
+	{0x370a, 0x37, 0, 0},
+	{0x3705, 0x00, 0, 0},
+	{0x3706, 0x63, 0, 0},
+	{0x3709, 0x3c, 0, 0},
+	{0x370b, 0x01, 0, 0},
+	{0x370c, 0x30, 0, 0},
+	{0x3710, 0x24, 0, 0},
+	{0x3711, 0x0c, 0, 0},
+	{0x3716, 0x00, 0, 0},
+	{0x3720, 0x28, 0, 0},
+	{0x3729, 0x7b, 0, 0},
+	{0x372a, 0x84, 0, 0},
+	{0x372b, 0xbd, 0, 0},
+	{0x372c, 0xbc, 0, 0},
+	{0x372e, 0x52, 0, 0},
+	{0x373c, 0x0e, 0, 0},
+	{0x373e, 0x33, 0, 0},
+	{0x3743, 0x10, 0, 0},
+	{0x3744, 0x88, 0, 0},
+	{0x3745, 0xc0, 0, 0},
+	{0x374a, 0x43, 0, 0},
+	{0x374c, 0x00, 0, 0},
+	{0x374e, 0x23, 0, 0},
+	{0x3751, 0x7b, 0, 0},
+	{0x3752, 0x84, 0, 0},
+	{0x3753, 0xbd, 0, 0},
+	{0x3754, 0xbc, 0, 0},
+	{0x3756, 0x52, 0, 0},
+	{0x375c, 0x00, 0, 0},
+	{0x3760, 0x00, 0, 0},
+	{0x3761, 0x00, 0, 0},
+	{0x3762, 0x00, 0, 0},
+	{0x3763, 0x00, 0, 0},
+	{0x3764, 0x00, 0, 0},
+	{0x3767, 0x04, 0, 0},
+	{0x3768, 0x04, 0, 0},
+	{0x3769, 0x08, 0, 0},
+	{0x376a, 0x08, 0, 0},
+	{0x376b, 0x20, 0, 0},
+	{0x376c, 0x00, 0, 0},
+	{0x376d, 0x00, 0, 0},
+	{0x376e, 0x00, 0, 0},
+	{0x3773, 0x00, 0, 0},
+	{0x3774, 0x51, 0, 0},
+	{0x3776, 0xbd, 0, 0},
+	{0x3777, 0xbd, 0, 0},
+	{0x3781, 0x18, 0, 0},
+	{0x3783, 0x25, 0, 0},
+	{0x3798, 0x1b, 0, 0},
+	{0x3800, 0x00, 0, 0},
+	{0x3801, 0x08, 0, 0},
+	{0x3802, 0x00, 0, 0},
+	{0x3803, 0x04, 0, 0},
+	{0x3804, 0x0a, 0, 0},
+	{0x3805, 0x97, 0, 0},
+	{0x3806, 0x05, 0, 0},
+	{0x3807, 0xfb, 0, 0},
+	{0x3808, 0x0a, 0, 0},
+	{0x3809, 0x80, 0, 0},
+	{0x380a, 0x05, 0, 0},
+	{0x380b, 0xf0, 0, 0},
+	{0x380c, 0x03, 0, 0},
+	{0x380d, 0x5c, 0, 0},
+	{0x380e, 0x06, 0, 0},
+	{0x380f, 0x12, 0, 0},
+	{0x3810, 0x00, 0, 0},
+	{0x3811, 0x08, 0, 0},
+	{0x3812, 0x00, 0, 0},
+	{0x3813, 0x04, 0, 0},
+	{0x3814, 0x01, 0, 0},
+	{0x3815, 0x01, 0, 0},
+	{0x3819, 0x01, 0, 0},
+	{0x3820, 0x00, 0, 0},
+	{0x3821, 0x06, 0, 0},
+	{0x3829, 0x00, 0, 0},
+	{0x382a, 0x01, 0, 0},
+	{0x382b, 0x01, 0, 0},
+	{0x382d, 0x7f, 0, 0},
+	{0x3830, 0x04, 0, 0},
+	{0x3836, 0x01, 0, 0},
+	{0x3837, 0x00, 0, 0},
+	{0x3841, 0x02, 0, 0},
+	{0x3846, 0x08, 0, 0},
+	{0x3847, 0x07, 0, 0},
+	{0x3d85, 0x36, 0, 0},
+	{0x3d8c, 0x71, 0, 0},
+	{0x3d8d, 0xcb, 0, 0},
+	{0x3f0a, 0x00, 0, 0},
+	{0x4000, 0x71, 0, 0},
+	{0x4001, 0x40, 0, 0},
+	{0x4002, 0x04, 0, 0},
+	{0x4003, 0x14, 0, 0},
+	{0x400e, 0x00, 0, 0},
+	{0x4011, 0x00, 0, 0},
+	{0x401a, 0x00, 0, 0},
+	{0x401b, 0x00, 0, 0},
+	{0x401c, 0x00, 0, 0},
+	{0x401d, 0x00, 0, 0},
+	{0x401f, 0x00, 0, 0},
+	{0x4020, 0x00, 0, 0},
+	{0x4021, 0x10, 0, 0},
+	{0x4022, 0x07, 0, 0},
+	{0x4023, 0xcf, 0, 0},
+	{0x4024, 0x09, 0, 0},
+	{0x4025, 0x60, 0, 0},
+	{0x4026, 0x09, 0, 0},
+	{0x4027, 0x6f, 0, 0},
+	{0x4028, 0x00, 0, 0},
+	{0x4029, 0x02, 0, 0},
+	{0x402a, 0x06, 0, 0},
+	{0x402b, 0x04, 0, 0},
+	{0x402c, 0x02, 0, 0},
+	{0x402d, 0x02, 0, 0},
+	{0x402e, 0x0e, 0, 0},
+	{0x402f, 0x04, 0, 0},
+	{0x4302, 0xff, 0, 0},
+	{0x4303, 0xff, 0, 0},
+	{0x4304, 0x00, 0, 0},
+	{0x4305, 0x00, 0, 0},
+	{0x4306, 0x00, 0, 0},
+	{0x4308, 0x02, 0, 0},
+	{0x4500, 0x6c, 0, 0},
+	{0x4501, 0xc4, 0, 0},
+	{0x4502, 0x40, 0, 0},
+	{0x4503, 0x01, 0, 0},
+	{0x4601, 0x04, 0, 0},
+	{0x4800, 0x04, 0, 0},
+	{0x4813, 0x08, 0, 0},
+	{0x481f, 0x40, 0, 0},
+	{0x4829, 0x78, 0, 0},
+	{0x4837, 0x14, 0, 0},
+	{0x4b00, 0x2a, 0, 0},
+	{0x4b0d, 0x00, 0, 0},
+	{0x4d00, 0x04, 0, 0},
+	{0x4d01, 0x42, 0, 0},
+	{0x4d02, 0xd1, 0, 0},
+	{0x4d03, 0x93, 0, 0},
+	{0x4d04, 0xf5, 0, 0},
+	{0x4d05, 0xc1, 0, 0},
+	{0x5000, 0xf3, 0, 0},
+	{0x5001, 0x11, 0, 0},
+	{0x5004, 0x00, 0, 0},
+	{0x500a, 0x00, 0, 0},
+	{0x500b, 0x00, 0, 0},
+	{0x5032, 0x00, 0, 0},
+	{0x5040, 0x00, 0, 0},
+	{0x5050, 0x0c, 0, 0},
+	{0x5500, 0x00, 0, 0},
+	{0x5501, 0x10, 0, 0},
+	{0x5502, 0x01, 0, 0},
+	{0x5503, 0x0f, 0, 0},
+	{0x8000, 0x00, 0, 0},
+	{0x8001, 0x00, 0, 0},
+	{0x8002, 0x00, 0, 0},
+	{0x8003, 0x00, 0, 0},
+	{0x8004, 0x00, 0, 0},
+	{0x8005, 0x00, 0, 0},
+	{0x8006, 0x00, 0, 0},
+	{0x8007, 0x00, 0, 0},
+	{0x8008, 0x00, 0, 0},
+	{0x3638, 0x00, 0, 0},
+	{0x380c, 0x0A, 0, 0},
+	{0x380d, 0x0A, 0, 0},
+	{0x380e, 0x06, 0, 0},
+	{0x380f, 0x12, 0, 0},
+	{0x3105, 0x31, 0, 0},
+	{0x301a, 0xf9, 0, 0},
+	{0x3508, 0x07, 0, 0},
+	{0x484b, 0x05, 0, 0},
+	{0x4805, 0x03, 0, 0},
+	{0x3601, 0x01, 0, 0},
+	{0x3745, 0xc0, 0, 0},
+	{0x3798, 0x1b, 0, 0},
+	{0xffff, 0x0a, 0, 0},
+	{0x3105, 0x11, 0, 0},
+	{0x301a, 0xf1, 0, 0},
+	{0x4805, 0x00, 0, 0},
+	{0x301a, 0xf0, 0, 0},
+	{0x3208, 0x00, 0, 0},
+	{0x302a, 0x00, 0, 0},
+	{0x302a, 0x00, 0, 0},
+	{0x302a, 0x00, 0, 0},
+	{0x302a, 0x00, 0, 0},
+	{0x302a, 0x00, 0, 0},
+	{0x3601, 0x00, 0, 0},
+	{0x3638, 0x00, 0, 0},
+	{0x3208, 0x10, 0, 0},
+	{0x3208, 0xa0, 0, 0},
+};
+
+/* power-on sensor init reg table */
+static const struct ov4689_mode_info ov4689_mode_init_data = {
+	OV4689_MODE_1080P_1920_1080, SCALING,
+	1920, 0x6e0, 1080, 0x470,
+	ov4689_init_setting_30fps_1080P,
+	ARRAY_SIZE(ov4689_init_setting_30fps_1080P),
+	OV4689_60_FPS,
+};
+
+static const struct ov4689_mode_info
+ov4689_mode_data[OV4689_NUM_MODES] = {
+	{OV4689_MODE_720P_1280_720, SUBSAMPLING,
+	 1280, 0x408, 720, 0x305,
+	 ov4689_setting_720P_1280_720,
+	 ARRAY_SIZE(ov4689_setting_720P_1280_720),
+	 OV4689_150_FPS},
+	{OV4689_MODE_1080P_1920_1080, SCALING,
+	 1920, 0x6e0, 1080, 0x470,
+	 ov4689_setting_1080P_1920_1080,
+	 ARRAY_SIZE(ov4689_setting_1080P_1920_1080),
+	 OV4689_60_FPS},
+	{OV4689_MODE_4M_2688_1520, SCALING,
+	 2688, 0xa0a, 1520, 0x612,
+	 ov4689_setting_4M_2688_1520,
+	 ARRAY_SIZE(ov4689_setting_4M_2688_1520),
+	 OV4689_60_FPS},
+};
+
+static int ov4689_write_reg(struct ov4689_dev *sensor, u16 reg, u8 val)
+{
+	struct i2c_client *client = sensor->i2c_client;
+	struct i2c_msg msg;
+	u8 buf[3];
+	int ret;
+
+	buf[0] = reg >> 8;
+	buf[1] = reg & 0xff;
+	buf[2] = val;
+
+	msg.addr = client->addr;
+	msg.flags = client->flags;
+	msg.buf = buf;
+	msg.len = sizeof(buf);
+
+	ret = i2c_transfer(client->adapter, &msg, 1);
+	if (ret < 0) {
+		dev_err(&client->dev, "%s: error: reg=%x, val=%x\n",
+			__func__, reg, val);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int ov4689_read_reg(struct ov4689_dev *sensor, u16 reg, u8 *val)
+{
+	struct i2c_client *client = sensor->i2c_client;
+	struct i2c_msg msg[2];
+	u8 buf[2];
+	int ret;
+
+	buf[0] = reg >> 8;
+	buf[1] = reg & 0xff;
+
+	msg[0].addr = client->addr;
+	msg[0].flags = client->flags;
+	msg[0].buf = buf;
+	msg[0].len = sizeof(buf);
+
+	msg[1].addr = client->addr;
+	msg[1].flags = client->flags | I2C_M_RD;
+	msg[1].buf = buf;
+	msg[1].len = 1;
+
+	ret = i2c_transfer(client->adapter, msg, 2);
+	if (ret < 0) {
+		dev_err(&client->dev, "%s: error: reg=%x\n",__func__, reg);
+		return ret;
+	}
+
+	*val = buf[0];
+	return 0;
+}
+
+static int ov4689_read_reg16(struct ov4689_dev *sensor, u16 reg, u16 *val)
+{
+	u8 hi, lo;
+	int ret;
+
+	ret = ov4689_read_reg(sensor, reg, &hi);
+	if (ret)
+		return ret;
+	ret = ov4689_read_reg(sensor, reg + 1, &lo);
+	if (ret)
+		return ret;
+
+	*val = ((u16)hi << 8) | (u16)lo;
+	return 0;
+}
+
+static int ov4689_write_reg16(struct ov4689_dev *sensor, u16 reg, u16 val)
+{
+	int ret;
+
+	ret = ov4689_write_reg(sensor, reg, val >> 8);
+	if (ret)
+		return ret;
+
+	return ov4689_write_reg(sensor, reg + 1, val & 0xff);
+}
+
+static int ov4689_mod_reg(struct ov4689_dev *sensor, u16 reg, u8 mask, u8 val)
+{
+	u8 readval;
+	int ret;
+
+	ret = ov4689_read_reg(sensor, reg, &readval);
+	if (ret)
+		return ret;
+
+	readval &= ~mask;
+	val &= mask;
+	val |= readval;
+
+	return ov4689_write_reg(sensor, reg, val);
+}
+
+static int ov4689_set_timings(struct ov4689_dev *sensor,
+			const struct ov4689_mode_info *mode)
+{
+	int ret;
+
+	return 0;
+}
+
+static int ov4689_load_regs(struct ov4689_dev *sensor,
+			const struct ov4689_mode_info *mode)
+{
+	const struct reg_value *regs = mode->reg_data;
+	unsigned int i;
+	u32 delay_ms;
+	u16 reg_addr;
+	u8 mask, val;
+	int ret = 0;
+
+	st_info(ST_SENSOR, "%s, mode = 0x%x\n", __func__, mode->id);
+	for (i = 0; i < mode->reg_data_size; ++i, ++regs) {
+		delay_ms = regs->delay_ms;
+		reg_addr = regs->reg_addr;
+		val = regs->val;
+		mask = regs->mask;
+
+		if (mask)
+			ret = ov4689_mod_reg(sensor, reg_addr, mask, val);
+		else
+			ret = ov4689_write_reg(sensor, reg_addr, val);
+		if (ret)
+			break;
+
+		if (delay_ms)
+			usleep_range(1000 * delay_ms, 1000 * delay_ms + 100);
+	}
+
+	return ov4689_set_timings(sensor, mode);
+}
+
+static int ov4689_get_exposure(struct ov4689_dev *sensor)
+{
+	int exp, ret;
+	u8 temp;
+
+	ret = ov4689_read_reg(sensor, OV4689_REG_EXPOSURE_HI, &temp);
+	if (ret)
+		return ret;
+	exp = ((int)temp & 0x0f) << 16;
+	ret = ov4689_read_reg(sensor, OV4689_REG_EXPOSURE_MED, &temp);
+	if (ret)
+		return ret;
+	exp |= ((int)temp << 8);
+	ret = ov4689_read_reg(sensor, OV4689_REG_EXPOSURE_LO, &temp);
+	if (ret)
+		return ret;
+	exp |= (int)temp;
+
+	return exp >> 4;
+}
+
+static int ov4689_set_exposure(struct ov4689_dev *sensor, u32 exposure)
+{
+	int ret;
+
+	st_info(ST_SENSOR, "%s, exposure = 0x%x\n", __func__, exposure);
+	exposure <<= 4;
+
+	ret = ov4689_write_reg(sensor, OV4689_REG_EXPOSURE_LO, exposure & 0xff);
+	if (ret)
+		return ret;
+	ret = ov4689_write_reg(sensor, OV4689_REG_EXPOSURE_MED,
+			(exposure >> 8) & 0xff);
+	if (ret)
+		return ret;
+	return ov4689_write_reg(sensor, OV4689_REG_EXPOSURE_HI,
+				(exposure >> 16) & 0x0f);
+}
+
+static int ov4689_get_gain(struct ov4689_dev *sensor)
+{
+	u32 gain = 0;
+	u8 val;
+
+	ov4689_read_reg(sensor, OV4689_REG_GAIN_H, &val);
+	gain = (val & 0x3) << 16;
+	ov4689_read_reg(sensor, OV4689_REG_GAIN_M, &val);
+	gain |= val << 8;
+	ov4689_read_reg(sensor, OV4689_REG_GAIN_L, &val);
+	gain |= val;
+
+	return gain;
+}
+
+static int ov4689_set_gain(struct ov4689_dev *sensor, int gain)
+{
+	u8 val;
+
+	ov4689_write_reg(sensor, OV4689_REG_GAIN_H, (gain >> 16) & 0x3);
+	ov4689_write_reg(sensor, OV4689_REG_GAIN_M, (gain >> 8) & 0xff);
+	ov4689_write_reg(sensor, OV4689_REG_GAIN_L, gain & 0xff);
+	return 0;
+}
+
+static int ov4689_set_stream_mipi(struct ov4689_dev *sensor, bool on)
+{
+	return 0;
+}
+
+static int ov4689_get_sysclk(struct ov4689_dev *sensor)
+{
+	return 0;
+}
+
+static int ov4689_set_night_mode(struct ov4689_dev *sensor)
+{
+	return 0;
+}
+
+static int ov4689_get_hts(struct ov4689_dev *sensor)
+{
+	/* read HTS from register settings */
+	u16 hts;
+	int ret;
+
+	ret = ov4689_read_reg16(sensor, OV4689_REG_TIMING_HTS, &hts);
+	if (ret)
+		return ret;
+	return hts;
+}
+
+static int ov4689_get_vts(struct ov4689_dev *sensor)
+{
+	u16 vts;
+	int ret;
+
+	ret = ov4689_read_reg16(sensor, OV4689_REG_TIMING_VTS, &vts);
+	if (ret)
+		return ret;
+	return vts;
+}
+
+static int ov4689_set_vts(struct ov4689_dev *sensor, int vts)
+{
+	return ov4689_write_reg16(sensor, OV4689_REG_TIMING_VTS, vts);
+}
+
+static int ov4689_get_light_freq(struct ov4689_dev *sensor)
+{
+	return 0;
+}
+
+static int ov4689_set_bandingfilter(struct ov4689_dev *sensor)
+{
+	return 0;
+}
+
+static int ov4689_set_ae_target(struct ov4689_dev *sensor, int target)
+{
+	return 0;
+}
+
+static int ov4689_get_binning(struct ov4689_dev *sensor)
+{
+	return 0;
+}
+
+static int ov4689_set_binning(struct ov4689_dev *sensor, bool enable)
+{
+	return 0;
+}
+
+static const struct ov4689_mode_info *
+ov4689_find_mode(struct ov4689_dev *sensor, enum ov4689_frame_rate fr,
+		int width, int height, bool nearest)
+{
+	const struct ov4689_mode_info *mode;
+
+	mode = v4l2_find_nearest_size(ov4689_mode_data,
+		    ARRAY_SIZE(ov4689_mode_data), hact, vact, width, height);
+
+	if (!mode ||
+		(!nearest && (mode->hact != width || mode->vact != height)))
+		return NULL;
+
+	/* Check to see if the current mode exceeds the max frame rate */
+	if (ov4689_framerates[fr] > ov4689_framerates[mode->max_fps])
+		return NULL;
+
+	return mode;
+}
+
+static u64 ov4689_calc_pixel_rate(struct ov4689_dev *sensor)
+{
+	u64 rate;
+
+	rate = sensor->current_mode->vact * sensor->current_mode->hact;
+	rate *= ov4689_framerates[sensor->current_fr];
+
+	return rate;
+}
+
+/*
+ * After trying the various combinations, reading various
+ * documentations spread around the net, and from the various
+ * feedback, the clock tree is probably as follows:
+ *
+ *   +--------------+
+ *   |  Ext. Clock  |
+ *   +-+------------+
+ *     |  +----------+
+ *     +->|   PLL1   | - reg 0x030a, bit0 for the pre-dividerp
+ *        +-+--------+ - reg 0x0300, bits 0-2 for the pre-divider
+ *        +-+--------+ - reg 0x0301~0x0302, for the multiplier
+ *          |  +--------------+
+ *          +->| MIPI Divider |  - reg 0x0303, bits 0-3 for the pre-divider
+ *               | +---------> MIPI PHY CLK
+ *               |    +-----+
+ *               | +->| PLL1_DIV_MIPI | - reg 0x0304, bits 0-1 for the divider
+ *                 |    +----------------> PCLK
+ *               |    +-----+
+ *
+ *   +--------------+
+ *   |  Ext. Clock  |
+ *   +-+------------+
+ *     |  +----------+
+ *     +->|   PLL2  | - reg 0x0311, bit0 for the pre-dividerp
+ *        +-+--------+ - reg 0x030b, bits 0-2 for the pre-divider
+ *        +-+--------+ - reg 0x030c~0x030d, for the multiplier
+ *          |  +--------------+
+ *          +->| SCLK Divider |  - reg 0x030F, bits 0-3 for the pre-divider
+ *               +-+--------+    - reg 0x030E, bits 0-2 for the divider
+ *               |    +---------> SCLK
+ *
+ *          |       +-----+
+ *          +->| DAC Divider | - reg 0x0312, bits 0-3 for the divider
+ *                    |    +----------------> DACCLK
+ **
+ */
+
+/*
+ * ov4689_set_mipi_pclk() - Calculate the clock tree configuration values
+ *			for the MIPI CSI-2 output.
+ *
+ * @rate: The requested bandwidth per lane in bytes per second.
+ *	'Bandwidth Per Lane' is calculated as:
+ *	bpl = HTOT * VTOT * FPS * bpp / num_lanes;
+ *
+ * This function use the requested bandwidth to calculate:
+ *
+ * - mipi_pclk   = bpl / 2; ( / 2 is for CSI-2 DDR)
+ * - mipi_phy_clk   = mipi_pclk * PLL1_DIV_MIPI;
+ *
+ * with these fixed parameters:
+ *	PLL1_PREDIVP    = 1;
+ *	PLL1_PREDIV     = 1; (MIPI_BIT_MODE == 8 ? 2 : 2,5);
+ *	PLL1_DIVM       = 1;
+ *	PLL1_DIV_MIPI   = 4;
+ *
+ * FIXME: this have been tested with 10-bit raw and 2 lanes setup only.
+ * MIPI_DIV is fixed to value 2, but it -might- be changed according to the
+ * above formula for setups with 1 lane or image formats with different bpp.
+ *
+ * FIXME: this deviates from the sensor manual documentation which is quite
+ * thin on the MIPI clock tree generation part.
+ */
+
+#define PLL1_PREDIVP         1     // bypass
+#define PLL1_PREDIV          1     // bypass
+#define PLL1_DIVM            1  // bypass
+#define PLL1_DIV_MIPI        3  // div
+#define PLL1_DIV_MIPI_BASE   1  // div
+
+#define PLL1_DIVSP    1   // no use
+#define PLL1_DIVS     1   // no use
+
+#define PLL2_PREDIVP       0
+#define PLL2_PREDIV        0
+#define PLL2_DIVSP       1
+#define PLL2_DIVS        4
+#define PLL2_DIVDAC      1
+
+#define OV4689_PLL1_PREDIVP         0x030a   // bits[0]
+#define OV4689_PLL1_PREDIV          0x0300   // bits[2:0]
+#define OV4689_PLL1_MULTIPLIER      0x0301   // bits[9:8]  0x0302 bits[7:0]
+#define OV4689_PLL1_DIVM            0x0303   // bits[3:0]
+#define OV4689_PLL1_DIV_MIPI        0x0304   // bits[1:0]
+
+#define OV4689_PLL1_DIVSP           0x0305   //bits[1:0]
+#define OV4689_PLL1_DIVS            0x0306   // bits[0]
+
+#define OV4689_PLL2_PREDIVP         0x0311   // bits[0]
+#define OV4689_PLL2_PREDIV          0x030b   // bits[2:0]
+#define OV4689_PLL2_MULTIPLIER      0x030c   // bits[9:8]   0x030d bits[7:0]
+#define OV4689_PLL2_DIVSP           0x030f  // bits[3:0]
+#define OV4689_PLL2_DIVS            0x030e  // bits[2:0]
+#define OV4689_PLL2_DIVDAC          0x0312  // bits[3:0]
+
+#define OV4689_TIMING_HTS           0x380c
+
+static int ov4689_set_mipi_pclk(struct ov4689_dev *sensor,
+				unsigned long rate)
+{
+	const struct ov4689_mode_info *mode = sensor->current_mode;
+	const struct ov4689_mode_info *orig_mode = sensor->last_mode;
+	u8 mult, val;
+	int ret = 0;
+	int fps = ov4689_framerates[sensor->current_fr];
+	u16 htot, val16;
+
+	htot = mode->htot * ov4689_framerates[mode->max_fps] / fps;
+
+	ret = ov4689_write_reg16(sensor, OV4689_TIMING_HTS, htot);
+
+	ret = ov4689_read_reg(sensor, OV4689_TIMING_HTS, &val);
+	val16 = val << 8;
+	ret = ov4689_read_reg(sensor, OV4689_TIMING_HTS + 1, &val);
+	val16 |= val;
+	st_info(ST_SENSOR, "fps = %d, max_fps = %d, mode->htot = 0x%x, "
+			"htot = 0x%x, 0x%x = 0x%x\n",
+			fps, mode->max_fps, mode->htot,
+			htot, OV4689_TIMING_HTS, val16);
+	return 0;
+}
+
+/*
+ * if sensor changes inside scaling or subsampling
+ * change mode directly
+ */
+static int ov4689_set_mode_direct(struct ov4689_dev *sensor,
+				const struct ov4689_mode_info *mode)
+{
+	if (!mode->reg_data)
+		return -EINVAL;
+
+	/* Write capture setting */
+	return ov4689_load_regs(sensor, mode);
+}
+
+static int ov4689_set_mode(struct ov4689_dev *sensor)
+{
+	const struct ov4689_mode_info *mode = sensor->current_mode;
+	const struct ov4689_mode_info *orig_mode = sensor->last_mode;
+	int ret = 0;
+
+	ret = ov4689_set_mode_direct(sensor, mode);
+	if (ret < 0)
+		return ret;
+
+	/*
+	 * we support have 10 bits raw RGB(mipi)
+	 */
+	if (sensor->ep.bus_type == V4L2_MBUS_CSI2_DPHY)
+		ret = ov4689_set_mipi_pclk(sensor, 0);
+
+	if (ret < 0)
+		return 0;
+
+	sensor->pending_mode_change = false;
+	sensor->last_mode = mode;
+	return 0;
+}
+
+/* restore the last set video mode after chip power-on */
+static int ov4689_restore_mode(struct ov4689_dev *sensor)
+{
+	int ret;
+
+	/* first load the initial register values */
+	ret = ov4689_load_regs(sensor, &ov4689_mode_init_data);
+	if (ret < 0)
+		return ret;
+	sensor->last_mode = &ov4689_mode_init_data;
+
+	/* now restore the last capture mode */
+	ret = ov4689_set_mode(sensor);
+	if (ret < 0)
+		return ret;
+
+	return ret;
+}
+
+static void ov4689_power(struct ov4689_dev *sensor, bool enable)
+{
+	if (!sensor->pwdn_gpio)
+		return;
+	gpiod_set_value_cansleep(sensor->pwdn_gpio, enable ? 0 : 1);
+}
+
+static void ov4689_reset(struct ov4689_dev *sensor)
+{
+	if (!sensor->reset_gpio)
+		return;
+
+	gpiod_set_value_cansleep(sensor->reset_gpio, 0);
+
+	usleep_range(5000, 25000);
+
+	gpiod_set_value_cansleep(sensor->reset_gpio, 1);
+	usleep_range(1000, 2000);
+}
+
+static int ov4689_set_power_on(struct ov4689_dev *sensor)
+{
+	struct i2c_client *client = sensor->i2c_client;
+	int ret;
+
+	ret = clk_prepare_enable(sensor->xclk);
+	if (ret) {
+		dev_err(&client->dev, "%s: failed to enable clock\n", __func__);
+		return ret;
+	}
+
+	ret = regulator_bulk_enable(OV4689_NUM_SUPPLIES,
+				sensor->supplies);
+	if (ret) {
+		dev_err(&client->dev, "%s: failed to enable regulators\n", __func__);
+		goto xclk_off;
+	}
+
+	ov4689_reset(sensor);
+	ov4689_power(sensor, true);
+
+	return 0;
+
+xclk_off:
+	clk_disable_unprepare(sensor->xclk);
+	return ret;
+}
+
+static void ov4689_set_power_off(struct ov4689_dev *sensor)
+{
+	ov4689_power(sensor, false);
+	regulator_bulk_disable(OV4689_NUM_SUPPLIES, sensor->supplies);
+	clk_disable_unprepare(sensor->xclk);
+}
+
+static int ov4689_set_power_mipi(struct ov4689_dev *sensor, bool on)
+{
+	return 0;
+}
+
+static int ov4689_set_power(struct ov4689_dev *sensor, bool on)
+{
+	int ret = 0;
+	u16 chip_id;
+
+	if (on) {
+		ret = ov4689_set_power_on(sensor);
+		if (ret)
+			return ret;
+
+		ret = ov4689_read_reg16(sensor, OV4689_REG_CHIP_ID, &chip_id);
+		if (ret) {
+			dev_err(&sensor->i2c_client->dev, "%s: failed to read chip identifier\n",
+				__func__);
+			ret = -ENODEV;
+			goto power_off;
+		}
+
+		if (chip_id != OV4689_CHIP_ID) {
+			dev_err(&sensor->i2c_client->dev,
+					"%s: wrong chip identifier, expected 0x%x, got 0x%x\n",
+					__func__, OV4689_CHIP_ID, chip_id);
+			ret = -ENXIO;
+			goto power_off;
+		}
+		dev_err(&sensor->i2c_client->dev, "%s: chip identifier, got 0x%x\n",
+			__func__, chip_id);
+
+		ret = ov4689_restore_mode(sensor);
+		if (ret)
+			goto power_off;
+	}
+
+	if (sensor->ep.bus_type == V4L2_MBUS_CSI2_DPHY)
+		ret = ov4689_set_power_mipi(sensor, on);
+	if (ret)
+		goto power_off;
+
+	if (!on)
+		ov4689_set_power_off(sensor);
+
+	return 0;
+
+power_off:
+	ov4689_set_power_off(sensor);
+	return ret;
+}
+
+static int ov4689_s_power(struct v4l2_subdev *sd, int on)
+{
+	struct ov4689_dev *sensor = to_ov4689_dev(sd);
+	int ret = 0;
+
+	mutex_lock(&sensor->lock);
+
+	/*
+	 * If the power count is modified from 0 to != 0 or from != 0 to 0,
+	 * update the power state.
+	 */
+	if (sensor->power_count == !on) {
+		ret = ov4689_set_power(sensor, !!on);
+		if (ret)
+			goto out;
+	}
+
+	/* Update the power count. */
+	sensor->power_count += on ? 1 : -1;
+	WARN_ON(sensor->power_count < 0);
+out:
+	mutex_unlock(&sensor->lock);
+
+	if (on && !ret && sensor->power_count == 1) {
+		/* restore controls */
+		ret = v4l2_ctrl_handler_setup(&sensor->ctrls.handler);
+	}
+
+	return ret;
+}
+
+static int ov4689_try_frame_interval(struct ov4689_dev *sensor,
+				struct v4l2_fract *fi,
+				u32 width, u32 height)
+{
+	const struct ov4689_mode_info *mode;
+	enum ov4689_frame_rate rate = OV4689_15_FPS;
+	int minfps, maxfps, best_fps, fps;
+	int i;
+
+	minfps = ov4689_framerates[OV4689_15_FPS];
+	maxfps = ov4689_framerates[OV4689_NUM_FRAMERATES - 1];
+
+	if (fi->numerator == 0) {
+		fi->denominator = maxfps;
+		fi->numerator = 1;
+		rate = OV4689_60_FPS;
+		goto find_mode;
+	}
+
+	fps = clamp_val(DIV_ROUND_CLOSEST(fi->denominator, fi->numerator),
+			minfps, maxfps);
+
+	best_fps = minfps;
+	for (i = 0; i < ARRAY_SIZE(ov4689_framerates); i++) {
+		int curr_fps = ov4689_framerates[i];
+		if (abs(curr_fps - fps) < abs(best_fps - fps)) {
+			best_fps = curr_fps;
+			rate = i;
+		}
+	}
+	st_info(ST_SENSOR, "best_fps = %d, fps = %d\n", best_fps, fps);
+
+	fi->numerator = 1;
+	fi->denominator = best_fps;
+
+find_mode:
+	mode = ov4689_find_mode(sensor, rate, width, height, false);
+	return mode ? rate : -EINVAL;
+}
+
+static int ov4689_enum_mbus_code(struct v4l2_subdev *sd,
+				struct v4l2_subdev_pad_config *cfg,
+				struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->pad != 0)
+		return -EINVAL;
+
+	if (code->index)
+		return -EINVAL;
+
+	code->code = MEDIA_BUS_FMT_SBGGR10_1X10;
+	return 0;
+}
+
+static int ov4689_get_fmt(struct v4l2_subdev *sd,
+			struct v4l2_subdev_pad_config *cfg,
+			struct v4l2_subdev_format *format)
+{
+	struct ov4689_dev *sensor = to_ov4689_dev(sd);
+	struct v4l2_mbus_framefmt *fmt;
+
+	if (format->pad != 0)
+		return -EINVAL;
+
+	mutex_lock(&sensor->lock);
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY)
+		fmt = v4l2_subdev_get_try_format(&sensor->sd, cfg, format->pad);
+	else
+		fmt = &sensor->fmt;
+
+	format->format = *fmt;
+
+	mutex_unlock(&sensor->lock);
+
+	return 0;
+}
+
+static int ov4689_try_fmt_internal(struct v4l2_subdev *sd,
+				struct v4l2_mbus_framefmt *fmt,
+				enum ov4689_frame_rate fr,
+				const struct ov4689_mode_info **new_mode)
+{
+	struct ov4689_dev *sensor = to_ov4689_dev(sd);
+	const struct ov4689_mode_info *mode;
+	int i;
+
+	mode = ov4689_find_mode(sensor, fr, fmt->width, fmt->height, true);
+	if (!mode)
+		return -EINVAL;
+	fmt->width = mode->hact;
+	fmt->height = mode->vact;
+
+	if (new_mode)
+		*new_mode = mode;
+
+	fmt->code = MEDIA_BUS_FMT_SBGGR10_1X10;
+
+	return 0;
+}
+
+static int ov4689_set_fmt(struct v4l2_subdev *sd,
+			struct v4l2_subdev_pad_config *cfg,
+			struct v4l2_subdev_format *format)
+{
+	struct ov4689_dev *sensor = to_ov4689_dev(sd);
+	const struct ov4689_mode_info *new_mode;
+	struct v4l2_mbus_framefmt *mbus_fmt = &format->format;
+	struct v4l2_mbus_framefmt *fmt;
+	int ret;
+
+	if (format->pad != 0)
+		return -EINVAL;
+
+	mutex_lock(&sensor->lock);
+
+	if (sensor->streaming) {
+		ret = -EBUSY;
+		goto out;
+	}
+
+	ret = ov4689_try_fmt_internal(sd, mbus_fmt, 0, &new_mode);
+	if (ret)
+		goto out;
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY)
+		fmt = v4l2_subdev_get_try_format(sd, cfg, 0);
+	else
+		fmt = &sensor->fmt;
+
+	*fmt = *mbus_fmt;
+
+	if (new_mode != sensor->current_mode) {
+		sensor->current_mode = new_mode;
+		sensor->pending_mode_change = true;
+	}
+	if (new_mode->max_fps < sensor->current_fr) {
+		sensor->current_fr = new_mode->max_fps;
+		sensor->frame_interval.numerator = 1;
+		sensor->frame_interval.denominator =
+			ov4689_framerates[sensor->current_fr];
+		sensor->current_mode = new_mode;
+		sensor->pending_mode_change = true;
+	}
+
+	__v4l2_ctrl_s_ctrl_int64(sensor->ctrls.pixel_rate,
+				ov4689_calc_pixel_rate(sensor));
+out:
+	mutex_unlock(&sensor->lock);
+	return ret;
+}
+
+/*
+ * Sensor Controls.
+ */
+
+static int ov4689_set_ctrl_hue(struct ov4689_dev *sensor, int value)
+{
+	int ret = 0;
+
+	return ret;
+}
+
+static int ov4689_set_ctrl_contrast(struct ov4689_dev *sensor, int value)
+{
+	int ret = 0;
+
+	return ret;
+}
+
+static int ov4689_set_ctrl_saturation(struct ov4689_dev *sensor, int value)
+{
+	int ret  = 0;
+
+	return ret;
+}
+
+static int ov4689_set_ctrl_white_balance(struct ov4689_dev *sensor, int awb)
+{
+	struct ov4689_ctrls *ctrls = &sensor->ctrls;
+	int ret = 0;
+
+	if (!awb && (ctrls->red_balance->is_new
+			|| ctrls->blue_balance->is_new)) {
+		u16 red = (u16)ctrls->red_balance->val;
+		u16 blue = (u16)ctrls->blue_balance->val;
+
+		st_info(ST_SENSOR, "red = 0x%x, blue = 0x%x\n", red, blue);
+		ret = ov4689_write_reg16(sensor, OV4689_REG_AWB_R_GAIN, red);
+		if (ret)
+			return ret;
+		ret = ov4689_write_reg16(sensor, OV4689_REG_AWB_B_GAIN, blue);
+	}
+	return ret;
+}
+
+static int ov4689_set_ctrl_exposure(struct ov4689_dev *sensor,
+				enum v4l2_exposure_auto_type auto_exposure)
+{
+	struct ov4689_ctrls *ctrls = &sensor->ctrls;
+	bool auto_exp = (auto_exposure == V4L2_EXPOSURE_AUTO);
+	int ret = 0;
+
+	if (!auto_exp && ctrls->exposure->is_new) {
+		u16 max_exp = 0;
+
+		ret = ov4689_read_reg16(sensor, OV4689_REG_V_OUTPUT_SIZE, &max_exp);
+
+		ret = ov4689_get_vts(sensor);
+		if (ret < 0)
+			return ret;
+		max_exp += ret;
+		ret = 0;
+
+		st_info(ST_SENSOR, "%s, max_exp = 0x%x\n", __func__, max_exp);
+		if (ctrls->exposure->val < max_exp)
+			ret = ov4689_set_exposure(sensor, ctrls->exposure->val);
+	}
+
+	return ret;
+}
+
+static const s64 link_freq_menu_items[] = {
+	OV4689_LINK_FREQ_500MHZ
+};
+
+static const char * const test_pattern_menu[] = {
+	"Disabled",
+	"Color bars",
+	"Color bars w/ rolling bar",
+	"Color squares",
+	"Color squares w/ rolling bar",
+};
+
+#define OV4689_TEST_ENABLE		BIT(7)
+#define OV4689_TEST_ROLLING		BIT(6)  /* rolling horizontal bar */
+#define OV4689_TEST_TRANSPARENT		BIT(5)
+#define OV4689_TEST_SQUARE_BW		BIT(4)  /* black & white squares */
+#define OV4689_TEST_BAR_STANDARD	(0 << 2)
+#define OV4689_TEST_BAR_DARKER_1	(1 << 2)
+#define OV4689_TEST_BAR_DARKER_2	(2 << 2)
+#define OV4689_TEST_BAR_DARKER_3	(3 << 2)
+#define OV4689_TEST_BAR			(0 << 0)
+#define OV4689_TEST_RANDOM		(1 << 0)
+#define OV4689_TEST_SQUARE		(2 << 0)
+#define OV4689_TEST_BLACK		(3 << 0)
+
+static const u8 test_pattern_val[] = {
+	0,
+	OV4689_TEST_ENABLE | OV4689_TEST_BAR_STANDARD |
+		OV4689_TEST_BAR,
+	OV4689_TEST_ENABLE | OV4689_TEST_ROLLING |
+		OV4689_TEST_BAR_DARKER_1 | OV4689_TEST_BAR,
+	OV4689_TEST_ENABLE | OV4689_TEST_SQUARE,
+	OV4689_TEST_ENABLE | OV4689_TEST_ROLLING | OV4689_TEST_SQUARE,
+};
+
+static int ov4689_set_ctrl_test_pattern(struct ov4689_dev *sensor, int value)
+{
+	return ov4689_write_reg(sensor, OV4689_REG_TEST_PATTERN, test_pattern_val[value]);
+}
+
+static int ov4689_set_ctrl_light_freq(struct ov4689_dev *sensor, int value)
+{
+	return 0;
+}
+
+static int ov4689_set_ctrl_hflip(struct ov4689_dev *sensor, int value)
+{
+	/*
+	 * TIMING TC REG21:
+	 * - [2]:       Digital mirror
+	 * - [1]:       Array mirror
+	 */
+	return ov4689_mod_reg(sensor, OV4689_REG_TIMING_TC_REG21, BIT(2) | BIT(1),
+			(!(value ^ sensor->upside_down)) ? (BIT(2) | BIT(1)) : 0);
+}
+
+static int ov4689_set_ctrl_vflip(struct ov4689_dev *sensor, int value)
+{
+	/*
+	 * TIMING TC REG20:
+	 * - [2]:       Digital vflip
+	 * - [1]:       Array vflip
+	 */
+	return ov4689_mod_reg(sensor, OV4689_REG_TIMING_TC_REG20, BIT(2) | BIT(1),
+				(value ^ sensor->upside_down) ? (BIT(2) | BIT(1)) : 0);
+}
+
+static int ov4689_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
+	struct ov4689_dev *sensor = to_ov4689_dev(sd);
+	int val;
+
+	/* v4l2_ctrl_lock() locks our own mutex */
+
+	switch (ctrl->id) {
+	case V4L2_CID_ANALOGUE_GAIN:
+		val = ov4689_get_gain(sensor);
+		break;
+	}
+
+	return 0;
+}
+
+static int ov4689_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
+	struct ov4689_dev *sensor = to_ov4689_dev(sd);
+	int ret;
+
+	/* v4l2_ctrl_lock() locks our own mutex */
+
+	/*
+	 * If the device is not powered up by the host driver do
+	 * not apply any controls to H/W at this time. Instead
+	 * the controls will be restored right after power-up.
+	 */
+	if (sensor->power_count == 0)
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_ANALOGUE_GAIN:
+		ret = ov4689_set_gain(sensor, ctrl->val);
+		break;
+	case V4L2_CID_EXPOSURE:
+		ret = ov4689_set_ctrl_exposure(sensor, V4L2_EXPOSURE_MANUAL);
+		break;
+	case V4L2_CID_AUTO_WHITE_BALANCE:
+		ret = ov4689_set_ctrl_white_balance(sensor, ctrl->val);
+		break;
+	case V4L2_CID_HUE:
+		ret = ov4689_set_ctrl_hue(sensor, ctrl->val);
+		break;
+	case V4L2_CID_CONTRAST:
+		ret = ov4689_set_ctrl_contrast(sensor, ctrl->val);
+		break;
+	case V4L2_CID_SATURATION:
+		ret = ov4689_set_ctrl_saturation(sensor, ctrl->val);
+		break;
+	case V4L2_CID_TEST_PATTERN:
+		ret = ov4689_set_ctrl_test_pattern(sensor, ctrl->val);
+		break;
+	case V4L2_CID_POWER_LINE_FREQUENCY:
+		ret = ov4689_set_ctrl_light_freq(sensor, ctrl->val);
+		break;
+	case V4L2_CID_HFLIP:
+		ret = ov4689_set_ctrl_hflip(sensor, ctrl->val);
+		break;
+	case V4L2_CID_VFLIP:
+		ret = ov4689_set_ctrl_vflip(sensor, ctrl->val);
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops ov4689_ctrl_ops = {
+	.g_volatile_ctrl = ov4689_g_volatile_ctrl,
+	.s_ctrl = ov4689_s_ctrl,
+};
+
+static int ov4689_init_controls(struct ov4689_dev *sensor)
+{
+	const struct v4l2_ctrl_ops *ops = &ov4689_ctrl_ops;
+	struct ov4689_ctrls *ctrls = &sensor->ctrls;
+	struct v4l2_ctrl_handler *hdl = &ctrls->handler;
+	int ret;
+
+	v4l2_ctrl_handler_init(hdl, 32);
+
+	/* we can use our own mutex for the ctrl lock */
+	hdl->lock = &sensor->lock;
+
+	/* Clock related controls */
+	ctrls->pixel_rate = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_PIXEL_RATE,
+					0, INT_MAX, 1,
+					ov4689_calc_pixel_rate(sensor));
+
+	/* Auto/manual white balance */
+	ctrls->auto_wb = v4l2_ctrl_new_std(hdl, ops,
+					V4L2_CID_AUTO_WHITE_BALANCE,
+					0, 1, 1, 0);
+	ctrls->blue_balance = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_BLUE_BALANCE,
+						0, 4095, 1, 1024);
+	ctrls->red_balance = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_RED_BALANCE,
+						0, 4095, 1, 1024);
+
+	ctrls->exposure = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_EXPOSURE,
+					4, 0xfff8, 1, 0x4c00);
+	ctrls->anal_gain = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_ANALOGUE_GAIN,
+					0x10, 0xfff8, 1, 0x0080);
+	ctrls->test_pattern =
+		v4l2_ctrl_new_std_menu_items(hdl, ops, V4L2_CID_TEST_PATTERN,
+					ARRAY_SIZE(test_pattern_menu) - 1,
+					0, 0, test_pattern_menu);
+	ctrls->hflip = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HFLIP,
+					0, 1, 1, 0);
+	ctrls->vflip = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VFLIP,
+					0, 1, 1, 0);
+	ctrls->light_freq = v4l2_ctrl_new_std_menu(hdl, ops,
+					V4L2_CID_POWER_LINE_FREQUENCY,
+					V4L2_CID_POWER_LINE_FREQUENCY_AUTO, 0,
+					V4L2_CID_POWER_LINE_FREQUENCY_50HZ);
+	ctrls->link_freq = v4l2_ctrl_new_int_menu(hdl, ops, V4L2_CID_LINK_FREQ,
+					0, 0, link_freq_menu_items);
+	if (hdl->error) {
+		ret = hdl->error;
+		goto free_ctrls;
+	}
+
+	ctrls->pixel_rate->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+	ctrls->link_freq->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+	// ctrls->exposure->flags |= V4L2_CTRL_FLAG_VOLATILE;
+	// ctrls->anal_gain->flags |= V4L2_CTRL_FLAG_VOLATILE;
+
+	v4l2_ctrl_auto_cluster(3, &ctrls->auto_wb, 0, false);
+
+	sensor->sd.ctrl_handler = hdl;
+	return 0;
+
+free_ctrls:
+	v4l2_ctrl_handler_free(hdl);
+	return ret;
+}
+
+static int ov4689_enum_frame_size(struct v4l2_subdev *sd,
+				struct v4l2_subdev_pad_config *cfg,
+				struct v4l2_subdev_frame_size_enum *fse)
+{
+	if (fse->pad != 0)
+		return -EINVAL;
+	if (fse->index >= OV4689_NUM_MODES)
+		return -EINVAL;
+
+	fse->min_width =
+		ov4689_mode_data[fse->index].hact;
+	fse->max_width = fse->min_width;
+	fse->min_height =
+		ov4689_mode_data[fse->index].vact;
+	fse->max_height = fse->min_height;
+
+	return 0;
+}
+
+static int ov4689_enum_frame_interval(
+	struct v4l2_subdev *sd,
+	struct v4l2_subdev_pad_config *cfg,
+	struct v4l2_subdev_frame_interval_enum *fie)
+{
+	struct ov4689_dev *sensor = to_ov4689_dev(sd);
+	struct v4l2_fract tpf;
+	int ret;
+
+	if (fie->pad != 0)
+		return -EINVAL;
+	if (fie->index >= OV4689_NUM_FRAMERATES)
+		return -EINVAL;
+
+	tpf.numerator = 1;
+	tpf.denominator = ov4689_framerates[fie->index];
+
+	ret = ov4689_try_frame_interval(sensor, &tpf,
+					fie->width, fie->height);
+	if (ret < 0)
+		return -EINVAL;
+
+	fie->interval = tpf;
+	return 0;
+}
+
+static int ov4689_g_frame_interval(struct v4l2_subdev *sd,
+				struct v4l2_subdev_frame_interval *fi)
+{
+	struct ov4689_dev *sensor = to_ov4689_dev(sd);
+
+	mutex_lock(&sensor->lock);
+	fi->interval = sensor->frame_interval;
+	mutex_unlock(&sensor->lock);
+
+	return 0;
+}
+
+static int ov4689_s_frame_interval(struct v4l2_subdev *sd,
+				struct v4l2_subdev_frame_interval *fi)
+{
+	struct ov4689_dev *sensor = to_ov4689_dev(sd);
+	const struct ov4689_mode_info *mode;
+	int frame_rate, ret = 0;
+
+	if (fi->pad != 0)
+		return -EINVAL;
+
+	mutex_lock(&sensor->lock);
+
+	if (sensor->streaming) {
+		ret = -EBUSY;
+		goto out;
+	}
+
+	mode = sensor->current_mode;
+
+	frame_rate = ov4689_try_frame_interval(sensor, &fi->interval,
+					mode->hact, mode->vact);
+	if (frame_rate < 0) {
+		/* Always return a valid frame interval value */
+		fi->interval = sensor->frame_interval;
+		goto out;
+	}
+
+	mode = ov4689_find_mode(sensor, frame_rate, mode->hact,
+				mode->vact, true);
+	if (!mode) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	if (mode != sensor->current_mode ||
+		frame_rate != sensor->current_fr) {
+		sensor->current_fr = frame_rate;
+		sensor->frame_interval = fi->interval;
+		sensor->current_mode = mode;
+		sensor->pending_mode_change = true;
+
+		__v4l2_ctrl_s_ctrl_int64(sensor->ctrls.pixel_rate,
+					ov4689_calc_pixel_rate(sensor));
+	}
+out:
+	mutex_unlock(&sensor->lock);
+	return ret;
+}
+
+static int ov4689_stream_start(struct ov4689_dev *sensor, int enable)
+{
+	u8 val;
+	int result = ov4689_write_reg(sensor, 0x100, enable);
+
+	ov4689_read_reg(sensor, 0x100, &val);
+	mdelay(200);
+	return result;
+}
+
+static int ov4689_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct ov4689_dev *sensor = to_ov4689_dev(sd);
+	int ret = 0;
+
+	mutex_lock(&sensor->lock);
+
+	if (sensor->streaming == !enable) {
+		if (enable && sensor->pending_mode_change) {
+			ret = ov4689_set_mode(sensor);
+			if (ret)
+				goto out;
+		}
+
+		if (sensor->ep.bus_type == V4L2_MBUS_CSI2_DPHY)
+			ret = ov4689_set_stream_mipi(sensor, enable);
+
+		ret = ov4689_stream_start(sensor, enable);
+
+		if (!ret)
+			sensor->streaming = enable;
+	}
+out:
+	mutex_unlock(&sensor->lock);
+	return ret;
+}
+
+static const struct v4l2_subdev_core_ops ov4689_core_ops = {
+	.s_power = ov4689_s_power,
+	.log_status = v4l2_ctrl_subdev_log_status,
+	.subscribe_event = v4l2_ctrl_subdev_subscribe_event,
+	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
+};
+
+static const struct v4l2_subdev_video_ops ov4689_video_ops = {
+	.g_frame_interval = ov4689_g_frame_interval,
+	.s_frame_interval = ov4689_s_frame_interval,
+	.s_stream = ov4689_s_stream,
+};
+
+static const struct v4l2_subdev_pad_ops ov4689_pad_ops = {
+	.enum_mbus_code = ov4689_enum_mbus_code,
+	.get_fmt = ov4689_get_fmt,
+	.set_fmt = ov4689_set_fmt,
+	.enum_frame_size = ov4689_enum_frame_size,
+	.enum_frame_interval = ov4689_enum_frame_interval,
+};
+
+static const struct v4l2_subdev_ops ov4689_subdev_ops = {
+	.core = &ov4689_core_ops,
+	.video = &ov4689_video_ops,
+	.pad = &ov4689_pad_ops,
+};
+
+static int ov4689_get_regulators(struct ov4689_dev *sensor)
+{
+	int i;
+
+	for (i = 0; i < OV4689_NUM_SUPPLIES; i++)
+		sensor->supplies[i].supply = ov4689_supply_name[i];
+
+	return devm_regulator_bulk_get(&sensor->i2c_client->dev,
+					OV4689_NUM_SUPPLIES, sensor->supplies);
+}
+
+static int ov4689_check_chip_id(struct ov4689_dev *sensor)
+{
+	struct i2c_client *client = sensor->i2c_client;
+	int ret = 0;
+	u16 chip_id;
+
+	ret = ov4689_set_power_on(sensor);
+	if (ret)
+		return ret;
+
+	ret = ov4689_read_reg16(sensor, OV4689_REG_CHIP_ID, &chip_id);
+	if (ret) {
+		dev_err(&client->dev, "%s: failed to read chip identifier\n",
+			__func__);
+		goto power_off;
+	}
+
+	if (chip_id != OV4689_CHIP_ID) {
+		dev_err(&client->dev, "%s: wrong chip identifier, expected 0x%x,  got 0x%x\n",
+			__func__, OV4689_CHIP_ID, chip_id);
+		ret = -ENXIO;
+	}
+	dev_err(&client->dev, "%s: chip identifier, got 0x%x\n", __func__, chip_id);
+
+power_off:
+	ov4689_set_power_off(sensor);
+	return ret;
+}
+
+static int ov4689_probe(struct i2c_client *client)
+{
+	struct device *dev = &client->dev;
+	struct fwnode_handle *endpoint;
+	struct ov4689_dev *sensor;
+	struct v4l2_mbus_framefmt *fmt;
+	u32 rotation;
+	int ret;
+	u8 chip_id_high, chip_id_low;
+
+	sensor = devm_kzalloc(dev, sizeof(*sensor), GFP_KERNEL);
+	if (!sensor)
+		return -ENOMEM;
+
+	sensor->i2c_client = client;
+
+	fmt = &sensor->fmt;
+	fmt->code = MEDIA_BUS_FMT_SBGGR10_1X10;
+	fmt->colorspace = V4L2_COLORSPACE_SRGB;
+	fmt->ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(fmt->colorspace);
+	fmt->quantization = V4L2_QUANTIZATION_FULL_RANGE;
+	fmt->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(fmt->colorspace);
+	fmt->width = 1920;
+	fmt->height = 1080;
+	fmt->field = V4L2_FIELD_NONE;
+	sensor->frame_interval.numerator = 1;
+	sensor->frame_interval.denominator = ov4689_framerates[OV4689_30_FPS];
+	sensor->current_fr = OV4689_30_FPS;
+	sensor->current_mode =
+		&ov4689_mode_data[OV4689_MODE_1080P_1920_1080];
+	sensor->last_mode = sensor->current_mode;
+
+	sensor->ae_target = 52;
+
+	/* optional indication of physical rotation of sensor */
+	ret = fwnode_property_read_u32(dev_fwnode(&client->dev), "rotation",
+					&rotation);
+	if (!ret) {
+		switch (rotation) {
+		case 180:
+			sensor->upside_down = true;
+			fallthrough;
+		case 0:
+			break;
+		default:
+			dev_warn(dev, "%u degrees rotation is not supported, ignoring...\n",
+				rotation);
+		}
+	}
+
+	endpoint = fwnode_graph_get_next_endpoint(dev_fwnode(&client->dev),
+						NULL);
+	if (!endpoint) {
+		dev_err(dev, "endpoint node not found\n");
+		return -EINVAL;
+	}
+
+	ret = v4l2_fwnode_endpoint_parse(endpoint, &sensor->ep);
+	fwnode_handle_put(endpoint);
+	if (ret) {
+		dev_err(dev, "Could not parse endpoint\n");
+		return ret;
+	}
+
+	if (sensor->ep.bus_type != V4L2_MBUS_PARALLEL &&
+		sensor->ep.bus_type != V4L2_MBUS_CSI2_DPHY &&
+		sensor->ep.bus_type != V4L2_MBUS_BT656) {
+		dev_err(dev, "Unsupported bus type %d\n", sensor->ep.bus_type);
+		return -EINVAL;
+	}
+
+	/* get system clock (xclk) */
+	sensor->xclk = devm_clk_get(dev, "xclk");
+	if (IS_ERR(sensor->xclk)) {
+		dev_err(dev, "failed to get xclk\n");
+		return PTR_ERR(sensor->xclk);
+	}
+
+	sensor->xclk_freq = clk_get_rate(sensor->xclk);
+	if (sensor->xclk_freq < OV4689_XCLK_MIN ||
+		sensor->xclk_freq > OV4689_XCLK_MAX) {
+		dev_err(dev, "xclk frequency out of range: %d Hz\n",
+			sensor->xclk_freq);
+		return -EINVAL;
+	}
+
+	/* request optional power down pin */
+	sensor->pwdn_gpio = devm_gpiod_get_optional(dev, "powerdown",
+						GPIOD_OUT_HIGH);
+	if (IS_ERR(sensor->pwdn_gpio))
+		return PTR_ERR(sensor->pwdn_gpio);
+
+	/* request optional reset pin */
+	sensor->reset_gpio = devm_gpiod_get_optional(dev, "reset",
+						GPIOD_OUT_HIGH);
+	if (IS_ERR(sensor->reset_gpio))
+		return PTR_ERR(sensor->reset_gpio);
+
+	v4l2_i2c_subdev_init(&sensor->sd, client, &ov4689_subdev_ops);
+
+	sensor->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
+			V4L2_SUBDEV_FL_HAS_EVENTS;
+	sensor->pad.flags = MEDIA_PAD_FL_SOURCE;
+	sensor->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+	ret = media_entity_pads_init(&sensor->sd.entity, 1, &sensor->pad);
+	if (ret)
+		return ret;
+
+	ret = ov4689_get_regulators(sensor);
+	if (ret)
+		return ret;
+
+	mutex_init(&sensor->lock);
+
+	ret = ov4689_check_chip_id(sensor);
+	if (ret)
+		goto entity_cleanup;
+
+	ret = ov4689_init_controls(sensor);
+	if (ret)
+		goto entity_cleanup;
+
+	ret = v4l2_async_register_subdev_sensor(&sensor->sd);
+	if (ret)
+		goto free_ctrls;
+
+	return 0;
+
+free_ctrls:
+	v4l2_ctrl_handler_free(&sensor->ctrls.handler);
+entity_cleanup:
+	media_entity_cleanup(&sensor->sd.entity);
+	mutex_destroy(&sensor->lock);
+	return ret;
+}
+
+static int ov4689_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ov4689_dev *sensor = to_ov4689_dev(sd);
+
+	v4l2_async_unregister_subdev(&sensor->sd);
+	media_entity_cleanup(&sensor->sd.entity);
+	v4l2_ctrl_handler_free(&sensor->ctrls.handler);
+	mutex_destroy(&sensor->lock);
+
+	return 0;
+}
+
+static const struct i2c_device_id ov4689_id[] = {
+	{"ov4689", 0},
+	{},
+};
+MODULE_DEVICE_TABLE(i2c, ov4689_id);
+
+static const struct of_device_id ov4689_dt_ids[] = {
+	{ .compatible = "ovti,ov4689" },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, ov4689_dt_ids);
+
+static struct i2c_driver ov4689_i2c_driver = {
+	.driver = {
+		.name  = "ov4689",
+		.of_match_table = ov4689_dt_ids,
+	},
+	.id_table = ov4689_id,
+	.probe_new = ov4689_probe,
+	.remove   = ov4689_remove,
+};
+
+module_i2c_driver(ov4689_i2c_driver);
+
+MODULE_DESCRIPTION("OV4689 MIPI Camera Subdev Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/platform/starfive/ov5640.c b/drivers/media/platform/starfive/ov5640.c
new file mode 100644
index 000000000000..dba06a5186fd
--- /dev/null
+++ b/drivers/media/platform/starfive/ov5640.c
@@ -0,0 +1,3225 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Copyright (C) 2011-2013 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright (C) 2014-2017 Mentor Graphics Inc.
+ * Copyright (C) 2021 StarFive Technology Co., Ltd.
+ */
+
+#include <linux/clk.h>
+#include <linux/clk-provider.h>
+#include <linux/clkdev.h>
+#include <linux/ctype.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/gpio/consumer.h>
+#include <linux/i2c.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/of_device.h>
+#include <linux/regulator/consumer.h>
+#include <linux/slab.h>
+#include <linux/types.h>
+#include <media/v4l2-async.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-subdev.h>
+#include "stfcamss.h"
+
+/* min/typical/max system clock (xclk) frequencies */
+#define OV5640_XCLK_MIN  6000000
+#define OV5640_XCLK_MAX 54000000
+
+#define OV5640_SKIP_FRAMES      4
+
+#define OV5640_CHIP_ID		0x5640
+#define OV5640_DEFAULT_SLAVE_ID 0x3c
+
+#define OV5640_REG_SYS_RESET02		0x3002
+#define OV5640_REG_SYS_CLOCK_ENABLE02	0x3006
+#define OV5640_REG_SYS_CTRL0		0x3008
+#define OV5640_REG_SYS_CTRL0_SW_PWDN	0x42
+#define OV5640_REG_SYS_CTRL0_SW_PWUP	0x02
+#define OV5640_REG_CHIP_ID		0x300a
+#define OV5640_REG_IO_MIPI_CTRL00	0x300e
+#define OV5640_REG_PAD_OUTPUT_ENABLE01	0x3017
+#define OV5640_REG_PAD_OUTPUT_ENABLE02	0x3018
+#define OV5640_REG_PAD_OUTPUT00		0x3019
+#define OV5640_REG_SYSTEM_CONTROL1	0x302e
+#define OV5640_REG_SC_PLL_CTRL0		0x3034
+#define OV5640_REG_SC_PLL_CTRL1		0x3035
+#define OV5640_REG_SC_PLL_CTRL2		0x3036
+#define OV5640_REG_SC_PLL_CTRL3		0x3037
+#define OV5640_REG_SLAVE_ID		0x3100
+#define OV5640_REG_SCCB_SYS_CTRL1	0x3103
+#define OV5640_REG_SYS_ROOT_DIVIDER	0x3108
+#define OV5640_REG_AWB_R_GAIN		0x3400
+#define OV5640_REG_AWB_G_GAIN		0x3402
+#define OV5640_REG_AWB_B_GAIN		0x3404
+#define OV5640_REG_AWB_MANUAL_CTRL	0x3406
+#define OV5640_REG_AEC_PK_EXPOSURE_HI	0x3500
+#define OV5640_REG_AEC_PK_EXPOSURE_MED	0x3501
+#define OV5640_REG_AEC_PK_EXPOSURE_LO	0x3502
+#define OV5640_REG_AEC_PK_MANUAL	0x3503
+#define OV5640_REG_AEC_PK_REAL_GAIN	0x350a
+#define OV5640_REG_AEC_PK_VTS		0x350c
+#define OV5640_REG_TIMING_DVPHO		0x3808
+#define OV5640_REG_TIMING_DVPVO		0x380a
+#define OV5640_REG_TIMING_HTS		0x380c
+#define OV5640_REG_TIMING_VTS		0x380e
+#define OV5640_REG_TIMING_TC_REG20	0x3820
+#define OV5640_REG_TIMING_TC_REG21	0x3821
+#define OV5640_REG_AEC_CTRL00		0x3a00
+#define OV5640_REG_AEC_B50_STEP		0x3a08
+#define OV5640_REG_AEC_B60_STEP		0x3a0a
+#define OV5640_REG_AEC_CTRL0D		0x3a0d
+#define OV5640_REG_AEC_CTRL0E		0x3a0e
+#define OV5640_REG_AEC_CTRL0F		0x3a0f
+#define OV5640_REG_AEC_CTRL10		0x3a10
+#define OV5640_REG_AEC_CTRL11		0x3a11
+#define OV5640_REG_AEC_CTRL1B		0x3a1b
+#define OV5640_REG_AEC_CTRL1E		0x3a1e
+#define OV5640_REG_AEC_CTRL1F		0x3a1f
+#define OV5640_REG_HZ5060_CTRL00	0x3c00
+#define OV5640_REG_HZ5060_CTRL01	0x3c01
+#define OV5640_REG_SIGMADELTA_CTRL0C	0x3c0c
+#define OV5640_REG_FRAME_CTRL01		0x4202
+#define OV5640_REG_FORMAT_CONTROL00	0x4300
+#define OV5640_REG_VFIFO_HSIZE		0x4602
+#define OV5640_REG_VFIFO_VSIZE		0x4604
+#define OV5640_REG_JPG_MODE_SELECT	0x4713
+#define OV5640_REG_CCIR656_CTRL00	0x4730
+#define OV5640_REG_POLARITY_CTRL00	0x4740
+#define OV5640_REG_MIPI_CTRL00		0x4800
+#define OV5640_REG_DEBUG_MODE		0x4814
+#define OV5640_REG_ISP_FORMAT_MUX_CTRL	0x501f
+#define OV5640_REG_PRE_ISP_TEST_SET1	0x503d
+#define OV5640_REG_SDE_CTRL0		0x5580
+#define OV5640_REG_SDE_CTRL1		0x5581
+#define OV5640_REG_SDE_CTRL3		0x5583
+#define OV5640_REG_SDE_CTRL4		0x5584
+#define OV5640_REG_SDE_CTRL5		0x5585
+#define OV5640_REG_AVG_READOUT		0x56a1
+
+enum ov5640_mode_id {
+	OV5640_MODE_QCIF_176_144 = 0,
+	OV5640_MODE_QVGA_320_240,
+	OV5640_MODE_VGA_640_480,
+	OV5640_MODE_NTSC_720_480,
+	OV5640_MODE_PAL_720_576,
+	OV5640_MODE_XGA_1024_768,
+	OV5640_MODE_720P_1280_720,
+	OV5640_MODE_1080P_1920_1080,
+	OV5640_MODE_QSXGA_2592_1944,
+	OV5640_NUM_MODES,
+};
+
+enum ov5640_frame_rate {
+	OV5640_15_FPS = 0,
+	OV5640_30_FPS,
+	OV5640_60_FPS,
+	OV5640_NUM_FRAMERATES,
+};
+
+enum ov5640_format_mux {
+	OV5640_FMT_MUX_YUV422 = 0,
+	OV5640_FMT_MUX_RGB,
+	OV5640_FMT_MUX_DITHER,
+	OV5640_FMT_MUX_RAW_DPC,
+	OV5640_FMT_MUX_SNR_RAW,
+	OV5640_FMT_MUX_RAW_CIP,
+};
+
+struct ov5640_pixfmt {
+	u32 code;
+	u32 colorspace;
+};
+
+static const struct ov5640_pixfmt ov5640_formats[] = {
+	{ MEDIA_BUS_FMT_JPEG_1X8, V4L2_COLORSPACE_JPEG, },
+	{ MEDIA_BUS_FMT_UYVY8_2X8, V4L2_COLORSPACE_SRGB, },
+	{ MEDIA_BUS_FMT_YUYV8_2X8, V4L2_COLORSPACE_SRGB, },
+	{ MEDIA_BUS_FMT_RGB565_2X8_LE, V4L2_COLORSPACE_SRGB, },
+	{ MEDIA_BUS_FMT_RGB565_2X8_BE, V4L2_COLORSPACE_SRGB, },
+	{ MEDIA_BUS_FMT_SBGGR8_1X8, V4L2_COLORSPACE_SRGB, },
+	{ MEDIA_BUS_FMT_SGBRG8_1X8, V4L2_COLORSPACE_SRGB, },
+	{ MEDIA_BUS_FMT_SGRBG8_1X8, V4L2_COLORSPACE_SRGB, },
+	{ MEDIA_BUS_FMT_SRGGB8_1X8, V4L2_COLORSPACE_SRGB, },
+};
+
+/*
+ * FIXME: remove this when a subdev API becomes available
+ * to set the MIPI CSI-2 virtual channel.
+ */
+static unsigned int virtual_channel;
+module_param(virtual_channel, uint, 0444);
+MODULE_PARM_DESC(virtual_channel,
+		 "MIPI CSI-2 virtual channel (0..3), default 0");
+
+static const int ov5640_framerates[] = {
+	[OV5640_15_FPS] = 15,
+	[OV5640_30_FPS] = 30,
+	[OV5640_60_FPS] = 60,
+};
+
+/* regulator supplies */
+static const char * const ov5640_supply_name[] = {
+	"DOVDD", /* Digital I/O (1.8V) supply */
+	"AVDD",  /* Analog (2.8V) supply */
+	"DVDD",  /* Digital Core (1.5V) supply */
+};
+
+#define OV5640_NUM_SUPPLIES ARRAY_SIZE(ov5640_supply_name)
+
+/*
+ * Image size under 1280 * 960 are SUBSAMPLING
+ * Image size upper 1280 * 960 are SCALING
+ */
+enum ov5640_downsize_mode {
+	SUBSAMPLING,
+	SCALING,
+};
+
+struct reg_value {
+	u16 reg_addr;
+	u8 val;
+	u8 mask;
+	u32 delay_ms;
+};
+
+struct ov5640_mode_info {
+	enum ov5640_mode_id id;
+	enum ov5640_downsize_mode dn_mode;
+	u32 hact;
+	u32 htot;
+	u32 vact;
+	u32 vtot;
+	const struct reg_value *reg_data;
+	u32 reg_data_size;
+	u32 max_fps;
+};
+
+struct ov5640_ctrls {
+	struct v4l2_ctrl_handler handler;
+	struct v4l2_ctrl *pixel_rate;
+	struct {
+		struct v4l2_ctrl *auto_exp;
+		struct v4l2_ctrl *exposure;
+	};
+	struct {
+		struct v4l2_ctrl *auto_wb;
+		struct v4l2_ctrl *blue_balance;
+		struct v4l2_ctrl *red_balance;
+	};
+	struct {
+		struct v4l2_ctrl *auto_gain;
+		struct v4l2_ctrl *gain;
+	};
+	struct v4l2_ctrl *brightness;
+	struct v4l2_ctrl *light_freq;
+	struct v4l2_ctrl *saturation;
+	struct v4l2_ctrl *contrast;
+	struct v4l2_ctrl *hue;
+	struct v4l2_ctrl *test_pattern;
+	struct v4l2_ctrl *hflip;
+	struct v4l2_ctrl *vflip;
+};
+
+struct ov5640_dev {
+	struct i2c_client *i2c_client;
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+	struct v4l2_fwnode_endpoint ep; /* the parsed DT endpoint info */
+	struct clk *xclk; /* system clock to OV5640 */
+	u32 xclk_freq;
+
+	struct regulator_bulk_data supplies[OV5640_NUM_SUPPLIES];
+	struct gpio_desc *reset_gpio;
+	struct gpio_desc *pwdn_gpio;
+	bool   upside_down;
+
+	/* lock to protect all members below */
+	struct mutex lock;
+
+	int power_count;
+
+	struct v4l2_mbus_framefmt fmt;
+	bool pending_fmt_change;
+
+	const struct ov5640_mode_info *current_mode;
+	const struct ov5640_mode_info *last_mode;
+	enum ov5640_frame_rate current_fr;
+	struct v4l2_fract frame_interval;
+
+	struct ov5640_ctrls ctrls;
+
+	u32 prev_sysclk, prev_hts;
+	u32 ae_low, ae_high, ae_target;
+
+	bool pending_mode_change;
+	bool streaming;
+};
+
+static inline struct ov5640_dev *to_ov5640_dev(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct ov5640_dev, sd);
+}
+
+static inline struct v4l2_subdev *ctrl_to_sd(struct v4l2_ctrl *ctrl)
+{
+	return &container_of(ctrl->handler, struct ov5640_dev,
+			     ctrls.handler)->sd;
+}
+
+/*
+ * FIXME: all of these register tables are likely filled with
+ * entries that set the register to their power-on default values,
+ * and which are otherwise not touched by this driver. Those entries
+ * should be identified and removed to speed register load time
+ * over i2c.
+ */
+/* YUV422 UYVY VGA@30fps */
+static const struct reg_value ov5640_init_setting_30fps_VGA[] = {
+	{0x3103, 0x11, 0, 0}, {0x3008, 0x82, 0, 5}, {0x3008, 0x42, 0, 0},
+	{0x3103, 0x03, 0, 0}, {0x3630, 0x36, 0, 0},
+	{0x3631, 0x0e, 0, 0}, {0x3632, 0xe2, 0, 0}, {0x3633, 0x12, 0, 0},
+	{0x3621, 0xe0, 0, 0}, {0x3704, 0xa0, 0, 0}, {0x3703, 0x5a, 0, 0},
+	{0x3715, 0x78, 0, 0}, {0x3717, 0x01, 0, 0}, {0x370b, 0x60, 0, 0},
+	{0x3705, 0x1a, 0, 0}, {0x3905, 0x02, 0, 0}, {0x3906, 0x10, 0, 0},
+	{0x3901, 0x0a, 0, 0}, {0x3731, 0x12, 0, 0}, {0x3600, 0x08, 0, 0},
+	{0x3601, 0x33, 0, 0}, {0x302d, 0x60, 0, 0}, {0x3620, 0x52, 0, 0},
+	{0x371b, 0x20, 0, 0}, {0x471c, 0x50, 0, 0}, {0x3a13, 0x43, 0, 0},
+	{0x3a18, 0x00, 0, 0}, {0x3a19, 0xf8, 0, 0}, {0x3635, 0x13, 0, 0},
+	{0x3636, 0x03, 0, 0}, {0x3634, 0x40, 0, 0}, {0x3622, 0x01, 0, 0},
+	{0x3c01, 0xa4, 0, 0}, {0x3c04, 0x28, 0, 0}, {0x3c05, 0x98, 0, 0},
+	{0x3c06, 0x00, 0, 0}, {0x3c07, 0x08, 0, 0}, {0x3c08, 0x00, 0, 0},
+	{0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
+	{0x3820, 0x41, 0, 0}, {0x3821, 0x07, 0, 0}, {0x3814, 0x31, 0, 0},
+	{0x3815, 0x31, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
+	{0x3802, 0x00, 0, 0}, {0x3803, 0x04, 0, 0}, {0x3804, 0x0a, 0, 0},
+	{0x3805, 0x3f, 0, 0}, {0x3806, 0x07, 0, 0}, {0x3807, 0x9b, 0, 0},
+	{0x3810, 0x00, 0, 0},
+	{0x3811, 0x10, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x06, 0, 0},
+	{0x3618, 0x00, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x64, 0, 0},
+	{0x3709, 0x52, 0, 0}, {0x370c, 0x03, 0, 0}, {0x3a02, 0x03, 0, 0},
+	{0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x27, 0, 0},
+	{0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
+	{0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
+	{0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0}, {0x3000, 0x00, 0, 0},
+	{0x3002, 0x1c, 0, 0}, {0x3004, 0xff, 0, 0}, {0x3006, 0xc3, 0, 0},
+	{0x302e, 0x08, 0, 0}, {0x4300, 0x3f, 0, 0},
+	{0x501f, 0x00, 0, 0}, {0x4407, 0x04, 0, 0},
+	{0x440e, 0x00, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
+	{0x4837, 0x0a, 0, 0}, {0x3824, 0x02, 0, 0},
+	{0x5000, 0xa7, 0, 0}, {0x5001, 0xa3, 0, 0}, {0x5180, 0xff, 0, 0},
+	{0x5181, 0xf2, 0, 0}, {0x5182, 0x00, 0, 0}, {0x5183, 0x14, 0, 0},
+	{0x5184, 0x25, 0, 0}, {0x5185, 0x24, 0, 0}, {0x5186, 0x09, 0, 0},
+	{0x5187, 0x09, 0, 0}, {0x5188, 0x09, 0, 0}, {0x5189, 0x88, 0, 0},
+	{0x518a, 0x54, 0, 0}, {0x518b, 0xee, 0, 0}, {0x518c, 0xb2, 0, 0},
+	{0x518d, 0x50, 0, 0}, {0x518e, 0x34, 0, 0}, {0x518f, 0x6b, 0, 0},
+	{0x5190, 0x46, 0, 0}, {0x5191, 0xf8, 0, 0}, {0x5192, 0x04, 0, 0},
+	{0x5193, 0x70, 0, 0}, {0x5194, 0xf0, 0, 0}, {0x5195, 0xf0, 0, 0},
+	{0x5196, 0x03, 0, 0}, {0x5197, 0x01, 0, 0}, {0x5198, 0x04, 0, 0},
+	{0x5199, 0x6c, 0, 0}, {0x519a, 0x04, 0, 0}, {0x519b, 0x00, 0, 0},
+	{0x519c, 0x09, 0, 0}, {0x519d, 0x2b, 0, 0}, {0x519e, 0x38, 0, 0},
+	{0x5381, 0x1e, 0, 0}, {0x5382, 0x5b, 0, 0}, {0x5383, 0x08, 0, 0},
+	{0x5384, 0x0a, 0, 0}, {0x5385, 0x7e, 0, 0}, {0x5386, 0x88, 0, 0},
+	{0x5387, 0x7c, 0, 0}, {0x5388, 0x6c, 0, 0}, {0x5389, 0x10, 0, 0},
+	{0x538a, 0x01, 0, 0}, {0x538b, 0x98, 0, 0}, {0x5300, 0x08, 0, 0},
+	{0x5301, 0x30, 0, 0}, {0x5302, 0x10, 0, 0}, {0x5303, 0x00, 0, 0},
+	{0x5304, 0x08, 0, 0}, {0x5305, 0x30, 0, 0}, {0x5306, 0x08, 0, 0},
+	{0x5307, 0x16, 0, 0}, {0x5309, 0x08, 0, 0}, {0x530a, 0x30, 0, 0},
+	{0x530b, 0x04, 0, 0}, {0x530c, 0x06, 0, 0}, {0x5480, 0x01, 0, 0},
+	{0x5481, 0x08, 0, 0}, {0x5482, 0x14, 0, 0}, {0x5483, 0x28, 0, 0},
+	{0x5484, 0x51, 0, 0}, {0x5485, 0x65, 0, 0}, {0x5486, 0x71, 0, 0},
+	{0x5487, 0x7d, 0, 0}, {0x5488, 0x87, 0, 0}, {0x5489, 0x91, 0, 0},
+	{0x548a, 0x9a, 0, 0}, {0x548b, 0xaa, 0, 0}, {0x548c, 0xb8, 0, 0},
+	{0x548d, 0xcd, 0, 0}, {0x548e, 0xdd, 0, 0}, {0x548f, 0xea, 0, 0},
+	{0x5490, 0x1d, 0, 0}, {0x5580, 0x02, 0, 0}, {0x5583, 0x40, 0, 0},
+	{0x5584, 0x10, 0, 0}, {0x5589, 0x10, 0, 0}, {0x558a, 0x00, 0, 0},
+	{0x558b, 0xf8, 0, 0}, {0x5800, 0x23, 0, 0}, {0x5801, 0x14, 0, 0},
+	{0x5802, 0x0f, 0, 0}, {0x5803, 0x0f, 0, 0}, {0x5804, 0x12, 0, 0},
+	{0x5805, 0x26, 0, 0}, {0x5806, 0x0c, 0, 0}, {0x5807, 0x08, 0, 0},
+	{0x5808, 0x05, 0, 0}, {0x5809, 0x05, 0, 0}, {0x580a, 0x08, 0, 0},
+	{0x580b, 0x0d, 0, 0}, {0x580c, 0x08, 0, 0}, {0x580d, 0x03, 0, 0},
+	{0x580e, 0x00, 0, 0}, {0x580f, 0x00, 0, 0}, {0x5810, 0x03, 0, 0},
+	{0x5811, 0x09, 0, 0}, {0x5812, 0x07, 0, 0}, {0x5813, 0x03, 0, 0},
+	{0x5814, 0x00, 0, 0}, {0x5815, 0x01, 0, 0}, {0x5816, 0x03, 0, 0},
+	{0x5817, 0x08, 0, 0}, {0x5818, 0x0d, 0, 0}, {0x5819, 0x08, 0, 0},
+	{0x581a, 0x05, 0, 0}, {0x581b, 0x06, 0, 0}, {0x581c, 0x08, 0, 0},
+	{0x581d, 0x0e, 0, 0}, {0x581e, 0x29, 0, 0}, {0x581f, 0x17, 0, 0},
+	{0x5820, 0x11, 0, 0}, {0x5821, 0x11, 0, 0}, {0x5822, 0x15, 0, 0},
+	{0x5823, 0x28, 0, 0}, {0x5824, 0x46, 0, 0}, {0x5825, 0x26, 0, 0},
+	{0x5826, 0x08, 0, 0}, {0x5827, 0x26, 0, 0}, {0x5828, 0x64, 0, 0},
+	{0x5829, 0x26, 0, 0}, {0x582a, 0x24, 0, 0}, {0x582b, 0x22, 0, 0},
+	{0x582c, 0x24, 0, 0}, {0x582d, 0x24, 0, 0}, {0x582e, 0x06, 0, 0},
+	{0x582f, 0x22, 0, 0}, {0x5830, 0x40, 0, 0}, {0x5831, 0x42, 0, 0},
+	{0x5832, 0x24, 0, 0}, {0x5833, 0x26, 0, 0}, {0x5834, 0x24, 0, 0},
+	{0x5835, 0x22, 0, 0}, {0x5836, 0x22, 0, 0}, {0x5837, 0x26, 0, 0},
+	{0x5838, 0x44, 0, 0}, {0x5839, 0x24, 0, 0}, {0x583a, 0x26, 0, 0},
+	{0x583b, 0x28, 0, 0}, {0x583c, 0x42, 0, 0}, {0x583d, 0xce, 0, 0},
+	{0x5025, 0x00, 0, 0}, {0x3a0f, 0x30, 0, 0}, {0x3a10, 0x28, 0, 0},
+	{0x3a1b, 0x30, 0, 0}, {0x3a1e, 0x26, 0, 0}, {0x3a11, 0x60, 0, 0},
+	{0x3a1f, 0x14, 0, 0}, {0x3008, 0x02, 0, 0}, {0x3c00, 0x04, 0, 300},
+};
+
+static const struct reg_value ov5640_setting_VGA_640_480[] = {
+	{0x3c07, 0x08, 0, 0},
+	{0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
+	{0x3814, 0x31, 0, 0},
+	{0x3815, 0x31, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
+	{0x3802, 0x00, 0, 0}, {0x3803, 0x04, 0, 0}, {0x3804, 0x0a, 0, 0},
+	{0x3805, 0x3f, 0, 0}, {0x3806, 0x07, 0, 0}, {0x3807, 0x9b, 0, 0},
+	{0x3810, 0x00, 0, 0},
+	{0x3811, 0x10, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x06, 0, 0},
+	{0x3618, 0x00, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x64, 0, 0},
+	{0x3709, 0x52, 0, 0}, {0x370c, 0x03, 0, 0}, {0x3a02, 0x03, 0, 0},
+	{0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x27, 0, 0},
+	{0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
+	{0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
+	{0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0},
+	{0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
+	{0x3824, 0x02, 0, 0}, {0x5001, 0xa3, 0, 0},
+};
+
+static const struct reg_value ov5640_setting_XGA_1024_768[] = {
+	{0x3c07, 0x08, 0, 0},
+	{0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
+	{0x3814, 0x31, 0, 0},
+	{0x3815, 0x31, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
+	{0x3802, 0x00, 0, 0}, {0x3803, 0x04, 0, 0}, {0x3804, 0x0a, 0, 0},
+	{0x3805, 0x3f, 0, 0}, {0x3806, 0x07, 0, 0}, {0x3807, 0x9b, 0, 0},
+	{0x3810, 0x00, 0, 0},
+	{0x3811, 0x10, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x06, 0, 0},
+	{0x3618, 0x00, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x64, 0, 0},
+	{0x3709, 0x52, 0, 0}, {0x370c, 0x03, 0, 0}, {0x3a02, 0x03, 0, 0},
+	{0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x27, 0, 0},
+	{0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
+	{0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
+	{0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0},
+	{0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
+	{0x3824, 0x02, 0, 0}, {0x5001, 0xa3, 0, 0},
+};
+
+static const struct reg_value ov5640_setting_QVGA_320_240[] = {
+	{0x3c07, 0x08, 0, 0},
+	{0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
+	{0x3814, 0x31, 0, 0},
+	{0x3815, 0x31, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
+	{0x3802, 0x00, 0, 0}, {0x3803, 0x04, 0, 0}, {0x3804, 0x0a, 0, 0},
+	{0x3805, 0x3f, 0, 0}, {0x3806, 0x07, 0, 0}, {0x3807, 0x9b, 0, 0},
+	{0x3810, 0x00, 0, 0},
+	{0x3811, 0x10, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x06, 0, 0},
+	{0x3618, 0x00, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x64, 0, 0},
+	{0x3709, 0x52, 0, 0}, {0x370c, 0x03, 0, 0}, {0x3a02, 0x03, 0, 0},
+	{0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x27, 0, 0},
+	{0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
+	{0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
+	{0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0},
+	{0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
+	{0x3824, 0x02, 0, 0}, {0x5001, 0xa3, 0, 0},
+};
+
+static const struct reg_value ov5640_setting_QCIF_176_144[] = {
+	{0x3c07, 0x08, 0, 0},
+	{0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
+	{0x3814, 0x31, 0, 0},
+	{0x3815, 0x31, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
+	{0x3802, 0x00, 0, 0}, {0x3803, 0x04, 0, 0}, {0x3804, 0x0a, 0, 0},
+	{0x3805, 0x3f, 0, 0}, {0x3806, 0x07, 0, 0}, {0x3807, 0x9b, 0, 0},
+	{0x3810, 0x00, 0, 0},
+	{0x3811, 0x10, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x06, 0, 0},
+	{0x3618, 0x00, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x64, 0, 0},
+	{0x3709, 0x52, 0, 0}, {0x370c, 0x03, 0, 0}, {0x3a02, 0x03, 0, 0},
+	{0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x27, 0, 0},
+	{0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
+	{0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
+	{0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0},
+	{0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
+	{0x3824, 0x02, 0, 0}, {0x5001, 0xa3, 0, 0},
+};
+
+static const struct reg_value ov5640_setting_NTSC_720_480[] = {
+	{0x3c07, 0x08, 0, 0},
+	{0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
+	{0x3814, 0x31, 0, 0},
+	{0x3815, 0x31, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
+	{0x3802, 0x00, 0, 0}, {0x3803, 0x04, 0, 0}, {0x3804, 0x0a, 0, 0},
+	{0x3805, 0x3f, 0, 0}, {0x3806, 0x07, 0, 0}, {0x3807, 0x9b, 0, 0},
+	{0x3810, 0x00, 0, 0},
+	{0x3811, 0x10, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x3c, 0, 0},
+	{0x3618, 0x00, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x64, 0, 0},
+	{0x3709, 0x52, 0, 0}, {0x370c, 0x03, 0, 0}, {0x3a02, 0x03, 0, 0},
+	{0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x27, 0, 0},
+	{0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
+	{0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
+	{0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0},
+	{0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
+	{0x3824, 0x02, 0, 0}, {0x5001, 0xa3, 0, 0},
+};
+
+static const struct reg_value ov5640_setting_PAL_720_576[] = {
+	{0x3c07, 0x08, 0, 0},
+	{0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
+	{0x3814, 0x31, 0, 0},
+	{0x3815, 0x31, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
+	{0x3802, 0x00, 0, 0}, {0x3803, 0x04, 0, 0}, {0x3804, 0x0a, 0, 0},
+	{0x3805, 0x3f, 0, 0}, {0x3806, 0x07, 0, 0}, {0x3807, 0x9b, 0, 0},
+	{0x3810, 0x00, 0, 0},
+	{0x3811, 0x38, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x06, 0, 0},
+	{0x3618, 0x00, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x64, 0, 0},
+	{0x3709, 0x52, 0, 0}, {0x370c, 0x03, 0, 0}, {0x3a02, 0x03, 0, 0},
+	{0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x27, 0, 0},
+	{0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
+	{0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
+	{0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0},
+	{0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
+	{0x3824, 0x02, 0, 0}, {0x5001, 0xa3, 0, 0},
+};
+
+static const struct reg_value ov5640_setting_720P_1280_720[] = {
+	{0x3c07, 0x07, 0, 0},
+	{0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
+	{0x3814, 0x31, 0, 0},
+	{0x3815, 0x31, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
+	{0x3802, 0x00, 0, 0}, {0x3803, 0xfa, 0, 0}, {0x3804, 0x0a, 0, 0},
+	{0x3805, 0x3f, 0, 0}, {0x3806, 0x06, 0, 0}, {0x3807, 0xa9, 0, 0},
+	{0x3810, 0x00, 0, 0},
+	{0x3811, 0x10, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x04, 0, 0},
+	{0x3618, 0x00, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x64, 0, 0},
+	{0x3709, 0x52, 0, 0}, {0x370c, 0x03, 0, 0}, {0x3a02, 0x02, 0, 0},
+	{0x3a03, 0xe4, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0xbc, 0, 0},
+	{0x3a0a, 0x01, 0, 0}, {0x3a0b, 0x72, 0, 0}, {0x3a0e, 0x01, 0, 0},
+	{0x3a0d, 0x02, 0, 0}, {0x3a14, 0x02, 0, 0}, {0x3a15, 0xe4, 0, 0},
+	{0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0},
+	{0x4407, 0x04, 0, 0}, {0x460b, 0x37, 0, 0}, {0x460c, 0x20, 0, 0},
+	{0x3824, 0x04, 0, 0}, {0x5001, 0x83, 0, 0},
+};
+
+static const struct reg_value ov5640_setting_1080P_1920_1080[] = {
+	{0x3c07, 0x08, 0, 0},
+	{0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
+	{0x3814, 0x11, 0, 0},
+	{0x3815, 0x11, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
+	{0x3802, 0x00, 0, 0}, {0x3803, 0x00, 0, 0}, {0x3804, 0x0a, 0, 0},
+	{0x3805, 0x3f, 0, 0}, {0x3806, 0x07, 0, 0}, {0x3807, 0x9f, 0, 0},
+	{0x3810, 0x00, 0, 0},
+	{0x3811, 0x10, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x04, 0, 0},
+	{0x3618, 0x04, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x21, 0, 0},
+	{0x3709, 0x12, 0, 0}, {0x370c, 0x00, 0, 0}, {0x3a02, 0x03, 0, 0},
+	{0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x27, 0, 0},
+	{0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
+	{0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
+	{0x4001, 0x02, 0, 0}, {0x4004, 0x06, 0, 0},
+	{0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
+	{0x3824, 0x02, 0, 0}, {0x5001, 0x83, 0, 0},
+	{0x3c07, 0x07, 0, 0}, {0x3c08, 0x00, 0, 0},
+	{0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
+	{0x3800, 0x01, 0, 0}, {0x3801, 0x50, 0, 0}, {0x3802, 0x01, 0, 0},
+	{0x3803, 0xb2, 0, 0}, {0x3804, 0x08, 0, 0}, {0x3805, 0xef, 0, 0},
+	{0x3806, 0x05, 0, 0}, {0x3807, 0xf1, 0, 0},
+	{0x3612, 0x2b, 0, 0}, {0x3708, 0x64, 0, 0},
+	{0x3a02, 0x04, 0, 0}, {0x3a03, 0x60, 0, 0}, {0x3a08, 0x01, 0, 0},
+	{0x3a09, 0x50, 0, 0}, {0x3a0a, 0x01, 0, 0}, {0x3a0b, 0x18, 0, 0},
+	{0x3a0e, 0x03, 0, 0}, {0x3a0d, 0x04, 0, 0}, {0x3a14, 0x04, 0, 0},
+	{0x3a15, 0x60, 0, 0}, {0x4407, 0x04, 0, 0},
+	{0x460b, 0x37, 0, 0}, {0x460c, 0x20, 0, 0}, {0x3824, 0x04, 0, 0},
+	{0x4005, 0x1a, 0, 0},
+};
+
+static const struct reg_value ov5640_setting_QSXGA_2592_1944[] = {
+	{0x3c07, 0x08, 0, 0},
+	{0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
+	{0x3814, 0x11, 0, 0},
+	{0x3815, 0x11, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
+	{0x3802, 0x00, 0, 0}, {0x3803, 0x00, 0, 0}, {0x3804, 0x0a, 0, 0},
+	{0x3805, 0x3f, 0, 0}, {0x3806, 0x07, 0, 0}, {0x3807, 0x9f, 0, 0},
+	{0x3810, 0x00, 0, 0},
+	{0x3811, 0x10, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x04, 0, 0},
+	{0x3618, 0x04, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x21, 0, 0},
+	{0x3709, 0x12, 0, 0}, {0x370c, 0x00, 0, 0}, {0x3a02, 0x03, 0, 0},
+	{0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x27, 0, 0},
+	{0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
+	{0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
+	{0x4001, 0x02, 0, 0}, {0x4004, 0x06, 0, 0},
+	{0x4407, 0x04, 0, 0}, {0x460b, 0x37, 0, 0}, {0x460c, 0x20, 0, 0},
+	{0x3824, 0x04, 0, 0}, {0x5001, 0x83, 0, 70},
+};
+
+/* power-on sensor init reg table */
+static const struct ov5640_mode_info ov5640_mode_init_data = {
+	0, SUBSAMPLING, 640, 1896, 480, 984,
+	ov5640_init_setting_30fps_VGA,
+	ARRAY_SIZE(ov5640_init_setting_30fps_VGA),
+	OV5640_30_FPS,
+};
+
+static const struct ov5640_mode_info
+ov5640_mode_data[OV5640_NUM_MODES] = {
+	{OV5640_MODE_QCIF_176_144, SUBSAMPLING,
+	 176, 1896, 144, 984,
+	 ov5640_setting_QCIF_176_144,
+	 ARRAY_SIZE(ov5640_setting_QCIF_176_144),
+	 OV5640_30_FPS},
+	{OV5640_MODE_QVGA_320_240, SUBSAMPLING,
+	 320, 1896, 240, 984,
+	 ov5640_setting_QVGA_320_240,
+	 ARRAY_SIZE(ov5640_setting_QVGA_320_240),
+	 OV5640_30_FPS},
+	{OV5640_MODE_VGA_640_480, SUBSAMPLING,
+	 640, 1896, 480, 1080,
+	 ov5640_setting_VGA_640_480,
+	 ARRAY_SIZE(ov5640_setting_VGA_640_480),
+	 OV5640_60_FPS},
+	{OV5640_MODE_NTSC_720_480, SUBSAMPLING,
+	 720, 1896, 480, 984,
+	 ov5640_setting_NTSC_720_480,
+	 ARRAY_SIZE(ov5640_setting_NTSC_720_480),
+	OV5640_30_FPS},
+	{OV5640_MODE_PAL_720_576, SUBSAMPLING,
+	 720, 1896, 576, 984,
+	 ov5640_setting_PAL_720_576,
+	 ARRAY_SIZE(ov5640_setting_PAL_720_576),
+	 OV5640_30_FPS},
+	{OV5640_MODE_XGA_1024_768, SUBSAMPLING,
+	 1024, 1896, 768, 1080,
+	 ov5640_setting_XGA_1024_768,
+	 ARRAY_SIZE(ov5640_setting_XGA_1024_768),
+	 OV5640_30_FPS},
+	{OV5640_MODE_720P_1280_720, SUBSAMPLING,
+	 1280, 1892, 720, 740,
+	 ov5640_setting_720P_1280_720,
+	 ARRAY_SIZE(ov5640_setting_720P_1280_720),
+	 OV5640_30_FPS},
+	{OV5640_MODE_1080P_1920_1080, SCALING,
+	 1920, 2500, 1080, 1120,
+	 ov5640_setting_1080P_1920_1080,
+	 ARRAY_SIZE(ov5640_setting_1080P_1920_1080),
+	 OV5640_30_FPS},
+	{OV5640_MODE_QSXGA_2592_1944, SCALING,
+	 2592, 2844, 1944, 1968,
+	 ov5640_setting_QSXGA_2592_1944,
+	 ARRAY_SIZE(ov5640_setting_QSXGA_2592_1944),
+	 OV5640_15_FPS},
+};
+
+static int ov5640_init_slave_id(struct ov5640_dev *sensor)
+{
+	struct i2c_client *client = sensor->i2c_client;
+	struct i2c_msg msg;
+	u8 buf[3];
+	int ret;
+
+	if (client->addr == OV5640_DEFAULT_SLAVE_ID)
+		return 0;
+
+	buf[0] = OV5640_REG_SLAVE_ID >> 8;
+	buf[1] = OV5640_REG_SLAVE_ID & 0xff;
+	buf[2] = client->addr << 1;
+
+	msg.addr = OV5640_DEFAULT_SLAVE_ID;
+	msg.flags = 0;
+	msg.buf = buf;
+	msg.len = sizeof(buf);
+
+	ret = i2c_transfer(client->adapter, &msg, 1);
+	if (ret < 0) {
+		dev_err(&client->dev, "%s: failed with %d\n", __func__, ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int ov5640_write_reg(struct ov5640_dev *sensor, u16 reg, u8 val)
+{
+	struct i2c_client *client = sensor->i2c_client;
+	struct i2c_msg msg;
+	u8 buf[3];
+	int ret;
+
+	buf[0] = reg >> 8;
+	buf[1] = reg & 0xff;
+	buf[2] = val;
+
+	msg.addr = client->addr;
+	msg.flags = client->flags;
+	msg.buf = buf;
+	msg.len = sizeof(buf);
+
+	ret = i2c_transfer(client->adapter, &msg, 1);
+	if (ret < 0) {
+		dev_err(&client->dev, "%s: error: reg=%x, val=%x\n",
+			__func__, reg, val);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int ov5640_read_reg(struct ov5640_dev *sensor, u16 reg, u8 *val)
+{
+	struct i2c_client *client = sensor->i2c_client;
+	struct i2c_msg msg[2];
+	u8 buf[2];
+	int ret;
+
+	buf[0] = reg >> 8;
+	buf[1] = reg & 0xff;
+
+	msg[0].addr = client->addr;
+	msg[0].flags = client->flags;
+	msg[0].buf = buf;
+	msg[0].len = sizeof(buf);
+
+	msg[1].addr = client->addr;
+	msg[1].flags = client->flags | I2C_M_RD;
+	msg[1].buf = buf;
+	msg[1].len = 1;
+
+	ret = i2c_transfer(client->adapter, msg, 2);
+	if (ret < 0) {
+		dev_err(&client->dev, "%s: error: reg=%x\n",
+			__func__, reg);
+		return ret;
+	}
+
+	*val = buf[0];
+	return 0;
+}
+
+static int ov5640_read_reg16(struct ov5640_dev *sensor, u16 reg, u16 *val)
+{
+	u8 hi, lo;
+	int ret;
+
+	ret = ov5640_read_reg(sensor, reg, &hi);
+	if (ret)
+		return ret;
+	ret = ov5640_read_reg(sensor, reg + 1, &lo);
+	if (ret)
+		return ret;
+
+	*val = ((u16)hi << 8) | (u16)lo;
+	return 0;
+}
+
+static int ov5640_write_reg16(struct ov5640_dev *sensor, u16 reg, u16 val)
+{
+	int ret;
+
+	ret = ov5640_write_reg(sensor, reg, val >> 8);
+	if (ret)
+		return ret;
+
+	return ov5640_write_reg(sensor, reg + 1, val & 0xff);
+}
+
+static int ov5640_mod_reg(struct ov5640_dev *sensor, u16 reg,
+			  u8 mask, u8 val)
+{
+	u8 readval;
+	int ret;
+
+	ret = ov5640_read_reg(sensor, reg, &readval);
+	if (ret)
+		return ret;
+
+	readval &= ~mask;
+	val &= mask;
+	val |= readval;
+
+	return ov5640_write_reg(sensor, reg, val);
+}
+
+/*
+ * After trying the various combinations, reading various
+ * documentations spread around the net, and from the various
+ * feedback, the clock tree is probably as follows:
+ *
+ *   +--------------+
+ *   |  Ext. Clock  |
+ *   +-+------------+
+ *     |  +----------+
+ *     +->|   PLL1   | - reg 0x3036, for the multiplier
+ *        +-+--------+ - reg 0x3037, bits 0-3 for the pre-divider
+ *          |  +--------------+
+ *          +->| System Clock |  - reg 0x3035, bits 4-7
+ *             +-+------------+
+ *               |  +--------------+
+ *               +->| MIPI Divider | - reg 0x3035, bits 0-3
+ *               |  +-+------------+
+ *               |    +----------------> MIPI SCLK
+ *               |    +  +-----+
+ *               |    +->| / 2 |-------> MIPI BIT CLK
+ *               |       +-----+
+ *               |  +--------------+
+ *               +->| PLL Root Div | - reg 0x3037, bit 4
+ *                  +-+------------+
+ *                    |  +---------+
+ *                    +->| Bit Div | - reg 0x3034, bits 0-3
+ *                       +-+-------+
+ *                         |  +-------------+
+ *                         +->| SCLK Div    | - reg 0x3108, bits 0-1
+ *                         |  +-+-----------+
+ *                         |    +---------------> SCLK
+ *                         |  +-------------+
+ *                         +->| SCLK 2X Div | - reg 0x3108, bits 2-3
+ *                         |  +-+-----------+
+ *                         |    +---------------> SCLK 2X
+ *                         |  +-------------+
+ *                         +->| PCLK Div    | - reg 0x3108, bits 4-5
+ *                            ++------------+
+ *                             +  +-----------+
+ *                             +->|   P_DIV   | - reg 0x3035, bits 0-3
+ *                                +-----+-----+
+ *                                       +------------> PCLK
+ *
+ * This is deviating from the datasheet at least for the register
+ * 0x3108, since it's said here that the PCLK would be clocked from
+ * the PLL.
+ *
+ * There seems to be also (unverified) constraints:
+ *  - the PLL pre-divider output rate should be in the 4-27MHz range
+ *  - the PLL multiplier output rate should be in the 500-1000MHz range
+ *  - PCLK >= SCLK * 2 in YUV, >= SCLK in Raw or JPEG
+ *
+ * In the two latter cases, these constraints are met since our
+ * factors are hardcoded. If we were to change that, we would need to
+ * take this into account. The only varying parts are the PLL
+ * multiplier and the system clock divider, which are shared between
+ * all these clocks so won't cause any issue.
+ */
+
+/*
+ * This is supposed to be ranging from 1 to 8, but the value is always
+ * set to 3 in the vendor kernels.
+ */
+#define OV5640_PLL_PREDIV	3
+
+#define OV5640_PLL_MULT_MIN	4
+#define OV5640_PLL_MULT_MAX	252
+
+/*
+ * This is supposed to be ranging from 1 to 16, but the value is
+ * always set to either 1 or 2 in the vendor kernels.
+ */
+#define OV5640_SYSDIV_MIN	1
+#define OV5640_SYSDIV_MAX	16
+
+/*
+ * Hardcode these values for scaler and non-scaler modes.
+ * FIXME: to be re-calcualted for 1 data lanes setups
+ */
+#define OV5640_MIPI_DIV_PCLK	2
+#define OV5640_MIPI_DIV_SCLK	1
+
+/*
+ * This is supposed to be ranging from 1 to 2, but the value is always
+ * set to 2 in the vendor kernels.
+ */
+#define OV5640_PLL_ROOT_DIV			2
+#define OV5640_PLL_CTRL3_PLL_ROOT_DIV_2		BIT(4)
+
+/*
+ * We only supports 8-bit formats at the moment
+ */
+#define OV5640_BIT_DIV				2
+#define OV5640_PLL_CTRL0_MIPI_MODE_8BIT		0x08
+
+/*
+ * This is supposed to be ranging from 1 to 8, but the value is always
+ * set to 2 in the vendor kernels.
+ */
+#define OV5640_SCLK_ROOT_DIV	2
+
+/*
+ * This is hardcoded so that the consistency is maintained between SCLK and
+ * SCLK 2x.
+ */
+#define OV5640_SCLK2X_ROOT_DIV (OV5640_SCLK_ROOT_DIV / 2)
+
+/*
+ * This is supposed to be ranging from 1 to 8, but the value is always
+ * set to 1 in the vendor kernels.
+ */
+#define OV5640_PCLK_ROOT_DIV			1
+#define OV5640_PLL_SYS_ROOT_DIVIDER_BYPASS	0x00
+
+static unsigned long ov5640_compute_sys_clk(struct ov5640_dev *sensor,
+					    u8 pll_prediv, u8 pll_mult,
+					    u8 sysdiv)
+{
+	unsigned long sysclk = sensor->xclk_freq / pll_prediv * pll_mult;
+
+	/* PLL1 output cannot exceed 1GHz. */
+	if (sysclk / 1000000 > 1000)
+		return 0;
+
+	return sysclk / sysdiv;
+}
+
+static unsigned long ov5640_calc_sys_clk(struct ov5640_dev *sensor,
+					 unsigned long rate,
+					 u8 *pll_prediv, u8 *pll_mult,
+					 u8 *sysdiv)
+{
+	unsigned long best = ~0;
+	u8 best_sysdiv = 1, best_mult = 1;
+	u8 _sysdiv, _pll_mult;
+
+	for (_sysdiv = OV5640_SYSDIV_MIN;
+	     _sysdiv <= OV5640_SYSDIV_MAX;
+	     _sysdiv++) {
+		for (_pll_mult = OV5640_PLL_MULT_MIN;
+		     _pll_mult <= OV5640_PLL_MULT_MAX;
+		     _pll_mult++) {
+			unsigned long _rate;
+
+			/*
+			 * The PLL multiplier cannot be odd if above
+			 * 127.
+			 */
+			if (_pll_mult > 127 && (_pll_mult % 2))
+				continue;
+
+			_rate = ov5640_compute_sys_clk(sensor,
+						       OV5640_PLL_PREDIV,
+						       _pll_mult, _sysdiv);
+
+			/*
+			 * We have reached the maximum allowed PLL1 output,
+			 * increase sysdiv.
+			 */
+			if (!_rate)
+				break;
+
+			/*
+			 * Prefer rates above the expected clock rate than
+			 * below, even if that means being less precise.
+			 */
+			if (_rate < rate)
+				continue;
+
+			if (abs(rate - _rate) < abs(rate - best)) {
+				best = _rate;
+				best_sysdiv = _sysdiv;
+				best_mult = _pll_mult;
+			}
+
+			if (_rate == rate)
+				goto out;
+		}
+	}
+
+out:
+	*sysdiv = best_sysdiv;
+	*pll_prediv = OV5640_PLL_PREDIV;
+	*pll_mult = best_mult;
+
+	return best;
+}
+
+/*
+ * ov5640_set_mipi_pclk() - Calculate the clock tree configuration values
+ *			    for the MIPI CSI-2 output.
+ *
+ * @rate: The requested bandwidth per lane in bytes per second.
+ *	  'Bandwidth Per Lane' is calculated as:
+ *	  bpl = HTOT * VTOT * FPS * bpp / num_lanes;
+ *
+ * This function use the requested bandwidth to calculate:
+ * - sample_rate = bpl / (bpp / num_lanes);
+ *	         = bpl / (PLL_RDIV * BIT_DIV * PCLK_DIV * MIPI_DIV / num_lanes);
+ *
+ * - mipi_sclk   = bpl / MIPI_DIV / 2; ( / 2 is for CSI-2 DDR)
+ *
+ * with these fixed parameters:
+ *	PLL_RDIV	= 2;
+ *	BIT_DIVIDER	= 2; (MIPI_BIT_MODE == 8 ? 2 : 2,5);
+ *	PCLK_DIV	= 1;
+ *
+ * The MIPI clock generation differs for modes that use the scaler and modes
+ * that do not. In case the scaler is in use, the MIPI_SCLK generates the MIPI
+ * BIT CLk, and thus:
+ *
+ * - mipi_sclk = bpl / MIPI_DIV / 2;
+ *   MIPI_DIV = 1;
+ *
+ * For modes that do not go through the scaler, the MIPI BIT CLOCK is generated
+ * from the pixel clock, and thus:
+ *
+ * - sample_rate = bpl / (bpp / num_lanes);
+ *	         = bpl / (2 * 2 * 1 * MIPI_DIV / num_lanes);
+ *		 = bpl / (4 * MIPI_DIV / num_lanes);
+ * - MIPI_DIV	 = bpp / (4 * num_lanes);
+ *
+ * FIXME: this have been tested with 16bpp and 2 lanes setup only.
+ * MIPI_DIV is fixed to value 2, but it -might- be changed according to the
+ * above formula for setups with 1 lane or image formats with different bpp.
+ *
+ * FIXME: this deviates from the sensor manual documentation which is quite
+ * thin on the MIPI clock tree generation part.
+ */
+static int ov5640_set_mipi_pclk(struct ov5640_dev *sensor,
+				unsigned long rate)
+{
+	const struct ov5640_mode_info *mode = sensor->current_mode;
+	u8 prediv, mult, sysdiv;
+	u8 mipi_div;
+	int ret;
+
+	/*
+	 * 1280x720 is reported to use 'SUBSAMPLING' only,
+	 * but according to the sensor manual it goes through the
+	 * scaler before subsampling.
+	 */
+	if (mode->dn_mode == SCALING ||
+	   (mode->id == OV5640_MODE_720P_1280_720))
+		mipi_div = OV5640_MIPI_DIV_SCLK;
+	else
+		mipi_div = OV5640_MIPI_DIV_PCLK;
+
+	ov5640_calc_sys_clk(sensor, rate, &prediv, &mult, &sysdiv);
+
+	ret = ov5640_mod_reg(sensor, OV5640_REG_SC_PLL_CTRL0,
+			     0x0f, OV5640_PLL_CTRL0_MIPI_MODE_8BIT);
+
+	ret = ov5640_mod_reg(sensor, OV5640_REG_SC_PLL_CTRL1,
+			     0xff, sysdiv << 4 | mipi_div);
+	if (ret)
+		return ret;
+
+	ret = ov5640_mod_reg(sensor, OV5640_REG_SC_PLL_CTRL2, 0xff, mult);
+	if (ret)
+		return ret;
+
+	ret = ov5640_mod_reg(sensor, OV5640_REG_SC_PLL_CTRL3,
+			     0x1f, OV5640_PLL_CTRL3_PLL_ROOT_DIV_2 | prediv);
+	if (ret)
+		return ret;
+
+	return ov5640_mod_reg(sensor, OV5640_REG_SYS_ROOT_DIVIDER,
+			      0x30, OV5640_PLL_SYS_ROOT_DIVIDER_BYPASS);
+}
+
+static unsigned long ov5640_calc_pclk(struct ov5640_dev *sensor,
+				      unsigned long rate,
+				      u8 *pll_prediv, u8 *pll_mult, u8 *sysdiv,
+				      u8 *pll_rdiv, u8 *bit_div, u8 *pclk_div)
+{
+	unsigned long _rate = rate * OV5640_PLL_ROOT_DIV * OV5640_BIT_DIV *
+				OV5640_PCLK_ROOT_DIV;
+
+	_rate = ov5640_calc_sys_clk(sensor, _rate, pll_prediv, pll_mult,
+				    sysdiv);
+	*pll_rdiv = OV5640_PLL_ROOT_DIV;
+	*bit_div = OV5640_BIT_DIV;
+	*pclk_div = OV5640_PCLK_ROOT_DIV;
+
+	return _rate / *pll_rdiv / *bit_div / *pclk_div;
+}
+
+static int ov5640_set_dvp_pclk(struct ov5640_dev *sensor, unsigned long rate)
+{
+	u8 prediv, mult, sysdiv, pll_rdiv, bit_div, pclk_div;
+	int ret;
+
+	ov5640_calc_pclk(sensor, rate, &prediv, &mult, &sysdiv, &pll_rdiv,
+			 &bit_div, &pclk_div);
+
+#ifndef CONFIG_VIN_SENSOR_OV5640
+	if (bit_div == 2)
+		bit_div = 8;
+#else
+	bit_div = 0xa;
+#endif
+	ret = ov5640_mod_reg(sensor, OV5640_REG_SC_PLL_CTRL0,
+			     0x0f, bit_div);
+	if (ret)
+		return ret;
+
+	/*
+	 * We need to set sysdiv according to the clock, and to clear
+	 * the MIPI divider.
+	 */
+	ret = ov5640_mod_reg(sensor, OV5640_REG_SC_PLL_CTRL1,
+			     0xff, sysdiv << 4);
+	if (ret)
+		return ret;
+
+	ret = ov5640_mod_reg(sensor, OV5640_REG_SC_PLL_CTRL2,
+			     0xff, mult);
+	if (ret)
+		return ret;
+
+	ret = ov5640_mod_reg(sensor, OV5640_REG_SC_PLL_CTRL3,
+			     0x1f, prediv | ((pll_rdiv - 1) << 4));
+	if (ret)
+		return ret;
+
+	return ov5640_mod_reg(sensor, OV5640_REG_SYS_ROOT_DIVIDER, 0x30,
+			      (ilog2(pclk_div) << 4));
+}
+
+/* set JPEG framing sizes */
+static int ov5640_set_jpeg_timings(struct ov5640_dev *sensor,
+				   const struct ov5640_mode_info *mode)
+{
+	int ret;
+
+	/*
+	 * compression mode 3 timing
+	 *
+	 * Data is transmitted with programmable width (VFIFO_HSIZE).
+	 * No padding done. Last line may have less data. Varying
+	 * number of lines per frame, depending on amount of data.
+	 */
+	ret = ov5640_mod_reg(sensor, OV5640_REG_JPG_MODE_SELECT, 0x7, 0x3);
+	if (ret < 0)
+		return ret;
+
+	ret = ov5640_write_reg16(sensor, OV5640_REG_VFIFO_HSIZE, mode->hact);
+	if (ret < 0)
+		return ret;
+
+	return ov5640_write_reg16(sensor, OV5640_REG_VFIFO_VSIZE, mode->vact);
+}
+
+/* download ov5640 settings to sensor through i2c */
+static int ov5640_set_timings(struct ov5640_dev *sensor,
+			      const struct ov5640_mode_info *mode)
+{
+	int ret;
+
+	if (sensor->fmt.code == MEDIA_BUS_FMT_JPEG_1X8) {
+		ret = ov5640_set_jpeg_timings(sensor, mode);
+		if (ret < 0)
+			return ret;
+	}
+
+	ret = ov5640_write_reg16(sensor, OV5640_REG_TIMING_DVPHO, mode->hact);
+	if (ret < 0)
+		return ret;
+
+	ret = ov5640_write_reg16(sensor, OV5640_REG_TIMING_DVPVO, mode->vact);
+	if (ret < 0)
+		return ret;
+
+	ret = ov5640_write_reg16(sensor, OV5640_REG_TIMING_HTS, mode->htot);
+	if (ret < 0)
+		return ret;
+
+	return ov5640_write_reg16(sensor, OV5640_REG_TIMING_VTS, mode->vtot);
+}
+
+static int ov5640_load_regs(struct ov5640_dev *sensor,
+			    const struct ov5640_mode_info *mode)
+{
+	const struct reg_value *regs = mode->reg_data;
+	unsigned int i;
+	u32 delay_ms;
+	u16 reg_addr;
+	u8 mask, val;
+	int ret = 0;
+
+	st_info(ST_SENSOR, "%s, mode = 0x%x\n", __func__, mode->id);
+	for (i = 0; i < mode->reg_data_size; ++i, ++regs) {
+		delay_ms = regs->delay_ms;
+		reg_addr = regs->reg_addr;
+		val = regs->val;
+		mask = regs->mask;
+
+		/* remain in power down mode for DVP */
+		if (regs->reg_addr == OV5640_REG_SYS_CTRL0 &&
+		    val == OV5640_REG_SYS_CTRL0_SW_PWUP &&
+		    sensor->ep.bus_type != V4L2_MBUS_CSI2_DPHY)
+			continue;
+
+		if (mask)
+			ret = ov5640_mod_reg(sensor, reg_addr, mask, val);
+		else
+			ret = ov5640_write_reg(sensor, reg_addr, val);
+		if (ret)
+			break;
+
+		if (delay_ms)
+			usleep_range(1000 * delay_ms, 1000 * delay_ms + 100);
+	}
+
+	return ov5640_set_timings(sensor, mode);
+}
+
+static int ov5640_set_autoexposure(struct ov5640_dev *sensor, bool on)
+{
+	return ov5640_mod_reg(sensor, OV5640_REG_AEC_PK_MANUAL,
+			      BIT(0), on ? 0 : BIT(0));
+}
+
+/* read exposure, in number of line periods */
+static int ov5640_get_exposure(struct ov5640_dev *sensor)
+{
+	int exp, ret;
+	u8 temp;
+
+	ret = ov5640_read_reg(sensor, OV5640_REG_AEC_PK_EXPOSURE_HI, &temp);
+	if (ret)
+		return ret;
+	exp = ((int)temp & 0x0f) << 16;
+	ret = ov5640_read_reg(sensor, OV5640_REG_AEC_PK_EXPOSURE_MED, &temp);
+	if (ret)
+		return ret;
+	exp |= ((int)temp << 8);
+	ret = ov5640_read_reg(sensor, OV5640_REG_AEC_PK_EXPOSURE_LO, &temp);
+	if (ret)
+		return ret;
+	exp |= (int)temp;
+
+	return exp >> 4;
+}
+
+/* write exposure, given number of line periods */
+static int ov5640_set_exposure(struct ov5640_dev *sensor, u32 exposure)
+{
+	int ret;
+
+	exposure <<= 4;
+
+	ret = ov5640_write_reg(sensor,
+			       OV5640_REG_AEC_PK_EXPOSURE_LO,
+			       exposure & 0xff);
+	if (ret)
+		return ret;
+	ret = ov5640_write_reg(sensor,
+			       OV5640_REG_AEC_PK_EXPOSURE_MED,
+			       (exposure >> 8) & 0xff);
+	if (ret)
+		return ret;
+	return ov5640_write_reg(sensor,
+				OV5640_REG_AEC_PK_EXPOSURE_HI,
+				(exposure >> 16) & 0x0f);
+}
+
+static int ov5640_get_gain(struct ov5640_dev *sensor)
+{
+	u16 gain;
+	int ret;
+
+	ret = ov5640_read_reg16(sensor, OV5640_REG_AEC_PK_REAL_GAIN, &gain);
+	if (ret)
+		return ret;
+
+	return gain & 0x3ff;
+}
+
+static int ov5640_set_gain(struct ov5640_dev *sensor, int gain)
+{
+	return ov5640_write_reg16(sensor, OV5640_REG_AEC_PK_REAL_GAIN,
+				  (u16)gain & 0x3ff);
+}
+
+static int ov5640_set_autogain(struct ov5640_dev *sensor, bool on)
+{
+	return ov5640_mod_reg(sensor, OV5640_REG_AEC_PK_MANUAL,
+			      BIT(1), on ? 0 : BIT(1));
+}
+
+static int ov5640_set_stream_dvp(struct ov5640_dev *sensor, bool on)
+{
+	return ov5640_write_reg(sensor, OV5640_REG_SYS_CTRL0, on ?
+				OV5640_REG_SYS_CTRL0_SW_PWUP :
+				OV5640_REG_SYS_CTRL0_SW_PWDN);
+}
+
+static int ov5640_set_stream_mipi(struct ov5640_dev *sensor, bool on)
+{
+	int ret;
+
+	/*
+	 * Enable/disable the MIPI interface
+	 *
+	 * 0x300e = on ? 0x45 : 0x40
+	 *
+	 * FIXME: the sensor manual (version 2.03) reports
+	 * [7:5] = 000  : 1 data lane mode
+	 * [7:5] = 001  : 2 data lanes mode
+	 * But this settings do not work, while the following ones
+	 * have been validated for 2 data lanes mode.
+	 *
+	 * [7:5] = 010	: 2 data lanes mode
+	 * [4] = 0	: Power up MIPI HS Tx
+	 * [3] = 0	: Power up MIPI LS Rx
+	 * [2] = 1/0	: MIPI interface enable/disable
+	 * [1:0] = 01/00: FIXME: 'debug'
+	 */
+	ret = ov5640_write_reg(sensor, OV5640_REG_IO_MIPI_CTRL00,
+			       on ? 0x45 : 0x40);
+	if (ret)
+		return ret;
+
+	return ov5640_write_reg(sensor, OV5640_REG_FRAME_CTRL01,
+				on ? 0x00 : 0x0f);
+}
+
+static int ov5640_get_sysclk(struct ov5640_dev *sensor)
+{
+	 /* calculate sysclk */
+	u32 xvclk = sensor->xclk_freq / 10000;
+	u32 multiplier, prediv, VCO, sysdiv, pll_rdiv;
+	u32 sclk_rdiv_map[] = {1, 2, 4, 8};
+	u32 bit_div2x = 1, sclk_rdiv, sysclk;
+	u8 temp1, temp2;
+	int ret;
+
+	ret = ov5640_read_reg(sensor, OV5640_REG_SC_PLL_CTRL0, &temp1);
+	if (ret)
+		return ret;
+	temp2 = temp1 & 0x0f;
+	if (temp2 == 8 || temp2 == 10)
+		bit_div2x = temp2 / 2;
+
+	ret = ov5640_read_reg(sensor, OV5640_REG_SC_PLL_CTRL1, &temp1);
+	if (ret)
+		return ret;
+	sysdiv = temp1 >> 4;
+	if (sysdiv == 0)
+		sysdiv = 16;
+
+	ret = ov5640_read_reg(sensor, OV5640_REG_SC_PLL_CTRL2, &temp1);
+	if (ret)
+		return ret;
+	multiplier = temp1;
+
+	ret = ov5640_read_reg(sensor, OV5640_REG_SC_PLL_CTRL3, &temp1);
+	if (ret)
+		return ret;
+	prediv = temp1 & 0x0f;
+	pll_rdiv = ((temp1 >> 4) & 0x01) + 1;
+
+	ret = ov5640_read_reg(sensor, OV5640_REG_SYS_ROOT_DIVIDER, &temp1);
+	if (ret)
+		return ret;
+	temp2 = temp1 & 0x03;
+	sclk_rdiv = sclk_rdiv_map[temp2];
+
+	if (!prediv || !sysdiv || !pll_rdiv || !bit_div2x)
+		return -EINVAL;
+
+	VCO = xvclk * multiplier / prediv;
+
+	sysclk = VCO / sysdiv / pll_rdiv * 2 / bit_div2x / sclk_rdiv;
+
+	return sysclk;
+}
+
+static int ov5640_set_night_mode(struct ov5640_dev *sensor)
+{
+	 /* read HTS from register settings */
+	u8 mode;
+	int ret;
+
+	ret = ov5640_read_reg(sensor, OV5640_REG_AEC_CTRL00, &mode);
+	if (ret)
+		return ret;
+	mode &= 0xfb;
+	return ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL00, mode);
+}
+
+static int ov5640_get_hts(struct ov5640_dev *sensor)
+{
+	/* read HTS from register settings */
+	u16 hts;
+	int ret;
+
+	ret = ov5640_read_reg16(sensor, OV5640_REG_TIMING_HTS, &hts);
+	if (ret)
+		return ret;
+	return hts;
+}
+
+static int ov5640_get_vts(struct ov5640_dev *sensor)
+{
+	u16 vts;
+	int ret;
+
+	ret = ov5640_read_reg16(sensor, OV5640_REG_TIMING_VTS, &vts);
+	if (ret)
+		return ret;
+	return vts;
+}
+
+static int ov5640_set_vts(struct ov5640_dev *sensor, int vts)
+{
+	return ov5640_write_reg16(sensor, OV5640_REG_TIMING_VTS, vts);
+}
+
+static int ov5640_get_light_freq(struct ov5640_dev *sensor)
+{
+	/* get banding filter value */
+	int ret, light_freq = 0;
+	u8 temp, temp1;
+
+	ret = ov5640_read_reg(sensor, OV5640_REG_HZ5060_CTRL01, &temp);
+	if (ret)
+		return ret;
+
+	if (temp & 0x80) {
+		/* manual */
+		ret = ov5640_read_reg(sensor, OV5640_REG_HZ5060_CTRL00,
+				      &temp1);
+		if (ret)
+			return ret;
+		if (temp1 & 0x04) {
+			/* 50Hz */
+			light_freq = 50;
+		} else {
+			/* 60Hz */
+			light_freq = 60;
+		}
+	} else {
+		/* auto */
+		ret = ov5640_read_reg(sensor, OV5640_REG_SIGMADELTA_CTRL0C,
+				      &temp1);
+		if (ret)
+			return ret;
+
+		if (temp1 & 0x01) {
+			/* 50Hz */
+			light_freq = 50;
+		} else {
+			/* 60Hz */
+		}
+	}
+
+	return light_freq;
+}
+
+static int ov5640_set_bandingfilter(struct ov5640_dev *sensor)
+{
+	u32 band_step60, max_band60, band_step50, max_band50, prev_vts;
+	int ret;
+
+	/* read preview PCLK */
+	ret = ov5640_get_sysclk(sensor);
+	if (ret < 0)
+		return ret;
+	if (ret == 0)
+		return -EINVAL;
+	sensor->prev_sysclk = ret;
+	/* read preview HTS */
+	ret = ov5640_get_hts(sensor);
+	if (ret < 0)
+		return ret;
+	if (ret == 0)
+		return -EINVAL;
+	sensor->prev_hts = ret;
+
+	/* read preview VTS */
+	ret = ov5640_get_vts(sensor);
+	if (ret < 0)
+		return ret;
+	prev_vts = ret;
+
+	/* calculate banding filter */
+	/* 60Hz */
+	band_step60 = sensor->prev_sysclk * 100 / sensor->prev_hts * 100 / 120;
+	ret = ov5640_write_reg16(sensor, OV5640_REG_AEC_B60_STEP, band_step60);
+	if (ret)
+		return ret;
+	if (!band_step60)
+		return -EINVAL;
+	max_band60 = (int)((prev_vts - 4) / band_step60);
+	ret = ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL0D, max_band60);
+	if (ret)
+		return ret;
+
+	/* 50Hz */
+	band_step50 = sensor->prev_sysclk * 100 / sensor->prev_hts;
+	ret = ov5640_write_reg16(sensor, OV5640_REG_AEC_B50_STEP, band_step50);
+	if (ret)
+		return ret;
+	if (!band_step50)
+		return -EINVAL;
+	max_band50 = (int)((prev_vts - 4) / band_step50);
+	return ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL0E, max_band50);
+}
+
+static int ov5640_set_ae_target(struct ov5640_dev *sensor, int target)
+{
+	/* stable in high */
+	u32 fast_high, fast_low;
+	int ret;
+
+	sensor->ae_low = target * 23 / 25;	/* 0.92 */
+	sensor->ae_high = target * 27 / 25;	/* 1.08 */
+
+	fast_high = sensor->ae_high << 1;
+	if (fast_high > 255)
+		fast_high = 255;
+
+	fast_low = sensor->ae_low >> 1;
+
+	ret = ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL0F, sensor->ae_high);
+	if (ret)
+		return ret;
+	ret = ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL10, sensor->ae_low);
+	if (ret)
+		return ret;
+	ret = ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL1B, sensor->ae_high);
+	if (ret)
+		return ret;
+	ret = ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL1E, sensor->ae_low);
+	if (ret)
+		return ret;
+	ret = ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL11, fast_high);
+	if (ret)
+		return ret;
+	return ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL1F, fast_low);
+}
+
+static int ov5640_get_binning(struct ov5640_dev *sensor)
+{
+	u8 temp;
+	int ret;
+
+	ret = ov5640_read_reg(sensor, OV5640_REG_TIMING_TC_REG21, &temp);
+	if (ret)
+		return ret;
+
+	return temp & BIT(0);
+}
+
+static int ov5640_set_binning(struct ov5640_dev *sensor, bool enable)
+{
+	int ret;
+
+	/*
+	 * TIMING TC REG21:
+	 * - [0]:	Horizontal binning enable
+	 */
+	ret = ov5640_mod_reg(sensor, OV5640_REG_TIMING_TC_REG21,
+			     BIT(0), enable ? BIT(0) : 0);
+	if (ret)
+		return ret;
+	/*
+	 * TIMING TC REG20:
+	 * - [0]:	Undocumented, but hardcoded init sequences
+	 *		are always setting REG21/REG20 bit 0 to same value...
+	 */
+	return ov5640_mod_reg(sensor, OV5640_REG_TIMING_TC_REG20,
+			      BIT(0), enable ? BIT(0) : 0);
+}
+
+static int ov5640_set_virtual_channel(struct ov5640_dev *sensor)
+{
+	struct i2c_client *client = sensor->i2c_client;
+	u8 temp, channel = virtual_channel;
+	int ret;
+
+	if (channel > 3) {
+		dev_err(&client->dev,
+			"%s: wrong virtual_channel parameter, expected (0..3), got %d\n",
+			__func__, channel);
+		return -EINVAL;
+	}
+
+	ret = ov5640_read_reg(sensor, OV5640_REG_DEBUG_MODE, &temp);
+	if (ret)
+		return ret;
+	temp &= ~(3 << 6);
+	temp |= (channel << 6);
+	return ov5640_write_reg(sensor, OV5640_REG_DEBUG_MODE, temp);
+}
+
+static const struct ov5640_mode_info *
+ov5640_find_mode(struct ov5640_dev *sensor, enum ov5640_frame_rate fr,
+		 int width, int height, bool nearest)
+{
+	const struct ov5640_mode_info *mode;
+
+	mode = v4l2_find_nearest_size(ov5640_mode_data,
+				      ARRAY_SIZE(ov5640_mode_data),
+				      hact, vact,
+				      width, height);
+
+	if (!mode ||
+	    (!nearest && (mode->hact != width || mode->vact != height)))
+		return NULL;
+
+	/* Check to see if the current mode exceeds the max frame rate */
+	if (ov5640_framerates[fr] > ov5640_framerates[mode->max_fps])
+		return NULL;
+
+	return mode;
+}
+
+static u64 ov5640_calc_pixel_rate(struct ov5640_dev *sensor)
+{
+	u64 rate;
+
+	rate = sensor->current_mode->vtot * sensor->current_mode->htot;
+	rate *= ov5640_framerates[sensor->current_fr];
+
+	return rate;
+}
+
+/*
+ * sensor changes between scaling and subsampling, go through
+ * exposure calculation
+ */
+static int ov5640_set_mode_exposure_calc(struct ov5640_dev *sensor,
+					 const struct ov5640_mode_info *mode)
+{
+	u32 prev_shutter, prev_gain16;
+	u32 cap_shutter, cap_gain16;
+	u32 cap_sysclk, cap_hts, cap_vts;
+	u32 light_freq, cap_bandfilt, cap_maxband;
+	u32 cap_gain16_shutter;
+	u8 average;
+	int ret;
+
+	if (!mode->reg_data)
+		return -EINVAL;
+
+	/* read preview shutter */
+	ret = ov5640_get_exposure(sensor);
+	if (ret < 0)
+		return ret;
+	prev_shutter = ret;
+	ret = ov5640_get_binning(sensor);
+	if (ret < 0)
+		return ret;
+	if (ret && mode->id != OV5640_MODE_720P_1280_720 &&
+	    mode->id != OV5640_MODE_1080P_1920_1080)
+		prev_shutter *= 2;
+
+	/* read preview gain */
+	ret = ov5640_get_gain(sensor);
+	if (ret < 0)
+		return ret;
+	prev_gain16 = ret;
+
+	/* get average */
+	ret = ov5640_read_reg(sensor, OV5640_REG_AVG_READOUT, &average);
+	if (ret)
+		return ret;
+
+	/* turn off night mode for capture */
+	ret = ov5640_set_night_mode(sensor);
+	if (ret < 0)
+		return ret;
+
+	/* Write capture setting */
+	ret = ov5640_load_regs(sensor, mode);
+	if (ret < 0)
+		return ret;
+
+	/* read capture VTS */
+	ret = ov5640_get_vts(sensor);
+	if (ret < 0)
+		return ret;
+	cap_vts = ret;
+	ret = ov5640_get_hts(sensor);
+	if (ret < 0)
+		return ret;
+	if (ret == 0)
+		return -EINVAL;
+	cap_hts = ret;
+
+	ret = ov5640_get_sysclk(sensor);
+	if (ret < 0)
+		return ret;
+	if (ret == 0)
+		return -EINVAL;
+	cap_sysclk = ret;
+
+	/* calculate capture banding filter */
+	ret = ov5640_get_light_freq(sensor);
+	if (ret < 0)
+		return ret;
+	light_freq = ret;
+
+	if (light_freq == 60) {
+		/* 60Hz */
+		cap_bandfilt = cap_sysclk * 100 / cap_hts * 100 / 120;
+	} else {
+		/* 50Hz */
+		cap_bandfilt = cap_sysclk * 100 / cap_hts;
+	}
+
+	if (!sensor->prev_sysclk) {
+		ret = ov5640_get_sysclk(sensor);
+		if (ret < 0)
+			return ret;
+		if (ret == 0)
+			return -EINVAL;
+		sensor->prev_sysclk = ret;
+	}
+
+	if (!cap_bandfilt)
+		return -EINVAL;
+
+	cap_maxband = (int)((cap_vts - 4) / cap_bandfilt);
+
+	/* calculate capture shutter/gain16 */
+	if (average > sensor->ae_low && average < sensor->ae_high) {
+		/* in stable range */
+		cap_gain16_shutter =
+			prev_gain16 * prev_shutter *
+			cap_sysclk / sensor->prev_sysclk *
+			sensor->prev_hts / cap_hts *
+			sensor->ae_target / average;
+	} else {
+		cap_gain16_shutter =
+			prev_gain16 * prev_shutter *
+			cap_sysclk / sensor->prev_sysclk *
+			sensor->prev_hts / cap_hts;
+	}
+
+	/* gain to shutter */
+	if (cap_gain16_shutter < (cap_bandfilt * 16)) {
+		/* shutter < 1/100 */
+		cap_shutter = cap_gain16_shutter / 16;
+		if (cap_shutter < 1)
+			cap_shutter = 1;
+
+		cap_gain16 = cap_gain16_shutter / cap_shutter;
+		if (cap_gain16 < 16)
+			cap_gain16 = 16;
+	} else {
+		if (cap_gain16_shutter > (cap_bandfilt * cap_maxband * 16)) {
+			/* exposure reach max */
+			cap_shutter = cap_bandfilt * cap_maxband;
+			if (!cap_shutter)
+				return -EINVAL;
+
+			cap_gain16 = cap_gain16_shutter / cap_shutter;
+		} else {
+			/* 1/100 < (cap_shutter = n/100) =< max */
+			cap_shutter =
+				((int)(cap_gain16_shutter / 16 / cap_bandfilt))
+				* cap_bandfilt;
+			if (!cap_shutter)
+				return -EINVAL;
+
+			cap_gain16 = cap_gain16_shutter / cap_shutter;
+		}
+	}
+
+	/* set capture gain */
+	ret = ov5640_set_gain(sensor, cap_gain16);
+	if (ret)
+		return ret;
+
+	/* write capture shutter */
+	if (cap_shutter > (cap_vts - 4)) {
+		cap_vts = cap_shutter + 4;
+		ret = ov5640_set_vts(sensor, cap_vts);
+		if (ret < 0)
+			return ret;
+	}
+
+	/* set exposure */
+	return ov5640_set_exposure(sensor, cap_shutter);
+}
+
+/*
+ * if sensor changes inside scaling or subsampling
+ * change mode directly
+ */
+static int ov5640_set_mode_direct(struct ov5640_dev *sensor,
+				  const struct ov5640_mode_info *mode)
+{
+	if (!mode->reg_data)
+		return -EINVAL;
+
+	/* Write capture setting */
+	return ov5640_load_regs(sensor, mode);
+}
+
+static int ov5640_set_mode(struct ov5640_dev *sensor)
+{
+	const struct ov5640_mode_info *mode = sensor->current_mode;
+	const struct ov5640_mode_info *orig_mode = sensor->last_mode;
+	enum ov5640_downsize_mode dn_mode, orig_dn_mode;
+	bool auto_gain = sensor->ctrls.auto_gain->val == 1;
+	bool auto_exp =  sensor->ctrls.auto_exp->val == V4L2_EXPOSURE_AUTO;
+	unsigned long rate;
+	int ret;
+
+	dn_mode = mode->dn_mode;
+	orig_dn_mode = orig_mode->dn_mode;
+
+	/* auto gain and exposure must be turned off when changing modes */
+	if (auto_gain) {
+		ret = ov5640_set_autogain(sensor, false);
+		if (ret)
+			return ret;
+	}
+
+	if (auto_exp) {
+		ret = ov5640_set_autoexposure(sensor, false);
+		if (ret)
+			goto restore_auto_gain;
+	}
+
+	/*
+	 * All the formats we support have 16 bits per pixel, seems to require
+	 * the same rate than YUV, so we can just use 16 bpp all the time.
+	 */
+	rate = ov5640_calc_pixel_rate(sensor) * 16;
+	if (sensor->ep.bus_type == V4L2_MBUS_CSI2_DPHY) {
+		rate = rate / sensor->ep.bus.mipi_csi2.num_data_lanes;
+		ret = ov5640_set_mipi_pclk(sensor, rate);
+	} else {
+		rate = rate / sensor->ep.bus.parallel.bus_width;
+		ret = ov5640_set_dvp_pclk(sensor, rate);
+	}
+
+	if (ret < 0)
+		return 0;
+
+	if ((dn_mode == SUBSAMPLING && orig_dn_mode == SCALING) ||
+	    (dn_mode == SCALING && orig_dn_mode == SUBSAMPLING)) {
+		/*
+		 * change between subsampling and scaling
+		 * go through exposure calculation
+		 */
+		ret = ov5640_set_mode_exposure_calc(sensor, mode);
+	} else {
+		/*
+		 * change inside subsampling or scaling
+		 * download firmware directly
+		 */
+		ret = ov5640_set_mode_direct(sensor, mode);
+	}
+	if (ret < 0)
+		goto restore_auto_exp_gain;
+
+	/* restore auto gain and exposure */
+	if (auto_gain)
+		ov5640_set_autogain(sensor, true);
+	if (auto_exp)
+		ov5640_set_autoexposure(sensor, true);
+
+	ret = ov5640_set_binning(sensor, dn_mode != SCALING);
+	if (ret < 0)
+		return ret;
+	ret = ov5640_set_ae_target(sensor, sensor->ae_target);
+	if (ret < 0)
+		return ret;
+	ret = ov5640_get_light_freq(sensor);
+	if (ret < 0)
+		return ret;
+	ret = ov5640_set_bandingfilter(sensor);
+	if (ret < 0)
+		return ret;
+	ret = ov5640_set_virtual_channel(sensor);
+	if (ret < 0)
+		return ret;
+
+	sensor->pending_mode_change = false;
+	sensor->last_mode = mode;
+
+	return 0;
+
+restore_auto_exp_gain:
+	if (auto_exp)
+		ov5640_set_autoexposure(sensor, true);
+restore_auto_gain:
+	if (auto_gain)
+		ov5640_set_autogain(sensor, true);
+
+	return ret;
+}
+
+static int ov5640_set_framefmt(struct ov5640_dev *sensor,
+			       struct v4l2_mbus_framefmt *format);
+
+/* restore the last set video mode after chip power-on */
+static int ov5640_restore_mode(struct ov5640_dev *sensor)
+{
+	int ret;
+
+	/* first load the initial register values */
+	ret = ov5640_load_regs(sensor, &ov5640_mode_init_data);
+	if (ret < 0)
+		return ret;
+	sensor->last_mode = &ov5640_mode_init_data;
+
+	ret = ov5640_mod_reg(sensor, OV5640_REG_SYS_ROOT_DIVIDER, 0x3f,
+			     (ilog2(OV5640_SCLK2X_ROOT_DIV) << 2) |
+			     ilog2(OV5640_SCLK_ROOT_DIV));
+	if (ret)
+		return ret;
+
+	/* now restore the last capture mode */
+	ret = ov5640_set_mode(sensor);
+	if (ret < 0)
+		return ret;
+
+	return ov5640_set_framefmt(sensor, &sensor->fmt);
+}
+
+static void ov5640_power(struct ov5640_dev *sensor, bool enable)
+{
+	if (!sensor->pwdn_gpio)
+		return;
+	gpiod_set_value_cansleep(sensor->pwdn_gpio, enable ? 0 : 1);
+}
+
+static void ov5640_reset(struct ov5640_dev *sensor)
+{
+	if (!sensor->reset_gpio)
+		return;
+
+	gpiod_set_value_cansleep(sensor->reset_gpio, 0);
+
+	/* camera power cycle */
+	ov5640_power(sensor, false);
+	usleep_range(5000, 10000);
+	ov5640_power(sensor, true);
+	usleep_range(5000, 10000);
+
+	gpiod_set_value_cansleep(sensor->reset_gpio, 1);
+	usleep_range(1000, 2000);
+
+	gpiod_set_value_cansleep(sensor->reset_gpio, 0);
+	usleep_range(20000, 25000);
+}
+
+static int ov5640_set_power_on(struct ov5640_dev *sensor)
+{
+	struct i2c_client *client = sensor->i2c_client;
+	int ret;
+
+	ret = clk_prepare_enable(sensor->xclk);
+	if (ret) {
+		dev_err(&client->dev, "%s: failed to enable clock\n",
+			__func__);
+		return ret;
+	}
+
+	ret = regulator_bulk_enable(OV5640_NUM_SUPPLIES,
+				    sensor->supplies);
+	if (ret) {
+		dev_err(&client->dev, "%s: failed to enable regulators\n",
+			__func__);
+		goto xclk_off;
+	}
+
+	ov5640_reset(sensor);
+	ov5640_power(sensor, true);
+
+	ret = ov5640_init_slave_id(sensor);
+	if (ret)
+		goto power_off;
+
+	return 0;
+
+power_off:
+	ov5640_power(sensor, false);
+	regulator_bulk_disable(OV5640_NUM_SUPPLIES, sensor->supplies);
+xclk_off:
+	clk_disable_unprepare(sensor->xclk);
+	return ret;
+}
+
+static void ov5640_set_power_off(struct ov5640_dev *sensor)
+{
+	ov5640_power(sensor, false);
+	regulator_bulk_disable(OV5640_NUM_SUPPLIES, sensor->supplies);
+	clk_disable_unprepare(sensor->xclk);
+}
+
+static int ov5640_set_power_mipi(struct ov5640_dev *sensor, bool on)
+{
+	int ret;
+
+	if (!on) {
+		/* Reset MIPI bus settings to their default values. */
+		ov5640_write_reg(sensor, OV5640_REG_IO_MIPI_CTRL00, 0x58);
+		ov5640_write_reg(sensor, OV5640_REG_MIPI_CTRL00, 0x04);
+		ov5640_write_reg(sensor, OV5640_REG_PAD_OUTPUT00, 0x00);
+		return 0;
+	}
+
+	/*
+	 * Power up MIPI HS Tx and LS Rx; 2 data lanes mode
+	 *
+	 * 0x300e = 0x40
+	 * [7:5] = 010	: 2 data lanes mode (see FIXME note in
+	 *		  "ov5640_set_stream_mipi()")
+	 * [4] = 0	: Power up MIPI HS Tx
+	 * [3] = 0	: Power up MIPI LS Rx
+	 * [2] = 0	: MIPI interface disabled
+	 */
+	ret = ov5640_write_reg(sensor, OV5640_REG_IO_MIPI_CTRL00, 0x40);
+	if (ret)
+		return ret;
+
+	/*
+	 * Gate clock and set LP11 in 'no packets mode' (idle)
+	 *
+	 * 0x4800 = 0x24
+	 * [5] = 1	: Gate clock when 'no packets'
+	 * [2] = 1	: MIPI bus in LP11 when 'no packets'
+	 */
+	ret = ov5640_write_reg(sensor, OV5640_REG_MIPI_CTRL00, 0x24);
+	if (ret)
+		return ret;
+
+	/*
+	 * Set data lanes and clock in LP11 when 'sleeping'
+	 *
+	 * 0x3019 = 0x70
+	 * [6] = 1	: MIPI data lane 2 in LP11 when 'sleeping'
+	 * [5] = 1	: MIPI data lane 1 in LP11 when 'sleeping'
+	 * [4] = 1	: MIPI clock lane in LP11 when 'sleeping'
+	 */
+	ret = ov5640_write_reg(sensor, OV5640_REG_PAD_OUTPUT00, 0x70);
+	if (ret)
+		return ret;
+
+	/* Give lanes some time to coax into LP11 state. */
+	usleep_range(500, 1000);
+
+	return 0;
+}
+
+static int ov5640_set_power_dvp(struct ov5640_dev *sensor, bool on)
+{
+	unsigned int flags = sensor->ep.bus.parallel.flags;
+	bool bt656 = sensor->ep.bus_type == V4L2_MBUS_BT656;
+	u8 polarities = 0;
+	int ret;
+
+	if (!on) {
+		/* Reset settings to their default values. */
+		ov5640_write_reg(sensor, OV5640_REG_CCIR656_CTRL00, 0x00);
+		ov5640_write_reg(sensor, OV5640_REG_IO_MIPI_CTRL00, 0x58);
+		ov5640_write_reg(sensor, OV5640_REG_POLARITY_CTRL00, 0x20);
+		ov5640_write_reg(sensor, OV5640_REG_PAD_OUTPUT_ENABLE01, 0x00);
+		ov5640_write_reg(sensor, OV5640_REG_PAD_OUTPUT_ENABLE02, 0x00);
+		return 0;
+	}
+
+	/*
+	 * Note about parallel port configuration.
+	 *
+	 * When configured in parallel mode, the OV5640 will
+	 * output 10 bits data on DVP data lines [9:0].
+	 * If only 8 bits data are wanted, the 8 bits data lines
+	 * of the camera interface must be physically connected
+	 * on the DVP data lines [9:2].
+	 *
+	 * Control lines polarity can be configured through
+	 * devicetree endpoint control lines properties.
+	 * If no endpoint control lines properties are set,
+	 * polarity will be as below:
+	 * - VSYNC:	active high
+	 * - HREF:	active low
+	 * - PCLK:	active low
+	 *
+	 * VSYNC & HREF are not configured if BT656 bus mode is selected
+	 */
+
+	/*
+	 * BT656 embedded synchronization configuration
+	 *
+	 * CCIR656 CTRL00
+	 * - [7]:	SYNC code selection (0: auto generate sync code,
+	 *		1: sync code from regs 0x4732-0x4735)
+	 * - [6]:	f value in CCIR656 SYNC code when fixed f value
+	 * - [5]:	Fixed f value
+	 * - [4:3]:	Blank toggle data options (00: data=1'h040/1'h200,
+	 *		01: data from regs 0x4736-0x4738, 10: always keep 0)
+	 * - [1]:	Clip data disable
+	 * - [0]:	CCIR656 mode enable
+	 *
+	 * Default CCIR656 SAV/EAV mode with default codes
+	 * SAV=0xff000080 & EAV=0xff00009d is enabled here with settings:
+	 * - CCIR656 mode enable
+	 * - auto generation of sync codes
+	 * - blank toggle data 1'h040/1'h200
+	 * - clip reserved data (0x00 & 0xff changed to 0x01 & 0xfe)
+	 */
+	ret = ov5640_write_reg(sensor, OV5640_REG_CCIR656_CTRL00,
+			       bt656 ? 0x01 : 0x00);
+	if (ret)
+		return ret;
+
+	/*
+	 * configure parallel port control lines polarity
+	 *
+	 * POLARITY CTRL0
+	 * - [5]:	PCLK polarity (0: active low, 1: active high)
+	 * - [1]:	HREF polarity (0: active low, 1: active high)
+	 * - [0]:	VSYNC polarity (mismatch here between
+	 *		datasheet and hardware, 0 is active high
+	 *		and 1 is active low...)
+	 */
+	if (!bt656) {
+		if (flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH)
+			polarities |= BIT(1);
+		if (flags & V4L2_MBUS_VSYNC_ACTIVE_LOW)
+			polarities |= BIT(0);
+	}
+	if (flags & V4L2_MBUS_PCLK_SAMPLE_RISING)
+		polarities |= BIT(5);
+
+	ret = ov5640_write_reg(sensor, OV5640_REG_POLARITY_CTRL00, polarities);
+	if (ret)
+		return ret;
+
+	/*
+	 * powerdown MIPI TX/RX PHY & enable DVP
+	 *
+	 * MIPI CONTROL 00
+	 * [4] = 1	: Power down MIPI HS Tx
+	 * [3] = 1	: Power down MIPI LS Rx
+	 * [2] = 0	: DVP enable (MIPI disable)
+	 */
+	ret = ov5640_write_reg(sensor, OV5640_REG_IO_MIPI_CTRL00, 0x58);
+	if (ret)
+		return ret;
+
+	/*
+	 * enable VSYNC/HREF/PCLK DVP control lines
+	 * & D[9:6] DVP data lines
+	 *
+	 * PAD OUTPUT ENABLE 01
+	 * - 6:		VSYNC output enable
+	 * - 5:		HREF output enable
+	 * - 4:		PCLK output enable
+	 * - [3:0]:	D[9:6] output enable
+	 */
+	ret = ov5640_write_reg(sensor, OV5640_REG_PAD_OUTPUT_ENABLE01,
+			       bt656 ? 0x1f : 0x7f);
+	if (ret)
+		return ret;
+
+	/*
+	 * enable D[5:0] DVP data lines
+	 *
+	 * PAD OUTPUT ENABLE 02
+	 * - [7:2]:	D[5:0] output enable
+	 */
+	return ov5640_write_reg(sensor, OV5640_REG_PAD_OUTPUT_ENABLE02, 0xfc);
+}
+
+static int ov5640_set_power(struct ov5640_dev *sensor, bool on)
+{
+	int ret = 0;
+
+	if (on) {
+		ret = ov5640_set_power_on(sensor);
+		if (ret)
+			return ret;
+
+		ret = ov5640_restore_mode(sensor);
+		if (ret)
+			goto power_off;
+	}
+
+	if (sensor->ep.bus_type == V4L2_MBUS_CSI2_DPHY)
+		ret = ov5640_set_power_mipi(sensor, on);
+	else
+		ret = ov5640_set_power_dvp(sensor, on);
+	if (ret)
+		goto power_off;
+
+	if (!on)
+		ov5640_set_power_off(sensor);
+
+	return 0;
+
+power_off:
+	ov5640_set_power_off(sensor);
+	return ret;
+}
+
+/* --------------- Subdev Operations --------------- */
+
+static int ov5640_s_power(struct v4l2_subdev *sd, int on)
+{
+	struct ov5640_dev *sensor = to_ov5640_dev(sd);
+	int ret = 0;
+
+	mutex_lock(&sensor->lock);
+
+	/*
+	 * If the power count is modified from 0 to != 0 or from != 0 to 0,
+	 * update the power state.
+	 */
+	if (sensor->power_count == !on) {
+		ret = ov5640_set_power(sensor, !!on);
+		if (ret)
+			goto out;
+	}
+
+	/* Update the power count. */
+	sensor->power_count += on ? 1 : -1;
+	WARN_ON(sensor->power_count < 0);
+out:
+	mutex_unlock(&sensor->lock);
+
+	if (on && !ret && sensor->power_count == 1) {
+		/* restore controls */
+		ret = v4l2_ctrl_handler_setup(&sensor->ctrls.handler);
+	}
+
+	return ret;
+}
+
+static int ov5640_try_frame_interval(struct ov5640_dev *sensor,
+				     struct v4l2_fract *fi,
+				     u32 width, u32 height)
+{
+	const struct ov5640_mode_info *mode;
+	enum ov5640_frame_rate rate = OV5640_15_FPS;
+	int minfps, maxfps, best_fps, fps;
+	int i;
+
+	minfps = ov5640_framerates[OV5640_15_FPS];
+	maxfps = ov5640_framerates[OV5640_60_FPS];
+
+	if (fi->numerator == 0) {
+		fi->denominator = maxfps;
+		fi->numerator = 1;
+		rate = OV5640_60_FPS;
+		goto find_mode;
+	}
+
+	fps = clamp_val(DIV_ROUND_CLOSEST(fi->denominator, fi->numerator),
+			minfps, maxfps);
+
+	best_fps = minfps;
+	for (i = 0; i < ARRAY_SIZE(ov5640_framerates); i++) {
+		int curr_fps = ov5640_framerates[i];
+
+		if (abs(curr_fps - fps) < abs(best_fps - fps)) {
+			best_fps = curr_fps;
+			rate = i;
+		}
+	}
+
+	fi->numerator = 1;
+	fi->denominator = best_fps;
+
+find_mode:
+	mode = ov5640_find_mode(sensor, rate, width, height, false);
+	return mode ? rate : -EINVAL;
+}
+
+static int ov5640_get_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_pad_config *cfg,
+			  struct v4l2_subdev_format *format)
+{
+	struct ov5640_dev *sensor = to_ov5640_dev(sd);
+	struct v4l2_mbus_framefmt *fmt;
+
+	if (format->pad != 0)
+		return -EINVAL;
+
+	mutex_lock(&sensor->lock);
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY)
+		fmt = v4l2_subdev_get_try_format(&sensor->sd, cfg,
+						 format->pad);
+	else
+		fmt = &sensor->fmt;
+
+	format->format = *fmt;
+
+	mutex_unlock(&sensor->lock);
+
+	return 0;
+}
+
+static int ov5640_try_fmt_internal(struct v4l2_subdev *sd,
+				   struct v4l2_mbus_framefmt *fmt,
+				   enum ov5640_frame_rate fr,
+				   const struct ov5640_mode_info **new_mode)
+{
+	struct ov5640_dev *sensor = to_ov5640_dev(sd);
+	const struct ov5640_mode_info *mode;
+	int i;
+
+	mode = ov5640_find_mode(sensor, fr, fmt->width, fmt->height, true);
+	if (!mode)
+		return -EINVAL;
+	fmt->width = mode->hact;
+	fmt->height = mode->vact;
+
+	if (new_mode)
+		*new_mode = mode;
+
+	for (i = 0; i < ARRAY_SIZE(ov5640_formats); i++)
+		if (ov5640_formats[i].code == fmt->code)
+			break;
+	if (i >= ARRAY_SIZE(ov5640_formats))
+		i = 0;
+
+	fmt->code = ov5640_formats[i].code;
+	fmt->colorspace = ov5640_formats[i].colorspace;
+	fmt->ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(fmt->colorspace);
+	fmt->quantization = V4L2_QUANTIZATION_FULL_RANGE;
+	fmt->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(fmt->colorspace);
+
+	return 0;
+}
+
+static int ov5640_set_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_pad_config *cfg,
+			  struct v4l2_subdev_format *format)
+{
+	struct ov5640_dev *sensor = to_ov5640_dev(sd);
+	const struct ov5640_mode_info *new_mode;
+	struct v4l2_mbus_framefmt *mbus_fmt = &format->format;
+	struct v4l2_mbus_framefmt *fmt;
+	int ret;
+
+	if (format->pad != 0)
+		return -EINVAL;
+
+	mutex_lock(&sensor->lock);
+
+	if (sensor->streaming) {
+		ret = -EBUSY;
+		goto out;
+	}
+
+	ret = ov5640_try_fmt_internal(sd, mbus_fmt, 0, &new_mode);
+	if (ret)
+		goto out;
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY)
+		fmt = v4l2_subdev_get_try_format(sd, cfg, 0);
+	else
+		fmt = &sensor->fmt;
+
+	if (mbus_fmt->code != sensor->fmt.code)
+		sensor->pending_fmt_change = true;
+
+	*fmt = *mbus_fmt;
+
+	if (new_mode != sensor->current_mode) {
+		sensor->current_mode = new_mode;
+		sensor->pending_mode_change = true;
+	}
+	if (new_mode->max_fps < sensor->current_fr) {
+		sensor->current_fr = new_mode->max_fps;
+		sensor->frame_interval.numerator = 1;
+		sensor->frame_interval.denominator =
+			ov5640_framerates[sensor->current_fr];
+		sensor->current_mode = new_mode;
+		sensor->pending_mode_change = true;
+	}
+
+	__v4l2_ctrl_s_ctrl_int64(sensor->ctrls.pixel_rate,
+				 ov5640_calc_pixel_rate(sensor));
+out:
+	mutex_unlock(&sensor->lock);
+	return ret;
+}
+
+static int ov5640_set_framefmt(struct ov5640_dev *sensor,
+			       struct v4l2_mbus_framefmt *format)
+{
+	int ret = 0;
+	bool is_jpeg = false;
+	u8 fmt, mux;
+
+	switch (format->code) {
+	case MEDIA_BUS_FMT_UYVY8_2X8:
+		/* YUV422, UYVY */
+		fmt = 0x3f;
+		mux = OV5640_FMT_MUX_YUV422;
+		break;
+	case MEDIA_BUS_FMT_YUYV8_2X8:
+		/* YUV422, YUYV */
+		fmt = 0x30;
+		mux = OV5640_FMT_MUX_YUV422;
+		break;
+	case MEDIA_BUS_FMT_RGB565_2X8_LE:
+		/* RGB565 {g[2:0],b[4:0]},{r[4:0],g[5:3]} */
+		fmt = 0x6F;
+		mux = OV5640_FMT_MUX_RGB;
+		break;
+	case MEDIA_BUS_FMT_RGB565_2X8_BE:
+		/* RGB565 {r[4:0],g[5:3]},{g[2:0],b[4:0]} */
+		fmt = 0x61;
+		mux = OV5640_FMT_MUX_RGB;
+		break;
+	case MEDIA_BUS_FMT_JPEG_1X8:
+		/* YUV422, YUYV */
+		fmt = 0x30;
+		mux = OV5640_FMT_MUX_YUV422;
+		is_jpeg = true;
+		break;
+	case MEDIA_BUS_FMT_SBGGR8_1X8:
+		/* Raw, BGBG... / GRGR... */
+		fmt = 0x00;
+		mux = OV5640_FMT_MUX_RAW_DPC;
+		break;
+	case MEDIA_BUS_FMT_SGBRG8_1X8:
+		/* Raw bayer, GBGB... / RGRG... */
+		fmt = 0x01;
+		mux = OV5640_FMT_MUX_RAW_DPC;
+		break;
+	case MEDIA_BUS_FMT_SGRBG8_1X8:
+		/* Raw bayer, GRGR... / BGBG... */
+		fmt = 0x02;
+		mux = OV5640_FMT_MUX_RAW_DPC;
+		break;
+	case MEDIA_BUS_FMT_SRGGB8_1X8:
+		/* Raw bayer, RGRG... / GBGB... */
+		fmt = 0x03;
+		mux = OV5640_FMT_MUX_RAW_DPC;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	/* FORMAT CONTROL00: YUV and RGB formatting */
+	ret = ov5640_write_reg(sensor, OV5640_REG_FORMAT_CONTROL00, fmt);
+	if (ret)
+		return ret;
+
+	/* FORMAT MUX CONTROL: ISP YUV or RGB */
+	ret = ov5640_write_reg(sensor, OV5640_REG_ISP_FORMAT_MUX_CTRL, mux);
+	if (ret)
+		return ret;
+
+	/*
+	 * TIMING TC REG21:
+	 * - [5]:	JPEG enable
+	 */
+	ret = ov5640_mod_reg(sensor, OV5640_REG_TIMING_TC_REG21,
+			     BIT(5), is_jpeg ? BIT(5) : 0);
+	if (ret)
+		return ret;
+
+	/*
+	 * SYSTEM RESET02:
+	 * - [4]:	Reset JFIFO
+	 * - [3]:	Reset SFIFO
+	 * - [2]:	Reset JPEG
+	 */
+	ret = ov5640_mod_reg(sensor, OV5640_REG_SYS_RESET02,
+			     BIT(4) | BIT(3) | BIT(2),
+			     is_jpeg ? 0 : (BIT(4) | BIT(3) | BIT(2)));
+	if (ret)
+		return ret;
+
+	/*
+	 * CLOCK ENABLE02:
+	 * - [5]:	Enable JPEG 2x clock
+	 * - [3]:	Enable JPEG clock
+	 */
+	return ov5640_mod_reg(sensor, OV5640_REG_SYS_CLOCK_ENABLE02,
+			      BIT(5) | BIT(3),
+			      is_jpeg ? (BIT(5) | BIT(3)) : 0);
+}
+
+/*
+ * Sensor Controls.
+ */
+
+static int ov5640_set_ctrl_hue(struct ov5640_dev *sensor, int value)
+{
+	int ret;
+
+	if (value) {
+		ret = ov5640_mod_reg(sensor, OV5640_REG_SDE_CTRL0,
+				     BIT(0), BIT(0));
+		if (ret)
+			return ret;
+		ret = ov5640_write_reg16(sensor, OV5640_REG_SDE_CTRL1, value);
+	} else {
+		ret = ov5640_mod_reg(sensor, OV5640_REG_SDE_CTRL0, BIT(0), 0);
+	}
+
+	return ret;
+}
+
+static int ov5640_set_ctrl_contrast(struct ov5640_dev *sensor, int value)
+{
+	int ret;
+
+	if (value) {
+		ret = ov5640_mod_reg(sensor, OV5640_REG_SDE_CTRL0,
+				     BIT(2), BIT(2));
+		if (ret)
+			return ret;
+		ret = ov5640_write_reg(sensor, OV5640_REG_SDE_CTRL5,
+				       value & 0xff);
+	} else {
+		ret = ov5640_mod_reg(sensor, OV5640_REG_SDE_CTRL0, BIT(2), 0);
+	}
+
+	return ret;
+}
+
+static int ov5640_set_ctrl_saturation(struct ov5640_dev *sensor, int value)
+{
+	int ret;
+
+	if (value) {
+		ret = ov5640_mod_reg(sensor, OV5640_REG_SDE_CTRL0,
+				     BIT(1), BIT(1));
+		if (ret)
+			return ret;
+		ret = ov5640_write_reg(sensor, OV5640_REG_SDE_CTRL3,
+				       value & 0xff);
+		if (ret)
+			return ret;
+		ret = ov5640_write_reg(sensor, OV5640_REG_SDE_CTRL4,
+				       value & 0xff);
+	} else {
+		ret = ov5640_mod_reg(sensor, OV5640_REG_SDE_CTRL0, BIT(1), 0);
+	}
+
+	return ret;
+}
+
+static int ov5640_set_ctrl_white_balance(struct ov5640_dev *sensor, int awb)
+{
+	int ret;
+
+	ret = ov5640_mod_reg(sensor, OV5640_REG_AWB_MANUAL_CTRL,
+			     BIT(0), awb ? 0 : 1);
+	if (ret)
+		return ret;
+
+	if (!awb) {
+		u16 red = (u16)sensor->ctrls.red_balance->val;
+		u16 blue = (u16)sensor->ctrls.blue_balance->val;
+
+		ret = ov5640_write_reg16(sensor, OV5640_REG_AWB_R_GAIN, red);
+		if (ret)
+			return ret;
+		ret = ov5640_write_reg16(sensor, OV5640_REG_AWB_B_GAIN, blue);
+	}
+
+	return ret;
+}
+
+static int ov5640_set_ctrl_exposure(struct ov5640_dev *sensor,
+				    enum v4l2_exposure_auto_type auto_exposure)
+{
+	struct ov5640_ctrls *ctrls = &sensor->ctrls;
+	bool auto_exp = (auto_exposure == V4L2_EXPOSURE_AUTO);
+	int ret = 0;
+
+	if (ctrls->auto_exp->is_new) {
+		ret = ov5640_set_autoexposure(sensor, auto_exp);
+		if (ret)
+			return ret;
+	}
+
+	if (!auto_exp && ctrls->exposure->is_new) {
+		u16 max_exp;
+
+		ret = ov5640_read_reg16(sensor, OV5640_REG_AEC_PK_VTS,
+					&max_exp);
+		if (ret)
+			return ret;
+		ret = ov5640_get_vts(sensor);
+		if (ret < 0)
+			return ret;
+		max_exp += ret;
+		ret = 0;
+
+		if (ctrls->exposure->val < max_exp)
+			ret = ov5640_set_exposure(sensor, ctrls->exposure->val);
+	}
+
+	return ret;
+}
+
+static int ov5640_set_ctrl_gain(struct ov5640_dev *sensor, bool auto_gain)
+{
+	struct ov5640_ctrls *ctrls = &sensor->ctrls;
+	int ret = 0;
+
+	if (ctrls->auto_gain->is_new) {
+		ret = ov5640_set_autogain(sensor, auto_gain);
+		if (ret)
+			return ret;
+	}
+
+	if (!auto_gain && ctrls->gain->is_new)
+		ret = ov5640_set_gain(sensor, ctrls->gain->val);
+
+	return ret;
+}
+
+static const char * const test_pattern_menu[] = {
+	"Disabled",
+	"Color bars",
+	"Color bars w/ rolling bar",
+	"Color squares",
+	"Color squares w/ rolling bar",
+};
+
+#define OV5640_TEST_ENABLE		BIT(7)
+#define OV5640_TEST_ROLLING		BIT(6)	/* rolling horizontal bar */
+#define OV5640_TEST_TRANSPARENT		BIT(5)
+#define OV5640_TEST_SQUARE_BW		BIT(4)	/* black & white squares */
+#define OV5640_TEST_BAR_STANDARD	(0 << 2)
+#define OV5640_TEST_BAR_VERT_CHANGE_1	(1 << 2)
+#define OV5640_TEST_BAR_HOR_CHANGE	(2 << 2)
+#define OV5640_TEST_BAR_VERT_CHANGE_2	(3 << 2)
+#define OV5640_TEST_BAR			(0 << 0)
+#define OV5640_TEST_RANDOM		(1 << 0)
+#define OV5640_TEST_SQUARE		(2 << 0)
+#define OV5640_TEST_BLACK		(3 << 0)
+
+static const u8 test_pattern_val[] = {
+	0,
+	OV5640_TEST_ENABLE | OV5640_TEST_BAR_VERT_CHANGE_1 |
+		OV5640_TEST_BAR,
+	OV5640_TEST_ENABLE | OV5640_TEST_ROLLING |
+		OV5640_TEST_BAR_VERT_CHANGE_1 | OV5640_TEST_BAR,
+	OV5640_TEST_ENABLE | OV5640_TEST_SQUARE,
+	OV5640_TEST_ENABLE | OV5640_TEST_ROLLING | OV5640_TEST_SQUARE,
+};
+
+static int ov5640_set_ctrl_test_pattern(struct ov5640_dev *sensor, int value)
+{
+	return ov5640_write_reg(sensor, OV5640_REG_PRE_ISP_TEST_SET1,
+				test_pattern_val[value]);
+}
+
+static int ov5640_set_ctrl_light_freq(struct ov5640_dev *sensor, int value)
+{
+	int ret;
+
+	ret = ov5640_mod_reg(sensor, OV5640_REG_HZ5060_CTRL01, BIT(7),
+			     (value == V4L2_CID_POWER_LINE_FREQUENCY_AUTO) ?
+			     0 : BIT(7));
+	if (ret)
+		return ret;
+
+	return ov5640_mod_reg(sensor, OV5640_REG_HZ5060_CTRL00, BIT(2),
+			      (value == V4L2_CID_POWER_LINE_FREQUENCY_50HZ) ?
+			      BIT(2) : 0);
+}
+
+static int ov5640_set_ctrl_hflip(struct ov5640_dev *sensor, int value)
+{
+	/*
+	 * If sensor is mounted upside down, mirror logic is inversed.
+	 *
+	 * Sensor is a BSI (Back Side Illuminated) one,
+	 * so image captured is physically mirrored.
+	 * This is why mirror logic is inversed in
+	 * order to cancel this mirror effect.
+	 */
+
+	/*
+	 * TIMING TC REG21:
+	 * - [2]:	ISP mirror
+	 * - [1]:	Sensor mirror
+	 */
+	return ov5640_mod_reg(sensor, OV5640_REG_TIMING_TC_REG21,
+			      BIT(2) | BIT(1),
+			      (!(value ^ sensor->upside_down)) ?
+			      (BIT(2) | BIT(1)) : 0);
+}
+
+static int ov5640_set_ctrl_vflip(struct ov5640_dev *sensor, int value)
+{
+	/* If sensor is mounted upside down, flip logic is inversed */
+
+	/*
+	 * TIMING TC REG20:
+	 * - [2]:	ISP vflip
+	 * - [1]:	Sensor vflip
+	 */
+	return ov5640_mod_reg(sensor, OV5640_REG_TIMING_TC_REG20,
+			      BIT(2) | BIT(1),
+			      (value ^ sensor->upside_down) ?
+			      (BIT(2) | BIT(1)) : 0);
+}
+
+static int ov5640_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
+	struct ov5640_dev *sensor = to_ov5640_dev(sd);
+	int val;
+
+	/* v4l2_ctrl_lock() locks our own mutex */
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUTOGAIN:
+		val = ov5640_get_gain(sensor);
+		if (val < 0)
+			return val;
+		sensor->ctrls.gain->val = val;
+		break;
+	case V4L2_CID_EXPOSURE_AUTO:
+		val = ov5640_get_exposure(sensor);
+		if (val < 0)
+			return val;
+		sensor->ctrls.exposure->val = val;
+		break;
+	}
+
+	return 0;
+}
+
+static int ov5640_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
+	struct ov5640_dev *sensor = to_ov5640_dev(sd);
+	int ret;
+
+	/* v4l2_ctrl_lock() locks our own mutex */
+
+	/*
+	 * If the device is not powered up by the host driver do
+	 * not apply any controls to H/W at this time. Instead
+	 * the controls will be restored right after power-up.
+	 */
+	if (sensor->power_count == 0)
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUTOGAIN:
+		ret = ov5640_set_ctrl_gain(sensor, ctrl->val);
+		break;
+	case V4L2_CID_EXPOSURE_AUTO:
+		ret = ov5640_set_ctrl_exposure(sensor, ctrl->val);
+		break;
+	case V4L2_CID_AUTO_WHITE_BALANCE:
+		ret = ov5640_set_ctrl_white_balance(sensor, ctrl->val);
+		break;
+	case V4L2_CID_HUE:
+		ret = ov5640_set_ctrl_hue(sensor, ctrl->val);
+		break;
+	case V4L2_CID_CONTRAST:
+		ret = ov5640_set_ctrl_contrast(sensor, ctrl->val);
+		break;
+	case V4L2_CID_SATURATION:
+		ret = ov5640_set_ctrl_saturation(sensor, ctrl->val);
+		break;
+	case V4L2_CID_TEST_PATTERN:
+		ret = ov5640_set_ctrl_test_pattern(sensor, ctrl->val);
+		break;
+	case V4L2_CID_POWER_LINE_FREQUENCY:
+		ret = ov5640_set_ctrl_light_freq(sensor, ctrl->val);
+		break;
+	case V4L2_CID_HFLIP:
+		ret = ov5640_set_ctrl_hflip(sensor, ctrl->val);
+		break;
+	case V4L2_CID_VFLIP:
+		ret = ov5640_set_ctrl_vflip(sensor, ctrl->val);
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops ov5640_ctrl_ops = {
+	.g_volatile_ctrl = ov5640_g_volatile_ctrl,
+	.s_ctrl = ov5640_s_ctrl,
+};
+
+static int ov5640_init_controls(struct ov5640_dev *sensor)
+{
+	const struct v4l2_ctrl_ops *ops = &ov5640_ctrl_ops;
+	struct ov5640_ctrls *ctrls = &sensor->ctrls;
+	struct v4l2_ctrl_handler *hdl = &ctrls->handler;
+	int ret;
+
+	v4l2_ctrl_handler_init(hdl, 32);
+
+	/* we can use our own mutex for the ctrl lock */
+	hdl->lock = &sensor->lock;
+
+	/* Clock related controls */
+	ctrls->pixel_rate = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_PIXEL_RATE,
+					      0, INT_MAX, 1,
+					      ov5640_calc_pixel_rate(sensor));
+
+	/* Auto/manual white balance */
+	ctrls->auto_wb = v4l2_ctrl_new_std(hdl, ops,
+					   V4L2_CID_AUTO_WHITE_BALANCE,
+					   0, 1, 1, 1);
+	ctrls->blue_balance = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_BLUE_BALANCE,
+						0, 4095, 1, 0);
+	ctrls->red_balance = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_RED_BALANCE,
+					       0, 4095, 1, 0);
+	/* Auto/manual exposure */
+	ctrls->auto_exp = v4l2_ctrl_new_std_menu(hdl, ops,
+						 V4L2_CID_EXPOSURE_AUTO,
+						 V4L2_EXPOSURE_MANUAL, 0,
+						 V4L2_EXPOSURE_AUTO);
+	ctrls->exposure = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_EXPOSURE,
+					    0, 65535, 1, 0);
+	/* Auto/manual gain */
+	ctrls->auto_gain = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_AUTOGAIN,
+					     0, 1, 1, 1);
+	ctrls->gain = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_GAIN,
+					0, 1023, 1, 0);
+
+	ctrls->saturation = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_SATURATION,
+					      0, 255, 1, 64);
+	ctrls->hue = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HUE,
+				       0, 359, 1, 0);
+	ctrls->contrast = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_CONTRAST,
+					    0, 255, 1, 0);
+	ctrls->test_pattern =
+		v4l2_ctrl_new_std_menu_items(hdl, ops, V4L2_CID_TEST_PATTERN,
+					     ARRAY_SIZE(test_pattern_menu) - 1,
+					     0, 0, test_pattern_menu);
+	ctrls->hflip = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HFLIP,
+					 0, 1, 1, 0);
+	ctrls->vflip = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VFLIP,
+					 0, 1, 1, 0);
+
+	ctrls->light_freq =
+		v4l2_ctrl_new_std_menu(hdl, ops,
+				       V4L2_CID_POWER_LINE_FREQUENCY,
+				       V4L2_CID_POWER_LINE_FREQUENCY_AUTO, 0,
+				       V4L2_CID_POWER_LINE_FREQUENCY_50HZ);
+
+	if (hdl->error) {
+		ret = hdl->error;
+		goto free_ctrls;
+	}
+
+	ctrls->pixel_rate->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+	ctrls->gain->flags |= V4L2_CTRL_FLAG_VOLATILE;
+	ctrls->exposure->flags |= V4L2_CTRL_FLAG_VOLATILE;
+
+	v4l2_ctrl_auto_cluster(3, &ctrls->auto_wb, 0, false);
+	v4l2_ctrl_auto_cluster(2, &ctrls->auto_gain, 0, true);
+	v4l2_ctrl_auto_cluster(2, &ctrls->auto_exp, 1, true);
+
+	sensor->sd.ctrl_handler = hdl;
+	return 0;
+
+free_ctrls:
+	v4l2_ctrl_handler_free(hdl);
+	return ret;
+}
+
+static int ov5640_enum_frame_size(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_pad_config *cfg,
+				  struct v4l2_subdev_frame_size_enum *fse)
+{
+	if (fse->pad != 0)
+		return -EINVAL;
+	if (fse->index >= OV5640_NUM_MODES)
+		return -EINVAL;
+
+	fse->min_width =
+		ov5640_mode_data[fse->index].hact;
+	fse->max_width = fse->min_width;
+	fse->min_height =
+		ov5640_mode_data[fse->index].vact;
+	fse->max_height = fse->min_height;
+
+	return 0;
+}
+
+static int ov5640_enum_frame_interval(
+	struct v4l2_subdev *sd,
+	struct v4l2_subdev_pad_config *cfg,
+	struct v4l2_subdev_frame_interval_enum *fie)
+{
+	struct ov5640_dev *sensor = to_ov5640_dev(sd);
+	struct v4l2_fract tpf;
+	int ret;
+
+	if (fie->pad != 0)
+		return -EINVAL;
+	if (fie->index >= OV5640_NUM_FRAMERATES)
+		return -EINVAL;
+
+	tpf.numerator = 1;
+	tpf.denominator = ov5640_framerates[fie->index];
+
+	ret = ov5640_try_frame_interval(sensor, &tpf,
+					fie->width, fie->height);
+	if (ret < 0)
+		return -EINVAL;
+
+	fie->interval = tpf;
+	return 0;
+}
+
+static int ov5640_g_frame_interval(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_frame_interval *fi)
+{
+	struct ov5640_dev *sensor = to_ov5640_dev(sd);
+
+	mutex_lock(&sensor->lock);
+	fi->interval = sensor->frame_interval;
+	mutex_unlock(&sensor->lock);
+
+	return 0;
+}
+
+static int ov5640_s_frame_interval(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_frame_interval *fi)
+{
+	struct ov5640_dev *sensor = to_ov5640_dev(sd);
+	const struct ov5640_mode_info *mode;
+	int frame_rate, ret = 0;
+
+	if (fi->pad != 0)
+		return -EINVAL;
+
+	mutex_lock(&sensor->lock);
+
+	if (sensor->streaming) {
+		ret = -EBUSY;
+		goto out;
+	}
+
+	mode = sensor->current_mode;
+
+	frame_rate = ov5640_try_frame_interval(sensor, &fi->interval,
+					       mode->hact, mode->vact);
+	if (frame_rate < 0) {
+		/* Always return a valid frame interval value */
+		fi->interval = sensor->frame_interval;
+		goto out;
+	}
+
+	mode = ov5640_find_mode(sensor, frame_rate, mode->hact,
+				mode->vact, true);
+	if (!mode) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	if (mode != sensor->current_mode ||
+	    frame_rate != sensor->current_fr) {
+		sensor->current_fr = frame_rate;
+		sensor->frame_interval = fi->interval;
+		sensor->current_mode = mode;
+		sensor->pending_mode_change = true;
+
+		__v4l2_ctrl_s_ctrl_int64(sensor->ctrls.pixel_rate,
+					 ov5640_calc_pixel_rate(sensor));
+	}
+out:
+	mutex_unlock(&sensor->lock);
+	return ret;
+}
+
+static int ov5640_enum_mbus_code(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_pad_config *cfg,
+				 struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->pad != 0)
+		return -EINVAL;
+	if (code->index >= ARRAY_SIZE(ov5640_formats))
+		return -EINVAL;
+
+	code->code = ov5640_formats[code->index].code;
+	return 0;
+}
+
+static int ov5640_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct ov5640_dev *sensor = to_ov5640_dev(sd);
+	int ret = 0;
+
+	mutex_lock(&sensor->lock);
+
+	if (sensor->streaming == !enable) {
+		if (enable && sensor->pending_mode_change) {
+			ret = ov5640_set_mode(sensor);
+			if (ret)
+				goto out;
+		}
+
+		if (enable && sensor->pending_fmt_change) {
+			ret = ov5640_set_framefmt(sensor, &sensor->fmt);
+			if (ret)
+				goto out;
+			sensor->pending_fmt_change = false;
+		}
+
+		if (sensor->ep.bus_type == V4L2_MBUS_CSI2_DPHY)
+			ret = ov5640_set_stream_mipi(sensor, enable);
+		else
+			ret = ov5640_set_stream_dvp(sensor, enable);
+
+		if (!ret)
+			sensor->streaming = enable;
+	}
+out:
+	mutex_unlock(&sensor->lock);
+	return ret;
+}
+
+int ov5640_skip_frames(struct v4l2_subdev *sd, u32 *frames)
+{
+	*frames = OV5640_SKIP_FRAMES;
+	return 0;
+}
+
+static const struct v4l2_subdev_core_ops ov5640_core_ops = {
+	.s_power = ov5640_s_power,
+	.log_status = v4l2_ctrl_subdev_log_status,
+	.subscribe_event = v4l2_ctrl_subdev_subscribe_event,
+	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
+};
+
+static const struct v4l2_subdev_video_ops ov5640_video_ops = {
+	.g_frame_interval = ov5640_g_frame_interval,
+	.s_frame_interval = ov5640_s_frame_interval,
+	.s_stream = ov5640_s_stream,
+};
+
+static const struct v4l2_subdev_pad_ops ov5640_pad_ops = {
+	.enum_mbus_code = ov5640_enum_mbus_code,
+	.get_fmt = ov5640_get_fmt,
+	.set_fmt = ov5640_set_fmt,
+	.enum_frame_size = ov5640_enum_frame_size,
+	.enum_frame_interval = ov5640_enum_frame_interval,
+};
+
+static const struct v4l2_subdev_sensor_ops ov5640_sensor_ops = {
+	.g_skip_frames = ov5640_skip_frames,
+};
+
+static const struct v4l2_subdev_ops ov5640_subdev_ops = {
+	.core = &ov5640_core_ops,
+	.video = &ov5640_video_ops,
+	.pad = &ov5640_pad_ops,
+	.sensor = &ov5640_sensor_ops,
+};
+
+static int ov5640_get_regulators(struct ov5640_dev *sensor)
+{
+	int i;
+
+	for (i = 0; i < OV5640_NUM_SUPPLIES; i++)
+		sensor->supplies[i].supply = ov5640_supply_name[i];
+
+	return devm_regulator_bulk_get(&sensor->i2c_client->dev,
+				       OV5640_NUM_SUPPLIES,
+				       sensor->supplies);
+}
+
+static int ov5640_check_chip_id(struct ov5640_dev *sensor)
+{
+	struct i2c_client *client = sensor->i2c_client;
+	int ret = 0;
+	u16 chip_id;
+
+	ret = ov5640_set_power_on(sensor);
+	if (ret)
+		return ret;
+
+	ret = ov5640_read_reg16(sensor, OV5640_REG_CHIP_ID, &chip_id);
+	if (ret) {
+		dev_err(&client->dev, "%s: failed to read chip identifier\n",
+			__func__);
+		goto power_off;
+	}
+
+	if (chip_id != OV5640_CHIP_ID) {
+		dev_err(&client->dev, "%s: wrong chip identifier, expected 0x%x, got 0x%x\n",
+			__func__, OV5640_CHIP_ID, chip_id);
+		ret = -ENXIO;
+	}
+	dev_err(&client->dev, "%s: chip identifier, got 0x%x\n",
+		__func__, chip_id);
+
+power_off:
+	ov5640_set_power_off(sensor);
+	return ret;
+}
+
+static int ov5640_probe(struct i2c_client *client)
+{
+	struct device *dev = &client->dev;
+	struct fwnode_handle *endpoint;
+	struct ov5640_dev *sensor;
+	struct v4l2_mbus_framefmt *fmt;
+	u32 rotation;
+	int ret;
+
+	sensor = devm_kzalloc(dev, sizeof(*sensor), GFP_KERNEL);
+	if (!sensor)
+		return -ENOMEM;
+
+	sensor->i2c_client = client;
+
+	/*
+	 * default init sequence initialize sensor to
+	 * YUV422 UYVY VGA@30fps
+	 */
+	fmt = &sensor->fmt;
+	fmt->code = MEDIA_BUS_FMT_UYVY8_2X8;
+	fmt->colorspace = V4L2_COLORSPACE_SRGB;
+	fmt->ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(fmt->colorspace);
+	fmt->quantization = V4L2_QUANTIZATION_FULL_RANGE;
+	fmt->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(fmt->colorspace);
+	fmt->width = 640;
+	fmt->height = 480;
+	fmt->field = V4L2_FIELD_NONE;
+	sensor->frame_interval.numerator = 1;
+	sensor->frame_interval.denominator = ov5640_framerates[OV5640_30_FPS];
+	sensor->current_fr = OV5640_30_FPS;
+	sensor->current_mode =
+		&ov5640_mode_data[OV5640_MODE_VGA_640_480];
+	sensor->last_mode = sensor->current_mode;
+
+	sensor->ae_target = 52;
+
+	/* optional indication of physical rotation of sensor */
+	ret = fwnode_property_read_u32(dev_fwnode(&client->dev), "rotation",
+				       &rotation);
+	if (!ret) {
+		switch (rotation) {
+		case 180:
+			sensor->upside_down = true;
+			fallthrough;
+		case 0:
+			break;
+		default:
+			dev_warn(dev, "%u degrees rotation is not supported, ignoring...\n",
+				 rotation);
+		}
+	}
+
+	endpoint = fwnode_graph_get_next_endpoint(dev_fwnode(&client->dev),
+						  NULL);
+	if (!endpoint) {
+		dev_err(dev, "endpoint node not found\n");
+		return -EINVAL;
+	}
+
+	ret = v4l2_fwnode_endpoint_parse(endpoint, &sensor->ep);
+	fwnode_handle_put(endpoint);
+	if (ret) {
+		dev_err(dev, "Could not parse endpoint\n");
+		return ret;
+	}
+
+	if (sensor->ep.bus_type != V4L2_MBUS_PARALLEL &&
+	    sensor->ep.bus_type != V4L2_MBUS_CSI2_DPHY &&
+	    sensor->ep.bus_type != V4L2_MBUS_BT656) {
+		dev_err(dev, "Unsupported bus type %d\n", sensor->ep.bus_type);
+		return -EINVAL;
+	}
+
+	/* get system clock (xclk) */
+	sensor->xclk = devm_clk_get(dev, "xclk");
+	if (IS_ERR(sensor->xclk)) {
+		dev_err(dev, "failed to get xclk\n");
+		return PTR_ERR(sensor->xclk);
+	}
+
+	sensor->xclk_freq = clk_get_rate(sensor->xclk);
+	if (sensor->xclk_freq < OV5640_XCLK_MIN ||
+	    sensor->xclk_freq > OV5640_XCLK_MAX) {
+		dev_err(dev, "xclk frequency out of range: %d Hz\n",
+			sensor->xclk_freq);
+		return -EINVAL;
+	}
+
+	/* request optional power down pin */
+	sensor->pwdn_gpio = devm_gpiod_get_optional(dev, "powerdown",
+						    GPIOD_OUT_HIGH);
+	if (IS_ERR(sensor->pwdn_gpio))
+		return PTR_ERR(sensor->pwdn_gpio);
+
+	/* request optional reset pin */
+	sensor->reset_gpio = devm_gpiod_get_optional(dev, "reset",
+						     GPIOD_OUT_HIGH);
+	if (IS_ERR(sensor->reset_gpio))
+		return PTR_ERR(sensor->reset_gpio);
+
+	v4l2_i2c_subdev_init(&sensor->sd, client, &ov5640_subdev_ops);
+
+	sensor->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
+			    V4L2_SUBDEV_FL_HAS_EVENTS;
+	sensor->pad.flags = MEDIA_PAD_FL_SOURCE;
+	sensor->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+	ret = media_entity_pads_init(&sensor->sd.entity, 1, &sensor->pad);
+	if (ret)
+		return ret;
+
+	ret = ov5640_get_regulators(sensor);
+	if (ret)
+		return ret;
+
+	mutex_init(&sensor->lock);
+
+	ret = ov5640_check_chip_id(sensor);
+	if (ret)
+		goto entity_cleanup;
+
+	ret = ov5640_init_controls(sensor);
+	if (ret)
+		goto entity_cleanup;
+
+	ret = v4l2_async_register_subdev_sensor(&sensor->sd);
+	if (ret)
+		goto free_ctrls;
+
+	return 0;
+
+free_ctrls:
+	v4l2_ctrl_handler_free(&sensor->ctrls.handler);
+entity_cleanup:
+	media_entity_cleanup(&sensor->sd.entity);
+	mutex_destroy(&sensor->lock);
+	return ret;
+}
+
+static int ov5640_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ov5640_dev *sensor = to_ov5640_dev(sd);
+
+	v4l2_async_unregister_subdev(&sensor->sd);
+	media_entity_cleanup(&sensor->sd.entity);
+	v4l2_ctrl_handler_free(&sensor->ctrls.handler);
+	mutex_destroy(&sensor->lock);
+
+	return 0;
+}
+
+static const struct i2c_device_id ov5640_id[] = {
+	{"ov5640", 0},
+	{},
+};
+MODULE_DEVICE_TABLE(i2c, ov5640_id);
+
+static const struct of_device_id ov5640_dt_ids[] = {
+	{ .compatible = "ovti,ov5640" },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, ov5640_dt_ids);
+
+static struct i2c_driver ov5640_i2c_driver = {
+	.driver = {
+		.name  = "ov5640",
+		.of_match_table	= ov5640_dt_ids,
+	},
+	.id_table = ov5640_id,
+	.probe_new = ov5640_probe,
+	.remove   = ov5640_remove,
+};
+
+module_i2c_driver(ov5640_i2c_driver);
+
+MODULE_DESCRIPTION("OV5640 MIPI Camera Subdev Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/platform/starfive/sc2235.c b/drivers/media/platform/starfive/sc2235.c
new file mode 100644
index 000000000000..263c331ed016
--- /dev/null
+++ b/drivers/media/platform/starfive/sc2235.c
@@ -0,0 +1,1911 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Copyright (C) 2011-2013 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright (C) 2014-2017 Mentor Graphics Inc.
+ * Copyright (C) 2021 StarFive Technology Co., Ltd.
+ */
+
+#include <linux/clk.h>
+#include <linux/clk-provider.h>
+#include <linux/clkdev.h>
+#include <linux/ctype.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/gpio/consumer.h>
+#include <linux/i2c.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/of_device.h>
+#include <linux/regulator/consumer.h>
+#include <linux/slab.h>
+#include <linux/types.h>
+#include <media/v4l2-async.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-subdev.h>
+#include "stfcamss.h"
+
+/* min/typical/max system clock (xclk) frequencies */
+#define SC2235_XCLK_MIN  6000000
+#define SC2235_XCLK_MAX 54000000
+
+#define SC2235_CHIP_ID		(0x2235)
+
+#define SC2235_REG_CHIP_ID              0x3107
+#define SC2235_REG_AEC_PK_MANUAL        0x3e03
+#define SC2235_REG_AEC_PK_EXPOSURE_HI   0x3e01
+#define SC2235_REG_AEC_PK_EXPOSURE_LO   0x3e02
+#define SC2235_REG_AEC_PK_REAL_GAIN     0x3e08
+#define SC2235_REG_TIMING_HTS           0x320c
+#define SC2235_REG_TIMING_VTS           0x320e
+#define SC2235_REG_TEST_SET0            0x4501
+#define SC2235_REG_TEST_SET1            0x3902
+#define SC2235_REG_TIMING_TC_REG21      0x3221
+#define SC2235_REG_SC_PLL_CTRL0         0x3039
+#define SC2235_REG_SC_PLL_CTRL1         0x303a
+
+enum sc2235_mode_id {
+	SC2235_MODE_1080P_1920_1080 = 0,
+	SC2235_NUM_MODES,
+};
+
+enum sc2235_frame_rate {
+	SC2235_15_FPS = 0,
+	SC2235_30_FPS,
+	SC2235_60_FPS,
+	SC2235_NUM_FRAMERATES,
+};
+
+struct sc2235_pixfmt {
+	u32 code;
+	u32 colorspace;
+};
+
+static const struct sc2235_pixfmt sc2235_formats[] = {
+	{ MEDIA_BUS_FMT_SGBRG10_1X10, V4L2_COLORSPACE_SRGB, },
+};
+
+static const int sc2235_framerates[] = {
+	[SC2235_15_FPS] = 15,
+	[SC2235_30_FPS] = 30,
+	[SC2235_60_FPS] = 60,
+};
+
+/* regulator supplies */
+static const char * const sc2235_supply_name[] = {
+	"DOVDD", /* Digital I/O (1.8V) supply */
+	"AVDD",  /* Analog (2.8V) supply */
+	"DVDD",  /* Digital Core (1.5V) supply */
+};
+
+#define SC2235_NUM_SUPPLIES ARRAY_SIZE(sc2235_supply_name)
+
+struct reg_value {
+	u16 reg_addr;
+	u8 val;
+	u8 mask;
+	u32 delay_ms;
+};
+
+struct sc2235_mode_info {
+	enum sc2235_mode_id id;
+	u32 hact;
+	u32 htot;
+	u32 vact;
+	u32 vtot;
+	const struct reg_value *reg_data;
+	u32 reg_data_size;
+	u32 max_fps;
+};
+
+struct sc2235_ctrls {
+	struct v4l2_ctrl_handler handler;
+	struct v4l2_ctrl *pixel_rate;
+	struct {
+		struct v4l2_ctrl *auto_exp;
+		struct v4l2_ctrl *exposure;
+	};
+	struct {
+		struct v4l2_ctrl *auto_wb;
+		struct v4l2_ctrl *blue_balance;
+		struct v4l2_ctrl *red_balance;
+	};
+	struct {
+		struct v4l2_ctrl *auto_gain;
+		struct v4l2_ctrl *gain;
+	};
+	struct v4l2_ctrl *brightness;
+	struct v4l2_ctrl *light_freq;
+	struct v4l2_ctrl *saturation;
+	struct v4l2_ctrl *contrast;
+	struct v4l2_ctrl *hue;
+	struct v4l2_ctrl *test_pattern;
+	struct v4l2_ctrl *hflip;
+	struct v4l2_ctrl *vflip;
+};
+
+struct sc2235_dev {
+	struct i2c_client *i2c_client;
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+	struct v4l2_fwnode_endpoint ep; /* the parsed DT endpoint info */
+	struct clk *xclk; /* system clock to SC2235 */
+	u32 xclk_freq;
+
+	struct regulator_bulk_data supplies[SC2235_NUM_SUPPLIES];
+	struct gpio_desc *reset_gpio;
+	struct gpio_desc *pwdn_gpio;
+	bool   upside_down;
+
+	/* lock to protect all members below */
+	struct mutex lock;
+
+	int power_count;
+
+	struct v4l2_mbus_framefmt fmt;
+	bool pending_fmt_change;
+
+	const struct sc2235_mode_info *current_mode;
+	const struct sc2235_mode_info *last_mode;
+	enum sc2235_frame_rate current_fr;
+	struct v4l2_fract frame_interval;
+
+	struct sc2235_ctrls ctrls;
+
+	u32 prev_sysclk, prev_hts;
+	u32 ae_low, ae_high, ae_target;
+
+	bool pending_mode_change;
+	bool streaming;
+};
+
+static inline struct sc2235_dev *to_sc2235_dev(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct sc2235_dev, sd);
+}
+
+static inline struct v4l2_subdev *ctrl_to_sd(struct v4l2_ctrl *ctrl)
+{
+	return &container_of(ctrl->handler, struct sc2235_dev,
+				ctrls.handler)->sd;
+}
+
+/* sc2235 initial register */
+static struct reg_value sc2235_init_regs_tbl_1080[] = {
+	{0x0103, 0x01, 0, 0},
+	{0x0100, 0x00, 0, 0},
+	// {0x3039, 0x80, 0, 0},
+	{0x3621, 0x28, 0, 0},
+
+	{0x3309, 0x60, 0, 0},
+	{0x331f, 0x4d, 0, 0},
+	{0x3321, 0x4f, 0, 0},
+	{0x33b5, 0x10, 0, 0},
+
+	{0x3303, 0x20, 0, 0},
+	{0x331e, 0x0d, 0, 0},
+	{0x3320, 0x0f, 0, 0},
+
+	{0x3622, 0x02, 0, 0},
+	{0x3633, 0x42, 0, 0},
+	{0x3634, 0x42, 0, 0},
+
+	{0x3306, 0x66, 0, 0},
+	{0x330b, 0xd1, 0, 0},
+
+	{0x3301, 0x0e, 0, 0},
+
+	{0x320c, 0x08, 0, 0},
+	{0x320d, 0x98, 0, 0},
+
+	{0x3364, 0x05, 0, 0},		// [2] 1: write at sampling ending
+
+	{0x363c, 0x28, 0, 0},		//bypass nvdd
+	{0x363b, 0x0a, 0, 0},		//HVDD
+	{0x3635, 0xa0, 0, 0},		//TXVDD
+
+	{0x4500, 0x59, 0, 0},
+	{0x3d08, 0x02, 0, 0},
+	{0x3908, 0x11, 0, 0},
+
+	{0x363c, 0x08, 0, 0},
+
+	{0x3e03, 0x03, 0, 0},
+	{0x3e01, 0x46, 0, 0},
+
+	//0703
+	{0x3381, 0x0a, 0, 0},
+	{0x3348, 0x09, 0, 0},
+	{0x3349, 0x50, 0, 0},
+	{0x334a, 0x02, 0, 0},
+	{0x334b, 0x60, 0, 0},
+
+	{0x3380, 0x04, 0, 0},
+	{0x3340, 0x06, 0, 0},
+	{0x3341, 0x50, 0, 0},
+	{0x3342, 0x02, 0, 0},
+	{0x3343, 0x60, 0, 0},
+
+	//0707
+
+	{0x3632, 0x88, 0, 0},		//anti sm
+	{0x3309, 0xa0, 0, 0},
+	{0x331f, 0x8d, 0, 0},
+	{0x3321, 0x8f, 0, 0},
+
+	{0x335e, 0x01, 0, 0},		//ana dithering
+	{0x335f, 0x03, 0, 0},
+	{0x337c, 0x04, 0, 0},
+	{0x337d, 0x06, 0, 0},
+	{0x33a0, 0x05, 0, 0},
+	{0x3301, 0x05, 0, 0},
+
+	{0x337f, 0x03, 0, 0},
+	{0x3368, 0x02, 0, 0},
+	{0x3369, 0x00, 0, 0},
+	{0x336a, 0x00, 0, 0},
+	{0x336b, 0x00, 0, 0},
+	{0x3367, 0x08, 0, 0},
+	{0x330e, 0x30, 0, 0},
+
+	{0x3366, 0x7c, 0, 0},		// div_rst gap
+
+	{0x3635, 0xc1, 0, 0},
+	{0x363b, 0x09, 0, 0},
+	{0x363c, 0x07, 0, 0},
+
+	{0x391e, 0x00, 0, 0},
+
+	{0x3637, 0x14, 0, 0},		//fullwell 7K
+
+	{0x3306, 0x54, 0, 0},
+	{0x330b, 0xd8, 0, 0},
+	{0x366e, 0x08, 0, 0},		// ofs auto en [3]
+	{0x366f, 0x2f, 0, 0},
+
+	{0x3631, 0x84, 0, 0},
+	{0x3630, 0x48, 0, 0},
+	{0x3622, 0x06, 0, 0},
+
+	//ramp by sc
+	{0x3638, 0x1f, 0, 0},
+	{0x3625, 0x02, 0, 0},
+	{0x3636, 0x24, 0, 0},
+
+	//0714
+	{0x3348, 0x08, 0, 0},
+	{0x3e03, 0x0b, 0, 0},
+
+	//7.17 fpn
+	{0x3342, 0x03, 0, 0},
+	{0x3343, 0xa0, 0, 0},
+	{0x334a, 0x03, 0, 0},
+	{0x334b, 0xa0, 0, 0},
+
+	//0718
+	{0x3343, 0xb0, 0, 0},
+	{0x334b, 0xb0, 0, 0},
+
+	//0720
+	//digital ctr, 0, 0l
+	{0x3802, 0x01, 0, 0},
+	{0x3235, 0x04, 0, 0},
+	{0x3236, 0x63, 0, 0},		// vts-2
+
+	//fpn
+	{0x3343, 0xd0, 0, 0},
+	{0x334b, 0xd0, 0, 0},
+	{0x3348, 0x07, 0, 0},
+	{0x3349, 0x80, 0, 0},
+
+	//0724
+	{0x391b, 0x4d, 0, 0},
+
+	{0x3342, 0x04, 0, 0},
+	{0x3343, 0x20, 0, 0},
+	{0x334a, 0x04, 0, 0},
+	{0x334b, 0x20, 0, 0},
+
+	//0804
+	{0x3222, 0x29, 0, 0},
+	{0x3901, 0x02, 0, 0},
+
+	//0808
+
+	//digital ctr, 0, 0l
+	{0x3f00, 0x07, 0, 0},		// bit[2] = 1
+	{0x3f04, 0x08, 0, 0},
+	{0x3f05, 0x74, 0, 0},		// hts - { 0x24
+
+	//0809
+	{0x330b, 0xc8, 0, 0},
+
+	//0817
+	{0x3306, 0x4a, 0, 0},
+	{0x330b, 0xca, 0, 0},
+	{0x3639, 0x09, 0, 0},
+
+	//manual DPC
+	{0x5780, 0xff, 0, 0},
+	{0x5781, 0x04, 0, 0},
+	{0x5785, 0x18, 0, 0},
+
+	//0822
+	// {0x3039, 0x35, 0, 0},		//fps
+	// {0x303a, 0x2e, 0, 0},
+	{0x3034, 0x05, 0, 0},
+	{0x3035, 0x2a, 0, 0},
+
+	{0x320c, 0x08, 0, 0},
+	{0x320d, 0xca, 0, 0},
+	{0x320e, 0x04, 0, 0},
+	{0x320f, 0xb0, 0, 0},
+
+	{0x3f04, 0x08, 0, 0},
+	{0x3f05, 0xa6, 0, 0},		// hts - { 0x24
+
+	{0x3235, 0x04, 0, 0},
+	{0x3236, 0xae, 0, 0},		// vts-2
+
+	//0825
+	{0x3313, 0x05, 0, 0},
+	{0x3678, 0x42, 0, 0},
+
+	//for AE control per frame
+	{0x3670, 0x00, 0, 0},
+	{0x3633, 0x42, 0, 0},
+
+	{0x3802, 0x00, 0, 0},
+
+	//20180126
+	{0x3677, 0x3f, 0, 0},
+	{0x3306, 0x44, 0, 0},		//20180126[3c },4a]
+	{0x330b, 0xca, 0, 0},		//20180126[c2 },d3]
+
+	//20180202
+	{0x3237, 0x08, 0, 0},
+	{0x3238, 0x9a, 0, 0},		//hts-0x30
+
+	//20180417
+	{0x3640, 0x01, 0, 0},
+	{0x3641, 0x02, 0, 0},
+
+	{0x3301, 0x12, 0, 0},		//[8 },15]20180126
+	{0x3631, 0x84, 0, 0},
+	{0x366f, 0x2f, 0, 0},
+	{0x3622, 0xc6, 0, 0},		//20180117
+	{0x0100, 0x01, 0, 0},
+	//{ 0x4501, 0xc8, 0, 0},	//bar testing
+	//{ 0x3902, 0x45, 0, 0},
+};
+
+static struct reg_value sc2235_setting_1080P_1920_1080[] = {
+
+};
+
+/* power-on sensor init reg table */
+static const struct sc2235_mode_info sc2235_mode_init_data = {
+	SC2235_MODE_1080P_1920_1080,
+	1920, 0x8ca, 1080, 0x4b0,
+	sc2235_init_regs_tbl_1080,
+	ARRAY_SIZE(sc2235_init_regs_tbl_1080),
+	SC2235_60_FPS,
+};
+
+static const struct sc2235_mode_info
+sc2235_mode_data[SC2235_NUM_MODES] = {
+	{SC2235_MODE_1080P_1920_1080,
+	 1920, 0x8ca, 1080, 0x4b0,
+	 sc2235_setting_1080P_1920_1080,
+	 ARRAY_SIZE(sc2235_setting_1080P_1920_1080),
+	 SC2235_60_FPS},
+};
+
+static int sc2235_write_reg(struct sc2235_dev *sensor, u16 reg, u8 val)
+{
+	struct i2c_client *client = sensor->i2c_client;
+	struct i2c_msg msg;
+	u8 buf[3];
+	int ret;
+
+	buf[0] = reg >> 8;
+	buf[1] = reg & 0xff;
+	buf[2] = val;
+
+	msg.addr = client->addr;
+	msg.flags = client->flags;
+	msg.buf = buf;
+	msg.len = sizeof(buf);
+
+	ret = i2c_transfer(client->adapter, &msg, 1);
+	if (ret < 0) {
+		dev_err(&client->dev, "%s: error: reg=%x, val=%x\n",
+			__func__, reg, val);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int sc2235_read_reg(struct sc2235_dev *sensor, u16 reg, u8 *val)
+{
+	struct i2c_client *client = sensor->i2c_client;
+	struct i2c_msg msg[2];
+	u8 buf[2];
+	int ret;
+
+	buf[0] = reg >> 8;
+	buf[1] = reg & 0xff;
+
+	msg[0].addr = client->addr;
+	msg[0].flags = client->flags;
+	msg[0].buf = buf;
+	msg[0].len = sizeof(buf);
+
+	msg[1].addr = client->addr;
+	msg[1].flags = client->flags | I2C_M_RD;
+	msg[1].buf = buf;
+	msg[1].len = 1;
+
+	ret = i2c_transfer(client->adapter, msg, 2);
+	if (ret < 0) {
+		dev_err(&client->dev, "%s: error: reg=%x\n",
+			__func__, reg);
+		return ret;
+	}
+
+	*val = buf[0];
+	return 0;
+}
+
+static int sc2235_read_reg16(struct sc2235_dev *sensor, u16 reg, u16 *val)
+{
+	u8 hi, lo;
+	int ret;
+
+	ret = sc2235_read_reg(sensor, reg, &hi);
+	if (ret)
+		return ret;
+	ret = sc2235_read_reg(sensor, reg + 1, &lo);
+	if (ret)
+		return ret;
+
+	*val = ((u16)hi << 8) | (u16)lo;
+	return 0;
+}
+
+static int sc2235_write_reg16(struct sc2235_dev *sensor, u16 reg, u16 val)
+{
+	int ret;
+
+	ret = sc2235_write_reg(sensor, reg, val >> 8);
+	if (ret)
+		return ret;
+
+	return sc2235_write_reg(sensor, reg + 1, val & 0xff);
+}
+
+static int sc2235_mod_reg(struct sc2235_dev *sensor, u16 reg,
+			u8 mask, u8 val)
+{
+	u8 readval;
+	int ret;
+
+	ret = sc2235_read_reg(sensor, reg, &readval);
+	if (ret)
+		return ret;
+
+	readval &= ~mask;
+	val &= mask;
+	val |= readval;
+
+	return sc2235_write_reg(sensor, reg, val);
+}
+
+#define SC2235_PLL_PREDIV	3
+
+#define SC2235_SYSDIV_MIN	0
+#define SC2235_SYSDIV_MAX	7
+
+#define SC2235_PLL_MULT_MIN	0
+#define SC2235_PLL_MULT_MAX	63
+
+static unsigned long sc2235_compute_sys_clk(struct sc2235_dev *sensor,
+					u8 pll_pre, u8 pll_mult,
+					u8 sysdiv)
+{
+	unsigned long sysclk =
+		sensor->xclk_freq * (64 - pll_mult) / (pll_pre * (sysdiv + 1));
+
+	/* PLL1 output cannot exceed 1GHz. */
+	if (sysclk / 1000000 > 1000)
+		return 0;
+
+	return sysclk;
+}
+
+static unsigned long sc2235_calc_sys_clk(struct sc2235_dev *sensor,
+					unsigned long rate,
+					u8 *pll_prediv, u8 *pll_mult,
+					u8 *sysdiv)
+{
+	unsigned long best = ~0;
+	u8 best_sysdiv = 1, best_mult = 1;
+	u8 _sysdiv, _pll_mult;
+
+	for (_sysdiv = SC2235_SYSDIV_MIN;
+		_sysdiv <= SC2235_SYSDIV_MAX;
+		_sysdiv++) {
+		for (_pll_mult = SC2235_PLL_MULT_MIN;
+			_pll_mult <= SC2235_PLL_MULT_MAX;
+			_pll_mult++) {
+			unsigned long _rate;
+
+			_rate = sc2235_compute_sys_clk(sensor,
+							SC2235_PLL_PREDIV,
+							_pll_mult, _sysdiv);
+
+			/*
+			 * We have reached the maximum allowed PLL1 output,
+			 * increase sysdiv.
+			 */
+			if (!_rate)
+				break;
+
+			/*
+			 * Prefer rates above the expected clock rate than
+			 * below, even if that means being less precise.
+			 */
+			if (_rate < rate)
+				continue;
+
+			if (abs(rate - _rate) < abs(rate - best)) {
+				best = _rate;
+				best_sysdiv = _sysdiv;
+				best_mult = _pll_mult;
+			}
+
+			if (_rate == rate)
+				goto out;
+		}
+	}
+
+out:
+	*sysdiv = best_sysdiv;
+	*pll_prediv = SC2235_PLL_PREDIV;
+	*pll_mult = best_mult;
+
+	return best;
+}
+
+static int sc2235_set_timings(struct sc2235_dev *sensor,
+				const struct sc2235_mode_info *mode)
+{
+	int ret = 0;
+
+	return ret;
+}
+
+static int sc2235_load_regs(struct sc2235_dev *sensor,
+				const struct sc2235_mode_info *mode)
+{
+	const struct reg_value *regs = mode->reg_data;
+	unsigned int i;
+	u32 delay_ms;
+	u16 reg_addr;
+	u8 mask, val;
+	int ret = 0;
+
+	st_info(ST_SENSOR, "%s, mode = 0x%x\n", __func__, mode->id);
+	for (i = 0; i < mode->reg_data_size; ++i, ++regs) {
+		delay_ms = regs->delay_ms;
+		reg_addr = regs->reg_addr;
+		val = regs->val;
+		mask = regs->mask;
+
+		if (mask)
+			ret = sc2235_mod_reg(sensor, reg_addr, mask, val);
+		else
+			ret = sc2235_write_reg(sensor, reg_addr, val);
+		if (ret)
+			break;
+
+		if (delay_ms)
+			usleep_range(1000 * delay_ms, 1000 * delay_ms + 100);
+	}
+
+	return sc2235_set_timings(sensor, mode);
+}
+
+static int sc2235_set_autoexposure(struct sc2235_dev *sensor, bool on)
+{
+	return sc2235_mod_reg(sensor, SC2235_REG_AEC_PK_MANUAL,
+				BIT(0), on ? 0 : BIT(0));
+}
+
+static int sc2235_get_exposure(struct sc2235_dev *sensor)
+{
+	int exp = 0, ret = 0;
+	u8 temp;
+
+	ret = sc2235_read_reg(sensor, SC2235_REG_AEC_PK_EXPOSURE_HI, &temp);
+	if (ret)
+		return ret;
+	exp |= (int)temp << 8;
+	ret = sc2235_read_reg(sensor, SC2235_REG_AEC_PK_EXPOSURE_LO, &temp);
+	if (ret)
+		return ret;
+	exp |= (int)temp;
+
+	return exp >> 4;
+}
+
+static int sc2235_set_exposure(struct sc2235_dev *sensor, u32 exposure)
+{
+	int ret;
+
+	exposure <<= 4;
+
+	ret = sc2235_write_reg(sensor,
+				SC2235_REG_AEC_PK_EXPOSURE_LO,
+				exposure & 0xff);
+	if (ret)
+		return ret;
+	return sc2235_write_reg(sensor,
+				SC2235_REG_AEC_PK_EXPOSURE_HI,
+				(exposure >> 8) & 0xff);
+}
+
+static int sc2235_get_gain(struct sc2235_dev *sensor)
+{
+	u16 gain;
+	int ret;
+
+	ret = sc2235_read_reg16(sensor, SC2235_REG_AEC_PK_REAL_GAIN, &gain);
+	if (ret)
+		return ret;
+
+	return gain & 0x1fff;
+}
+
+static int sc2235_set_gain(struct sc2235_dev *sensor, int gain)
+{
+	return sc2235_write_reg16(sensor, SC2235_REG_AEC_PK_REAL_GAIN,
+					(u16)gain & 0x1fff);
+}
+
+static int sc2235_set_autogain(struct sc2235_dev *sensor, bool on)
+{
+	return sc2235_mod_reg(sensor, SC2235_REG_AEC_PK_MANUAL,
+				BIT(1), on ? 0 : BIT(1));
+}
+
+static int sc2235_set_stream_dvp(struct sc2235_dev *sensor, bool on)
+{
+	return 0;
+}
+
+static int sc2235_get_sysclk(struct sc2235_dev *sensor)
+{
+	return 0;
+}
+
+static int sc2235_set_night_mode(struct sc2235_dev *sensor)
+{
+	return 0;
+}
+
+static int sc2235_get_hts(struct sc2235_dev *sensor)
+{
+	u16 hts;
+	int ret;
+
+	ret = sc2235_read_reg16(sensor, SC2235_REG_TIMING_HTS, &hts);
+	if (ret)
+		return ret;
+	return hts;
+}
+
+static int sc2235_get_vts(struct sc2235_dev *sensor)
+{
+	u16 vts;
+	int ret;
+
+	ret = sc2235_read_reg16(sensor, SC2235_REG_TIMING_VTS, &vts);
+	if (ret)
+		return ret;
+	return vts;
+}
+
+static int sc2235_set_vts(struct sc2235_dev *sensor, int vts)
+{
+	return sc2235_write_reg16(sensor, SC2235_REG_TIMING_VTS, vts);
+}
+
+static int sc2235_get_light_freq(struct sc2235_dev *sensor)
+{
+	return 0;
+}
+
+static int sc2235_set_bandingfilter(struct sc2235_dev *sensor)
+{
+	return 0;
+}
+
+static int sc2235_set_ae_target(struct sc2235_dev *sensor, int target)
+{
+	return 0;
+}
+
+static int sc2235_get_binning(struct sc2235_dev *sensor)
+{
+	return 0;
+}
+
+static int sc2235_set_binning(struct sc2235_dev *sensor, bool enable)
+{
+	return 0;
+}
+
+static const struct sc2235_mode_info *
+sc2235_find_mode(struct sc2235_dev *sensor, enum sc2235_frame_rate fr,
+		 int width, int height, bool nearest)
+{
+	const struct sc2235_mode_info *mode;
+
+	mode = v4l2_find_nearest_size(sc2235_mode_data,
+					ARRAY_SIZE(sc2235_mode_data),
+					hact, vact,
+					width, height);
+
+	if (!mode ||
+		(!nearest && (mode->hact != width || mode->vact != height)))
+		return NULL;
+
+	/* Check to see if the current mode exceeds the max frame rate */
+	if (sc2235_framerates[fr] > sc2235_framerates[mode->max_fps])
+		return NULL;
+
+	return mode;
+}
+
+static u64 sc2235_calc_pixel_rate(struct sc2235_dev *sensor)
+{
+	u64 rate;
+
+	rate = sensor->current_mode->vtot * sensor->current_mode->htot;
+	rate *= sc2235_framerates[sensor->current_fr];
+
+	return rate;
+}
+
+/*
+ * sc2235_set_dvp_pclk() - Calculate the clock tree configuration values
+ *				for the dvp output.
+ *
+ * @rate: The requested bandwidth per lane in bytes per second.
+ *	'Bandwidth Per Lane' is calculated as:
+ *	rate = HTOT * VTOT * FPS;
+ *
+ * This function use the requested bandwidth to calculate:
+ * - rate = xclk * (64 - M) / (N * (S + 1));
+ *
+ */
+
+#define PLL_PREDIV  1
+#define PLL_SYSEL   0
+
+static int sc2235_set_dvp_pclk(struct sc2235_dev *sensor,
+				unsigned long rate)
+{
+	const struct sc2235_mode_info *mode = sensor->current_mode;
+	const struct sc2235_mode_info *orig_mode = sensor->last_mode;
+	u8 prediv, mult, sysdiv;
+	int ret = 0;
+
+	sc2235_calc_sys_clk(sensor, rate, &prediv, &mult,
+				&sysdiv);
+
+	st_info(ST_SENSOR, "%s, prediv = %d, mult = %d, sysdiv = %d\n",
+			__func__, prediv, mult, sysdiv);
+
+	ret = sc2235_mod_reg(sensor, SC2235_REG_SC_PLL_CTRL0, 0x7f,
+			(sysdiv << 4) | (prediv << 1) | ((mult & 0x20) >> 5));
+	if (ret)
+		return ret;
+
+	return sc2235_mod_reg(sensor, SC2235_REG_SC_PLL_CTRL1,
+			0xf8, mult << 3);
+}
+
+/*
+ * if sensor changes inside scaling or subsampling
+ * change mode directly
+ */
+static int sc2235_set_mode_direct(struct sc2235_dev *sensor,
+				const struct sc2235_mode_info *mode)
+{
+	if (!mode->reg_data)
+		return -EINVAL;
+
+	/* Write capture setting */
+	return sc2235_load_regs(sensor, mode);
+}
+
+static int sc2235_set_mode(struct sc2235_dev *sensor)
+{
+	const struct sc2235_mode_info *mode = sensor->current_mode;
+	const struct sc2235_mode_info *orig_mode = sensor->last_mode;
+	bool auto_gain = sensor->ctrls.auto_gain->val == 1;
+	bool auto_exp =  sensor->ctrls.auto_exp->val == V4L2_EXPOSURE_AUTO;
+	unsigned long rate;
+	int ret = 0;
+
+	/* auto gain and exposure must be turned off when changing modes */
+	if (auto_gain) {
+		ret = sc2235_set_autogain(sensor, false);
+		if (ret)
+			return ret;
+	}
+
+	if (auto_exp) {
+		ret = sc2235_set_autoexposure(sensor, false);
+		if (ret)
+			goto restore_auto_gain;
+	}
+
+	rate = sc2235_calc_pixel_rate(sensor);
+	if (sensor->ep.bus_type == V4L2_MBUS_PARALLEL)
+		ret = sc2235_set_dvp_pclk(sensor, rate);
+
+	if (ret < 0)
+		return 0;
+
+	ret = sc2235_set_mode_direct(sensor, mode);
+	if (ret < 0)
+		goto restore_auto_exp_gain;
+
+	/* restore auto gain and exposure */
+	if (auto_gain)
+		sc2235_set_autogain(sensor, true);
+	if (auto_exp)
+		sc2235_set_autoexposure(sensor, true);
+
+	sensor->pending_mode_change = false;
+	sensor->last_mode = mode;
+	return 0;
+
+restore_auto_exp_gain:
+	if (auto_exp)
+		sc2235_set_autoexposure(sensor, true);
+restore_auto_gain:
+	if (auto_gain)
+		sc2235_set_autogain(sensor, true);
+
+	return ret;
+}
+
+static int sc2235_set_framefmt(struct sc2235_dev *sensor,
+				struct v4l2_mbus_framefmt *format);
+
+/* restore the last set video mode after chip power-on */
+static int sc2235_restore_mode(struct sc2235_dev *sensor)
+{
+	int ret;
+
+	/* first load the initial register values */
+	ret = sc2235_load_regs(sensor, &sc2235_mode_init_data);
+	if (ret < 0)
+		return ret;
+	sensor->last_mode = &sc2235_mode_init_data;
+
+	/* now restore the last capture mode */
+	ret = sc2235_set_mode(sensor);
+	if (ret < 0)
+		return ret;
+
+	return sc2235_set_framefmt(sensor, &sensor->fmt);
+}
+
+static void sc2235_power(struct sc2235_dev *sensor, bool enable)
+{
+	if (!sensor->pwdn_gpio)
+		return;
+	gpiod_set_value_cansleep(sensor->pwdn_gpio, enable ? 0 : 1);
+}
+
+static void sc2235_reset(struct sc2235_dev *sensor)
+{
+	if (!sensor->reset_gpio)
+		return;
+
+	gpiod_set_value_cansleep(sensor->reset_gpio, 0);
+
+	/* camera power cycle */
+	sc2235_power(sensor, false);
+	usleep_range(5000, 10000);
+	sc2235_power(sensor, true);
+	usleep_range(5000, 10000);
+
+	gpiod_set_value_cansleep(sensor->reset_gpio, 1);
+	usleep_range(1000, 2000);
+
+	gpiod_set_value_cansleep(sensor->reset_gpio, 0);
+	usleep_range(20000, 25000);
+}
+
+static int sc2235_set_power_on(struct sc2235_dev *sensor)
+{
+	struct i2c_client *client = sensor->i2c_client;
+	int ret;
+
+	ret = clk_prepare_enable(sensor->xclk);
+	if (ret) {
+		dev_err(&client->dev, "%s: failed to enable clock\n",
+			__func__);
+		return ret;
+	}
+
+	ret = regulator_bulk_enable(SC2235_NUM_SUPPLIES,
+					sensor->supplies);
+	if (ret) {
+		dev_err(&client->dev, "%s: failed to enable regulators\n",
+			__func__);
+		goto xclk_off;
+	}
+
+	sc2235_reset(sensor);
+	sc2235_power(sensor, true);
+
+	return 0;
+
+xclk_off:
+	clk_disable_unprepare(sensor->xclk);
+	return ret;
+}
+
+static void sc2235_set_power_off(struct sc2235_dev *sensor)
+{
+	sc2235_power(sensor, false);
+	regulator_bulk_disable(SC2235_NUM_SUPPLIES, sensor->supplies);
+	clk_disable_unprepare(sensor->xclk);
+}
+
+static int sc2235_set_power_dvp(struct sc2235_dev *sensor, bool on)
+{
+	unsigned int flags = sensor->ep.bus.parallel.flags;
+	bool bt656 = sensor->ep.bus_type == V4L2_MBUS_BT656;
+	u8 polarities = 0;
+	int ret;
+
+	/*
+	 * configure parallel port control lines polarity
+	 *
+	 * POLARITY CTRL0
+	 * - [5]:	PCLK polarity (0: active low, 1: active high)
+	 * - [1]:	HREF polarity (0: active low, 1: active high)
+	 * - [0]:	VSYNC polarity (mismatch here between
+	 *		datasheet and hardware, 0 is active high
+	 *		and 1 is active low...)
+	 */
+	if (flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH)
+		polarities |= BIT(1);
+	if (flags & V4L2_MBUS_VSYNC_ACTIVE_LOW)
+		polarities |= BIT(0);
+	if (flags & V4L2_MBUS_PCLK_SAMPLE_RISING)
+		polarities |= BIT(5);
+
+	// ret = sc2235_write_reg(sensor,
+	//		SC2235_REG_POLARITY_CTRL00,
+	//		polarities);
+	// if (ret)
+	//	return ret;
+
+	return 0;
+}
+
+static int sc2235_set_power(struct sc2235_dev *sensor, bool on)
+{
+	int ret = 0;
+
+	if (on) {
+		ret = sc2235_set_power_on(sensor);
+		if (ret)
+			return ret;
+
+		ret = sc2235_restore_mode(sensor);
+		if (ret)
+			goto power_off;
+	}
+
+	if (sensor->ep.bus_type == V4L2_MBUS_PARALLEL)
+		ret = sc2235_set_power_dvp(sensor, on);
+	if (ret)
+		goto power_off;
+
+	if (!on)
+		sc2235_set_power_off(sensor);
+
+	return 0;
+
+power_off:
+	sc2235_set_power_off(sensor);
+	return ret;
+}
+
+static int sc2235_s_power(struct v4l2_subdev *sd, int on)
+{
+	struct sc2235_dev *sensor = to_sc2235_dev(sd);
+	int ret = 0;
+
+	mutex_lock(&sensor->lock);
+
+	/*
+	 * If the power count is modified from 0 to != 0 or from != 0 to 0,
+	 * update the power state.
+	 */
+	if (sensor->power_count == !on) {
+		ret = sc2235_set_power(sensor, !!on);
+		if (ret)
+			goto out;
+	}
+
+	/* Update the power count. */
+	sensor->power_count += on ? 1 : -1;
+	WARN_ON(sensor->power_count < 0);
+out:
+	mutex_unlock(&sensor->lock);
+
+	if (on && !ret && sensor->power_count == 1) {
+		/* restore controls */
+		ret = v4l2_ctrl_handler_setup(&sensor->ctrls.handler);
+	}
+
+	return ret;
+}
+
+static int sc2235_try_frame_interval(struct sc2235_dev *sensor,
+				struct v4l2_fract *fi,
+				u32 width, u32 height)
+{
+	const struct sc2235_mode_info *mode;
+	enum sc2235_frame_rate rate = SC2235_15_FPS;
+	int minfps, maxfps, best_fps, fps;
+	int i;
+
+	minfps = sc2235_framerates[SC2235_15_FPS];
+	maxfps = sc2235_framerates[SC2235_60_FPS];
+
+	if (fi->numerator == 0) {
+		fi->denominator = maxfps;
+		fi->numerator = 1;
+		rate = SC2235_60_FPS;
+		goto find_mode;
+	}
+
+	fps = clamp_val(DIV_ROUND_CLOSEST(fi->denominator, fi->numerator),
+			minfps, maxfps);
+
+	best_fps = minfps;
+	for (i = 0; i < ARRAY_SIZE(sc2235_framerates); i++) {
+		int curr_fps = sc2235_framerates[i];
+
+		if (abs(curr_fps - fps) < abs(best_fps - fps)) {
+			best_fps = curr_fps;
+			rate = i;
+		}
+	}
+
+	fi->numerator = 1;
+	fi->denominator = best_fps;
+
+find_mode:
+	mode = sc2235_find_mode(sensor, rate, width, height, false);
+	return mode ? rate : -EINVAL;
+}
+
+static int sc2235_get_fmt(struct v4l2_subdev *sd,
+			struct v4l2_subdev_pad_config *cfg,
+			struct v4l2_subdev_format *format)
+{
+	struct sc2235_dev *sensor = to_sc2235_dev(sd);
+	struct v4l2_mbus_framefmt *fmt;
+
+	if (format->pad != 0)
+		return -EINVAL;
+
+	mutex_lock(&sensor->lock);
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY)
+		fmt = v4l2_subdev_get_try_format(&sensor->sd, cfg,
+						format->pad);
+	else
+		fmt = &sensor->fmt;
+
+	format->format = *fmt;
+
+	mutex_unlock(&sensor->lock);
+
+	return 0;
+}
+
+static int sc2235_try_fmt_internal(struct v4l2_subdev *sd,
+				struct v4l2_mbus_framefmt *fmt,
+				enum sc2235_frame_rate fr,
+				const struct sc2235_mode_info **new_mode)
+{
+	struct sc2235_dev *sensor = to_sc2235_dev(sd);
+	const struct sc2235_mode_info *mode;
+	int i;
+
+	mode = sc2235_find_mode(sensor, fr, fmt->width, fmt->height, true);
+	if (!mode)
+		return -EINVAL;
+	fmt->width = mode->hact;
+	fmt->height = mode->vact;
+
+	if (new_mode)
+		*new_mode = mode;
+
+	for (i = 0; i < ARRAY_SIZE(sc2235_formats); i++)
+		if (sc2235_formats[i].code == fmt->code)
+			break;
+	if (i >= ARRAY_SIZE(sc2235_formats))
+		i = 0;
+
+	fmt->code = sc2235_formats[i].code;
+	fmt->colorspace = sc2235_formats[i].colorspace;
+	fmt->ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(fmt->colorspace);
+	fmt->quantization = V4L2_QUANTIZATION_FULL_RANGE;
+	fmt->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(fmt->colorspace);
+
+	return 0;
+}
+
+static int sc2235_set_fmt(struct v4l2_subdev *sd,
+			struct v4l2_subdev_pad_config *cfg,
+			struct v4l2_subdev_format *format)
+{
+	struct sc2235_dev *sensor = to_sc2235_dev(sd);
+	const struct sc2235_mode_info *new_mode;
+	struct v4l2_mbus_framefmt *mbus_fmt = &format->format;
+	struct v4l2_mbus_framefmt *fmt;
+	int ret;
+
+	if (format->pad != 0)
+		return -EINVAL;
+	mutex_lock(&sensor->lock);
+
+	if (sensor->streaming) {
+		ret = -EBUSY;
+		goto out;
+	}
+
+	ret = sc2235_try_fmt_internal(sd, mbus_fmt, 0, &new_mode);
+	if (ret)
+		goto out;
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY)
+		fmt = v4l2_subdev_get_try_format(sd, cfg, 0);
+	else
+		fmt = &sensor->fmt;
+
+	if (mbus_fmt->code != sensor->fmt.code)
+		sensor->pending_fmt_change = true;
+
+	*fmt = *mbus_fmt;
+
+	if (new_mode != sensor->current_mode) {
+		sensor->current_mode = new_mode;
+		sensor->pending_mode_change = true;
+	}
+	if (new_mode->max_fps < sensor->current_fr) {
+		sensor->current_fr = new_mode->max_fps;
+		sensor->frame_interval.numerator = 1;
+		sensor->frame_interval.denominator =
+			sc2235_framerates[sensor->current_fr];
+		sensor->current_mode = new_mode;
+		sensor->pending_mode_change = true;
+	}
+
+	__v4l2_ctrl_s_ctrl_int64(sensor->ctrls.pixel_rate,
+				sc2235_calc_pixel_rate(sensor));
+out:
+	mutex_unlock(&sensor->lock);
+	return ret;
+}
+
+static int sc2235_set_framefmt(struct sc2235_dev *sensor,
+				struct v4l2_mbus_framefmt *format)
+{
+	int ret = 0;
+
+	switch (format->code) {
+	default:
+		return ret;
+	}
+	return ret;
+}
+
+/*
+ * Sensor Controls.
+ */
+
+static int sc2235_set_ctrl_hue(struct sc2235_dev *sensor, int value)
+{
+	int ret = 0;
+	return ret;
+}
+
+static int sc2235_set_ctrl_contrast(struct sc2235_dev *sensor, int value)
+{
+	int ret = 0;
+	return ret;
+}
+
+static int sc2235_set_ctrl_saturation(struct sc2235_dev *sensor, int value)
+{
+	int ret  = 0;
+	return ret;
+}
+
+static int sc2235_set_ctrl_white_balance(struct sc2235_dev *sensor, int awb)
+{
+	int ret = 0;
+	return ret;
+}
+
+static int sc2235_set_ctrl_exposure(struct sc2235_dev *sensor,
+				enum v4l2_exposure_auto_type auto_exposure)
+{
+	struct sc2235_ctrls *ctrls = &sensor->ctrls;
+	bool auto_exp = (auto_exposure == V4L2_EXPOSURE_AUTO);
+	int ret = 0;
+
+	if (ctrls->auto_exp->is_new) {
+		ret = sc2235_set_autoexposure(sensor, auto_exp);
+		if (ret)
+			return ret;
+	}
+
+	if (!auto_exp && ctrls->exposure->is_new) {
+		u16 max_exp = 0;
+
+		ret = sc2235_get_vts(sensor);
+		if (ret < 0)
+			return ret;
+		max_exp += ret - 4;
+		ret = 0;
+
+		if (ctrls->exposure->val < max_exp)
+			ret = sc2235_set_exposure(sensor, ctrls->exposure->val);
+	}
+
+	return ret;
+}
+
+static int sc2235_set_ctrl_gain(struct sc2235_dev *sensor, bool auto_gain)
+{
+	struct sc2235_ctrls *ctrls = &sensor->ctrls;
+	int ret = 0;
+
+	if (ctrls->auto_gain->is_new) {
+		ret = sc2235_set_autogain(sensor, auto_gain);
+		if (ret)
+			return ret;
+	}
+
+	if (!auto_gain && ctrls->gain->is_new)
+		ret = sc2235_set_gain(sensor, ctrls->gain->val);
+
+	return ret;
+}
+
+static const char * const test_pattern_menu[] = {
+	"Disabled",
+	"Black bars",
+	"Auto Black bars",
+};
+
+#define SC2235_TEST_ENABLE		BIT(3)
+#define SC2235_TEST_BLACK		(3 << 0)
+
+static int sc2235_set_ctrl_test_pattern(struct sc2235_dev *sensor, int value)
+{
+	int ret = 0;
+
+	ret = sc2235_mod_reg(sensor, SC2235_REG_TEST_SET0, BIT(3),
+				!!value << 3);
+
+	ret |= sc2235_mod_reg(sensor, SC2235_REG_TEST_SET1, BIT(6),
+				(value >> 1) << 6);
+
+	return ret;
+}
+
+static int sc2235_set_ctrl_light_freq(struct sc2235_dev *sensor, int value)
+{
+	return 0;
+}
+
+static int sc2235_set_ctrl_hflip(struct sc2235_dev *sensor, int value)
+{
+	return sc2235_mod_reg(sensor, SC2235_REG_TIMING_TC_REG21,
+				BIT(2) | BIT(1),
+				(!(value ^ sensor->upside_down)) ?
+				(BIT(2) | BIT(1)) : 0);
+}
+
+static int sc2235_set_ctrl_vflip(struct sc2235_dev *sensor, int value)
+{
+	return sc2235_mod_reg(sensor, SC2235_REG_TIMING_TC_REG21,
+				BIT(6) | BIT(5),
+				(value ^ sensor->upside_down) ?
+				(BIT(6) | BIT(5)) : 0);
+}
+
+static int sc2235_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
+	struct sc2235_dev *sensor = to_sc2235_dev(sd);
+	int val;
+
+	/* v4l2_ctrl_lock() locks our own mutex */
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUTOGAIN:
+		val = sc2235_get_gain(sensor);
+		if (val < 0)
+			return val;
+		sensor->ctrls.gain->val = val;
+		break;
+	case V4L2_CID_EXPOSURE_AUTO:
+		val = sc2235_get_exposure(sensor);
+		if (val < 0)
+			return val;
+		sensor->ctrls.exposure->val = val;
+		break;
+	}
+
+	return 0;
+}
+
+static int sc2235_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
+	struct sc2235_dev *sensor = to_sc2235_dev(sd);
+	int ret;
+
+	/* v4l2_ctrl_lock() locks our own mutex */
+
+	/*
+	 * If the device is not powered up by the host driver do
+	 * not apply any controls to H/W at this time. Instead
+	 * the controls will be restored right after power-up.
+	 */
+	if (sensor->power_count == 0)
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUTOGAIN:
+		ret = sc2235_set_ctrl_gain(sensor, ctrl->val);
+		break;
+	case V4L2_CID_EXPOSURE_AUTO:
+		ret = sc2235_set_ctrl_exposure(sensor, ctrl->val);
+		break;
+	case V4L2_CID_AUTO_WHITE_BALANCE:
+		ret = sc2235_set_ctrl_white_balance(sensor, ctrl->val);
+		break;
+	case V4L2_CID_HUE:
+		ret = sc2235_set_ctrl_hue(sensor, ctrl->val);
+		break;
+	case V4L2_CID_CONTRAST:
+		ret = sc2235_set_ctrl_contrast(sensor, ctrl->val);
+		break;
+	case V4L2_CID_SATURATION:
+		ret = sc2235_set_ctrl_saturation(sensor, ctrl->val);
+		break;
+	case V4L2_CID_TEST_PATTERN:
+		ret = sc2235_set_ctrl_test_pattern(sensor, ctrl->val);
+		break;
+	case V4L2_CID_POWER_LINE_FREQUENCY:
+		ret = sc2235_set_ctrl_light_freq(sensor, ctrl->val);
+		break;
+	case V4L2_CID_HFLIP:
+		ret = sc2235_set_ctrl_hflip(sensor, ctrl->val);
+		break;
+	case V4L2_CID_VFLIP:
+		ret = sc2235_set_ctrl_vflip(sensor, ctrl->val);
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops sc2235_ctrl_ops = {
+	.g_volatile_ctrl = sc2235_g_volatile_ctrl,
+	.s_ctrl = sc2235_s_ctrl,
+};
+
+static int sc2235_init_controls(struct sc2235_dev *sensor)
+{
+	const struct v4l2_ctrl_ops *ops = &sc2235_ctrl_ops;
+	struct sc2235_ctrls *ctrls = &sensor->ctrls;
+	struct v4l2_ctrl_handler *hdl = &ctrls->handler;
+	int ret;
+
+	v4l2_ctrl_handler_init(hdl, 32);
+
+	/* we can use our own mutex for the ctrl lock */
+	hdl->lock = &sensor->lock;
+
+	/* Clock related controls */
+	ctrls->pixel_rate = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_PIXEL_RATE,
+						0, INT_MAX, 1,
+						sc2235_calc_pixel_rate(sensor));
+
+	/* Auto/manual white balance */
+	ctrls->auto_wb = v4l2_ctrl_new_std(hdl, ops,
+					V4L2_CID_AUTO_WHITE_BALANCE,
+					0, 1, 1, 1);
+	ctrls->blue_balance = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_BLUE_BALANCE,
+						0, 4095, 1, 0);
+	ctrls->red_balance = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_RED_BALANCE,
+						0, 4095, 1, 0);
+	/* Auto/manual exposure */
+	ctrls->auto_exp = v4l2_ctrl_new_std_menu(hdl, ops,
+						V4L2_CID_EXPOSURE_AUTO,
+						V4L2_EXPOSURE_MANUAL, 0,
+						V4L2_EXPOSURE_AUTO);
+	ctrls->exposure = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_EXPOSURE,
+					0, 65535, 1, 0);
+	/* Auto/manual gain */
+	ctrls->auto_gain = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_AUTOGAIN,
+						0, 1, 1, 1);
+	ctrls->gain = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_GAIN,
+					0, 1023, 1, 0);
+
+	ctrls->saturation = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_SATURATION,
+						0, 255, 1, 64);
+	ctrls->hue = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HUE,
+					0, 359, 1, 0);
+	ctrls->contrast = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_CONTRAST,
+						0, 255, 1, 0);
+	ctrls->test_pattern =
+		v4l2_ctrl_new_std_menu_items(hdl, ops, V4L2_CID_TEST_PATTERN,
+					ARRAY_SIZE(test_pattern_menu) - 1,
+					0, 0, test_pattern_menu);
+	ctrls->hflip = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HFLIP,
+					0, 1, 1, 0);
+	ctrls->vflip = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VFLIP,
+					0, 1, 1, 0);
+
+	ctrls->light_freq =
+		v4l2_ctrl_new_std_menu(hdl, ops,
+					V4L2_CID_POWER_LINE_FREQUENCY,
+					V4L2_CID_POWER_LINE_FREQUENCY_AUTO, 0,
+					V4L2_CID_POWER_LINE_FREQUENCY_50HZ);
+
+	if (hdl->error) {
+		ret = hdl->error;
+		goto free_ctrls;
+	}
+
+	ctrls->pixel_rate->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+	ctrls->gain->flags |= V4L2_CTRL_FLAG_VOLATILE;
+	ctrls->exposure->flags |= V4L2_CTRL_FLAG_VOLATILE;
+
+	v4l2_ctrl_auto_cluster(3, &ctrls->auto_wb, 0, false);
+	v4l2_ctrl_auto_cluster(2, &ctrls->auto_gain, 0, true);
+	v4l2_ctrl_auto_cluster(2, &ctrls->auto_exp, 1, true);
+
+	sensor->sd.ctrl_handler = hdl;
+	return 0;
+
+free_ctrls:
+	v4l2_ctrl_handler_free(hdl);
+	return ret;
+}
+
+static int sc2235_enum_frame_size(struct v4l2_subdev *sd,
+				struct v4l2_subdev_pad_config *cfg,
+				struct v4l2_subdev_frame_size_enum *fse)
+{
+	if (fse->pad != 0)
+		return -EINVAL;
+	if (fse->index >= SC2235_NUM_MODES)
+		return -EINVAL;
+
+	fse->min_width =
+		sc2235_mode_data[fse->index].hact;
+	fse->max_width = fse->min_width;
+	fse->min_height =
+		sc2235_mode_data[fse->index].vact;
+	fse->max_height = fse->min_height;
+
+	return 0;
+}
+
+static int sc2235_enum_frame_interval(
+	struct v4l2_subdev *sd,
+	struct v4l2_subdev_pad_config *cfg,
+	struct v4l2_subdev_frame_interval_enum *fie)
+{
+	struct sc2235_dev *sensor = to_sc2235_dev(sd);
+	struct v4l2_fract tpf;
+	int ret;
+
+	if (fie->pad != 0)
+		return -EINVAL;
+	if (fie->index >= SC2235_NUM_FRAMERATES)
+		return -EINVAL;
+
+	tpf.numerator = 1;
+	tpf.denominator = sc2235_framerates[fie->index];
+
+	ret = sc2235_try_frame_interval(sensor, &tpf,
+					fie->width, fie->height);
+	if (ret < 0)
+		return -EINVAL;
+
+	fie->interval = tpf;
+	return 0;
+}
+
+static int sc2235_g_frame_interval(struct v4l2_subdev *sd,
+				struct v4l2_subdev_frame_interval *fi)
+{
+	struct sc2235_dev *sensor = to_sc2235_dev(sd);
+
+	mutex_lock(&sensor->lock);
+	fi->interval = sensor->frame_interval;
+	mutex_unlock(&sensor->lock);
+
+	return 0;
+}
+
+static int sc2235_s_frame_interval(struct v4l2_subdev *sd,
+				struct v4l2_subdev_frame_interval *fi)
+{
+	struct sc2235_dev *sensor = to_sc2235_dev(sd);
+	const struct sc2235_mode_info *mode;
+	int frame_rate, ret = 0;
+
+	if (fi->pad != 0)
+		return -EINVAL;
+
+	mutex_lock(&sensor->lock);
+
+	if (sensor->streaming) {
+		ret = -EBUSY;
+		goto out;
+	}
+
+	mode = sensor->current_mode;
+
+	frame_rate = sc2235_try_frame_interval(sensor, &fi->interval,
+						mode->hact, mode->vact);
+	if (frame_rate < 0) {
+		/* Always return a valid frame interval value */
+		fi->interval = sensor->frame_interval;
+		goto out;
+	}
+
+	mode = sc2235_find_mode(sensor, frame_rate, mode->hact,
+				mode->vact, true);
+	if (!mode) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	if (mode != sensor->current_mode ||
+	    frame_rate != sensor->current_fr) {
+		sensor->current_fr = frame_rate;
+		sensor->frame_interval = fi->interval;
+		sensor->current_mode = mode;
+		sensor->pending_mode_change = true;
+
+		__v4l2_ctrl_s_ctrl_int64(sensor->ctrls.pixel_rate,
+					sc2235_calc_pixel_rate(sensor));
+	}
+out:
+	mutex_unlock(&sensor->lock);
+	return ret;
+}
+
+static int sc2235_enum_mbus_code(struct v4l2_subdev *sd,
+				struct v4l2_subdev_pad_config *cfg,
+				struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->pad != 0)
+		return -EINVAL;
+	if (code->index >= ARRAY_SIZE(sc2235_formats))
+		return -EINVAL;
+
+	code->code = sc2235_formats[code->index].code;
+	return 0;
+}
+
+static int sc2235_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct sc2235_dev *sensor = to_sc2235_dev(sd);
+	int ret = 0;
+
+	mutex_lock(&sensor->lock);
+
+	if (sensor->streaming == !enable) {
+		if (enable && sensor->pending_mode_change) {
+			ret = sc2235_set_mode(sensor);
+			if (ret)
+				goto out;
+		}
+
+		if (enable && sensor->pending_fmt_change) {
+			ret = sc2235_set_framefmt(sensor, &sensor->fmt);
+			if (ret)
+				goto out;
+			sensor->pending_fmt_change = false;
+		}
+
+		if (sensor->ep.bus_type == V4L2_MBUS_PARALLEL)
+			ret = sc2235_set_stream_dvp(sensor, enable);
+
+		if (!ret)
+			sensor->streaming = enable;
+	}
+out:
+	mutex_unlock(&sensor->lock);
+	return ret;
+}
+
+static const struct v4l2_subdev_core_ops sc2235_core_ops = {
+	.s_power = sc2235_s_power,
+	.log_status = v4l2_ctrl_subdev_log_status,
+	.subscribe_event = v4l2_ctrl_subdev_subscribe_event,
+	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
+};
+
+static const struct v4l2_subdev_video_ops sc2235_video_ops = {
+	.g_frame_interval = sc2235_g_frame_interval,
+	.s_frame_interval = sc2235_s_frame_interval,
+	.s_stream = sc2235_s_stream,
+};
+
+static const struct v4l2_subdev_pad_ops sc2235_pad_ops = {
+	.enum_mbus_code = sc2235_enum_mbus_code,
+	.get_fmt = sc2235_get_fmt,
+	.set_fmt = sc2235_set_fmt,
+	.enum_frame_size = sc2235_enum_frame_size,
+	.enum_frame_interval = sc2235_enum_frame_interval,
+};
+
+static const struct v4l2_subdev_ops sc2235_subdev_ops = {
+	.core = &sc2235_core_ops,
+	.video = &sc2235_video_ops,
+	.pad = &sc2235_pad_ops,
+};
+
+static int sc2235_get_regulators(struct sc2235_dev *sensor)
+{
+	int i;
+
+	for (i = 0; i < SC2235_NUM_SUPPLIES; i++)
+		sensor->supplies[i].supply = sc2235_supply_name[i];
+
+	return devm_regulator_bulk_get(&sensor->i2c_client->dev,
+					SC2235_NUM_SUPPLIES,
+					sensor->supplies);
+}
+
+static int sc2235_check_chip_id(struct sc2235_dev *sensor)
+{
+	struct i2c_client *client = sensor->i2c_client;
+	int ret = 0;
+	u16 chip_id;
+
+	ret = sc2235_set_power_on(sensor);
+	if (ret)
+		return ret;
+
+	ret = sc2235_read_reg16(sensor, SC2235_REG_CHIP_ID, &chip_id);
+	if (ret) {
+		dev_err(&client->dev, "%s: failed to read chip identifier\n",
+			__func__);
+		goto power_off;
+	}
+
+	if (chip_id != SC2235_CHIP_ID) {
+		dev_err(&client->dev, "%s: wrong chip identifier, expected 0x%x, got 0x%x\n",
+			__func__, SC2235_CHIP_ID, chip_id);
+		ret = -ENXIO;
+	}
+	dev_err(&client->dev, "%s: chip identifier, got 0x%x\n",
+		__func__, chip_id);
+
+power_off:
+	sc2235_set_power_off(sensor);
+	return ret;
+}
+
+static int sc2235_probe(struct i2c_client *client)
+{
+	struct device *dev = &client->dev;
+	struct fwnode_handle *endpoint;
+	struct sc2235_dev *sensor;
+	struct v4l2_mbus_framefmt *fmt;
+	u32 rotation;
+	int ret;
+	u8 chip_id_high, chip_id_low;
+
+	sensor = devm_kzalloc(dev, sizeof(*sensor), GFP_KERNEL);
+	if (!sensor)
+		return -ENOMEM;
+
+	sensor->i2c_client = client;
+
+	dev_err(&client->dev, "probe %s: read chipid: 0x%x\n",
+		__func__, (chip_id_high << 8) | chip_id_low);
+
+	fmt = &sensor->fmt;
+	fmt->code = MEDIA_BUS_FMT_SGBRG10_1X10;
+	fmt->colorspace = V4L2_COLORSPACE_SRGB;
+	fmt->ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(fmt->colorspace);
+	fmt->quantization = V4L2_QUANTIZATION_FULL_RANGE;
+	fmt->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(fmt->colorspace);
+	fmt->width = 1920;
+	fmt->height = 1080;
+	fmt->field = V4L2_FIELD_NONE;
+	sensor->frame_interval.numerator = 1;
+	sensor->frame_interval.denominator = sc2235_framerates[SC2235_30_FPS];
+	sensor->current_fr = SC2235_30_FPS;
+	sensor->current_mode =
+		&sc2235_mode_data[SC2235_MODE_1080P_1920_1080];
+	sensor->last_mode = sensor->current_mode;
+
+	sensor->ae_target = 52;
+
+	/* optional indication of physical rotation of sensor */
+	ret = fwnode_property_read_u32(dev_fwnode(&client->dev), "rotation",
+					&rotation);
+	if (!ret) {
+		switch (rotation) {
+		case 180:
+			sensor->upside_down = true;
+			fallthrough;
+		case 0:
+			break;
+		default:
+			dev_warn(dev, "%u degrees rotation is not supported, ignoring...\n",
+				rotation);
+		}
+	}
+
+	endpoint = fwnode_graph_get_next_endpoint(dev_fwnode(&client->dev),
+						NULL);
+	if (!endpoint) {
+		dev_err(dev, "endpoint node not found\n");
+		return -EINVAL;
+	}
+
+	ret = v4l2_fwnode_endpoint_parse(endpoint, &sensor->ep);
+	fwnode_handle_put(endpoint);
+	if (ret) {
+		dev_err(dev, "Could not parse endpoint\n");
+		return ret;
+	}
+
+	if (sensor->ep.bus_type != V4L2_MBUS_PARALLEL &&
+	    sensor->ep.bus_type != V4L2_MBUS_CSI2_DPHY &&
+	    sensor->ep.bus_type != V4L2_MBUS_BT656) {
+		dev_err(dev, "Unsupported bus type %d\n", sensor->ep.bus_type);
+		return -EINVAL;
+	}
+
+	/* get system clock (xclk) */
+	sensor->xclk = devm_clk_get(dev, "xclk");
+	if (IS_ERR(sensor->xclk)) {
+		dev_err(dev, "failed to get xclk\n");
+		return PTR_ERR(sensor->xclk);
+	}
+
+	sensor->xclk_freq = clk_get_rate(sensor->xclk);
+	if (sensor->xclk_freq < SC2235_XCLK_MIN ||
+	    sensor->xclk_freq > SC2235_XCLK_MAX) {
+		dev_err(dev, "xclk frequency out of range: %d Hz\n",
+			sensor->xclk_freq);
+		return -EINVAL;
+	}
+
+	/* request optional power down pin */
+	sensor->pwdn_gpio = devm_gpiod_get_optional(dev, "powerdown",
+						GPIOD_OUT_HIGH);
+	if (IS_ERR(sensor->pwdn_gpio))
+		return PTR_ERR(sensor->pwdn_gpio);
+
+	/* request optional reset pin */
+	sensor->reset_gpio = devm_gpiod_get_optional(dev, "reset",
+						GPIOD_OUT_HIGH);
+	if (IS_ERR(sensor->reset_gpio))
+		return PTR_ERR(sensor->reset_gpio);
+
+	v4l2_i2c_subdev_init(&sensor->sd, client, &sc2235_subdev_ops);
+
+	sensor->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
+			V4L2_SUBDEV_FL_HAS_EVENTS;
+	sensor->pad.flags = MEDIA_PAD_FL_SOURCE;
+	sensor->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+	ret = media_entity_pads_init(&sensor->sd.entity, 1, &sensor->pad);
+	if (ret)
+		return ret;
+
+	ret = sc2235_get_regulators(sensor);
+	if (ret)
+		return ret;
+	mutex_init(&sensor->lock);
+
+	ret = sc2235_check_chip_id(sensor);
+	if (ret)
+		goto entity_cleanup;
+
+	ret = sc2235_init_controls(sensor);
+	if (ret)
+		goto entity_cleanup;
+
+	ret = v4l2_async_register_subdev_sensor(&sensor->sd);
+	if (ret)
+		goto free_ctrls;
+
+	return 0;
+
+free_ctrls:
+	v4l2_ctrl_handler_free(&sensor->ctrls.handler);
+entity_cleanup:
+	media_entity_cleanup(&sensor->sd.entity);
+	mutex_destroy(&sensor->lock);
+	return ret;
+}
+
+static int sc2235_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct sc2235_dev *sensor = to_sc2235_dev(sd);
+
+	v4l2_async_unregister_subdev(&sensor->sd);
+	media_entity_cleanup(&sensor->sd.entity);
+	v4l2_ctrl_handler_free(&sensor->ctrls.handler);
+	mutex_destroy(&sensor->lock);
+
+	return 0;
+}
+
+static const struct i2c_device_id sc2235_id[] = {
+	{"sc2235", 0},
+	{},
+};
+MODULE_DEVICE_TABLE(i2c, sc2235_id);
+
+static const struct of_device_id sc2235_dt_ids[] = {
+	{ .compatible = "sc2235" },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, sc2235_dt_ids);
+
+static struct i2c_driver sc2235_i2c_driver = {
+	.driver = {
+		.name  = "sc2235",
+		.of_match_table	= sc2235_dt_ids,
+	},
+	.id_table = sc2235_id,
+	.probe_new = sc2235_probe,
+	.remove   = sc2235_remove,
+};
+
+module_i2c_driver(sc2235_i2c_driver);
+
+MODULE_DESCRIPTION("SC2235 MIPI Camera Subdev Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/platform/starfive/stf_common.h b/drivers/media/platform/starfive/stf_common.h
new file mode 100755
index 000000000000..23920a574349
--- /dev/null
+++ b/drivers/media/platform/starfive/stf_common.h
@@ -0,0 +1,184 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021 StarFive Technology Co., Ltd.
+ */
+#ifndef STF_COMMON_H
+#define STF_COMMON_H
+
+#include <linux/kern_levels.h>
+
+// #define STF_DEBUG
+
+#define USE_CLK_TREE  1
+
+// #define USE_CSIDPHY_ONE_CLK_MODE 1
+
+enum {
+	ST_DVP = 0x0001,
+	ST_CSIPHY = 0x0002,
+	ST_CSI = 0x0004,
+	ST_ISP = 0x0008,
+	ST_VIN = 0x0010,
+	ST_VIDEO = 0x0020,
+	ST_CAMSS = 0x0040,
+	ST_SENSOR= 0x0080,
+};
+
+enum {
+	ST_NONE = 0x00,
+	ST_ERR = 0x01,
+	ST_WARN = 0x02,
+	ST_INFO = 0x03,
+	ST_DEBUG = 0x04,
+};
+
+extern unsigned int stdbg_level;
+extern unsigned int stdbg_mask;
+
+#define ST_MODULE2STRING(__module) ({ \
+	char *__str; \
+	\
+	switch (__module) { \
+	case ST_DVP: \
+		__str = "st_dvp"; \
+		break; \
+	case ST_CSIPHY: \
+		__str = "st_csiphy"; \
+		break; \
+	case ST_CSI: \
+		__str = "st_csi"; \
+		break; \
+	case ST_ISP: \
+		__str = "st_isp"; \
+		break; \
+	case ST_VIN: \
+		__str = "st_vin"; \
+		break; \
+	case ST_VIDEO: \
+		__str = "st_video"; \
+		break; \
+	case ST_CAMSS: \
+		__str = "st_camss"; \
+		break; \
+	case ST_SENSOR: \
+		__str = "st_sensor"; \
+		break; \
+	default: \
+		__str = "unknow"; \
+		break; \
+	} \
+	\
+	__str; \
+	})
+
+#define st_debug(module, __fmt, arg...)	\
+	do { \
+		if (stdbg_level > ST_INFO) { \
+			if (stdbg_mask & module)  \
+				pr_err("[%s] debug: " __fmt, \
+						ST_MODULE2STRING(module), \
+						## arg); \
+		} \
+	} while (0)
+
+#define st_info(module, __fmt, arg...)	\
+	do { \
+		if (stdbg_level > ST_WARN) { \
+			if (stdbg_mask & module)  \
+				pr_err("[%s] info: " __fmt, \
+						ST_MODULE2STRING(module), \
+						## arg); \
+		} \
+	} while (0)
+
+#define st_warn(module, __fmt, arg...)	\
+	do { \
+		if (stdbg_level > ST_ERR) { \
+			if (stdbg_mask & module)  \
+				pr_err("[%s] warn: " __fmt, \
+						ST_MODULE2STRING(module), \
+						## arg); \
+		} \
+	} while (0)
+
+#define st_err(module, __fmt, arg...)	\
+	do { \
+		if (stdbg_level > ST_NONE) { \
+			if (stdbg_mask & module) \
+				pr_err("[%s] error: " __fmt, \
+						ST_MODULE2STRING(module), \
+						## arg); \
+		} \
+	} while (0)
+
+#define st_err_ratelimited(module, fmt, ...)                 \
+	do {                                                                    \
+		static DEFINE_RATELIMIT_STATE(_rs,                              \
+						DEFAULT_RATELIMIT_INTERVAL,     \
+						DEFAULT_RATELIMIT_BURST);       \
+		if (__ratelimit(&_rs) && (stdbg_level > ST_NONE)) {             \
+			if (stdbg_mask & module)                                \
+				pr_err("[%s] error: " fmt,                      \
+						ST_MODULE2STRING(module),       \
+						##__VA_ARGS__);                 \
+		} \
+	} while (0)
+
+#define set_bits(p, v, b, m)	(((p) & ~(m)) | ((v) << (b)))
+
+static inline u32 reg_read(void __iomem * base, u32 reg)
+{
+	return ioread32(base + reg);
+}
+
+static inline void reg_write(void __iomem * base, u32 reg, u32 val)
+{
+	iowrite32(val, base + reg);
+}
+
+static inline void reg_set_bit(void __iomem * base, u32 reg, u32 mask, u32 val)
+{
+	u32 value;
+
+	value = ioread32(base + reg) & ~mask;
+	val &= mask;
+	val |= value;
+	iowrite32(val, base + reg);
+}
+
+static inline void reg_set(void __iomem * base, u32 reg, u32 mask)
+{
+	iowrite32(ioread32(base + reg) | mask, base + reg);
+}
+
+static inline void reg_clear(void __iomem * base, u32 reg, u32 mask)
+{
+	iowrite32(ioread32(base + reg) & ~mask, base + reg);
+}
+
+static inline void reg_set_highest_bit(void __iomem * base, u32 reg)
+{
+	u32 val;
+
+	val = ioread32(base + reg);
+	val &= ~(0x1 << 31);
+	val |= (0x1 & 0x1) << 31;
+	iowrite32(val, base + reg);
+}
+
+static inline void reg_clr_highest_bit(void __iomem * base, u32 reg)
+{
+	u32 val;
+
+	val = ioread32(base + reg);
+	val &= ~(0x1 << 31);
+	val |= (0x0 & 0x1) << 31;
+	iowrite32(val, base + reg);
+}
+
+static inline void print_reg(unsigned int module, void __iomem * base, u32 reg)
+{
+	st_debug(module, "0x%08x = 0x%08x\n", reg, ioread32(base + reg));
+}
+
+#endif /* STF_COMMON_H */
diff --git a/drivers/media/platform/starfive/stf_csi.c b/drivers/media/platform/starfive/stf_csi.c
new file mode 100755
index 000000000000..efcfb1b54764
--- /dev/null
+++ b/drivers/media/platform/starfive/stf_csi.c
@@ -0,0 +1,394 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021 StarFive Technology Co., Ltd.
+ */
+#include "stfcamss.h"
+#include <media/v4l2-async.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-subdev.h>
+
+#define STF_CSI_NAME "stf_csi"
+
+static const struct csi_format csi_formats_st7110[] = {
+	{ MEDIA_BUS_FMT_YUYV8_2X8, 16},
+	{ MEDIA_BUS_FMT_RGB565_2X8_LE, 16},
+	{ MEDIA_BUS_FMT_SRGGB10_1X10, 12},
+	{ MEDIA_BUS_FMT_SGRBG10_1X10, 12},
+	{ MEDIA_BUS_FMT_SGBRG10_1X10, 12},
+	{ MEDIA_BUS_FMT_SBGGR10_1X10, 12},
+};
+
+static int csi_find_format(u32 code,
+		const struct csi_format *formats,
+		unsigned int nformats)
+{
+	int i;
+
+	for (i = 0; i < nformats; i++)
+		if (formats[i].code == code)
+			return i;
+	return -EINVAL;
+}
+
+int stf_csi_subdev_init(struct stfcamss *stfcamss, int id)
+{
+	struct stf_csi_dev *csi_dev = &stfcamss->csi_dev[id];
+
+	csi_dev->id = id;
+	csi_dev->csiphy_id = id;
+	csi_dev->s_type = SENSOR_VIN;
+	csi_dev->hw_ops = &csi_ops;
+	csi_dev->stfcamss = stfcamss;
+	csi_dev->formats = csi_formats_st7110;
+	csi_dev->nformats = ARRAY_SIZE(csi_formats_st7110);
+	mutex_init(&csi_dev->stream_lock);
+	return 0;
+}
+
+static int csi_set_power(struct v4l2_subdev *sd, int on)
+{
+	return 0;
+}
+
+static struct v4l2_mbus_framefmt *__csi_get_format(struct stf_csi_dev *csi_dev,
+		struct v4l2_subdev_state *state,
+		unsigned int pad,
+		enum v4l2_subdev_format_whence which)
+{
+	if (which == V4L2_SUBDEV_FORMAT_TRY)
+		return v4l2_subdev_get_try_format(&csi_dev->subdev, state, pad);
+
+	return &csi_dev->fmt[pad];
+}
+
+static int csi_set_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct stf_csi_dev *csi_dev = v4l2_get_subdevdata(sd);
+	struct stf_csi_dev *csi0_dev = &csi_dev->stfcamss->csi_dev[0];
+	struct v4l2_mbus_framefmt *format;
+	int ret = 0;
+	int is_raw10 = 0;
+	u32 code;
+
+	if (csi_dev->id == 1)
+		csi_set_stream(&csi0_dev->subdev, enable);
+
+	format = __csi_get_format(csi_dev, NULL, STF_CSI_PAD_SRC,
+				V4L2_SUBDEV_FORMAT_ACTIVE);
+	if (format == NULL)
+		return -EINVAL;
+	ret = csi_find_format(format->code, csi_dev->formats, csi_dev->nformats);
+	if (ret < 0)
+		return ret;
+
+	code = csi_dev->formats[ret].code;
+	if (code == MEDIA_BUS_FMT_SBGGR10_1X10 ||
+		code == MEDIA_BUS_FMT_SGBRG10_1X10 ||
+		code == MEDIA_BUS_FMT_SGRBG10_1X10 ||
+		code == MEDIA_BUS_FMT_SRGGB10_1X10)
+		is_raw10 = 1;
+
+	mutex_lock(&csi_dev->stream_lock);
+	if (enable) {
+		if (csi_dev->stream_count == 0) {
+			csi_dev->hw_ops->csi_config_set(csi_dev);
+			csi_dev->hw_ops->csi_clk_enable(csi_dev);
+			csi_dev->hw_ops->csi_set_format(csi_dev, format->height,
+					csi_dev->formats[ret].bpp, is_raw10);
+			csi_dev->hw_ops->csi_stream_set(csi_dev, enable);
+		}
+		csi_dev->stream_count++;
+	} else {
+		if (csi_dev->stream_count == 0)
+			goto exit;
+		if (csi_dev->stream_count == 1) {
+			csi_dev->hw_ops->csi_stream_set(csi_dev, enable);
+			csi_dev->hw_ops->csi_clk_disable(csi_dev);
+		}
+		csi_dev->stream_count--;
+	}
+exit:
+	mutex_unlock(&csi_dev->stream_lock);
+	return 0;
+}
+
+static void csi_try_format(struct stf_csi_dev *csi_dev,
+			struct v4l2_subdev_state *state,
+			unsigned int pad,
+			struct v4l2_mbus_framefmt *fmt,
+			enum v4l2_subdev_format_whence which)
+{
+	unsigned int i;
+
+	switch (pad) {
+	case STF_CSI_PAD_SINK:
+		/* Set format on sink pad */
+
+		for (i = 0; i < csi_dev->nformats; i++)
+			if (fmt->code == csi_dev->formats[i].code)
+				break;
+
+		if (i >= csi_dev->nformats)
+			fmt->code = MEDIA_BUS_FMT_RGB565_2X8_LE;
+
+		fmt->width = clamp_t(u32, fmt->width, 1, STFCAMSS_FRAME_MAX_WIDTH);
+		fmt->height = clamp_t(u32, fmt->height, 1,
+						STFCAMSS_FRAME_MAX_HEIGHT_PIX);
+
+		fmt->field = V4L2_FIELD_NONE;
+		fmt->colorspace = V4L2_COLORSPACE_SRGB;
+		fmt->flags = 0;
+		break;
+
+	case STF_CSI_PAD_SRC:
+		*fmt = *__csi_get_format(csi_dev, state, STF_CSI_PAD_SINK, which);
+		break;
+	}
+}
+
+static int csi_enum_mbus_code(struct v4l2_subdev *sd,
+			struct v4l2_subdev_state *state,
+			struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct stf_csi_dev *csi_dev = v4l2_get_subdevdata(sd);
+
+	if (code->index >= csi_dev->nformats)
+		return -EINVAL;
+	if (code->pad == STF_CSI_PAD_SINK) {
+		code->code = csi_dev->formats[code->index].code;
+	} else {
+		struct v4l2_mbus_framefmt *sink_fmt;
+
+		sink_fmt = __csi_get_format(csi_dev, state,
+					STF_CSI_PAD_SINK, code->which);
+
+		code->code = sink_fmt->code;
+		if (!code->code)
+			return -EINVAL;
+	}
+	code->flags = 0;
+
+	return 0;
+}
+
+static int csi_enum_frame_size(struct v4l2_subdev *sd,
+				struct v4l2_subdev_state *state,
+				struct v4l2_subdev_frame_size_enum *fse)
+{
+	struct stf_csi_dev *csi_dev = 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;
+	csi_try_format(csi_dev, state, 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;
+	csi_try_format(csi_dev, state, fse->pad, &format, fse->which);
+	fse->max_width = format.width;
+	fse->max_height = format.height;
+
+	return 0;
+}
+
+static int csi_get_format(struct v4l2_subdev *sd,
+			struct v4l2_subdev_state *state,
+			struct v4l2_subdev_format *fmt)
+{
+	struct stf_csi_dev *csi_dev = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	format = __csi_get_format(csi_dev, state, fmt->pad, fmt->which);
+	if (format == NULL)
+		return -EINVAL;
+
+	fmt->format = *format;
+
+	return 0;
+}
+
+static int csi_set_format(struct v4l2_subdev *sd,
+			struct v4l2_subdev_state *state,
+			struct v4l2_subdev_format *fmt)
+{
+	struct stf_csi_dev *csi_dev = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	format = __csi_get_format(csi_dev, state, fmt->pad, fmt->which);
+	if (format == NULL)
+		return -EINVAL;
+
+	csi_try_format(csi_dev, state, fmt->pad, &fmt->format, fmt->which);
+	*format = fmt->format;
+
+	/* Propagate the format from sink to source */
+	if (fmt->pad == STF_CSI_PAD_SINK) {
+		format = __csi_get_format(csi_dev, state, STF_CSI_PAD_SRC, fmt->which);
+		*format = fmt->format;
+		csi_try_format(csi_dev, state, STF_CSI_PAD_SRC, format, fmt->which);
+	}
+
+	return 0;
+}
+
+static int csi_init_formats(struct v4l2_subdev *sd,
+			struct v4l2_subdev_fh *fh)
+{
+	struct v4l2_subdev_format format = {
+		.pad = STF_CSI_PAD_SINK,
+		.which = fh ? V4L2_SUBDEV_FORMAT_TRY : V4L2_SUBDEV_FORMAT_ACTIVE,
+		.format = {
+			.code = MEDIA_BUS_FMT_RGB565_2X8_LE,
+			.width = 1920,
+			.height = 1080
+		}
+	};
+
+	return csi_set_format(sd, fh ? fh->state : NULL, &format);
+}
+
+static int csi_link_setup(struct media_entity *entity,
+			const struct media_pad *local,
+			const struct media_pad *remote, u32 flags)
+{
+	if ((local->flags & MEDIA_PAD_FL_SOURCE) &&
+		(flags & MEDIA_LNK_FL_ENABLED)) {
+		struct v4l2_subdev *sd;
+		struct stf_csi_dev *csi_dev;
+		struct vin_line *line;
+
+		if (media_entity_remote_pad(local))
+			return -EBUSY;
+
+		sd = media_entity_to_v4l2_subdev(entity);
+		csi_dev = v4l2_get_subdevdata(sd);
+
+		sd = media_entity_to_v4l2_subdev(remote->entity);
+		line = v4l2_get_subdevdata(sd);
+		if (line->sdev_type == VIN_DEV_TYPE)
+			csi_dev->s_type = SENSOR_VIN;
+		if (line->sdev_type == ISP0_DEV_TYPE)
+			csi_dev->s_type = SENSOR_ISP0;
+		if (line->sdev_type == ISP1_DEV_TYPE)
+			csi_dev->s_type = SENSOR_ISP1;
+		st_info(ST_CSI, "CSI%d device sensor type: %d\n",
+				csi_dev->id, csi_dev->s_type);
+	}
+
+	if ((local->flags & MEDIA_PAD_FL_SINK) &&
+		(flags & MEDIA_LNK_FL_ENABLED)) {
+		struct v4l2_subdev *sd;
+		struct stf_csi_dev *csi_dev;
+		struct stf_csiphy_dev *csiphy_dev;
+
+		if (media_entity_remote_pad(local))
+			return -EBUSY;
+
+		sd = media_entity_to_v4l2_subdev(entity);
+		csi_dev = v4l2_get_subdevdata(sd);
+
+		sd = media_entity_to_v4l2_subdev(remote->entity);
+		csiphy_dev = v4l2_get_subdevdata(sd);
+
+		csi_dev->csiphy_id = csiphy_dev->id;
+		st_info(ST_SENSOR, "CSI%d link to csiphy%d\n",
+				csi_dev->id, csi_dev->csiphy_id);
+	}
+
+	return 0;
+}
+
+static const struct v4l2_subdev_core_ops csi_core_ops = {
+	.s_power = csi_set_power,
+};
+
+static const struct v4l2_subdev_video_ops csi_video_ops = {
+	.s_stream = csi_set_stream,
+};
+
+static const struct v4l2_subdev_pad_ops csi_pad_ops = {
+	.enum_mbus_code = csi_enum_mbus_code,
+	.enum_frame_size = csi_enum_frame_size,
+	.get_fmt = csi_get_format,
+	.set_fmt = csi_set_format,
+};
+
+static const struct v4l2_subdev_ops csi_v4l2_ops = {
+	.core = &csi_core_ops,
+	.video = &csi_video_ops,
+	.pad = &csi_pad_ops,
+};
+
+static const struct v4l2_subdev_internal_ops csi_v4l2_internal_ops = {
+	.open = csi_init_formats,
+};
+
+static const struct media_entity_operations csi_media_ops = {
+	.link_setup = csi_link_setup,
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+int stf_csi_register(struct stf_csi_dev *csi_dev, struct v4l2_device *v4l2_dev)
+{
+	struct v4l2_subdev *sd = &csi_dev->subdev;
+	struct device *dev = csi_dev->stfcamss->dev;
+	struct media_pad *pads = csi_dev->pads;
+	int ret;
+
+	v4l2_subdev_init(sd, &csi_v4l2_ops);
+	sd->internal_ops = &csi_v4l2_internal_ops;
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	snprintf(sd->name, ARRAY_SIZE(sd->name), "%s%d",
+		STF_CSI_NAME, csi_dev->id);
+	v4l2_set_subdevdata(sd, csi_dev);
+
+	ret = csi_init_formats(sd, NULL);
+	if (ret < 0) {
+		dev_err(dev, "Failed to init format: %d\n", ret);
+		return ret;
+	}
+
+	pads[STF_CSI_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
+	pads[STF_CSI_PAD_SRC].flags = MEDIA_PAD_FL_SOURCE;
+
+	sd->entity.function = MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER;
+	sd->entity.ops = &csi_media_ops;
+	ret = media_entity_pads_init(&sd->entity, STF_CSI_PADS_NUM, pads);
+	if (ret < 0) {
+		dev_err(dev, "Failed to init media entity: %d\n", ret);
+		return ret;
+	}
+
+	ret = v4l2_device_register_subdev(v4l2_dev, sd);
+	if (ret < 0) {
+		dev_err(dev, "Failed to register subdev: %d\n", ret);
+		goto err_sreg;
+	}
+
+	return 0;
+
+err_sreg:
+	media_entity_cleanup(&sd->entity);
+	return ret;
+}
+
+int stf_csi_unregister(struct stf_csi_dev *csi_dev)
+{
+	v4l2_device_unregister_subdev(&csi_dev->subdev);
+	media_entity_cleanup(&csi_dev->subdev.entity);
+	mutex_destroy(&csi_dev->stream_lock);
+	return 0;
+}
diff --git a/drivers/media/platform/starfive/stf_csi.h b/drivers/media/platform/starfive/stf_csi.h
new file mode 100644
index 000000000000..0fa786b67162
--- /dev/null
+++ b/drivers/media/platform/starfive/stf_csi.h
@@ -0,0 +1,60 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021 StarFive Technology Co., Ltd.
+ */
+#ifndef STF_CSI_H
+#define STF_CSI_H
+
+#include <media/v4l2-subdev.h>
+#include <media/v4l2-device.h>
+#include <media/media-entity.h>
+#include <video/stf-vin.h>
+
+#define STF_CSI_PAD_SINK     0
+#define STF_CSI_PAD_SRC      1
+#define STF_CSI_PADS_NUM     2
+
+struct csi_format {
+	u32 code;
+	u8 bpp;
+};
+
+struct stf_csi_dev;
+
+struct csi_hw_ops {
+	int (*csi_clk_enable)(struct stf_csi_dev *csi_dev);
+	int (*csi_clk_disable)(struct stf_csi_dev *csi_dev);
+	int (*csi_config_set)(struct stf_csi_dev *csi_dev);
+	int (*csi_set_format)(struct stf_csi_dev *csi_dev,
+			u32 vsize, u8 bpp, int is_raw10);
+	int (*csi_stream_set)(struct stf_csi_dev *csi_dev, int on);
+};
+
+struct stf_csi_dev {
+	struct stfcamss *stfcamss;
+	u8 id;
+	u8 csiphy_id;
+	enum sensor_type s_type;
+	struct v4l2_subdev subdev;
+	struct media_pad pads[STF_CSI_PADS_NUM];
+	struct v4l2_mbus_framefmt fmt[STF_CSI_PADS_NUM];
+	const struct csi_format *formats;
+	unsigned int nformats;
+	struct csi_hw_ops *hw_ops;
+	struct mutex stream_lock;
+	int stream_count;
+};
+
+extern int stf_csi_subdev_init(struct stfcamss *stfcamss, int id);
+extern int stf_csi_register(struct stf_csi_dev *csi_dev,
+			struct v4l2_device *v4l2_dev);
+extern int stf_csi_unregister(struct stf_csi_dev *csi_dev);
+extern int raw_csi2rx_dphy_config(struct stf_vin_dev *vin,
+			const csi2rx_dphy_cfg_t *cfg);
+extern int raw_csi2rx_config(struct stf_vin_dev *vin,
+			int id,
+			const csi2rx_cfg_t *cfg);
+extern struct csi_hw_ops csi_ops;
+extern void dump_csi_reg(void *__iomem csibase, int id);
+
+#endif /* STF_CSI_H */
diff --git a/drivers/media/platform/starfive/stf_csi_hw_ops.c b/drivers/media/platform/starfive/stf_csi_hw_ops.c
new file mode 100755
index 000000000000..ce72499bb589
--- /dev/null
+++ b/drivers/media/platform/starfive/stf_csi_hw_ops.c
@@ -0,0 +1,427 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * linux/drivers/media/platform/starfive/stf_csi.c
+ *
+ * Copyright (C) 2021 StarFive Technology Co., Ltd.
+ *
+ */
+#include "stfcamss.h"
+
+#define CSI2RX_DEVICE_CFG_REG			0x000
+
+#define CSI2RX_SOFT_RESET_REG			0x004
+#define CSI2RX_SOFT_RESET_PROTOCOL		BIT(1)
+#define CSI2RX_SOFT_RESET_FRONT			BIT(0)
+
+#define CSI2RX_DPHY_LANE_CONTROL                0x040
+
+#define CSI2RX_STATIC_CFG_REG			0x008
+#define CSI2RX_STATIC_CFG_DLANE_MAP(llane, plane)	\
+		((plane) << (16 + (llane) * 4))
+#define CSI2RX_STATIC_CFG_LANES_MASK		GENMASK(11, 8)
+
+#define CSI2RX_STREAM_BASE(n)		(((n) + 1) * 0x100)
+
+#define CSI2RX_STREAM_CTRL_REG(n)		(CSI2RX_STREAM_BASE(n) + 0x000)
+#define CSI2RX_STREAM_CTRL_START		BIT(0)
+
+#define CSI2RX_STREAM_DATA_CFG_REG(n)		(CSI2RX_STREAM_BASE(n) + 0x008)
+#define CSI2RX_STREAM_DATA_CFG_EN_VC_SELECT	BIT(31)
+#define CSI2RX_STREAM_DATA_CFG_VC_SELECT(n)	BIT((n) + 16)
+
+#define CSI2RX_STREAM_CFG_REG(n)		(CSI2RX_STREAM_BASE(n) + 0x00c)
+#define CSI2RX_STREAM_CFG_FIFO_MODE_LARGE_BUF	(1 << 8)
+
+#define CSI2RX_LANES_MAX	4
+#define CSI2RX_STREAMS_MAX	4
+
+static int apb_clk_set(struct stf_vin_dev *vin, int on)
+{
+	static int init_flag;
+	static struct mutex count_lock;
+	static int count;
+
+	if (!init_flag) {
+		init_flag = 1;
+		mutex_init(&count_lock);
+	}
+	mutex_lock(&count_lock);
+	if (on) {
+		if (count == 0)
+			reg_set_highest_bit(vin->clkgen_base, CLK_CSI2RX0_APB_CTRL);
+		count++;
+	} else {
+		if (count == 0)
+			goto exit;
+		if (count == 1)
+			reg_clr_highest_bit(vin->clkgen_base, CLK_CSI2RX0_APB_CTRL);
+		count--;
+	}
+exit:
+	mutex_unlock(&count_lock);
+	return 0;
+
+}
+static int stf_csi_clk_enable(struct stf_csi_dev *csi_dev)
+{
+	struct stfcamss *stfcamss = csi_dev->stfcamss;
+	struct stf_vin_dev *vin = stfcamss->vin;
+	int ret = 0;
+
+// #ifdef USE_CLK_TREE
+#if 0
+	// enable clk
+	ret = stfcamss_enable_clocks(1, &stfcamss->sys_clk[STFCLK_CSI_2RX_APH_CLK],
+			stfcamss->dev);
+	if (ret < 0) {
+		st_err(ST_CSI, "%s enable clk failed\n", __func__);
+		return ret;
+	}
+#else
+	apb_clk_set(vin, 1);
+#endif
+
+	if (csi_dev->id == 0) {
+	// #ifdef USE_CLK_TREE
+	#if 0
+		// enable clk
+		ret = stfcamss_enable_clocks(5, &stfcamss->sys_clk[STFCLK_MIPIRX0_PIXEL0],
+				stfcamss->dev);
+		if (ret < 0) {
+			st_err(ST_CSI, "%s enable clk failed\n", __func__);
+			return ret;
+		}
+	#else
+		reg_set_bit(vin->clkgen_base, CLK_MIPI_RX0_PXL_CTRL, 0x1F, 0x3);
+		reg_set_highest_bit(vin->clkgen_base, CLK_MIPI_RX0_PXL_0_CTRL);
+		reg_set_highest_bit(vin->clkgen_base, CLK_MIPI_RX0_PXL_1_CTRL);
+		reg_set_highest_bit(vin->clkgen_base, CLK_MIPI_RX0_PXL_2_CTRL);
+		reg_set_highest_bit(vin->clkgen_base, CLK_MIPI_RX0_PXL_3_CTRL);
+		reg_set_highest_bit(vin->clkgen_base, CLK_MIPI_RX0_SYS0_CTRL);
+	#endif
+	} else {
+	// #ifdef USE_CLK_TREE
+	#if 0
+		// enable clk
+		ret = stfcamss_enable_clocks(5, &stfcamss->sys_clk[STFCLK_MIPIRX1_PIXEL0],
+				stfcamss->dev);
+		if (ret < 0) {
+			st_err(ST_CSI, "%s enable clk failed\n", __func__);
+			return ret;
+		}
+	#else
+		reg_set_bit(vin->clkgen_base, CLK_MIPI_RX1_PXL_CTRL, 0x1F, 0x3);
+		reg_set_highest_bit(vin->clkgen_base, CLK_MIPI_RX1_PXL_0_CTRL);
+		reg_set_highest_bit(vin->clkgen_base, CLK_MIPI_RX1_PXL_1_CTRL);
+		reg_set_highest_bit(vin->clkgen_base, CLK_MIPI_RX1_PXL_2_CTRL);
+		reg_set_highest_bit(vin->clkgen_base, CLK_MIPI_RX1_PXL_3_CTRL);
+		reg_set_highest_bit(vin->clkgen_base, CLK_MIPI_RX1_SYS1_CTRL);
+	#endif
+	}
+
+	return ret;
+}
+
+static int stf_csi_clk_disable(struct stf_csi_dev *csi_dev)
+{
+	struct stfcamss *stfcamss = csi_dev->stfcamss;
+	struct stf_vin_dev *vin = stfcamss->vin;
+
+// #ifdef USE_CLK_TREE
+#if 0
+	stfcamss_disable_clocks(1, &stfcamss->sys_clk[STFCLK_CSI_2RX_APH_CLK]);
+#else
+	// reg_clr_highest_bit(vin->clkgen_base, CLK_CSI2RX0_APB_CTRL);
+	apb_clk_set(vin, 0);
+#endif
+
+	if (csi_dev->id == 0) {
+	// #ifdef USE_CLK_TREE
+	#if 0
+		// disable clk
+		stfcamss_disable_clocks(5, &stfcamss->sys_clk[STFCLK_MIPIRX0_PIXEL0]);
+	#else
+		reg_clr_highest_bit(vin->clkgen_base, CLK_MIPI_RX0_PXL_0_CTRL);
+		reg_clr_highest_bit(vin->clkgen_base, CLK_MIPI_RX0_PXL_1_CTRL);
+		reg_clr_highest_bit(vin->clkgen_base, CLK_MIPI_RX0_PXL_2_CTRL);
+		reg_clr_highest_bit(vin->clkgen_base, CLK_MIPI_RX0_PXL_3_CTRL);
+		reg_clr_highest_bit(vin->clkgen_base, CLK_MIPI_RX0_SYS0_CTRL);
+	#endif
+	} else {
+	// #ifdef USE_CLK_TREE
+	#if 0
+		// disable clk
+		stfcamss_disable_clocks(5, &stfcamss->sys_clk[STFCLK_MIPIRX1_PIXEL0]);
+	#else
+		reg_clr_highest_bit(vin->clkgen_base, CLK_MIPI_RX1_PXL_0_CTRL);
+		reg_clr_highest_bit(vin->clkgen_base, CLK_MIPI_RX1_PXL_1_CTRL);
+		reg_clr_highest_bit(vin->clkgen_base, CLK_MIPI_RX1_PXL_2_CTRL);
+		reg_clr_highest_bit(vin->clkgen_base, CLK_MIPI_RX1_PXL_3_CTRL);
+		reg_clr_highest_bit(vin->clkgen_base, CLK_MIPI_RX1_SYS1_CTRL);
+	#endif
+	}
+
+	return 0;
+}
+
+static int stf_csi_config_set(struct stf_csi_dev *csi_dev)
+{
+	struct stf_vin_dev *vin = csi_dev->stfcamss->vin;
+	u32 mipi_channel_sel, mipi_vc = 0;
+
+	st_info(ST_CSI, "%s, csi_id = %d\n", __func__, csi_dev->id);
+
+	switch (csi_dev->s_type) {
+	case SENSOR_VIN:
+		st_info(ST_CSI, "%s, %d: need todo\n", __func__, __LINE__);
+		break;
+	case SENSOR_ISP0:
+	// #ifdef USE_CLK_TREE
+	#if 0
+		st_err(ST_CSI, "%s, %d: need todo\n", __func__, __LINE__);
+	#else
+		reg_set_bit(vin->clkgen_base, CLK_ISP0_MIPI_CTRL, BIT(24),
+				csi_dev->id << 24);
+
+		reg_set_bit(vin->clkgen_base, CLK_C_ISP0_CTRL,BIT(25) | BIT(24),
+				csi_dev->id << 24);
+	#endif
+		mipi_channel_sel = csi_dev->id * 4 + mipi_vc;
+		reg_set_bit(vin->sysctrl_base, SYSCTRL_VIN_SRC_CHAN_SEL,
+				0xF, mipi_channel_sel);
+		break;
+	case SENSOR_ISP1:
+	// #ifdef USE_CLK_TREE
+	#if 0
+		st_err(ST_CSI, "%s, %d: need todo\n", __func__, __LINE__);
+	#else
+		reg_set_bit(vin->clkgen_base, CLK_ISP1_MIPI_CTRL,
+				BIT(24), csi_dev->id << 24);
+		reg_set_bit(vin->clkgen_base, CLK_C_ISP1_CTRL, BIT(25) | BIT(24),
+				csi_dev->id << 24);
+	#endif
+
+		mipi_channel_sel = csi_dev->id * 4 + mipi_vc;
+		reg_set_bit(vin->sysctrl_base, SYSCTRL_VIN_SRC_CHAN_SEL,
+				0xF << 4, mipi_channel_sel << 4);
+	default:
+		break;
+	}
+
+	return 0;
+}
+
+static int stf_csi_set_format(struct stf_csi_dev *csi_dev,
+			u32 vsize, u8 bpp, int is_raw10)
+{
+	struct stf_vin_dev *vin = csi_dev->stfcamss->vin;
+	void *reg_base = NULL;
+
+	if (csi_dev->id == 0)
+		reg_base = vin->mipi0_base;
+	else if (csi_dev->id == 1)
+		reg_base = vin->mipi1_base;
+	else
+		return 0;
+
+	switch (csi_dev->s_type) {
+	case SENSOR_VIN:
+		st_info(ST_CSI, "%s, %d: need todo\n", __func__, __LINE__);
+		break;
+	case SENSOR_ISP0:
+		if (is_raw10)
+			reg_set_bit(vin->sysctrl_base, SYSCTRL_VIN_SRC_DW_SEL,
+					BIT(4), 1 << 4);
+		break;
+	case SENSOR_ISP1:
+		if (is_raw10)
+			reg_set_bit(vin->sysctrl_base, SYSCTRL_VIN_SRC_DW_SEL,
+					BIT(5), 1 << 5);
+	default:
+		break;
+	}
+
+	return 0;
+}
+
+static void csi2rx_reset(void *reg_base)
+{
+	writel(CSI2RX_SOFT_RESET_PROTOCOL | CSI2RX_SOFT_RESET_FRONT,
+			reg_base + CSI2RX_SOFT_RESET_REG);
+	udelay(10);
+	writel(0, reg_base + CSI2RX_SOFT_RESET_REG);
+}
+
+#if 0
+static void csi2rx_debug_config(void *reg_base, u32 frame_lines)
+{
+	// data_id, ecc, crc error to irq
+	union error_bypass_cfg err_bypass_cfg = {
+		.data_id = 0,
+		.ecc     = 0,
+		.crc     = 0,
+	};
+	union stream_monitor_ctrl stream0_monitor_ctrl = {
+		.frame_length = frame_lines,
+		.frame_mon_en = 1,
+		.frame_mon_vc = 0,
+		.lb_en = 1,
+		.lb_vc = 0,
+	};
+	reg_write(reg_base, ERROR_BYPASS_CFG, err_bypass_cfg.value);
+	reg_write(reg_base, MONITOR_IRQS_MASK_CFG, 0xffffffff);
+	reg_write(reg_base, INFO_IRQS_MASK_CFG, 0xffffffff);
+	reg_write(reg_base, ERROR_IRQS_MASK_CFG, 0xffffffff);
+	reg_write(reg_base, DPHY_ERR_IRQ_MASK_CFG, 0xffffffff);
+
+	reg_write(reg_base, STREAM0_MONITOR_CTRL, stream0_monitor_ctrl.value);
+	reg_write(reg_base, STREAM0_FCC_CTRL, (0x0 << 1) | 0x1);
+}
+#endif
+
+static int csi2rx_start(struct stf_csi_dev *csi_dev)
+{
+	struct stfcamss *stfcamss = csi_dev->stfcamss;
+	struct stf_vin_dev *vin = stfcamss->vin;
+	struct csi2phy_cfg *csiphy =
+		stfcamss->csiphy_dev[csi_dev->csiphy_id].csiphy;
+	unsigned int i;
+	unsigned long lanes_used = 0;
+	u32 reg;
+
+	void *reg_base = NULL;
+
+	if (!csiphy) {
+		st_info(ST_CSI, "csiphy%d sensor not exist use csiphy%d init.\n",
+				csi_dev->csiphy_id, !csi_dev->csiphy_id);
+		csiphy = stfcamss->csiphy_dev[!csi_dev->csiphy_id].csiphy;
+		if (!csiphy) {
+			st_err(ST_CSI, "csiphy%d sensor not exist\n",
+				!csi_dev->csiphy_id);
+			return -EINVAL;
+		}
+	}
+
+	if (csi_dev->id == 0)
+		reg_base = vin->mipi0_base;
+	else if (csi_dev->id == 1)
+		reg_base = vin->mipi1_base;
+	else
+		return 0;
+
+	csi2rx_reset(reg_base);
+
+	reg = csiphy->num_data_lanes << 8;
+	for (i = 0; i < csiphy->num_data_lanes; i++) {
+#ifndef USE_CSIDPHY_ONE_CLK_MODE
+		reg |= CSI2RX_STATIC_CFG_DLANE_MAP(i, csiphy->data_lanes[i]);
+		set_bit(csiphy->data_lanes[i] - 1, &lanes_used);
+#else
+		reg |= CSI2RX_STATIC_CFG_DLANE_MAP(i, i + 1);
+		set_bit(i, &lanes_used);
+#endif
+	}
+
+	/*
+	 * Even the unused lanes need to be mapped. In order to avoid
+	 * to map twice to the same physical lane, keep the lanes used
+	 * in the previous loop, and only map unused physical lanes to
+	 * the rest of our logical lanes.
+	 */
+	for (i = csiphy->num_data_lanes; i < CSI2RX_LANES_MAX; i++) {
+		unsigned int idx = find_first_zero_bit(&lanes_used, CSI2RX_LANES_MAX);
+		set_bit(idx, &lanes_used);
+		reg |= CSI2RX_STATIC_CFG_DLANE_MAP(i, idx + 1);
+	}
+
+	writel(reg, reg_base + CSI2RX_STATIC_CFG_REG);
+
+	// 0x40 DPHY_LANE_CONTROL
+	reg = 0;
+#ifndef USE_CSIDPHY_ONE_CLK_MODE
+	for (i = 0; i < csiphy->num_data_lanes; i++)
+		reg |= 1 << (csiphy->data_lanes[i] - 1)
+			| 1 << (csiphy->data_lanes[i] + 11);
+#else
+	for (i = 0; i < csiphy->num_data_lanes; i++)
+		reg |= 1 << i | 1 << (i + 12);
+#endif
+
+	reg |= 1 << 4 | 1 << 16;
+	writel(reg, reg_base + CSI2RX_DPHY_LANE_CONTROL);
+
+	// csi2rx_debug_config(reg_base, 1080);
+
+	/*
+	 * Create a static mapping between the CSI virtual channels
+	 * and the output stream.
+	 *
+	 * This should be enhanced, but v4l2 lacks the support for
+	 * changing that mapping dynamically.
+	 *
+	 * We also cannot enable and disable independent streams here,
+	 * hence the reference counting.
+	 */
+	for (i = 0; i < CSI2RX_STREAMS_MAX; i++) {
+		writel(CSI2RX_STREAM_CFG_FIFO_MODE_LARGE_BUF,
+		       reg_base + CSI2RX_STREAM_CFG_REG(i));
+
+		writel(CSI2RX_STREAM_DATA_CFG_EN_VC_SELECT |
+		       CSI2RX_STREAM_DATA_CFG_VC_SELECT(i),
+		       reg_base + CSI2RX_STREAM_DATA_CFG_REG(i));
+
+		writel(CSI2RX_STREAM_CTRL_START, reg_base + CSI2RX_STREAM_CTRL_REG(i));
+	}
+
+	return 0;
+}
+
+static void csi2rx_stop(void *reg_base)
+{
+	unsigned int i;
+
+	for (i = 0; i < CSI2RX_STREAMS_MAX; i++)
+		writel(0, reg_base + CSI2RX_STREAM_CTRL_REG(i));
+}
+
+static int stf_csi_stream_set(struct stf_csi_dev *csi_dev, int on)
+{
+	struct stf_vin_dev *vin = csi_dev->stfcamss->vin;
+	void *reg_base = NULL;
+
+	if (csi_dev->id == 0)
+		reg_base = vin->mipi0_base;
+	else if (csi_dev->id == 1)
+		reg_base = vin->mipi1_base;
+	else
+		return 0;
+
+	if (on)
+		csi2rx_start(csi_dev);
+	else
+		csi2rx_stop(reg_base);
+
+	return 0;
+}
+
+void dump_csi_reg(void *__iomem csibase, int id)
+{
+	st_info(ST_CSI, "DUMP CSI%d register:\n", id);
+	print_reg(ST_CSI, csibase, 0x00);
+	print_reg(ST_CSI, csibase, 0x04);
+	print_reg(ST_CSI, csibase, 0x08);
+	print_reg(ST_CSI, csibase, 0x10);
+
+	print_reg(ST_CSI, csibase, 0x40);
+	print_reg(ST_CSI, csibase, 0x48);
+	print_reg(ST_CSI, csibase, 0x4c);
+	print_reg(ST_CSI, csibase, 0x50);
+}
+
+struct csi_hw_ops csi_ops = {
+	.csi_clk_enable        = stf_csi_clk_enable,
+	.csi_clk_disable       = stf_csi_clk_disable,
+	.csi_config_set        = stf_csi_config_set,
+	.csi_set_format        = stf_csi_set_format,
+	.csi_stream_set        = stf_csi_stream_set,
+};
diff --git a/drivers/media/platform/starfive/stf_csiphy.c b/drivers/media/platform/starfive/stf_csiphy.c
new file mode 100755
index 000000000000..a1ca59426d3e
--- /dev/null
+++ b/drivers/media/platform/starfive/stf_csiphy.c
@@ -0,0 +1,358 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021 StarFive Technology Co., Ltd.
+ */
+#include "stfcamss.h"
+#include <media/v4l2-async.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-subdev.h>
+
+#define STF_CSIPHY_NAME "stf_csiphy"
+
+static const struct csiphy_format csiphy_formats_st7110[] = {
+	{ MEDIA_BUS_FMT_YUYV8_2X8, 16},
+	{ MEDIA_BUS_FMT_RGB565_2X8_LE, 16},
+	{ MEDIA_BUS_FMT_SRGGB10_1X10, 12},
+	{ MEDIA_BUS_FMT_SGRBG10_1X10, 12},
+	{ MEDIA_BUS_FMT_SGBRG10_1X10, 12},
+	{ MEDIA_BUS_FMT_SBGGR10_1X10, 12},
+};
+
+int stf_csiphy_subdev_init(struct stfcamss *stfcamss, int id)
+{
+	struct stf_csiphy_dev *csiphy_dev = &stfcamss->csiphy_dev[id];
+
+	csiphy_dev->id = id;
+	csiphy_dev->csi_id = id;
+	csiphy_dev->hw_ops = &csiphy_ops;
+	csiphy_dev->stfcamss = stfcamss;
+	csiphy_dev->formats = csiphy_formats_st7110;
+	csiphy_dev->nformats = ARRAY_SIZE(csiphy_formats_st7110);
+	mutex_init(&csiphy_dev->stream_lock);
+	return 0;
+}
+
+static int csiphy_set_power(struct v4l2_subdev *sd, int on)
+{
+
+#ifdef CONFIG_VIDEO_CADENCE_CSI2RX
+	struct stf_csiphy_dev *csiphy_dev = v4l2_get_subdevdata(sd);
+	if (on)
+		csiphy_dev->hw_ops->cdns_csi_power(csiphy_dev, on);
+#endif
+	return 0;
+}
+
+static int csiphy_set_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct stf_csiphy_dev *csiphy_dev = v4l2_get_subdevdata(sd);
+
+	mutex_lock(&csiphy_dev->stream_lock);
+	if (enable) {
+		if (csiphy_dev->stream_count == 0) {
+			csiphy_dev->hw_ops->csiphy_clk_enable(csiphy_dev);
+			csiphy_dev->hw_ops->csiphy_config_set(csiphy_dev);
+			csiphy_dev->hw_ops->csiphy_stream_set(csiphy_dev, 1);
+		}
+		csiphy_dev->stream_count++;
+	} else {
+		if (csiphy_dev->stream_count == 0)
+			goto exit;
+		if (csiphy_dev->stream_count == 1) {
+			csiphy_dev->hw_ops->csiphy_clk_disable(csiphy_dev);
+			csiphy_dev->hw_ops->csiphy_stream_set(csiphy_dev, 0);
+		}
+		csiphy_dev->stream_count--;
+	}
+exit:
+	mutex_unlock(&csiphy_dev->stream_lock);
+
+#ifdef CONFIG_VIDEO_CADENCE_CSI2RX
+	if (!enable)
+		csiphy_dev->hw_ops->cdns_csi_power(csiphy_dev, enable);
+#endif
+	return 0;
+}
+
+static struct v4l2_mbus_framefmt *
+__csiphy_get_format(struct stf_csiphy_dev *csiphy_dev,
+		struct v4l2_subdev_state *state,
+		unsigned int pad,
+		enum v4l2_subdev_format_whence which)
+{
+	if (which == V4L2_SUBDEV_FORMAT_TRY)
+		return v4l2_subdev_get_try_format( &csiphy_dev->subdev, state, pad);
+
+	return &csiphy_dev->fmt[pad];
+}
+
+static void csiphy_try_format(struct stf_csiphy_dev *csiphy_dev,
+			struct v4l2_subdev_state *state,
+			unsigned int pad,
+			struct v4l2_mbus_framefmt *fmt,
+			enum v4l2_subdev_format_whence which)
+{
+	unsigned int i;
+
+	switch (pad) {
+	case STF_CSIPHY_PAD_SINK:
+		/* Set format on sink pad */
+
+		for (i = 0; i < csiphy_dev->nformats; i++)
+			if (fmt->code == csiphy_dev->formats[i].code)
+				break;
+
+		if (i >= csiphy_dev->nformats)
+			fmt->code = MEDIA_BUS_FMT_RGB565_2X8_LE;
+
+		fmt->width = clamp_t(u32, fmt->width, 1, STFCAMSS_FRAME_MAX_WIDTH);
+		fmt->height = clamp_t(u32, fmt->height, 1,
+				STFCAMSS_FRAME_MAX_HEIGHT_PIX);
+		fmt->field = V4L2_FIELD_NONE;
+		fmt->colorspace = V4L2_COLORSPACE_SRGB;
+		fmt->flags = 0;
+
+		break;
+
+	case STF_CSIPHY_PAD_SRC:
+
+		*fmt = *__csiphy_get_format(csiphy_dev, state,
+				STF_CSIPHY_PAD_SINK, which);
+		break;
+	}
+}
+
+static int csiphy_enum_mbus_code(struct v4l2_subdev *sd,
+			struct v4l2_subdev_state *state,
+			struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct stf_csiphy_dev *csiphy_dev = v4l2_get_subdevdata(sd);
+
+	if (code->index >= csiphy_dev->nformats)
+		return -EINVAL;
+
+	if (code->pad == STF_CSIPHY_PAD_SINK) {
+		code->code = csiphy_dev->formats[code->index].code;
+	} else {
+		struct v4l2_mbus_framefmt *sink_fmt;
+
+		sink_fmt = __csiphy_get_format(csiphy_dev, state,
+					STF_CSIPHY_PAD_SINK,
+					code->which);
+
+		code->code = sink_fmt->code;
+		if (!code->code)
+			return -EINVAL;
+	}
+	code->flags = 0;
+	return 0;
+}
+
+static int csiphy_enum_frame_size(struct v4l2_subdev *sd,
+				struct v4l2_subdev_state *state,
+				struct v4l2_subdev_frame_size_enum *fse)
+{
+	struct stf_csiphy_dev *csiphy_dev = 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_dev, state, 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_dev, state, fse->pad, &format, fse->which);
+	fse->max_width = format.width;
+	fse->max_height = format.height;
+
+	return 0;
+}
+
+static int csiphy_get_format(struct v4l2_subdev *sd,
+			struct v4l2_subdev_state *state,
+			struct v4l2_subdev_format *fmt)
+{
+	struct stf_csiphy_dev *csiphy_dev = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	format = __csiphy_get_format(csiphy_dev, state, fmt->pad, fmt->which);
+	if (format == NULL)
+		return -EINVAL;
+
+	fmt->format = *format;
+
+	return 0;
+}
+
+static int csiphy_set_format(struct v4l2_subdev *sd,
+			struct v4l2_subdev_state *state,
+			struct v4l2_subdev_format *fmt)
+{
+	struct stf_csiphy_dev *csiphy_dev = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	format = __csiphy_get_format(csiphy_dev, state, fmt->pad, fmt->which);
+	if (format == NULL)
+		return -EINVAL;
+
+	csiphy_try_format(csiphy_dev, state, fmt->pad, &fmt->format, fmt->which);
+	*format = fmt->format;
+
+	/* Propagate the format from sink to source */
+	if (fmt->pad == STF_CSIPHY_PAD_SINK) {
+		format = __csiphy_get_format(csiphy_dev, state, STF_CSIPHY_PAD_SRC,
+					fmt->which);
+		*format = fmt->format;
+		csiphy_try_format(csiphy_dev, state, STF_CSIPHY_PAD_SRC, format,
+					fmt->which);
+	}
+
+	return 0;
+}
+
+static int csiphy_init_formats(struct v4l2_subdev *sd,
+			struct v4l2_subdev_fh *fh)
+{
+	struct v4l2_subdev_format format = {
+		.pad = STF_CSIPHY_PAD_SINK,
+		.which = fh ? V4L2_SUBDEV_FORMAT_TRY : V4L2_SUBDEV_FORMAT_ACTIVE,
+		.format = {
+			.code = MEDIA_BUS_FMT_RGB565_2X8_LE,
+			.width = 1920,
+			.height = 1080
+		}
+	};
+
+	return csiphy_set_format(sd, fh ? fh->state : NULL, &format);
+}
+
+static int csiphy_link_setup(struct media_entity *entity,
+			const struct media_pad *local,
+			const struct media_pad *remote, u32 flags)
+{
+	if ((local->flags & MEDIA_PAD_FL_SOURCE) &&
+		(flags & MEDIA_LNK_FL_ENABLED)) {
+		struct v4l2_subdev *sd;
+		struct stf_csiphy_dev *csiphy_dev;
+		struct stf_csi_dev *csi_dev;
+
+		if (media_entity_remote_pad(local))
+			return -EBUSY;
+
+		sd = media_entity_to_v4l2_subdev(entity);
+		csiphy_dev = v4l2_get_subdevdata(sd);
+
+		sd = media_entity_to_v4l2_subdev(remote->entity);
+		csi_dev = v4l2_get_subdevdata(sd);
+		csiphy_dev->csi_id = csi_dev->id;
+		st_info(ST_CSIPHY, "CSIPHY%d link to CSI%d\n",
+				csiphy_dev->id, csiphy_dev->csi_id);
+	}
+
+	return 0;
+}
+
+static const struct v4l2_subdev_core_ops csiphy_core_ops = {
+	.s_power = csiphy_set_power,
+};
+
+static const struct v4l2_subdev_video_ops csiphy_video_ops = {
+	.s_stream = csiphy_set_stream,
+};
+
+static const struct v4l2_subdev_pad_ops csiphy_pad_ops = {
+	.enum_mbus_code = csiphy_enum_mbus_code,
+	.enum_frame_size = csiphy_enum_frame_size,
+	.get_fmt = csiphy_get_format,
+	.set_fmt = csiphy_set_format,
+};
+
+static const struct v4l2_subdev_ops csiphy_v4l2_ops = {
+	.core = &csiphy_core_ops,
+	.video = &csiphy_video_ops,
+	.pad = &csiphy_pad_ops,
+};
+
+static const struct v4l2_subdev_internal_ops csiphy_v4l2_internal_ops = {
+	.open = csiphy_init_formats,
+};
+
+static const struct media_entity_operations csiphy_media_ops = {
+	.link_setup = csiphy_link_setup,
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+int stf_csiphy_register(struct stf_csiphy_dev *csiphy_dev,
+			struct v4l2_device *v4l2_dev)
+{
+	struct v4l2_subdev *sd = &csiphy_dev->subdev;
+	struct device *dev = csiphy_dev->stfcamss->dev;
+	struct media_pad *pads = csiphy_dev->pads;
+	int ret;
+
+	v4l2_subdev_init(sd, &csiphy_v4l2_ops);
+	sd->internal_ops = &csiphy_v4l2_internal_ops;
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	snprintf(sd->name, ARRAY_SIZE(sd->name), "%s%d",
+		STF_CSIPHY_NAME, csiphy_dev->id);
+	v4l2_set_subdevdata(sd, csiphy_dev);
+
+	ret = csiphy_init_formats(sd, NULL);
+	if (ret < 0) {
+		dev_err(dev, "Failed to init format: %d\n", ret);
+		return ret;
+	}
+
+	pads[STF_CSIPHY_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
+	pads[STF_CSIPHY_PAD_SRC].flags = MEDIA_PAD_FL_SOURCE;
+
+	sd->entity.function = MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER;
+	sd->entity.ops = &csiphy_media_ops;
+	ret = media_entity_pads_init(&sd->entity, STF_CSIPHY_PADS_NUM, pads);
+	if (ret < 0) {
+		dev_err(dev, "Failed to init media entity: %d\n", ret);
+		return ret;
+	}
+
+#ifndef CONFIG_VIDEO_CADENCE_CSI2RX
+	ret = v4l2_device_register_subdev(v4l2_dev, sd);
+	if (ret < 0) {
+		dev_err(dev, "Failed to register subdev: %d\n", ret);
+		goto err_sreg;
+	}
+#else
+	sd->dev = dev;
+	ret = v4l2_async_register_subdev(sd);
+	if (ret < 0) {
+		dev_err(dev, "Failed to async register subdev: %d\n", ret);
+		goto err_sreg;
+	}
+#endif
+
+	return 0;
+
+err_sreg:
+	media_entity_cleanup(&sd->entity);
+	return ret;
+}
+
+int stf_csiphy_unregister(struct stf_csiphy_dev *csiphy_dev)
+{
+	v4l2_device_unregister_subdev(&csiphy_dev->subdev);
+	media_entity_cleanup(&csiphy_dev->subdev.entity);
+	mutex_destroy(&csiphy_dev->stream_lock);
+	return 0;
+}
diff --git a/drivers/media/platform/starfive/stf_csiphy.h b/drivers/media/platform/starfive/stf_csiphy.h
new file mode 100755
index 000000000000..23292e8f60a8
--- /dev/null
+++ b/drivers/media/platform/starfive/stf_csiphy.h
@@ -0,0 +1,188 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021 StarFive Technology Co., Ltd.
+ */
+#ifndef STF_CSIPHY_H
+#define STF_CSIPHY_H
+
+#include <media/v4l2-subdev.h>
+#include <media/v4l2-device.h>
+#include <media/media-entity.h>
+#include <video/stf-vin.h>
+
+#define STF_CSIPHY_PAD_SINK     0
+#define STF_CSIPHY_PAD_SRC      1
+#define STF_CSIPHY_PADS_NUM     2
+
+#define STF_CSI2_MAX_DATA_LANES      4
+
+union static_config {
+	u32 raw;
+	struct {
+		u32 sel                 : 2;
+		u32 rsvd_6              : 2;
+		u32 v2p0_support_enable : 1;
+		u32 rsvd_5              : 3;
+		u32 lane_nb             : 3;
+		u32 rsvd_4              : 5;
+		u32 dl0_map             : 3;
+		u32 rsvd_3              : 1;
+		u32 dl1_map             : 3;
+		u32 rsvd_2              : 1;
+		u32 dl2_map             : 3;
+		u32 rsvd_1              : 1;
+		u32 dl3_map             : 3;
+		u32 rsvd_0              : 1;
+	} bits;
+};
+
+union error_bypass_cfg {
+	u32 value;
+	struct {
+		u32 crc             :  1;
+		u32 ecc             :  1;
+		u32 data_id         :  1;
+		u32 rsvd_0          : 29;
+	};
+};
+
+union stream_monitor_ctrl {
+	u32 value;
+	struct {
+		u32 lb_vc             : 4;
+		u32 lb_en             : 1;
+		u32 timer_vc          : 4;
+		u32 timer_en          : 1;
+		u32 timer_eof         : 1;
+		u32 frame_mon_vc      : 4;
+		u32 frame_mon_en      : 1;
+		u32 frame_length      : 16;
+	};
+};
+
+union stream_cfg {
+	u32 value;
+	struct {
+		u32 interface_mode :  1;
+		u32 ls_le_mode     :  1;
+		u32 rsvd_3         :  2;
+		u32 num_pixels     :  2;
+		u32 rsvd_2         :  2;
+		u32 fifo_mode      :  2;
+		u32 rsvd_1         :  2;
+		u32 bpp_bypass     :  3;
+		u32 rsvd_0         :  1;
+		u32 fifo_fill      : 16;
+	};
+};
+
+union dphy_lane_ctrl {
+	u32 raw;
+	struct {
+		u32 dl0_en    : 1;
+		u32 dl1_en    : 1;
+		u32 dl2_en    : 1;
+		u32 dl3_en    : 1;
+		u32 cl_en     : 1;
+		u32 rsvd_1    : 7;
+		u32 dl0_reset : 1;
+		u32 dl1_reset : 1;
+		u32 dl2_reset : 1;
+		u32 dl3_reset : 1;
+		u32 cl_reset  : 1;
+		u32 rsvd_0    : 15;
+	} bits;
+};
+
+union dphy_lane_swap {
+	u32 raw;
+	struct {
+		u32 rx_1c2c_sel        : 1;
+		u32 lane_swap_clk      : 3;
+		u32 lane_swap_clk1     : 3;
+		u32 lane_swap_lan0     : 3;
+		u32 lane_swap_lan1     : 3;
+		u32 lane_swap_lan2     : 3;
+		u32 lane_swap_lan3     : 3;
+		u32 dpdn_swap_clk      : 1;
+		u32 dpdn_swap_clk1     : 1;
+		u32 dpdn_swap_lan0     : 1;
+		u32 dpdn_swap_lan1     : 1;
+		u32 dpdn_swap_lan2     : 1;
+		u32 dpdn_swap_lan3     : 1;
+		u32 hs_freq_chang_clk0 : 1;
+		u32 hs_freq_chang_clk1 : 1;
+		u32 reserved           : 5;
+	} bits;
+};
+
+union dphy_lane_en {
+	u32 raw;
+	struct {
+		u32 gpio_en		: 6;
+		u32 mp_test_mode_sel	: 5;
+		u32 mp_test_en		: 1;
+		u32 dphy_enable_lan0	: 1;
+		u32 dphy_enable_lan1	: 1;
+		u32 dphy_enable_lan2	: 1;
+		u32 dphy_enable_lan3	: 1;
+		u32 rsvd_0		: 16;
+	} bits;
+};
+
+struct csiphy_format {
+	u32 code;
+	u8 bpp;
+};
+
+struct csi2phy_cfg {
+	unsigned int flags;
+	unsigned char data_lanes[STF_CSI2_MAX_DATA_LANES];
+	unsigned char clock_lane;
+	unsigned char num_data_lanes;
+	bool lane_polarities[1 + STF_CSI2_MAX_DATA_LANES];
+};
+
+struct csi2phy_cfg2 {
+	unsigned char data_lanes[STF_CSI2_MAX_DATA_LANES];
+	unsigned char num_data_lanes;
+	unsigned char num_clks;
+	unsigned char clock_lane;
+	unsigned char clock1_lane;
+	bool lane_polarities[2 + STF_CSI2_MAX_DATA_LANES];
+};
+
+struct stf_csiphy_dev;
+
+struct csiphy_hw_ops {
+	int (*csiphy_clk_enable)(struct stf_csiphy_dev *csiphy_dev);
+	int (*csiphy_clk_disable)(struct stf_csiphy_dev *csiphy_dev);
+	int (*csiphy_config_set)(struct stf_csiphy_dev *csiphy_dev);
+	int (*csiphy_stream_set)(struct stf_csiphy_dev *csiphy_dev, int on);
+#ifdef CONFIG_VIDEO_CADENCE_CSI2RX
+	int (*cdns_csi_power)(struct stf_csiphy_dev *csiphy_dev, int on);
+#endif
+};
+
+struct stf_csiphy_dev {
+	struct stfcamss *stfcamss;
+	struct csi2phy_cfg *csiphy;
+	u8 id;
+	u8 csi_id;
+	struct v4l2_subdev subdev;
+	struct media_pad pads[STF_CSIPHY_PADS_NUM];
+	struct v4l2_mbus_framefmt fmt[STF_CSIPHY_PADS_NUM];
+	const struct csiphy_format *formats;
+	unsigned int nformats;
+	struct csiphy_hw_ops *hw_ops;
+	struct mutex stream_lock;
+	int stream_count;
+};
+
+extern int stf_csiphy_subdev_init(struct stfcamss *stfcamss, int id);
+extern int stf_csiphy_register(struct stf_csiphy_dev *csiphy_dev,
+			struct v4l2_device *v4l2_dev);
+extern int stf_csiphy_unregister(struct stf_csiphy_dev *csiphy_dev);
+extern struct csiphy_hw_ops csiphy_ops;
+
+#endif /* STF_CSIPHY_H */
diff --git a/drivers/media/platform/starfive/stf_csiphy_hw_ops.c b/drivers/media/platform/starfive/stf_csiphy_hw_ops.c
new file mode 100755
index 000000000000..43b82ad3b791
--- /dev/null
+++ b/drivers/media/platform/starfive/stf_csiphy_hw_ops.c
@@ -0,0 +1,358 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021 StarFive Technology Co., Ltd.
+ */
+#include "stfcamss.h"
+#include <linux/sort.h>
+
+static int stf_csiphy_clk_set(struct stf_csiphy_dev *csiphy_dev, int on)
+{
+	struct stf_vin_dev *vin = csiphy_dev->stfcamss->vin;
+	static int init_flag;
+	static struct mutex count_lock;
+	static int count;
+
+	if (!init_flag) {
+		init_flag = 1;
+		mutex_init(&count_lock);
+	}
+	mutex_lock(&count_lock);
+	if (on) {
+		if (count == 0) {
+			reg_set_bit(vin->clkgen_base,
+				CLK_DPHY_CFGCLK_ISPCORE_2X_CTRL,
+				0x1F, 0x08);
+			reg_set_bit(vin->clkgen_base,
+				CLK_DPHY_CFGCLK_ISPCORE_2X_CTRL,
+				1 << 31, 1 << 31);
+			reg_set_bit(vin->clkgen_base,
+				CLK_DPHY_REFCLK_ISPCORE_2X_CTRL,
+				0x1F, 0x10);
+			reg_set_bit(vin->clkgen_base,
+				CLK_DPHY_REFCLK_ISPCORE_2X_CTRL,
+				1 << 31, 1 << 31);
+			reg_set_bit(vin->clkgen_base,
+				CLK_DPHY_TXCLKESC_IN_CTRL,
+				0x3F, 0x28);
+			reg_set_bit(vin->clkgen_base,
+				CLK_DPHY_TXCLKESC_IN_CTRL,
+				1 << 31, 1 << 31);
+		}
+		count++;
+	} else {
+		if (count == 0)
+			goto exit;
+		if (count == 1) {
+			reg_set_bit(vin->clkgen_base,
+				CLK_DPHY_CFGCLK_ISPCORE_2X_CTRL,
+				1 << 31, 0 << 31);
+			reg_set_bit(vin->clkgen_base,
+				CLK_DPHY_REFCLK_ISPCORE_2X_CTRL,
+				1 << 31, 0 << 31);
+			reg_set_bit(vin->clkgen_base,
+				CLK_DPHY_TXCLKESC_IN_CTRL,
+				1 << 31, 0 << 31);
+		}
+		count--;
+	}
+exit:
+	mutex_unlock(&count_lock);
+	return 0;
+}
+
+static int stf_csiphy_clk_enable(struct stf_csiphy_dev *csiphy_dev)
+{
+
+
+// #ifdef USE_CLK_TREE
+#if 0
+	// enable clk
+	struct stfcamss *stfcamss = csiphy_dev->stfcamss;
+	int ret = 0;
+	ret = stfcamss_enable_clocks(3, &stfcamss->sys_clk[STFCLK_CSIDPHY_CFGCLK],
+			stfcamss->dev);
+	if (ret < 0) {
+		st_debug(ST_CSIPHY, "%s enable clk failed\n", __func__);
+		return ret;
+	}
+	return ret;
+#else
+	return stf_csiphy_clk_set(csiphy_dev, 1);
+#endif
+}
+
+static int stf_csiphy_clk_disable(struct stf_csiphy_dev *csiphy_dev)
+{
+
+// #ifdef USE_CLK_TREE
+#if 0
+	struct stfcamss *stfcamss = csiphy_dev->stfcamss;
+	stfcamss_disable_clocks(3, &stfcamss->sys_clk[STFCLK_CSIDPHY_CFGCLK]);
+	return 0;
+#else
+	return stf_csiphy_clk_set(csiphy_dev, 0);
+#endif
+}
+
+static int cmp_func(const void *x1, const void *x2)
+{
+	return *((unsigned char *)x1) - *((unsigned char *)x2);
+}
+
+int try_cfg(struct csi2phy_cfg2 *cfg, struct csi2phy_cfg *cfg0,
+		struct csi2phy_cfg *cfg1)
+{
+	int i = 0;
+
+	if (cfg0 && cfg1) {
+		st_debug(ST_CSIPHY, "CSIPHY use 2 clk mode\n");
+		cfg->num_clks = 2;
+		cfg->num_data_lanes =
+			cfg1->num_data_lanes + cfg0->num_data_lanes;
+		if (cfg->num_data_lanes > STF_CSI2_MAX_DATA_LANES)
+			return -EINVAL;
+		cfg->clock_lane = cfg0->clock_lane;
+		cfg->lane_polarities[0] = cfg0->lane_polarities[0];
+		cfg->clock1_lane = cfg1->clock_lane;
+		cfg->lane_polarities[1] = cfg1->lane_polarities[0];
+		for (i = 0; i < cfg0->num_data_lanes; i++) {
+			cfg->data_lanes[i] = cfg0->data_lanes[i];
+			cfg->lane_polarities[i + 2] =
+				cfg0->lane_polarities[i + 1];
+		}
+
+		for (i = cfg0->num_data_lanes; i < cfg->num_data_lanes; i++) {
+			cfg->data_lanes[i] =
+				cfg1->data_lanes[i - cfg0->num_data_lanes];
+			cfg->lane_polarities[i + 2] =
+				cfg1->lane_polarities[i - cfg0->num_data_lanes + 1];
+		}
+	} else if (cfg0 && !cfg1) {
+		st_debug(ST_CSIPHY, "CSIPHY cfg0 use 1 clk mode\n");
+		cfg->num_clks = 1;
+		cfg->num_data_lanes = cfg0->num_data_lanes;
+		cfg->clock_lane = cfg->clock1_lane  = cfg0->clock_lane;
+		cfg->lane_polarities[0] = cfg->lane_polarities[1] =
+						cfg0->lane_polarities[0];
+		for (i = 0; i < cfg0->num_data_lanes; i++) {
+			cfg->data_lanes[i] = cfg0->data_lanes[i];
+			cfg->lane_polarities[i + 2] = cfg0->lane_polarities[i + 1];
+		}
+	} else if (!cfg0 && cfg1) {
+		st_debug(ST_CSIPHY, "CSIPHY cfg1 use 1 clk mode\n");
+		cfg->num_clks = 1;
+		cfg->num_data_lanes = cfg1->num_data_lanes;
+		cfg->clock_lane = cfg->clock1_lane  = cfg1->clock_lane;
+		cfg->lane_polarities[0] = cfg->lane_polarities[1] =
+						cfg1->lane_polarities[0];
+		for (i = 0; i < cfg1->num_data_lanes; i++) {
+			cfg->data_lanes[i] = cfg1->data_lanes[i];
+			cfg->lane_polarities[i + 2] = cfg1->lane_polarities[i + 1];
+		}
+	} else {
+		return -EINVAL;
+	}
+
+#ifndef USE_CSIDPHY_ONE_CLK_MODE
+	sort(cfg->data_lanes, cfg->num_data_lanes,
+			sizeof(cfg->data_lanes[0]), cmp_func, NULL);
+#endif
+	for (i = 0; i < cfg->num_data_lanes; i++)
+		st_debug(ST_CSIPHY, "%d: %d\n", i, cfg->data_lanes[i]);
+	return 0;
+}
+
+static int csi2rx_dphy_config(struct stf_vin_dev *vin,
+		struct stf_csiphy_dev *csiphy_dev)
+{
+	struct csi2phy_cfg2 cfg2 = {0};
+	struct csi2phy_cfg2 *cfg = &cfg2;
+	struct stf_csiphy_dev *csiphy0_dev =
+		&csiphy_dev->stfcamss->csiphy_dev[0];
+	struct stf_csiphy_dev *csiphy1_dev =
+		&csiphy_dev->stfcamss->csiphy_dev[1];
+	struct csi2phy_cfg *phy0cfg = csiphy0_dev->csiphy;
+	struct csi2phy_cfg *phy1cfg = csiphy1_dev->csiphy;
+	int i;
+	int id = csiphy_dev->id;
+	u32 reg = 0;
+
+	if (!phy0cfg && !phy1cfg)
+		return -EINVAL;
+
+#ifdef USE_CSIDPHY_ONE_CLK_MODE
+	if (id == 0) {
+		phy0cfg = csiphy0_dev->csiphy;
+		phy1cfg = NULL;
+	} else {
+		phy0cfg = NULL;
+		phy1cfg = csiphy1_dev->csiphy;
+	}
+#endif
+
+	if (try_cfg(cfg, phy0cfg, phy1cfg))
+		return -EINVAL;
+
+	id = cfg->num_clks == 2 ? 1 : 0;
+
+	reg = reg_read(vin->sysctrl_base, SYSCTRL_REG4);
+
+	st_debug(ST_CSIPHY, "id = %d, clock_lane = %d, SYSCTRL_REG4: 0x%x\n",
+			id, cfg->clock_lane, reg);
+	st_debug(ST_CSIPHY, "csiphy_dev: csi_id = %d, id = %d\n",
+			csiphy_dev->csi_id, csiphy_dev->id);
+
+	reg = set_bits(reg, id, 0, 0x1);
+	reg = set_bits(reg, cfg->clock_lane, 1, 0x7 << 1);
+	reg = set_bits(reg, cfg->lane_polarities[0], 19, 0x1 << 19);
+	reg = set_bits(reg, cfg->clock1_lane, 4, 0x7 << 4);
+	reg = set_bits(reg, cfg->lane_polarities[1], 20, 0x1 << 20);
+
+	for (i = 0; i < cfg->num_data_lanes; i++) {
+		reg = set_bits(reg, cfg->data_lanes[i], (7 + i * 3),
+				0x7 << (7 + i * 3));
+		reg = set_bits(reg, !!cfg->lane_polarities[i + 2],
+					(21 + i), 0x1 << (21 + i));
+	}
+
+	reg_write(vin->sysctrl_base, SYSCTRL_REG4, reg);
+	reg = reg_read(vin->sysctrl_base, SYSCTRL_DPHY_CTRL);
+	for (i = 0; i < cfg->num_data_lanes; i++) {
+		reg = set_bits(reg, 1, (11 + cfg->data_lanes[i]),
+				0x1 << (11 + cfg->data_lanes[i]));
+	}
+
+	reg_write(vin->sysctrl_base, SYSCTRL_DPHY_CTRL, reg);
+	print_reg(ST_CSIPHY, vin->sysctrl_base, SYSCTRL_REG4);
+	print_reg(ST_CSIPHY, vin->sysctrl_base, SYSCTRL_DPHY_CTRL);
+	return 0;
+}
+
+static int stf_csiphy_config_set(struct stf_csiphy_dev *csiphy_dev)
+{
+	struct stf_vin_dev *vin = csiphy_dev->stfcamss->vin;
+
+	st_debug(ST_CSIPHY, "%s, csiphy id = %d\n",__func__, csiphy_dev->id);
+	csi2rx_dphy_config(vin, csiphy_dev);
+	return 0;
+}
+
+static int stf_csiphy_stream_set(struct stf_csiphy_dev *csiphy_dev, int on)
+{
+	return 0;
+}
+
+#ifdef CONFIG_VIDEO_CADENCE_CSI2RX
+static int stf_csi_clk_enable(struct stf_csiphy_dev *csiphy_dev)
+{
+	struct stf_vin_dev *vin = csiphy_dev->stfcamss->vin;
+
+	reg_set_highest_bit(vin->clkgen_base, CLK_CSI2RX0_APB_CTRL);
+
+	if (csiphy_dev->id == 0) {
+		reg_set_bit(vin->clkgen_base,CLK_MIPI_RX0_PXL_CTRL,0x1F, 0x3);
+		reg_set_highest_bit(vin->clkgen_base, CLK_MIPI_RX0_PXL_0_CTRL);
+		reg_set_highest_bit(vin->clkgen_base, CLK_MIPI_RX0_PXL_1_CTRL);
+		reg_set_highest_bit(vin->clkgen_base, CLK_MIPI_RX0_PXL_2_CTRL);
+		reg_set_highest_bit(vin->clkgen_base, CLK_MIPI_RX0_PXL_3_CTRL);
+		reg_set_highest_bit(vin->clkgen_base, CLK_MIPI_RX0_SYS0_CTRL);
+	} else {
+		reg_set_bit(vin->clkgen_base,CLK_MIPI_RX1_PXL_CTRL,0x1F, 0x3);
+		reg_set_highest_bit(vin->clkgen_base, CLK_MIPI_RX1_PXL_0_CTRL);
+		reg_set_highest_bit(vin->clkgen_base, CLK_MIPI_RX1_PXL_1_CTRL);
+		reg_set_highest_bit(vin->clkgen_base, CLK_MIPI_RX1_PXL_2_CTRL);
+		reg_set_highest_bit(vin->clkgen_base, CLK_MIPI_RX1_PXL_3_CTRL);
+		reg_set_highest_bit(vin->clkgen_base, CLK_MIPI_RX1_SYS1_CTRL);
+	}
+
+	return 0;
+}
+
+static int stf_csi_clk_disable(struct stf_csiphy_dev *csiphy_dev)
+{
+	struct stf_vin_dev *vin = csiphy_dev->stfcamss->vin;
+
+	reg_clr_highest_bit(vin->clkgen_base, CLK_CSI2RX0_APB_CTRL);
+
+	if (csiphy_dev->id == 0) {
+		reg_clr_highest_bit(vin->clkgen_base, CLK_MIPI_RX0_PXL_0_CTRL);
+		reg_clr_highest_bit(vin->clkgen_base, CLK_MIPI_RX0_PXL_1_CTRL);
+		reg_clr_highest_bit(vin->clkgen_base, CLK_MIPI_RX0_PXL_2_CTRL);
+		reg_clr_highest_bit(vin->clkgen_base, CLK_MIPI_RX0_PXL_3_CTRL);
+		reg_clr_highest_bit(vin->clkgen_base, CLK_MIPI_RX0_SYS0_CTRL);
+	} else {
+		reg_clr_highest_bit(vin->clkgen_base, CLK_MIPI_RX1_PXL_0_CTRL);
+		reg_clr_highest_bit(vin->clkgen_base, CLK_MIPI_RX1_PXL_1_CTRL);
+		reg_clr_highest_bit(vin->clkgen_base, CLK_MIPI_RX1_PXL_2_CTRL);
+		reg_clr_highest_bit(vin->clkgen_base, CLK_MIPI_RX1_PXL_3_CTRL);
+		reg_clr_highest_bit(vin->clkgen_base, CLK_MIPI_RX1_SYS1_CTRL);
+	}
+
+	return 0;
+}
+
+static int stf_csi_config_set(struct stf_csiphy_dev *csiphy_dev, int is_raw10)
+{
+	struct stf_vin_dev *vin = csiphy_dev->stfcamss->vin;
+	u32 mipi_channel_sel, mipi_vc = 0;
+	enum sensor_type s_type = SENSOR_ISP0;
+
+	switch (s_type) {
+	case SENSOR_VIN:
+		break;
+	case SENSOR_ISP0:
+		reg_set_bit(vin->clkgen_base, CLK_ISP0_MIPI_CTRL,
+				BIT(24), csiphy_dev->id << 24);
+
+		reg_set_bit(vin->clkgen_base, CLK_C_ISP0_CTRL, BIT(25) | BIT(24),
+				csiphy_dev->id << 24);
+
+		mipi_channel_sel = csiphy_dev->id * 4 + mipi_vc;
+		reg_set_bit(vin->sysctrl_base, SYSCTRL_VIN_SRC_CHAN_SEL,
+				0xF, mipi_channel_sel);
+
+		if (is_raw10)
+			reg_set_bit(vin->sysctrl_base, SYSCTRL_VIN_SRC_DW_SEL,
+					BIT(4), 1 << 4);
+		break;
+	case SENSOR_ISP1:
+		reg_set_bit(vin->clkgen_base, CLK_ISP1_MIPI_CTRL,
+				BIT(24), csiphy_dev->id << 24);
+		reg_set_bit(vin->clkgen_base, CLK_C_ISP1_CTRL, BIT(25) | BIT(24),
+				csiphy_dev->id << 24);
+
+		mipi_channel_sel = csiphy_dev->id * 4 + mipi_vc;
+		reg_set_bit(vin->sysctrl_base, SYSCTRL_VIN_SRC_CHAN_SEL,
+				0xF << 4, mipi_channel_sel << 4);
+
+		if (is_raw10)
+			reg_set_bit(vin->sysctrl_base, SYSCTRL_VIN_SRC_DW_SEL,
+					BIT(5), 1 << 5);
+	default:
+		break;
+	}
+
+	return 0;
+}
+
+static int stf_cdns_csi_power(struct stf_csiphy_dev *csiphy_dev, int on)
+{
+	if (on) {
+		stf_csi_config_set(csiphy_dev, 1);
+		stf_csi_clk_enable(csiphy_dev);
+	} else
+		stf_csi_clk_disable(csiphy_dev);
+
+	return 0;
+}
+#endif
+
+struct csiphy_hw_ops csiphy_ops = {
+	.csiphy_clk_enable        = stf_csiphy_clk_enable,
+	.csiphy_clk_disable       = stf_csiphy_clk_disable,
+	.csiphy_config_set        = stf_csiphy_config_set,
+	.csiphy_stream_set        = stf_csiphy_stream_set,
+#ifdef CONFIG_VIDEO_CADENCE_CSI2RX
+	.cdns_csi_power           = stf_cdns_csi_power,
+#endif
+};
diff --git a/drivers/media/platform/starfive/stf_dvp.c b/drivers/media/platform/starfive/stf_dvp.c
new file mode 100755
index 000000000000..1a0c2141a2c0
--- /dev/null
+++ b/drivers/media/platform/starfive/stf_dvp.c
@@ -0,0 +1,361 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021 StarFive Technology Co., Ltd.
+ */
+#include "stfcamss.h"
+#include <media/v4l2-async.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-subdev.h>
+
+#define STF_DVP_NAME "stf_dvp"
+
+static const struct dvp_format dvp_formats_st7110[] = {
+	{ MEDIA_BUS_FMT_YUYV8_2X8, 16},
+	{ MEDIA_BUS_FMT_RGB565_2X8_LE, 16},
+	{ MEDIA_BUS_FMT_SRGGB10_1X10, 12},
+	{ MEDIA_BUS_FMT_SGRBG10_1X10, 12},
+	{ MEDIA_BUS_FMT_SGBRG10_1X10, 12},
+	{ MEDIA_BUS_FMT_SBGGR10_1X10, 12},
+};
+
+static int dvp_find_format(u32 code,
+		const struct dvp_format *formats,
+		unsigned int nformats)
+{
+	int i;
+
+	for (i = 0; i < nformats; i++)
+		if (formats[i].code == code)
+			return i;
+
+	return -EINVAL;
+}
+
+int stf_dvp_subdev_init(struct stfcamss *stfcamss)
+{
+	struct stf_dvp_dev *dvp_dev = stfcamss->dvp_dev;
+
+	dvp_dev->s_type = SENSOR_VIN;
+	dvp_dev->hw_ops = &dvp_ops;
+	dvp_dev->stfcamss = stfcamss;
+	dvp_dev->formats = dvp_formats_st7110;
+	dvp_dev->nformats = ARRAY_SIZE(dvp_formats_st7110);
+	mutex_init(&dvp_dev->stream_lock);
+	dvp_dev->stream_count = 0;
+
+	return 0;
+}
+
+static int dvp_set_power(struct v4l2_subdev *sd, int on)
+{
+	return 0;
+}
+
+static struct v4l2_mbus_framefmt *
+__dvp_get_format(struct stf_dvp_dev *dvp_dev,
+		struct v4l2_subdev_state *state,
+		unsigned int pad,
+		enum v4l2_subdev_format_whence which)
+{
+	if (which == V4L2_SUBDEV_FORMAT_TRY)
+		return v4l2_subdev_get_try_format(
+				&dvp_dev->subdev, state, pad);
+
+	return &dvp_dev->fmt[pad];
+}
+
+static int dvp_set_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct stf_dvp_dev *dvp_dev = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+	int ret = 0;
+
+	format = __dvp_get_format(dvp_dev, NULL, STF_DVP_PAD_SRC,
+				V4L2_SUBDEV_FORMAT_ACTIVE);
+	if (format == NULL)
+		return -EINVAL;
+	ret = dvp_find_format(format->code, dvp_dev->formats, dvp_dev->nformats);
+	if (ret < 0)
+		return ret;
+
+	mutex_lock(&dvp_dev->stream_lock);
+	if (enable) {
+		if (dvp_dev->stream_count == 0) {
+			dvp_dev->hw_ops->dvp_clk_init(dvp_dev);
+			dvp_dev->hw_ops->dvp_config_set(dvp_dev);
+			dvp_dev->hw_ops->dvp_set_format(dvp_dev,
+				format->width, dvp_dev->formats[ret].bpp);
+			dvp_dev->hw_ops->dvp_stream_set(dvp_dev, 1);
+		}
+		dvp_dev->stream_count++;
+	} else {
+		if (dvp_dev->stream_count == 0)
+			goto exit;
+		if (dvp_dev->stream_count == 1)
+			dvp_dev->hw_ops->dvp_stream_set(dvp_dev, 0);
+		dvp_dev->stream_count--;
+	}
+exit:
+	mutex_unlock(&dvp_dev->stream_lock);
+	return 0;
+}
+
+static void dvp_try_format(struct stf_dvp_dev *dvp_dev,
+			struct v4l2_subdev_state *state,
+			unsigned int pad,
+			struct v4l2_mbus_framefmt *fmt,
+			enum v4l2_subdev_format_whence which)
+{
+	unsigned int i;
+
+	switch (pad) {
+	case STF_DVP_PAD_SINK:
+		/* Set format on sink pad */
+
+		for (i = 0; i < dvp_dev->nformats; i++)
+			if (fmt->code == dvp_dev->formats[i].code)
+				break;
+
+		if (i >= dvp_dev->nformats)
+			fmt->code = MEDIA_BUS_FMT_RGB565_2X8_LE;
+
+		fmt->width = clamp_t(u32, fmt->width, 1, STFCAMSS_FRAME_MAX_WIDTH);
+		fmt->height = clamp_t(u32,
+				fmt->height, 1, STFCAMSS_FRAME_MAX_HEIGHT_PIX);
+
+		fmt->field = V4L2_FIELD_NONE;
+		fmt->colorspace = V4L2_COLORSPACE_SRGB;
+		fmt->flags = 0;
+		break;
+
+	case STF_DVP_PAD_SRC:
+
+		*fmt = *__dvp_get_format(dvp_dev, state, STF_DVP_PAD_SINK, which);
+		break;
+	}
+}
+
+static int dvp_enum_mbus_code(struct v4l2_subdev *sd,
+			struct v4l2_subdev_state *state,
+			struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct stf_dvp_dev *dvp_dev = v4l2_get_subdevdata(sd);
+
+	if (code->index >= dvp_dev->nformats)
+		return -EINVAL;
+
+	if (code->pad == STF_DVP_PAD_SINK) {
+		code->code = dvp_dev->formats[code->index].code;
+	} else {
+		struct v4l2_mbus_framefmt *sink_fmt;
+		sink_fmt = __dvp_get_format(dvp_dev, state, STF_DVP_PAD_SINK,
+					code->which);
+		code->code = sink_fmt->code;
+		if (!code->code)
+			return -EINVAL;
+	}
+	code->flags = 0;
+
+	return 0;
+}
+
+static int dvp_enum_frame_size(struct v4l2_subdev *sd,
+				struct v4l2_subdev_state *state,
+				struct v4l2_subdev_frame_size_enum *fse)
+{
+	struct stf_dvp_dev *dvp_dev = 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;
+	dvp_try_format(dvp_dev, state, 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;
+	dvp_try_format(dvp_dev, state, fse->pad, &format, fse->which);
+	fse->max_width = format.width;
+	fse->max_height = format.height;
+
+	return 0;
+}
+
+static int dvp_get_format(struct v4l2_subdev *sd,
+			struct v4l2_subdev_state *state,
+			struct v4l2_subdev_format *fmt)
+{
+	struct stf_dvp_dev *dvp_dev = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	format = __dvp_get_format(dvp_dev, state, fmt->pad, fmt->which);
+	if (format == NULL)
+		return -EINVAL;
+
+	fmt->format = *format;
+
+	return 0;
+}
+
+static int dvp_set_format(struct v4l2_subdev *sd,
+			struct v4l2_subdev_state *state,
+			struct v4l2_subdev_format *fmt)
+{
+	struct stf_dvp_dev *dvp_dev = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	format = __dvp_get_format(dvp_dev, state, fmt->pad, fmt->which);
+	if (format == NULL)
+		return -EINVAL;
+
+	dvp_try_format(dvp_dev, state, fmt->pad, &fmt->format, fmt->which);
+	*format = fmt->format;
+
+	/* Propagate the format from sink to source */
+	if (fmt->pad == STF_DVP_PAD_SINK) {
+		format = __dvp_get_format(dvp_dev, state, STF_DVP_PAD_SRC, fmt->which);
+		*format = fmt->format;
+		dvp_try_format(dvp_dev, state, STF_DVP_PAD_SRC, format, fmt->which);
+	}
+
+	return 0;
+}
+
+static int dvp_init_formats(struct v4l2_subdev *sd,
+			struct v4l2_subdev_fh *fh)
+{
+	struct v4l2_subdev_format format = {
+		.pad = STF_DVP_PAD_SINK,
+		.which = fh ? V4L2_SUBDEV_FORMAT_TRY : V4L2_SUBDEV_FORMAT_ACTIVE,
+		.format = {
+			.code = MEDIA_BUS_FMT_RGB565_2X8_LE,
+			.width = 1920,
+			.height = 1080
+		}
+	};
+
+	return dvp_set_format(sd, fh ? fh->state : NULL, &format);
+}
+
+static int dvp_link_setup(struct media_entity *entity,
+			const struct media_pad *local,
+			const struct media_pad *remote, u32 flags)
+{
+	if ((local->flags & MEDIA_PAD_FL_SOURCE) &&
+		(flags & MEDIA_LNK_FL_ENABLED)) {
+		struct v4l2_subdev *sd;
+		struct stf_dvp_dev *dvp_dev;
+		struct vin_line *line;
+
+		if (media_entity_remote_pad(local))
+			return -EBUSY;
+
+		sd = media_entity_to_v4l2_subdev(entity);
+		dvp_dev = v4l2_get_subdevdata(sd);
+
+		sd = media_entity_to_v4l2_subdev(remote->entity);
+		line = v4l2_get_subdevdata(sd);
+		if (line->sdev_type == VIN_DEV_TYPE)
+			dvp_dev->s_type = SENSOR_VIN;
+		if (line->sdev_type == ISP0_DEV_TYPE)
+			dvp_dev->s_type = SENSOR_ISP0;
+		if (line->sdev_type == ISP1_DEV_TYPE)
+			dvp_dev->s_type = SENSOR_ISP1;
+		st_info(ST_DVP, "DVP device sensor type: %d\n", dvp_dev->s_type);
+	}
+
+	return 0;
+}
+
+static const struct v4l2_subdev_core_ops dvp_core_ops = {
+	.s_power = dvp_set_power,
+};
+
+static const struct v4l2_subdev_video_ops dvp_video_ops = {
+	.s_stream = dvp_set_stream,
+};
+
+static const struct v4l2_subdev_pad_ops dvp_pad_ops = {
+	.enum_mbus_code = dvp_enum_mbus_code,
+	.enum_frame_size = dvp_enum_frame_size,
+	.get_fmt = dvp_get_format,
+	.set_fmt = dvp_set_format,
+};
+
+static const struct v4l2_subdev_ops dvp_v4l2_ops = {
+	.core = &dvp_core_ops,
+	.video = &dvp_video_ops,
+	.pad = &dvp_pad_ops,
+};
+
+static const struct v4l2_subdev_internal_ops dvp_v4l2_internal_ops = {
+	.open = dvp_init_formats,
+};
+
+static const struct media_entity_operations dvp_media_ops = {
+	.link_setup = dvp_link_setup,
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+int stf_dvp_register(struct stf_dvp_dev *dvp_dev, struct v4l2_device *v4l2_dev)
+{
+	struct v4l2_subdev *sd = &dvp_dev->subdev;
+	struct media_pad *pads = dvp_dev->pads;
+	int ret;
+
+	v4l2_subdev_init(sd, &dvp_v4l2_ops);
+	sd->internal_ops = &dvp_v4l2_internal_ops;
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	snprintf(sd->name, ARRAY_SIZE(sd->name), "%s%d",
+		STF_DVP_NAME, dvp_dev->id);
+	v4l2_set_subdevdata(sd, dvp_dev);
+
+	ret = dvp_init_formats(sd, NULL);
+	if (ret < 0) {
+		st_err(ST_DVP, "Failed to init format: %d\n", ret);
+		return ret;
+	}
+
+	pads[STF_DVP_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
+	pads[STF_DVP_PAD_SRC].flags = MEDIA_PAD_FL_SOURCE;
+
+	sd->entity.function = MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER;
+	sd->entity.ops = &dvp_media_ops;
+	ret = media_entity_pads_init(&sd->entity, STF_DVP_PADS_NUM, pads);
+	if (ret < 0) {
+		st_err(ST_DVP, "Failed to init media entity: %d\n", ret);
+		return ret;
+	}
+
+	ret = v4l2_device_register_subdev(v4l2_dev, sd);
+	if (ret < 0) {
+		st_err(ST_DVP, "Failed to register subdev: %d\n", ret);
+		goto err_sreg;
+	}
+
+	return 0;
+
+err_sreg:
+	media_entity_cleanup(&sd->entity);
+	return ret;
+}
+
+int stf_dvp_unregister(struct stf_dvp_dev *dvp_dev)
+{
+	v4l2_device_unregister_subdev(&dvp_dev->subdev);
+	media_entity_cleanup(&dvp_dev->subdev.entity);
+	mutex_destroy(&dvp_dev->stream_lock);
+
+	return 0;
+}
diff --git a/drivers/media/platform/starfive/stf_dvp.h b/drivers/media/platform/starfive/stf_dvp.h
new file mode 100755
index 000000000000..d3130a417f5a
--- /dev/null
+++ b/drivers/media/platform/starfive/stf_dvp.h
@@ -0,0 +1,61 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021 StarFive Technology Co., Ltd.
+ */
+#ifndef STF_DVP_H
+#define STF_DVP_H
+
+#include <media/v4l2-subdev.h>
+#include <media/v4l2-device.h>
+#include <media/media-entity.h>
+#include <video/stf-vin.h>
+
+#define STF_DVP_PAD_SINK     0
+#define STF_DVP_PAD_SRC      1
+#define STF_DVP_PADS_NUM     2
+
+struct dvp_format {
+	u32 code;
+	u8 bpp;
+};
+
+enum sensor_type;
+enum subdev_type;
+
+struct dvp_cfg {
+	unsigned int flags;
+	unsigned char bus_width;
+	unsigned char data_shift;
+};
+
+struct stf_dvp_dev;
+
+struct dvp_hw_ops {
+	int (*dvp_clk_init)(struct stf_dvp_dev *dvp_dev);
+	int (*dvp_config_set)(struct stf_dvp_dev *dvp_dev);
+	int (*dvp_set_format)(struct stf_dvp_dev *dvp_dev, u32 pix_width, u8 bpp);
+	int (*dvp_stream_set)(struct stf_dvp_dev *dvp_dev, int on);
+};
+
+struct stf_dvp_dev {
+	struct stfcamss *stfcamss;
+	struct dvp_cfg *dvp;
+	u8 id;
+	enum sensor_type s_type;
+	struct v4l2_subdev subdev;
+	struct media_pad pads[STF_DVP_PADS_NUM];
+	struct v4l2_mbus_framefmt fmt[STF_DVP_PADS_NUM];
+	const struct dvp_format *formats;
+	unsigned int nformats;
+	struct dvp_hw_ops *hw_ops;
+	struct mutex stream_lock;
+	int stream_count;
+};
+
+extern int stf_dvp_subdev_init(struct stfcamss *stfcamss);
+extern int stf_dvp_register(struct stf_dvp_dev *dvp_dev,
+			struct v4l2_device *v4l2_dev);
+extern int stf_dvp_unregister(struct stf_dvp_dev *dvp_dev);
+extern struct dvp_hw_ops dvp_ops;
+
+#endif /* STF_DVP_H */
diff --git a/drivers/media/platform/starfive/stf_dvp_hw_ops.c b/drivers/media/platform/starfive/stf_dvp_hw_ops.c
new file mode 100755
index 000000000000..3e7a101441b3
--- /dev/null
+++ b/drivers/media/platform/starfive/stf_dvp_hw_ops.c
@@ -0,0 +1,162 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021 StarFive Technology Co., Ltd.
+ */
+#include "stfcamss.h"
+
+static int stf_dvp_clk_init(struct stf_dvp_dev *dvp_dev)
+{
+	return 0;
+}
+
+static void stf_dvp_io_pad_config(struct stf_vin_dev *vin)
+{
+	/*
+	 * pin: 49 ~ 57
+	 * offset: 0x144 ~ 0x164
+	 * SCFG_funcshare_pad_ctrl
+	 */
+	u32 val_scfg_funcshare_config = 0x800080;
+
+	iowrite32(val_scfg_funcshare_config,
+			vin->vin_top_iopad_base + IOPAD_REG81);
+	iowrite32(val_scfg_funcshare_config,
+			vin->vin_top_iopad_base + IOPAD_REG82);
+	iowrite32(val_scfg_funcshare_config,
+			vin->vin_top_iopad_base + IOPAD_REG83);
+	iowrite32(val_scfg_funcshare_config,
+			vin->vin_top_iopad_base + IOPAD_REG84);
+	iowrite32(val_scfg_funcshare_config,
+			vin->vin_top_iopad_base + IOPAD_REG85);
+	iowrite32(val_scfg_funcshare_config,
+			vin->vin_top_iopad_base + IOPAD_REG86);
+	iowrite32(val_scfg_funcshare_config,
+			vin->vin_top_iopad_base + IOPAD_REG87);
+	iowrite32(val_scfg_funcshare_config,
+			vin->vin_top_iopad_base + IOPAD_REG88);
+	iowrite32(val_scfg_funcshare_config,
+			vin->vin_top_iopad_base + IOPAD_REG89);
+}
+
+static int stf_dvp_config_set(struct stf_dvp_dev *dvp_dev)
+{
+	struct stf_vin_dev *vin = dvp_dev->stfcamss->vin;
+	unsigned int flags = 0;
+	unsigned char data_shift = 0;
+	u32 polarities = 0;
+
+	if (!dvp_dev->dvp)
+		return -EINVAL;
+
+	flags = dvp_dev->dvp->flags;
+	data_shift = dvp_dev->dvp->data_shift;
+	st_info(ST_DVP, "%s, polarities = 0x%x, flags = 0x%x\n",
+			__func__, polarities, flags);
+
+	stf_dvp_io_pad_config(vin);
+
+	if (flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH)
+		polarities |= BIT(9);
+
+	if (flags & V4L2_MBUS_VSYNC_ACTIVE_HIGH)
+		polarities |= BIT(8);
+
+	print_reg(ST_DVP, vin->sysctrl_base, SYSCTRL_VIN_RW_CTRL);
+	reg_set_bit(vin->sysctrl_base, SYSCTRL_VIN_RW_CTRL,
+			BIT(9) | BIT(8), polarities);
+	print_reg(ST_DVP, vin->sysctrl_base, SYSCTRL_VIN_RW_CTRL);
+
+	switch (data_shift) {
+	case 0:
+		data_shift = 0;
+		break;
+	case 2:
+		data_shift = 1;
+		break;
+	case 4:
+		data_shift = 2;
+		break;
+	case 6:
+		data_shift = 3;
+		break;
+	default:
+		data_shift = 0;
+		break;
+	};
+	print_reg(ST_DVP, vin->sysctrl_base, SYSCTRL_VIN_RW_CTRL);
+	reg_set_bit(vin->sysctrl_base, SYSCTRL_VIN_RW_CTRL,
+			BIT(13) | BIT(12), data_shift << 12);
+	print_reg(ST_DVP, vin->sysctrl_base, SYSCTRL_VIN_RW_CTRL);
+
+	return 0;
+}
+
+static int set_vin_axiwr_pix_ct(struct stf_vin_dev *vin, u8 bpp)
+{
+	u32 value = 0;
+	int cnfg_axiwr_pix_ct = 64 / bpp;
+
+	// need check
+	if (cnfg_axiwr_pix_ct == 2)
+		value = 1;
+	else if (cnfg_axiwr_pix_ct == 4)
+		value = 2;
+	else if (cnfg_axiwr_pix_ct == 8)
+		value = 0;
+	else
+		return 0;
+
+	print_reg(ST_DVP, vin->sysctrl_base, SYSCTRL_VIN_RW_CTRL);
+	reg_set_bit(vin->sysctrl_base,
+			SYSCTRL_VIN_RW_CTRL, BIT(1) | BIT(0), value);
+	print_reg(ST_DVP, vin->sysctrl_base, SYSCTRL_VIN_RW_CTRL);
+
+	return cnfg_axiwr_pix_ct;
+}
+
+static int stf_dvp_set_format(struct stf_dvp_dev *dvp_dev,
+		u32 pix_width, u8 bpp)
+{
+	struct stf_vin_dev *vin = dvp_dev->stfcamss->vin;
+	int val, pix_ct;
+
+	if (dvp_dev->s_type == SENSOR_VIN) {
+		pix_ct = set_vin_axiwr_pix_ct(vin, bpp);
+		val = (pix_width / pix_ct) - 1;
+		print_reg(ST_DVP, vin->sysctrl_base, SYSCTRL_VIN_WR_PIX_TOTAL);
+		reg_write(vin->sysctrl_base, SYSCTRL_VIN_WR_PIX_TOTAL, val);
+		print_reg(ST_DVP, vin->sysctrl_base, SYSCTRL_VIN_WR_PIX_TOTAL);
+	}
+	return 0;
+}
+
+static int stf_dvp_stream_set(struct stf_dvp_dev *dvp_dev, int on)
+{
+	struct stf_vin_dev *vin = dvp_dev->stfcamss->vin;
+
+	switch (dvp_dev->s_type) {
+	case SENSOR_VIN:
+		reg_set_bit(vin->sysctrl_base, SYSCTRL_VIN_AXI_CTRL, BIT(0), on);
+		reg_set_bit(vin->clkgen_base, CLK_VIN_AXI_WR_CTRL,
+				BIT(25) | BIT(24), 2 << 24);
+		break;
+	case SENSOR_ISP0:
+		reg_set_bit(vin->sysctrl_base,
+				SYSCTRL_VIN_SRC_CHAN_SEL, BIT(8), !!on << 8);
+		break;
+	case SENSOR_ISP1:
+		reg_set_bit(vin->sysctrl_base,
+				SYSCTRL_VIN_SRC_CHAN_SEL, BIT(12), !!on << 12);
+		break;
+	default:
+		break;
+	}
+	return 0;
+}
+
+struct dvp_hw_ops dvp_ops = {
+	.dvp_clk_init          = stf_dvp_clk_init,
+	.dvp_config_set        = stf_dvp_config_set,
+	.dvp_set_format        = stf_dvp_set_format,
+	.dvp_stream_set        = stf_dvp_stream_set,
+};
diff --git a/drivers/media/platform/starfive/stf_event.c b/drivers/media/platform/starfive/stf_event.c
new file mode 100755
index 000000000000..21534e5a33ea
--- /dev/null
+++ b/drivers/media/platform/starfive/stf_event.c
@@ -0,0 +1,36 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Event for VIC Video In
+ *
+ * Copyright (C) 2021 StarFive Technology Co., Ltd.
+ */
+#include <linux/notifier.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/fs.h>
+
+static ATOMIC_NOTIFIER_HEAD(vin_notifier_list);
+
+int vin_notifier_register(struct notifier_block *nb)
+{
+	return atomic_notifier_chain_register(&vin_notifier_list, nb);
+}
+EXPORT_SYMBOL_GPL(vin_notifier_register);
+
+void vin_notifier_unregister(struct notifier_block *nb)
+{
+	atomic_notifier_chain_unregister(&vin_notifier_list, nb);
+}
+EXPORT_SYMBOL_GPL(vin_notifier_unregister);
+
+int vin_notifier_call(unsigned long e, void *v)
+{
+	return atomic_notifier_call_chain(&vin_notifier_list, e, v);
+}
+EXPORT_SYMBOL_GPL(vin_notifier_call);
+
+MODULE_AUTHOR("StarFive Technology Co., Ltd.");
+MODULE_DESCRIPTION("Starfive VIC video in notifier");
+MODULE_LICENSE("GPL");
+
diff --git a/drivers/media/platform/starfive/stf_isp.c b/drivers/media/platform/starfive/stf_isp.c
new file mode 100755
index 000000000000..9e3df92f3755
--- /dev/null
+++ b/drivers/media/platform/starfive/stf_isp.c
@@ -0,0 +1,917 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021 StarFive Technology Co., Ltd.
+ */
+#include "stfcamss.h"
+#include <media/v4l2-async.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-subdev.h>
+#include <linux/firmware.h>
+
+#define STF_ISP_NAME "stf_isp"
+
+static const struct isp_format isp_formats_st7110[] = {
+	{ MEDIA_BUS_FMT_YUYV8_2X8, 16},
+	{ MEDIA_BUS_FMT_RGB565_2X8_LE, 16},
+	{ MEDIA_BUS_FMT_SRGGB10_1X10, 12},
+	{ MEDIA_BUS_FMT_SGRBG10_1X10, 12},
+	{ MEDIA_BUS_FMT_SGBRG10_1X10, 12},
+	{ MEDIA_BUS_FMT_SBGGR10_1X10, 12},
+};
+
+int stf_isp_subdev_init(struct stfcamss *stfcamss, int id)
+{
+	struct stf_isp_dev *isp_dev = &stfcamss->isp_dev[id];
+
+	atomic_set(&isp_dev->ref_count, 0);
+	isp_dev->sdev_type = id == 0 ? ISP0_DEV_TYPE : ISP1_DEV_TYPE;
+	isp_dev->id = id;
+	isp_dev->hw_ops = &isp_ops;
+	isp_dev->stfcamss = stfcamss;
+	isp_dev->formats = isp_formats_st7110;
+	isp_dev->nformats = ARRAY_SIZE(isp_formats_st7110);
+	mutex_init(&isp_dev->stream_lock);
+	mutex_init(&isp_dev->setfile_lock);
+	return 0;
+}
+
+/*
+ * ISP Controls.
+ */
+
+static inline struct v4l2_subdev *ctrl_to_sd(struct v4l2_ctrl *ctrl)
+{
+	return &container_of(ctrl->handler, struct stf_isp_dev,
+			     ctrls.handler)->subdev;
+}
+
+static u64 isp_calc_pixel_rate(struct stf_isp_dev *isp_dev)
+{
+	u64 rate = 0;
+
+	return rate;
+}
+
+static int isp_set_ctrl_hue(struct stf_isp_dev *isp_dev, int value)
+{
+	int ret = 0;
+
+	return ret;
+}
+
+static int isp_set_ctrl_contrast(struct stf_isp_dev *isp_dev, int value)
+{
+	int ret = 0;
+
+	return ret;
+}
+
+static int isp_set_ctrl_saturation(struct stf_isp_dev *isp_dev, int value)
+{
+	int ret = 0;
+
+	return ret;
+}
+
+static int isp_set_ctrl_white_balance(struct stf_isp_dev *isp_dev, int awb)
+{
+	struct isp_ctrls *ctrls = &isp_dev->ctrls;
+	int ret = 0;
+
+	if (!awb && (ctrls->red_balance->is_new || ctrls->blue_balance->is_new)) {
+		u16 red = (u16)ctrls->red_balance->val;
+		u16 blue = (u16)ctrls->blue_balance->val;
+		st_debug(ST_ISP, "red = 0x%x, blue = 0x%x\n", red, blue);
+		// isp_dev->hw_ops->isp_set_awb_r_gain(isp_dev, red);
+		// if (ret)
+		// 	return ret;
+		// isp_dev->hw_ops->isp_set_awb_b_gain(isp_dev, blue);
+	}
+
+	return ret;
+}
+
+static int isp_set_ctrl_exposure(struct stf_isp_dev *isp_dev,
+				    enum v4l2_exposure_auto_type auto_exposure)
+{
+	int ret = 0;
+
+	return ret;
+}
+
+static int isp_set_ctrl_gain(struct stf_isp_dev *isp_dev, bool auto_gain)
+{
+	int ret = 0;
+
+	return ret;
+}
+
+static const char * const test_pattern_menu[] = {
+	"Disabled",
+	"Color bars",
+	"Color bars w/ rolling bar",
+	"Color squares",
+	"Color squares w/ rolling bar",
+};
+
+#define ISP_TEST_ENABLE			BIT(7)
+#define ISP_TEST_ROLLING		BIT(6)	/* rolling horizontal bar */
+#define ISP_TEST_TRANSPARENT		BIT(5)
+#define ISP_TEST_SQUARE_BW		BIT(4)	/* black & white squares */
+#define ISP_TEST_BAR_STANDARD		(0 << 2)
+#define ISP_TEST_BAR_VERT_CHANGE_1	(1 << 2)
+#define ISP_TEST_BAR_HOR_CHANGE		(2 << 2)
+#define ISP_TEST_BAR_VERT_CHANGE_2	(3 << 2)
+#define ISP_TEST_BAR			(0 << 0)
+#define ISP_TEST_RANDOM			(1 << 0)
+#define ISP_TEST_SQUARE			(2 << 0)
+#define ISP_TEST_BLACK			(3 << 0)
+
+static const u8 test_pattern_val[] = {
+	0,
+	ISP_TEST_ENABLE | ISP_TEST_BAR_VERT_CHANGE_1 |
+		ISP_TEST_BAR,
+	ISP_TEST_ENABLE | ISP_TEST_ROLLING |
+		ISP_TEST_BAR_VERT_CHANGE_1 | ISP_TEST_BAR,
+	ISP_TEST_ENABLE | ISP_TEST_SQUARE,
+	ISP_TEST_ENABLE | ISP_TEST_ROLLING | ISP_TEST_SQUARE,
+};
+
+static int isp_set_ctrl_test_pattern(struct stf_isp_dev *isp_dev, int value)
+{
+	int ret = 0;
+
+	// return isp_write_reg(isp_dev, ISP_REG_PRE_ISP_TEST_SET1,
+	//			test_pattern_val[value]);
+	return ret;
+}
+
+static int isp_set_ctrl_light_freq(struct stf_isp_dev *isp_dev, int value)
+{
+	int ret = 0;
+
+	return ret;
+}
+
+static int isp_set_ctrl_hflip(struct stf_isp_dev *isp_dev, int value)
+{
+	int ret = 0;
+
+	return ret;
+}
+
+static int isp_set_ctrl_vflip(struct stf_isp_dev *isp_dev, int value)
+{
+	int ret = 0;
+
+	return ret;
+}
+
+static int isp_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUTOGAIN:
+		break;
+	case V4L2_CID_EXPOSURE_AUTO:
+		break;
+	}
+
+	return 0;
+}
+
+static int isp_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
+	struct stf_isp_dev *isp_dev = v4l2_get_subdevdata(sd);
+	int ret;
+
+	/*
+	 * If the device is not powered up by the host driver do
+	 * not apply any controls to H/W at this time. Instead
+	 * the controls will be restored right after power-up.
+	 */
+	if (!atomic_read(&isp_dev->ref_count))
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUTOGAIN:
+		ret = isp_set_ctrl_gain(isp_dev, ctrl->val);
+		break;
+	case V4L2_CID_EXPOSURE_AUTO:
+		ret = isp_set_ctrl_exposure(isp_dev, ctrl->val);
+		break;
+	case V4L2_CID_AUTO_WHITE_BALANCE:
+		ret = isp_set_ctrl_white_balance(isp_dev, ctrl->val);
+		break;
+	case V4L2_CID_HUE:
+		ret = isp_set_ctrl_hue(isp_dev, ctrl->val);
+		break;
+	case V4L2_CID_CONTRAST:
+		ret = isp_set_ctrl_contrast(isp_dev, ctrl->val);
+		break;
+	case V4L2_CID_SATURATION:
+		ret = isp_set_ctrl_saturation(isp_dev, ctrl->val);
+		break;
+	case V4L2_CID_TEST_PATTERN:
+		ret = isp_set_ctrl_test_pattern(isp_dev, ctrl->val);
+		break;
+	case V4L2_CID_POWER_LINE_FREQUENCY:
+		ret = isp_set_ctrl_light_freq(isp_dev, ctrl->val);
+		break;
+	case V4L2_CID_HFLIP:
+		ret = isp_set_ctrl_hflip(isp_dev, ctrl->val);
+		break;
+	case V4L2_CID_VFLIP:
+		ret = isp_set_ctrl_vflip(isp_dev, ctrl->val);
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops isp_ctrl_ops = {
+	.g_volatile_ctrl = isp_g_volatile_ctrl,
+	.s_ctrl = isp_s_ctrl,
+};
+
+static int isp_init_controls(struct stf_isp_dev *isp_dev)
+{
+	const struct v4l2_ctrl_ops *ops = &isp_ctrl_ops;
+	struct isp_ctrls *ctrls = &isp_dev->ctrls;
+	struct v4l2_ctrl_handler *hdl = &ctrls->handler;
+	int ret;
+
+	v4l2_ctrl_handler_init(hdl, 32);
+
+	/* Clock related controls */
+	ctrls->pixel_rate = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_PIXEL_RATE,
+						0, INT_MAX, 1, isp_calc_pixel_rate(isp_dev));
+
+	/* Auto/manual white balance */
+	ctrls->auto_wb = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_AUTO_WHITE_BALANCE,
+					0, 1, 1, 1);
+	ctrls->blue_balance = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_BLUE_BALANCE,
+						0, 4095, 1, 0);
+	ctrls->red_balance = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_RED_BALANCE,
+						0, 4095, 1, 0);
+	/* Auto/manual exposure */
+	ctrls->auto_exp = v4l2_ctrl_new_std_menu(hdl, ops, V4L2_CID_EXPOSURE_AUTO,
+						V4L2_EXPOSURE_MANUAL, 0, V4L2_EXPOSURE_AUTO);
+	ctrls->exposure = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_EXPOSURE,
+						0, 65535, 1, 0);
+	/* Auto/manual gain */
+	ctrls->auto_gain = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_AUTOGAIN,
+						0, 1, 1, 1);
+	ctrls->gain = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_GAIN, 0, 1023, 1, 0);
+
+	ctrls->saturation = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_SATURATION,
+						0, 255, 1, 64);
+	ctrls->hue = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HUE, 0, 359, 1, 0);
+	ctrls->contrast = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_CONTRAST,
+						0, 255, 1, 0);
+	ctrls->test_pattern = v4l2_ctrl_new_std_menu_items(hdl, ops,
+						V4L2_CID_TEST_PATTERN,
+						ARRAY_SIZE(test_pattern_menu) - 1,
+						0, 0, test_pattern_menu);
+	ctrls->hflip = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HFLIP, 0, 1, 1, 0);
+	ctrls->vflip = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VFLIP, 0, 1, 1, 0);
+
+	ctrls->light_freq = v4l2_ctrl_new_std_menu(hdl, ops,
+						V4L2_CID_POWER_LINE_FREQUENCY,
+						V4L2_CID_POWER_LINE_FREQUENCY_AUTO, 0,
+						V4L2_CID_POWER_LINE_FREQUENCY_50HZ);
+
+	if (hdl->error) {
+		ret = hdl->error;
+		goto free_ctrls;
+	}
+
+	ctrls->pixel_rate->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+	ctrls->gain->flags |= V4L2_CTRL_FLAG_VOLATILE;
+	ctrls->exposure->flags |= V4L2_CTRL_FLAG_VOLATILE;
+
+	v4l2_ctrl_auto_cluster(3, &ctrls->auto_wb, 0, false);
+	v4l2_ctrl_auto_cluster(2, &ctrls->auto_gain, 0, true);
+	v4l2_ctrl_auto_cluster(2, &ctrls->auto_exp, 1, true);
+
+	isp_dev->subdev.ctrl_handler = hdl;
+	return 0;
+
+free_ctrls:
+	v4l2_ctrl_handler_free(hdl);
+	return ret;
+}
+
+static int isp_set_power(struct v4l2_subdev *sd, int on)
+{
+	struct stf_isp_dev *isp_dev = v4l2_get_subdevdata(sd);
+
+	if (on)
+		atomic_inc(&isp_dev->ref_count);
+	else
+		atomic_dec(&isp_dev->ref_count);
+
+	return 0;
+}
+
+static struct v4l2_mbus_framefmt * __isp_get_format(struct stf_isp_dev *isp_dev,
+		struct v4l2_subdev_state *state,
+		unsigned int pad,
+		enum v4l2_subdev_format_whence which)
+{
+	if (which == V4L2_SUBDEV_FORMAT_TRY)
+		return v4l2_subdev_get_try_format(&isp_dev->subdev, state, pad);
+
+	return &isp_dev->fmt[pad];
+}
+
+static int isp_set_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct stf_isp_dev *isp_dev = v4l2_get_subdevdata(sd);
+	int ret = 0;
+	struct v4l2_mbus_framefmt *fmt;
+
+	fmt = __isp_get_format(isp_dev, NULL, STF_ISP_PAD_SINK,
+			V4L2_SUBDEV_FORMAT_ACTIVE);
+	mutex_lock(&isp_dev->stream_lock);
+	if (enable) {
+		if (isp_dev->stream_count == 0) {
+			isp_dev->hw_ops->isp_clk_enable(isp_dev);
+			isp_dev->hw_ops->isp_reset(isp_dev);
+			isp_dev->hw_ops->isp_set_format(isp_dev,
+					&isp_dev->crop, fmt->code);
+				// format->width, format->height);
+			isp_dev->hw_ops->isp_config_set(isp_dev);
+			isp_dev->hw_ops->isp_stream_set(isp_dev, enable);
+		}
+		isp_dev->stream_count++;
+	} else {
+		if (isp_dev->stream_count == 0)
+			goto exit;
+		if (isp_dev->stream_count == 1) {
+			isp_dev->hw_ops->isp_stream_set(isp_dev, enable);
+			isp_dev->hw_ops->isp_clk_disable(isp_dev);
+		}
+		isp_dev->stream_count--;
+	}
+exit:
+	mutex_unlock(&isp_dev->stream_lock);
+
+	if (enable && atomic_read(&isp_dev->ref_count) == 1) {
+		/* restore controls */
+		ret = v4l2_ctrl_handler_setup(&isp_dev->ctrls.handler);
+	}
+
+	return ret;
+}
+
+static void isp_try_format(struct stf_isp_dev *isp_dev,
+			struct v4l2_subdev_state *state,
+			unsigned int pad,
+			struct v4l2_mbus_framefmt *fmt,
+			enum v4l2_subdev_format_whence which)
+{
+	unsigned int i;
+
+	switch (pad) {
+	case STF_ISP_PAD_SINK:
+		/* Set format on sink pad */
+
+		for (i = 0; i < isp_dev->nformats; i++)
+			if (fmt->code == isp_dev->formats[i].code)
+				break;
+
+		if (i >= isp_dev->nformats)
+			fmt->code = MEDIA_BUS_FMT_RGB565_2X8_LE;
+
+		fmt->width = clamp_t(u32, fmt->width, 8, STFCAMSS_FRAME_MAX_WIDTH);
+		fmt->width &= ~0x7 ;
+		fmt->height = clamp_t(u32, fmt->height, 1,
+					STFCAMSS_FRAME_MAX_HEIGHT_PIX);
+
+		fmt->field = V4L2_FIELD_NONE;
+		fmt->colorspace = V4L2_COLORSPACE_SRGB;
+		fmt->flags = 0;
+
+		break;
+
+	case STF_ISP_PAD_SRC:
+
+		*fmt = *__isp_get_format(isp_dev, state, STF_ISP_PAD_SINK, which);
+
+		break;
+	}
+}
+
+static int isp_enum_mbus_code(struct v4l2_subdev *sd,
+			struct v4l2_subdev_state *state,
+			struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct stf_isp_dev *isp_dev = v4l2_get_subdevdata(sd);
+
+	if (code->index >= isp_dev->nformats)
+		return -EINVAL;
+	if (code->pad == STF_ISP_PAD_SINK) {
+		code->code = isp_dev->formats[code->index].code;
+	} else {
+		struct v4l2_mbus_framefmt *sink_fmt;
+		sink_fmt = __isp_get_format(isp_dev, state, STF_ISP_PAD_SINK,
+					code->which);
+
+		code->code = sink_fmt->code;
+		if (!code->code)
+			return -EINVAL;
+	}
+	code->flags = 0;
+
+	return 0;
+}
+
+static int isp_enum_frame_size(struct v4l2_subdev *sd,
+				struct v4l2_subdev_state *state,
+				struct v4l2_subdev_frame_size_enum *fse)
+{
+	struct stf_isp_dev *isp_dev = 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;
+	isp_try_format(isp_dev, state, 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;
+	isp_try_format(isp_dev, state, fse->pad, &format, fse->which);
+	fse->max_width = format.width;
+	fse->max_height = format.height;
+
+	return 0;
+}
+
+static int isp_get_format(struct v4l2_subdev *sd,
+			struct v4l2_subdev_state *state,
+			struct v4l2_subdev_format *fmt)
+{
+	struct stf_isp_dev *isp_dev = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	format = __isp_get_format(isp_dev, state, fmt->pad, fmt->which);
+	if (format == NULL)
+		return -EINVAL;
+
+	fmt->format = *format;
+
+	return 0;
+}
+
+static int isp_set_selection(struct v4l2_subdev *sd,
+			struct v4l2_subdev_state *state,
+			struct v4l2_subdev_selection *sel);
+
+static int isp_set_format(struct v4l2_subdev *sd,
+			struct v4l2_subdev_state *state,
+			struct v4l2_subdev_format *fmt)
+{
+	struct stf_isp_dev *isp_dev = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	format = __isp_get_format(isp_dev, state, fmt->pad, fmt->which);
+	if (format == NULL)
+		return -EINVAL;
+
+	isp_try_format(isp_dev, state, fmt->pad, &fmt->format, fmt->which);
+	*format = fmt->format;
+
+	/* Propagate the format from sink to source */
+	if (fmt->pad == STF_ISP_PAD_SINK) {
+		struct v4l2_subdev_selection sel = { 0 };
+		int ret;
+
+		format = __isp_get_format(isp_dev, state, STF_ISP_PAD_SRC, fmt->which);
+		*format = fmt->format;
+		isp_try_format(isp_dev, state, STF_ISP_PAD_SRC, format, fmt->which);
+
+		/* Reset sink pad compose selection */
+		sel.which = fmt->which;
+		sel.pad = STF_ISP_PAD_SINK;
+		sel.target = V4L2_SEL_TGT_COMPOSE;
+		sel.r.width = fmt->format.width;
+		sel.r.height = fmt->format.height;
+		ret = isp_set_selection(sd, state, &sel);
+		if (ret < 0)
+			return ret;
+
+	}
+
+	return 0;
+}
+
+static struct v4l2_rect * __isp_get_compose(struct stf_isp_dev *isp_dev,
+			struct v4l2_subdev_state *state,
+			enum v4l2_subdev_format_whence which)
+{
+	if (which == V4L2_SUBDEV_FORMAT_TRY)
+		return v4l2_subdev_get_try_compose(&isp_dev->subdev,
+				state, STF_ISP_PAD_SINK);
+
+	return &isp_dev->compose;
+}
+
+static struct v4l2_rect *__isp_get_crop(struct stf_isp_dev *isp_dev,
+			struct v4l2_subdev_state *state,
+			enum v4l2_subdev_format_whence which)
+{
+	if (which == V4L2_SUBDEV_FORMAT_TRY)
+		return v4l2_subdev_get_try_crop(&isp_dev->subdev, state, STF_ISP_PAD_SRC);
+
+	return &isp_dev->crop;
+}
+
+static void isp_try_compose(struct stf_isp_dev *isp_dev,
+			    struct v4l2_subdev_state *state,
+			    struct v4l2_rect *rect,
+			    enum v4l2_subdev_format_whence which)
+{
+	struct v4l2_mbus_framefmt *fmt;
+
+	fmt = __isp_get_format(isp_dev, state, STF_ISP_PAD_SINK, which);
+
+	if (rect->width > fmt->width)
+		rect->width = fmt->width;
+
+	if (rect->height > fmt->height)
+		rect->height = fmt->height;
+
+	if (fmt->width > rect->width * SCALER_RATIO_MAX)
+		rect->width = (fmt->width + SCALER_RATIO_MAX - 1) /
+							SCALER_RATIO_MAX;
+
+	rect->width &= ~0x7;
+
+	if (fmt->height > rect->height * SCALER_RATIO_MAX)
+		rect->height = (fmt->height + SCALER_RATIO_MAX - 1) /
+							SCALER_RATIO_MAX;
+
+	if (rect->width < 16)
+		rect->width = 16;
+
+	if (rect->height < 4)
+		rect->height = 4;
+}
+
+static void isp_try_crop(struct stf_isp_dev *isp_dev,
+			struct v4l2_subdev_state *state,
+			struct v4l2_rect *rect,
+			enum v4l2_subdev_format_whence which)
+{
+	struct v4l2_rect *compose;
+
+	compose = __isp_get_compose(isp_dev, state, which);
+
+	if (rect->width > compose->width)
+		rect->width = compose->width;
+
+	if (rect->width + rect->left > compose->width)
+		rect->left = compose->width - rect->width;
+
+	if (rect->height > compose->height)
+		rect->height = compose->height;
+
+	if (rect->height + rect->top > compose->height)
+		rect->top = compose->height - rect->height;
+
+	// /* isp in line based mode writes multiple of 16 horizontally */
+	rect->left &= ~0x1;
+	rect->top &= ~0x1;
+	rect->width &= ~0x7;
+
+	if (rect->width < 16) {
+		rect->left = 0;
+		rect->width = 16;
+	}
+
+	if (rect->height < 4) {
+		rect->top = 0;
+		rect->height = 4;
+	}
+}
+
+static int isp_get_selection(struct v4l2_subdev *sd,
+			struct v4l2_subdev_state *state,
+			struct v4l2_subdev_selection *sel)
+{
+	struct stf_isp_dev *isp_dev = v4l2_get_subdevdata(sd);
+	struct v4l2_subdev_format fmt = { 0 };
+	struct v4l2_rect *rect;
+	int ret;
+
+	switch (sel->target) {
+	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
+	case V4L2_SEL_TGT_COMPOSE_DEFAULT:
+		fmt.pad = sel->pad;
+		fmt.which = sel->which;
+		ret = isp_get_format(sd, state, &fmt);
+		if (ret < 0)
+			return ret;
+
+		sel->r.left = 0;
+		sel->r.top = 0;
+		sel->r.width = fmt.format.width;
+		sel->r.height = fmt.format.height;
+		break;
+	case V4L2_SEL_TGT_COMPOSE:
+		rect = __isp_get_compose(isp_dev, state, sel->which);
+		if (rect == NULL)
+			return -EINVAL;
+
+		sel->r = *rect;
+		break;
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+		rect = __isp_get_compose(isp_dev, state, sel->which);
+		if (rect == NULL)
+			return -EINVAL;
+
+		sel->r.left = rect->left;
+		sel->r.top = rect->top;
+		sel->r.width = rect->width;
+		sel->r.height = rect->height;
+		break;
+	case V4L2_SEL_TGT_CROP:
+		rect = __isp_get_crop(isp_dev, state, sel->which);
+		if (rect == NULL)
+			return -EINVAL;
+
+		sel->r = *rect;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	st_info(ST_ISP, "get left = %d, %d, %d, %d\n",
+			sel->r.left, sel->r.top, sel->r.width, sel->r.height);
+
+	return 0;
+}
+
+static int isp_set_selection(struct v4l2_subdev *sd,
+			     struct v4l2_subdev_state *state,
+			     struct v4l2_subdev_selection *sel)
+{
+	struct stf_isp_dev *isp_dev = v4l2_get_subdevdata(sd);
+	struct v4l2_rect *rect;
+	int ret;
+
+	st_info(ST_ISP, "left = %d, %d, %d, %d\n",
+			sel->r.left, sel->r.top, sel->r.width, sel->r.height);
+	if (sel->target == V4L2_SEL_TGT_COMPOSE) {
+		struct v4l2_subdev_selection crop = { 0 };
+
+		rect = __isp_get_compose(isp_dev, state, sel->which);
+		if (rect == NULL)
+			return -EINVAL;
+
+		isp_try_compose(isp_dev, state, &sel->r, sel->which);
+		*rect = sel->r;
+
+		/* Reset source crop selection */
+		crop.which = sel->which;
+		crop.pad = STF_ISP_PAD_SRC;
+		crop.target = V4L2_SEL_TGT_CROP;
+		crop.r = *rect;
+		ret = isp_set_selection(sd, state, &crop);
+	} else if (sel->target == V4L2_SEL_TGT_CROP) {
+		struct v4l2_subdev_format fmt = { 0 };
+
+		rect = __isp_get_crop(isp_dev, state, sel->which);
+		if (rect == NULL)
+			return -EINVAL;
+
+		isp_try_crop(isp_dev, state, &sel->r, sel->which);
+
+		*rect = sel->r;
+
+		/* Reset source pad format width and height */
+		fmt.which = sel->which;
+		fmt.pad = STF_ISP_PAD_SRC;
+		ret = isp_get_format(sd, state, &fmt);
+		if (ret < 0)
+			return ret;
+
+		fmt.format.width = rect->width;
+		fmt.format.height = rect->height;
+		ret = isp_set_format(sd, state, &fmt);
+	} else {
+		ret = -EINVAL;
+	}
+
+	st_info(ST_ISP, "out left = %d, %d, %d, %d\n",
+			sel->r.left, sel->r.top, sel->r.width, sel->r.height);
+
+	return ret;
+}
+
+static int isp_init_formats(struct v4l2_subdev *sd,
+			struct v4l2_subdev_fh *fh)
+{
+	struct v4l2_subdev_format format = {
+		.pad = STF_ISP_PAD_SINK,
+		.which = fh ? V4L2_SUBDEV_FORMAT_TRY :
+				V4L2_SUBDEV_FORMAT_ACTIVE,
+		.format = {
+			.code = MEDIA_BUS_FMT_RGB565_2X8_LE,
+			.width = 1920,
+			.height = 1080
+		}
+	};
+
+	return isp_set_format(sd, fh ? fh->state : NULL, &format);
+}
+
+static int isp_link_setup(struct media_entity *entity,
+			const struct media_pad *local,
+			const struct media_pad *remote, u32 flags)
+{
+	if (flags & MEDIA_LNK_FL_ENABLED)
+		if (media_entity_remote_pad(local))
+			return -EBUSY;
+	return 0;
+}
+
+static int stf_isp_load_setfile(struct stf_isp_dev *isp_dev, char *file_name)
+{
+	struct device *dev = isp_dev->stfcamss->dev;
+	const struct firmware *fw;
+	u8 *buf = NULL;
+	int *regval_num;
+	int ret;
+
+	st_debug(ST_ISP, "%s, file_name %s\n", __func__, file_name);
+	ret = request_firmware(&fw, file_name, dev);
+	if (ret < 0) {
+		st_err(ST_ISP, "firmware request failed (%d)\n", ret);
+		return ret;
+	}
+	buf = devm_kzalloc(dev, fw->size, GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
+	memcpy(buf, fw->data, fw->size);
+
+	mutex_lock(&isp_dev->setfile_lock);
+	if (isp_dev->setfile.state == 1)
+		devm_kfree(dev, isp_dev->setfile.data);
+	isp_dev->setfile.data = buf;
+	isp_dev->setfile.size = fw->size;
+	isp_dev->setfile.state = 1;
+	regval_num = (int *)&buf[fw->size - sizeof(unsigned int)];
+	isp_dev->setfile.settings.regval_num = *regval_num;
+	isp_dev->setfile.settings.regval = (regval_t *)buf;
+	mutex_unlock(&isp_dev->setfile_lock);
+
+	st_debug(ST_ISP, "stf_isp setfile loaded size: %zu B, reg_nul: %d\n",
+			fw->size, isp_dev->setfile.settings.regval_num);
+
+	release_firmware(fw);
+	return ret;
+}
+
+static long stf_isp_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void *arg)
+{
+	struct stf_isp_dev *isp_dev = v4l2_get_subdevdata(sd);
+	int ret = -ENOIOCTLCMD;
+
+	switch (cmd) {
+	case VIDIOC_STFISP_LOAD_FW: {
+		struct stfisp_fw_info *fw_info = arg;
+
+		if (IS_ERR(fw_info)) {
+			st_err(ST_ISP, "fw_info failed, params invaild\n");
+			return -EINVAL;
+		}
+
+		ret = stf_isp_load_setfile(isp_dev, fw_info->filename);
+		break;
+	}
+	default:
+		break;
+	}
+
+	return ret;
+}
+
+static const struct v4l2_subdev_core_ops isp_core_ops = {
+	.s_power = isp_set_power,
+	.ioctl = stf_isp_ioctl,
+	.log_status = v4l2_ctrl_subdev_log_status,
+	.subscribe_event = v4l2_ctrl_subdev_subscribe_event,
+	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
+};
+
+static const struct v4l2_subdev_video_ops isp_video_ops = {
+	.s_stream = isp_set_stream,
+};
+
+static const struct v4l2_subdev_pad_ops isp_pad_ops = {
+	.enum_mbus_code = isp_enum_mbus_code,
+	.enum_frame_size = isp_enum_frame_size,
+	.get_fmt = isp_get_format,
+	.set_fmt = isp_set_format,
+	.get_selection = isp_get_selection,
+	.set_selection = isp_set_selection,
+};
+
+static const struct v4l2_subdev_ops isp_v4l2_ops = {
+	.core = &isp_core_ops,
+	.video = &isp_video_ops,
+	.pad = &isp_pad_ops,
+};
+
+static const struct v4l2_subdev_internal_ops isp_v4l2_internal_ops = {
+	.open = isp_init_formats,
+};
+
+static const struct media_entity_operations isp_media_ops = {
+	.link_setup = isp_link_setup,
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+int stf_isp_register(struct stf_isp_dev *isp_dev,
+		struct v4l2_device *v4l2_dev)
+{
+	struct v4l2_subdev *sd = &isp_dev->subdev;
+	struct media_pad *pads = isp_dev->pads;
+	int ret;
+
+	v4l2_subdev_init(sd, &isp_v4l2_ops);
+	sd->internal_ops = &isp_v4l2_internal_ops;
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE | V4L2_SUBDEV_FL_HAS_EVENTS;
+	snprintf(sd->name, ARRAY_SIZE(sd->name), "%s%d",
+		STF_ISP_NAME, isp_dev->id);
+	v4l2_set_subdevdata(sd, isp_dev);
+
+	ret = isp_init_formats(sd, NULL);
+	if (ret < 0) {
+		st_err(ST_ISP, "Failed to init format: %d\n", ret);
+		return ret;
+	}
+
+	pads[STF_ISP_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
+	pads[STF_ISP_PAD_SRC].flags = MEDIA_PAD_FL_SOURCE;
+
+	sd->entity.function = MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER;
+	sd->entity.ops = &isp_media_ops;
+	ret = media_entity_pads_init(&sd->entity, STF_ISP_PADS_NUM, pads);
+	if (ret < 0) {
+		st_err(ST_ISP, "Failed to init media entity: %d\n", ret);
+		return ret;
+	}
+
+	ret = isp_init_controls(isp_dev);
+	if (ret)
+		goto err_sreg;
+
+	ret = v4l2_device_register_subdev(v4l2_dev, sd);
+	if (ret < 0) {
+		st_err(ST_ISP, "Failed to register subdev: %d\n", ret);
+		goto free_ctrls;
+	}
+
+	if (isp_dev->id == 0)
+		stf_isp_load_setfile(isp_dev, STF_ISP0_SETFILE);
+	else
+		stf_isp_load_setfile(isp_dev, STF_ISP1_SETFILE);
+
+	return 0;
+
+free_ctrls:
+	v4l2_ctrl_handler_free(&isp_dev->ctrls.handler);
+err_sreg:
+	media_entity_cleanup(&sd->entity);
+	return ret;
+}
+
+int stf_isp_unregister(struct stf_isp_dev *isp_dev)
+{
+	v4l2_device_unregister_subdev(&isp_dev->subdev);
+	media_entity_cleanup(&isp_dev->subdev.entity);
+	v4l2_ctrl_handler_free(&isp_dev->ctrls.handler);
+	mutex_destroy(&isp_dev->stream_lock);
+	mutex_destroy(&isp_dev->setfile_lock);
+	return 0;
+}
diff --git a/drivers/media/platform/starfive/stf_isp.h b/drivers/media/platform/starfive/stf_isp.h
new file mode 100644
index 000000000000..64ee98e63997
--- /dev/null
+++ b/drivers/media/platform/starfive/stf_isp.h
@@ -0,0 +1,130 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021 StarFive Technology Co., Ltd.
+ */
+#ifndef STF_ISP_H
+#define STF_ISP_H
+
+#include <media/v4l2-subdev.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/media-entity.h>
+#include <video/stf-vin.h>
+
+#define STF_ISP_PAD_SINK     0
+#define STF_ISP_PAD_SRC      1
+#define STF_ISP_PADS_NUM     2
+
+#define STF_ISP0_SETFILE     "stf_isp0_fw.bin"
+#define STF_ISP1_SETFILE     "stf_isp1_fw.bin"
+#define FILENAME_MAX_LEN     30
+
+#define SCALER_RATIO_MAX     1  // no compose function
+#define STF_ISP_REG_OFFSET_MAX  0x0FFF
+#define STF_ISP_REG_DELAY_MAX   100
+
+#define ISP_REG_CSIINTS_ADDR    0x00000008
+#define ISP_REG_DUMP_CFG_0      0x00000024
+#define ISP_REG_DUMP_CFG_1      0x00000028
+#define ISP_REG_IESHD_ADDR      0x00000A50
+
+struct stfisp_fw_info {
+	char __user filename[FILENAME_MAX_LEN];
+};
+
+#define VIDIOC_STFISP_LOAD_FW \
+        _IOW('V', BASE_VIDIOC_PRIVATE + 1, struct stfisp_fw_info)
+
+struct isp_format {
+	u32 code;
+	u8 bpp;
+};
+
+typedef struct {
+	u32 addr;
+	u32 val;
+	u32 mask;
+	u32 delay_ms;
+} regval_t;
+
+struct reg_table {
+	const regval_t *regval;
+	int regval_num;
+};
+
+struct stf_isp_dev;
+enum subdev_type;
+
+struct isp_hw_ops {
+	int (*isp_clk_enable)(struct stf_isp_dev *isp_dev);
+	int (*isp_clk_disable)(struct stf_isp_dev *isp_dev);
+	int (*isp_reset)(struct stf_isp_dev *isp_dev);
+	int (*isp_config_set)(struct stf_isp_dev *isp_dev);
+	int (*isp_set_format)(struct stf_isp_dev *isp_dev,
+			struct v4l2_rect *crop, u32 mcode);
+			// u32 width, u32 height);
+	int (*isp_stream_set)(struct stf_isp_dev *isp_dev, int on);
+};
+
+struct isp_ctrls {
+	struct v4l2_ctrl_handler handler;
+	struct v4l2_ctrl *pixel_rate;
+	struct {
+		struct v4l2_ctrl *auto_exp;
+		struct v4l2_ctrl *exposure;
+	};
+	struct {
+		struct v4l2_ctrl *auto_wb;
+		struct v4l2_ctrl *blue_balance;
+		struct v4l2_ctrl *red_balance;
+	};
+	struct {
+		struct v4l2_ctrl *auto_gain;
+		struct v4l2_ctrl *gain;
+	};
+	struct v4l2_ctrl *brightness;
+	struct v4l2_ctrl *light_freq;
+	struct v4l2_ctrl *saturation;
+	struct v4l2_ctrl *contrast;
+	struct v4l2_ctrl *hue;
+	struct v4l2_ctrl *test_pattern;
+	struct v4l2_ctrl *hflip;
+	struct v4l2_ctrl *vflip;
+};
+
+struct isp_setfile {
+	struct reg_table settings;
+	const u8 *data;
+	unsigned int size;
+	unsigned int state;
+};
+
+struct stf_isp_dev {
+	enum subdev_type sdev_type;  // must be frist
+	struct stfcamss *stfcamss;
+	atomic_t ref_count;
+	u8 id;
+	struct v4l2_subdev subdev;
+	struct media_pad pads[STF_ISP_PADS_NUM];
+	struct v4l2_mbus_framefmt fmt[STF_ISP_PADS_NUM];
+	struct v4l2_rect compose;
+	struct v4l2_rect crop;
+	const struct isp_format *formats;
+	unsigned int nformats;
+	struct isp_hw_ops *hw_ops;
+	struct mutex stream_lock;
+	int stream_count;
+
+	struct isp_ctrls ctrls;
+	struct mutex setfile_lock;
+	struct isp_setfile setfile;
+};
+
+extern int stf_isp_subdev_init(struct stfcamss *stfcamss, int id);
+extern int stf_isp_register(struct stf_isp_dev *isp_dev,
+		struct v4l2_device *v4l2_dev);
+extern int stf_isp_unregister(struct stf_isp_dev *isp_dev);
+extern struct isp_hw_ops isp_ops;
+extern void dump_isp_reg(void *__iomem ispbase, int id);
+
+#endif /* STF_ISP_H */
diff --git a/drivers/media/platform/starfive/stf_isp_hw_ops.c b/drivers/media/platform/starfive/stf_isp_hw_ops.c
new file mode 100755
index 000000000000..d019fd9af608
--- /dev/null
+++ b/drivers/media/platform/starfive/stf_isp_hw_ops.c
@@ -0,0 +1,391 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021 StarFive Technology Co., Ltd.
+ *
+ * linux/drivers/media/platform/starfive/stf_isp.c
+ *
+ * PURPOSE:	This files contains the driver of VPP.
+ */
+#include "stfcamss.h"
+#include <asm/io.h>
+#include <linux/fb.h>
+#include <linux/module.h>
+#include <video/stf-vin.h>
+#include <linux/delay.h>
+
+static const regval_t isp_sc2235_reg_config_list[] = {
+	{0x00000014, 0x00000008, 0, 0},
+	// {0x00000018, 0x000011BB, 0, 0},
+	{0x00000A1C, 0x00000030, 0, 0},
+	// {0x0000001C, 0x00000000, 0, 0},
+	// {0x00000020, 0x0437077F, 0, 0},
+	// {0x00000A0C, 0x04380780, 0, 0},
+	// {0x00000A80, 0xF9000000, 0, 0},
+	// {0x00000A84, 0xF91FA400, 0, 0},
+	// {0x00000A88, 0x00000780, 0, 0},
+	{0x00000A8C, 0x00000010, 0, 0},
+	{0x00000A90, 0x00000000, 0, 0},
+	{0x00000AC4, 0x00000000, 0, 0},
+	{0x00000E40, 0x0000004D, 0, 0},
+	{0x00000E44, 0x00000096, 0, 0},
+	{0x00000E48, 0x0000001D, 0, 0},
+	{0x00000E4C, 0x000001DA, 0, 0},
+	{0x00000E50, 0x000001B6, 0, 0},
+	{0x00000E54, 0x00000070, 0, 0},
+	{0x00000E58, 0x0000009D, 0, 0},
+	{0x00000E5C, 0x0000017C, 0, 0},
+	{0x00000E60, 0x000001E6, 0, 0},
+	{0x00000010, 0x00000000, 0, 0},
+	{0x00000A08, 0x10000022, 0xFFFFFFF, 0},
+	{0x00000044, 0x00000000, 0, 0},
+	{0x00000008, 0x00010005, 0, 0},
+	{0x00000A00, 0x00120002, 0, 0},
+	{0x00000A00, 0x00120000, 0, 0},
+	{0x00000A00, 0x00120001, 0, 0},
+	{0x00000008, 0x00010004, 0, 0},
+	{0x00000000, 0x00000001, 0, 0},
+};
+
+static const regval_t isp_1080p_reg_config_list[] = {
+	{0x00000014, 0x0000000D, 0, 0},
+	// {0x00000018, 0x000011BB, 0, 0},
+	{0x00000A1C, 0x00000032, 0, 0},
+	// {0x0000001C, 0x00000000, 0, 0},
+	// {0x00000020, 0x0437077F, 0, 0},
+	// {0x00000A0C, 0x04380780, 0, 0},
+	// {0x00000A80, 0xF9000000, 0, 0},
+	// {0x00000A84, 0xF91FA400, 0, 0},
+	// {0x00000A88, 0x00000780, 0, 0},
+	{0x00000A8C, 0x00000000, 0, 0},
+	{0x00000A90, 0x00000000, 0, 0},
+	{0x00000E40, 0x0000004C, 0, 0},
+	{0x00000E44, 0x00000097, 0, 0},
+	{0x00000E48, 0x0000001D, 0, 0},
+	{0x00000E4C, 0x000001D5, 0, 0},
+	{0x00000E50, 0x000001AC, 0, 0},
+	{0x00000E54, 0x00000080, 0, 0},
+	{0x00000E58, 0x00000080, 0, 0},
+	{0x00000E5C, 0x00000194, 0, 0},
+	{0x00000E60, 0x000001EC, 0, 0},
+	{0x00000280, 0x00000000, 0, 0},
+	{0x00000284, 0x00000000, 0, 0},
+	{0x00000288, 0x00000000, 0, 0},
+	{0x0000028C, 0x00000000, 0, 0},
+	{0x00000290, 0x00000000, 0, 0},
+	{0x00000294, 0x00000000, 0, 0},
+	{0x00000298, 0x00000000, 0, 0},
+	{0x0000029C, 0x00000000, 0, 0},
+	{0x000002A0, 0x00000000, 0, 0},
+	{0x000002A4, 0x00000000, 0, 0},
+	{0x000002A8, 0x00000000, 0, 0},
+	{0x000002AC, 0x00000000, 0, 0},
+	{0x000002B0, 0x00000000, 0, 0},
+	{0x000002B4, 0x00000000, 0, 0},
+	{0x000002B8, 0x00000000, 0, 0},
+	{0x000002BC, 0x00000000, 0, 0},
+	{0x000002C0, 0x00F000F0, 0, 0},
+	{0x000002C4, 0x00F000F0, 0, 0},
+	{0x000002C8, 0x00800080, 0, 0},
+	{0x000002CC, 0x00800080, 0, 0},
+	{0x000002D0, 0x00800080, 0, 0},
+	{0x000002D4, 0x00800080, 0, 0},
+	{0x000002D8, 0x00B000B0, 0, 0},
+	{0x000002DC, 0x00B000B0, 0, 0},
+	{0x00000E00, 0x24000000, 0, 0},
+	{0x00000E04, 0x159500A5, 0, 0},
+	{0x00000E08, 0x0F9900EE, 0, 0},
+	{0x00000E0C, 0x0CE40127, 0, 0},
+	{0x00000E10, 0x0B410157, 0, 0},
+	{0x00000E14, 0x0A210181, 0, 0},
+	{0x00000E18, 0x094B01A8, 0, 0},
+	{0x00000E1C, 0x08A401CC, 0, 0},
+	{0x00000E20, 0x081D01EE, 0, 0},
+	{0x00000E24, 0x06B20263, 0, 0},
+	{0x00000E28, 0x05D802C7, 0, 0},
+	{0x00000E2C, 0x05420320, 0, 0},
+	{0x00000E30, 0x04D30370, 0, 0},
+	{0x00000E34, 0x047C03BB, 0, 0},
+	{0x00000E38, 0x043703FF, 0, 0},
+	{0x00000010, 0x00000080, 0, 0},
+	{0x00000A08, 0x10000032, 0xFFFFFFF, 0},
+	{0x00000A00, 0x00120002, 0, 0},
+	{0x00000A00, 0x00120000, 0, 0},
+	{0x00000A50, 0x00000002, 0, 0},
+	{0x00000A00, 0x00120001, 0, 0},
+	{0x00000008, 0x00010000, 0, 0},
+	{0x00000008, 0x0002000A, 0, 0},
+	{0x00000000, 0x00000001, 0, 0},
+};
+
+const struct reg_table isp_1920_1080_settings[] = {
+	{isp_1080p_reg_config_list,
+	ARRAY_SIZE(isp_1080p_reg_config_list)},
+};
+
+const struct reg_table isp_sc2235_settings[] = {
+	{isp_sc2235_reg_config_list,
+	ARRAY_SIZE(isp_sc2235_reg_config_list)},
+};
+
+static regval_t isp_format_reg_list[] = {
+	{0x0000001C, 0x00000000, 0, 0},
+	{0x00000020, 0x0437077F, 0, 0},
+	{0x00000A0C, 0x04380780, 0, 0},
+	{0x00000A88, 0x00000780, 0, 0},
+	{0x00000018, 0x000011BB, 0, 0},
+	{0x00000A08, 0x10000000, 0xF0000000, 0},
+};
+
+const struct reg_table  isp_format_settings[] = {
+	{isp_format_reg_list,
+	ARRAY_SIZE(isp_format_reg_list)},
+};
+
+static const struct reg_table *isp_settings = isp_1920_1080_settings;
+
+static void isp_load_regs(void __iomem *ispbase, const struct reg_table *table)
+{
+	int j;
+	u32 delay_ms, reg_addr, mask, val;
+
+	for (j = 0; j < table->regval_num; j++) {
+		delay_ms = table->regval[j].delay_ms;
+		reg_addr = table->regval[j].addr;
+		val = table->regval[j].val;
+		mask = table->regval[j].mask;
+
+		if (reg_addr % 4 || reg_addr > STF_ISP_REG_OFFSET_MAX
+			|| delay_ms > STF_ISP_REG_DELAY_MAX)
+			continue;
+
+		if (mask)
+			reg_set_bit(ispbase, reg_addr, mask, val);
+		else
+			reg_write(ispbase, reg_addr, val);
+		if (delay_ms)
+			usleep_range(1000 * delay_ms, 1000 * delay_ms + 100);
+	}
+}
+
+static int stf_isp_clk_enable(struct stf_isp_dev *isp_dev)
+{
+	struct stfcamss *stfcamss = isp_dev->stfcamss;
+	struct stf_vin_dev *vin = stfcamss->vin;
+	int ret = 0;
+
+	if (isp_dev->id == 0) {
+	// #ifdef USE_CLK_TREE
+#if 0
+		// enable clk
+		ret = stfcamss_enable_clocks(3, &stfcamss->sys_clk[STFCLK_ISP0_CTRL],
+				stfcamss->dev);
+		if (ret < 0) {
+			st_err(ST_ISP, "%s enable clk failed\n", __func__);
+			return ret;
+		}
+#else
+		reg_set_highest_bit(vin->clkgen_base, CLK_ISP0_CTRL);
+		reg_set_highest_bit(vin->clkgen_base, CLK_ISP0_2X_CTRL);
+		reg_set_highest_bit(vin->clkgen_base, CLK_ISP0_MIPI_CTRL);
+#endif
+	} else {
+	//#ifdef USE_CLK_TREE
+#if 0
+		// enable clk
+		ret = stfcamss_enable_clocks(3, &stfcamss->sys_clk[STFCLK_ISP1_CTRL],
+				stfcamss->dev);
+		if (ret < 0) {
+			st_err(ST_ISP, "%s enable clk failed\n", __func__);
+			return ret;
+		}
+#else
+		reg_set_highest_bit(vin->clkgen_base, CLK_ISP1_CTRL);
+		reg_set_highest_bit(vin->clkgen_base, CLK_ISP1_2X_CTRL);
+		reg_set_highest_bit(vin->clkgen_base, CLK_ISP1_MIPI_CTRL);
+#endif
+	}
+
+	return ret;
+}
+
+static int stf_isp_clk_disable(struct stf_isp_dev *isp_dev)
+{
+	struct stfcamss *stfcamss = isp_dev->stfcamss;
+	struct stf_vin_dev *vin = stfcamss->vin;
+
+	if (isp_dev->id == 0) {
+	// #ifdef USE_CLK_TREE
+#if 0
+		stfcamss_disable_clocks(3, &stfcamss->sys_clk[STFCLK_ISP0_CTRL]);
+#else
+		reg_clr_highest_bit(vin->clkgen_base, CLK_ISP0_CTRL);
+		reg_clr_highest_bit(vin->clkgen_base, CLK_ISP0_2X_CTRL);
+		reg_clr_highest_bit(vin->clkgen_base, CLK_ISP0_MIPI_CTRL);
+#endif
+	} else {
+	// #ifdef USE_CLK_TREE
+#if 0
+		stfcamss_disable_clocks(3, &stfcamss->sys_clk[STFCLK_ISP1_CTRL]);
+#else
+		reg_clr_highest_bit(vin->clkgen_base, CLK_ISP1_CTRL);
+		reg_clr_highest_bit(vin->clkgen_base, CLK_ISP1_2X_CTRL);
+		reg_clr_highest_bit(vin->clkgen_base, CLK_ISP1_MIPI_CTRL);
+#endif
+	}
+
+	return 0;
+}
+
+static int stf_isp_reset(struct stf_isp_dev *isp_dev)
+{
+	return 0;
+}
+
+static int stf_isp_config_set(struct stf_isp_dev *isp_dev)
+{
+	struct stf_vin_dev *vin = isp_dev->stfcamss->vin;
+	void __iomem *ispbase;
+
+	if (isp_dev->id == 0)
+		ispbase = vin->isp_isp0_base;
+	else
+		ispbase = vin->isp_isp1_base;
+
+	st_debug(ST_ISP, "%s, isp_id = %d\n", __func__, isp_dev->id);
+
+	isp_load_regs(ispbase, isp_format_settings);
+	mutex_lock(&isp_dev->setfile_lock);
+	if (isp_dev->setfile.state)
+		isp_load_regs(ispbase, &isp_dev->setfile.settings);
+	else
+		isp_load_regs(ispbase, isp_settings);
+	mutex_unlock(&isp_dev->setfile_lock);
+
+	st_debug(ST_ISP, "config 0x%x = 0x%x\n", isp_format_reg_list[0].addr,
+			isp_format_reg_list[0].val);
+	st_debug(ST_ISP, "config 0x%x = 0x%x\n", isp_format_reg_list[1].addr,
+			isp_format_reg_list[1].val);
+	st_debug(ST_ISP, "config 0x%x = 0x%x\n", isp_format_reg_list[2].addr,
+			isp_format_reg_list[2].val);
+	st_debug(ST_ISP, "config 0x%x = 0x%x\n", isp_format_reg_list[3].addr,
+			isp_format_reg_list[3].val);
+
+	return 0;
+}
+
+static int stf_isp_set_format(struct stf_isp_dev *isp_dev,
+		struct v4l2_rect *crop, u32 mcode)
+		// u32 width, u32 height)
+{
+	struct stf_vin_dev *vin = isp_dev->stfcamss->vin;
+	void __iomem *ispbase;
+	u32 val, val1;
+
+	if (isp_dev->id == 0) {
+		ispbase = vin->isp_isp0_base;
+		isp_settings = isp_1920_1080_settings;
+	} else {
+		ispbase = vin->isp_isp1_base;
+		isp_settings = isp_sc2235_settings;
+	}
+
+	val = crop->left + (crop->top << 16);
+	isp_format_reg_list[0].addr = ISP_REG_PIC_CAPTURE_START_CFG;
+	isp_format_reg_list[0].val = val;
+
+	val = (crop->width + crop->left - 1)
+		+ ((crop->height + crop->top - 1) << 16);
+	isp_format_reg_list[1].addr = ISP_REG_PIC_CAPTURE_END_CFG;
+	isp_format_reg_list[1].val = val;
+
+	val = crop->width + (crop->height << 16);
+	isp_format_reg_list[2].addr = ISP_REG_PIPELINE_XY_SIZE;
+	isp_format_reg_list[2].val = val;
+
+	isp_format_reg_list[3].addr = ISP_REG_STRIDE;
+	isp_format_reg_list[3].val = crop->width;
+
+	switch (mcode) {
+	case MEDIA_BUS_FMT_SRGGB10_1X10:
+	case MEDIA_BUS_FMT_SRGGB8_1X8:
+		// 3 2 3 2 1 0 1 0 B Gb B Gb Gr R Gr R
+		val = 0x0000EE44;
+		val1 = 0x00000000;
+		break;
+	case MEDIA_BUS_FMT_SGRBG10_1X10:
+	case MEDIA_BUS_FMT_SGRBG8_1X8:
+		// 2 3 2 3 0 1 0 1, Gb B Gb B R Gr R Gr
+		val = 0x0000BB11;
+		val1 = 0x20000000;
+		break;
+	case MEDIA_BUS_FMT_SGBRG10_1X10:
+	case MEDIA_BUS_FMT_SGBRG8_1X8:
+		// 1 0 1 0 3 2 3 2, Gr R Gr R B Gb B Gb
+		val = 0x000044EE;
+		val1 = 0x30000000;
+		break;
+	case MEDIA_BUS_FMT_SBGGR10_1X10:
+	case MEDIA_BUS_FMT_SBGGR8_1X8:
+		// 0 1 0 1 2 3 2 3 R Gr R Gr Gb B Gb B
+		val = 0x000011BB;
+		val1 = 0x10000000;
+		break;
+	default:
+		st_err(ST_ISP, "UNKNOW format\n");
+		val = 0x000011BB;
+		val1 = 0x10000000;
+		break;
+	}
+	isp_format_reg_list[4].addr = ISP_REG_RAW_FORMAT_CFG;
+	isp_format_reg_list[4].val = val;
+	isp_format_reg_list[5].addr = ISP_REG_ISP_CTRL_1;
+	isp_format_reg_list[5].val = val1;
+	isp_format_reg_list[5].mask = 0xF0000000;
+
+	st_info(ST_ISP, "left: %d, top: %d, width = %d, height = %d, code = 0x%x\n",
+		crop->left, crop->top, crop->width, crop->height, mcode);
+
+	return 0;
+}
+
+static int stf_isp_stream_set(struct stf_isp_dev *isp_dev, int on)
+{
+	return 0;
+}
+
+void dump_isp_reg(void *__iomem ispbase, int id)
+{
+	int j;
+	u32 addr, val;
+
+	st_debug(ST_ISP, "DUMP ISP%d register:\n", id);
+	for (j = 0; j < isp_format_settings->regval_num; j++) {
+		addr = isp_format_settings->regval[j].addr;
+		val = ioread32(ispbase + addr);
+		st_debug(ST_ISP, "{0x%08x, 0x%08x}\n", addr, val);
+	}
+
+	val = ioread32(ispbase + ISP_REG_Y_PLANE_START_ADDR);
+	st_debug(ST_ISP, "{0x%08x, 0x%08x}\n", ISP_REG_Y_PLANE_START_ADDR, val);
+	val = ioread32(ispbase + ISP_REG_UV_PLANE_START_ADDR);
+	st_debug(ST_ISP, "{0x%08x, 0x%08x}\n", ISP_REG_UV_PLANE_START_ADDR, val);
+	val = ioread32(ispbase + ISP_REG_DUMP_CFG_0);
+	st_debug(ST_ISP, "{0x%08x, 0x%08x}\n", ISP_REG_DUMP_CFG_0, val);
+	val = ioread32(ispbase + ISP_REG_DUMP_CFG_1);
+	st_debug(ST_ISP, "{0x%08x, 0x%08x}\n", ISP_REG_DUMP_CFG_1, val);
+
+	for (j = 0; j < isp_settings->regval_num; j++) {
+		addr = isp_settings->regval[j].addr;
+		val = ioread32(ispbase + addr);
+		st_debug(ST_ISP, "{0x%08x, 0x%08x}\n", addr, val);
+	}
+}
+
+struct isp_hw_ops isp_ops = {
+	.isp_clk_enable        = stf_isp_clk_enable,
+	.isp_clk_disable       = stf_isp_clk_disable,
+	.isp_reset             = stf_isp_reset,
+	.isp_config_set        = stf_isp_config_set,
+	.isp_set_format        = stf_isp_set_format,
+	.isp_stream_set        = stf_isp_stream_set,
+};
diff --git a/drivers/media/platform/starfive/stf_video.c b/drivers/media/platform/starfive/stf_video.c
new file mode 100755
index 000000000000..cb3f6b9d400c
--- /dev/null
+++ b/drivers/media/platform/starfive/stf_video.c
@@ -0,0 +1,1626 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021 StarFive Technology Co., Ltd.
+ */
+#include "stfcamss.h"
+#include "stf_video.h"
+#include <media/media-entity.h>
+#include <media/v4l2-mc.h>
+#include <media/videobuf2-dma-sg.h>
+#include <media/videobuf2-vmalloc.h>
+#include <media/videobuf2-dma-contig.h>
+
+#define USE_MEDIA_PIPELINE
+
+static const struct stfcamss_format_info formats_pix_st7110_wr[] = {
+	{ MEDIA_BUS_FMT_YUYV8_2X8, V4L2_PIX_FMT_YUYV, 1,
+		{ { 1, 1 } }, { { 1, 1 } }, { 16 } },
+	{ MEDIA_BUS_FMT_RGB565_2X8_LE, V4L2_PIX_FMT_RGB565, 1,
+		{ { 1, 1 } }, { { 1, 1 } }, { 16 } },
+};
+
+static const struct stfcamss_format_info formats_raw_st7110_isp[] = {
+	{ MEDIA_BUS_FMT_SRGGB10_1X10, V4L2_PIX_FMT_SRGGB12, 1,
+		{ { 1, 1 } }, { { 1, 1 } }, { 12 } },
+	{ MEDIA_BUS_FMT_SGRBG10_1X10, V4L2_PIX_FMT_SGRBG12, 1,
+		{ { 1, 1 } }, { { 1, 1 } }, { 12 } },
+	{ MEDIA_BUS_FMT_SGBRG10_1X10, V4L2_PIX_FMT_SGBRG12, 1,
+		{ { 1, 1 } }, { { 1, 1 } }, { 12 } },
+	{ MEDIA_BUS_FMT_SBGGR10_1X10, V4L2_PIX_FMT_SBGGR12, 1,
+		{ { 1, 1 } }, { { 1, 1 } }, { 12 } },
+};
+
+static const struct stfcamss_format_info formats_pix_st7110_isp[] = {
+	// { MEDIA_BUS_FMT_YUYV12_2X12, V4L2_PIX_FMT_NV21M, 2,
+	//  { { 1, 1 }, { 1, 1 } }, { { 1, 1 }, { 1, 1 } }, { 8 , 4 } },
+	{ MEDIA_BUS_FMT_SRGGB10_1X10, V4L2_PIX_FMT_NV21, 1,
+		{ { 1, 1 } }, { { 2, 3 } }, { 8 } },
+	{ MEDIA_BUS_FMT_SGRBG10_1X10, V4L2_PIX_FMT_NV21, 1,
+		{ { 1, 1 } }, { { 2, 3 } }, { 8 } },
+	{ MEDIA_BUS_FMT_SGBRG10_1X10, V4L2_PIX_FMT_NV21, 1,
+		{ { 1, 1 } }, { { 2, 3 } }, { 8 } },
+	{ MEDIA_BUS_FMT_SBGGR10_1X10, V4L2_PIX_FMT_NV21, 1,
+		{ { 1, 1 } }, { { 2, 3 } }, { 8 } },
+	{ MEDIA_BUS_FMT_SRGGB10_1X10, V4L2_PIX_FMT_NV12, 1,
+		{ { 1, 1 } }, { { 2, 3 } }, { 8 } },
+	{ MEDIA_BUS_FMT_SGRBG10_1X10, V4L2_PIX_FMT_NV12, 1,
+		{ { 1, 1 } }, { { 2, 3 } }, { 8 } },
+	{ MEDIA_BUS_FMT_SGBRG10_1X10, V4L2_PIX_FMT_NV12, 1,
+		{ { 1, 1 } }, { { 2, 3 } }, { 8 } },
+	{ MEDIA_BUS_FMT_SBGGR10_1X10, V4L2_PIX_FMT_NV12, 1,
+		{ { 1, 1 } }, { { 2, 3 } }, { 8 } },
+};
+
+static int video_find_format(u32 code, u32 pixelformat,
+				const struct stfcamss_format_info *formats,
+				unsigned int nformats)
+{
+	int i;
+
+	for (i = 0; i < nformats; i++) {
+		if (formats[i].code == code && formats[i].pixelformat == pixelformat)
+			return i;
+	}
+
+	for (i = 0; i < nformats; i++)
+		if (formats[i].code == code)
+			return i;
+
+	for (i = 0; i < nformats; i++)
+		if (formats[i].pixelformat == pixelformat)
+			return i;
+
+	return -EINVAL;
+}
+
+static int __video_try_fmt(struct stfcamss_video *video,
+		struct v4l2_format *f, int is_mp)
+{
+	struct v4l2_pix_format *pix;
+	struct v4l2_pix_format_mplane *pix_mp;
+	const struct stfcamss_format_info *fi;
+	u32 width, height;
+	u32 bpl;
+	int i, j;
+
+	st_debug(ST_VIDEO, "%s, fmt.type = 0x%x\n", __func__, f->type);
+	pix = &f->fmt.pix;
+	pix_mp = &f->fmt.pix_mp;
+
+	if (is_mp) {
+		for (i = 0; i < video->nformats; i++)
+			if (pix_mp->pixelformat == video->formats[i].pixelformat)
+				break;
+
+		if (i == video->nformats)
+			i = 0; /* default format */
+
+		fi = &video->formats[i];
+		width = pix_mp->width;
+		height = pix_mp->height;
+
+		memset(pix_mp, 0, sizeof(*pix_mp));
+
+		pix_mp->pixelformat = fi->pixelformat;
+		pix_mp->width = clamp_t(u32, width, 1,
+				STFCAMSS_FRAME_MAX_WIDTH);
+		pix_mp->height = clamp_t(u32, height, 1,
+				STFCAMSS_FRAME_MAX_HEIGHT_RDI);
+		pix_mp->num_planes = fi->planes;
+		for (j = 0; j < pix_mp->num_planes; j++) {
+			bpl = pix_mp->width / fi->hsub[j].numerator *
+				fi->hsub[j].denominator * fi->bpp[j] / 8;
+			bpl = ALIGN(bpl, video->bpl_alignment);
+			pix_mp->plane_fmt[j].bytesperline = bpl;
+			pix_mp->plane_fmt[j].sizeimage = pix_mp->height /
+				fi->vsub[j].numerator * fi->vsub[j].denominator * bpl;
+		}
+
+		pix_mp->field = V4L2_FIELD_NONE;
+		pix_mp->colorspace = V4L2_COLORSPACE_SRGB;
+		pix_mp->flags = 0;
+		pix_mp->ycbcr_enc =
+			V4L2_MAP_YCBCR_ENC_DEFAULT(pix_mp->colorspace);
+		pix_mp->quantization =
+			V4L2_MAP_QUANTIZATION_DEFAULT(true,
+				pix_mp->colorspace, pix_mp->ycbcr_enc);
+		pix_mp->xfer_func =
+			V4L2_MAP_XFER_FUNC_DEFAULT(pix_mp->colorspace);
+		st_info(ST_VIDEO, "w, h = %d, %d, bpp = %d, "
+			"i = %d, p = %d, s = 0x%x\n",
+			pix_mp->width, pix_mp->height, fi->bpp[0], i,
+			pix_mp->num_planes, pix_mp->plane_fmt[0].sizeimage);
+
+	} else {
+		for (i = 0; i < video->nformats; i++)
+			if (pix->pixelformat == video->formats[i].pixelformat)
+				break;
+
+		if (i == video->nformats)
+			i = 0; /* default format */
+
+		fi = &video->formats[i];
+		width = pix->width;
+		height = pix->height;
+
+		memset(pix, 0, sizeof(*pix));
+
+		pix->pixelformat = fi->pixelformat;
+		pix->width = clamp_t(u32, width, 1,
+				STFCAMSS_FRAME_MAX_WIDTH);
+		pix->height = clamp_t(u32, height, 1,
+				STFCAMSS_FRAME_MAX_HEIGHT_RDI);
+		bpl = pix->width / fi->hsub[0].numerator *
+			fi->hsub[0].denominator * fi->bpp[0] / 8;
+		bpl = ALIGN(bpl, video->bpl_alignment);
+		pix->bytesperline = bpl;
+		pix->sizeimage = pix->height / fi->vsub[0].numerator
+			* fi->vsub[0].denominator * bpl;
+		pix->field = V4L2_FIELD_NONE;
+		pix->colorspace = V4L2_COLORSPACE_SRGB;
+		pix->flags = 0;
+		pix->ycbcr_enc =
+			V4L2_MAP_YCBCR_ENC_DEFAULT(pix->colorspace);
+		pix->quantization = V4L2_MAP_QUANTIZATION_DEFAULT(true,
+							pix->colorspace, pix->ycbcr_enc);
+		pix->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(pix->colorspace);
+		st_info(ST_VIDEO, "w, h = %d, %d, bpp = %d, "
+				"i = %d, s = 0x%x\n",
+				pix->width, pix->height,
+				fi->bpp[0], i, pix->sizeimage);
+	}
+	return 0;
+}
+
+static int stf_video_init_format(struct stfcamss_video *video, int is_mp)
+{
+	int ret;
+	struct v4l2_format format = {
+		.type = V4L2_BUF_TYPE_VIDEO_CAPTURE,
+		.fmt.pix = {
+			.width = 1920,
+			.height = 1080,
+			.pixelformat = V4L2_PIX_FMT_RGB565,
+		},
+	};
+
+	struct v4l2_format format_mp = {
+		.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE,
+		.fmt.pix = {
+			.width = 1920,
+			.height = 1080,
+			.pixelformat = V4L2_PIX_FMT_NV21,
+		},
+	};
+
+	if (is_mp)
+		ret = __video_try_fmt(video, &format_mp, true);
+	else
+		ret = __video_try_fmt(video, &format, false);
+
+	if (ret < 0)
+		return ret;
+
+	video->active_fmt = is_mp ? format_mp : format;
+
+	return 0;
+}
+
+static int video_queue_setup(struct vb2_queue *q,
+	unsigned int *num_buffers, unsigned int *num_planes,
+	unsigned int sizes[], struct device *alloc_devs[])
+{
+	struct stfcamss_video *video = vb2_get_drv_priv(q);
+	const struct v4l2_pix_format *format = &video->active_fmt.fmt.pix;
+	const struct v4l2_pix_format_mplane *format_mp =
+			&video->active_fmt.fmt.pix_mp;
+	unsigned int i;
+
+	st_debug(ST_VIDEO, "%s, planes = %d\n", __func__, *num_planes);
+
+	if (video->is_mp) {
+		if (*num_planes) {
+			if (*num_planes != format_mp->num_planes)
+				return -EINVAL;
+
+			for (i = 0; i < *num_planes; i++)
+				if (sizes[i] <
+					format_mp->plane_fmt[i].sizeimage)
+					return -EINVAL;
+
+			return 0;
+		}
+
+		*num_planes = format_mp->num_planes;
+
+		for (i = 0; i < *num_planes; i++)
+			sizes[i] = format_mp->plane_fmt[i].sizeimage;
+	} else {
+		if (*num_planes) {
+			if (*num_planes != 1)
+				return -EINVAL;
+
+			if (sizes[0] < format->sizeimage)
+				return -EINVAL;
+		}
+
+		*num_planes  = 1;
+		sizes[0] = format->sizeimage;
+		if (!sizes[0])
+			st_err(ST_VIDEO, "%s: error size is zero!!!\n", __func__);
+	}
+
+	st_info(ST_VIDEO, "%s, planes = %d, size = %d\n",
+			__func__, *num_planes, sizes[0]);
+	return 0;
+}
+
+static int video_buf_init(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct stfcamss_video *video = vb2_get_drv_priv(vb->vb2_queue);
+	struct stfcamss_buffer *buffer =
+		container_of(vbuf, struct stfcamss_buffer, vb);
+	const struct v4l2_pix_format *fmt = &video->active_fmt.fmt.pix;
+	const struct v4l2_pix_format_mplane *fmt_mp =
+				&video->active_fmt.fmt.pix_mp;
+	//struct sg_table *sgt;
+	dma_addr_t *paddr;
+	unsigned int i;
+
+	if (video->is_mp) {
+		for (i = 0; i < fmt_mp->num_planes; i++) {
+			paddr = vb2_plane_cookie(vb, i);
+			buffer->addr[i] = *paddr;
+		}
+
+		if (fmt_mp->num_planes == 1
+			&& (fmt_mp->pixelformat == V4L2_PIX_FMT_NV12
+			|| fmt_mp->pixelformat == V4L2_PIX_FMT_NV21
+			|| fmt_mp->pixelformat == V4L2_PIX_FMT_NV16
+			|| fmt_mp->pixelformat == V4L2_PIX_FMT_NV61))
+			buffer->addr[1] = buffer->addr[0] + fmt_mp->width * fmt_mp->height;
+	} else {
+		paddr = vb2_plane_cookie(vb, 0);
+		buffer->addr[0] = *paddr;
+		if (fmt->pixelformat == V4L2_PIX_FMT_NV12
+			|| fmt->pixelformat == V4L2_PIX_FMT_NV21
+			|| fmt->pixelformat == V4L2_PIX_FMT_NV16
+			|| fmt->pixelformat == V4L2_PIX_FMT_NV61)
+			buffer->addr[1] = buffer->addr[0] + fmt->width * fmt->height;
+	}
+
+	return 0;
+}
+
+static int video_buf_prepare(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct stfcamss_video *video = vb2_get_drv_priv(vb->vb2_queue);
+	const struct v4l2_pix_format *fmt = &video->active_fmt.fmt.pix;
+	const struct v4l2_pix_format_mplane *fmt_mp =
+					&video->active_fmt.fmt.pix_mp;
+	unsigned int i;
+
+	if (video->is_mp) {
+		for (i = 0; i < fmt_mp->num_planes; i++) {
+			if (fmt_mp->plane_fmt[i].sizeimage > vb2_plane_size(vb, i))
+				return -EINVAL;
+			vb2_set_plane_payload(vb, i, fmt_mp->plane_fmt[i].sizeimage);
+		}
+	} else {
+		if (fmt->sizeimage > vb2_plane_size(vb, 0))
+			return -EINVAL;
+		vb2_set_plane_payload(vb, 0, fmt->sizeimage);
+	}
+
+	vbuf->field = V4L2_FIELD_NONE;
+
+	return 0;
+}
+
+static void video_buf_queue(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct stfcamss_video *video = vb2_get_drv_priv(vb->vb2_queue);
+	struct stfcamss_buffer *buffer =
+		container_of(vbuf, struct stfcamss_buffer, vb);
+
+	video->ops->queue_buffer(video, buffer);
+}
+
+static int video_mbus_to_pix_mp(const struct v4l2_mbus_framefmt *mbus,
+				struct v4l2_pix_format_mplane *pix,
+				const struct stfcamss_format_info *f,
+				unsigned int alignment)
+{
+	unsigned int i;
+	u32 bytesperline;
+
+	memset(pix, 0, sizeof(*pix));
+	v4l2_fill_pix_format_mplane(pix, mbus);
+	pix->pixelformat = f->pixelformat;
+	pix->num_planes = f->planes;
+	for (i = 0; i < pix->num_planes; i++) {
+		bytesperline = pix->width / f->hsub[i].numerator *
+			f->hsub[i].denominator * f->bpp[i] / 8;
+		bytesperline = ALIGN(bytesperline, alignment);
+		pix->plane_fmt[i].bytesperline = bytesperline;
+		pix->plane_fmt[i].sizeimage = pix->height /
+				f->vsub[i].numerator * f->vsub[i].denominator *
+				bytesperline;
+	}
+
+	return 0;
+}
+
+static int video_mbus_to_pix(const struct v4l2_mbus_framefmt *mbus,
+			struct v4l2_pix_format *pix,
+			const struct stfcamss_format_info *f,
+			unsigned int alignment)
+{
+	u32 bytesperline;
+
+	memset(pix, 0, sizeof(*pix));
+	v4l2_fill_pix_format(pix, mbus);
+	pix->pixelformat = f->pixelformat;
+	bytesperline = pix->width / f->hsub[0].numerator *
+		f->hsub[0].denominator * f->bpp[0] / 8;
+	bytesperline = ALIGN(bytesperline, alignment);
+	pix->bytesperline = bytesperline;
+	pix->sizeimage = pix->height /
+			f->vsub[0].numerator * f->vsub[0].denominator *
+			bytesperline;
+	return 0;
+}
+
+static struct v4l2_subdev *video_remote_subdev(
+		struct stfcamss_video *video, u32 *pad)
+{
+	struct media_pad *remote;
+
+	remote = media_entity_remote_pad(&video->pad);
+
+	if (!remote || !is_media_entity_v4l2_subdev(remote->entity))
+		return NULL;
+
+	if (pad)
+		*pad = remote->index;
+
+	return media_entity_to_v4l2_subdev(remote->entity);
+}
+
+static int video_get_subdev_format(struct stfcamss_video *video,
+		struct v4l2_format *format)
+{
+	struct v4l2_subdev_format fmt;
+	struct v4l2_subdev *subdev;
+	u32 pixelformat;
+	u32 pad;
+	int ret;
+
+	subdev = video_remote_subdev(video, &pad);
+	if (subdev == NULL)
+		return -EPIPE;
+
+	fmt.pad = pad;
+	fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+
+	ret = v4l2_subdev_call(subdev, pad, get_fmt, NULL, &fmt);
+	if (ret)
+		return ret;
+
+	pixelformat = video->is_mp ? format->fmt.pix.pixelformat
+				: format->fmt.pix_mp.pixelformat;
+	ret = video_find_format(fmt.format.code, pixelformat,
+				video->formats, video->nformats);
+	if (ret < 0)
+		return ret;
+
+	format->type = video->type;
+
+	if (video->is_mp)
+		return video_mbus_to_pix_mp(&fmt.format, &format->fmt.pix_mp,
+				&video->formats[ret], video->bpl_alignment);
+	else
+		return video_mbus_to_pix(&fmt.format, &format->fmt.pix,
+				&video->formats[ret], video->bpl_alignment);
+}
+
+static int video_check_format(struct stfcamss_video *video)
+{
+	struct v4l2_pix_format *pix = &video->active_fmt.fmt.pix;
+	struct v4l2_pix_format_mplane *pix_mp = &video->active_fmt.fmt.pix_mp;
+	struct v4l2_format format;
+	struct v4l2_pix_format *sd_pix = &format.fmt.pix;
+	struct v4l2_pix_format_mplane *sd_pix_mp = &format.fmt.pix_mp;
+	int ret;
+
+	if (video->is_mp) {
+		sd_pix_mp->pixelformat = pix_mp->pixelformat;
+		ret = video_get_subdev_format(video, &format);
+		if (ret < 0)
+			return ret;
+
+		if (pix_mp->pixelformat != sd_pix_mp->pixelformat ||
+			pix_mp->height > sd_pix_mp->height ||
+			pix_mp->width > sd_pix_mp->width ||
+			pix_mp->num_planes != sd_pix_mp->num_planes ||
+			pix_mp->field != format.fmt.pix_mp.field) {
+			st_err(ST_VIDEO,
+				"%s, not match:\n"
+				"0x%x 0x%x\n0x%x 0x%x\n0x%x 0x%x\n",
+				__func__,
+				pix_mp->pixelformat, sd_pix_mp->pixelformat,
+				pix_mp->height, sd_pix_mp->height,
+				pix_mp->field, format.fmt.pix_mp.field);
+			return -EPIPE;
+		}
+
+	} else {
+		sd_pix->pixelformat = pix->pixelformat;
+		ret = video_get_subdev_format(video, &format);
+		if (ret < 0)
+			return ret;
+
+		if (pix->pixelformat != sd_pix->pixelformat ||
+			pix->height > sd_pix->height ||
+			pix->width > sd_pix->width ||
+			pix->field != format.fmt.pix.field) {
+			st_err(ST_VIDEO,
+				"%s, not match:\n"
+				"0x%x 0x%x\n0x%x 0x%x\n0x%x 0x%x\n",
+				__func__,
+				pix->pixelformat, sd_pix->pixelformat,
+				pix->height, sd_pix->height,
+				pix->field, format.fmt.pix.field);
+			return -EPIPE;
+		}
+	}
+	return 0;
+}
+
+static int video_start_streaming(struct vb2_queue *q, unsigned int count)
+{
+	struct stfcamss_video *video = vb2_get_drv_priv(q);
+	struct video_device *vdev = &video->vdev;
+	struct media_entity *entity;
+	struct media_pad *pad;
+	struct v4l2_subdev *subdev;
+	int ret;
+
+#ifdef USE_MEDIA_PIPELINE
+	ret = media_pipeline_start(&vdev->entity, &video->pipe);
+	if (ret < 0) {
+		st_err(ST_VIDEO,
+			"Failed to media_pipeline_start: %d\n", ret);
+		return ret;
+	}
+#endif
+	ret = video_check_format(video);
+	if (ret < 0)
+		goto error;
+	entity = &vdev->entity;
+	while (1) {
+		pad = &entity->pads[0];
+		if (!(pad->flags & MEDIA_PAD_FL_SINK))
+			break;
+
+		pad = media_entity_remote_pad(pad);
+		if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
+			break;
+
+		entity = pad->entity;
+		subdev = media_entity_to_v4l2_subdev(entity);
+
+		ret = v4l2_subdev_call(subdev, video, s_stream, 1);
+		if (ret < 0 && ret != -ENOIOCTLCMD)
+			goto error;
+	}
+	return 0;
+
+error:
+#ifdef USE_MEDIA_PIPELINE
+	media_pipeline_stop(&vdev->entity);
+#endif
+	video->ops->flush_buffers(video, VB2_BUF_STATE_QUEUED);
+	return ret;
+}
+
+static void video_stop_streaming(struct vb2_queue *q)
+{
+	struct stfcamss_video *video = vb2_get_drv_priv(q);
+	struct video_device *vdev = &video->vdev;
+	struct media_entity *entity;
+	struct media_pad *pad;
+	struct v4l2_subdev *subdev;
+
+	entity = &vdev->entity;
+	while (1) {
+		pad = &entity->pads[0];
+		if (!(pad->flags & MEDIA_PAD_FL_SINK))
+			break;
+
+		pad = media_entity_remote_pad(pad);
+		if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
+			break;
+
+		entity = pad->entity;
+		subdev = media_entity_to_v4l2_subdev(entity);
+
+		v4l2_subdev_call(subdev, video, s_stream, 0);
+	}
+
+#ifdef USE_MEDIA_PIPELINE
+	media_pipeline_stop(&vdev->entity);
+#endif
+	video->ops->flush_buffers(video, VB2_BUF_STATE_ERROR);
+}
+
+static const struct vb2_ops stf_video_vb2_q_ops = {
+	.queue_setup     = video_queue_setup,
+	.wait_prepare    = vb2_ops_wait_prepare,
+	.wait_finish     = vb2_ops_wait_finish,
+	.buf_init        = video_buf_init,
+	.buf_prepare     = video_buf_prepare,
+	.buf_queue       = video_buf_queue,
+	.start_streaming = video_start_streaming,
+	.stop_streaming  = video_stop_streaming,
+};
+
+/* -----------------------------------------------------
+ * V4L2 ioctls
+ */
+
+static int video_querycap(struct file *file, void *fh,
+			struct v4l2_capability *cap)
+{
+	struct stfcamss_video *video = video_drvdata(file);
+
+	strscpy(cap->driver, "stf camss", sizeof(cap->driver));
+	strscpy(cap->card, "Starfive Camera Subsystem", sizeof(cap->card));
+	snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s",
+		dev_name(video->stfcamss->dev));
+	return 0;
+}
+
+static int video_get_unique_pixelformat_by_index(struct stfcamss_video *video,
+						int ndx)
+{
+	int i, j, k;
+	/* find index "i" of "k"th unique pixelformat in formats array */
+	k = -1;
+
+	for (i = 0; i < video->nformats; i++) {
+		for (j = 0; j < i; j++) {
+			if (video->formats[i].pixelformat ==
+				video->formats[j].pixelformat)
+				break;
+		}
+
+		if (j == i)
+			k++;
+
+		if (k == ndx)
+			return i;
+	}
+
+	return -EINVAL;
+}
+
+static int video_get_pixelformat_by_mbus_code(struct stfcamss_video *video,
+						u32 mcode)
+{
+	int i;
+
+	for (i = 0; i < video->nformats; i++) {
+		if (video->formats[i].code == mcode)
+			return i;
+	}
+
+	return -EINVAL;
+}
+
+static int video_enum_fmt(struct file *file, void *fh, struct v4l2_fmtdesc *f)
+{
+	struct stfcamss_video *video = video_drvdata(file);
+	int i;
+
+	st_debug(ST_VIDEO, "%s:\n0x%x 0x%x\n 0x%x, 0x%x\n0x%x\n", __func__,
+		f->type, video->type, f->index, video->nformats, f->mbus_code);
+
+	if (f->type != video->type)
+		return -EINVAL;
+	if (f->index >= video->nformats)
+		return -EINVAL;
+
+	if (f->mbus_code) {
+		/* Each entry in formats[] table has unique mbus_code */
+		if (f->index > 0)
+			return -EINVAL;
+
+		i = video_get_pixelformat_by_mbus_code(video, f->mbus_code);
+	} else {
+		i = video_get_unique_pixelformat_by_index(video, f->index);
+	}
+
+	if (i < 0)
+		return -EINVAL;
+	f->pixelformat = video->formats[i].pixelformat;
+
+	return 0;
+}
+static struct v4l2_subdev *get_senname(struct file *file, char *name) {
+	struct stfcamss_video *video = video_drvdata(file);
+	struct video_device *vdev = &video->vdev;
+	struct media_entity *entity = &vdev->entity;
+	struct v4l2_subdev *subdev;
+	struct media_pad *pad;
+	char vin_name[40];
+	int ret;
+
+	strcpy(vin_name,entity->name);
+	while (1) {
+		pad = &entity->pads[0];
+		if (!(pad->flags & MEDIA_PAD_FL_SINK))
+			break;
+		pad = media_entity_remote_pad(pad);
+		if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
+			break;
+		entity = pad->entity;
+	}
+	if(strncmp(vin_name,entity->name,13)==0) {
+		st_err(ST_VIDEO, "===== [%s] Please configure pipeline first =====\n", name);
+		return NULL;
+	}
+	subdev = media_entity_to_v4l2_subdev(entity);
+
+	return subdev;
+}
+
+static int video_enum_framesizes(struct file *file, void *fh,
+				struct v4l2_frmsizeenum *fsize)
+{
+	struct v4l2_subdev_frame_size_enum fse = {0};
+	struct v4l2_subdev_mbus_code_enum code = {0};
+	struct stfcamss_video *video = video_drvdata(file);
+	struct video_device *vdev = &video->vdev;
+	struct media_entity *entity = &vdev->entity;
+	struct media_entity *sensor;
+	struct v4l2_subdev *subdev;
+	int i;
+	int ret;
+
+	for (i = 0; i < video->nformats; i++) {
+		if (video->formats[i].pixelformat == fsize->pixel_format)
+			break;
+	}
+
+	if (i == video->nformats)
+		return -EINVAL;
+
+	sensor = stfcamss_find_sensor(entity);
+	if (sensor) {
+		subdev = media_entity_to_v4l2_subdev(sensor);
+		code.index = 0;
+		code.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+		ret = v4l2_subdev_call(subdev, pad, enum_mbus_code, NULL, &code);
+		if (ret < 0)
+				return EINVAL;
+		fse.index = fsize->index;
+		fse.code = code.code;
+		fse.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+		ret = v4l2_subdev_call(subdev, pad, enum_frame_size, NULL,&fse);
+		if (ret < 0)
+			return -EINVAL;
+		fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
+		fsize->discrete.width = fse.min_width;
+		fsize->discrete.height = fse.min_height;
+	} else {
+		if (fsize->index)
+			return -EINVAL;
+		fsize->type = V4L2_FRMSIZE_TYPE_CONTINUOUS;
+		fsize->stepwise.min_width = STFCAMSS_FRAME_MIN_WIDTH;
+		fsize->stepwise.max_width = STFCAMSS_FRAME_MAX_WIDTH;
+		fsize->stepwise.min_height = STFCAMSS_FRAME_MIN_HEIGHT;
+		fsize->stepwise.max_height = STFCAMSS_FRAME_MAX_HEIGHT_PIX;
+		fsize->stepwise.step_width = 1;
+		fsize->stepwise.step_height = 1;
+	}
+
+	return 0;
+}
+static int video_enum_frameintervals(struct file *file, void *fh,
+				struct v4l2_frmivalenum *fival)
+{
+	int ret = 0;
+	struct stfcamss_video *video = video_drvdata(file);
+	struct video_device *vdev = &video->vdev;
+	struct media_entity *entity = &vdev->entity;
+	struct media_entity *sensor;
+	struct v4l2_subdev *subdev;
+	struct v4l2_subdev_mbus_code_enum code = {0};
+	struct v4l2_subdev_frame_interval_enum fie = {0};
+
+	sensor = stfcamss_find_sensor(entity);
+	if (!sensor)
+		return -EINVAL;
+
+	fie.index = fival->index;
+	fie.width = fival->width;
+	fie.height = fival->height;
+	fie.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+	subdev = media_entity_to_v4l2_subdev(sensor);
+
+	code.index = 0;
+	code.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+
+	ret = v4l2_subdev_call(subdev, pad, enum_mbus_code, NULL, &code);
+	if (ret < 0)
+		return -EINVAL;
+
+	fie.code = code.code;
+	ret = v4l2_subdev_call(subdev, pad, enum_frame_interval, NULL, &fie);
+	if (ret < 0)
+		return ret;
+
+	fival->type = V4L2_FRMSIZE_TYPE_DISCRETE;
+	fival->discrete = fie.interval;
+
+	return 0;
+}
+
+static int video_g_fmt(struct file *file, void *fh, struct v4l2_format *f)
+{
+	struct stfcamss_video *video = video_drvdata(file);
+
+	st_debug(ST_VIDEO, "%s, fmt.type = 0x%x\n", __func__, f->type);
+	st_debug(ST_VIDEO, "%s, active_fmt.type = 0x%x\n",
+			__func__, video->active_fmt.type);
+	*f = video->active_fmt;
+	return 0;
+}
+
+static int video_g_fmt_mp(struct file *file, void *fh, struct v4l2_format *f)
+{
+	struct stfcamss_video *video = video_drvdata(file);
+
+	st_debug(ST_VIDEO, "%s, fmt.type = 0x%x\n", __func__, f->type);
+	st_debug(ST_VIDEO, "%s, active_fmt.type = 0x%x\n",
+			__func__, video->active_fmt.type);
+	*f = video->active_fmt;
+	return 0;
+}
+
+static int video_pipeline_s_fmt(struct stfcamss_video *video,
+			struct v4l2_subdev_state *state,
+			struct v4l2_format *f)
+{
+	struct video_device *vdev = &video->vdev;
+	struct media_entity *entity = &vdev->entity;
+	struct v4l2_subdev *subdev;
+	struct media_device *mdev = entity->graph_obj.mdev;
+	struct media_graph graph;
+	int ret, index;
+	struct v4l2_subdev_format fmt = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	struct v4l2_mbus_framefmt *mf = &fmt.format;
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+	struct v4l2_pix_format_mplane *pix_mp = &f->fmt.pix_mp;
+	u32 width, height, code;
+
+	/* pix to mbus format */
+	if (video->is_mp) {
+		index = video_find_format(mf->code,
+					pix_mp->pixelformat,
+					video->formats, video->nformats);
+		if (index < 0)
+			return index;
+		v4l2_fill_mbus_format_mplane(mf, pix_mp);
+		mf->code = video->formats[index].code;
+		code = mf->code;
+		width = mf->width;
+		height = mf->height;
+	} else {
+		index = video_find_format(mf->code, pix->pixelformat,
+				video->formats, video->nformats);
+		if (index < 0)
+			return index;
+		v4l2_fill_mbus_format(mf, pix, video->formats[index].code);
+		code = mf->code;
+		width = mf->width;
+		height = mf->height;
+	}
+	/*
+	 * Starting from sensor subdevice, walk within
+	 * pipeline and set format on each subdevice
+	 */
+	mutex_lock(&mdev->graph_mutex);
+	ret = media_graph_walk_init(&graph, mdev);
+	if (ret) {
+		mutex_unlock(&mdev->graph_mutex);
+		return ret;
+	}
+
+	media_graph_walk_start(&graph, entity);
+
+	while (!ret && (entity = media_graph_walk_next(&graph))) {
+		if (is_media_entity_v4l2_subdev(entity)) {
+			subdev = media_entity_to_v4l2_subdev(entity);
+			ret = v4l2_subdev_call(subdev, pad, set_fmt, state, &fmt);
+			if (ret < 0 && ret != -ENOIOCTLCMD)
+				break;
+			if (mf->code != code ||
+			    mf->width != width || mf->height != height) {
+				st_warn(ST_VIDEO,
+					"\"%s\":%d pad fmt has been"
+					" changed to 0x%x %ux%u\n",
+					subdev->name, fmt.pad, mf->code,
+					mf->width, mf->height);
+			}
+		}
+	}
+	mutex_unlock(&mdev->graph_mutex);
+
+	media_graph_walk_cleanup(&graph);
+
+	if (ret < 0 && ret != -ENOIOCTLCMD) {
+		st_err(ST_VIDEO,
+			"%s: Failed to set fmt 0x%x %ux%u"
+			" on \"%s\":%d pad (%d)\n",
+			__func__, mf->code,
+			mf->width, mf->height,
+			subdev->name, fmt.pad, ret);
+		return ret;
+	}
+
+	if (video->is_mp)
+		video_mbus_to_pix_mp(mf, pix_mp, &video->formats[index],
+				video->bpl_alignment);
+	else
+		video_mbus_to_pix(mf, pix, &video->formats[index],
+				video->bpl_alignment);
+
+	ret = __video_try_fmt(video, f, video->is_mp);
+	if (ret < 0)
+		return ret;
+
+	return 0;
+}
+
+static int video_s_fmt(struct file *file, void *fh, struct v4l2_format *f)
+{
+	struct stfcamss_video *video = video_drvdata(file);
+	int ret;
+
+	st_debug(ST_VIDEO, "%s, fmt.type = 0x%x\n", __func__, f->type);
+
+	if (vb2_is_busy(&video->vb2_q))
+		return -EBUSY;
+
+	ret = __video_try_fmt(video, f, false);
+	if (ret < 0)
+		return ret;
+
+	ret = video_pipeline_s_fmt(video, NULL, f);
+	if (ret < 0)
+		return ret;
+
+	video->active_fmt = *f;
+
+	return 0;
+}
+
+static int video_s_fmt_mp(struct file *file, void *fh, struct v4l2_format *f)
+{
+	struct stfcamss_video *video = video_drvdata(file);
+	int ret;
+
+	st_debug(ST_VIDEO, "%s, fmt.type = 0x%x\n", __func__, f->type);
+	if (vb2_is_busy(&video->vb2_q))
+		return -EBUSY;
+
+	ret = __video_try_fmt(video, f, true);
+	if (ret < 0)
+		return ret;
+
+	ret = video_pipeline_s_fmt(video, NULL, f);
+	if (ret < 0)
+		return ret;
+
+	video->active_fmt = *f;
+
+	return 0;
+}
+
+static int video_try_fmt(struct file *file, void *fh, struct v4l2_format *f)
+{
+	struct stfcamss_video *video = video_drvdata(file);
+
+	return __video_try_fmt(video, f, false);
+}
+
+static int video_try_fmt_mp(struct file *file, void *fh, struct v4l2_format *f)
+{
+	struct stfcamss_video *video = video_drvdata(file);
+
+	return __video_try_fmt(video, f, true);
+}
+
+static int video_enum_input(struct file *file, void *fh,
+				struct v4l2_input *input)
+{
+	if (input->index > 0)
+		return -EINVAL;
+
+	strscpy(input->name, "camera", sizeof(input->name));
+	input->type = V4L2_INPUT_TYPE_CAMERA;
+
+	return 0;
+}
+
+static int video_g_input(struct file *file, void *fh, unsigned int *input)
+{
+	*input = 0;
+
+	return 0;
+}
+
+static int video_s_input(struct file *file, void *fh, unsigned int input)
+{
+	return input == 0 ? 0 : -EINVAL;
+}
+
+static int video_g_parm(struct file *file, void *priv,
+				struct v4l2_streamparm *p)
+{
+	struct stfcamss_video *video = video_drvdata(file);
+	struct video_device *vdev = &video->vdev;
+	struct media_entity *entity;
+	struct v4l2_subdev *subdev;
+	struct media_pad *pad;
+	int ret, is_support = 0;
+
+	entity = &vdev->entity;
+	while (1) {
+		pad = &entity->pads[0];
+		if (!(pad->flags & MEDIA_PAD_FL_SINK))
+			break;
+
+		pad = media_entity_remote_pad(pad);
+		if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
+			break;
+
+		entity = pad->entity;
+		subdev = media_entity_to_v4l2_subdev(entity);
+
+		ret = v4l2_g_parm_cap(vdev, subdev, p);
+		if (ret < 0 && ret != -ENOIOCTLCMD)
+			break;
+		if (!ret)
+			is_support = 1;
+	}
+
+	return is_support ? 0 : ret;
+}
+
+static int video_s_parm(struct file *file, void *priv,
+				struct v4l2_streamparm *p)
+{
+	struct stfcamss_video *video = video_drvdata(file);
+	struct video_device *vdev = &video->vdev;
+	struct media_entity *entity;
+	struct v4l2_subdev *subdev;
+	struct media_pad *pad;
+	struct v4l2_streamparm tmp_p;
+	int ret, is_support = 0;
+
+	entity = &vdev->entity;
+	while (1) {
+		pad = &entity->pads[0];
+		if (!(pad->flags & MEDIA_PAD_FL_SINK))
+			break;
+
+		pad = media_entity_remote_pad(pad);
+		if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
+			break;
+
+		entity = pad->entity;
+		subdev = media_entity_to_v4l2_subdev(entity);
+
+		tmp_p = *p;
+		ret = v4l2_s_parm_cap(vdev, subdev, &tmp_p);
+		if (ret < 0 && ret != -ENOIOCTLCMD)
+			break;
+		if (!ret) {
+			is_support = 1;
+			*p = tmp_p;
+		}
+	}
+
+	return is_support ? 0 : ret;
+}
+
+/* Crop ioctls */
+int video_g_pixelaspect(struct file *file, void *fh,
+				int buf_type, struct v4l2_fract *aspect)
+{
+	return 0;
+}
+
+int video_g_selection(struct file *file, void *fh,
+				struct v4l2_selection *s)
+{
+	struct stfcamss_video *video = video_drvdata(file);
+	struct video_device *vdev = &video->vdev;
+	struct media_entity *entity;
+	struct v4l2_subdev *subdev;
+	struct media_pad *pad;
+	struct v4l2_subdev_selection sel = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+		.target = s->target,
+		.r = s->r,
+		.flags = s->flags,
+	};
+	int ret;
+
+	st_debug(ST_VIDEO, "%s, target = 0x%x, 0x%x\n",
+			__func__, sel.target, s->target);
+	if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE
+		&& s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+		return -EINVAL;
+
+	entity = &vdev->entity;
+	while (1) {
+		pad = &entity->pads[0];
+		if (!(pad->flags & MEDIA_PAD_FL_SINK))
+			break;
+
+		pad = media_entity_remote_pad(pad);
+		if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
+			break;
+
+		entity = pad->entity;
+		subdev = media_entity_to_v4l2_subdev(entity);
+
+		ret = v4l2_subdev_call(subdev, pad, get_selection, NULL, &sel);
+		if (!ret) {
+			s->r = sel.r;
+			s->flags = sel.flags;
+			break;
+		}
+		if (ret != -ENOIOCTLCMD)
+			break;
+	}
+
+	return ret;
+}
+
+int video_s_selection(struct file *file, void *fh,
+			struct v4l2_selection *s)
+{
+	struct stfcamss_video *video = video_drvdata(file);
+	struct video_device *vdev = &video->vdev;
+	struct media_entity *entity;
+	struct v4l2_subdev *subdev;
+	struct media_pad *pad;
+	struct v4l2_subdev_selection sel = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+		.target = s->target,
+		.r = s->r,
+		.flags = s->flags,
+	};
+	struct v4l2_pix_format *format = &video->active_fmt.fmt.pix;
+	struct v4l2_pix_format_mplane *format_mp = &video->active_fmt.fmt.pix_mp;
+	int ret;
+
+	st_debug(ST_VIDEO, "%s, target = 0x%x, 0x%x\n",
+			__func__, sel.target, s->target);
+	if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE
+		&& s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+		return -EINVAL;
+
+	entity = &vdev->entity;
+	while (1) {
+		pad = &entity->pads[0];
+		if (!(pad->flags & MEDIA_PAD_FL_SINK))
+			break;
+
+		pad = media_entity_remote_pad(pad);
+		if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
+			break;
+
+		entity = pad->entity;
+		subdev = media_entity_to_v4l2_subdev(entity);
+
+		ret = v4l2_subdev_call(subdev, pad, set_selection, NULL, &sel);
+		if (!ret) {
+			s->r = sel.r;
+			s->flags = sel.flags;
+			format->width = s->r.width;
+			format->height = s->r.height;
+			format_mp->width = s->r.width;
+			format_mp->height = s->r.height;
+			ret = __video_try_fmt(video, &video->active_fmt, video->is_mp);
+			if (ret < 0)
+				return ret;
+			break;
+		}
+		if (ret != -ENOIOCTLCMD)
+			break;
+	}
+
+	st_debug(ST_VIDEO, "ret = 0x%x, -EINVAL = 0x%x\n", ret, -EINVAL);
+
+	return ret;
+}
+
+int video_g_ctrl(struct file *file, void *fh,
+				struct v4l2_control *ctrls)
+{
+	struct v4l2_subdev *subdev;
+	int ret;
+
+	subdev = get_senname(file,__func__);
+	if( subdev == NULL)
+		return -EINVAL;
+
+	ret = v4l2_g_ctrl(subdev->ctrl_handler, ctrls);
+
+	return ret;
+}
+
+static int video_s_ctrl(struct file *file, void *fh,
+				struct v4l2_control *ctrl)
+{
+	struct v4l2_subdev *subdev;
+	struct v4l2_fh *vfh;
+	int ret;
+
+	subdev = get_senname(file,__func__);
+	if( subdev == NULL)
+		return -EINVAL;
+
+	vfh = container_of(&subdev->ctrl_handler, struct v4l2_fh , ctrl_handler);
+	if (!vfh->ctrl_handler)
+		return -ENOTTY;
+
+	ret = v4l2_s_ctrl(vfh,subdev->ctrl_handler, ctrl);
+
+	return ret;
+}
+
+static int video_query_ext_ctrl(struct file *file, void *fh,
+				    struct v4l2_query_ext_ctrl *qec)
+{
+	struct v4l2_subdev *subdev;
+	int ret;
+
+	subdev = get_senname(file,__func__);
+	if( subdev == NULL)
+		return -EINVAL;
+
+	ret = v4l2_query_ext_ctrl(subdev->ctrl_handler, qec);
+
+	return ret;
+}
+
+static int video_g_ext_ctrls(struct file *file, void *fh,
+				 struct v4l2_ext_controls *ctrls)
+{
+	struct stfcamss_video *video = video_drvdata(file);
+	struct video_device *vdev = &video->vdev;
+	struct v4l2_subdev *subdev;
+	int ret;
+
+	subdev = get_senname(file,__func__);
+	if( subdev == NULL)
+		return -EINVAL;
+
+	ret = v4l2_g_ext_ctrls(subdev->ctrl_handler,
+						vdev, subdev->v4l2_dev->mdev, ctrls);
+
+	return ret;
+}
+
+static int video_queryctrl(struct file *file, void *fh,
+			       struct v4l2_queryctrl *qc)
+{
+#if 1
+	struct stfcamss_video *video = video_drvdata(file);
+	struct video_device *vdev = &video->vdev;
+	struct media_entity *entity = &vdev->entity;
+	struct media_entity *sensor;
+	struct v4l2_subdev *subdev;
+	int ret = 0;
+
+	sensor = stfcamss_find_sensor(entity);
+	if(sensor) {
+		subdev = media_entity_to_v4l2_subdev(sensor);
+		ret= v4l2_queryctrl(subdev->ctrl_handler, qc);
+	} else {
+	//	st_err(ST_VIDEO, "== [%s] Please configure pipeline first ==\n", __func__);
+		return -EINVAL;
+	}
+
+	return ret;
+#else
+	struct stfcamss_video *video = video_drvdata(file);
+	struct video_device *vdev = &video->vdev;
+	struct v4l2_subdev *subdev;
+	struct v4l2_fh *vfh;
+	int ret;
+
+	subdev = get_senname(file,__func__);
+	if( subdev == NULL )
+		return -EINVAL; 
+
+	vfh = container_of(&subdev->ctrl_handler, struct v4l2_fh , ctrl_handler);
+	ret= v4l2_queryctrl(subdev->ctrl_handler, qc);
+
+	return ret;
+#endif
+}
+
+static int video_s_ext_ctrls(struct file *file, void *fh,
+				 struct v4l2_ext_controls *ctrls)
+{
+	struct stfcamss_video *video = video_drvdata(file);
+	struct video_device *vdev = &video->vdev;
+	struct v4l2_subdev *subdev;
+	struct v4l2_fh *vfh;
+	int ret;
+
+	subdev = get_senname(file,__func__);
+	if( subdev == NULL )
+		return -EINVAL; 
+
+	vfh = container_of(&subdev->ctrl_handler, struct v4l2_fh , ctrl_handler);
+	if (!vfh->ctrl_handler)
+		return -ENOTTY;
+	ret = v4l2_s_ext_ctrls(vfh, subdev->ctrl_handler,
+					vdev, subdev->v4l2_dev->mdev, ctrls);
+
+	return ret;
+}
+
+static int video_try_ext_ctrls(struct file *file, void *fh,
+				   struct v4l2_ext_controls *ctrls)
+{
+	struct stfcamss_video *video = video_drvdata(file);
+	struct video_device *vdev = &video->vdev;
+	struct v4l2_subdev *subdev;
+	struct v4l2_fh *vfh;
+	int ret;
+
+	subdev = get_senname(file,__func__);
+	if( subdev == NULL )
+		return -EINVAL;
+
+	vfh = container_of(&subdev->ctrl_handler, struct v4l2_fh , ctrl_handler);
+	if (!vfh->ctrl_handler)
+		return -ENOTTY;
+	ret = v4l2_try_ext_ctrls(vfh->ctrl_handler,
+					  vdev, subdev->v4l2_dev->mdev, ctrls);
+
+	return ret;
+}
+
+static int video_querymenu(struct file *file, void *fh,
+			       struct v4l2_querymenu *qm)
+{
+	struct v4l2_subdev *subdev;
+	struct v4l2_fh *vfh;
+	int ret;
+
+	subdev = get_senname(file,__func__);
+	if( subdev == NULL )
+		return -EINVAL;
+	ret = v4l2_querymenu(subdev->ctrl_handler, qm);
+
+	return ret;
+}
+
+static const struct v4l2_ioctl_ops stf_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,
+	.vidioc_reqbufs                 = vb2_ioctl_reqbufs,
+	.vidioc_querybuf                = vb2_ioctl_querybuf,
+	.vidioc_qbuf                    = vb2_ioctl_qbuf,
+	.vidioc_expbuf                  = vb2_ioctl_expbuf,
+	.vidioc_dqbuf                   = vb2_ioctl_dqbuf,
+	.vidioc_create_bufs             = vb2_ioctl_create_bufs,
+	.vidioc_prepare_buf             = vb2_ioctl_prepare_buf,
+	.vidioc_streamon                = vb2_ioctl_streamon,
+	.vidioc_streamoff               = vb2_ioctl_streamoff,
+	.vidioc_enum_input              = video_enum_input,
+	.vidioc_g_input                 = video_g_input,
+	.vidioc_s_input                 = video_s_input,
+	.vidioc_g_parm                  = video_g_parm,
+	.vidioc_s_parm                  = video_s_parm,
+	.vidioc_s_selection             = video_s_selection,
+	.vidioc_g_selection             = video_g_selection,
+	.vidioc_g_ctrl                  = video_g_ctrl,
+	.vidioc_s_ctrl                  = video_s_ctrl,
+	.vidioc_g_ext_ctrls             = video_g_ext_ctrls,
+	.vidioc_queryctrl               = video_queryctrl,
+	.vidioc_s_ext_ctrls             = video_s_ext_ctrls,
+	.vidioc_try_ext_ctrls           = video_try_ext_ctrls,
+	.vidioc_querymenu               = video_querymenu,
+	//.vidioc_query_ext_ctrl			= video_query_ext_ctrl,
+
+};
+
+static const struct v4l2_ioctl_ops stf_vid_ioctl_ops_mp = {
+	.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_mplane    = video_g_fmt_mp,
+	.vidioc_s_fmt_vid_cap_mplane    = video_s_fmt_mp,
+	.vidioc_try_fmt_vid_cap_mplane  = video_try_fmt_mp,
+	.vidioc_reqbufs                 = vb2_ioctl_reqbufs,
+	.vidioc_querybuf                = vb2_ioctl_querybuf,
+	.vidioc_qbuf                    = vb2_ioctl_qbuf,
+	.vidioc_expbuf                  = vb2_ioctl_expbuf,
+	.vidioc_dqbuf                   = vb2_ioctl_dqbuf,
+	.vidioc_create_bufs             = vb2_ioctl_create_bufs,
+	.vidioc_prepare_buf             = vb2_ioctl_prepare_buf,
+	.vidioc_streamon                = vb2_ioctl_streamon,
+	.vidioc_streamoff               = vb2_ioctl_streamoff,
+	.vidioc_enum_input              = video_enum_input,
+	.vidioc_g_input                 = video_g_input,
+	.vidioc_s_input                 = video_s_input,
+	.vidioc_g_parm                  = video_g_parm,
+	.vidioc_s_parm                  = video_s_parm,
+	.vidioc_s_selection             = video_s_selection,
+	.vidioc_g_selection             = video_g_selection,
+	.vidioc_g_ctrl                  = video_g_ctrl,
+	.vidioc_s_ctrl                  = video_s_ctrl,
+    .vidioc_g_ext_ctrls             = video_g_ext_ctrls,
+	.vidioc_queryctrl               = video_queryctrl,
+	.vidioc_s_ext_ctrls             = video_s_ext_ctrls,
+	.vidioc_try_ext_ctrls           = video_try_ext_ctrls,
+	.vidioc_querymenu               = video_querymenu,
+	//.vidioc_query_ext_ctrl          = video_query_ext_ctrl,
+};
+
+static int video_open(struct file *file)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct stfcamss_video *video = video_drvdata(file);
+	struct v4l2_fh *vfh;
+	int ret;
+
+	mutex_lock(&video->lock);
+
+	vfh = kzalloc(sizeof(*vfh), GFP_KERNEL);
+	if (vfh == NULL) {
+		ret = -ENOMEM;
+		goto error_alloc;
+	}
+
+	v4l2_fh_init(vfh, vdev);
+	v4l2_fh_add(vfh);
+
+	file->private_data = vfh;
+
+#ifdef USE_MEDIA_PIPELINE
+	ret = v4l2_pipeline_pm_get(&vdev->entity);
+	if (ret < 0) {
+		st_err(ST_VIDEO,
+			"Failed to power up pipeline: %d\n", ret);
+		goto error_pm_use;
+	}
+#else
+	struct media_entity *entity;
+	struct media_pad *pad;
+	struct v4l2_subdev *subdev;
+	int i = 0;
+
+	entity = &vdev->entity;
+	while (1) {
+		pad = &entity->pads[0];
+		if (!(pad->flags & MEDIA_PAD_FL_SINK))
+			break;
+
+		pad = media_entity_remote_pad(pad);
+		if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
+			break;
+
+		entity = pad->entity;
+		subdev = media_entity_to_v4l2_subdev(entity);
+
+		ret = v4l2_subdev_call(subdev, core, s_power, 1);
+		if (ret < 0 && ret != -ENOIOCTLCMD)
+			goto error_power;
+		i++;
+	}
+#endif
+	mutex_unlock(&video->lock);
+
+	return 0;
+#ifndef USE_MEDIA_PIPELINE
+error_power:
+	entity = &vdev->entity;
+	while (i--) {
+		pad = &entity->pads[0];
+		if (!(pad->flags & MEDIA_PAD_FL_SINK))
+			break;
+
+		pad = media_entity_remote_pad(pad);
+		if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
+			break;
+
+		entity = pad->entity;
+		subdev = media_entity_to_v4l2_subdev(entity);
+
+		v4l2_subdev_call(subdev, core, s_power, 0);
+	}
+#endif
+error_pm_use:
+	v4l2_fh_release(file);
+error_alloc:
+	mutex_unlock(&video->lock);
+	return ret;
+}
+
+static int video_release(struct file *file)
+{
+	struct video_device *vdev = video_devdata(file);
+
+	vb2_fop_release(file);
+#ifdef USE_MEDIA_PIPELINE
+	v4l2_pipeline_pm_put(&vdev->entity);
+#else
+	struct media_entity *entity;
+	struct media_pad *pad;
+	struct v4l2_subdev *subdev;
+
+	entity = &vdev->entity;
+	while (1) {
+		pad = &entity->pads[0];
+		if (!(pad->flags & MEDIA_PAD_FL_SINK))
+			break;
+
+		pad = media_entity_remote_pad(pad);
+		if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
+			break;
+
+		entity = pad->entity;
+		subdev = media_entity_to_v4l2_subdev(entity);
+
+		v4l2_subdev_call(subdev, core, s_power, 0);
+	}
+#endif
+	file->private_data = NULL;
+
+	return 0;
+}
+
+static const struct v4l2_file_operations stf_vid_fops = {
+	.owner          = THIS_MODULE,
+	.unlocked_ioctl = video_ioctl2,
+	.open           = video_open,
+	.release        = video_release,
+	.poll           = vb2_fop_poll,
+	.mmap           = vb2_fop_mmap,
+	.read           = vb2_fop_read,
+};
+
+static void stf_video_release(struct video_device *vdev)
+{
+	struct stfcamss_video *video = video_get_drvdata(vdev);
+
+	media_entity_cleanup(&vdev->entity);
+
+	mutex_destroy(&video->q_lock);
+	mutex_destroy(&video->lock);
+}
+
+int stf_video_register(struct stfcamss_video *video,
+			struct v4l2_device *v4l2_dev,
+			const char *name, int is_mp)
+{
+	struct video_device *vdev;
+	struct vb2_queue *q;
+	struct media_pad *pad = &video->pad;
+	int ret;
+
+	vdev = &video->vdev;
+
+	mutex_init(&video->q_lock);
+
+	q = &video->vb2_q;
+	q->drv_priv = video;
+	q->mem_ops = &vb2_dma_contig_memops;
+	q->ops = &stf_video_vb2_q_ops;
+	q->type = is_mp ? V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE :
+		V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	q->io_modes = VB2_DMABUF | VB2_MMAP | VB2_READ;
+	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	q->buf_struct_size = sizeof(struct stfcamss_buffer);
+	q->dev = video->stfcamss->dev;
+	q->lock = &video->q_lock;
+	ret = vb2_queue_init(q);
+	if (ret < 0) {
+		st_err(ST_VIDEO,
+			"Failed to init vb2 queue: %d\n", ret);
+		goto err_vb2_init;
+	}
+
+	pad->flags = MEDIA_PAD_FL_SINK;
+	ret = media_entity_pads_init(&vdev->entity, 1, pad);
+	if (ret < 0) {
+		st_err(ST_VIDEO,
+			"Failed to init video entity: %d\n",
+			ret);
+		goto err_vb2_init;
+	}
+
+	mutex_init(&video->lock);
+
+	if (video->id == VIN_LINE_WR) {
+		video->formats = formats_pix_st7110_wr;
+		video->nformats = ARRAY_SIZE(formats_pix_st7110_wr);
+		video->bpl_alignment = 8;
+	} else if (video->id == VIN_LINE_ISP0
+		|| video->id == VIN_LINE_ISP1) {  // ISP0/ISP1
+		video->formats = formats_pix_st7110_isp;
+		video->nformats = ARRAY_SIZE(formats_pix_st7110_isp);
+		video->bpl_alignment = 8;
+	} else {
+		video->formats = formats_raw_st7110_isp;
+		video->nformats = ARRAY_SIZE(formats_raw_st7110_isp);
+		video->bpl_alignment = 16 * 8;
+	}
+	video->is_mp = is_mp;
+
+	ret = stf_video_init_format(video, is_mp);
+	if (ret < 0) {
+		st_err(ST_VIDEO, "Failed to init format: %d\n", ret);
+		goto err_vid_init_format;
+	}
+
+	vdev->fops = &stf_vid_fops;
+	vdev->device_caps = is_mp ? V4L2_CAP_VIDEO_CAPTURE_MPLANE :
+			V4L2_CAP_VIDEO_CAPTURE;
+	vdev->device_caps |= V4L2_CAP_STREAMING | V4L2_CAP_READWRITE;
+	vdev->ioctl_ops = is_mp ? &stf_vid_ioctl_ops_mp : &stf_vid_ioctl_ops;
+	vdev->release = stf_video_release;
+	vdev->v4l2_dev = v4l2_dev;
+	vdev->vfl_dir = VFL_DIR_RX;
+	vdev->queue = &video->vb2_q;
+	vdev->lock = &video->lock;
+	strlcpy(vdev->name, name, sizeof(vdev->name));
+
+	ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1);
+	if (ret < 0) {
+		st_err(ST_VIDEO,
+			"Failed to register video device: %d\n",
+			ret);
+		goto err_vid_reg;
+	}
+
+	video_set_drvdata(vdev, video);
+	return 0;
+
+err_vid_reg:
+err_vid_init_format:
+	media_entity_cleanup(&vdev->entity);
+	mutex_destroy(&video->lock);
+err_vb2_init:
+	mutex_destroy(&video->q_lock);
+	return ret;
+}
+
+void stf_video_unregister(struct stfcamss_video *video)
+{
+	vb2_video_unregister_device(&video->vdev);
+}
diff --git a/drivers/media/platform/starfive/stf_video.h b/drivers/media/platform/starfive/stf_video.h
new file mode 100644
index 000000000000..d17b3d1c6507
--- /dev/null
+++ b/drivers/media/platform/starfive/stf_video.h
@@ -0,0 +1,75 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021 StarFive Technology Co., Ltd.
+ */
+#ifndef STF_VIDEO_H
+#define STF_VIDEO_H
+
+#include <linux/mutex.h>
+#include <media/videobuf2-v4l2.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-dev.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-fh.h>
+#include <media/v4l2-ioctl.h>
+
+#define STFCAMSS_FRAME_MIN_WIDTH           1
+#define STFCAMSS_FRAME_MAX_WIDTH           8191
+#define STFCAMSS_FRAME_MIN_HEIGHT          1
+#define STFCAMSS_FRAME_MAX_HEIGHT_RDI      8191
+#define STFCAMSS_FRAME_MAX_HEIGHT_PIX      4096
+#define STFCAMSS_FRAME_WIDTH_ALIGN         8
+
+struct stfcamss_buffer {
+	struct vb2_v4l2_buffer vb;
+	dma_addr_t addr[3];
+	struct list_head queue;
+};
+
+struct stfcamss_video;
+
+struct stfcamss_video_ops {
+	int (*queue_buffer)(struct stfcamss_video *vid,
+			struct stfcamss_buffer *buf);
+	int (*flush_buffers)(struct stfcamss_video *vid,
+			enum vb2_buffer_state state);
+};
+
+struct fract {
+	u8 numerator;
+	u8 denominator;
+};
+
+struct stfcamss_format_info {
+	u32 code;
+	u32 pixelformat;
+	u8 planes;
+	struct fract hsub[3];
+	struct fract vsub[3];
+	u8 bpp[3];
+};
+
+struct stfcamss_video {
+	struct stfcamss *stfcamss;
+	u8 id;
+	struct vb2_queue vb2_q;
+	struct video_device vdev;
+	struct media_pad pad;
+	struct media_pipeline pipe;
+	struct v4l2_format active_fmt;
+	enum v4l2_buf_type type;
+	const struct stfcamss_video_ops *ops;
+	struct mutex lock;
+	struct mutex q_lock;
+	unsigned int bpl_alignment;
+	const struct stfcamss_format_info *formats;
+	unsigned int nformats;
+	unsigned int is_mp;
+};
+
+int stf_video_register(struct stfcamss_video *video,
+		struct v4l2_device *v4l2_dev, const char *name, int is_mp);
+
+void stf_video_unregister(struct stfcamss_video *video);
+
+#endif /* STF_VIDEO_H */
diff --git a/drivers/media/platform/starfive/stf_vin.c b/drivers/media/platform/starfive/stf_vin.c
new file mode 100755
index 000000000000..24672f1d22de
--- /dev/null
+++ b/drivers/media/platform/starfive/stf_vin.c
@@ -0,0 +1,971 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021 StarFive Technology Co., Ltd.
+ */
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/dma-mapping.h>
+
+#include "stfcamss.h"
+
+#define STF_VIN_NAME "stf_vin"
+
+#define vin_line_array(ptr_line)        \
+        ((const struct vin_line (*)[]) &(ptr_line[-(ptr_line->id)]))
+
+#define line_to_vin2_dev(ptr_line)        \
+        container_of(vin_line_array(ptr_line), struct stf_vin2_dev, line)
+
+#define VIN_FRAME_DROP_MAX_VAL 30
+#define VIN_FRAME_DROP_MIN_VAL 4
+
+// #define VIN_TWO_BUFFER
+
+static const struct vin2_format vin2_formats_st7110[] = {
+	{ MEDIA_BUS_FMT_YUYV8_2X8, 16},
+	{ MEDIA_BUS_FMT_RGB565_2X8_LE, 16},
+	{ MEDIA_BUS_FMT_SRGGB10_1X10, 12},
+	{ MEDIA_BUS_FMT_SGRBG10_1X10, 12},
+	{ MEDIA_BUS_FMT_SGBRG10_1X10, 12},
+	{ MEDIA_BUS_FMT_SBGGR10_1X10, 12},
+};
+
+static void vin_buffer_done(struct vin_line *line, struct vin_params *params);
+static struct stfcamss_buffer *vin_buf_get_pending(struct vin_output *output);
+static void vin_output_init_addrs(struct vin_line *line);
+static void vin_init_outputs(struct vin_line *line);
+
+static char *get_line_subdevname(int line_id)
+{
+	char *name = NULL;
+
+	switch (line_id) {
+	case VIN_LINE_WR:
+		name = "wr";
+		break;
+	case VIN_LINE_ISP0:
+		name = "isp0";
+		break;
+	case VIN_LINE_ISP1:
+		name = "isp1";
+		break;
+	case VIN_LINE_ISP0_RAW:
+		name = "isp0_raw";
+		break;
+	case VIN_LINE_ISP1_RAW:
+		name = "isp1_raw";
+		break;
+	default:
+		name = "unknow";
+		break;
+	}
+	return name;
+}
+
+int stf_vin_subdev_init(struct stfcamss *stfcamss)
+{
+	struct stf_vin_dev *vin;
+	struct device *dev = stfcamss->dev;
+	struct stf_vin2_dev *vin_dev = stfcamss->vin_dev;
+	int ret = 0, i;
+
+	vin_dev->stfcamss = stfcamss;
+	vin_dev->hw_ops = &vin_ops;
+	vin_dev->hw_ops->isr_buffer_done = vin_buffer_done;
+
+	vin = stfcamss->vin;
+	atomic_set(&vin_dev->ref_count, 0);
+
+	ret = devm_request_irq(dev,
+			vin->irq, vin_dev->hw_ops->vin_wr_irq_handler,
+			0, "vin_axiwr_irq", vin_dev);
+	if (ret) {
+		st_err(ST_VIN, "failed to request irq\n");
+		goto out;
+	}
+
+	ret = devm_request_irq(dev,
+			vin->isp0_irq, vin_dev->hw_ops->vin_isp_irq_handler,
+			0, "vin_isp0_irq", vin_dev);
+	if (ret) {
+		st_err(ST_VIN, "failed to request isp0 irq\n");
+		goto out;
+	}
+
+	ret = devm_request_irq(dev,
+			vin->isp1_irq, vin_dev->hw_ops->vin_isp_irq_handler,
+			0, "vin_isp1_irq", vin_dev);
+	if (ret) {
+		st_err(ST_VIN, "failed to request isp1 irq\n");
+		goto out;
+	}
+
+	vin_dev->hw_ops->vin_wr_irq_enable(vin_dev, 1);
+	vin_dev->hw_ops->vin_wr_irq_enable(vin_dev, 0);
+
+	/* Reset device */
+	ret = vin_dev->hw_ops->vin_clk_init(vin_dev);
+	if (ret) {
+		st_err(ST_VIN, "Failed to reset device\n");
+		goto out;
+	}
+
+	// /* set the sysctl config */
+	// ret = vin_dev->hw_ops->vin_config_set(vin_dev);
+	// if (ret) {
+	//	st_err(ST_VIN, "Failed to config device\n");
+	//	goto out;
+	// }
+
+	mutex_init(&vin_dev->power_lock);
+	vin_dev->power_count = 0;
+
+	for (i = VIN_LINE_WR; i < VIN_LINE_MAX; i++) {
+		struct vin_line *l = &vin_dev->line[i];
+		int is_mp;
+
+		is_mp = i == VIN_LINE_WR ? false : true;
+		is_mp = false;
+		l->video_out.type = is_mp ? V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE :
+				V4L2_BUF_TYPE_VIDEO_CAPTURE;
+		l->video_out.stfcamss = stfcamss;
+		l->id = i;
+		l->sdev_type = VIN_DEV_TYPE;
+		l->formats = vin2_formats_st7110;
+		l->nformats = ARRAY_SIZE(vin2_formats_st7110);
+		spin_lock_init(&l->output_lock);
+
+		mutex_init(&l->stream_lock);
+		l->stream_count = 0;
+		mutex_init(&l->power_lock);
+		l->power_count = 0;
+	}
+
+	return 0;
+out:
+	return ret;
+}
+
+static int vin_set_power(struct v4l2_subdev *sd, int on)
+{
+	struct vin_line *line = v4l2_get_subdevdata(sd);
+	struct stf_vin2_dev *vin_dev = line_to_vin2_dev(line);
+
+	mutex_lock(&line->power_lock);
+	if (on) {
+		if (line->power_count == 0)
+			vin_init_outputs(line);
+		line->power_count++;
+	} else {
+		if (line->power_count == 0) {
+			st_err(ST_VIN,
+				"line power off on power_count == 0\n");
+			goto exit_line;
+		}
+		line->power_count--;
+	}
+exit_line:
+	mutex_unlock(&line->power_lock);
+
+	mutex_lock(&vin_dev->power_lock);
+	if (on) {
+		if (vin_dev->power_count == 0) {
+			vin_dev->hw_ops->vin_clk_enable(vin_dev);
+			vin_dev->hw_ops->vin_config_set(vin_dev);
+		}
+		vin_dev->power_count++;
+	} else {
+		if (vin_dev->power_count == 0) {
+			st_err(ST_VIN,
+				"vin_dev power off on power_count == 0\n");
+			goto exit;
+		}
+		if (vin_dev->power_count == 1)
+			vin_dev->hw_ops->vin_clk_disable(vin_dev);
+		vin_dev->power_count--;
+	}
+exit:
+	mutex_unlock(&vin_dev->power_lock);
+
+	return 0;
+}
+
+static int vin_enable_output(struct vin_line *line)
+{
+	struct vin_output *output = &line->output;
+	unsigned long flags;
+	unsigned int frame_skip = 0;
+	struct media_entity *sensor;
+
+	sensor = stfcamss_find_sensor(&line->subdev.entity);
+	if (sensor) {
+		struct v4l2_subdev *subdev =
+					media_entity_to_v4l2_subdev(sensor);
+
+		v4l2_subdev_call(subdev, sensor, g_skip_frames, &frame_skip);
+		frame_skip += VIN_FRAME_DROP_MIN_VAL;
+		if (frame_skip > VIN_FRAME_DROP_MAX_VAL)
+			frame_skip = VIN_FRAME_DROP_MAX_VAL;
+		st_debug(ST_VIN, "%s, frame_skip %d\n", __func__, frame_skip);
+	}
+
+	spin_lock_irqsave(&line->output_lock, flags);
+	output->frame_skip = frame_skip;
+
+	output->state = VIN_OUTPUT_IDLE;
+
+	output->buf[0] = vin_buf_get_pending(output);
+#ifdef VIN_TWO_BUFFER
+	if (line->id == VIN_LINE_WR)
+		output->buf[1] = vin_buf_get_pending(output);
+#endif
+	if (!output->buf[0] && output->buf[1]) {
+		output->buf[0] = output->buf[1];
+		output->buf[1] = NULL;
+	}
+
+	if (output->buf[0])
+		output->state = VIN_OUTPUT_SINGLE;
+
+#ifdef VIN_TWO_BUFFER
+	if (output->buf[1] && line->id == VIN_LINE_WR)
+		output->state = VIN_OUTPUT_CONTINUOUS;
+#endif
+	output->sequence = 0;
+
+	vin_output_init_addrs(line);
+	spin_unlock_irqrestore(&line->output_lock, flags);
+
+	return 0;
+}
+
+static int vin_disable_output(struct vin_line *line)
+{
+	struct vin_output *output = &line->output;
+	unsigned long flags;
+
+	spin_lock_irqsave(&line->output_lock, flags);
+
+	output->state = VIN_OUTPUT_OFF;
+
+	spin_unlock_irqrestore(&line->output_lock, flags);
+
+	return 0;
+}
+
+static int vin_set_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct vin_line *line = v4l2_get_subdevdata(sd);
+	struct stf_vin2_dev *vin_dev = line_to_vin2_dev(line);
+
+	if (line->id == VIN_LINE_WR) {
+		mutex_lock(&line->stream_lock);
+		if (enable) {
+			if (line->stream_count == 0) {
+				vin_dev->hw_ops->vin_wr_irq_enable(vin_dev, 1);
+				vin_dev->hw_ops->vin_wr_stream_set(vin_dev, 1);
+			}
+			line->stream_count++;
+		} else {
+			if (line->stream_count == 1) {
+				vin_dev->hw_ops->vin_wr_irq_enable(vin_dev, 0);
+				vin_dev->hw_ops->vin_wr_stream_set(vin_dev, 0);
+			}
+			line->stream_count--;
+		}
+		mutex_unlock(&line->stream_lock);
+	}
+
+	if (enable)
+		vin_enable_output(line);
+	else
+		vin_disable_output(line);
+
+	return 0;
+}
+
+static struct v4l2_mbus_framefmt *
+__vin_get_format(struct vin_line *line,
+		struct v4l2_subdev_state *state,
+		unsigned int pad,
+		enum v4l2_subdev_format_whence which)
+{
+	if (which == V4L2_SUBDEV_FORMAT_TRY)
+		return v4l2_subdev_get_try_format(&line->subdev, state, pad);
+	return &line->fmt[pad];
+}
+
+static void vin_try_format(struct vin_line *line,
+				struct v4l2_subdev_state *state,
+				unsigned int pad,
+				struct v4l2_mbus_framefmt *fmt,
+				enum v4l2_subdev_format_whence which)
+{
+	unsigned int i;
+
+	switch (pad) {
+	case STF_VIN_PAD_SINK:
+		/* Set format on sink pad */
+
+		for (i = 0; i < line->nformats; i++)
+			if (fmt->code == line->formats[i].code)
+				break;
+
+		/* If not found, use UYVY as default */
+		if (i >= line->nformats)
+			fmt->code = MEDIA_BUS_FMT_RGB565_2X8_LE;
+
+		fmt->width = clamp_t(u32,
+				fmt->width, 1, STFCAMSS_FRAME_MAX_WIDTH);
+		fmt->height = clamp_t(u32,
+				fmt->height, 1, STFCAMSS_FRAME_MAX_HEIGHT_PIX);
+
+		fmt->field = V4L2_FIELD_NONE;
+		fmt->colorspace = V4L2_COLORSPACE_SRGB;
+		fmt->flags = 0;
+		break;
+
+	case STF_VIN_PAD_SRC:
+		/* Set and return a format same as sink pad */
+		*fmt = *__vin_get_format(line, state, STF_VIN_PAD_SINK, which);
+		break;
+	}
+
+	fmt->colorspace = V4L2_COLORSPACE_SRGB;
+}
+
+static int vin_enum_mbus_code(struct v4l2_subdev *sd,
+				struct v4l2_subdev_state *state,
+				struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct vin_line *line = v4l2_get_subdevdata(sd);
+
+	if (code->index >= line->nformats)
+		return -EINVAL;
+	if (code->pad == STF_VIN_PAD_SINK) {
+		code->code = line->formats[code->index].code;
+	} else {
+		struct v4l2_mbus_framefmt *sink_fmt;
+		sink_fmt = __vin_get_format(line, state, STF_VIN_PAD_SINK, code->which);
+		code->code = sink_fmt->code;
+		if (!code->code)
+			return -EINVAL;
+	}
+	code->flags = 0;
+
+	return 0;
+}
+
+static int vin_enum_frame_size(struct v4l2_subdev *sd,
+				struct v4l2_subdev_state *state,
+				struct v4l2_subdev_frame_size_enum *fse)
+{
+	struct vin_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;
+	vin_try_format(line, state, 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;
+	vin_try_format(line, state, fse->pad, &format, fse->which);
+	fse->max_width = format.width;
+	fse->max_height = format.height;
+
+	return 0;
+}
+
+static int vin_get_format(struct v4l2_subdev *sd,
+			struct v4l2_subdev_state *state,
+			struct v4l2_subdev_format *fmt)
+{
+	struct vin_line *line = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	format = __vin_get_format(line, state, fmt->pad, fmt->which);
+	if (format == NULL)
+		return -EINVAL;
+	fmt->format = *format;
+
+	return 0;
+}
+
+static int vin_set_format(struct v4l2_subdev *sd,
+			struct v4l2_subdev_state *state,
+			struct v4l2_subdev_format *fmt)
+{
+	struct vin_line *line = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	format = __vin_get_format(line, state, fmt->pad, fmt->which);
+	if (format == NULL)
+		return -EINVAL;
+
+	vin_try_format(line, state, fmt->pad, &fmt->format, fmt->which);
+	*format = fmt->format;
+
+	if (fmt->pad == STF_VIN_PAD_SINK) {
+		/* Propagate the format from sink to source */
+		format = __vin_get_format(line, state, STF_VIN_PAD_SRC, fmt->which);
+		*format = fmt->format;
+		vin_try_format(line, state, STF_VIN_PAD_SRC, format, fmt->which);
+	}
+
+	return 0;
+}
+
+static int vin_init_formats(struct v4l2_subdev *sd,
+			struct v4l2_subdev_fh *fh)
+{
+	struct v4l2_subdev_format format = {
+		.pad = STF_VIN_PAD_SINK,
+		.which = fh ? V4L2_SUBDEV_FORMAT_TRY : V4L2_SUBDEV_FORMAT_ACTIVE,
+		.format = {
+			.code = MEDIA_BUS_FMT_RGB565_2X8_LE,
+			.width = 1920,
+			.height = 1080
+		}
+	};
+
+	return vin_set_format(sd, fh ? fh->state : NULL, &format);
+}
+
+static void vin_output_init_addrs(struct vin_line *line)
+{
+	struct vin_output *output = &line->output;
+	struct stf_vin2_dev *vin_dev = line_to_vin2_dev(line);
+	dma_addr_t ping_addr;
+	dma_addr_t pong_addr;
+	dma_addr_t y_addr, uv_addr;
+
+	output->active_buf = 0;
+
+	if (output->buf[0]) {
+		ping_addr = output->buf[0]->addr[0];
+		y_addr = output->buf[0]->addr[0];
+		uv_addr = output->buf[0]->addr[1];
+
+	} else {
+		ping_addr = 0;
+	}
+
+	if (output->buf[1])
+		pong_addr = output->buf[1]->addr[0];
+	else
+		pong_addr = ping_addr;
+
+	switch (line->id) {
+	case VIN_LINE_WR:  // wr
+		vin_dev->hw_ops->vin_wr_set_ping_addr(vin_dev, ping_addr);
+#ifdef VIN_TWO_BUFFER
+		vin_dev->hw_ops->vin_wr_set_pong_addr(vin_dev, pong_addr);
+#else
+		vin_dev->hw_ops->vin_wr_set_pong_addr(vin_dev, ping_addr);
+#endif
+		break;
+	case VIN_LINE_ISP0: // isp0
+	case VIN_LINE_ISP1: // isp1
+		vin_dev->hw_ops->vin_isp_set_yuv_addr(vin_dev,
+			line->id - VIN_LINE_ISP0,
+			y_addr, uv_addr);
+		break;
+	case VIN_LINE_ISP0_RAW: // isp0_raw
+	case VIN_LINE_ISP1_RAW: // isp1_raw
+			vin_dev->hw_ops->vin_isp_set_raw_addr(vin_dev,
+			line->id - VIN_LINE_ISP0_RAW, y_addr);
+		break;
+	default:
+		break;
+	}
+}
+
+static void vin_init_outputs(struct vin_line *line)
+{
+	struct vin_output *output = &line->output;
+
+	output->state = VIN_OUTPUT_OFF;
+	output->buf[0] = NULL;
+	output->buf[1] = NULL;
+	output->active_buf = 0;
+	INIT_LIST_HEAD(&output->pending_bufs);
+}
+
+static void vin_buf_add_pending(struct vin_output *output,
+				struct stfcamss_buffer *buffer)
+{
+	INIT_LIST_HEAD(&buffer->queue);
+	list_add_tail(&buffer->queue, &output->pending_bufs);
+}
+
+static struct stfcamss_buffer *vin_buf_get_pending(struct vin_output *output)
+{
+	struct stfcamss_buffer *buffer = NULL;
+
+	if (!list_empty(&output->pending_bufs)) {
+		buffer = list_first_entry(&output->pending_bufs,
+					struct stfcamss_buffer, queue);
+		list_del(&buffer->queue);
+	}
+
+	return buffer;
+}
+
+#if 0
+static void vin_output_checkpending(struct vin_line *line)
+{
+	struct vin_output *output = &line->output;
+
+	if (output->state == VIN_OUTPUT_STOPPING) {
+		/* Release last buffer when hw is idle */
+		if (output->last_buffer) {
+			// vb2_buffer_done(&output->last_buffer->vb.vb2_buf,
+			//		VB2_BUF_STATE_DONE);
+			vin_buf_add_pending(output, output->last_buffer);
+			output->last_buffer = NULL;
+		}
+		output->state = VIN_OUTPUT_IDLE;
+
+		/* Buffers received in stopping state are queued in */
+		/* dma pending queue, start next capture here */
+		output->buf[0] = vin_buf_get_pending(output);
+#ifdef VIN_TWO_BUFFER
+		if (line->id == VIN_LINE_WR)
+			output->buf[1] = vin_buf_get_pending(output);
+#endif
+
+		if (!output->buf[0] && output->buf[1]) {
+			output->buf[0] = output->buf[1];
+			output->buf[1] = NULL;
+		}
+
+		if (output->buf[0])
+			output->state = VIN_OUTPUT_SINGLE;
+
+#ifdef VIN_TWO_BUFFER
+		if (output->buf[1] && line->id == VIN_LINE_WR)
+			output->state = VIN_OUTPUT_CONTINUOUS;
+#endif
+		vin_output_init_addrs(line);
+	}
+}
+#endif
+
+static void vin_buf_update_on_last(struct vin_line *line)
+{
+	struct vin_output *output = &line->output;
+
+	switch (output->state) {
+	case VIN_OUTPUT_CONTINUOUS:
+		output->state = VIN_OUTPUT_SINGLE;
+		output->active_buf = !output->active_buf;
+		break;
+	case VIN_OUTPUT_SINGLE:
+		output->state = VIN_OUTPUT_STOPPING;
+		break;
+	default:
+		st_err_ratelimited(ST_VIN, "Last buff in wrong state! %d\n",
+				output->state);
+		break;
+	}
+}
+
+static void vin_buf_update_on_next(struct vin_line *line)
+{
+	struct vin_output *output = &line->output;
+
+	switch (output->state) {
+	case VIN_OUTPUT_CONTINUOUS:
+		output->active_buf = !output->active_buf;
+		break;
+	case VIN_OUTPUT_SINGLE:
+	default:
+#ifdef VIN_TWO_BUFFER
+		if (line->id == VIN_LINE_WR)
+			st_err_ratelimited(ST_VIN, "Next buf in wrong state! %d\n",
+				output->state);
+#endif
+		break;
+	}
+}
+
+static void vin_buf_update_on_new(struct vin_line *line,
+				struct vin_output *output,
+				struct stfcamss_buffer *new_buf)
+{
+
+	switch (output->state) {
+	case VIN_OUTPUT_SINGLE:
+#ifdef VIN_TWO_BUFFER
+		struct stf_vin2_dev *vin_dev = line_to_vin2_dev(line);
+		int inactive_idx;
+		inactive_idx = !output->active_buf;
+
+		if (!output->buf[inactive_idx] && line->id == VIN_LINE_WR) {
+			output->buf[inactive_idx] = new_buf;
+			if (inactive_idx)
+				vin_dev->hw_ops->vin_wr_set_pong_addr(vin_dev,
+						output->buf[1]->addr[0]);
+			else
+				vin_dev->hw_ops->vin_wr_set_ping_addr(vin_dev,
+						output->buf[0]->addr[0]);
+			output->state = VIN_OUTPUT_CONTINUOUS;
+
+		} else {
+			vin_buf_add_pending(output, new_buf);
+			if (line->id == VIN_LINE_WR)
+				st_warn(ST_VIN, "Inactive buffer is busy\n");
+		}
+#else
+		vin_buf_add_pending(output, new_buf);
+#endif
+		break;
+	case VIN_OUTPUT_IDLE:
+		st_warn(ST_VIN,	"Output idle buffer set!\n");
+		if (!output->buf[0]) {
+			output->buf[0] = new_buf;
+			vin_output_init_addrs(line);
+			output->state = VIN_OUTPUT_SINGLE;
+		} else {
+			vin_buf_add_pending(output, new_buf);
+			st_warn(ST_VIN,	"Output idle with buffer set!\n");
+		}
+		break;
+	case VIN_OUTPUT_STOPPING:
+		if (output->last_buffer) {
+			output->buf[output->active_buf] = output->last_buffer;
+			output->last_buffer = NULL;
+		} else
+			st_err(ST_VIN,	"stop state lost lastbuffer!\n");
+		output->state = VIN_OUTPUT_SINGLE;
+		// vin_output_checkpending(line);
+		vin_buf_add_pending(output, new_buf);
+		break;
+	case VIN_OUTPUT_CONTINUOUS:
+	default:
+		vin_buf_add_pending(output, new_buf);
+		break;
+	}
+}
+
+static void vin_buf_flush_pending(struct vin_output *output,
+				enum vb2_buffer_state state)
+{
+	struct stfcamss_buffer *buf;
+	struct stfcamss_buffer *t;
+
+	list_for_each_entry_safe(buf, t, &output->pending_bufs, queue) {
+		vb2_buffer_done(&buf->vb.vb2_buf, state);
+		list_del(&buf->queue);
+	}
+}
+
+static void vin_buffer_done(struct vin_line *line, struct vin_params *params)
+{
+	struct stfcamss_buffer *ready_buf;
+	struct vin_output *output = &line->output;
+	struct stf_vin2_dev *vin_dev = line_to_vin2_dev(line);
+	dma_addr_t *new_addr;
+	unsigned long flags;
+	u32 active_index;
+	u64 ts = ktime_get_ns();
+
+	if (output->state == VIN_OUTPUT_OFF
+		|| output->state == VIN_OUTPUT_STOPPING
+		|| output->state == VIN_OUTPUT_RESERVED
+		|| output->state == VIN_OUTPUT_IDLE) {
+		st_warn(ST_VIN,
+				"output state no ready %d!, %d\n",
+				output->state, line->id);
+		return;
+	}
+
+	spin_lock_irqsave(&line->output_lock, flags);
+
+	if (output->frame_skip) {
+		output->frame_skip--;
+		goto out_unlock;
+	}
+
+	active_index = output->active_buf;
+
+	ready_buf = output->buf[active_index];
+	if (!ready_buf) {
+		st_err_ratelimited(ST_VIN,
+					"Missing ready buf %d %d!\n",
+					active_index, output->state);
+		active_index = !active_index;
+		ready_buf = output->buf[active_index];
+		if (!ready_buf) {
+			st_err_ratelimited(ST_VIN,
+					"Missing ready buf 2 %d %d!\n",
+					active_index, output->state);
+			goto out_unlock;
+		}
+	}
+
+	/* Get next buffer */
+	output->buf[active_index] = vin_buf_get_pending(output);
+	if (!output->buf[active_index]) {
+		/* No next buffer - set same address */
+		new_addr = ready_buf->addr;
+		vin_buf_update_on_last(line);
+	} else {
+		new_addr = output->buf[active_index]->addr;
+		vin_buf_update_on_next(line);
+	}
+
+	switch (line->id) {
+	case VIN_LINE_WR:  // wr
+#ifdef VIN_TWO_BUFFER
+		if (active_index)
+			vin_dev->hw_ops->vin_wr_set_pong_addr(vin_dev, new_addr[0]);
+		else
+			vin_dev->hw_ops->vin_wr_set_ping_addr(vin_dev, new_addr[0]);
+#else
+		vin_dev->hw_ops->vin_wr_set_ping_addr(vin_dev, new_addr[0]);
+		vin_dev->hw_ops->vin_wr_set_pong_addr(vin_dev, new_addr[0]);
+#endif
+		break;
+	case VIN_LINE_ISP0: // isp0
+	case VIN_LINE_ISP1: // isp1
+		vin_dev->hw_ops->vin_isp_set_yuv_addr(vin_dev,
+			line->id - VIN_LINE_ISP0, new_addr[0], new_addr[1]);
+		break;
+	case VIN_LINE_ISP0_RAW: // isp0_raw
+	case VIN_LINE_ISP1_RAW: // isp1_raw
+		vin_dev->hw_ops->vin_isp_set_raw_addr(vin_dev,
+			line->id - VIN_LINE_ISP0_RAW, new_addr[0]);
+		break;
+
+	default:
+		break;
+	}
+
+	if (output->state == VIN_OUTPUT_STOPPING)
+		output->last_buffer = ready_buf;
+	else {
+		ready_buf->vb.vb2_buf.timestamp = ts;
+		ready_buf->vb.sequence = output->sequence++;
+		vb2_buffer_done(&ready_buf->vb.vb2_buf, VB2_BUF_STATE_DONE);
+	}
+
+	spin_unlock_irqrestore(&line->output_lock, flags);
+	return;
+
+out_unlock:
+	spin_unlock_irqrestore(&line->output_lock, flags);
+}
+
+static int vin_queue_buffer(struct stfcamss_video *vid,
+				struct stfcamss_buffer *buf)
+{
+	struct vin_line *line = container_of(vid, struct vin_line, video_out);
+	struct vin_output *output;
+	unsigned long flags;
+
+	output = &line->output;
+	spin_lock_irqsave(&line->output_lock, flags);
+	vin_buf_update_on_new(line, output, buf);
+	spin_unlock_irqrestore(&line->output_lock, flags);
+
+	return 0;
+}
+
+static int vin_flush_buffers(struct stfcamss_video *vid,
+				enum vb2_buffer_state state)
+{
+	struct vin_line *line = container_of(vid, struct vin_line, video_out);
+	struct vin_output *output = &line->output;
+	unsigned long flags;
+
+	spin_lock_irqsave(&line->output_lock, flags);
+
+	vin_buf_flush_pending(output, state);
+	if (output->buf[0])
+		vb2_buffer_done(&output->buf[0]->vb.vb2_buf, state);
+
+	if (output->buf[1])
+		vb2_buffer_done(&output->buf[1]->vb.vb2_buf, state);
+
+	if (output->last_buffer) {
+		vb2_buffer_done(&output->last_buffer->vb.vb2_buf, state);
+		output->last_buffer = NULL;
+	}
+	output->buf[0] = output->buf[1] = NULL;
+
+	spin_unlock_irqrestore(&line->output_lock, flags);
+
+	return 0;
+}
+
+static int vin_link_setup(struct media_entity *entity,
+			const struct media_pad *local,
+			const struct media_pad *remote, u32 flags)
+{
+	if (flags & MEDIA_LNK_FL_ENABLED)
+		if (media_entity_remote_pad(local))
+			return -EBUSY;
+	return 0;
+}
+
+static const struct v4l2_subdev_core_ops vin_core_ops = {
+	.s_power = vin_set_power,
+};
+
+static const struct v4l2_subdev_video_ops vin_video_ops = {
+	.s_stream = vin_set_stream,
+};
+
+static const struct v4l2_subdev_pad_ops vin_pad_ops = {
+	.enum_mbus_code   = vin_enum_mbus_code,
+	.enum_frame_size  = vin_enum_frame_size,
+	.get_fmt          = vin_get_format,
+	.set_fmt          = vin_set_format,
+};
+
+static const struct v4l2_subdev_ops vin_v4l2_ops = {
+	.core = &vin_core_ops,
+	.video = &vin_video_ops,
+	.pad = &vin_pad_ops,
+};
+
+static const struct v4l2_subdev_internal_ops vin_v4l2_internal_ops = {
+	.open = vin_init_formats,
+};
+
+static const struct stfcamss_video_ops stfcamss_vin_video_ops = {
+	.queue_buffer = vin_queue_buffer,
+	.flush_buffers = vin_flush_buffers,
+};
+
+static const struct media_entity_operations vin_media_ops = {
+	.link_setup = vin_link_setup,
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+int stf_vin_register(struct stf_vin2_dev *vin_dev, struct v4l2_device *v4l2_dev)
+{
+	struct v4l2_subdev *sd;
+	struct stfcamss_video *video_out;
+	struct media_pad *pads;
+	int ret;
+	int i;
+
+	for (i = 0; i < VIN_LINE_MAX; i++) {
+		char name[32];
+		char *sub_name = get_line_subdevname(i);
+		int is_mp;
+
+		is_mp = (i == VIN_LINE_ISP0) || (i == VIN_LINE_ISP1) ? true : false;
+		is_mp = false;
+		sd = &vin_dev->line[i].subdev;
+		pads = vin_dev->line[i].pads;
+		video_out = &vin_dev->line[i].video_out;
+		video_out->id = i;
+
+		v4l2_subdev_init(sd, &vin_v4l2_ops);
+		sd->internal_ops = &vin_v4l2_internal_ops;
+		sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+		snprintf(sd->name, ARRAY_SIZE(sd->name), "%s%d_%s",
+			STF_VIN_NAME, vin_dev->id, sub_name);
+		v4l2_set_subdevdata(sd, &vin_dev->line[i]);
+
+		ret = vin_init_formats(sd, NULL);
+		if (ret < 0) {
+			st_err(ST_VIN, "Failed to init format: %d\n", ret);
+			goto err_init;
+		}
+
+		pads[STF_VIN_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
+		pads[STF_VIN_PAD_SRC].flags = MEDIA_PAD_FL_SOURCE;
+
+		sd->entity.function =
+			MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER;
+		sd->entity.ops = &vin_media_ops;
+		ret = media_entity_pads_init(&sd->entity,
+				STF_VIN_PADS_NUM, pads);
+		if (ret < 0) {
+			st_err(ST_VIN, "Failed to init media entity: %d\n", ret);
+			goto err_init;
+		}
+
+		ret = v4l2_device_register_subdev(v4l2_dev, sd);
+		if (ret < 0) {
+			st_err(ST_VIN, "Failed to register subdev: %d\n", ret);
+			goto err_reg_subdev;
+		}
+
+		video_out->ops = &stfcamss_vin_video_ops;
+		video_out->bpl_alignment = 16 * 8;
+
+		snprintf(name, ARRAY_SIZE(name), "%s_%s%d",
+			sd->name, "video", i);
+		ret = stf_video_register(video_out, v4l2_dev, name, is_mp);
+		if (ret < 0) {
+			st_err(ST_VIN, "Failed to register video node: %d\n",
+					ret);
+			goto err_vid_reg;
+		}
+
+		ret = media_create_pad_link( &sd->entity, STF_VIN_PAD_SRC,
+			&video_out->vdev.entity, 0,
+			MEDIA_LNK_FL_IMMUTABLE | MEDIA_LNK_FL_ENABLED);
+		if (ret < 0) {
+			st_err(ST_VIN, "Failed to link %s->%s entities: %d\n",
+				sd->entity.name, video_out->vdev.entity.name,
+				ret);
+			goto err_create_link;
+		}
+	}
+
+	return 0;
+
+err_create_link:
+	stf_video_unregister(video_out);
+err_vid_reg:
+	v4l2_device_unregister_subdev(sd);
+err_reg_subdev:
+	media_entity_cleanup(&sd->entity);
+err_init:
+	for (i--; i >= 0; i--) {
+		sd = &vin_dev->line[i].subdev;
+		video_out = &vin_dev->line[i].video_out;
+
+		stf_video_unregister(video_out);
+		v4l2_device_unregister_subdev(sd);
+		media_entity_cleanup(&sd->entity);
+	}
+	return ret;
+}
+
+int stf_vin_unregister(struct stf_vin2_dev *vin_dev)
+{
+	struct v4l2_subdev *sd;
+	struct stfcamss_video *video_out;
+	int i;
+
+	mutex_destroy(&vin_dev->power_lock);
+	for (i = 0; i < VIN_LINE_MAX; i++) {
+		sd = &vin_dev->line[i].subdev;
+		video_out = &vin_dev->line[i].video_out;
+
+		stf_video_unregister(video_out);
+		v4l2_device_unregister_subdev(sd);
+		media_entity_cleanup(&sd->entity);
+		mutex_destroy(&vin_dev->line[i].stream_lock);
+		mutex_destroy(&vin_dev->line[i].power_lock);
+	}
+	return 0;
+}
diff --git a/drivers/media/platform/starfive/stf_vin.h b/drivers/media/platform/starfive/stf_vin.h
new file mode 100644
index 000000000000..1ef65de51028
--- /dev/null
+++ b/drivers/media/platform/starfive/stf_vin.h
@@ -0,0 +1,118 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021 StarFive Technology Co., Ltd.
+ */
+#ifndef STF_VIN_H
+#define STF_VIN_H
+
+#include <media/v4l2-device.h>
+#include <media/v4l2-subdev.h>
+#include <linux/spinlock_types.h>
+#include <video/stf-vin.h>
+#include <linux/platform_device.h>
+
+#include "stf_video.h"
+
+#define STF_VIN_PAD_SINK   0
+#define STF_VIN_PAD_SRC    1
+#define STF_VIN_PADS_NUM   2
+
+struct vin2_format {
+	u32 code;
+	u8 bpp;
+};
+
+enum vin_output_state {
+	VIN_OUTPUT_OFF,
+	VIN_OUTPUT_RESERVED,
+	VIN_OUTPUT_SINGLE,
+	VIN_OUTPUT_CONTINUOUS,
+	VIN_OUTPUT_IDLE,
+	VIN_OUTPUT_STOPPING
+};
+
+struct vin_output {
+	int active_buf;
+	struct stfcamss_buffer *buf[2];
+	struct stfcamss_buffer *last_buffer;
+	struct list_head pending_bufs;
+	enum vin_output_state state;
+	unsigned int sequence;
+	unsigned int frame_skip;
+};
+
+enum vin_line_id {
+	VIN_LINE_NONE = -1,
+	VIN_LINE_WR = 0,
+	VIN_LINE_ISP0 = 1,
+	VIN_LINE_ISP1 = 2,
+	VIN_LINE_ISP0_RAW = 3,
+	VIN_LINE_ISP1_RAW = 4,
+	VIN_LINE_MAX = 5
+};
+
+enum subdev_type;
+
+struct vin_line {
+	enum subdev_type sdev_type;  // must be frist
+	enum vin_line_id id;
+	struct v4l2_subdev subdev;
+	struct media_pad pads[STF_VIN_PADS_NUM];
+	struct v4l2_mbus_framefmt fmt[STF_VIN_PADS_NUM];
+	struct stfcamss_video video_out;
+	struct mutex stream_lock;
+	int stream_count;
+	struct mutex power_lock;
+	int power_count;
+	struct vin_output output;
+	spinlock_t output_lock;
+	const struct vin2_format *formats;
+	unsigned int nformats;
+};
+
+struct stf_vin2_dev;
+
+struct vin_hw_ops {
+	int (*vin_clk_init)(struct stf_vin2_dev *vin_dev);
+	int (*vin_clk_enable)(struct stf_vin2_dev *vin_dev);
+	int (*vin_clk_disable)(struct stf_vin2_dev *vin_dev);
+	int (*vin_config_set)(struct stf_vin2_dev *vin_dev);
+	int (*vin_wr_stream_set)(struct stf_vin2_dev *vin_dev, int on);
+	void (*vin_wr_irq_enable)(struct stf_vin2_dev *vin_dev, int enable);
+	void (*wr_rd_set_addr)(struct stf_vin2_dev *vin_dev,
+			dma_addr_t wr_addr, dma_addr_t rd_addr);
+	void (*vin_wr_set_ping_addr)(struct stf_vin2_dev *vin_dev,
+			dma_addr_t addr);
+	void (*vin_wr_set_pong_addr)(struct stf_vin2_dev *vin_dev,
+			dma_addr_t addr);
+	void (*vin_wr_get_ping_pong_status)(struct stf_vin2_dev *vin_dev);
+	void (*vin_isp_set_yuv_addr)(struct stf_vin2_dev *vin_dev,
+			int isp_id,
+			dma_addr_t y_addr, dma_addr_t uv_addr);
+	void (*vin_isp_set_raw_addr)(struct stf_vin2_dev *vin_dev,
+			int isp_id, dma_addr_t raw_addr);
+	irqreturn_t (*vin_wr_irq_handler)(int irq, void *priv);
+	irqreturn_t (*vin_isp_irq_handler)(int irq, void *priv);
+	void (*isr_buffer_done)(struct vin_line *line,
+			struct vin_params *params);
+};
+
+struct stf_vin2_dev {
+	struct stfcamss *stfcamss;
+	u8 id;
+	struct vin_line line[VIN_LINE_MAX];
+	struct vin_hw_ops *hw_ops;
+	atomic_t ref_count;
+	struct mutex power_lock;
+	int power_count;
+};
+
+extern int stf_vin_subdev_init(struct stfcamss *stfcamss);
+extern int stf_vin_register(struct stf_vin2_dev *vin_dev,
+		struct v4l2_device *v4l2_dev);
+extern int stf_vin_unregister(struct stf_vin2_dev *vin_dev);
+
+extern struct vin_hw_ops vin_ops;
+extern void dump_vin_reg(void *__iomem regbase);
+
+#endif /* STF_VIN_H */
diff --git a/drivers/media/platform/starfive/stf_vin_hw_ops.c b/drivers/media/platform/starfive/stf_vin_hw_ops.c
new file mode 100755
index 000000000000..e75ec940e424
--- /dev/null
+++ b/drivers/media/platform/starfive/stf_vin_hw_ops.c
@@ -0,0 +1,385 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021 StarFive Technology Co., Ltd.
+ */
+#include "stfcamss.h"
+
+static int vin_rstgen_assert_reset(struct stf_vin_dev *vin)
+{
+	u32 val;
+	/*
+	 *      Software_RESET_assert1 (0x11840004)
+	 *      ------------------------------------
+	 *      bit[15]         rstn_vin_src
+	 *      bit[16]         rstn_ispslv_axi
+	 *      bit[17]         rstn_vin_axi
+	 *      bit[18]         rstn_vinnoc_axi
+	 *      bit[19]         rstn_isp0_axi
+	 *      bit[20]         rstn_isp0noc_axi
+	 *      bit[21]         rstn_isp1_axi
+	 *      bit[22]         rstn_isp1noc_axi
+	 *
+	 */
+	u32 val_reg_reset_config = 0x7f8000;
+
+	val = ioread32(vin->vin_top_rstgen_base + SOFTWARE_RESET_ASSERT1);
+	val |= val_reg_reset_config;
+	iowrite32(val, vin->vin_top_rstgen_base + SOFTWARE_RESET_ASSERT1);
+
+	val = ioread32(vin->vin_top_rstgen_base + SOFTWARE_RESET_ASSERT1);
+	val &= ~(val_reg_reset_config);
+
+	iowrite32(val, vin->vin_top_rstgen_base + SOFTWARE_RESET_ASSERT1);
+
+	return 0;
+}
+
+static void vin_intr_clear(void __iomem * sysctrl_base)
+{
+	reg_set_bit(sysctrl_base, SYSCTRL_VIN_INTP_CTRL, BIT(0), 0x1);
+	reg_set_bit(sysctrl_base, SYSCTRL_VIN_INTP_CTRL, BIT(0), 0x0);
+}
+
+static irqreturn_t stf_vin_wr_irq_handler(int irq, void *priv)
+{
+	static struct vin_params params;
+	struct stf_vin2_dev *vin_dev = priv;
+	struct stf_vin_dev *vin = vin_dev->stfcamss->vin;
+
+	vin_dev->hw_ops->isr_buffer_done(&vin_dev->line[VIN_LINE_WR], &params);
+
+	vin_intr_clear(vin->sysctrl_base);
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t stf_vin_isp_irq_handler(int irq, void *priv)
+{
+	static struct vin_params params;
+	struct stf_vin2_dev *vin_dev = priv;
+	struct stf_vin_dev *vin = vin_dev->stfcamss->vin;
+	void __iomem *ispbase;
+	u32 int_status, value;
+	int isp_id = irq == vin->isp0_irq ? 0 : 1;
+
+	if (isp_id == 0)
+		ispbase = vin->isp_isp0_base;
+	else
+		ispbase = vin->isp_isp1_base;
+
+	int_status = reg_read(ispbase, ISP_REG_ISP_CTRL_0);
+
+	// if (int_status & BIT(24))
+	vin_dev->hw_ops->isr_buffer_done(
+		&vin_dev->line[VIN_LINE_ISP0 + isp_id], &params);
+
+	value = reg_read(ispbase, ISP_REG_CIS_MODULE_CFG);
+	if ((value & BIT(19)) && (int_status & BIT(25)))
+		vin_dev->hw_ops->isr_buffer_done(
+			&vin_dev->line[VIN_LINE_ISP0_RAW + isp_id], &params);
+
+	/* clear interrupt */
+	reg_write(ispbase, ISP_REG_ISP_CTRL_0, int_status);
+
+	return IRQ_HANDLED;
+}
+
+static int stf_vin_clk_init(struct stf_vin2_dev *vin_dev)
+{
+	struct stfcamss *stfcamss = vin_dev->stfcamss;
+	struct stf_vin_dev *vin = stfcamss->vin;
+	int ret = 0;
+
+#ifdef USE_CLK_TREE
+	// enable clk
+	ret = stfcamss_enable_clocks(8, &stfcamss->sys_clk[STFCLK_VIN_SRC],
+			stfcamss->dev);
+	if (ret < 0) {
+		st_err(ST_VIN, "%s enable clk failed\n", __func__);
+		return ret;
+	}
+
+	vin_rstgen_assert_reset(vin);
+
+	// hold vin resets for sub modules before csi2rx controller get configed
+	reg_write(vin->rstgen_base, SOFTWARE_RESET_ASSERT0, 0xffffffff);
+	mdelay(10);
+
+	// clear reset for all vin submodules
+	// except dphy-rx (follow lunhai's advice)
+	reg_write(vin->rstgen_base, SOFTWARE_RESET_ASSERT0, 1 << 17);
+	mdelay(10);
+
+	// disable clk
+	stfcamss_disable_clocks(8, &stfcamss->sys_clk[STFCLK_VIN_SRC]);
+	return ret;
+#else
+	val = ioread32(vin->vin_top_clkgen_base + 0x124) >> 24;
+	val &= 0x1;
+	if (val != 0) {
+		val = ioread32(vin->vin_top_clkgen_base + 0x124) >> 24;
+		val &= ~(0x1 << 24);
+		val |= (0x0 & 0x1) << 24;
+		iowrite32(val, vin->vin_top_clkgen_base + 0x124);
+	} else {
+		st_debug(ST_VIN, "nne bus clk src is already clk_cpu_axi\n");
+	}
+
+	// enable clk
+	reg_set_highest_bit(vin->vin_top_clkgen_base, CLK_VIN_SRC_CTRL);
+	reg_set_highest_bit(vin->vin_top_clkgen_base, CLK_ISP0_AXI_CTRL);
+	reg_set_highest_bit(vin->vin_top_clkgen_base, CLK_ISP0NOC_AXI_CTRL);
+	reg_set_highest_bit(vin->vin_top_clkgen_base, CLK_ISPSLV_AXI_CTRL);
+	reg_set_highest_bit(vin->vin_top_clkgen_base, CLK_ISP1_AXI_CTRL);
+	reg_set_highest_bit(vin->vin_top_clkgen_base, CLK_ISP1NOC_AXI_CTRL);
+	reg_set_highest_bit(vin->vin_top_clkgen_base, CLK_VIN_AXI);
+	reg_set_highest_bit(vin->vin_top_clkgen_base, CLK_VINNOC_AXI);
+
+	vin_rstgen_assert_reset(vin);
+
+	// hold vin resets for sub modules before csi2rx controller get configed
+	reg_write(vin->rstgen_base, SOFTWARE_RESET_ASSERT0, 0xffffffff);
+	mdelay(10);
+
+	// clear reset for all vin submodules
+	// except dphy-rx (follow lunhai's advice)
+	reg_write(vin->rstgen_base, SOFTWARE_RESET_ASSERT0, 1 << 17);
+	mdelay(10);
+
+	// disable clk
+	reg_clr_highest_bit(vin->vin_top_clkgen_base, CLK_VIN_SRC_CTRL);
+	reg_clr_highest_bit(vin->vin_top_clkgen_base, CLK_ISPSLV_AXI_CTRL);
+	reg_clr_highest_bit(vin->vin_top_clkgen_base, CLK_VIN_AXI);
+	reg_clr_highest_bit(vin->vin_top_clkgen_base, CLK_VINNOC_AXI);
+
+	reg_set_highest_bit(vin->vin_top_clkgen_base, CLK_ISP0_AXI_CTRL);
+	reg_set_highest_bit(vin->vin_top_clkgen_base, CLK_ISP0NOC_AXI_CTRL);
+
+	reg_set_highest_bit(vin->vin_top_clkgen_base, CLK_ISP1_AXI_CTRL);
+	reg_set_highest_bit(vin->vin_top_clkgen_base, CLK_ISP1NOC_AXI_CTRL);
+	return 0;
+#endif
+}
+
+static int stf_vin_clk_enable(struct stf_vin2_dev *vin_dev)
+{
+	struct stfcamss *stfcamss = vin_dev->stfcamss;
+	struct stf_vin_dev *vin = stfcamss->vin;
+	int ret = 0;
+
+#ifdef USE_CLK_TREE
+	// enable clk
+	ret = stfcamss_enable_clocks(8, &stfcamss->sys_clk[STFCLK_VIN_SRC],
+			stfcamss->dev);
+	if (ret < 0) {
+		st_err(ST_VIN, "%s enable clk failed\n", __func__);
+		return ret;
+	}
+
+	/* rst disable */
+	reg_write(vin->rstgen_base, SOFTWARE_RESET_ASSERT0, 0xFFFFFFFF);
+
+	/* rst enable */
+	reg_write(vin->rstgen_base, SOFTWARE_RESET_ASSERT0, 0x0);
+
+	return ret;
+#else
+	val = ioread32(vin->vin_top_clkgen_base + 0x124) >> 24;
+	val &= 0x1;
+	if (val != 0) {
+		val = ioread32(vin->vin_top_clkgen_base + 0x124) >> 24;
+		val &= ~(0x1 << 24);
+		val |= (0x0 & 0x1) << 24;
+		iowrite32(val, vin->vin_top_clkgen_base + 0x124);
+	} else {
+		st_debug(ST_VIN, "nne bus clk src is already clk_cpu_axi\n");
+	}
+
+	// enable clk
+	reg_set_highest_bit(vin->vin_top_clkgen_base, CLK_VIN_SRC_CTRL);
+	reg_set_highest_bit(vin->vin_top_clkgen_base, CLK_ISPSLV_AXI_CTRL);
+	reg_set_highest_bit(vin->vin_top_clkgen_base, CLK_VIN_AXI);
+	reg_set_highest_bit(vin->vin_top_clkgen_base, CLK_VINNOC_AXI);
+
+	reg_set_highest_bit(vin->vin_top_clkgen_base, CLK_ISP0_AXI_CTRL);
+	reg_set_highest_bit(vin->vin_top_clkgen_base, CLK_ISP0NOC_AXI_CTRL);
+
+	reg_set_highest_bit(vin->vin_top_clkgen_base, CLK_ISP1_AXI_CTRL);
+	reg_set_highest_bit(vin->vin_top_clkgen_base, CLK_ISP1NOC_AXI_CTRL);
+
+	/* rst disable */
+	reg_write(vin->rstgen_base, SOFTWARE_RESET_ASSERT0, 0xFFFFFFFF);
+
+	/* rst enable */
+	reg_write(vin->rstgen_base, SOFTWARE_RESET_ASSERT0, 0x0);
+
+	return 0;
+#endif
+}
+
+static int stf_vin_clk_disable(struct stf_vin2_dev *vin_dev)
+{
+	struct stfcamss *stfcamss = vin_dev->stfcamss;
+
+#ifdef USE_CLK_TREE
+	stfcamss_disable_clocks(8, &stfcamss->sys_clk[STFCLK_VIN_SRC]);
+#else
+	// disable clk
+	reg_clr_highest_bit(vin->vin_top_clkgen_base, CLK_VIN_SRC_CTRL);
+	reg_clr_highest_bit(vin->vin_top_clkgen_base, CLK_ISPSLV_AXI_CTRL);
+	reg_clr_highest_bit(vin->vin_top_clkgen_base, CLK_VIN_AXI);
+	reg_clr_highest_bit(vin->vin_top_clkgen_base, CLK_VINNOC_AXI);
+
+	reg_clr_highest_bit(vin->vin_top_clkgen_base, CLK_ISP0_AXI_CTRL);
+	reg_clr_highest_bit(vin->vin_top_clkgen_base, CLK_ISP0NOC_AXI_CTRL);
+
+	reg_clr_highest_bit(vin->vin_top_clkgen_base, CLK_ISP1_AXI_CTRL);
+	reg_clr_highest_bit(vin->vin_top_clkgen_base, CLK_ISP1NOC_AXI_CTRL);
+#endif
+	return 0;
+}
+
+static int stf_vin_config_set(struct stf_vin2_dev *vin_dev)
+{
+
+	return 0;
+}
+
+static int stf_vin_wr_stream_set(struct stf_vin2_dev *vin_dev, int on)
+{
+	struct stf_vin_dev *vin = vin_dev->stfcamss->vin;
+
+	print_reg(ST_VIN, vin->sysctrl_base, SYSCTRL_VIN_AXI_CTRL);
+	if (on) {
+		reg_set(vin->sysctrl_base,
+				SYSCTRL_VIN_AXI_CTRL, BIT(1));
+	} else {
+		reg_clear(vin->sysctrl_base,
+				SYSCTRL_VIN_AXI_CTRL, BIT(1));
+	}
+	print_reg(ST_VIN, vin->sysctrl_base, SYSCTRL_VIN_AXI_CTRL);
+	return 0;
+}
+
+static void stf_vin_wr_irq_enable(struct stf_vin2_dev *vin_dev,
+		int enable)
+{
+	struct stf_vin_dev *vin = vin_dev->stfcamss->vin;
+	unsigned int value = 0;
+
+	if (enable) {
+		// value = ~((0x1 << 4) | (0x1 << 20));
+		value = ~(0x1 << 4);
+
+		reg_set_bit(vin->sysctrl_base, SYSCTRL_VIN_INTP_CTRL, BIT(4), value);
+	} else {
+		/* mask and clear vin interrupt */
+		// mask_value = (0x1 << 4) | (0x1 << 20);
+		// value = 0x1 | (0x1 << 16) | mask_value;
+		reg_set_bit(vin->sysctrl_base, SYSCTRL_VIN_INTP_CTRL, BIT(0), 0x1);
+		reg_set_bit(vin->sysctrl_base, SYSCTRL_VIN_INTP_CTRL, BIT(0), 0x0);
+
+		value = 0x1 << 4;
+		reg_set_bit(vin->sysctrl_base, SYSCTRL_VIN_INTP_CTRL, BIT(4), value);
+	}
+}
+
+static void stf_vin_wr_rd_set_addr(struct stf_vin2_dev *vin_dev,
+		dma_addr_t wr_addr, dma_addr_t rd_addr)
+{
+	struct stf_vin_dev *vin = vin_dev->stfcamss->vin;
+
+	/* set the start address*/
+	reg_write(vin->sysctrl_base,
+			SYSCTRL_VIN_WR_START_ADDR, (long)wr_addr);
+	reg_write(vin->sysctrl_base,
+			SYSCTRL_VIN_RD_END_ADDR, (long)rd_addr);
+}
+
+void stf_vin_wr_set_ping_addr(struct stf_vin2_dev *vin_dev,
+		dma_addr_t addr)
+{
+	struct stf_vin_dev *vin = vin_dev->stfcamss->vin;
+
+	/* set the start address */
+	reg_write(vin->sysctrl_base, SYSCTRL_VIN_WR_START_ADDR, (long)addr);
+}
+
+void stf_vin_wr_set_pong_addr(struct stf_vin2_dev *vin_dev, dma_addr_t addr)
+{
+	struct stf_vin_dev *vin = vin_dev->stfcamss->vin;
+
+	/* set the start address */
+	reg_write(vin->sysctrl_base, SYSCTRL_VIN_RD_END_ADDR, (long)addr);
+}
+
+void stf_vin_isp_set_yuv_addr(struct stf_vin2_dev *vin_dev, int isp_id,
+				dma_addr_t y_addr, dma_addr_t uv_addr)
+{
+	struct stf_vin_dev *vin = vin_dev->stfcamss->vin;
+	void __iomem *ispbase =
+		isp_id ? vin->isp_isp1_base : vin->isp_isp0_base;
+
+	reg_set_bit(ispbase, ISP_REG_ISP_CTRL_0, BIT(0), 0);
+	reg_write(ispbase, ISP_REG_Y_PLANE_START_ADDR, y_addr);
+	reg_write(ispbase, ISP_REG_UV_PLANE_START_ADDR, uv_addr);
+	// shadow update
+	reg_set_bit(ispbase, ISP_REG_IESHD_ADDR, BIT(1) | BIT(0), 0x3);
+	reg_set_bit(ispbase, ISP_REG_ISP_CTRL_0, BIT(0), 1);
+}
+
+void stf_vin_isp_set_raw_addr(struct stf_vin2_dev *vin_dev, int isp_id,
+				dma_addr_t raw_addr)
+{
+	struct stf_vin_dev *vin = vin_dev->stfcamss->vin;
+	void __iomem *ispbase =
+		isp_id ? vin->isp_isp1_base : vin->isp_isp0_base;
+
+	reg_write(ispbase, ISP_REG_DUMP_CFG_0, raw_addr);
+	reg_set_bit(ispbase, ISP_REG_CSIINTS_ADDR, 0x3FFFF, 0x3000a);
+}
+
+void dump_vin_reg(void *__iomem regbase)
+{
+	st_debug(ST_VIN, "DUMP VIN register:\n");
+	print_reg(ST_VIN, regbase, 0x00);
+	print_reg(ST_VIN, regbase, 0x04);
+	print_reg(ST_VIN, regbase, 0x08);
+	print_reg(ST_VIN, regbase, 0x0c);
+	print_reg(ST_VIN, regbase, 0x10);
+	print_reg(ST_VIN, regbase, 0x14);
+	print_reg(ST_VIN, regbase, 0x18);
+	print_reg(ST_VIN, regbase, 0x1c);
+	print_reg(ST_VIN, regbase, 0x20);
+	print_reg(ST_VIN, regbase, 0x24);
+	print_reg(ST_VIN, regbase, 0x28);
+	print_reg(ST_VIN, regbase, 0x2c);
+	print_reg(ST_VIN, regbase, 0x30);
+	print_reg(ST_VIN, regbase, 0x34);
+	print_reg(ST_VIN, regbase, 0x38);
+	print_reg(ST_VIN, regbase, 0x3c);
+	print_reg(ST_VIN, regbase, 0x40);
+	print_reg(ST_VIN, regbase, 0x44);
+	print_reg(ST_VIN, regbase, 0x48);
+	print_reg(ST_VIN, regbase, 0x4c);
+	print_reg(ST_VIN, regbase, 0x50);
+	print_reg(ST_VIN, regbase, 0x54);
+	print_reg(ST_VIN, regbase, 0x58);
+	print_reg(ST_VIN, regbase, 0x5c);
+}
+
+struct vin_hw_ops vin_ops = {
+	.vin_clk_init          = stf_vin_clk_init,
+	.vin_clk_enable        = stf_vin_clk_enable,
+	.vin_clk_disable       = stf_vin_clk_disable,
+	.vin_config_set        = stf_vin_config_set,
+	.vin_wr_stream_set     = stf_vin_wr_stream_set,
+	.vin_wr_irq_enable     = stf_vin_wr_irq_enable,
+	.wr_rd_set_addr        = stf_vin_wr_rd_set_addr,
+	.vin_wr_set_ping_addr  = stf_vin_wr_set_ping_addr,
+	.vin_wr_set_pong_addr  = stf_vin_wr_set_pong_addr,
+	.vin_isp_set_yuv_addr  = stf_vin_isp_set_yuv_addr,
+	.vin_isp_set_raw_addr  = stf_vin_isp_set_raw_addr,
+	.vin_wr_irq_handler    = stf_vin_wr_irq_handler,
+	.vin_isp_irq_handler   = stf_vin_isp_irq_handler,
+};
diff --git a/drivers/media/platform/starfive/stfcamss.c b/drivers/media/platform/starfive/stfcamss.c
new file mode 100755
index 000000000000..96c9156d4e7d
--- /dev/null
+++ b/drivers/media/platform/starfive/stfcamss.c
@@ -0,0 +1,1258 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021 StarFive Technology Co., Ltd.
+ */
+#include <linux/clk.h>
+#include <linux/completion.h>
+#include <linux/delay.h>
+#include <linux/dmaengine.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/of_reserved_mem.h>
+#include <linux/of_graph.h>
+#include <linux/of_address.h>
+#include <linux/pinctrl/consumer.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/reset.h>
+#include <linux/io.h>
+#include <linux/dma-mapping.h>
+#include <linux/uaccess.h>
+
+#include <linux/videodev2.h>
+
+#include <media/media-device.h>
+#include <media/v4l2-async.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-mc.h>
+#include <media/v4l2-fwnode.h>
+#include <linux/debugfs.h>
+
+#include "stfcamss.h"
+
+#ifdef STF_DEBUG
+unsigned int stdbg_level = ST_DEBUG;
+unsigned int stdbg_mask = 0x7F;
+#else
+unsigned int stdbg_level = ST_ERR;
+unsigned int stdbg_mask = 0x7F;
+#endif
+
+static const struct reg_name mem_reg_name[] = {
+#ifndef CONFIG_VIDEO_CADENCE_CSI2RX
+	{"mipi0"},
+#endif
+	{"vclk"},
+	{"vrst"},
+	{"mipi1"},
+	{"sctrl"},
+	{"isp0"},
+	{"isp1"},
+	// {"tclk"},
+	// {"trst"},
+	// {"iopad"}
+};
+
+char * clocks[] = {
+	"vin_src",
+	"isp0_axi",
+	"isp0noc_axi",
+	"ispslv_axi",
+	"isp1_axi",
+	"isp1noc_axi",
+	"vin_axi",
+	"vinnoc_axi",
+	// "csi2rx_apb_clk",
+	// "mipirx0_pixel0",
+	// "mipirx0_pixel1",
+	// "mipirx0_pixel2",
+	// "mipirx0_pixel3",
+	// "mipirx0_sys",
+	// "mipirx1_pixel0",
+	// "mipirx1_pixel1",
+	// "mipirx1_pixel2",
+	// "mipirx1_pixel3",
+	// "mipirx1_sys",
+	// "csidphy_cfgclk",
+	// "csidphy_regclk",
+	// "csidphy_txclkesc",
+	// "isp0_ctrl",
+	// "isp0_2x_ctrl",
+	// "isp0_mipi_ctrl",
+	// "isp1_ctrl",
+	// "isp1_2x_ctrl",
+	// "isp1_mipi_ctrl",
+
+	NULL,
+};
+
+int stfcamss_enable_clocks(int nclocks, struct stfcamss_clk *clock,
+			struct device *dev)
+{
+	int ret;
+	int i;
+
+	for (i = 0; i < nclocks; i++) {
+		ret = clk_prepare_enable(clock[i].clk);
+		if (ret) {
+			st_err(ST_CAMSS, "clock enable failed: %d\n", ret);
+			goto error;
+		}
+		st_debug(ST_CAMSS, "%s, %s\n", __func__, clock[i].name);
+	}
+
+	return 0;
+
+error:
+	for (i--; i >= 0; i--)
+		clk_disable_unprepare(clock[i].clk);
+
+	return ret;
+}
+
+void stfcamss_disable_clocks(int nclocks, struct stfcamss_clk *clock)
+{
+	int i;
+
+	for (i = nclocks - 1; i >= 0; i--) {
+		clk_disable_unprepare(clock[i].clk);
+		st_debug(ST_CAMSS, "%s, %s\n", __func__, clock[i].name);
+	}
+}
+
+
+int stfcamss_get_mem_res(struct platform_device *pdev, struct stf_vin_dev *vin)
+{
+	struct device *dev = &pdev->dev;
+	struct resource	*res;
+	void __iomem *regs;
+	char *name;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(mem_reg_name); i++) {
+		name = (char *)(&mem_reg_name[i]);
+		res = platform_get_resource_byname(pdev, IORESOURCE_MEM, name);
+		regs = devm_ioremap_resource(dev, res);
+		if (IS_ERR(regs))
+			return PTR_ERR(regs);
+
+		if (!strcmp(name, "mipi0"))
+			vin->mipi0_base = regs;
+		else if (!strcmp(name, "vclk"))
+			vin->clkgen_base = regs;
+		else if (!strcmp(name, "vrst"))
+			vin->rstgen_base = regs;
+		else if (!strcmp(name, "mipi1"))
+			vin->mipi1_base = regs;
+		else if (!strcmp(name, "sctrl"))
+			vin->sysctrl_base = regs;
+		else if (!strcmp(name, "isp0"))
+			vin->isp_isp0_base = regs;
+		else if (!strcmp(name, "isp1"))
+			vin->isp_isp1_base = regs;
+		else if (!strcmp(name, "tclk"))
+			vin->vin_top_clkgen_base = regs;
+		else if (!strcmp(name, "trst"))
+			vin->vin_top_rstgen_base = regs;
+		else if (!strcmp(name, "iopad"))
+			vin->vin_top_iopad_base = regs;
+		else
+			st_err(ST_CAMSS, "Could not match resource name\n");
+	}
+
+	#ifdef USE_CLK_TREE
+		// vin->clkgen_base = ioremap(ISP_BASE_CLKGEN_ADDR, REG_B_LEN);
+		// vin->rstgen_base = ioremap(ISP_BASE_RSTGEN_ADDR, REG_B_LEN);
+		vin->vin_top_clkgen_base = ioremap(VIN_TOP_CLKGEN_BASE_ADDR, 0x10000);
+		vin->vin_top_rstgen_base = ioremap(VIN_TOP_RSTGEN_BASE_ADDR, 0x10000);
+		vin->vin_top_iopad_base = ioremap(VIN_TOP_IOPAD_BASE_ADDR, 0x10000);
+	#endif
+
+	return 0;
+}
+
+int vin_parse_dt(struct device *dev, struct stf_vin_dev *vin)
+{
+	int ret = 0;
+	struct device_node *np = dev->of_node;
+
+	if (!np)
+		return -EINVAL;
+
+	return ret;
+}
+
+struct media_entity *stfcamss_find_sensor(struct media_entity *entity)
+{
+	struct media_pad *pad;
+
+	while (1) {
+		pad = &entity->pads[0];
+		if (!(pad->flags & MEDIA_PAD_FL_SINK))
+			return NULL;
+
+		pad = media_entity_remote_pad(pad);
+		if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
+			return NULL;
+
+		entity = pad->entity;
+
+		if (entity->function == MEDIA_ENT_F_CAM_SENSOR)
+			return entity;
+	}
+}
+
+static int stfcamss_of_parse_endpoint_node(struct device *dev,
+				struct device_node *node,
+				struct stfcamss_async_subdev *csd)
+{
+	struct v4l2_fwnode_endpoint vep = { { 0 } };
+	struct v4l2_fwnode_bus_parallel *parallel_bus = &vep.bus.parallel;
+	struct v4l2_fwnode_bus_mipi_csi2 *csi2_bus = &vep.bus.mipi_csi2;
+	struct dvp_cfg *dvp = &csd->interface.dvp;
+	struct csi2phy_cfg *csiphy = &csd->interface.csiphy;
+
+	v4l2_fwnode_endpoint_parse(of_fwnode_handle(node), &vep);
+	st_debug(ST_CAMSS, "%s: vep.base.port = 0x%x, id = 0x%x\n",
+			__func__, vep.base.port, vep.base.id);
+
+	csd->port = vep.base.port;
+	switch (csd->port) {
+	case CSI2RX0_PORT_NUMBER:
+	case CSI2RX1_PORT_NUMBER:
+		break;
+	case DVP_SENSOR_PORT_NUMBER:
+		st_debug(ST_CAMSS, "%s, flags = 0x%x\n", __func__,
+				parallel_bus->flags);
+		dvp->flags = parallel_bus->flags;
+		dvp->bus_width = parallel_bus->bus_width;
+		dvp->data_shift = parallel_bus->data_shift;
+		break;
+	case CSI2RX0_SENSOR_PORT_NUMBER:
+	case CSI2RX1_SENSOR_PORT_NUMBER:
+		st_debug(ST_CAMSS, "%s, CSI2 flags = 0x%x\n",
+				__func__, parallel_bus->flags);
+		csiphy->flags = csi2_bus->flags;
+		memcpy(csiphy->data_lanes,
+				csi2_bus->data_lanes, csi2_bus->num_data_lanes);
+		csiphy->clock_lane = csi2_bus->clock_lane;
+		csiphy->num_data_lanes = csi2_bus->num_data_lanes;
+		memcpy(csiphy->lane_polarities, csi2_bus->lane_polarities,
+				csi2_bus->num_data_lanes + 1);
+		break;
+	default:
+		break;
+	};
+
+	return 0;
+}
+
+static int stfcamss_of_parse_ports(struct stfcamss *stfcamss)
+{
+	struct device *dev = stfcamss->dev;
+	struct device_node *node = NULL;
+	struct device_node *remote = NULL;
+	int ret, num_subdevs = 0;
+
+	for_each_endpoint_of_node(dev->of_node, node) {
+		struct stfcamss_async_subdev *csd;
+
+		if (!of_device_is_available(node))
+			continue;
+
+		remote = of_graph_get_remote_port_parent(node);
+		if (!remote) {
+			st_err(ST_CAMSS, "Cannot get remote parent\n");
+			ret = -EINVAL;
+			goto err_cleanup;
+		}
+
+		csd = v4l2_async_notifier_add_fwnode_subdev(
+			&stfcamss->notifier, of_fwnode_handle(remote),
+			struct stfcamss_async_subdev);
+		of_node_put(remote);
+		if (IS_ERR(csd)) {
+			ret = PTR_ERR(csd);
+			goto err_cleanup;
+		}
+
+		ret = stfcamss_of_parse_endpoint_node(dev, node, csd);
+		if (ret < 0)
+			goto err_cleanup;
+
+		num_subdevs++;
+	}
+
+	return num_subdevs;
+
+err_cleanup:
+	of_node_put(node);
+	return ret;
+}
+
+static int stfcamss_init_subdevices(struct stfcamss *stfcamss)
+{
+	int ret, i;
+
+	ret = stf_dvp_subdev_init(stfcamss);
+	if (ret < 0) {
+		st_err(ST_CAMSS,
+			"Failed to init stf_dvp sub-device: %d\n",
+			ret);
+		return ret;
+	}
+
+	for (i = 0; i < stfcamss->csiphy_num; i++) {
+		ret = stf_csiphy_subdev_init(stfcamss, i);
+		if (ret < 0) {
+			st_err(ST_CAMSS,
+				"Failed to init stf_csiphy sub-device: %d\n",
+				ret);
+			return ret;
+		}
+	}
+
+#ifndef CONFIG_VIDEO_CADENCE_CSI2RX
+	for (i = 0; i < stfcamss->csi_num; i++) {
+		ret = stf_csi_subdev_init(stfcamss, i);
+		if (ret < 0) {
+			st_err(ST_CAMSS,
+				"Failed to init stf_csi sub-device: %d\n",
+				ret);
+			return ret;
+		}
+	}
+#endif
+
+	for (i = 0; i < stfcamss->isp_num; i++) {
+		ret = stf_isp_subdev_init(stfcamss, i);
+		if (ret < 0) {
+			st_err(ST_CAMSS,
+				"Failed to init stf_isp sub-device: %d\n",
+				ret);
+			return ret;
+		}
+	}
+
+	ret = stf_vin_subdev_init(stfcamss);
+	if (ret < 0) {
+		st_err(ST_CAMSS,
+			"Failed to init stf_vin sub-device: %d\n",
+			ret);
+		return ret;
+	}
+
+	return ret;
+}
+
+static int stfcamss_register_subdevices(struct stfcamss *stfcamss)
+{
+	int ret, i, j;
+	struct stf_vin2_dev *vin_dev = stfcamss->vin_dev;
+	struct stf_dvp_dev *dvp_dev = stfcamss->dvp_dev;
+	struct stf_csiphy_dev *csiphy_dev = stfcamss->csiphy_dev;
+	struct stf_csi_dev *csi_dev = stfcamss->csi_dev;
+	struct stf_isp_dev *isp_dev = stfcamss->isp_dev;
+
+	ret = stf_dvp_register(dvp_dev, &stfcamss->v4l2_dev);
+	if (ret < 0) {
+		st_err(ST_CAMSS,
+			"Failed to register stf dvp entity: %d\n",
+			ret);
+		goto err_reg_dvp;
+	}
+
+	for (i = 0; i < stfcamss->csiphy_num; i++) {
+		ret = stf_csiphy_register(&csiphy_dev[i],
+				&stfcamss->v4l2_dev);
+		if (ret < 0) {
+			st_err(ST_CAMSS,
+				"Failed to register stf csiphy%d entity: %d\n",
+				i, ret);
+			goto err_reg_csiphy;
+		}
+	}
+
+#ifndef CONFIG_VIDEO_CADENCE_CSI2RX
+	for (i = 0; i < stfcamss->csi_num; i++) {
+		ret = stf_csi_register(&csi_dev[i],
+				&stfcamss->v4l2_dev);
+		if (ret < 0) {
+			st_err(ST_CAMSS,
+				"Failed to register stf csi%d entity: %d\n",
+				i, ret);
+			goto err_reg_csi;
+		}
+	}
+#endif
+
+	for (i = 0; i < stfcamss->isp_num; i++) {
+		ret = stf_isp_register(&isp_dev[i],
+				&stfcamss->v4l2_dev);
+		if (ret < 0) {
+			st_err(ST_CAMSS,
+				"Failed to register stf isp%d entity: %d\n",
+				i, ret);
+			goto err_reg_isp;
+		}
+	}
+
+	ret = stf_vin_register(vin_dev, &stfcamss->v4l2_dev);
+	if (ret < 0) {
+		st_err(ST_CAMSS,
+			"Failed to register vin entity: %d\n",
+			 ret);
+		goto err_reg_vin;
+	}
+
+	ret = media_create_pad_link(
+		&dvp_dev->subdev.entity,
+		STF_DVP_PAD_SRC,
+		&vin_dev->line[VIN_LINE_WR].subdev.entity,
+		STF_VIN_PAD_SINK,
+		0);
+	if (ret < 0) {
+		st_err(ST_CAMSS,
+			"Failed to link %s->vin entities: %d\n",
+			dvp_dev->subdev.entity.name,
+			ret);
+		goto err_link;
+	}
+
+#ifndef CONFIG_VIDEO_CADENCE_CSI2RX
+	for (i = 0; i < stfcamss->csi_num; i++) {
+		ret = media_create_pad_link(
+			&csi_dev[i].subdev.entity,
+			STF_CSI_PAD_SRC,
+			&vin_dev->line[VIN_LINE_WR].subdev.entity,
+			STF_VIN_PAD_SINK,
+			0);
+		if (ret < 0) {
+			st_err(ST_CAMSS,
+				"Failed to link %s->vin entities: %d\n",
+				csi_dev[i].subdev.entity.name,
+				ret);
+			goto err_link;
+		}
+	}
+
+	for (j = 0; j < stfcamss->csi_num; j++) {
+		ret = media_create_pad_link(
+			&csiphy_dev[j].subdev.entity,
+			STF_CSIPHY_PAD_SRC,
+			&csi_dev[j].subdev.entity,
+			STF_CSI_PAD_SINK,
+			0);
+		if (ret < 0) {
+			st_err(ST_CAMSS,
+				"Failed to link %s->%s entities: %d\n",
+				csiphy_dev[j].subdev.entity.name,
+				csi_dev[j].subdev.entity.name,
+				ret);
+			goto err_link;
+		}
+	}
+#endif
+	for (i = 0; i < stfcamss->isp_num; i++) {
+		ret = media_create_pad_link(
+			&isp_dev[i].subdev.entity,
+			STF_ISP_PAD_SRC,
+			&vin_dev->line[i + VIN_LINE_ISP0].subdev.entity,
+			STF_VIN_PAD_SINK,
+			0);
+		if (ret < 0) {
+			st_err(ST_CAMSS,
+				"Failed to link %s->%s entities: %d\n",
+				isp_dev[i].subdev.entity.name,
+				vin_dev->line[i + VIN_LINE_ISP0]
+				.subdev.entity.name,
+				ret);
+			goto err_link;
+		}
+
+		ret = media_create_pad_link(
+			&isp_dev[i].subdev.entity,
+			STF_ISP_PAD_SRC,
+			&vin_dev->line[i + VIN_LINE_ISP0_RAW].subdev.entity,
+			STF_VIN_PAD_SINK,
+			0);
+		if (ret < 0) {
+			st_err(ST_CAMSS,
+				"Failed to link %s->%s entities: %d\n",
+				isp_dev[i].subdev.entity.name,
+				vin_dev->line[i + VIN_LINE_ISP0_RAW]
+				.subdev.entity.name,
+				ret);
+			goto err_link;
+		}
+
+		ret = media_create_pad_link(
+			&dvp_dev->subdev.entity,
+			STF_DVP_PAD_SRC,
+			&isp_dev[i].subdev.entity,
+			STF_ISP_PAD_SINK,
+			0);
+		if (ret < 0) {
+			st_err(ST_CAMSS,
+				"Failed to link %s->%s entities: %d\n",
+				dvp_dev->subdev.entity.name,
+				isp_dev[i].subdev.entity.name,
+			ret);
+			goto err_link;
+		}
+	#ifndef CONFIG_VIDEO_CADENCE_CSI2RX
+		for (j = 0; j < stfcamss->csi_num; j++) {
+			ret = media_create_pad_link(
+				&csi_dev[j].subdev.entity,
+				STF_CSI_PAD_SRC,
+				&isp_dev[i].subdev.entity,
+				STF_ISP_PAD_SINK,
+				0);
+			if (ret < 0) {
+				st_err(ST_CAMSS,
+					"Failed to link %s->%s entities: %d\n",
+					csi_dev[j].subdev.entity.name,
+					isp_dev[i].subdev.entity.name,
+					ret);
+				goto err_link;
+			}
+		}
+	#endif
+	}
+
+	return ret;
+
+err_link:
+	stf_vin_unregister(stfcamss->vin_dev);
+err_reg_vin:
+	i = stfcamss->isp_num;
+err_reg_isp:
+	for (i--; i >= 0; i--)
+		stf_isp_unregister(&stfcamss->isp_dev[i]);
+
+#ifndef CONFIG_VIDEO_CADENCE_CSI2RX
+	i = stfcamss->csi_num;
+err_reg_csi:
+	for (i--; i >= 0; i--)
+		stf_csi_unregister(&stfcamss->csi_dev[i]);
+#endif
+
+	i = stfcamss->csiphy_num;
+err_reg_csiphy:
+	for (i--; i >= 0; i--)
+		stf_csiphy_unregister(&stfcamss->csiphy_dev[i]);
+
+	stf_dvp_unregister(stfcamss->dvp_dev);
+err_reg_dvp:
+	return ret;
+}
+
+static void stfcamss_unregister_subdevices(struct stfcamss *stfcamss)
+{
+	int i;
+
+	stf_dvp_unregister(stfcamss->dvp_dev);
+
+	i = stfcamss->csiphy_num;
+	for (i--; i >= 0; i--)
+		stf_csiphy_unregister(&stfcamss->csiphy_dev[i]);
+
+#ifndef CONFIG_VIDEO_CADENCE_CSI2RX
+	i = stfcamss->csi_num;
+	for (i--; i >= 0; i--)
+		stf_csi_unregister(&stfcamss->csi_dev[i]);
+#endif
+
+	i = stfcamss->isp_num;
+	for (i--; i >= 0; i--)
+		stf_isp_unregister(&stfcamss->isp_dev[i]);
+
+	stf_vin_unregister(stfcamss->vin_dev);
+}
+
+static int stfcamss_register_mediadevice_subdevnodes(
+		struct v4l2_async_notifier *async,
+		struct v4l2_subdev *sd)
+{
+	struct stfcamss *stfcamss =
+		container_of(async, struct stfcamss, notifier);
+	int ret;
+
+	if (sd->host_priv) {
+		struct media_entity *sensor = &sd->entity;
+		struct media_entity *input = sd->host_priv;
+		unsigned int i;
+
+		for (i = 0; i < sensor->num_pads; i++) {
+			if (sensor->pads[i].flags & MEDIA_PAD_FL_SOURCE)
+				break;
+		}
+		if (i == sensor->num_pads) {
+			st_err(ST_CAMSS,
+				"No source pad in external entity\n");
+			return -EINVAL;
+		}
+
+		ret = media_create_pad_link(sensor, i,
+			input, STF_PAD_SINK,
+			MEDIA_LNK_FL_IMMUTABLE | MEDIA_LNK_FL_ENABLED);
+		if (ret < 0) {
+			st_err(ST_CAMSS,
+				"Failed to link %s->%s entities: %d\n",
+				sensor->name, input->name, ret);
+			return ret;
+		}
+	}
+
+	ret = v4l2_device_register_subdev_nodes(&stfcamss->v4l2_dev);
+	if (ret < 0)
+		return ret;
+
+	if (stfcamss->media_dev.devnode)
+		return ret;
+
+	st_debug(ST_CAMSS, "stfcamss register media device\n");
+	return media_device_register(&stfcamss->media_dev);
+}
+
+static int stfcamss_subdev_notifier_bound(struct v4l2_async_notifier *async,
+					struct v4l2_subdev *subdev,
+					struct v4l2_async_subdev *asd)
+{
+	struct stfcamss *stfcamss = container_of(async, struct stfcamss, notifier);
+	struct stfcamss_async_subdev *csd =
+		container_of(asd, struct stfcamss_async_subdev, asd);
+	enum port_num port = csd->port;
+	struct stf_dvp_dev *dvp_dev = stfcamss->dvp_dev;
+	struct stf_csiphy_dev *csiphy_dev = stfcamss->csiphy_dev;
+	struct stf_isp_dev *isp_dev = stfcamss->isp_dev;
+	u32 id;
+
+	switch (port) {
+	case CSI2RX0_PORT_NUMBER:
+	case CSI2RX1_PORT_NUMBER:
+		id = port - CSI2RX0_PORT_NUMBER;
+		isp_dev = &isp_dev[id];
+		subdev->host_priv = &isp_dev->subdev.entity;
+		break;
+	case DVP_SENSOR_PORT_NUMBER:
+		dvp_dev->dvp = &csd->interface.dvp;
+		subdev->host_priv = &dvp_dev->subdev.entity;
+		break;
+	case CSI2RX0_SENSOR_PORT_NUMBER:
+	case CSI2RX1_SENSOR_PORT_NUMBER:
+		id = port - CSI2RX0_SENSOR_PORT_NUMBER;
+		csiphy_dev = &csiphy_dev[id];
+		csiphy_dev->csiphy = &csd->interface.csiphy;
+		subdev->host_priv = &csiphy_dev->subdev.entity;
+		break;
+	default:
+		break;
+	};
+
+	stfcamss_register_mediadevice_subdevnodes(async, subdev);
+
+	return 0;
+}
+
+#if 0
+static int stfcamss_subdev_notifier_complete(
+		struct v4l2_async_notifier *async)
+{
+	struct stfcamss *stfcamss = container_of(async, struct stfcamss, notifier);
+	struct v4l2_device *v4l2_dev = &stfcamss->v4l2_dev;
+	struct v4l2_subdev *sd;
+	int ret;
+
+	list_for_each_entry(sd, &v4l2_dev->subdevs, list) {
+		if (sd->host_priv) {
+			struct media_entity *sensor = &sd->entity;
+			struct media_entity *input = sd->host_priv;
+			unsigned int i;
+
+			for (i = 0; i < sensor->num_pads; i++) {
+				if (sensor->pads[i].flags & MEDIA_PAD_FL_SOURCE)
+					break;
+			}
+			if (i == sensor->num_pads) {
+				st_err(ST_CAMSS,
+					"No source pad in external entity\n");
+				return -EINVAL;
+			}
+
+			ret = media_create_pad_link(sensor, i, input, STF_PAD_SINK,
+				MEDIA_LNK_FL_IMMUTABLE | MEDIA_LNK_FL_ENABLED);
+			if (ret < 0) {
+				st_err(ST_CAMSS,
+					"Failed to link %s->%s entities: %d\n",
+					sensor->name, input->name, ret);
+				return ret;
+			}
+		}
+	}
+
+	ret = v4l2_device_register_subdev_nodes(&stfcamss->v4l2_dev);
+	if (ret < 0)
+		return ret;
+
+	return media_device_register(&stfcamss->media_dev);
+}
+#endif
+
+static const struct v4l2_async_notifier_operations
+stfcamss_subdev_notifier_ops = {
+	.bound = stfcamss_subdev_notifier_bound,
+	// .complete = stfcamss_subdev_notifier_complete,
+};
+
+static const struct media_device_ops stfcamss_media_ops = {
+	.link_notify = v4l2_pipeline_link_notify,
+};
+
+#ifdef CONFIG_DEBUG_FS
+enum module_id {
+	VIN_MODULE = 0,
+	ISP0_MODULE,
+	ISP1_MODULE,
+	CSI0_MODULE,
+	CSI1_MODULE,
+	CSIPHY_MODULE,
+	DVP_MODULE,
+	CLK_MODULE,
+};
+
+static enum module_id id_num = ISP0_MODULE;
+
+void dump_clk_reg(void __iomem *reg_base)
+{
+	int i;
+
+	st_info(ST_CAMSS, "DUMP Clk register:\n");
+	for (i = 0; i <= CLK_C_ISP1_CTRL; i += 4)
+		print_reg(ST_CAMSS, reg_base, i);
+}
+
+static ssize_t vin_debug_read(struct file *file, char __user *user_buf,
+			size_t count, loff_t *ppos)
+{
+	struct device *dev = file->private_data;
+	void __iomem *reg_base;
+	struct stfcamss *stfcamss = dev_get_drvdata(dev);
+	struct stf_vin_dev *vin = stfcamss->vin;
+	struct stf_vin2_dev *vin_dev = stfcamss->vin_dev;
+	struct stf_isp_dev *isp0_dev = &stfcamss->isp_dev[0];
+	struct stf_isp_dev *isp1_dev = &stfcamss->isp_dev[1];
+	struct stf_csi_dev *csi0_dev = &stfcamss->csi_dev[0];
+	struct stf_csi_dev *csi1_dev = &stfcamss->csi_dev[1];
+
+	switch (id_num) {
+	case VIN_MODULE:
+	case CSIPHY_MODULE:
+	case DVP_MODULE:
+		mutex_lock(&vin_dev->power_lock);
+		if (vin_dev->power_count > 0) {
+			reg_base = vin->sysctrl_base;
+			dump_vin_reg(reg_base);
+		}
+		mutex_unlock(&vin_dev->power_lock);
+		break;
+	case ISP0_MODULE:
+		mutex_lock(&isp0_dev->stream_lock);
+		if (isp0_dev->stream_count > 0) {
+			reg_base = vin->isp_isp0_base;
+			dump_isp_reg(reg_base, 0);
+		}
+		mutex_unlock(&isp0_dev->stream_lock);
+		break;
+	case ISP1_MODULE:
+		mutex_lock(&isp1_dev->stream_lock);
+		if (isp1_dev->stream_count > 0) {
+			reg_base = vin->isp_isp1_base;
+			dump_isp_reg(reg_base, 1);
+		}
+		mutex_unlock(&isp1_dev->stream_lock);
+		break;
+	case CSI0_MODULE:
+		mutex_lock(&csi0_dev->stream_lock);
+		if (csi0_dev->stream_count > 0) {
+			reg_base = vin->mipi0_base;
+			dump_csi_reg(reg_base, 0);
+		}
+		mutex_unlock(&csi0_dev->stream_lock);
+		break;
+	case CSI1_MODULE:
+		mutex_lock(&csi1_dev->stream_lock);
+		if (csi1_dev->stream_count > 0) {
+			reg_base = vin->mipi1_base;
+			dump_csi_reg(reg_base, 1);
+		}
+		mutex_unlock(&csi1_dev->stream_lock);
+		break;
+	case CLK_MODULE:
+		mutex_lock(&vin_dev->power_lock);
+		if (vin_dev->power_count > 0) {
+			reg_base = vin->clkgen_base;
+			dump_clk_reg(reg_base);
+		}
+		mutex_unlock(&vin_dev->power_lock);
+		break;
+	default:
+		break;
+	}
+
+	return 0;
+}
+
+static void set_reg_val(struct stfcamss *stfcamss, int id, u32 offset, u32 val)
+{
+	struct stf_vin_dev *vin = stfcamss->vin;
+	struct stf_vin2_dev *vin_dev = stfcamss->vin_dev;
+	struct stf_isp_dev *isp0_dev = &stfcamss->isp_dev[0];
+	struct stf_isp_dev *isp1_dev = &stfcamss->isp_dev[1];
+	struct stf_csi_dev *csi0_dev = &stfcamss->csi_dev[0];
+	struct stf_csi_dev *csi1_dev = &stfcamss->csi_dev[1];
+	void __iomem *reg_base;
+
+	switch (id) {
+	case VIN_MODULE:
+	case CSIPHY_MODULE:
+	case DVP_MODULE:
+		mutex_lock(&vin_dev->power_lock);
+		if (vin_dev->power_count > 0) {
+			reg_base = vin->sysctrl_base;
+			print_reg(ST_VIN, reg_base, offset);
+			reg_write(reg_base, offset, val);
+			print_reg(ST_VIN, reg_base, offset);
+		}
+		mutex_unlock(&vin_dev->power_lock);
+		break;
+	case ISP0_MODULE:
+		mutex_lock(&isp0_dev->stream_lock);
+		if (isp0_dev->stream_count > 0) {
+			reg_base = vin->isp_isp0_base;
+			print_reg(ST_ISP, reg_base, offset);
+			reg_write(reg_base, offset, val);
+			print_reg(ST_ISP, reg_base, offset);
+		}
+		mutex_unlock(&isp0_dev->stream_lock);
+		break;
+	case ISP1_MODULE:
+		mutex_lock(&isp1_dev->stream_lock);
+		if (isp1_dev->stream_count > 0) {
+			reg_base = vin->isp_isp1_base;
+			print_reg(ST_ISP, reg_base, offset);
+			reg_write(reg_base, offset, val);
+			print_reg(ST_ISP, reg_base, offset);
+		}
+		mutex_unlock(&isp1_dev->stream_lock);
+		break;
+	case CSI0_MODULE:
+		mutex_lock(&csi0_dev->stream_lock);
+		if (csi0_dev->stream_count > 0) {
+			reg_base = vin->mipi0_base;
+			print_reg(ST_CSI, reg_base, offset);
+			reg_write(reg_base, offset, val);
+			print_reg(ST_CSI, reg_base, offset);
+		}
+		mutex_unlock(&csi0_dev->stream_lock);
+		break;
+	case CSI1_MODULE:
+		mutex_lock(&csi1_dev->stream_lock);
+		if (csi1_dev->stream_count > 0) {
+			reg_base = vin->mipi1_base;
+			print_reg(ST_CSI, reg_base, offset);
+			reg_write(reg_base, offset, val);
+			print_reg(ST_CSI, reg_base, offset);
+		}
+		mutex_unlock(&csi1_dev->stream_lock);
+		break;
+	case CLK_MODULE:
+		mutex_lock(&vin_dev->power_lock);
+		if (vin_dev->power_count > 0) {
+			reg_base = vin->clkgen_base;
+			print_reg(ST_CAMSS, reg_base, offset);
+			reg_write(reg_base, offset, val);
+			print_reg(ST_CAMSS, reg_base, offset);
+		}
+		mutex_unlock(&vin_dev->power_lock);
+		break;
+	default:
+		break;
+
+	}
+}
+
+static u32 atoi(const char *s)
+{
+	u32 ret = 0, d = 0;
+	char ch;
+	int hex = 0;
+
+	if ((*s == '0') && (*(s+1) == 'x')) {
+		hex = 1;
+		s += 2;
+	}
+
+	while (1) {
+		if (!hex) {
+			d = (*s++) - '0';
+			if (d > 9)
+				break;
+			ret *= 10;
+			ret += d;
+		} else {
+			ch = tolower(*s++);
+			if (isdigit(ch))
+				d = ch - '0';
+			else if (islower(ch))
+				d = ch - 'a' + 10;
+			else
+				break;
+			if (d > 15)
+				break;
+			ret *= 16;
+			ret += d;
+		}
+	}
+
+	return ret;
+}
+
+static ssize_t vin_debug_write(struct file *file, const char __user *user_buf,
+				size_t count, loff_t *ppos)
+{
+	struct device *dev = file->private_data;
+	struct stfcamss *stfcamss = dev_get_drvdata(dev);
+	char *buf;
+	char *line;
+	char *p;
+	static const char *delims = " \t\r";
+	char *token;
+	u32 offset, val;
+
+	buf = memdup_user_nul(user_buf, min_t(size_t, PAGE_SIZE, count));
+	if (IS_ERR(buf))
+		return PTR_ERR(buf);
+	p = buf;
+	st_debug(ST_CAMSS, "dup buf: %s, len: %ld, count: %ld\n", p, strlen(p), count);
+	while (p && *p) {
+		p = skip_spaces(p);
+		line = strsep(&p, "\n");
+		if (!*line || *line == '#')
+			break;
+		token = strsep(&line, delims);
+		if (!token)
+			goto out;
+		id_num = atoi(token);
+		token = strsep(&line, delims);
+		if (!token)
+			goto out;
+		offset = atoi(token);
+		token = strsep(&line, delims);
+		if (!token)
+			goto out;
+		val = atoi(token);
+	}
+	set_reg_val(stfcamss, id_num, offset, val);
+out:
+	kfree(buf);
+	st_info(ST_CAMSS, "id_num = %d, offset = 0x%x, 0x%x\n", id_num, offset, val);
+	return count;
+}
+
+static const struct file_operations vin_debug_fops = {
+	.open = simple_open,
+	.read = vin_debug_read,
+	.write = vin_debug_write,
+};
+#endif /* CONFIG_DEBUG_FS */
+
+
+static int stfcamss_probe(struct platform_device *pdev)
+{
+	struct stfcamss *stfcamss;
+	struct stf_vin_dev *vin;
+	struct device *dev = &pdev->dev;
+	int ret = 0, i, num_subdevs;
+
+	st_info(ST_CAMSS, "stfcamss probe enter!\n");
+
+	stfcamss = devm_kzalloc(dev, sizeof(struct stfcamss), GFP_KERNEL);
+	if (!stfcamss)
+		return -ENOMEM;
+
+	stfcamss->isp_num = 2;
+	stfcamss->csi_num = 2;
+#ifndef CONFIG_VIDEO_CADENCE_CSI2RX
+	stfcamss->csiphy_num = 2;
+#else
+	stfcamss->csiphy_num = 1;
+#endif
+
+	stfcamss->dvp_dev = devm_kzalloc(dev,
+		sizeof(*stfcamss->dvp_dev), GFP_KERNEL);
+	if (!stfcamss->dvp_dev) {
+		ret = -ENOMEM;
+		goto err_cam;
+	}
+
+	stfcamss->csiphy_dev = devm_kzalloc(dev,
+		stfcamss->csiphy_num * sizeof(*stfcamss->csiphy_dev),
+		GFP_KERNEL);
+	if (!stfcamss->csiphy_dev) {
+		ret = -ENOMEM;
+		goto err_cam;
+	}
+
+	stfcamss->csi_dev = devm_kzalloc(dev,
+		stfcamss->csi_num * sizeof(*stfcamss->csi_dev),
+		GFP_KERNEL);
+	if (!stfcamss->csi_dev) {
+		ret = -ENOMEM;
+		goto err_cam;
+	}
+
+	stfcamss->isp_dev = devm_kzalloc(dev,
+		stfcamss->isp_num * sizeof(*stfcamss->isp_dev),
+		GFP_KERNEL);
+	if (!stfcamss->isp_dev) {
+		ret = -ENOMEM;
+		goto err_cam;
+	}
+
+	stfcamss->vin_dev = devm_kzalloc(dev,
+		sizeof(*stfcamss->vin_dev),
+		GFP_KERNEL);
+	if (!stfcamss->vin_dev) {
+		ret = -ENOMEM;
+		goto err_cam;
+	}
+
+	stfcamss->vin = devm_kzalloc(dev,
+		sizeof(struct stf_vin_dev),
+		GFP_KERNEL);
+	if (!stfcamss->vin) {
+		ret = -ENOMEM;
+		goto err_cam;
+	}
+
+	vin = stfcamss->vin;
+
+	vin->irq = platform_get_irq(pdev, 0);
+	if (vin->irq <= 0) {
+		st_err(ST_CAMSS, "Could not get irq\n");
+		goto err_cam;
+	}
+
+	vin->isp0_irq = platform_get_irq(pdev, 1);
+	if (vin->isp0_irq <= 0) {
+		st_err(ST_CAMSS, "Could not get isp0 irq\n");
+		goto err_cam;
+	}
+
+	vin->isp1_irq = platform_get_irq(pdev, 2);
+	if (vin->isp1_irq <= 0) {
+		st_err(ST_CAMSS, "Could not get isp1 irq\n");
+		goto err_cam;
+	}
+
+	/* Clocks */
+
+	stfcamss->nclks = 0;
+	while (clocks[stfcamss->nclks])
+		stfcamss->nclks++;
+
+	st_info(ST_CAMSS, "stfcamss->nclks %d\n", stfcamss->nclks);
+	stfcamss->sys_clk = devm_kzalloc(dev, stfcamss->nclks * sizeof(*stfcamss->sys_clk),
+			GFP_KERNEL);
+	if (!stfcamss->sys_clk) {
+		ret = -ENOMEM;
+		goto err_cam;
+	}
+
+	for (i = 0; i < stfcamss->nclks; i++) {
+		struct stfcamss_clk *clock = &stfcamss->sys_clk[i];
+
+		clock->clk = devm_clk_get(dev, clocks[i]);
+		if (IS_ERR(clock->clk)) {
+			ret = -ENOMEM;
+			st_err(ST_CAMSS, "get %s clocks name failed\n", clocks[i]);
+			goto err_cam_clk;
+		}
+		st_debug(ST_CAMSS, "get %s clocks name: \n", clocks[i]);
+
+		clock->name = clocks[i];
+	}
+
+	ret = stfcamss_get_mem_res(pdev, vin);
+	if (ret) {
+		st_err(ST_CAMSS, "Could not map registers\n");
+		goto err_cam_clk;
+	}
+
+	ret = vin_parse_dt(dev, vin);
+	if (ret)
+		goto err_cam_clk;
+
+	vin->dev = dev;
+	stfcamss->dev = dev;
+	platform_set_drvdata(pdev, stfcamss);
+
+	v4l2_async_notifier_init(&stfcamss->notifier);
+
+	num_subdevs = stfcamss_of_parse_ports(stfcamss);
+	if (num_subdevs < 0) {
+		ret = num_subdevs;
+		goto err_cam_noti;
+	}
+
+	ret = stfcamss_init_subdevices(stfcamss);
+	if (ret < 0) {
+		st_err(ST_CAMSS, "Failed to init subdevice: %d\n", ret);
+		goto err_cam_noti;
+	}
+
+	stfcamss->media_dev.dev = stfcamss->dev;
+	strscpy(stfcamss->media_dev.model, "Starfive Camera Subsystem",
+		sizeof(stfcamss->media_dev.model));
+	stfcamss->media_dev.ops = &stfcamss_media_ops;
+	media_device_init(&stfcamss->media_dev);
+
+	stfcamss->v4l2_dev.mdev = &stfcamss->media_dev;
+
+	ret = v4l2_device_register(stfcamss->dev, &stfcamss->v4l2_dev);
+	if (ret < 0) {
+		st_err(ST_CAMSS, "Failed to register V4L2 device: %d\n", ret);
+		goto err_cam_noti_med;
+	}
+
+	ret = stfcamss_register_subdevices(stfcamss);
+	if (ret < 0) {
+		st_err(ST_CAMSS, "Failed to register subdevice: %d\n", ret);
+		goto err_cam_noti_med_vreg;
+	}
+
+	if (num_subdevs) {
+		stfcamss->notifier.ops = &stfcamss_subdev_notifier_ops;
+		ret = v4l2_async_notifier_register(&stfcamss->v4l2_dev,
+				&stfcamss->notifier);
+		if (ret) {
+			st_err(ST_CAMSS,
+				"Failed to register async subdev nodes: %d\n",
+				ret);
+			goto err_cam_noti_med_vreg_sub;
+		}
+	} else {
+		ret = v4l2_device_register_subdev_nodes(&stfcamss->v4l2_dev);
+		if (ret < 0) {
+			st_err(ST_CAMSS, "Failed to register subdev nodes: %d\n", ret);
+			goto err_cam_noti_med_vreg_sub;
+		}
+
+		ret = media_device_register(&stfcamss->media_dev);
+		if (ret < 0) {
+			st_err(ST_CAMSS, "Failed to register media device: %d\n", ret);
+			goto err_cam_noti_med_vreg_sub_medreg;
+		}
+	}
+
+#ifdef CONFIG_DEBUG_FS
+	stfcamss->debugfs_entry = debugfs_create_dir("stfcamss", NULL);
+	stfcamss->vin_debugfs = debugfs_create_file("stf_vin",
+			S_IRUGO | S_IWUSR, stfcamss->debugfs_entry,
+			(void *)dev, &vin_debug_fops);
+	debugfs_create_u32("dbg_level",
+			S_IRUGO | S_IWUSR, stfcamss->debugfs_entry,
+			&stdbg_level);
+	debugfs_create_u32("dbg_mask",
+			S_IRUGO | S_IWUSR, stfcamss->debugfs_entry,
+			&stdbg_mask);
+#endif
+
+	return 0;
+
+#ifdef CONFIG_DEBUG_FS
+	debugfs_remove(stfcamss->vin_debugfs);
+	debugfs_remove_recursive(stfcamss->debugfs_entry);
+	stfcamss->debugfs_entry = NULL;
+#endif
+
+err_cam_noti_med_vreg_sub_medreg:
+err_cam_noti_med_vreg_sub:
+	stfcamss_unregister_subdevices(stfcamss);
+err_cam_noti_med_vreg:
+	v4l2_device_unregister(&stfcamss->v4l2_dev);
+err_cam_noti_med:
+	media_device_cleanup(&stfcamss->media_dev);
+err_cam_noti:
+	v4l2_async_notifier_cleanup(&stfcamss->notifier);
+	i = stfcamss->nclks;
+err_cam_clk:
+	for (; i > 0; i--) {
+		struct stfcamss_clk *clock = &stfcamss->sys_clk[i];
+
+		devm_clk_put(dev, clock->clk);
+		st_debug(ST_CAMSS, "put %s clocks\n", clock->name);
+	}
+
+err_cam:
+	// kfree(stfcamss);
+	return ret;
+}
+
+static int stfcamss_remove(struct platform_device *pdev)
+{
+	struct stfcamss *stfcamss = platform_get_drvdata(pdev);
+
+	dev_info(&pdev->dev, "remove done\n");
+
+#ifdef CONFIG_DEBUG_FS
+	debugfs_remove(stfcamss->vin_debugfs);
+	debugfs_remove_recursive(stfcamss->debugfs_entry);
+	stfcamss->debugfs_entry = NULL;
+#endif
+
+	stfcamss_unregister_subdevices(stfcamss);
+	v4l2_device_unregister(&stfcamss->v4l2_dev);
+	media_device_cleanup(&stfcamss->media_dev);
+
+	kfree(stfcamss);
+
+	return 0;
+}
+
+static const struct of_device_id stfcamss_of_match[] = {
+	{.compatible = "starfive,stf-vin"},
+	{ /* end node */ },
+};
+
+MODULE_DEVICE_TABLE(of, stfcamss_of_match);
+
+static struct platform_driver stfcamss_driver = {
+	.probe = stfcamss_probe,
+	.remove = stfcamss_remove,
+	.driver = {
+		.name = DRV_NAME,
+		.of_match_table = of_match_ptr(stfcamss_of_match),
+	},
+};
+
+static int __init stfcamss_init(void)
+{
+	return platform_driver_register(&stfcamss_driver);
+}
+
+static void __exit stfcamss_cleanup(void)
+{
+	platform_driver_unregister(&stfcamss_driver);
+}
+
+module_init(stfcamss_init);
+//fs_initcall(stfcamss_init);
+module_exit(stfcamss_cleanup);
+
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/platform/starfive/stfcamss.h b/drivers/media/platform/starfive/stfcamss.h
new file mode 100644
index 000000000000..df23ad86cdac
--- /dev/null
+++ b/drivers/media/platform/starfive/stfcamss.h
@@ -0,0 +1,115 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021 StarFive Technology Co., Ltd.
+ */
+#ifndef STFCAMSS_H
+#define STFCAMSS_H
+
+#include <linux/io.h>
+#include <linux/delay.h>
+
+enum sensor_type {
+	SENSOR_VIN,
+	SENSOR_ISP0,  // need replace sensor
+	SENSOR_ISP1,  // need replace sensor
+};
+
+enum subdev_type {
+	VIN_DEV_TYPE,
+	ISP0_DEV_TYPE,
+	ISP1_DEV_TYPE,
+};
+
+#include "stf_common.h"
+#include "stf_dvp.h"
+#include "stf_csi.h"
+#include "stf_csiphy.h"
+#include "stf_isp.h"
+#include "stf_vin.h"
+
+#define STF_PAD_SINK   0
+#define STF_PAD_SRC    1
+#define STF_PADS_NUM   2
+
+enum port_num {
+	CSI2RX0_PORT_NUMBER = 0,
+	CSI2RX1_PORT_NUMBER,
+	DVP_SENSOR_PORT_NUMBER,
+	CSI2RX0_SENSOR_PORT_NUMBER,
+	CSI2RX1_SENSOR_PORT_NUMBER
+};
+
+enum stf_clk_num {
+	STFCLK_VIN_SRC = 0,
+	STFCLK_ISP0_AXI,
+	STFCLK_ISP0NOC_AXI,
+	STFCLK_ISPSLV_AXI,
+	STFCLK_ISP1_AXI,
+	STFCLK_ISP1NOC_AXI,
+	STFCLK_VIN_AXI,
+	STFCLK_VINNOC_AXI,
+	STFCLK_CSI_2RX_APH_CLK,
+	STFCLK_MIPIRX0_PIXEL0,
+	STFCLK_MIPIRX0_PIXEL1,
+	STFCLK_MIPIRX0_PIXEL2,
+	STFCLK_MIPIRX0_PIXEL3,
+	STFCLK_MIPIRX0_SYS,
+	STFCLK_MIPIRX1_PIXEL0,
+	STFCLK_MIPIRX1_PIXEL1,
+	STFCLK_MIPIRX1_PIXEL2,
+	STFCLK_MIPIRX1_PIXEL3,
+	STFCLK_MIPIRX1_SYS,
+	STFCLK_CSIDPHY_CFGCLK,
+	STFCLK_CSIDPHY_REFCLK,
+	STFCLK_CSIDPHY_TXCLKESC,
+	STFCLK_ISP0_CTRL,
+	STFCLK_ISP0_2X_CTRL,
+	STFCLK_ISP0_MIPI_CTRL,
+	STFCLK_ISP1_CTRL,
+	STFCLK_ISP1_2X_CTRL,
+	STFCLK_ISP1_MIPI_CTRL,
+	STFCLK_NUM
+};
+
+struct stfcamss_clk {
+	struct clk *clk;
+	const char *name;
+};
+
+struct stfcamss {
+	struct stf_vin_dev *vin;  // stfcamss phy res
+	struct v4l2_device v4l2_dev;
+	struct media_device media_dev;
+	struct device *dev;
+	struct stf_vin2_dev *vin_dev;  // subdev
+	struct stf_dvp_dev *dvp_dev;   // subdev
+	int csi_num;
+	struct stf_csi_dev *csi_dev;   // subdev
+	int csiphy_num;
+	struct stf_csiphy_dev *csiphy_dev;   // subdev
+	int isp_num;
+	struct stf_isp_dev *isp_dev;   // subdev
+	struct v4l2_async_notifier notifier;
+	struct stfcamss_clk *sys_clk;
+	int nclks;
+#ifdef CONFIG_DEBUG_FS
+	struct dentry *debugfs_entry;
+	struct dentry *vin_debugfs;
+#endif
+};
+
+struct stfcamss_async_subdev {
+	struct v4l2_async_subdev asd;  // must be first
+	enum port_num port;
+	struct {
+		struct dvp_cfg dvp;
+		struct csi2phy_cfg csiphy;
+	} interface;
+};
+
+extern struct media_entity *stfcamss_find_sensor(struct media_entity *entity);
+extern int stfcamss_enable_clocks(int nclocks, struct stfcamss_clk *clock,
+			struct device *dev);
+extern void stfcamss_disable_clocks(int nclocks, struct stfcamss_clk *clock);
+
+#endif /* STFCAMSS_H */
diff --git a/drivers/phy/Kconfig b/drivers/phy/Kconfig
index 82b63e60c5a2..edffeb23609d 100644
--- a/drivers/phy/Kconfig
+++ b/drivers/phy/Kconfig
@@ -94,5 +94,6 @@ source "drivers/phy/tegra/Kconfig"
 source "drivers/phy/ti/Kconfig"
 source "drivers/phy/intel/Kconfig"
 source "drivers/phy/xilinx/Kconfig"
+source "drivers/phy/m31/Kconfig"
 
 endmenu
diff --git a/drivers/phy/Makefile b/drivers/phy/Makefile
index 01e9efffc726..0bba6d85a986 100644
--- a/drivers/phy/Makefile
+++ b/drivers/phy/Makefile
@@ -33,4 +33,5 @@ obj-y					+= allwinner/	\
 					   st/		\
 					   tegra/	\
 					   ti/		\
-					   xilinx/
+					   xilinx/	\
+					   m31/
diff --git a/drivers/phy/m31/Kconfig b/drivers/phy/m31/Kconfig
new file mode 100755
index 000000000000..e6e457669e70
--- /dev/null
+++ b/drivers/phy/m31/Kconfig
@@ -0,0 +1,15 @@
+# SPDX-License-Identifier: GPL-2.0-only
+#
+# Phy drivers for Starfive platforms
+#
+
+config PHY_M31_DPHY_RX0
+	tristate "Starfive M31 MIPI DPHY TX Driver"
+	select GENERIC_PHY_MIPI_DPHY
+	select GENERIC_PHY
+	help
+	  Enable this to support the starfive MIPI  DPHY TX0
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called phy-starfive-dphy-tx0.
+
diff --git a/drivers/phy/m31/Makefile b/drivers/phy/m31/Makefile
new file mode 100755
index 000000000000..7ce3391ac42f
--- /dev/null
+++ b/drivers/phy/m31/Makefile
@@ -0,0 +1,3 @@
+# SPDX-License-Identifier: GPL-2.0
+obj-$(CONFIG_PHY_M31_DPHY_RX0)     += phy-m31-dphy-tx0.o
+
diff --git a/drivers/phy/m31/phy-m31-dphy-tx0.c b/drivers/phy/m31/phy-m31-dphy-tx0.c
new file mode 100755
index 000000000000..89faa77896a5
--- /dev/null
+++ b/drivers/phy/m31/phy-m31-dphy-tx0.c
@@ -0,0 +1,521 @@
+// SPDX-License-Identifier: (GPL-2.0+ OR MIT)
+/*
+ * Rockchip MIPI Synopsys DPHY RX0 driver
+ *
+ * Copyright (C) 2019 Collabora, Ltd.
+ *
+ * Based on:
+ *
+ * drivers/media/platform/rockchip/isp1/mipi_dphy_sy.c
+ * in https://chromium.googlesource.com/chromiumos/third_party/kernel,
+ * chromeos-4.4 branch.
+ *
+ * Copyright (C) 2017 Fuzhou Rockchip Electronics Co., Ltd.
+ *   Jacob Chen <jacob2.chen@rock-chips.com>
+ *   Shunqian Zheng <zhengsq@rock-chips.com>
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/io.h>
+#include <linux/mfd/syscon.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/phy/phy.h>
+#include <linux/phy/phy-mipi-dphy.h>
+#include <linux/platform_device.h>
+#include <linux/regmap.h>
+
+//syscfg registers
+#define SCFG_DSI_CSI_SEL	        0x2c
+#define SCFG_PHY_RESETB	            0x30
+#define SCFG_REFCLK_SEL	            0x34
+#define SCFG_DBUS_PW_PLL_SSC_LD0	0x38
+#define SCFG_GRS_CDTX_PLL       	0x3c
+
+#define SCFG_RG_CDTX_PLL_FBK_PRE	0x44
+#define SCFG_RG_CLANE_DLANE_TIME   	0x58
+#define SCFG_RG_CLANE_HS_TIME   	0x58
+
+#define SCFG_RG_EXTD_CYCLE_SEL   	0x5c
+
+#define SCFG_L0N_L0P_HSTX	        0x60
+#define SCFG_L1N_L1P_HSTX	        0x64
+#define SCFG_L2N_L2P_HSTX	        0x68
+#define SCFG_L3N_L3P_HSTX	        0x6c
+#define SCFG_L4N_L4P_HSTX	        0x70
+#define SCFG_LX_SWAP_SEL	        0x78
+
+#define SCFG_HS_PRE_ZERO_T_D	    0xc4
+#define SCFG_TXREADY_SRC_SEL_D	    0xc8
+#define SCFG_HS_PRE_ZERO_T_C	    0xd4
+#define SCFG_TXREADY_SRC_SEL_C	    0xd8
+
+//reg SCFG_LX_SWAP_SEL
+#define	OFFSET_CFG_L0_SWAP_SEL 	0
+#define	OFFSET_CFG_L1_SWAP_SEL 	3
+#define	OFFSET_CFG_L2_SWAP_SEL 	6
+#define	OFFSET_CFG_L3_SWAP_SEL 	9
+#define OFFSET_CFG_L4_SWAP_SEL 	12
+
+//reg SCFG_DBUS_PW_PLL_SSC_LD0
+#define OFFSET_SCFG_CFG_DATABUD16_SEL    0
+#define OFFSET_SCFG_PWRON_READY_N        1
+#define OFFSET_RG_CDTX_PLL_FM_EN         2
+#define OFFSET_SCFG_PLLSSC_EN            3
+#define OFFSET_RG_CDTX_PLL_LDO_STB_X2_EN 4
+
+//reg SCFG_RG_CLANE_DLANE_TIME
+#define OFFSET_DHS_PRE_TIME          8
+#define OFFSET_DHS_TRIAL_TIME        16
+#define OFFSET_DHS_ZERO_TIME         24
+
+//reg SCFG_RG_CLANE_HS_TIME
+#define OFFSET_CHS_PRE_TIME          8
+#define OFFSET_CHS_TRIAL_TIME        16
+#define OFFSET_CHS_ZERO_TIME         24
+
+//dsitx registers
+#define  VID_MCTL_MAIN_DATA_CTL	        0x04
+#define  VID_MCTL_MAIN_PHY_CTL	        0x08
+#define  VID_MCTL_MAIN_EN	            0x0c
+#define  VID_MAIN_CTRL_ADDR    0xb0
+#define  VID_VSIZE1_ADDR       0xb4
+#define  VID_VSIZE2_ADDR       0xb8
+#define  VID_HSIZE1_ADDR       0xc0
+#define  VID_HSIZE2_ADDR       0xc4
+#define  VID_BLKSIZE1_ADDR     0xCC
+#define  VID_BLKSIZE2_ADDR     0xd0
+#define  VID_PCK_TIME_ADDR     0xd8
+#define  VID_DPHY_TIME_ADDR    0xdc
+#define  VID_ERR_COLOR1_ADDR   0xe0
+#define  VID_ERR_COLOR2_ADDR   0xe4
+#define  VID_VPOS_ADDR         0xe8
+#define  VID_HPOS_ADDR         0xec
+#define  VID_MODE_STAT_ADDR    0xf0
+#define  VID_VCA_SET1_ADDR     0xf4
+#define  VID_VCA_SET2_ADDR     0xf8
+
+
+#define  VID_MODE_STAT_CLR_ADDR    0x160
+#define  VID_MODE_STAT_FLAG_ADDR   0x180
+
+#define  TVG_CTRL_ADDR      0x0fc
+#define  TVG_IMG_SIZE_ADDR  0x100
+#define  TVG_COLOR1_ADDR    0x104
+#define  TVG_COLOR1BIT_ADDR 0x108
+#define  TVG_COLOR2_ADDR    0x10c
+#define  TVG_COLOR2BIT_ADDR 0x110
+#define  TVG_STAT_ADDR      0x114
+#define  TVG_STAT_CTRL_ADDR 0x144
+#define  TVG_STAT_CLR_ADDR  0x164
+#define  TVG_STAT_FLAG_ADDR 0x184
+
+#define  DPI_IRQ_EN_ADDR   0x1a0
+#define  DPI_IRQ_CLR_ADDR  0x1a4
+#define  DPI_IRQ_STAT_ADDR 0x1a4
+#define  DPI_CFG_ADDR      0x1ac
+
+
+//sysrst registers
+#define SRST_ASSERT0	    0x00
+#define SRST_STATUS0    	0x04
+/* Definition controller bit for syd rst registers */
+#define BIT_RST_DSI_DPI_PIX		17
+
+struct sf_dphy {
+	struct device *dev;
+	void __iomem *topsys;
+
+	struct clk_bulk_data *clks;
+
+	struct phy_configure_opts_mipi_dphy config;
+
+	u8 hsfreq;
+
+	struct phy *phy;
+};
+
+static u32 top_sys_read32(struct sf_dphy *priv, u32 reg)
+{
+	return ioread32(priv->topsys + reg);
+}
+
+
+static inline void top_sys_write32(struct sf_dphy *priv, u32 reg, u32 val)
+{
+	iowrite32(val, priv->topsys + reg);
+}
+
+static void dsi_csi2tx_sel(struct sf_dphy *priv, int sel)
+{
+  u32 temp = 0;
+  temp = top_sys_read32(priv, SCFG_DSI_CSI_SEL);
+  temp &= ~(0x1);
+  temp |= (sel & 0x1);
+  top_sys_write32(priv, SCFG_DSI_CSI_SEL, temp);
+}
+
+static void dphy_clane_hs_txready_sel(struct sf_dphy *priv, u32 ready_sel)
+{
+	top_sys_write32(priv, SCFG_TXREADY_SRC_SEL_D, ready_sel);
+	top_sys_write32(priv, SCFG_TXREADY_SRC_SEL_C, ready_sel);
+	top_sys_write32(priv, SCFG_HS_PRE_ZERO_T_D, 0x30);
+	top_sys_write32(priv, SCFG_HS_PRE_ZERO_T_C, 0x30);
+}
+
+static void mipi_tx_lxn_set(struct sf_dphy *priv, u32 reg, u32 n_hstx, u32 p_hstx)
+{
+	u32 temp = 0;
+
+	temp = n_hstx;
+	temp |= p_hstx << 5;
+	top_sys_write32(priv, reg, temp);
+}
+
+static void dphy_config(struct sf_dphy *priv, int bit_rate)
+{
+	int pre_div,      fbk_int,       extd_cycle_sel;
+	int dhs_pre_time, dhs_zero_time, dhs_trial_time;
+	int chs_pre_time, chs_zero_time, chs_trial_time;
+	int chs_clk_pre_time, chs_clk_post_time;
+	u32 set_val = 0;
+
+	mipi_tx_lxn_set(priv, SCFG_L0N_L0P_HSTX, 0x10, 0x10);
+	mipi_tx_lxn_set(priv, SCFG_L1N_L1P_HSTX, 0x10, 0x10);
+	mipi_tx_lxn_set(priv, SCFG_L2N_L2P_HSTX, 0x10, 0x10);
+	mipi_tx_lxn_set(priv, SCFG_L3N_L3P_HSTX, 0x10, 0x10);
+	mipi_tx_lxn_set(priv, SCFG_L4N_L4P_HSTX, 0x10, 0x10);
+
+	if(bit_rate == 80) {
+		pre_div=0x1,		fbk_int=2*0x33,		extd_cycle_sel=0x4,
+		dhs_pre_time=0xe,	dhs_zero_time=0x1d,	dhs_trial_time=0x15,
+		chs_pre_time=0x5,	chs_zero_time=0x2b,	chs_trial_time=0xd,
+		chs_clk_pre_time=0xf,
+		chs_clk_post_time=0x71;
+	} else if (bit_rate == 100) {
+		pre_div=0x1,		fbk_int=2*0x40,		extd_cycle_sel=0x4,
+		dhs_pre_time=0x10,	dhs_zero_time=0x21,	dhs_trial_time=0x17,
+		chs_pre_time=0x7,	chs_zero_time=0x35,	chs_trial_time=0xf,
+		chs_clk_pre_time=0xf,
+		chs_clk_post_time=0x73;
+	} else if (bit_rate == 200) {
+		pre_div=0x1,		fbk_int=2*0x40,		extd_cycle_sel=0x3;
+		dhs_pre_time=0xc,	dhs_zero_time=0x1b,	dhs_trial_time=0x13;
+		chs_pre_time=0x7,	chs_zero_time=0x35,	chs_trial_time=0xf,
+		chs_clk_pre_time=0x7,
+		chs_clk_post_time=0x3f;
+	} else if(bit_rate == 300) {
+		pre_div=0x1,		fbk_int=2*0x60, 	extd_cycle_sel=0x3,
+		dhs_pre_time=0x11,	dhs_zero_time=0x25, dhs_trial_time=0x19,
+		chs_pre_time=0xa, 	chs_zero_time=0x50, chs_trial_time=0x15,
+		chs_clk_pre_time=0x7,
+		chs_clk_post_time=0x45;
+    } else if(bit_rate == 400) {
+		pre_div=0x1,      	fbk_int=2*0x40,		extd_cycle_sel=0x2,
+		dhs_pre_time=0xa, 	dhs_zero_time=0x18,	dhs_trial_time=0x11,
+		chs_pre_time=0x7, 	chs_zero_time=0x35, chs_trial_time=0xf,
+		chs_clk_pre_time=0x3,
+		chs_clk_post_time=0x25;
+    } else if(bit_rate == 500 ) {
+		pre_div=0x1,      fbk_int=2*0x50,       extd_cycle_sel=0x2,
+		dhs_pre_time=0xc, dhs_zero_time=0x1d,	dhs_trial_time=0x14,
+		chs_pre_time=0x9, chs_zero_time=0x42,	chs_trial_time=0x12,
+		chs_clk_pre_time=0x3,
+		chs_clk_post_time=0x28;
+    } else if(bit_rate == 600 ) {
+		pre_div=0x1,      fbk_int=2*0x60,       extd_cycle_sel=0x2,
+		dhs_pre_time=0xe, dhs_zero_time=0x23,	dhs_trial_time=0x17,
+		chs_pre_time=0xa, chs_zero_time=0x50,	chs_trial_time=0x15,
+		chs_clk_pre_time=0x3,
+		chs_clk_post_time=0x2b;
+    } else if(bit_rate == 700) {
+		pre_div=0x1,      fbk_int=2*0x38,       extd_cycle_sel=0x1,
+		dhs_pre_time=0x8, dhs_zero_time=0x14,	dhs_trial_time=0xf,
+		chs_pre_time=0x6, chs_zero_time=0x2f,	chs_trial_time=0xe,
+		chs_clk_pre_time=0x1,
+		chs_clk_post_time=0x16;
+    } else if(bit_rate == 800 ) {
+		pre_div=0x1,      fbk_int=2*0x40,       extd_cycle_sel=0x1,
+		dhs_pre_time=0x9, dhs_zero_time=0x17,	dhs_trial_time=0x10,
+		chs_pre_time=0x7, chs_zero_time=0x35,	chs_trial_time=0xf,
+		chs_clk_pre_time=0x1,
+		chs_clk_post_time=0x18;
+    } else if(bit_rate == 900 ) {
+		pre_div=0x1,      fbk_int=2*0x48,       extd_cycle_sel=0x1,
+		dhs_pre_time=0xa, dhs_zero_time=0x19, 	dhs_trial_time=0x12,
+		chs_pre_time=0x8, chs_zero_time=0x3c, 	chs_trial_time=0x10,
+		chs_clk_pre_time=0x1,
+		chs_clk_post_time=0x19;
+    } else if(bit_rate == 1000) {
+		pre_div=0x1,      fbk_int=2*0x50,       extd_cycle_sel=0x1,
+		dhs_pre_time=0xb, dhs_zero_time=0x1c,	dhs_trial_time=0x13,
+		chs_pre_time=0x9, chs_zero_time=0x42,	chs_trial_time=0x12,
+		chs_clk_pre_time=0x1,
+		chs_clk_post_time=0x1b;
+    } else if(bit_rate == 1100) {
+		pre_div=0x1,      fbk_int=2*0x58,       extd_cycle_sel=0x1,
+		dhs_pre_time=0xc, dhs_zero_time=0x1e,	dhs_trial_time=0x15,
+		chs_pre_time=0x9, chs_zero_time=0x4a,	chs_trial_time=0x14,
+		chs_clk_pre_time=0x1,
+		chs_clk_post_time=0x1d;
+    } else if(bit_rate == 1200) {
+		pre_div=0x1,      fbk_int=2*0x60,       extd_cycle_sel=0x1,
+		dhs_pre_time=0xe, dhs_zero_time=0x20,	dhs_trial_time=0x16,
+		chs_pre_time=0xa, chs_zero_time=0x50,	chs_trial_time=0x15,
+		chs_clk_pre_time=0x1,
+		chs_clk_post_time=0x1e;
+    } else if(bit_rate == 1300) {
+		pre_div=0x1,      fbk_int=2*0x34,       extd_cycle_sel=0x0,
+		dhs_pre_time=0x7, dhs_zero_time=0x12,	dhs_trial_time=0xd,
+		chs_pre_time=0x5, chs_zero_time=0x2c,	chs_trial_time=0xd,
+		chs_clk_pre_time=0x0,
+		chs_clk_post_time=0xf;
+    } else if(bit_rate == 1400) {
+		pre_div=0x1,      fbk_int=2*0x38,       extd_cycle_sel=0x0,
+		dhs_pre_time=0x7, dhs_zero_time=0x14,	dhs_trial_time=0xe,
+		chs_pre_time=0x6, chs_zero_time=0x2f,	chs_trial_time=0xe,
+		chs_clk_pre_time=0x0,
+		chs_clk_post_time=0x10;
+    } else if(bit_rate == 1500) {
+		pre_div=0x1,      fbk_int=2*0x3c,       extd_cycle_sel=0x0,
+		dhs_pre_time=0x8, dhs_zero_time=0x14,	dhs_trial_time=0xf,
+		chs_pre_time=0x6, chs_zero_time=0x32,	chs_trial_time=0xe,
+		chs_clk_pre_time=0x0,
+		chs_clk_post_time=0x11;
+    } else if(bit_rate == 1600) {
+		pre_div=0x1,      fbk_int=2*0x40,       extd_cycle_sel=0x0,
+		dhs_pre_time=0x9, dhs_zero_time=0x15,	dhs_trial_time=0x10,
+		chs_pre_time=0x7, chs_zero_time=0x35,	chs_trial_time=0xf,
+		chs_clk_pre_time=0x0,
+		chs_clk_post_time=0x12;
+    } else if(bit_rate == 1700) {
+		pre_div=0x1,      fbk_int=2*0x44,       extd_cycle_sel=0x0,
+		dhs_pre_time=0x9, dhs_zero_time=0x17,	dhs_trial_time=0x10,
+		chs_pre_time=0x7, chs_zero_time=0x39,	chs_trial_time=0x10,
+		chs_clk_pre_time=0x0,
+		chs_clk_post_time=0x12;
+    } else if(bit_rate == 1800) {
+		pre_div=0x1,      fbk_int=2*0x48,       extd_cycle_sel=0x0,
+		dhs_pre_time=0xa, dhs_zero_time=0x18,	dhs_trial_time=0x11,
+		chs_pre_time=0x8, chs_zero_time=0x3c,	chs_trial_time=0x10,
+		chs_clk_pre_time=0x0,
+		chs_clk_post_time=0x13;
+    } else if(bit_rate == 1900) {
+		pre_div=0x1,      fbk_int=2*0x4c,       extd_cycle_sel=0x0,
+		dhs_pre_time=0xa, dhs_zero_time=0x1a,	dhs_trial_time=0x12,
+		chs_pre_time=0x8, chs_zero_time=0x3f,	chs_trial_time=0x11,
+		chs_clk_pre_time=0x0,
+		chs_clk_post_time=0x14;
+    } else if(bit_rate == 2000) {
+		pre_div=0x1,      fbk_int=2*0x50,       extd_cycle_sel=0x0,
+		dhs_pre_time=0xb, dhs_zero_time=0x1b,	dhs_trial_time=0x13,
+		chs_pre_time=0x9, chs_zero_time=0x42,	chs_trial_time=0x12,
+		chs_clk_pre_time=0x0,
+		chs_clk_post_time=0x15;
+    } else if(bit_rate == 2100) {
+		pre_div=0x1,      fbk_int=2*0x54,       extd_cycle_sel=0x0,
+		dhs_pre_time=0xb, dhs_zero_time=0x1c,	dhs_trial_time=0x13,
+		chs_pre_time=0x9, chs_zero_time=0x46,	chs_trial_time=0x13,
+		chs_clk_pre_time=0x0,
+		chs_clk_post_time=0x15;
+    } else if(bit_rate == 2200) {
+		pre_div=0x1,      fbk_int=2*0x5b,       extd_cycle_sel=0x0,
+		dhs_pre_time=0xc, dhs_zero_time=0x1d,	dhs_trial_time=0x14,
+		chs_pre_time=0x9, chs_zero_time=0x4a,	chs_trial_time=0x14,
+		chs_clk_pre_time=0x0,
+		chs_clk_post_time=0x16;
+    } else if(bit_rate == 2300) {
+		pre_div=0x1,      fbk_int=2*0x5c,       extd_cycle_sel=0x0,
+		dhs_pre_time=0xc, dhs_zero_time=0x1f,	dhs_trial_time=0x15,
+		chs_pre_time=0xa, chs_zero_time=0x4c,	chs_trial_time=0x14,
+		chs_clk_pre_time=0x0,
+		chs_clk_post_time=0x17;
+    } else if(bit_rate == 2400) {
+		pre_div=0x1,      fbk_int=2*0x60,       extd_cycle_sel=0x0,
+		dhs_pre_time=0xd, dhs_zero_time=0x20,	dhs_trial_time=0x16,
+		chs_pre_time=0xa, chs_zero_time=0x50,	chs_trial_time=0x15,
+		chs_clk_pre_time=0x0,
+		chs_clk_post_time=0x18;
+    } else if(bit_rate == 2500) {
+		pre_div=0x1,      fbk_int=2*0x64,       extd_cycle_sel=0x0,
+		dhs_pre_time=0xe, dhs_zero_time=0x21,	dhs_trial_time=0x16,
+		chs_pre_time=0xb, chs_zero_time=0x53,	chs_trial_time=0x16,
+		chs_clk_pre_time=0x0,
+		chs_clk_post_time=0x18;
+    } else {
+		//default bit_rate == 700
+		pre_div=0x1,      fbk_int=2*0x38,       extd_cycle_sel=0x1,
+		dhs_pre_time=0x8, dhs_zero_time=0x14,	dhs_trial_time=0xf,
+		chs_pre_time=0x6, chs_zero_time=0x2f,	chs_trial_time=0xe,
+		chs_clk_pre_time=0x1,
+		chs_clk_post_time=0x16;
+    }
+	top_sys_write32(priv, SCFG_REFCLK_SEL, 0x3);
+
+	set_val = 0
+			| (1 << OFFSET_CFG_L1_SWAP_SEL)
+			| (4 << OFFSET_CFG_L2_SWAP_SEL)
+			| (2 << OFFSET_CFG_L3_SWAP_SEL)
+			| (3 << OFFSET_CFG_L4_SWAP_SEL);
+	top_sys_write32(priv, SCFG_LX_SWAP_SEL, set_val);
+
+	set_val = 0
+			| (0 << OFFSET_SCFG_PWRON_READY_N)
+			| (1 << OFFSET_RG_CDTX_PLL_FM_EN)
+			| (0 << OFFSET_SCFG_PLLSSC_EN)
+			| (1 << OFFSET_RG_CDTX_PLL_LDO_STB_X2_EN);
+	top_sys_write32(priv, SCFG_DBUS_PW_PLL_SSC_LD0, set_val);
+
+	set_val = fbk_int
+			| (pre_div << 9);
+	top_sys_write32(priv, SCFG_RG_CDTX_PLL_FBK_PRE, set_val);
+
+	top_sys_write32(priv, SCFG_RG_EXTD_CYCLE_SEL, extd_cycle_sel);
+
+	set_val = chs_zero_time
+			| (dhs_pre_time << OFFSET_DHS_PRE_TIME)
+			| (dhs_trial_time << OFFSET_DHS_TRIAL_TIME)
+			| (dhs_zero_time << OFFSET_DHS_ZERO_TIME);
+	top_sys_write32(priv, SCFG_RG_CLANE_DLANE_TIME, set_val);
+
+	set_val = chs_clk_post_time
+			| (chs_clk_pre_time << OFFSET_CHS_PRE_TIME)
+			| (chs_pre_time << OFFSET_CHS_TRIAL_TIME)
+			| (chs_trial_time << OFFSET_CHS_ZERO_TIME);
+	top_sys_write32(priv, SCFG_RG_CLANE_HS_TIME, set_val);
+
+}
+
+static void reset_dphy(struct sf_dphy *priv, int resetb)
+{
+	u32 cfg_dsc_enable = 0x01;//bit0
+
+	u32 precfg = top_sys_read32(priv, SCFG_PHY_RESETB);
+	precfg &= ~(cfg_dsc_enable);
+	precfg |= (resetb&cfg_dsc_enable);
+	top_sys_write32(priv, SCFG_PHY_RESETB, precfg);
+}
+
+static void polling_dphy_lock(struct sf_dphy *priv)
+{
+	int pll_unlock;
+
+	udelay(10);
+
+	do {
+		pll_unlock = top_sys_read32(priv, SCFG_GRS_CDTX_PLL) >> 3;
+		pll_unlock &= 0x1;
+	} while(pll_unlock == 0x1);
+}
+
+static int sf_dphy_configure(struct phy *phy, union phy_configure_opts *opts)
+{
+	struct sf_dphy *dphy = phy_get_drvdata(phy);
+
+	uint32_t bit_rate = 700000000/1000000UL;//(1920 * 1080 * bpp / dlanes * fps / 1000000 + 99) / 100 * 100;
+
+	dphy_config(dphy, bit_rate);
+	reset_dphy(dphy, 1);
+	mdelay(10);
+	polling_dphy_lock(dphy);
+
+	return 0;
+}
+
+static int sf_dphy_power_on(struct phy *phy)
+{
+	return 0;
+}
+
+static int sf_dphy_power_off(struct phy *phy)
+{
+	return 0;
+}
+
+static int sf_dphy_init(struct phy *phy)
+{
+	struct sf_dphy *dphy = phy_get_drvdata(phy);
+
+	dsi_csi2tx_sel(dphy, 0);
+	dphy_clane_hs_txready_sel(dphy, 0x1);
+
+	return 0;
+}
+
+static int sf_dphy_validate(struct phy *phy, enum phy_mode mode, int submode,
+			union phy_configure_opts *opts)
+{
+	return 0;
+}
+
+static int sf_dphy_set_mode(struct phy *phy, enum phy_mode mode, int submode)
+{
+	return 0;
+}
+
+
+static int sf_dphy_exit(struct phy *phy)
+{
+	return 0;
+}
+
+static const struct phy_ops sf_dphy_ops = {
+	.power_on	= sf_dphy_power_on,
+	.power_off	= sf_dphy_power_off,
+	.init		= sf_dphy_init,
+	.exit		= sf_dphy_exit,
+	.configure	= sf_dphy_configure,
+	.validate  = sf_dphy_validate,
+	.set_mode  = sf_dphy_set_mode,
+	.owner		= THIS_MODULE,
+};
+
+static const struct of_device_id sf_dphy_dt_ids[] = {
+	{
+		.compatible = "starfive,jh7100-mipi-dphy-tx",
+	},
+	{}
+};
+MODULE_DEVICE_TABLE(of, sf_dphy_dt_ids);
+
+static int sf_dphy_probe(struct platform_device *pdev)
+{
+	struct phy_provider *phy_provider;
+	struct sf_dphy *dphy;
+	struct resource *res;
+	int ret;
+	dphy = devm_kzalloc(&pdev->dev, sizeof(*dphy), GFP_KERNEL);
+	if (!dphy)
+		return -ENOMEM;
+	dev_set_drvdata(&pdev->dev, dphy);
+
+	dev_info(&pdev->dev,"===> %s enter, %d \n", __func__, __LINE__);
+
+	dphy->topsys = ioremap(0x12260000, 0x10000);
+
+	dphy->phy = devm_phy_create(&pdev->dev, NULL, &sf_dphy_ops);
+	if (IS_ERR(dphy->phy)) {
+		dev_err(&pdev->dev, "failed to create phy\n");
+		return PTR_ERR(dphy->phy);
+	}
+	phy_set_drvdata(dphy->phy, dphy);
+
+	phy_provider = devm_of_phy_provider_register(&pdev->dev, of_phy_simple_xlate);
+
+	return PTR_ERR_OR_ZERO(phy_provider);
+}
+
+static struct platform_driver sf_dphy_driver = {
+	.probe = sf_dphy_probe,
+	.driver = {
+		.name	= "sf-mipi-dphy-tx",
+		.of_match_table = sf_dphy_dt_ids,
+	},
+};
+module_platform_driver(sf_dphy_driver);
+
+MODULE_AUTHOR("Ezequiel Garcia <ezequiel@collabora.com>");
+MODULE_DESCRIPTION("sf MIPI  DPHY TX0 driver");
+MODULE_LICENSE("Dual MIT/GPL");
diff --git a/drivers/video/fbdev/Kconfig b/drivers/video/fbdev/Kconfig
index 6ed5e608dd04..b8880e7d383d 100644
--- a/drivers/video/fbdev/Kconfig
+++ b/drivers/video/fbdev/Kconfig
@@ -1797,6 +1797,15 @@ config PXA3XX_GCU
 
 	  If you compile this as a module, it will be called pxa3xx_gcu.
 
+config FB_STARFIVE
+	tristate "Starfive framebuffer support"
+	depends on FB
+	select FB_CFB_FILLRECT
+	select FB_CFB_COPYAREA
+	select FB_CFB_IMAGEBLIT
+	help
+	  Framebuffer driver for the Starfive SOC
+
 config FB_FSL_DIU
 	tristate "Freescale DIU framebuffer support"
 	depends on FB && FSL_SOC
@@ -2241,3 +2250,4 @@ config FB_SM712
 source "drivers/video/fbdev/omap/Kconfig"
 source "drivers/video/fbdev/omap2/Kconfig"
 source "drivers/video/fbdev/mmp/Kconfig"
+source "drivers/video/fbdev/starfive/Kconfig"
diff --git a/drivers/video/fbdev/Makefile b/drivers/video/fbdev/Makefile
index 477b9624b703..bb7b6d0455d8 100644
--- a/drivers/video/fbdev/Makefile
+++ b/drivers/video/fbdev/Makefile
@@ -31,6 +31,7 @@ obj-$(CONFIG_FB_VIA)		  += via/
 obj-$(CONFIG_FB_KYRO)             += kyro/
 obj-$(CONFIG_FB_SAVAGE)		  += savage/
 obj-$(CONFIG_FB_GEODE)		  += geode/
+obj-$(CONFIG_FB_STARFIVE)		  += starfive/
 obj-$(CONFIG_FB_NEOMAGIC)         += neofb.o
 obj-$(CONFIG_FB_3DFX)             += tdfxfb.o
 obj-$(CONFIG_FB_CONTROL)          += controlfb.o
diff --git a/drivers/video/fbdev/starfive/Kconfig b/drivers/video/fbdev/starfive/Kconfig
new file mode 100644
index 000000000000..35511e508e3a
--- /dev/null
+++ b/drivers/video/fbdev/starfive/Kconfig
@@ -0,0 +1,36 @@
+#
+# Video TX driver configuration
+#
+menu "Displayer Configuration for VIC board"
+
+choice
+	prompt "Displayer support for VIC board"
+	default FB_STARFIVE_HDMI_TDA998X
+	depends on FB_STARFIVE
+	optional
+
+config FB_STARFIVE_HDMI_ADV7513
+	bool "HDMI ADV7513 displayer support"
+	help
+	  Say Y here if you want to have support for ADV7513 displayer
+
+config FB_STARFIVE_HDMI_TDA998X
+	bool "HDMI TDA998X displayer support"
+	depends on DRM_FBDEV_EMULATION
+	help
+	  Say Y here if you want to have support for TDA998X displayer
+
+config FB_STARFIVE_SEEED5INCH
+	bool "seeed5inch mipi LCD panel displayer support"
+	help
+	  Say Y here if you want to have support for seeed5inch mipi LCD panel
+
+endchoice
+
+config FB_STARFIVE_VIDEO
+	bool "Cam sensor image data to framebuffer, instead of Console"
+	depends on FB_STARFIVE && VIDEO_STARFIVE_VIN
+	help
+	  Say Y here if you want to display video from sensor, instead of console.
+
+endmenu
diff --git a/drivers/video/fbdev/starfive/Makefile b/drivers/video/fbdev/starfive/Makefile
new file mode 100644
index 000000000000..da6c6f8fb2a9
--- /dev/null
+++ b/drivers/video/fbdev/starfive/Makefile
@@ -0,0 +1,11 @@
+# SPDX-License-Identifier: GPL-2.0
+#
+# Makefile for RTC class/drivers.
+#
+
+obj-$(CONFIG_FB_STARFIVE)		+= starfive_fb.o starfive_lcdc.o starfive_vpp.o \
+					starfive_display_dev.o starfive_displayer.o starfive_mipi_tx.o
+
+obj-$(CONFIG_FB_STARFIVE_HDMI_ADV7513)		+= adv7513.o
+obj-$(CONFIG_FB_STARFIVE_HDMI_TDA998X)		+= tda998x.o
+obj-$(CONFIG_FB_STARFIVE_SEEED5INCH)		+= seeed5inch.o
diff --git a/drivers/video/fbdev/starfive/adv7513.c b/drivers/video/fbdev/starfive/adv7513.c
new file mode 100755
index 000000000000..3ddb9da5c820
--- /dev/null
+++ b/drivers/video/fbdev/starfive/adv7513.c
@@ -0,0 +1,265 @@
+/*
+ ******************************************************************************
+ * @file  adv7513.c
+ * @author  StarFive Technology
+ * @version  V1.0
+ * @date  09/21/2020
+ * @brief
+ ******************************************************************************
+ * @copy
+ *
+ * THE PRESENT SOFTWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
+ * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE
+ * TIME. AS A RESULT, STARFIVE SHALL NOT BE HELD LIABLE FOR ANY
+ * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING
+ * FROM THE CONTENT OF SUCH SOFTWARE AND/OR THE USE MADE BY CUSTOMERS OF THE
+ * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
+ *
+ * <h2><center>&copy; COPYRIGHT 2020 Shanghai StarFive Technology Co., Ltd. </center></h2>
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/gpio.h>
+#include <linux/gpio/consumer.h>
+#include <linux/device.h>
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/media.h>
+#include <linux/module.h>
+#include <linux/ratelimit.h>
+#include <linux/slab.h>
+#include <linux/string.h>
+#include "adv7513.h"
+
+static int adv7513_write(struct i2c_client *client, u16 reg, u8 val)
+{
+	struct i2c_msg msg;
+	u8 buf[2];
+	int ret;
+	buf[0] = reg;
+	buf[1] = val;
+
+	msg.addr = client->addr;
+	msg.flags = 0;
+	msg.buf = buf;
+	msg.len = 2;
+
+	ret = i2c_transfer(client->adapter, &msg, 1);
+	if (ret >= 0)
+		return 0;
+
+	dev_err(&client->dev,
+		"adv7513 write reg(0x%x val:0x%x) failed !\n", reg, val);
+
+	return ret;
+}
+
+static int adv7513_read(struct i2c_client *client, u8 reg, u8 *val)
+{
+	struct i2c_msg msg[2];
+	u8 buf[2];
+	int ret;
+
+	buf[0] = reg;
+
+	msg[0].addr = client->addr;
+	msg[0].flags = 0;
+	msg[0].buf = buf;
+	msg[0].len = 1;
+
+	msg[1].addr = client->addr;
+	msg[1].flags = I2C_M_RD;
+	msg[1].buf = val;
+	msg[1].len = 1;
+
+	ret = i2c_transfer(client->adapter, msg, 2);
+	if (ret >= 0) {
+		return 0;
+	}
+
+	dev_err(&client->dev,
+		"adv7513 read reg(0x%x val:0x%x) failed,ret = %d !\n", reg, *val, ret);
+
+	return ret;
+}
+
+/*============================================================================
+ * Read up to 8-bit field from a single 8-bit register
+ *              ________
+ * Example     |___***__|  Mask = 0x1C     BitPos = 2
+ *
+ *
+ * Entry:   DevAddr = Device Address
+ *          RegAddr = 8-bit register address
+ *          Mask    = Field mask
+ *          BitPos  = Field LSBit position in the register (0-7)
+ *
+ * Return:  Field value in the LSBits of the return value
+ *
+ *===========================================================================*/
+static u8 adv7513_I2CReadField8 (struct i2c_client *client, u8 RegAddr, u8 Mask,
+                         u8 BitPos)
+{
+    u8 data;
+
+	adv7513_read(client, RegAddr, &data);
+
+	return (data&Mask)>>BitPos;
+}
+
+/*============================================================================
+ * Write up to 8-bit field to a single 8-bit register
+ *              ________
+ * Example     |___****_|  Mask = 0x1E     BitPos = 1
+ *
+ * Entry:   DevAddr = Device Address
+ *          RegAddr = 8-bit register address
+ *          Mask    = Field mask
+ *          BitPos  = Field LSBit position in the register (0-7)
+ *                    Set to 0 if FieldVal is in correct position of the reg
+ *          FieldVal= Value (in the LSBits) of the field to be written
+ *                    If FieldVal is already in the correct position (i.e.,
+ *                    does not need to be shifted,) set BitPos to 0
+ *
+ * Return:  None
+ *
+ *===========================================================================*/
+static void adv7513_I2CWriteField8 (struct i2c_client *client,u8 RegAddr, u8 Mask,
+                         u8 BitPos, u8 FieldVal)
+{
+	u8 rdata, wdata;
+
+	adv7513_read(client, RegAddr, &rdata);
+	rdata &= (~Mask);
+	wdata = rdata | ((FieldVal<<BitPos)&Mask);
+	adv7513_write(client, RegAddr, wdata);
+}
+
+static int adv7513_probe(struct i2c_client *client, const struct i2c_device_id *id)
+{
+	struct adv7513_data *adv7513;
+	struct device *dev = &client->dev;
+	u8 value;
+	int ret = 0;
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		dev_warn(&client->dev,
+			 "I2C adapter doesn't support I2C_FUNC_SMBUS_BYTE\n");
+		return -EIO;
+	}
+
+	adv7513 = devm_kzalloc(&client->dev, sizeof(*adv7513), GFP_KERNEL);
+	if (!adv7513)
+		return -ENOMEM;
+
+	if (of_property_read_u32(dev->of_node, "def-width", &adv7513->def_width)) {
+		dev_err(dev,"Missing def_width property in the DT \n");
+		ret = -EINVAL;
+	}
+
+	adv7513->client = client;
+	i2c_set_clientdata(client, adv7513);
+
+	adv7513_read(client, 0x00, &value);
+	if (value != 0x13) {
+		dev_info(&client->dev, "%s[%d],version = 0x%x(NOT 0x13), not find device !\n",__func__,__LINE__,value);
+		return -ENODEV;
+	}
+
+	adv7513_I2CWriteField8(client, 0x41, 0x40, 0x6, 0x00);
+	adv7513_I2CWriteField8(client, 0x98, 0xFF, 0x0, 0x03);
+	adv7513_I2CWriteField8(client, 0x9A, 0xE0, 0x5, 0x07);
+	adv7513_I2CWriteField8(client, 0x9C, 0xFF, 0x0, 0x30);
+	adv7513_I2CWriteField8(client, 0x9D, 0x03, 0x0, 0x01);
+	adv7513_I2CWriteField8(client, 0xA2, 0xFF, 0x0, 0xA4);
+	adv7513_I2CWriteField8(client, 0xA3, 0xFF, 0x0, 0xA4);
+	adv7513_I2CWriteField8(client, 0xE0, 0xFF, 0x0, 0xD0);
+	adv7513_I2CWriteField8(client, 0xF9, 0xFF, 0x0, 0x00);
+
+	adv7513_I2CWriteField8(client, 0x15, 0x0F, 0x0, 0x00);
+	adv7513_I2CWriteField8(client, 0x16, 0x30, 0x4, 0x03);
+	adv7513_I2CWriteField8(client, 0x16, 0x0C, 0x2, 0x00);
+	adv7513_I2CWriteField8(client, 0x17, 0x02, 0x1, 0x00);
+
+	adv7513_I2CWriteField8(client, 0x16, 0xC0, 0x6, 0x00);
+	adv7513_I2CWriteField8(client, 0x18, 0x80, 0x7, 0x00);
+	adv7513_I2CWriteField8(client, 0x18, 0x60, 0x5, 0x00);
+	adv7513_I2CWriteField8(client, 0xAF, 0x02, 0x1, 0x01);
+	adv7513_I2CWriteField8(client, 0x3C, 0x3F, 0x0, 0x01);
+
+	switch(adv7513->def_width) {
+		case 288:
+			adv7513_I2CWriteField8(client, 0x3C, 0x3F, 0x0, 0x18);//288P
+			adv7513_I2CWriteField8(client, 0x3B, 0xFF, 0x0, 0x00);
+			break;
+		case 640:
+			adv7513_I2CWriteField8(client, 0x3C, 0x3F, 0x0, 0x01);
+			adv7513_I2CWriteField8(client, 0x3B, 0xFF, 0x0, 0x4A); //b01001010
+			break;
+		case 1280:
+			adv7513_I2CWriteField8(client, 0x3C, 0x3F, 0x0, 0x04);//720P
+			adv7513_I2CWriteField8(client, 0x3B, 0xFF, 0x0, 0x00);//b01011000
+			break;
+		case 1920:
+			adv7513_I2CWriteField8(client, 0x3C, 0x3F, 0x0, 0x10);//1080P
+			adv7513_I2CWriteField8(client, 0x3B, 0xFF, 0x0, 0x00);//b01011000
+			break;
+		default:
+			dev_err(dev,"not support width %d \n",adv7513->def_width);
+	}
+
+	return ret;
+}
+
+static int adv7513_remove(struct i2c_client *client)
+{
+	struct adv7513 *adv7513 = i2c_get_clientdata(client);
+
+	return 0;
+}
+
+static const struct i2c_device_id adv7513_id[] = {
+	{ "adv7513", 0 },
+	{ }
+};
+
+static const struct of_device_id dvp_adv7513_dt_ids[] = {
+	{ .compatible = "adv7513", },
+	{ /* sentinel */ }
+};
+
+static struct i2c_driver adv7513_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.name	= "adv7513",
+		.of_match_table = dvp_adv7513_dt_ids,
+	},
+	.probe		= adv7513_probe,
+	.remove		= adv7513_remove,
+	.id_table	= adv7513_id,
+};
+
+static __init int init_adv7513(void)
+{
+	int err;
+
+	err = i2c_add_driver(&adv7513_driver);
+	if (err != 0)
+		printk("i2c driver registration failed, error=%d\n", err);
+
+	return err;
+}
+
+static __exit void exit_adv7513(void)
+{
+	i2c_del_driver(&adv7513_driver);
+}
+
+//late_initcall(init_adv7513);
+fs_initcall(init_adv7513);
+module_exit(exit_adv7513);
+MODULE_DESCRIPTION("A driver for adv7513");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/fbdev/starfive/adv7513.h b/drivers/video/fbdev/starfive/adv7513.h
new file mode 100644
index 000000000000..fe98b00ff2b9
--- /dev/null
+++ b/drivers/video/fbdev/starfive/adv7513.h
@@ -0,0 +1,22 @@
+/*
+ * Analog Devices ADV7513 HDMI transmitter driver
+ *
+ * Copyright 2020 StarFive Inc.
+ *
+ * Licensed under the GPL-2.
+ */
+
+#ifndef __AD_I2C_ADV7513_H__
+#define __AD_I2C_ADV7513_H__
+
+#define  H_SIZE   1920//352//1920//1280
+#define  V_SIZE   1080//288//1080//720
+
+struct adv7513_data {
+	struct i2c_client *client;
+	struct device   *dev;
+	int 			irq;
+	int		def_width;
+};
+
+#endif
diff --git a/drivers/video/fbdev/starfive/seeed5inch.c b/drivers/video/fbdev/starfive/seeed5inch.c
new file mode 100755
index 000000000000..053dc6110ea1
--- /dev/null
+++ b/drivers/video/fbdev/starfive/seeed5inch.c
@@ -0,0 +1,243 @@
+/*
+ ******************************************************************************
+ * @file  seeed5inch.c
+ * @author  StarFive Technology
+ * @version  V1.0
+ * @date  01/07/2021
+ * @brief
+ ******************************************************************************
+ * @copy
+ *
+ * THE PRESENT SOFTWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
+ * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE
+ * TIME. AS A RESULT, STARFIVE SHALL NOT BE HELD LIABLE FOR ANY
+ * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING
+ * FROM THE CONTENT OF SUCH SOFTWARE AND/OR THE USE MADE BY CUSTOMERS OF THE
+ * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
+ *
+ * <h2><center>&copy; COPYRIGHT 2021 Shanghai StarFive Technology Co., Ltd. </center></h2>
+ */
+#include <linux/clk.h>
+#include <linux/gpio.h>
+#include <linux/gpio/consumer.h>
+#include <linux/device.h>
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/media.h>
+#include <linux/module.h>
+#include <linux/ratelimit.h>
+#include <linux/slab.h>
+#include <linux/string.h>
+
+/* I2C registers of the Atmel microcontroller. */
+enum REG_ADDR {
+    REG_ID = 0x80,
+    REG_PORTA, /* BIT(2) for horizontal flip, BIT(3) for vertical flip */
+    REG_PORTB,
+    REG_PORTC,
+    REG_PORTD,
+    REG_POWERON,
+    REG_PWM,
+    REG_DDRA,
+    REG_DDRB,
+    REG_DDRC,
+    REG_DDRD,
+    REG_TEST,
+    REG_WR_ADDRL,
+    REG_WR_ADDRH,
+    REG_READH,
+    REG_READL,
+    REG_WRITEH,
+    REG_WRITEL,
+    REG_ID2,
+};
+
+struct seeed_panel_dev {
+	struct i2c_client *client;
+	struct device   *dev;
+	int 			irq;
+};
+
+struct dcs_buffer {
+	u32 len;
+	union {
+		u32 val32;
+		char val8[4];
+	};
+};
+
+static int seeed_panel_i2c_write(struct i2c_client *client, u8 reg, u8 val)
+{
+	struct i2c_msg msg;
+	u8 buf[2];
+	int ret;
+	buf[0] = reg;
+	buf[1] = val;
+
+	msg.addr = client->addr;
+	msg.flags = 0;
+	msg.buf = buf;
+	msg.len = 2;
+
+	ret = i2c_transfer(client->adapter, &msg, 1);
+	if (ret >= 0)
+		return 0;
+
+	dev_err(&client->dev,
+		"seeed panel i2c write reg(0x%x val:0x%x) failed !\n", reg, val);
+
+	return ret;
+}
+
+static int seeed_panel_i2c_read(struct i2c_client *client, u8 reg, u8 *val)
+{
+	struct i2c_msg msg[2];
+	u8 buf[2];
+	int ret;
+
+	buf[0] = reg;
+
+	msg[0].addr = client->addr;
+	msg[0].flags = 0;
+	msg[0].buf = buf;
+	msg[0].len = 1;
+
+	msg[1].addr = client->addr;
+	msg[1].flags = I2C_M_RD;
+	msg[1].buf = val;
+	msg[1].len = 1;
+
+	ret = i2c_transfer(client->adapter, msg, 2);
+	if (ret >= 0) {
+		return 0;
+	}
+
+	dev_err(&client->dev,
+		"seeed panel i2c read reg(0x%x val:0x%x) failed,ret = %d !\n", reg, *val, ret);
+
+	return ret;
+}
+
+#if 0
+static int seeed_panel_disable(struct i2c_client *client)
+{
+	seeed_panel_i2c_write(client, REG_PWM, 0);
+	seeed_panel_i2c_write(client, REG_POWERON, 0);
+	udelay(1);
+
+	return 0;
+}
+#endif
+
+enum dsi_rgb_pattern_t {
+	RGB_PAT_WHITE,
+	RGB_PAT_BLACK,
+	RGB_PAT_RED,
+	RGB_PAT_GREEN,
+	RGB_PAT_BLUE,
+	RGB_PAT_HORIZ_COLORBAR,
+	RGB_PAT_VERT_COLORBAR,
+	RGB_PAT_NUM
+};
+
+static int seeed_panel_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	u8 reg_value = 0;
+//	int ret = 0;
+	int i;
+	struct seeed_panel_dev *seeed_panel;
+//	struct device *dev = &client->dev;
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		dev_warn(&client->dev,
+			 "I2C adapter doesn't support I2C_FUNC_SMBUS_BYTE\n");
+		return -EIO;
+	}
+
+	seeed_panel = devm_kzalloc(&client->dev,sizeof(struct seeed_panel_dev),
+					GFP_KERNEL);
+	if (!seeed_panel)
+		return -ENOMEM;
+
+	seeed_panel->client = client;
+	i2c_set_clientdata(client, seeed_panel);
+
+	seeed_panel_i2c_read(client, REG_ID, &reg_value);
+	dev_info(&client->dev, "%s[%d],reg[0x80] = 0x%x\n",
+			__func__, __LINE__, reg_value);
+	switch (reg_value) {
+		case 0xde: /* ver 1 */
+		case 0xc3: /* ver 2 */
+		break;
+
+		default:
+			dev_err(&client->dev, "Unknown Atmel firmware revision: 0x%02x\n", reg_value);
+			return -ENODEV;
+	}
+
+	seeed_panel_i2c_write(client, REG_POWERON, 1);
+	mdelay(5);
+	/* Wait for nPWRDWN to go low to indicate poweron is done. */
+	for (i = 0; i < 100; i++) {
+		seeed_panel_i2c_read(client, REG_PORTB, &reg_value);
+		if (reg_value & 1)
+			break;
+	}
+
+	seeed_panel_i2c_write(client, REG_PWM, 255);
+	seeed_panel_i2c_write(client, REG_PORTA, BIT(2));
+
+	return 0;
+}
+
+static int seeed_panel_remove(struct i2c_client *client)
+{
+	return 0;
+}
+
+static const struct i2c_device_id seeed_panel_id[] = {
+	{ "seeed_panel", 0 },
+	{ }
+};
+
+static const struct of_device_id seeed_panel_dt_ids[] = {
+	{ .compatible = "seeed_panel", },
+	{ /* sentinel */ }
+};
+
+static struct i2c_driver seeed_panel_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.name	= "seeed_panel",
+		.of_match_table = seeed_panel_dt_ids,
+	},
+	.probe		= seeed_panel_probe,
+	.remove		= seeed_panel_remove,
+	.id_table	= seeed_panel_id,
+};
+
+static __init int init_seeed_panel(void)
+{
+	int err;
+
+	err = i2c_add_driver(&seeed_panel_driver);
+	if (err != 0)
+		printk("i2c driver registration failed, error=%d\n", err);
+
+	return err;
+}
+
+static __exit void exit_seeed_panel(void)
+{
+	i2c_del_driver(&seeed_panel_driver);
+}
+
+//module_init(init_seeed_panel);
+fs_initcall(init_seeed_panel);
+module_exit(exit_seeed_panel);
+
+MODULE_DESCRIPTION("A driver for seeed_panel");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/fbdev/starfive/starfive_comm_regs.h b/drivers/video/fbdev/starfive/starfive_comm_regs.h
new file mode 100644
index 000000000000..07c1a21f09c2
--- /dev/null
+++ b/drivers/video/fbdev/starfive/starfive_comm_regs.h
@@ -0,0 +1,93 @@
+/*
+ * StarFive Vout driver
+ *
+ * Copyright 2020 StarFive Inc.
+ *
+ * Licensed under the GPL-2.
+ */
+
+#ifndef __SF_COMM_REGS_H__
+#define __SF_COMM_REGS_H__
+
+#include "starfive_fb.h"
+
+//syscfg registers
+#define SCFG_DSI_CSI_SEL	        0x2c
+#define SCFG_PHY_RESETB	            0x30
+#define SCFG_REFCLK_SEL	            0x34
+#define SCFG_DBUS_PW_PLL_SSC_LD0	0x38
+#define SCFG_GRS_CDTX_PLL       	0x3c
+
+#define SCFG_RG_CDTX_PLL_FBK_PRE	0x44
+#define SCFG_RG_CLANE_DLANE_TIME   	0x58
+#define SCFG_RG_CLANE_HS_TIME   	0x58
+
+#define SCFG_RG_EXTD_CYCLE_SEL   	0x5c
+
+#define SCFG_L0N_L0P_HSTX	        0x60
+#define SCFG_L1N_L1P_HSTX	        0x64
+#define SCFG_L2N_L2P_HSTX	        0x68
+#define SCFG_L3N_L3P_HSTX	        0x6c
+#define SCFG_L4N_L4P_HSTX	        0x70
+#define SCFG_LX_SWAP_SEL	        0x78
+
+#define SCFG_HS_PRE_ZERO_T_D	    0xc4
+#define SCFG_TXREADY_SRC_SEL_D	    0xc8
+#define SCFG_HS_PRE_ZERO_T_C	    0xd4
+#define SCFG_TXREADY_SRC_SEL_C	    0xd8
+
+//reg SCFG_LX_SWAP_SEL
+#define	OFFSET_CFG_L0_SWAP_SEL 	0
+#define	OFFSET_CFG_L1_SWAP_SEL 	3
+#define	OFFSET_CFG_L2_SWAP_SEL 	6
+#define	OFFSET_CFG_L3_SWAP_SEL 	9
+#define OFFSET_CFG_L4_SWAP_SEL 	12
+
+//reg SCFG_DBUS_PW_PLL_SSC_LD0
+#define OFFSET_SCFG_CFG_DATABUD16_SEL    0
+#define OFFSET_SCFG_PWRON_READY_N        1
+#define OFFSET_RG_CDTX_PLL_FM_EN         2
+#define OFFSET_SCFG_PLLSSC_EN            3
+#define OFFSET_RG_CDTX_PLL_LDO_STB_X2_EN 4
+
+//reg SCFG_RG_CLANE_DLANE_TIME
+#define OFFSET_DHS_PRE_TIME          8
+#define OFFSET_DHS_TRIAL_TIME        16
+#define OFFSET_DHS_ZERO_TIME         24
+
+//reg SCFG_RG_CLANE_HS_TIME
+#define OFFSET_CHS_PRE_TIME          8
+#define OFFSET_CHS_TRIAL_TIME        16
+#define OFFSET_CHS_ZERO_TIME         24
+
+
+
+//sysrst registers
+#define SRST_ASSERT0	    0x00
+#define SRST_STATUS0    	0x04
+/* Definition controller bit for syd rst registers */
+#define BIT_RST_DSI_DPI_PIX		17
+
+
+static inline u32 sf_fb_cfgread32(struct sf_fb_data *sf_dev, u32 reg)
+{
+	return ioread32(sf_dev->base_syscfg + reg);
+}
+
+static inline void sf_fb_cfgwrite32(struct sf_fb_data *sf_dev, u32 reg, u32 val)
+{
+	iowrite32(val, sf_dev->base_syscfg + reg);
+}
+
+static inline u32 sf_fb_rstread32(struct sf_fb_data *sf_dev, u32 reg)
+{
+	return ioread32(sf_dev->base_rst + reg);
+}
+
+static inline void sf_fb_rstwrite32(struct sf_fb_data *sf_dev, u32 reg, u32 val)
+{
+	iowrite32(val, sf_dev->base_rst + reg);
+}
+
+#endif
+
diff --git a/drivers/video/fbdev/starfive/starfive_display_dev.c b/drivers/video/fbdev/starfive/starfive_display_dev.c
new file mode 100755
index 000000000000..54cdbd6a5b0b
--- /dev/null
+++ b/drivers/video/fbdev/starfive/starfive_display_dev.c
@@ -0,0 +1,137 @@
+/* driver/video/starfive/starfive_display_dev.c
+**
+** This program is free software; you can redistribute it and/or modify
+** it under the terms of the GNU General Public License version 2 as
+** published by the Free Software Foundation.
+**
+** Copyright (C) 2020 StarFive, Inc.
+**
+** PURPOSE:	This files contains the driver of LCD controller and VPP.
+**
+** CHANGE HISTORY:
+**	Version		Date		Author		Description
+**	0.1.0		2020-11-10	starfive		created
+**
+*/
+#include <linux/of_device.h>
+#include <linux/of.h>
+#include <linux/mutex.h>
+#include "starfive_display_dev.h"
+
+struct sf_fb_display_dev_data {
+	struct sf_fb_display_dev* dev;
+	struct list_head list;
+};
+
+static DEFINE_MUTEX(sf_fb_display_dev_lock);
+static LIST_HEAD(sf_fb_display_dev_list);
+
+int sf_fb_display_dev_register(struct sf_fb_display_dev* dev)
+{
+	struct sf_fb_display_dev_data *display_dev;
+
+	display_dev = kzalloc(sizeof(struct sf_fb_display_dev_data), GFP_KERNEL);
+	if (!display_dev)
+		return -ENOMEM;
+
+	INIT_LIST_HEAD(&display_dev->list);
+	display_dev->dev = dev;
+
+	mutex_lock(&sf_fb_display_dev_lock);
+	list_add_tail(&display_dev->list, &sf_fb_display_dev_list);
+	mutex_unlock(&sf_fb_display_dev_lock);
+
+	return 0;
+}
+EXPORT_SYMBOL(sf_fb_display_dev_register);
+
+int sf_fb_display_dev_unregister(struct sf_fb_display_dev* dev)
+{
+	struct sf_fb_display_dev_data *display_dev;
+
+	mutex_lock(&sf_fb_display_dev_lock);
+	list_for_each_entry(display_dev, &sf_fb_display_dev_list, list) {
+		if (display_dev->dev == dev) {
+			list_del_init(&display_dev->list);
+			kfree(display_dev);
+		}
+	}
+	mutex_unlock(&sf_fb_display_dev_lock);
+
+	return 0;
+}
+EXPORT_SYMBOL(sf_fb_display_dev_unregister);
+
+static int build_dev_list(struct sf_fb_data *fb_data)
+{
+	int rc = 0;
+
+	rc = of_platform_populate(fb_data->dev->of_node, NULL, NULL, fb_data->dev);
+	if (rc) {
+		dev_err(fb_data->dev, "%s: failed to add child nodes, rc=%d\n",
+			__func__, rc);
+	}
+
+	return rc;
+}
+
+struct sf_fb_display_dev* sf_fb_display_dev_get_by_name(char *dev_name)
+{
+	struct sf_fb_display_dev_data *display_dev;
+	struct sf_fb_display_dev *dev = NULL;
+	char *connect_panel_name;
+
+	connect_panel_name = dev_name;
+	mutex_lock(&sf_fb_display_dev_lock);
+	list_for_each_entry(display_dev, &sf_fb_display_dev_list, list) {
+		if(!strcmp(connect_panel_name, display_dev->dev->name)) {
+			dev = display_dev->dev;
+			printk(KERN_INFO "select displayer: %s\n", dev->name);
+			break;
+		}
+	}
+
+	if (!dev) {
+			display_dev = list_first_entry(&sf_fb_display_dev_list, typeof(*display_dev), list);
+			dev = display_dev->dev;
+			printk(KERN_INFO "default get first displayer(%s)! \n", display_dev->dev->name);
+	}
+	mutex_unlock(&sf_fb_display_dev_lock);
+
+	return dev;
+}
+EXPORT_SYMBOL(sf_fb_display_dev_get_by_name);
+
+struct sf_fb_display_dev* sf_fb_display_dev_get(struct sf_fb_data *fb_data)
+{
+	struct sf_fb_display_dev_data *display_dev;
+	struct sf_fb_display_dev *dev = NULL;
+	char *connect_panel_name;
+
+	build_dev_list(fb_data);
+
+	connect_panel_name = fb_data->dis_dev_name;
+	mutex_lock(&sf_fb_display_dev_lock);
+	list_for_each_entry(display_dev, &sf_fb_display_dev_list, list) {
+		if(!strcmp(connect_panel_name, display_dev->dev->name)) {
+			dev = display_dev->dev;
+			dev_info(fb_data->dev, "select displayer: %s\n", dev->name);
+			break;
+		}
+	}
+
+	if (!dev) {
+			display_dev = list_first_entry(&sf_fb_display_dev_list, typeof(*display_dev), list);
+			dev = display_dev->dev;
+			dev_info(fb_data->dev,"default get first displayer(%s)! \n", display_dev->dev->name);
+	}
+
+	mutex_unlock(&sf_fb_display_dev_lock);
+
+	return dev;
+}
+EXPORT_SYMBOL(sf_fb_display_dev_get);
+
+MODULE_AUTHOR("StarFive Technology Co., Ltd.");
+MODULE_DESCRIPTION("framebuffer device for StarFive");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/fbdev/starfive/starfive_display_dev.h b/drivers/video/fbdev/starfive/starfive_display_dev.h
new file mode 100755
index 000000000000..36da01891fbc
--- /dev/null
+++ b/drivers/video/fbdev/starfive/starfive_display_dev.h
@@ -0,0 +1,273 @@
+/*
+ * StarFive Vout driver
+ *
+ * Copyright 2020 StarFive Inc.
+ *
+ * Licensed under the GPL-2.
+ */
+
+#ifndef __SF_FB_DISPLAY_DEV_H_
+#define __SF_FB_DISPLAY_DEV_H_
+
+#include <linux/fb.h>
+#include <linux/delay.h>
+#include <linux/gpio.h>
+#include "starfive_fb.h"
+
+#define RST_SEQ_LEN		16
+
+#define FB_HSYNC_HIGH_ACT		(0x03)
+#define FB_VSYNC_HIGH_ACT		(0x04)
+
+#define MIPI_VIDEO_MODE 		(0x05)
+#define MIPI_COMMAND_MODE 		(0x06)
+
+#define ARRAY_AND_SIZE(x)		(u8 *)(x), ARRAY_SIZE(x)
+
+/*display prefer and ce*/
+#define COLOR_TYPE_MAX			(3)
+#define PREFER_WARM			(1)
+#define PREFER_NATURE			(2)
+#define PREFER_COOL			(3)
+
+#define CE_BRIGHT 			(12)
+#define CE_VELVIA			(11)
+#define CE_STANDARD			(10)
+
+
+enum {
+	DCS_CMD = 2,
+	GEN_CMD,
+	SW_PACK0,
+	SW_PACK1,
+	SW_PACK2,
+	LW_PACK,
+	SHUTDOWN_SW_PACK,
+};
+
+/*Device flags*/
+#define	PREFER_CMD_SEND_MONOLITHIC	(0x00000001)
+#define	CE_CMD_SEND_MONOLITHIC		(0x00000002)
+
+#define RESUME_WITH_PREFER		(0x00000010)
+#define RESUME_WITH_CE			(0x00000020)
+
+
+/*
+ * Video stream type
+ */
+typedef enum {
+	VIDEO_NON_BURST_WITH_SYNC_PULSES = 0,
+	VIDEO_NON_BURST_WITH_SYNC_EVENTS,	// hs_freq and pck should be multipe
+	VIDEO_BURST_WITH_SYNC_PULSES,
+}dsih_video_mode_t;
+
+#ifdef CONFIG_FBCON_DRAW_PANIC_TEXT
+extern int kpanic_in_progress;
+#endif
+
+/*struct sf_fb_dev_cmds.cmds must follow this format*/
+struct wr_cmd_hdr {
+	u8 wait;
+	u8 cmd_type0;
+	u8 cmd_type1;
+	u8 dlen;
+} __packed;
+
+struct sf_fb_dev_cmds {
+	unsigned char *cmds;
+	unsigned short n_pack;
+	unsigned short n_cmds;
+};
+
+struct bl_cmds {
+	struct sf_fb_dev_cmds bl_cmd;
+	unsigned int brightness_bit;
+};
+
+struct mipi_color_bits {
+	unsigned int color_bits; // must be set !!
+	unsigned int is_18bit_loosely; // optional
+};
+
+struct rw_timeout {
+	unsigned int hs_rd_to_cnt;
+	unsigned int lp_rd_to_cnt;
+	unsigned int hs_wr_to_cnt;
+	unsigned int lp_wr_to_cnt;
+	unsigned int bta_to_cnt;
+};
+
+struct video_mode_info {
+	unsigned int hsync;	/* Horizontal Synchronization, unit : pclk. */
+	unsigned int hbp;	/* Horizontal Back Porch, unit : pclk. */
+	unsigned int hfp;	/* Horizontal Front Porch, unit : pclk. */
+	unsigned int vsync;	/* Vertical Synchronization, unit : line. */
+	unsigned int vbp;	/* Vertical Back Porch, unit : line. */
+	unsigned int vfp;	/* Vertical Front Porch, unit : line. */
+	unsigned int sync_pol;
+	unsigned int lp_cmd_en:1;
+	unsigned int frame_bta_ack:1;
+	unsigned int lp_hfp_en:1; // default should be 1
+	unsigned int lp_hbp_en:1;
+	unsigned int lp_vact_en:1;
+	unsigned int lp_vfp_en:1;
+	unsigned int lp_vbp_en:1;
+	unsigned int lp_vsa_en:1;
+	dsih_video_mode_t mipi_trans_type; /* burst or no burst*/
+};
+
+struct command_mode_info {
+	unsigned int tear_fx_en:1;
+	unsigned int ack_rqst_en:1;
+	unsigned int gen_sw_0p_tx:1;	// default should be 1
+	unsigned int gen_sw_1p_tx:1;	// default should be 1
+	unsigned int gen_sw_2p_tx:1;	// default should be 1
+	unsigned int gen_sr_0p_tx:1;	// default should be 1
+	unsigned int gen_sr_1p_tx:1;	// default should be 1
+	unsigned int gen_sr_2p_tx:1;	// default should be 1
+	unsigned int gen_lw_tx:1;		// default should be 1
+	unsigned int dcs_sw_0p_tx:1;	// default should be 1
+	unsigned int dcs_sw_1p_tx:1;	// default should be 1
+	unsigned int dcs_sr_0p_tx:1;	// default should be 1
+	unsigned int dcs_lw_tx:1;		// default should be 1
+	unsigned int max_rd_pkt_size:1;	// default should be 1
+	struct rw_timeout timeout;
+};
+
+struct external_info {
+	unsigned char crc_rx_en:1;
+	unsigned char ecc_rx_en:1;
+	unsigned char eotp_rx_en:1;
+	unsigned char eotp_tx_en:1;
+	unsigned int dev_read_time;	//HSBYTECLK is danwe
+};
+
+struct phy_time_info {
+	unsigned char lpx;
+	//unsigned char clk_lpx;
+	unsigned char clk_tprepare;
+	unsigned char clk_hs_zero;
+	unsigned char clk_hs_trail;
+	unsigned char clk_hs_exit;
+	unsigned char clk_hs_post;
+
+	//unsigned char data_lpx;
+	unsigned char data_tprepare;
+	unsigned char data_hs_zero;
+	unsigned char data_hs_trail;
+	unsigned char data_hs_exit;
+	unsigned char data_hs_post;
+};
+
+struct te_info {
+	unsigned int te_source;
+	unsigned int te_trigger_mode;
+	unsigned int te_en;
+	unsigned int te_sync_en; // In command mode should set 1,  video should set 0
+	unsigned int te_cps;	// te count per second
+};
+
+struct sf_fb_timing_mipi {
+	unsigned int hs_freq; /*PHY output freq, bytes KHZ*/
+	unsigned int lp_freq; /*default is 10MHZ*/
+	unsigned int fps;
+	unsigned int dphy_bps;
+	unsigned int dsi_burst_mode;
+	unsigned int dsi_sync_pulse;
+	unsigned int dsi_hsa;
+	unsigned int dsi_hbp;
+	unsigned int dsi_hfp;
+	unsigned int dsi_vsa;
+	unsigned int dsi_vbp;
+	unsigned int dsi_vfp;
+	unsigned int no_lanes; /*lane numbers*/
+	unsigned int display_mode; //video mode or command mode.
+	unsigned int auto_stop_clklane_en;
+	unsigned int im_pin_val; /*IM PIN val, if use gpio_im, default config is 1  ?? */
+	struct mipi_color_bits color_mode; /*color bits*/
+	struct video_mode_info videomode_info;
+	struct command_mode_info commandmode_info;
+	struct phy_time_info phytime_info;
+	struct te_info teinfo;
+	struct external_info ext_info;
+};
+
+struct sf_fb_timing_rgb {
+	struct video_mode_info videomode_info;
+};
+
+struct rd_cmd_hdr {
+	unsigned char pack_type;
+	unsigned char cmd;
+	unsigned char id_count;
+} __packed;
+struct common_id_info {
+	struct rd_cmd_hdr hdr;
+	unsigned char id[6];
+} __packed;
+
+struct sf_fb_id_info {
+	unsigned char num_id_info;
+	struct common_id_info *id_info;
+	struct sf_fb_dev_cmds prepare_cmd;
+};
+
+struct prefer_ce_info {
+	int type;
+	struct sf_fb_dev_cmds cmds;
+};
+
+struct sf_fb_prefer_ce {
+	int types;
+	struct prefer_ce_info *info;
+};
+
+struct sf_fb_display_dev {
+	const char* name;	/* Device name. */
+	unsigned int interface_info;//interface infomation  MIPI or RGB
+	unsigned int lcd_id;
+	unsigned int refresh_en;	/* Refresh enable. */
+	unsigned int pclk;		/* Pixel clock in HZ. */
+	unsigned int bpp;		/* Bits per pixel. */
+	unsigned int xres;		/* Device resolution. */
+	unsigned int yres;
+	unsigned int width;		/* Width of device in mm. */
+	unsigned int height;		/* Height of device in mm. */
+	unsigned int flags;		/* Device flags. */
+	unsigned int auto_fps;		/* auto adjust frame rate flag*/
+	unsigned int send_suspend_cmd_in_hs_mode;   /* send suspend cmd is hs mode */
+
+	struct {
+		struct sf_fb_timing_rgb rgb;
+		//struct comipfb_dev_timing_rgb rgb;
+		struct sf_fb_timing_mipi mipi;
+	} timing;
+
+	struct sf_fb_id_info panel_id_info;
+	struct sf_fb_id_info esd_id_info;
+	struct sf_fb_dev_cmds cmds_init;
+	struct sf_fb_dev_cmds cmds_suspend;
+	struct sf_fb_dev_cmds cmds_resume;
+	struct sf_fb_dev_cmds cmds_pre_suspend;
+	struct bl_cmds backlight_info;
+
+	struct sf_fb_prefer_ce resume_prefer_info;
+	struct sf_fb_prefer_ce display_prefer_info;
+	struct sf_fb_prefer_ce display_ce_info;
+	int init_last;		/*when resume, send gamma/ce cmd before init cmd*/
+	u32 rst_seq[RST_SEQ_LEN];
+	u32 rst_seq_len;
+
+	int (*power)(struct sf_fb_data *fb_data, int onoff);
+	int (*reset)(struct sf_fb_data *fb_data);
+	int (*suspend)(struct sf_fb_data *fb_data);
+	int (*resume)(struct sf_fb_data *fb_data);
+};
+
+extern int sf_fb_display_dev_register(struct sf_fb_display_dev* dev);
+extern int sf_fb_display_dev_unregister(struct sf_fb_display_dev* dev);
+extern struct sf_fb_display_dev* sf_fb_display_dev_get_by_name(char *dev_name);
+extern struct sf_fb_display_dev* sf_fb_display_dev_get(struct sf_fb_data *fb_data);
+
+#endif /*__SF_FB_DISPLAY_DEV_H_*/
diff --git a/drivers/video/fbdev/starfive/starfive_displayer.c b/drivers/video/fbdev/starfive/starfive_displayer.c
new file mode 100755
index 000000000000..1804475997a2
--- /dev/null
+++ b/drivers/video/fbdev/starfive/starfive_displayer.c
@@ -0,0 +1,852 @@
+/* driver/video/starfive/starfive_displayer.c
+**
+** This program is free software; you can redistribute it and/or modify
+** it under the terms of the GNU General Public License version 2 as
+** published by the Free Software Foundation.
+**
+** Copyright (C) 2020 StarFive, Inc.
+**
+** PURPOSE:	This files contains the driver of LCD controller and VPP.
+**
+** CHANGE HISTORY:
+**	Version		Date		Author		Description
+**	0.1.0		2020-11-10	starfive		created
+**
+*/
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/compiler.h>
+#include <linux/init.h>
+#include <linux/of_device.h>
+#include <linux/of.h>
+#include <linux/delay.h>
+#include <linux/errno.h>
+#include <linux/platform_device.h>
+#include <linux/err.h>
+#include "starfive_fb.h"
+#include "starfive_display_dev.h"
+
+#define DSI_CMD_LEN(hdr)	(sizeof(*hdr) + (hdr)->dlen)
+
+static int sf_displayer_reset(struct sf_fb_data *fbi)
+{
+	return 0;
+}
+
+static int sf_displayer_power_on(struct sf_fb_data *fbi, int onoff)
+{
+	return 0;
+}
+
+static int sf_displayer_suspend(struct sf_fb_data *fbi)
+{
+	return 0;
+}
+
+static int sf_displayer_resume(struct sf_fb_data *fbi)
+{
+	return 0;
+}
+
+static void __maybe_unused dump_panel_info(struct device *dev,
+				struct sf_fb_display_dev *dev_data)
+{
+	dev_dbg(dev, "id info: pack_type = 0x%x, cmd = 0x%x, id_count = %d, id = 0x%x, 0x%x\n",
+			dev_data->panel_id_info.id_info->hdr.pack_type,
+			dev_data->panel_id_info.id_info->hdr.cmd,
+			dev_data->panel_id_info.id_info->hdr.id_count,
+			dev_data->panel_id_info.id_info->id[0],
+			dev_data->panel_id_info.id_info->id[1]);
+
+}
+
+static int of_parse_video_mode(struct device_node *np,
+                           struct video_mode_info *videomode_info)
+{
+	int rc;
+	u32 temp_val;
+	const char *data;
+
+	rc = of_property_read_u32(np, "h-pulse-width", &temp_val);
+	if (rc) {
+		pr_err("%s:%d, h-pulse-width not specified\n",
+						__func__, __LINE__);
+		return -EINVAL;
+	}
+	videomode_info->hsync= temp_val;
+	rc = of_property_read_u32(np, "h-back-porch", &temp_val);
+	if (rc) {
+		pr_err("%s:%d, h-back-porch not specified\n",
+						__func__, __LINE__);
+		return -EINVAL;
+	}
+	videomode_info->hbp = temp_val;
+	rc = of_property_read_u32(np, "h-front-porch", &temp_val);
+	if (rc) {
+		pr_err("%s:%d, h-front-porch not specified\n",
+						__func__, __LINE__);
+		return -EINVAL;
+	}
+	videomode_info->hfp = temp_val;
+
+	rc = of_property_read_u32(np, "v-pulse-width", &temp_val);
+	if (rc) {
+		pr_err("%s:%d, v-pulse-width not specified\n",
+						__func__, __LINE__);
+		return -EINVAL;
+	}
+	videomode_info->vsync = temp_val;
+	rc = of_property_read_u32(np, "v-back-porch", &temp_val);
+	if (rc) {
+		pr_err("%s:%d, v-back-porch not specified\n",
+						__func__, __LINE__);
+		return -EINVAL;
+	}
+	videomode_info->vbp = temp_val;
+	rc = of_property_read_u32(np, "v-front-porch", &temp_val);
+	if (rc) {
+		pr_err("%s:%d, v-front-porch not specified\n",
+						__func__, __LINE__);
+		return -EINVAL;
+	}
+	videomode_info->vfp = temp_val;
+
+	videomode_info->sync_pol = FB_VSYNC_HIGH_ACT;
+	data = of_get_property(np, "sync_pol", NULL);
+	if (data) {
+		if (!strcmp(data, "vsync_high_act"))
+			videomode_info->sync_pol = FB_VSYNC_HIGH_ACT;
+		else if (!strcmp(data, "hsync_high_act"))
+			videomode_info->sync_pol = FB_HSYNC_HIGH_ACT;
+	}
+
+	videomode_info->lp_cmd_en = of_property_read_bool(np, "lp_cmd_en");
+	videomode_info->lp_hfp_en = of_property_read_bool(np, "lp_hfp_en");
+	videomode_info->lp_hbp_en = of_property_read_bool(np, "lp_hbp_en");
+	videomode_info->lp_vact_en = of_property_read_bool(np, "lp_vact_en");
+	videomode_info->lp_vfp_en = of_property_read_bool(np, "lp_vfp_en");
+	videomode_info->lp_vbp_en = of_property_read_bool(np, "lp_vbp_en");
+	videomode_info->lp_vsa_en = of_property_read_bool(np, "lp_vsa_en");
+
+	videomode_info->mipi_trans_type = VIDEO_BURST_WITH_SYNC_PULSES;
+	data = of_get_property(np, "traffic-mode", NULL);
+	if (data) {
+		if (!strcmp(data, "burst_with_sync_pulses"))
+			videomode_info->sync_pol = VIDEO_BURST_WITH_SYNC_PULSES;
+		else if (!strcmp(data, "non_burst_with_sync_pulses"))
+			videomode_info->sync_pol = VIDEO_NON_BURST_WITH_SYNC_PULSES;
+		else if (!strcmp(data, "non_burst_with_sync_events"))
+			videomode_info->sync_pol = VIDEO_NON_BURST_WITH_SYNC_EVENTS;
+	}
+
+	return 0;
+}
+
+static int of_parse_command_mode(struct device_node *np,
+                           struct command_mode_info *cmdmode_info)
+{
+	int rc;
+	u32 temp_val;
+
+	cmdmode_info->tear_fx_en = of_property_read_bool(np, "tear_fx_en");
+	cmdmode_info->ack_rqst_en = of_property_read_bool(np, "ack_rqst_en");
+	cmdmode_info->gen_sw_0p_tx = of_property_read_bool(np, "gen_sw_0p_tx");
+	cmdmode_info->gen_sw_1p_tx = of_property_read_bool(np, "gen_sw_1p_tx");
+	cmdmode_info->gen_sw_2p_tx = of_property_read_bool(np, "gen_sw_2p_tx");
+	cmdmode_info->gen_sr_0p_tx = of_property_read_bool(np, "gen_sr_0p_tx");
+	cmdmode_info->gen_sr_1p_tx = of_property_read_bool(np, "gen_sr_1p_tx");
+	cmdmode_info->gen_sr_2p_tx = of_property_read_bool(np, "gen_sr_2p_tx");
+	cmdmode_info->gen_lw_tx = of_property_read_bool(np, "gen_lw_tx");
+	cmdmode_info->dcs_sw_0p_tx = of_property_read_bool(np, "dcs_sw_0p_tx");
+	cmdmode_info->dcs_sw_1p_tx = of_property_read_bool(np, "dcs_sw_1p_tx");
+	cmdmode_info->dcs_sr_0p_tx = of_property_read_bool(np, "dcs_sr_0p_tx");
+	cmdmode_info->dcs_lw_tx = of_property_read_bool(np, "dcs_lw_tx");
+	cmdmode_info->max_rd_pkt_size = of_property_read_bool(np, "max_rd_pkt_size");
+
+	rc = of_property_read_u32(np, "hs_rd_to_cnt", &temp_val);
+	if (rc) {
+		pr_err("%s:%d, hs_rd_to_cnt not specified\n", __func__, __LINE__);
+		return -EINVAL;
+	}
+	cmdmode_info->timeout.hs_rd_to_cnt = temp_val;
+
+	rc = of_property_read_u32(np, "lp_rd_to_cnt", &temp_val);
+	if (rc) {
+		pr_err("%s:%d, lp_rd_to_cnt not specified\n", __func__, __LINE__);
+		return -EINVAL;
+	}
+	cmdmode_info->timeout.lp_rd_to_cnt = temp_val;
+
+	rc = of_property_read_u32(np, "hs_wr_to_cnt", &temp_val);
+	if (rc) {
+		pr_err("%s:%d, hs_wr_to_cnt not specified\n", __func__, __LINE__);
+		return -EINVAL;
+	}
+	cmdmode_info->timeout.hs_wr_to_cnt = temp_val;
+
+	rc = of_property_read_u32(np, "lp_wr_to_cnt", &temp_val);
+	if (rc) {
+		pr_err("%s:%d, lp_wr_to_cnt not specified\n", __func__, __LINE__);
+		return -EINVAL;
+	}
+	cmdmode_info->timeout.lp_wr_to_cnt = temp_val;
+
+	rc = of_property_read_u32(np, "bta_to_cnt", &temp_val);
+	if (rc) {
+		pr_err("%s:%d, bta_to_cnt not specified\n",
+						__func__, __LINE__);
+		return -EINVAL;
+	}
+	cmdmode_info->timeout.bta_to_cnt = temp_val;
+
+	return 0;
+
+}
+
+static int of_parse_phy_timing(struct device_node *np,
+				struct phy_time_info *phy_timing)
+{
+	int rc;
+	u8 temp_val;
+
+	rc = of_property_read_u8(np, "data_tprepare", &temp_val);
+	if (rc) {
+		pr_err("%s:%d, data_tprepare not specified\n", __func__, __LINE__);
+		return -EINVAL;
+	}
+	phy_timing->data_tprepare = (!rc ? temp_val : 0);
+
+	rc = of_property_read_u8(np, "data_hs_zero", &temp_val);
+	if (rc) {
+		pr_err("%s:%d, data_hs_zero not specified\n", __func__, __LINE__);
+		return -EINVAL;
+	}
+	phy_timing->data_hs_zero = temp_val;
+
+	rc = of_property_read_u8(np, "data_hs_exit", &temp_val);
+	if (rc) {
+		pr_err("%s:%d, data_hs_exit not specified\n",
+						__func__, __LINE__);
+		return -EINVAL;
+	}
+	phy_timing->data_hs_exit = temp_val;
+
+	rc = of_property_read_u8(np, "data_hs_trail", &temp_val);
+	if (rc) {
+		pr_err("%s:%d, data_hs_trail not specified\n",
+						__func__, __LINE__);
+		return -EINVAL;
+	}
+	phy_timing->data_hs_trail = temp_val;
+
+	return 0;
+}
+
+static int of_parse_te_info(struct device_node *np,
+				struct te_info *teinfo)
+{
+	int rc;
+	u32 temp_val;
+	const char *data;
+
+	teinfo->te_source = 1;
+	data = of_get_property(np, "te_source", NULL);
+	if (data) {
+		if (!strcmp(data, "external_pin"))
+			teinfo->te_source = 1;
+		else if (!strcmp(data, "dsi_te_trigger"))
+			teinfo->te_source = 0;
+	}
+
+	teinfo->te_trigger_mode = 1;
+	data = of_get_property(np, "te_trigger_mode", NULL);
+	if (data) {
+		if (!strcmp(data, "rising_edge"))
+			teinfo->te_source = 0;
+		else if (!strcmp(data, "high_1000us"))
+			teinfo->te_source = 1;
+	}
+
+	rc = of_property_read_u32(np, "te_enable", &temp_val);
+	if (rc) {
+		pr_err("%s:%d, te_enable not specified\n",
+						__func__, __LINE__);
+		return -EINVAL;
+	}
+	teinfo->te_en = temp_val;
+
+	rc = of_property_read_u32(np, "cm_te_effect_sync_enable", &temp_val);
+	if (rc) {
+		pr_err("%s:%d, cm_te_effect_sync_enable not specified\n",
+						__func__, __LINE__);
+		return -EINVAL;
+	}
+	teinfo->te_sync_en = temp_val;
+
+	rc = of_property_read_u32(np, "te_count_per_sec", &temp_val);
+	if (rc) {
+		pr_err("%s:%d, te_count_per_sec not specified\n",
+						__func__, __LINE__);
+		return -EINVAL;
+	}
+	teinfo->te_cps = temp_val;
+
+	return 0;
+}
+
+static int of_parse_ext_info(struct device_node *np,
+				struct external_info *ext_info)
+{
+	int rc;
+	u32 temp_val;
+
+	ext_info->crc_rx_en = of_property_read_bool(np, "crc_rx_en");
+	ext_info->ecc_rx_en = of_property_read_bool(np, "ecc_rx_en");
+	ext_info->eotp_rx_en = of_property_read_bool(np, "eotp_rx_en");
+	ext_info->eotp_tx_en = of_property_read_bool(np, "eotp_tx_en");
+
+	rc = of_property_read_u32(np, "dev_read_time", &temp_val);
+	if (rc) {
+		pr_err("%s:%d, dev_read_time not specified\n", __func__, __LINE__);
+		return -EINVAL;
+	}
+	ext_info->dev_read_time = temp_val;
+
+	return 0;
+}
+
+static int of_parse_mipi_timing(struct device_node *np,
+				struct sf_fb_timing_mipi *mipi_timing)
+{
+	int rc;
+	u32 temp_val;
+	const char *data;
+
+	rc = of_property_read_u32(np, "mipi-byte-clock", &temp_val);
+	if (rc) {
+		pr_err("%s:%d, mipi-byte-clock not specified\n",
+						__func__, __LINE__);
+		return -EINVAL;
+	}
+	mipi_timing->hs_freq = temp_val;
+
+	rc = of_property_read_u32(np, "mipi-escape-clock", &temp_val);
+	if (rc) {
+		pr_err("%s:%d, mipi-escape-clock not specified\n", __func__, __LINE__);
+		return -EINVAL;
+	}
+	mipi_timing->lp_freq= temp_val;
+
+	rc = of_property_read_u32(np, "lane-no", &temp_val);
+	if (rc) {
+		pr_err("%s:%d, lane-no not specified\n", __func__, __LINE__);
+		return -EINVAL;
+	}
+	mipi_timing->no_lanes= temp_val;
+
+	rc = of_property_read_u32(np, "fps", &temp_val);
+	if (rc) {
+		pr_err("%s:%d, fps not specified\n", __func__, __LINE__);
+		return -EINVAL;
+	}
+	mipi_timing->fps = temp_val;
+
+	rc = of_property_read_u32(np, "dphy_bps", &temp_val);
+	if (rc) {
+		pr_err("%s:%d, dphy_bps not specified\n", __func__, __LINE__);
+		return -EINVAL;
+	}
+	mipi_timing->dphy_bps = temp_val;
+
+	rc = of_property_read_u32(np, "dsi_burst_mode", &temp_val);
+	if (rc) {
+		pr_err("%s:%d, dsi_burst_mode not specified\n", __func__, __LINE__);
+		return -EINVAL;
+	}
+	mipi_timing->dsi_burst_mode = temp_val;
+
+	rc = of_property_read_u32(np, "dsi_sync_pulse", &temp_val);
+	if (rc) {
+		pr_err("%s:%d, dsi_sync_pulse not specified\n", __func__, __LINE__);
+		return -EINVAL;
+	}
+	mipi_timing->dsi_sync_pulse = temp_val;
+
+	rc = of_property_read_u32(np, "dsi_hsa", &temp_val);
+	if (rc) {
+		pr_err("%s:%d, dsi_hsa not specified\n", __func__, __LINE__);
+		return -EINVAL;
+	}
+	mipi_timing->dsi_hsa = temp_val;
+
+	rc = of_property_read_u32(np, "dsi_hbp", &temp_val);
+	if (rc) {
+		pr_err("%s:%d, dsi_hbp not specified\n", __func__, __LINE__);
+		return -EINVAL;
+	}
+	mipi_timing->dsi_hbp = temp_val;
+
+	rc = of_property_read_u32(np, "dsi_hfp", &temp_val);
+	if (rc) {
+		pr_err("%s:%d, dsi_hfp not specified\n", __func__, __LINE__);
+		return -EINVAL;
+	}
+	mipi_timing->dsi_hfp = temp_val;
+
+	rc = of_property_read_u32(np, "dsi_vsa", &temp_val);
+	if (rc) {
+		pr_err("%s:%d, dsi_vsa not specified\n", __func__, __LINE__);
+		return -EINVAL;
+	}
+	mipi_timing->dsi_vsa = temp_val;
+
+	rc = of_property_read_u32(np, "dsi_vbp", &temp_val);
+	if (rc) {
+		pr_err("%s:%d, dsi_vbp not specified\n", __func__, __LINE__);
+		return -EINVAL;
+	}
+	mipi_timing->dsi_vbp = temp_val;
+
+	rc = of_property_read_u32(np, "dsi_vfp", &temp_val);
+	if (rc) {
+		pr_err("%s:%d, dsi_vfp not specified\n", __func__, __LINE__);
+		return -EINVAL;
+	}
+	mipi_timing->dsi_vfp = temp_val;
+
+	/*default use video mode*/
+	mipi_timing->display_mode = MIPI_VIDEO_MODE;
+	data = of_get_property(np, "display_mode", NULL);
+	if (data) {
+		if (!strcmp(data, "video_mode"))
+			mipi_timing->display_mode = MIPI_VIDEO_MODE;
+		else if (!strcmp(data, "command_mode"))
+			mipi_timing->display_mode = MIPI_COMMAND_MODE;
+	}
+
+	mipi_timing->auto_stop_clklane_en = of_property_read_bool(np,
+										"auto_stop_clklane_en");
+	mipi_timing->im_pin_val = of_property_read_bool(np, "im_pin_val");
+
+	rc = of_property_read_u32(np, "color_bits", &temp_val);
+	if (rc) {
+		pr_err("%s:%d, color_bits not specified\n", __func__, __LINE__);
+		return -EINVAL;
+	}
+	mipi_timing->color_mode.color_bits= temp_val;
+	mipi_timing->color_mode.is_18bit_loosely = of_property_read_bool(np,
+						"is_18bit_loosely");
+
+	/*video mode info*/
+	if (mipi_timing->display_mode == MIPI_VIDEO_MODE) {
+		of_parse_video_mode(np, &mipi_timing->videomode_info);
+	} else if (mipi_timing->display_mode == MIPI_COMMAND_MODE) {
+		of_parse_command_mode(np, &mipi_timing->commandmode_info);
+	}
+
+	of_parse_phy_timing(np, &mipi_timing->phytime_info);
+	of_parse_te_info(np, &mipi_timing->teinfo);
+	of_parse_ext_info(np, &mipi_timing->ext_info);
+
+	return 0;
+}
+
+static int of_parse_rgb_timing(struct device_node *np,
+			struct sf_fb_timing_rgb *rgb_timing)
+{
+	int ret;
+
+	ret = of_parse_video_mode(np, &rgb_timing->videomode_info);
+	if (ret) {
+		return -EINVAL;
+	}
+
+	return ret;
+}
+
+static int of_parse_rd_cmd_info(struct device_node *np,
+		struct sf_fb_id_info *rd_id_info, const char *key)
+{
+	int blen = 0, len;
+	int i, cnt;
+	const char *data, *bp;
+	struct rd_cmd_hdr *hdr;
+
+	data = of_get_property(np, key, &blen);
+	if (!data) {
+		pr_err("%s: failed, key=%s\n", __func__, key);
+		return -ENOMEM;
+	}
+
+	bp = data;
+	len = blen;
+	cnt = 0;
+	while (len >= sizeof(*hdr)) {
+		hdr = (struct rd_cmd_hdr *)bp;
+		if (hdr->id_count > len) {
+			pr_err("%s: rd cmd parse error", __func__);
+			return -EINVAL;
+		}
+		bp += sizeof(*hdr);
+		len -= sizeof(*hdr);
+		bp += hdr->id_count;
+		len -= hdr->id_count;
+		cnt++;
+	}
+
+	if (len != 0) {
+		pr_err("%s: rd cmd parse error!", __func__);
+		return -EINVAL;
+	}
+
+	rd_id_info->num_id_info = cnt;
+	rd_id_info->id_info = kzalloc(cnt * sizeof(struct common_id_info),
+						GFP_KERNEL);
+	if (!rd_id_info->id_info)
+		return -ENOMEM;
+
+	bp = data;
+	for (i = 0; i < cnt; i++) {
+		hdr = (struct rd_cmd_hdr *)bp;
+		bp += sizeof(*hdr);
+		rd_id_info->id_info[i].hdr = *hdr;
+		memcpy(rd_id_info->id_info[i].id, bp, hdr->id_count);
+		bp += hdr->id_count;
+	}
+
+	return 0;
+}
+
+static int of_parse_wr_cmd(struct device_node *np,
+		struct sf_fb_dev_cmds *dev_cmds, const char *key)
+{
+	int blen = 0, len;
+	int i, cnt;
+	unsigned int alloc_bytes = 0;
+	const char *data, *bp;
+	char *buf;
+	struct wr_cmd_hdr *hdr;
+
+	data = of_get_property(np, key, &blen);
+	if (!data) {
+		pr_err("%s: failed, key=%s\n", __func__, key);
+		return -ENOMEM;
+	}
+
+	bp = data;
+	len = blen;
+	cnt = 0;
+	while (len >= sizeof(*hdr)) {
+		hdr = (struct wr_cmd_hdr *)bp;
+		if (hdr->dlen > len) {
+			pr_err("%s: wr parse error", __func__);
+			return -EINVAL;
+		}
+		bp += sizeof(*hdr);
+		len -= sizeof(*hdr);
+		bp += hdr->dlen;
+		len -= hdr->dlen;
+		cnt++;
+		alloc_bytes += DSI_CMD_LEN(hdr);
+	}
+
+	if (len != 0) {
+		pr_err("%s: wr parse error!", __func__);
+		return -EINVAL;
+	}
+	dev_cmds->n_pack = cnt;
+	dev_cmds->cmds = kzalloc(round_up(alloc_bytes, 4), GFP_KERNEL);
+
+	if (IS_ERR_OR_NULL(dev_cmds->cmds))
+		return -ENOMEM;
+
+	bp = data;
+	buf = dev_cmds->cmds;
+	for (i = 0; i < cnt; i++) {
+		len = 0;
+		hdr = (struct wr_cmd_hdr *)bp;
+		len += sizeof(*hdr);
+		len += hdr->dlen;
+		memcpy(buf, bp, len);
+		bp += len;
+		buf += DSI_CMD_LEN(hdr);
+	}
+
+	return 0;
+}
+
+static int of_parse_gamma_ce_cmd(struct device_node *np, struct sf_fb_prefer_ce
+				*color_info, const char *key)
+{
+	int types = 0;
+
+	/*FIX ME, we only support up to 3 types, do not overflow.
+	* when add new gamma/ce types, please increase COLOR_TYPE_MAX also
+	*/
+	color_info->info = kzalloc(COLOR_TYPE_MAX * sizeof(struct prefer_ce_info), GFP_KERNEL);
+	if (!color_info->info) {
+		pr_err("%s no memory!!\n", __func__);
+		return -ENOMEM;
+	}
+
+	if (!strcmp(key, "gamma")) {
+		if (of_find_property(np, "panel-gamma-warm-command", NULL)) {
+			of_parse_wr_cmd(np, &color_info->info[types].cmds,
+					"panel-gamma-warm-command");
+			color_info->info[types].type = PREFER_WARM;
+			types++;
+		}
+		if (of_find_property(np, "panel-gamma-nature-command", NULL)) {
+			of_parse_wr_cmd(np, &color_info->info[types].cmds,
+					"panel-gamma-nature-command");
+			color_info->info[types].type = PREFER_NATURE;
+			types++;
+		}
+		if (of_find_property(np, "panel-gamma-cool-command", NULL)) {
+			of_parse_wr_cmd(np, &color_info->info[types].cmds,
+					"panel-gamma-cool-command");
+			color_info->info[types].type = PREFER_COOL;
+			types++;
+		}
+	} else if (!strcmp(key, "ce")) {
+		if (of_find_property(np, "panel-ce-bright-command", NULL)) {
+			of_parse_wr_cmd(np, &color_info->info[types].cmds,
+					"panel-ce-bright-command");
+			color_info->info[types].type = CE_BRIGHT;
+			types++;
+		}
+		if (of_find_property(np, "panel-ce-std-command", NULL)) {
+			of_parse_wr_cmd(np, &color_info->info[types].cmds,
+					"panel-ce-std-command");
+			color_info->info[types].type = CE_STANDARD;
+			types++;
+		}
+		if (of_find_property(np, "panel-ce-vivid-command", NULL)) {
+			of_parse_wr_cmd(np, &color_info->info[types].cmds,
+					"panel-ce-vivid-command");
+			color_info->info[types].type = CE_VELVIA;
+			types++;
+		}
+	}
+	if (types > COLOR_TYPE_MAX) {
+		pr_err("%s types overflow %d\n", key, types);
+		types = COLOR_TYPE_MAX;
+	}
+	color_info->types = types;
+
+	pr_debug("%s support %d types\n", key, types);
+
+	return 0;
+}
+
+static int of_parse_reset_seq(struct device_node *np,u32 rst_seq[RST_SEQ_LEN],
+			u32 *rst_len, const char *name)
+{
+	int num = 0, i;
+	int rc;
+	struct property *data;
+	u32 tmp[RST_SEQ_LEN];
+
+	*rst_len = 0;
+	data = of_find_property(np, name, &num);
+	num /= sizeof(u32);
+	if (!data || !num || num > RST_SEQ_LEN || num % 2) {
+		pr_err("%s:%d, error reading %s, length found = %d\n",
+				__func__, __LINE__, name, num);
+	} else {
+		rc = of_property_read_u32_array(np, name, tmp, num);
+		if (rc)
+			pr_err("%s:%d, error reading %s, rc = %d\n",
+					__func__, __LINE__, name, rc);
+		else {
+			for (i = 0; i < num; ++i)
+				rst_seq[i] = tmp[i];
+			*rst_len = num;
+		}
+	}
+	return 0;
+}
+
+static int sf_displayer_parse_dt(struct device *dev,
+			struct sf_fb_display_dev *pandev)
+{
+	int rc;
+	struct device_node *np = dev->of_node;
+	const char *data;
+	u32 temp_val;
+
+	dev_dbg(dev, "dsi panel parse dt\n");
+	pandev->name = of_get_property(np, "panel_name", NULL);
+	pr_info("panel_name: %s\n", pandev->name);
+	pandev->interface_info = STARFIVEFB_MIPI_IF;
+	data = of_get_property(np, "interface_info", NULL);
+	if (data) {
+		if (!strcmp(data, "mipi_interface"))
+			pandev->interface_info = STARFIVEFB_MIPI_IF;
+		else if (!strcmp(data, "rgb_interface"))
+			pandev->interface_info = STARFIVEFB_RGB_IF;
+	}
+	pandev->send_suspend_cmd_in_hs_mode = of_property_read_bool(np,
+		"send_suspend_cmd_in_hs");
+	/*must define within video mode*/
+	rc = of_property_read_u32(np, "refresh_en", &temp_val);
+	if (rc && (rc != -EINVAL)) {
+		pr_err("%s:%d, Unable to read refresh_en\n",
+						__func__, __LINE__);
+		return rc;
+	} else if (rc != -EINVAL)
+		pandev->refresh_en= temp_val;
+
+	pandev->auto_fps = of_property_read_bool(np, "dyn_fps");
+
+	rc = of_property_read_u32(np, "pixel-clock", &temp_val);
+	if (rc) {
+		pr_err("%s:%d, pixel-clock not specified\n", __func__, __LINE__);
+		return -EINVAL;
+	}
+	pandev->pclk= temp_val;
+
+	rc = of_property_read_u32(np, "panel-width", &temp_val);
+	if (rc) {
+		pr_err("%s:%d, panel width not specified\n", __func__, __LINE__);
+		return -EINVAL;
+	}
+	pandev->xres = temp_val;
+
+	rc = of_property_read_u32(np, "panel-height", &temp_val);
+	if (rc) {
+		pr_err("%s:%d, panel width not specified\n", __func__, __LINE__);
+		return -EINVAL;
+	}
+	pandev->yres = temp_val;
+
+	rc = of_property_read_u32(np, "physical-width", &temp_val);
+	if (rc && (rc != -EINVAL)) {
+		pr_err("%s:%d, Unable to read physical-width\n", __func__, __LINE__);
+		return rc;
+	} else if (rc != -EINVAL)
+		pandev->width = temp_val;
+
+	rc = of_property_read_u32(np, "physical-height", &temp_val);
+	if (rc && (rc != -EINVAL)) {
+		pr_err("%s:%d, Unable to read physical-height\n", __func__, __LINE__);
+		return rc;
+	} else if (rc != -EINVAL)
+		pandev->height = temp_val;
+
+	rc = of_property_read_u32(np, "bits-per-pixel", &temp_val);
+	if (rc) {
+		pr_err("%s:%d, bpp not specified\n",
+						__func__, __LINE__);
+		return -EINVAL;
+	}
+	pandev->bpp = temp_val;
+
+	pandev->flags = 0;
+	if (of_property_read_bool(np, "gamma-command-monolithic"))
+		pandev->flags |= PREFER_CMD_SEND_MONOLITHIC;
+	if (of_property_read_bool(np, "ce-command-monolithic"))
+		pandev->flags |= CE_CMD_SEND_MONOLITHIC;
+	if (of_property_read_bool(np, "resume-with-gamma"))
+		pandev->flags |= RESUME_WITH_PREFER;
+	if (of_property_read_bool(np, "resume-with-ce"))
+		pandev->flags |= RESUME_WITH_CE;
+
+	pandev->init_last = of_property_read_bool(np, "init_last");
+
+	if(STARFIVEFB_MIPI_IF == pandev->interface_info)
+		of_parse_mipi_timing(np, &pandev->timing.mipi);	/*mipi info parse*/
+	else if (STARFIVEFB_RGB_IF == pandev->interface_info)
+		of_parse_rgb_timing(np, &pandev->timing.rgb);
+
+	of_parse_rd_cmd_info(np, &pandev->panel_id_info, "id_read_cmd_info");
+	if (of_find_property(np, "pre_id_cmd", NULL))
+		of_parse_wr_cmd(np, &pandev->panel_id_info.prepare_cmd, "pre_id_cmd");
+	of_parse_rd_cmd_info(np, &pandev->esd_id_info, "esd_read_cmd_info");
+
+	if (of_find_property(np, "pre_esd_cmd", NULL))
+		of_parse_wr_cmd(np, &pandev->esd_id_info.prepare_cmd, "pre_esd_cmd");
+
+	of_parse_wr_cmd(np, &pandev->cmds_init, "panel-on-command");
+	of_parse_wr_cmd(np, &pandev->cmds_suspend, "panel-off-command");
+
+	of_parse_reset_seq(np, pandev->rst_seq, &pandev->rst_seq_len, "reset-sequence");
+
+	of_parse_gamma_ce_cmd(np, &pandev->display_prefer_info, "gamma");
+	of_parse_gamma_ce_cmd(np, &pandev->display_ce_info, "ce");
+
+	dump_panel_info(dev, pandev);
+
+	return 0;
+}
+
+static int sf_displayer_probe(struct platform_device *pdev)
+{
+	struct sf_fb_display_dev *display_dev;
+	int ret;
+
+	if (pdev->dev.of_node) {
+		display_dev = devm_kzalloc(&pdev->dev,
+				sizeof(struct sf_fb_display_dev), GFP_KERNEL);
+		if (!display_dev) {
+			dev_err(&pdev->dev, "Failed to allocate memory\n");
+			return -ENOMEM;
+		}
+		ret = sf_displayer_parse_dt(&pdev->dev, display_dev);
+		if (ret) {
+			dev_err(&pdev->dev, "DT parsing failed\n");
+			return -ENODEV;
+		}
+	} else {
+		dev_err(&pdev->dev, "error: null panel device-tree node");
+		return -ENODEV;
+	}
+
+	display_dev->power = sf_displayer_power_on;
+	display_dev->reset = sf_displayer_reset;
+	display_dev->suspend = sf_displayer_suspend;
+	display_dev->resume = sf_displayer_resume;
+	sf_fb_display_dev_register(display_dev);
+
+	return 0;
+
+}
+
+static int __exit sf_displayer_remove(struct platform_device *dev)
+{
+	return 0;
+}
+
+static struct of_device_id sf_displayer_dt_match[] = {
+	{
+		.compatible = "starfive,display-dev",
+	},
+	{}
+};
+
+static struct platform_driver sf_displayer_driver = {
+	.probe = sf_displayer_probe,
+	.remove = __exit_p(sf_displayer_remove),
+	.driver = {
+		.name = "starfive,display-dev",
+		.owner = THIS_MODULE,
+		.of_match_table = sf_displayer_dt_match,
+	},
+};
+
+static int __init sf_displayer_init(void)
+{
+	return platform_driver_register(&sf_displayer_driver);
+}
+
+static void __exit sf_displayer_exit(void)
+{
+	platform_driver_unregister(&sf_displayer_driver);
+}
+
+subsys_initcall(sf_displayer_init);
+module_exit(sf_displayer_exit);
+MODULE_AUTHOR("StarFive Technology Co., Ltd.");
+MODULE_DESCRIPTION("DISPLAYER DRIVER");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/fbdev/starfive/starfive_fb.c b/drivers/video/fbdev/starfive/starfive_fb.c
new file mode 100755
index 000000000000..bdd15d551b00
--- /dev/null
+++ b/drivers/video/fbdev/starfive/starfive_fb.c
@@ -0,0 +1,1397 @@
+/* driver/video/starfive/starfivefb.c
+**
+** This program is free software; you can redistribute it and/or modify
+** it under the terms of the GNU General Public License version 2 as
+** published by the Free Software Foundation.
+**
+** Copyright (C) 2020 StarFive, Inc.
+**
+** PURPOSE:	This files contains the driver of LCD controller and VPP.
+**
+** CHANGE HISTORY:
+**	Version		Date		Author		Description
+**	0.1.0		2020-10-07	starfive		created
+**
+*/
+
+#include <linux/errno.h>
+#include <linux/string.h>
+#include <linux/interrupt.h>
+#include <linux/compiler.h>
+#include <linux/slab.h>
+#include <linux/fb.h>
+#include <linux/delay.h>
+#include <linux/init.h>
+#include <linux/ioport.h>
+#include <linux/cpufreq.h>
+#include <linux/platform_device.h>
+#include <linux/dma-mapping.h>
+#include <linux/err.h>
+#include <linux/uaccess.h>
+#include <linux/notifier.h>
+#include <linux/mtd/mtd.h>
+#include <linux/workqueue.h>
+#include <linux/module.h>
+#include <linux/leds.h>
+#include <asm/io.h>
+#include <asm/irq.h>
+#include <asm/div64.h>
+#include <asm/cacheflush.h>
+#include <linux/of.h>
+#include <linux/of_gpio.h>
+#include <linux/of_reserved_mem.h>
+#include <linux/of_address.h>
+#include <video/stf-vin.h>
+#include "starfive_fb.h"
+#include "starfive_lcdc.h"
+#include "starfive_vpp.h"
+#include "starfive_display_dev.h"
+#include "starfive_mipi_tx.h"
+#include "sys_comm_regs.h"
+
+static struct sf_fb_data *stf_dev = NULL;
+static DEFINE_MUTEX(stf_mutex);
+static int sf_fb_pp_get_2lcdc_id(struct sf_fb_data *sf_dev);
+//#define SF_FB_DEBUG	1
+#ifdef SF_FB_DEBUG
+	#define FB_PRT(format, args...)  printk(KERN_DEBUG "[FB]: " format, ## args)
+	#define FB_INFO(format, args...) printk(KERN_INFO "[FB]: " format, ## args)
+	#define FB_ERR(format, args...)	printk(KERN_ERR "[FB]: " format, ## args)
+#else
+	#define FB_PRT(x...)  do{} while(0)
+	#define FB_INFO(x...)  do{} while(0)
+	#define FB_ERR(x...)  do{} while(0)
+#endif
+
+static int sf_fb_pp_init(struct sf_fb_data *sf_dev);
+static int sf_fb_lcdc_init(struct sf_fb_data *sf_dev);
+static void sf_fb_pp_enable_intr(struct sf_fb_data *sf_dev, int enable);
+static int sf_fb_pp_run(struct sf_fb_data *sf_dev) ;
+static const struct res_name mem_res_name[] = {
+	{"lcdc"},
+	{"dsitx"},
+	{"vpp0"},
+	{"vpp1"},
+	{"vpp2"},
+	{"clk"},
+	{"rst"},
+	{"sys"}
+};
+
+static u32 sf_fb_clkread32(struct sf_fb_data *sf_dev, u32 reg)
+{
+	return ioread32(sf_dev->base_clk + reg);
+}
+
+static void sf_fb_clkwrite32(struct sf_fb_data *sf_dev, u32 reg, u32 val)
+{
+	iowrite32(val, sf_dev->base_clk + reg);
+}
+
+static int sf_fb_lcdc_clk_cfg(struct sf_fb_data *sf_dev)
+{
+	u32 tmp_val = 0;
+	int ret = 0;
+
+	switch(sf_dev->display_info.xres) {
+	case 640:
+		dev_warn(sf_dev->dev, "640 do nothing! need to set clk\n");
+		break;
+	case 800:
+		tmp_val = sf_fb_clkread32(sf_dev, CLK_LCDC_OCLK_CTRL);
+		tmp_val &= ~(0x3F);
+		tmp_val |= (54 & 0x3F);
+		sf_fb_clkwrite32(sf_dev, CLK_LCDC_OCLK_CTRL, tmp_val);
+		break;
+	case 1024:
+		tmp_val = sf_fb_clkread32(sf_dev, CLK_LCDC_OCLK_CTRL);
+		tmp_val &= ~(0x3F);
+		tmp_val |= (48 & 0x3F);
+		sf_fb_clkwrite32(sf_dev, CLK_LCDC_OCLK_CTRL, tmp_val);
+		break;
+	case 1280:
+		tmp_val = sf_fb_clkread32(sf_dev, CLK_LCDC_OCLK_CTRL);
+		tmp_val &= ~(0x3F);
+		tmp_val |= (30 & 0x3F);
+		dev_warn(sf_dev->dev, "1280  set clk\n");
+		sf_fb_clkwrite32(sf_dev, CLK_LCDC_OCLK_CTRL, tmp_val);
+		break;
+	case 1920:
+		tmp_val = sf_fb_clkread32(sf_dev, CLK_LCDC_OCLK_CTRL);
+		tmp_val &= ~(0x3F);
+		tmp_val |= (10 & 0x3F);
+		sf_fb_clkwrite32(sf_dev, CLK_LCDC_OCLK_CTRL, tmp_val);
+		break;
+	default:
+		dev_err(sf_dev->dev, "Fail to allocate video RAM\n");
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+#if defined(CONFIG_VIDEO_STARFIVE_VIN)
+static int vin_frame_complete_notify(struct notifier_block *nb,
+				      unsigned long val, void *v)
+{
+	struct vin_params *psy = v;
+	struct sf_fb_data *sf_dev = stf_dev;
+	unsigned int address;
+	unsigned int u_addr, v_addr, size;
+	unsigned int y_rgb_offset, u_offset, v_offset;
+
+	address = (unsigned int)psy->paddr;
+
+	if(NULL == sf_dev) {
+		return NOTIFY_OK;
+	}
+
+	if(sf_dev->pp_conn_lcdc < 0) {
+		//dev_warn(sf_dev->dev, "%s NO use PPx\n",__func__);
+	} else {
+		if(sf_dev->pp[sf_dev->pp_conn_lcdc].src.format >= COLOR_RGB888_ARGB) {
+			u_addr = 0;
+			v_addr = 0;
+			y_rgb_offset = 0;
+			u_offset = 0;
+			v_offset = 0;
+		} else if (COLOR_YUV420_NV21 == sf_dev->pp[sf_dev->pp_conn_lcdc].src.format) {
+			size = sf_dev->display_info.xres * sf_dev->display_info.yres;
+			u_addr = address + size + 1;
+			v_addr = address + size;
+			y_rgb_offset = 0;
+			u_offset = 0;
+			v_offset = size;
+		} else {
+			dev_err(sf_dev->dev, "format %d not SET\n", sf_dev->pp[sf_dev->pp_conn_lcdc].src.format);
+			return -EINVAL;
+		}
+		pp_srcAddr_next(sf_dev, sf_dev->pp_conn_lcdc, address, u_addr, v_addr);
+		pp_srcOffset_cfg(sf_dev, sf_dev->pp_conn_lcdc, y_rgb_offset, u_offset, v_offset);
+		//pp_run(sf_dev, sf_dev->pp_conn_lcdc, PP_RUN);
+	}
+
+	return NOTIFY_OK;
+}
+#endif
+
+static int sf_get_mem_res(struct platform_device *pdev, struct sf_fb_data *sf_dev)
+{
+	struct device *dev = &pdev->dev;
+	struct resource *res;
+	void __iomem *regs;
+	char *name;
+	int i;
+
+	for (i = 0; i < sizeof(mem_res_name)/sizeof(struct res_name); i++) {
+		name = (char *)(& mem_res_name[i]);
+		res = platform_get_resource_byname(pdev, IORESOURCE_MEM, name);
+		regs = devm_ioremap_resource(dev, res);
+		if (IS_ERR(regs))
+			return PTR_ERR(regs);
+
+		if(!strcmp(name, "lcdc")) {
+			sf_dev->base_lcdc = regs;
+		} else if (!strcmp(name, "dsitx")) {
+			sf_dev->base_dsitx = regs;
+		} else if (!strcmp(name, "vpp0")) {
+			sf_dev->base_vpp0 = regs;
+		} else if (!strcmp(name, "vpp1")) {
+			sf_dev->base_vpp1 = regs;
+		} else if (!strcmp(name, "vpp2")) {
+			sf_dev->base_vpp2 = regs;
+		} else if (!strcmp(name, "clk")) {
+			sf_dev->base_clk = regs;
+		} else if (!strcmp(name, "rst")) {
+			sf_dev->base_rst = regs;
+		} else if (!strcmp(name, "sys")) {
+			sf_dev->base_syscfg = regs;
+		} else {
+			dev_err(&pdev->dev, "Could not match resource name\n");
+		}
+	}
+
+	return 0;
+}
+
+static void sf_fb_get_var(struct fb_var_screeninfo *var, struct sf_fb_data *sf_dev)
+{
+	var->xres = sf_dev->display_info.xres;
+	var->yres = sf_dev->display_info.yres;
+	var->bits_per_pixel = sf_dev->display_dev->bpp;
+#if defined(CONFIG_FRAMEBUFFER_CONSOLE)
+	if(24 == var->bits_per_pixel)
+		var->bits_per_pixel = 16;//as vpp&lcdc miss support rgb888 ,config fb_console src format as rgb565 
+#endif
+	var->pixclock       = 1000000 / (sf_dev->pixclock / 1000000);
+	var->hsync_len      = sf_dev->display_info.hsync_len;
+	var->vsync_len      = sf_dev->display_info.vsync_len;
+	var->left_margin    = sf_dev->display_info.left_margin;
+	var->right_margin   = sf_dev->display_info.right_margin;
+	var->upper_margin   = sf_dev->display_info.upper_margin;
+	var->lower_margin   = sf_dev->display_info.lower_margin;
+	var->sync           = sf_dev->display_info.sync;
+	var->vmode          = FB_VMODE_NONINTERLACED;
+}
+
+/*
+ * sf_fb_set_par():
+ *	Set the user defined part of the display for the specified console
+ */
+static int sf_fb_set_par(struct fb_info *info)
+{
+	struct sf_fb_data *sf_dev = container_of(info, struct sf_fb_data, fb);
+	struct fb_var_screeninfo *var = &info->var;
+
+	FB_PRT("%s,%d\n",__func__, __LINE__);
+	if (var->bits_per_pixel == 16 ||
+		var->bits_per_pixel == 18 ||
+		var->bits_per_pixel == 24 ||
+		var->bits_per_pixel == 32)
+		sf_dev->fb.fix.visual = FB_VISUAL_TRUECOLOR;
+	else if (!sf_dev->cmap_static)
+		sf_dev->fb.fix.visual = FB_VISUAL_PSEUDOCOLOR;
+	else {
+		/*
+		 * Some people have weird ideas about wanting static
+		 * pseudocolor maps.  I suspect their user space
+		 * applications are broken.
+		 */
+		sf_dev->fb.fix.visual = FB_VISUAL_STATIC_PSEUDOCOLOR;
+	}
+
+	sf_dev->fb.fix.line_length = var->xres_virtual * var->bits_per_pixel / 8;
+	if (sf_dev->fb.var.bits_per_pixel == 16 ||
+		sf_dev->fb.var.bits_per_pixel == 18 ||
+		sf_dev->fb.var.bits_per_pixel == 24 ||
+		sf_dev->fb.var.bits_per_pixel == 32)
+		fb_dealloc_cmap(&sf_dev->fb.cmap);
+	else
+		fb_alloc_cmap(&sf_dev->fb.cmap, 1 << sf_dev->fb.var.bits_per_pixel, 0);
+
+	/*for fbcon, it need cmap*/
+	switch(var->bits_per_pixel) {
+		case 16:
+			var->red.offset   = 11; var->red.length   = 5;
+			var->green.offset = 5;	var->green.length = 6;
+			var->blue.offset  = 0;	var->blue.length  = 5;
+			var->transp.offset = var->transp.length = 0;
+			break;
+		case 18:
+			var->red.offset   = 12; var->red.length   = 6;
+			var->green.offset = 6;	var->green.length = 6;
+			var->blue.offset  = 0;	var->blue.length  = 6;
+			var->transp.offset = var->transp.length = 0;
+			break;
+		case 24:
+			var->red.offset   = 16; var->red.length   = 8;
+			var->green.offset = 8;	var->green.length = 8;
+			var->blue.offset  = 0;	var->blue.length  = 8;
+			var->transp.offset = var->transp.length = 0;
+			break;
+		case 32:
+			var->red.offset   = 16; var->red.length   = 8;
+			var->green.offset = 8;	var->green.length = 8;
+			var->blue.offset  = 0;	var->blue.length  = 8;
+			var->transp.offset = var->transp.length = 0;
+			break;
+		default:
+			var->red.offset = var->green.offset = \
+				var->blue.offset = var->transp.offset = 0;
+			var->red.length   = 8;
+			var->green.length = 8;
+			var->blue.length  = 8;
+			var->transp.length = 0;
+	}
+
+	if (!strcmp(sf_dev->dis_dev_name, "tda_998x_1080p")) {
+		var->red.offset   = 0;  var->red.length   = 5;
+		var->green.offset = 5;	var->green.length = 6;
+		var->blue.offset  = 11;	var->blue.length  = 5;
+		var->transp.offset = var->transp.length = 0;
+	}
+
+	return 0;
+}
+
+// Update grayscale accroding pp src format. 0 -- RGB, 1 -- YUV. grayscale used by gstreamer fbdevsink
+static void sf_fb_update_fbinfo(struct sf_fb_data *sf_dev)
+{
+	sf_dev->fb.var.grayscale = 0;
+	if (sf_dev->pp_conn_lcdc >= 0) {
+	if ((COLOR_YUV420_NV21 == sf_dev->pp[sf_dev->pp_conn_lcdc].src.format) ||
+		(COLOR_YUV420_NV12 == sf_dev->pp[sf_dev->pp_conn_lcdc].src.format) ||
+		(COLOR_YUV420P == sf_dev->pp[sf_dev->pp_conn_lcdc].src.format)) {
+			FB_PRT("[%s,%d]: pp src format is %d\n",__func__, __LINE__,
+			sf_dev->pp[sf_dev->pp_conn_lcdc].src.format);
+			sf_dev->fb.var.grayscale = 1;
+		}
+	}
+}
+
+// Clear frame buffer. Addr is sf_dev->fb.screen_base, size is sf_dev->fb.fix.smem_len
+static int sf_fb_clearscreen(struct sf_fb_data *sf_dev)
+{
+	int y_lenth = 0, uv_lenth = 0;
+
+	FB_PRT("%s,%d, pp_conn_lcdc=%d\n",__func__, __LINE__, sf_dev->pp_conn_lcdc);
+	if (sf_dev->pp_conn_lcdc >= 0) {
+		switch (sf_dev->pp[sf_dev->pp_conn_lcdc].src.format) {
+		case COLOR_YUV420P:
+		case COLOR_YUV420_NV21:
+		case COLOR_YUV420_NV12:
+			y_lenth = sf_dev->pp[sf_dev->pp_conn_lcdc].src.width * sf_dev->pp[sf_dev->pp_conn_lcdc].src.height;
+			uv_lenth = y_lenth / 2;
+			memset(sf_dev->fb.screen_base, 0x00, y_lenth);            // set y
+			memset(sf_dev->fb.screen_base + y_lenth, 0x80, uv_lenth); // set uv
+			break;
+		case COLOR_RGB565:
+			memset(sf_dev->fb.screen_base, 0x00, sf_dev->fb.fix.smem_len);
+		default:
+			memset(sf_dev->fb.screen_base, 0x80, sf_dev->fb.fix.smem_len);
+			break;
+		}
+	}
+	return 0;
+}
+
+static int sf_fb_open(struct fb_info *info, int user)
+{
+	struct sf_fb_data *sf_dev = container_of(info, struct sf_fb_data, fb);
+
+	FB_PRT("%s,%d\n",__func__, __LINE__);
+	sf_fb_set_par(info);
+	lcdc_run(sf_dev, sf_dev->winNum, LCDC_RUN);
+
+	//sf_fb_init_layer(layer, &info->var);
+	//if (layer->no == sf_fb_ids[0])
+		//sf_fb_enable_layer(layer);
+	return 0;
+}
+
+static int sf_fb_release(struct fb_info *info, int user)
+{
+	struct sf_fb_data *sf_dev = container_of(info, struct sf_fb_data, fb);
+
+	FB_PRT("%s,%d\n",__func__, __LINE__);
+	sf_fb_clearscreen(sf_dev);
+	lcdc_run(sf_dev, sf_dev->winNum, LCDC_STOP);
+	return 0;
+}
+
+static int sf_fb_ioctl(struct fb_info *info, unsigned cmd, unsigned long arg)
+{
+//	struct sf_fb_data *sf_dev = container_of(info, struct sf_fb_data, fb);
+	FB_PRT("%s,%d\n",__func__, __LINE__);
+	return 0;
+}
+
+/*
+ *	sf_fb_check_var():
+ *	  Get the video params out of 'var'. If a value doesn't fit, round it up,
+ *	  if it's too big, return -EINVAL.
+ *
+ *	  Round up in the following order: bits_per_pixel, xres,
+ *	  yres, xres_virtual, yres_virtual, xoffset, yoffset, grayscale,
+ *	  bitfields, horizontal timing, vertical timing.
+ */
+static int sf_fb_check_var(struct fb_var_screeninfo *var, struct fb_info *info)
+{
+	struct sf_fb_data *sf_dev = container_of(info, struct sf_fb_data, fb);
+
+	FB_PRT("%s,%d\n",__func__, __LINE__);
+	if (var->xres < MIN_XRES)
+		var->xres = MIN_XRES;
+	if (var->yres < MIN_YRES)
+		var->yres = MIN_YRES;
+
+	sf_fb_get_var(var, sf_dev);
+	var->xres_virtual = var->xres;
+	var->yres_virtual = var->yres * sf_dev->buf_num;
+	/*
+	 * Setup the RGB parameters for this display.
+	 */
+	switch(var->bits_per_pixel) {
+		case 16:
+			var->red.offset   = 11; var->red.length   = 5;
+			var->green.offset = 5;	var->green.length = 6;
+			var->blue.offset  = 0;	var->blue.length  = 5;
+			var->transp.offset = var->transp.length = 0;
+			break;
+		case 18:
+			var->red.offset   = 12; var->red.length   = 6;
+			var->green.offset = 6;	var->green.length = 6;
+			var->blue.offset  = 0;	var->blue.length  = 6;
+			var->transp.offset = var->transp.length = 0;
+			break;
+		case 24:
+			var->red.offset   = 16; var->red.length   = 8;
+			var->green.offset = 8;	var->green.length = 8;
+			var->blue.offset  = 0;	var->blue.length  = 8;
+			var->transp.offset = var->transp.length = 0;
+			break;
+		case 32:
+			var->red.offset   = 16; var->red.length   = 8;
+			var->green.offset = 8;	var->green.length = 8;
+			var->blue.offset  = 0;	var->blue.length  = 8;
+			var->transp.offset = var->transp.length = 0;
+			break;
+		default:
+			var->red.offset = var->green.offset = \
+				var->blue.offset = var->transp.offset = 0;
+			var->red.length   = 8;
+			var->green.length = 8;
+			var->blue.length  = 8;
+			var->transp.length = 0;
+	}
+
+	if (!strcmp(sf_dev->dis_dev_name, "tda_998x_1080p")) {
+		var->red.offset   = 0;  var->red.length   = 5;
+		var->green.offset = 5;	var->green.length = 6;
+		var->blue.offset  = 11;	var->blue.length  = 5;
+		var->transp.offset = var->transp.length = 0;
+	}
+
+	return 0;
+}
+
+static inline u_int sf_chan_to_field(u_int chan, struct fb_bitfield *bf)
+{
+	chan &= 0xffff;
+	chan >>= 16 - bf->length;
+	return chan << bf->offset;
+}
+
+static int sf_fb_setcolreg(u_int regno, u_int red, u_int green,
+			u_int blue, u_int trans, struct fb_info *info)
+{
+	struct sf_fb_data *sf_dev = container_of(info, struct sf_fb_data, fb);
+	unsigned int val;
+	int ret = 1;
+
+	FB_PRT("%s,%d\n",__func__, __LINE__);
+	/*
+	 * If inverse mode was selected, invert all the colours
+	 * rather than the register number.  The register number
+	 * is what you poke into the framebuffer to produce the
+	 * colour you requested.
+	 */
+	if (sf_dev->cmap_inverse) {
+		red = 0xffff - red;
+		green = 0xffff - green;
+		blue = 0xffff - blue;
+	}
+
+	/*
+	 * If greyscale is true, then we convert the RGB value
+	 * to greyscale no matter what visual we are using.
+	 */
+	if (sf_dev->fb.var.grayscale)
+		red = green = blue = (19595 * red + 38470 * green + 7471 * blue) >> 16;
+
+	switch (sf_dev->fb.fix.visual) {
+	case FB_VISUAL_TRUECOLOR:
+		/*
+		 * 16-bit True Colour.	We encode the RGB value
+		 * according to the RGB bitfield information.
+		 */
+		if (regno < 16) {
+			u32 *pal = sf_dev->fb.pseudo_palette;
+
+			val = sf_chan_to_field(red, &sf_dev->fb.var.red);
+			val |= sf_chan_to_field(green, &sf_dev->fb.var.green);
+			val |= sf_chan_to_field(blue, &sf_dev->fb.var.blue);
+
+			pal[regno] = val;
+			ret = 0;
+		}
+		break;
+
+	case FB_VISUAL_STATIC_PSEUDOCOLOR:
+	case FB_VISUAL_PSEUDOCOLOR:
+		/* haven't support this function. */
+		break;
+	}
+
+	return 0;
+}
+
+/*
+ * sf_fb_set_addr():
+ *	Configures LCD Controller based on entries in var parameter.  Settings are
+ *	only written to the controller if changes were made.
+ */
+static int sf_fb_set_addr(struct sf_fb_data *sf_dev, struct fb_var_screeninfo *var)
+{
+	unsigned int address;
+	unsigned int offset;
+	unsigned int u_addr, v_addr, size;
+	unsigned int y_rgb_offset, u_offset, v_offset;
+	int i = 0;
+
+	offset = var->yoffset * sf_dev->fb.fix.line_length +
+				var->xoffset * var->bits_per_pixel / 8;
+	address = sf_dev->fb.fix.smem_start + offset;
+	size = var->xres * var->yres;
+
+	FB_PRT("%s,%d\n",__func__, __LINE__);
+
+	if(sf_dev->pp_conn_lcdc < 0) {
+		dev_warn(sf_dev->dev, "%s NO use PPx\n",__func__);
+	} else {
+		if(sf_dev->pp[sf_dev->pp_conn_lcdc].src.format >= COLOR_RGB888_ARGB) {
+			u_addr = 0;
+			v_addr = 0;
+			y_rgb_offset = 0;
+			u_offset = 0;
+			v_offset = 0;
+		} else if (COLOR_YUV420_NV21 == sf_dev->pp[sf_dev->pp_conn_lcdc].src.format) {
+			u_addr = address + size + 1;
+			v_addr = address + size;
+			y_rgb_offset = 0;
+			u_offset = 0;
+			v_offset = size;
+		} else if (COLOR_YUV420_NV12 == sf_dev->pp[sf_dev->pp_conn_lcdc].src.format) {
+			u_addr = address + size;
+			v_addr = address + size + 1;
+			y_rgb_offset = 0;
+			u_offset = 0;
+			v_offset = size;
+		} else {
+			dev_err(sf_dev->dev, "format %d not SET\n",
+				sf_dev->pp[sf_dev->pp_conn_lcdc].src.format);
+			return -EINVAL;
+		}
+		pp_srcAddr_next(sf_dev, sf_dev->pp_conn_lcdc, address, u_addr, v_addr);
+		pp_srcOffset_cfg(sf_dev, sf_dev->pp_conn_lcdc, y_rgb_offset, u_offset, v_offset);
+		pp_nxtAddr_load(sf_dev, sf_dev->pp_conn_lcdc, 0x1, (i & 0x1));
+		pp_run(sf_dev, sf_dev->pp_conn_lcdc, PP_RUN);
+	}
+
+	return 0;
+}
+
+
+static int sf_fb_pan_display(struct fb_var_screeninfo *var,
+				struct fb_info *info)
+{
+	struct sf_fb_data *sf_dev = container_of(info, struct sf_fb_data, fb);
+
+	FB_PRT("%s,%d\n",__func__, __LINE__);
+	sf_fb_set_addr(sf_dev, var);
+
+	switch(sf_dev->display_dev->interface_info) {
+		case STARFIVEFB_MIPI_IF:
+		case STARFIVEFB_HDMI_IF:
+			lcdc_run(sf_dev,0x2, 0x1);
+			break;
+		case STARFIVEFB_RGB_IF:
+			lcdc_run(sf_dev,0x2, 0x1);
+			break;
+		default:
+			break;
+	}
+
+	return 0;
+}
+
+static int sf_fb_mmap(struct fb_info *info, struct vm_area_struct *vma)
+{
+	unsigned long off = vma->vm_pgoff << PAGE_SHIFT;
+	unsigned long start;
+	u32 len;
+
+	FB_PRT("%s,%d\n",__func__, __LINE__);
+	/* frame buffer memory */
+	start = info->fix.smem_start;
+	len = PAGE_ALIGN((start & ~PAGE_MASK) + info->fix.smem_len);
+
+	if (off >= len) {
+		/* memory mapped io */
+		off -= len;
+		if (info->var.accel_flags) {
+			mutex_unlock(&info->mm_lock);
+			return -EINVAL;
+		}
+		start = info->fix.mmio_start;
+		len = PAGE_ALIGN((start & ~PAGE_MASK) + info->fix.mmio_len);
+	}
+
+	start &= PAGE_MASK;
+	if ((vma->vm_end - vma->vm_start + off) > len) {
+		return -EINVAL;
+	}
+
+	off += start;
+	vma->vm_pgoff = off >> PAGE_SHIFT;
+	/* This is an IO map - tell maydump to skip this VMA */
+//	vma->vm_flags |= VM_IO | VM_RESERVED;
+	vma->vm_flags |= VM_IO;
+
+//	if (!(layer->parent->pdata->flags & FB_CACHED_BUFFER))
+//		vma->vm_page_prot = pgprot_writecombine(vma->vm_page_prot);
+
+	if (io_remap_pfn_range(vma, vma->vm_start, off >> PAGE_SHIFT,
+			vma->vm_end - vma->vm_start, vma->vm_page_prot)) {
+		return -EAGAIN;
+	}
+
+	return 0;
+}
+
+static int sf_fb_blank(int blank, struct fb_info *info)
+{
+	struct sf_fb_data *sf_dev = container_of(info, struct sf_fb_data, fb);
+	int ret;
+
+	FB_PRT("%s,%d\n",__func__, __LINE__);
+
+	switch (blank) {
+		case FB_BLANK_UNBLANK:
+			lcdc_run(sf_dev,0x2, 0x1);
+			break;
+		case FB_BLANK_NORMAL:
+		case FB_BLANK_VSYNC_SUSPEND:
+		case FB_BLANK_HSYNC_SUSPEND:
+		case FB_BLANK_POWERDOWN:
+			lcdc_run(sf_dev,0x2, 0x0);
+			break;
+		default:
+			ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+static struct fb_ops sf_fb_ops = {
+	.owner         = THIS_MODULE,
+	.fb_open       = sf_fb_open,
+	.fb_release    = sf_fb_release,
+	.fb_ioctl      = sf_fb_ioctl,
+	.fb_check_var  = sf_fb_check_var,
+	.fb_set_par    = sf_fb_set_par,
+	.fb_setcolreg  = sf_fb_setcolreg,
+	.fb_fillrect   = cfb_fillrect,
+	.fb_copyarea   = cfb_copyarea,
+	.fb_imageblit  = cfb_imageblit,
+	.fb_pan_display= sf_fb_pan_display,
+	.fb_mmap       = sf_fb_mmap,
+	.fb_blank      = sf_fb_blank,
+};
+
+static int sf_fb_map_video_memory(struct sf_fb_data *sf_dev)
+{
+	struct resource res_mem;
+	struct device_node *node;
+	int ret;
+
+	node = of_parse_phandle(sf_dev->dev->of_node, "memory-region", 0);
+	if (!node) {
+		dev_err(sf_dev->dev, "Could not get reserved memory.\n");
+		return -ENOMEM;
+	}
+
+	ret = of_address_to_resource(node, 0, &res_mem);
+	if (ret)
+		return ret;
+
+	sf_dev->fb.screen_size = resource_size(&res_mem);
+	sf_dev->fb.fix.smem_start = res_mem.start;
+
+	sf_dev->fb.screen_base = devm_ioremap(sf_dev->dev, res_mem.start,
+							resource_size(&res_mem));
+	if (IS_ERR(sf_dev->fb.screen_base))
+		return PTR_ERR(sf_dev->fb.screen_base);
+	memset(sf_dev->fb.screen_base, 0x00, sf_dev->fb.screen_size);
+
+	return 0;
+}
+
+static int sf_fb_init(struct sf_fb_data *sf_dev)
+{
+	int ret;
+
+	INIT_LIST_HEAD(&sf_dev->fb.modelist);
+	sf_dev->fb.device = sf_dev->dev;
+	sf_dev->fb.fbops = &sf_fb_ops;
+	sf_dev->fb.flags = FBINFO_DEFAULT;
+	sf_dev->fb.node = -1;
+	sf_dev->fb.pseudo_palette = sf_dev->pseudo_pal;
+	sf_dev->fb.mode = &sf_dev->display_info;
+
+	strcpy(sf_dev->fb.fix.id, STARFIVE_NAME);
+	sf_dev->fb.fix.type = FB_TYPE_PACKED_PIXELS;
+	sf_dev->fb.fix.type_aux = 0;
+	sf_dev->fb.fix.xpanstep = 0;
+	sf_dev->fb.fix.ypanstep = 1;
+	sf_dev->fb.fix.ywrapstep = 0;
+	sf_dev->fb.fix.accel = FB_ACCEL_NONE;
+
+/*	sf_dev->win_x_size = sf_dev->display_info.xres;
+	sf_dev->win_y_size = sf_dev->display_info.yres;
+	sf_dev->src_width = sf_dev->display_info.xres;
+	sf_dev->alpha = 255; // 255 = solid 0 = transparent
+	sf_dev->alpha_en = 0;
+	sf_dev->input_format = LCDC_LAYER_INPUT_FORMAT_ARGB8888;*/
+	sf_dev->buf_num = BUFFER_NUMS;
+
+	sf_fb_get_var(&sf_dev->fb.var, sf_dev);
+	sf_dev->fb.var.xres_virtual = sf_dev->display_info.xres;
+	sf_dev->fb.var.yres_virtual = sf_dev->display_info.yres * sf_dev->buf_num;
+	sf_dev->fb.var.xoffset = 0;
+	sf_dev->fb.var.yoffset = 0;
+	sf_dev->fb.var.nonstd = 0;
+	sf_dev->fb.var.activate = FB_ACTIVATE_NOW;
+	sf_dev->fb.var.width = sf_dev->display_dev->width;
+	sf_dev->fb.var.height = sf_dev->display_dev->height;
+	sf_dev->fb.var.accel_flags = 0;
+
+/*	layer->flags = 0;
+	layer->cmap_inverse = 0;
+	layer->cmap_static = 0;
+*/
+	if (sf_dev->display_dev->bpp <= 16 ) {
+		/* 8, 16 bpp */
+		sf_dev->buf_size = sf_dev->display_info.xres *
+			sf_dev->display_info.yres * sf_dev->display_dev->bpp / 8;
+	} else {
+		/* 18, 32 bpp*/
+		sf_dev->buf_size = sf_dev->display_info.xres * sf_dev->display_info.yres * 4;
+	}
+
+	sf_dev->fb.fix.smem_len = sf_dev->buf_size * sf_dev->buf_num;
+
+	ret = sf_fb_map_video_memory(sf_dev);
+	if (ret) {
+		dev_err(sf_dev->dev, "Fail to allocate video RAM\n");
+		return ret;
+	}
+
+	//layer->buf_addr = layer->map_dma;
+
+	return 0;
+}
+
+static int sf_fbinfo_init(struct device *dev, struct sf_fb_data *sf_dev)
+{
+	struct sf_fb_display_dev *display_dev = NULL;
+	int ret;
+
+	display_dev = sf_fb_display_dev_get(sf_dev);
+	if (!display_dev) {
+		dev_err(sf_dev->dev, "Could not get display dev\n");
+	}
+	sf_dev->display_dev = display_dev;
+	sf_dev->pixclock = display_dev->pclk;
+
+/*	clk_set_rate(sf_dev->mclk, sf_dev->pixclock);
+	sf_dev->pixclock = clk_get_rate(sf_dev->mclk);
+	dev_info(sf_dev->dev,"sf_dev->pixclock = %d\n", sf_dev->pixclock);
+*/
+	switch(display_dev->interface_info) {
+		case STARFIVEFB_MIPI_IF:
+			if (display_dev->timing.mipi.display_mode == MIPI_VIDEO_MODE){
+				sf_dev->refresh_en = 1;
+				display_dev->refresh_en = 1;
+				sf_dev->display_info.name = display_dev->name;
+				sf_dev->display_info.xres = display_dev->xres;
+				sf_dev->display_info.yres = display_dev->yres;
+				sf_dev->display_info.pixclock = 1000000 / (sf_dev->pixclock / 1000000);
+				sf_dev->display_info.sync = 0;
+				sf_dev->display_info.left_margin = display_dev->timing.mipi.videomode_info.hbp;
+				sf_dev->display_info.right_margin = display_dev->timing.mipi.videomode_info.hfp;
+				sf_dev->display_info.upper_margin = display_dev->timing.mipi.videomode_info.vbp;
+				sf_dev->display_info.lower_margin = display_dev->timing.mipi.videomode_info.vfp;
+				sf_dev->display_info.hsync_len = display_dev->timing.mipi.videomode_info.hsync;
+				sf_dev->display_info.vsync_len = display_dev->timing.mipi.videomode_info.vsync;
+				if (display_dev->timing.mipi.videomode_info.sync_pol == FB_HSYNC_HIGH_ACT)
+					sf_dev->display_info.sync = FB_SYNC_HOR_HIGH_ACT;
+				if (display_dev->timing.mipi.videomode_info.sync_pol == FB_VSYNC_HIGH_ACT)
+					sf_dev->display_info.sync = FB_SYNC_VERT_HIGH_ACT;
+
+				sf_dev->panel_info.name = display_dev->name;
+				sf_dev->panel_info.w = display_dev->xres;
+				sf_dev->panel_info.h = display_dev->yres;
+				sf_dev->panel_info.bpp = display_dev->bpp;
+				sf_dev->panel_info.fps = display_dev->timing.mipi.fps;
+				sf_dev->panel_info.dpi_pclk = display_dev->pclk;
+				sf_dev->panel_info.dpi_hsa = display_dev->timing.mipi.videomode_info.hsync;
+				sf_dev->panel_info.dpi_hbp = display_dev->timing.mipi.videomode_info.hbp;
+				sf_dev->panel_info.dpi_hfp = display_dev->timing.mipi.videomode_info.hfp;
+				sf_dev->panel_info.dpi_vsa = display_dev->timing.mipi.videomode_info.vsync;
+				sf_dev->panel_info.dpi_vbp = display_dev->timing.mipi.videomode_info.vbp;
+				sf_dev->panel_info.dpi_vfp = display_dev->timing.mipi.videomode_info.vfp;
+				sf_dev->panel_info.dphy_lanes = display_dev->timing.mipi.no_lanes;
+				sf_dev->panel_info.dphy_bps = display_dev->timing.mipi.dphy_bps;
+				sf_dev->panel_info.dsi_burst_mode = display_dev->timing.mipi.dsi_burst_mode;
+				sf_dev->panel_info.dsi_sync_pulse = display_dev->timing.mipi.dsi_sync_pulse;
+				sf_dev->panel_info.dsi_hsa = display_dev->timing.mipi.dsi_hsa;
+				sf_dev->panel_info.dsi_hbp = display_dev->timing.mipi.dsi_hbp;
+				sf_dev->panel_info.dsi_hfp = display_dev->timing.mipi.dsi_hfp;
+				sf_dev->panel_info.dsi_vsa = display_dev->timing.mipi.dsi_vsa;
+				sf_dev->panel_info.dsi_vbp = display_dev->timing.mipi.dsi_vbp;
+				sf_dev->panel_info.dsi_vfp = display_dev->timing.mipi.dsi_vfp;
+			}else if (display_dev->timing.mipi.display_mode == MIPI_COMMAND_MODE){
+				sf_dev->display_info.name = display_dev->name;
+				sf_dev->display_info.xres = display_dev->xres;
+				sf_dev->display_info.yres = display_dev->yres;
+				sf_dev->display_info.pixclock = 1000000 / (sf_dev->pixclock / 1000000);
+				sf_dev->display_info.left_margin = 0;
+				sf_dev->display_info.right_margin = 0;
+				sf_dev->display_info.upper_margin = 0;
+				sf_dev->display_info.lower_margin = 0;
+				sf_dev->display_info.hsync_len = 0;
+				sf_dev->display_info.vsync_len = 0;
+				sf_dev->display_info.sync = 0;
+			}
+			break;
+		case STARFIVEFB_RGB_IF:
+			sf_dev->refresh_en = 1;
+			display_dev->refresh_en = 1;
+			sf_dev->display_info.name = display_dev->name;
+			sf_dev->display_info.xres = display_dev->xres;
+			sf_dev->display_info.yres = display_dev->yres;
+			sf_dev->display_info.pixclock = 1000000 / (sf_dev->pixclock / 1000000);
+			sf_dev->display_info.sync = 0;
+			sf_dev->display_info.left_margin = display_dev->timing.rgb.videomode_info.hbp;
+			sf_dev->display_info.right_margin = display_dev->timing.rgb.videomode_info.hfp;
+			sf_dev->display_info.upper_margin = display_dev->timing.rgb.videomode_info.vbp;
+			sf_dev->display_info.lower_margin = display_dev->timing.rgb.videomode_info.vfp;
+			sf_dev->display_info.hsync_len = display_dev->timing.rgb.videomode_info.hsync;
+			sf_dev->display_info.vsync_len = display_dev->timing.rgb.videomode_info.vsync;
+			if (display_dev->timing.rgb.videomode_info.sync_pol == FB_HSYNC_HIGH_ACT)
+				sf_dev->display_info.sync = FB_SYNC_HOR_HIGH_ACT;
+			if (display_dev->timing.rgb.videomode_info.sync_pol == FB_VSYNC_HIGH_ACT)
+				sf_dev->display_info.sync = FB_SYNC_VERT_HIGH_ACT;
+			break;
+		default:
+			break;
+	}
+
+	ret = sf_fb_init(sf_dev);
+	if (ret) {
+		dev_err(sf_dev->dev, "starfive fb init fail\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+static int stfb_open(struct inode *inode, struct file *file)
+{
+	int ret = 0;
+
+	mutex_lock(&stf_mutex);
+	if (stf_dev != NULL)
+		file->private_data = stf_dev;
+	else {
+		ret = -ENODEV;
+		pr_err("stf_dev is NULL !\n");
+	}
+	mutex_unlock(&stf_mutex);
+
+	return ret;
+}
+
+static ssize_t stfb_read(struct file *file, char __user * buf,
+			size_t count, loff_t * ppos)
+{
+	int ret = 1;
+
+	return ret;
+}
+
+static int stfb_release(struct inode *inode, struct file *file)
+{
+	return 0;
+}
+
+static const struct vm_operations_struct mmap_mem_ops = {
+#ifdef CONFIG_HAVE_IOREMAP_PROT
+	.access = generic_access_phys
+#endif
+};
+
+static int stfb_mmap(struct file *file, struct vm_area_struct *vma)
+{
+//	struct sf_fb_data *sf_dev = file->private_data;
+	size_t size = vma->vm_end - vma->vm_start;
+	//unsigned long pfn = sf_dev->fb.fix.smem_start + 0x1000000;
+	unsigned long pfn = 0xfc000000;
+
+	vma->vm_ops = &mmap_mem_ops;
+	/* Remap-pfn-range will mark the range VM_IO */
+	if (remap_pfn_range(vma,
+			    vma->vm_start,
+			    pfn >> PAGE_SHIFT,
+			    size, vma->vm_page_prot)) {
+		return -EAGAIN;
+	}
+
+	return 0;
+}
+static int fb_pp_rst(void)
+{
+	sf_fb_pp_enable_intr(stf_dev, PP_INTR_DISABLE);
+	sf_fb_pp_init(stf_dev);
+	sf_fb_pp_run(stf_dev);
+	sf_fb_pp_enable_intr(stf_dev, PP_INTR_ENABLE);
+
+	return 0;
+}
+
+static int fb_set_pp_res(void)
+{
+	int pp_id;
+	int ret = 0;
+
+	// lcdc_disable_intr(stf_dev);
+	// stf_dev->fb.fix.smem_start = 0xfc000000;
+	for (pp_id = 0; pp_id < PP_NUM; pp_id++) {
+		if (1 == stf_dev->pp[pp_id].inited) {
+			stf_dev->pp[pp_id].src.height = stf_dev->fb.var.yres;
+			stf_dev->pp[pp_id].src.width = stf_dev->fb.var.xres;
+			stf_dev->pp[pp_id].dst.height = stf_dev->fb.var.yres;
+			stf_dev->pp[pp_id].dst.width = stf_dev->fb.var.xres;
+		}
+	}
+	fb_pp_rst();
+	return 0;
+}
+
+static int fb_set_lcdc_res(void)
+{
+	stf_dev->display_dev->xres = stf_dev->fb.var.xres;
+	stf_dev->display_dev->yres = stf_dev->fb.var.yres;
+	stf_dev->display_info.xres = stf_dev->fb.var.xres;
+	stf_dev->display_info.yres = stf_dev->fb.var.yres;
+
+	sf_fb_lcdc_clk_cfg(stf_dev);
+	sf_fb_lcdc_init(stf_dev);
+	lcdc_run(stf_dev, stf_dev->winNum, LCDC_RUN);
+
+	return 0;
+}
+
+static long int stfb_ioctl(struct file *file, unsigned int cmd, long unsigned int arg)
+{
+	struct fb_info *info;
+	struct pp_mode *ppinfo;
+	u32 tmp_val = 0;
+	int ret = 0;
+	const struct fb_ops *fb;
+	struct fb_var_screeninfo var;
+	struct fb_fix_screeninfo fix;
+	struct fb_cmap cmap_from;
+	struct fb_cmap_user cmap;
+	void __user *argp = (void __user *)arg;
+
+	switch (cmd) {
+	case FBIOPAN_GET_PP_MODE :
+		ppinfo = stf_dev->pp;
+		ret = copy_to_user(argp, ppinfo, sizeof(struct pp_mode) * PP_NUM) ? -EFAULT : 0;
+		break;
+	case FBIOPAN_SET_PP_MODE :
+		ppinfo = kmalloc(sizeof(struct pp_mode) * PP_NUM, GFP_KERNEL);
+		ret = copy_from_user(&ppinfo[0], argp, sizeof(struct pp_mode) * PP_NUM);
+		if (ppinfo != NULL) {
+			memcpy(stf_dev->pp, ppinfo, sizeof(struct pp_mode) * PP_NUM);
+		}
+		sf_fb_update_fbinfo(stf_dev); // update sf_dev->fb.var.grayscale
+		fb_pp_rst();
+		kfree(ppinfo);
+		break;
+	case FBIOPAN_GET_PIX_FORMAT :
+		info = &stf_dev->fb;
+		var = info->var;
+		ret = copy_to_user(argp, &var, sizeof(struct fb_var_screeninfo)) ? -EFAULT : 0;
+		printk("lqw FBIOPAN_GET_PIX_FORMAT :%d %d\n",var.xres,var.yres);
+		break;
+	case FBIOPAN_SET_PIX_FORMAT :
+		if (copy_from_user(&var, argp, sizeof(struct fb_var_screeninfo))) {
+			return -EFAULT;
+		}
+		stf_dev->fb.var = var;
+		vout_sys_clkrstsrc_init(0x1);
+		vout_sys_clkrst_init(0x1); //commented by zv
+		fb_set_pp_res();
+		fb_set_lcdc_res();
+		break;
+	default:
+		dev_err(stf_dev->dev, "stfb_ioctl error\n");
+	}
+
+	return 0;
+}
+
+static const struct file_operations stfb_fops = {
+	.owner = THIS_MODULE,
+	.open = stfb_open,
+	.read = stfb_read,
+	.release = stfb_release,
+	.unlocked_ioctl = stfb_ioctl,
+	.mmap = stfb_mmap,
+};
+
+static void sf_fb_pp_enable_intr(struct sf_fb_data *sf_dev, int enable) {
+	int pp_id;
+
+	for (pp_id = 0; pp_id < PP_NUM; pp_id++) {
+		if(1 == sf_dev->pp[pp_id].inited) {
+			if (enable) {
+				pp_enable_intr(sf_dev, pp_id);
+			} else {
+				pp_disable_intr(sf_dev, pp_id);
+			}
+		}
+	}
+}
+
+static int sf_fb_pp_get_2lcdc_id(struct sf_fb_data *sf_dev) {
+	int pp_id;
+
+	for (pp_id = 0; pp_id < PP_NUM; pp_id++) {
+		if(1 == sf_dev->pp[pp_id].inited) {
+			if ((1 == sf_dev->pp[pp_id].fifo_out)
+				&& (0 == sf_dev->pp[pp_id].bus_out)) {
+				return pp_id;
+			}
+		}
+	}
+
+	if (pp_id == PP_NUM - 1)
+		dev_warn(sf_dev->dev, "NO pp connect to LCDC\n");
+
+	return -ENODEV;
+}
+
+static int sf_fb_lcdc_init(struct sf_fb_data *sf_dev) {
+	int pp_id;
+	int lcd_in_pp;
+	int winNum;
+
+	pp_id = sf_dev->pp_conn_lcdc;
+	if (pp_id < 0) {
+		dev_info(sf_dev->dev, "DDR to LCDC\n");
+		lcd_in_pp = LCDC_IN_LCD_AXI;
+		winNum = lcdc_win_sel(sf_dev, lcd_in_pp);
+		sf_dev->winNum = winNum;
+		lcdc_config(sf_dev, winNum);
+	} else {
+		dev_info(sf_dev->dev, "pp to LCDC\n");
+		lcd_in_pp = (pp_id == 0) ? LCDC_IN_VPP0 : ((pp_id == 1) ? LCDC_IN_VPP1 : LCDC_IN_VPP2);
+		winNum = lcdc_win_sel(sf_dev, lcd_in_pp);
+		sf_dev->winNum = winNum;
+		lcdc_config(sf_dev, winNum);
+	}
+
+	return 0;
+}
+
+static int sf_fb_pp_video_mode_init(struct sf_fb_data *sf_dev, struct pp_video_mode *src,
+		struct pp_video_mode *dst, int pp_id) {
+
+	if ((NULL == src) || (NULL == dst)) {
+		dev_err(sf_dev->dev, "Invalid argument!\n");
+		return -EINVAL;
+	}
+
+	if ((pp_id < PP_NUM) && (pp_id >= 0 )) {
+		src->format = sf_dev->pp[pp_id].src.format;
+		src->width = sf_dev->pp[pp_id].src.width;
+		src->height = sf_dev->pp[pp_id].src.height;
+		src->addr = sf_dev->fb.fix.smem_start; // Note: 0xfb000000 got from dts, 0xf9000000 is not used
+		dst->format = sf_dev->pp[pp_id].dst.format;
+		dst->width = sf_dev->pp[pp_id].dst.width;
+		dst->height = sf_dev->pp[pp_id].dst.height;
+		if(true == sf_dev->pp[pp_id].bus_out)	/*out to ddr*/
+			dst->addr = 0xfc000000;
+		else if (true == sf_dev->pp[pp_id].fifo_out)	/*out to lcdc*/
+			dst->addr = 0;
+	} else {
+		dev_err(sf_dev->dev, "pp_id %d is not support\n", pp_id);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int sf_fb_pp_init(struct sf_fb_data *sf_dev) {
+	int pp_id;
+	int ret = 0;
+	struct pp_video_mode src, dst;
+
+	for (pp_id = 0; pp_id < PP_NUM; pp_id++) {
+		if(1 == sf_dev->pp[pp_id].inited) {
+			ret = sf_fb_pp_video_mode_init(sf_dev, &src, &dst, pp_id);
+			if (!ret)
+				pp_config(sf_dev, pp_id, &src, &dst);
+		}
+	}
+
+	return ret;
+}
+
+static int sf_fb_pp_run(struct sf_fb_data *sf_dev) {
+	int pp_id;
+	int ret = 0;
+
+	for (pp_id = 0; pp_id < PP_NUM; pp_id++) {
+		if(1 == sf_dev->pp[pp_id].inited) {
+			pp_run(sf_dev, pp_id, PP_RUN);
+		}
+	}
+
+	return ret;
+}
+
+static int sf_fb_parse_dt(struct device *dev, struct sf_fb_data *sf_dev) {
+	int ret;
+	struct device_node *np = dev->of_node;
+	struct device_node *child;
+	int pp_num = 0;
+
+	if(!np)
+		return -EINVAL;
+	sf_dev->pp = devm_kzalloc(dev, sizeof(struct pp_mode) * PP_NUM, GFP_KERNEL);
+	if (!sf_dev->pp) {
+		dev_err(dev,"allocate memory for platform data failed\n");
+		return -ENOMEM;
+	}
+
+	if (of_property_read_u32(np, "ddr-format", &sf_dev->ddr_format)) {
+		dev_err(dev,"Missing src-format property in the DT.\n");
+		ret = -EINVAL;
+	}
+
+#ifndef CONFIG_FB_STARFIVE_VIDEO
+	return ret;
+#endif
+
+	for_each_child_of_node(np, child) {
+		if (of_property_read_u32(child, "pp-id", &pp_num)) {
+			dev_err(dev,"Missing pp-id property in the DT.\n");
+			ret = -EINVAL;
+			continue;
+		}
+		if (pp_num >= PP_NUM)
+			dev_err(dev," pp-id number %d is not support!\n", pp_num);
+
+		sf_dev->pp[pp_num].pp_id = pp_num;
+		sf_dev->pp[pp_num].bus_out = of_property_read_bool(child, "sys-bus-out");
+		sf_dev->pp[pp_num].fifo_out = of_property_read_bool(child, "fifo-out");
+		if (of_property_read_u32(child, "src-format", &sf_dev->pp[pp_num].src.format)) {
+			dev_err(dev,"Missing src-format property in the DT.\n");
+			ret = -EINVAL;
+		}
+		if (of_property_read_u32(child, "src-width", &sf_dev->pp[pp_num].src.width)) {
+			dev_err(dev,"Missing src-width property in the DT. w %d \n", sf_dev->pp[pp_num].src.width);
+			ret = -EINVAL;
+		}
+		if (of_property_read_u32(child, "src-height", &sf_dev->pp[pp_num].src.height)) {
+			dev_err(dev,"Missing src-height property in the DT.\n");
+			ret = -EINVAL;
+		}
+		if (of_property_read_u32(child, "dst-format", &sf_dev->pp[pp_num].dst.format)) {
+			dev_err(dev,"Missing dst-format property in the DT.\n");
+			ret = -EINVAL;
+		}
+		if (of_property_read_u32(child, "dst-width", &sf_dev->pp[pp_num].dst.width)) {
+			dev_err(dev,"Missing dst-width property in the DT.\n");
+			ret = -EINVAL;
+		}
+		if (of_property_read_u32(child, "dst-height", &sf_dev->pp[pp_num].dst.height)) {
+			dev_err(dev,"Missing dst-height property in the DT.\n");
+			ret = -EINVAL;
+		}
+		sf_dev->pp[pp_num].inited = 1;
+	}
+
+	return ret;
+}
+
+static int starfive_fb_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct sf_fb_data *sf_dev;
+	int ret;
+
+	dev_info(dev, "%s\n", __func__);
+
+	sf_dev = devm_kzalloc(&pdev->dev, sizeof(struct sf_fb_data), GFP_KERNEL);
+	if (!sf_dev)
+		return -ENOMEM;
+
+	if (sf_get_mem_res(pdev, sf_dev)) {
+		dev_err(dev, "get memory resource FAIL\n");
+		return -ENOMEM;
+	}
+
+	ret = sf_fb_parse_dt(dev, sf_dev);
+
+// #if defined(CONFIG_VIDEO_STARFIVE_VIN)
+// 	sf_dev->vin.notifier_call = vin_frame_complete_notify;
+// 	sf_dev->vin.priority = 0;
+// 	ret = vin_notifier_register(&sf_dev->vin);
+// 	if (ret) {
+// 		return ret;
+// 	}
+// #endif
+
+#if defined(CONFIG_FB_STARFIVE_HDMI_TDA998X)
+	sf_dev->dis_dev_name = "tda_998x_1080p";
+#elif defined(CONFIG_FB_STARFIVE_HDMI_ADV7513)
+	sf_dev->dis_dev_name = "adv_7513_1080p";
+#elif defined(CONFIG_FB_STARFIVE_SEEED5INCH)
+	sf_dev->dis_dev_name = "seeed_5_inch";
+#else
+	dev_err(dev, "no dev name matched\n");
+	return -EINVAL;
+#endif
+	sf_dev->cmap_inverse = 0;
+	sf_dev->cmap_static = 0;
+	sf_dev->dev = &pdev->dev;
+	ret = sf_fbinfo_init(&pdev->dev, sf_dev);
+	if (ret) {
+		dev_err(dev, "fb info init FAIL\n");
+		return ret;
+	}
+
+	sf_dev->lcdc_irq = platform_get_irq_byname(pdev, "lcdc_irq");
+	if (sf_dev->lcdc_irq == -EPROBE_DEFER)
+		return sf_dev->lcdc_irq;
+	if (sf_dev->lcdc_irq < 0) {
+		dev_err(dev, "couldn't get lcdc irq\n");
+		return sf_dev->lcdc_irq;
+	}
+
+	sf_dev->vpp1_irq = platform_get_irq_byname(pdev, "vpp1_irq");
+	if (sf_dev->vpp1_irq == -EPROBE_DEFER)
+		return sf_dev->vpp1_irq;
+	if (sf_dev->vpp1_irq < 0) {
+		dev_err(dev, "couldn't get vpp1 irq\n");
+		return sf_dev->vpp1_irq;
+	}
+
+	lcdc_disable_intr(sf_dev);
+	sf_fb_pp_enable_intr(sf_dev, PP_INTR_DISABLE);
+
+	ret = devm_request_irq(&pdev->dev, sf_dev->lcdc_irq, lcdc_isr_handler, 0,
+			       "sf_lcdc", sf_dev);
+	if (ret) {
+		dev_err(&pdev->dev, "failure requesting irq %i: %d\n",
+			sf_dev->lcdc_irq, ret);
+		return ret;
+	}
+
+	ret = devm_request_irq(&pdev->dev, sf_dev->vpp1_irq, vpp1_isr_handler, 0,
+			       "sf_vpp1", sf_dev);
+	if (ret) {
+		dev_err(&pdev->dev, "failure requesting irq %i: %d\n",
+			sf_dev->vpp1_irq, ret);
+		return ret;
+	}
+
+
+    if(STARFIVEFB_MIPI_IF == sf_dev->display_dev->interface_info){
+		lcdc_dsi_sel(sf_dev);
+		sf_mipi_init(sf_dev);
+    }
+	if (sf_fb_pp_init(sf_dev)) {
+		dev_err(dev, "pp init fail\n");
+		return -ENODEV;
+	}
+	//pp_run(sf_dev, PP_ID_1, PP_RUN);
+	//pp_run(sf_dev, PP_ID_0, PP_RUN);
+	sf_fb_pp_run(sf_dev);
+
+	if (sf_fb_lcdc_clk_cfg(sf_dev)) {
+		dev_err(dev, "lcdc clock configure fail\n");
+		return -EINVAL;
+	}
+	sf_dev->pp_conn_lcdc = sf_fb_pp_get_2lcdc_id(sf_dev);
+	if (sf_fb_lcdc_init(sf_dev)) {
+		dev_err(dev, "lcdc init fail\n");
+		return -EINVAL;
+	}
+	sf_fb_clearscreen(sf_dev);
+	lcdc_run(sf_dev, sf_dev->winNum, LCDC_RUN);
+	sf_fb_update_fbinfo(sf_dev);
+
+	stf_dev = sf_dev;
+
+	platform_set_drvdata(pdev, sf_dev);
+	ret = register_framebuffer(&sf_dev->fb);
+	if (ret < 0) {
+		dev_err(&pdev->dev,"register framebuffer FAIL\n");
+		return ret;
+	}
+
+	sf_dev->stfbcdev.minor = MISC_DYNAMIC_MINOR;
+	sf_dev->stfbcdev.parent = &pdev->dev;
+	sf_dev->stfbcdev.name = "stfbcdev";
+	sf_dev->stfbcdev.fops = &stfb_fops;
+
+	ret = misc_register(&sf_dev->stfbcdev);
+	if (ret) {
+		dev_err(dev, "creare stfbcdev FAIL!\n");
+		return ret;
+	}
+
+	lcdc_enable_intr(sf_dev);
+	sf_fb_pp_enable_intr(sf_dev, PP_INTR_ENABLE);
+
+	return 0;
+}
+
+static int starfive_fb_remove(struct platform_device *pdev)
+{
+	struct sf_fb_data *sf_dev = platform_get_drvdata(pdev);
+
+	if(NULL == sf_dev) {
+		dev_err(&pdev->dev,"get sf_dev fail\n");
+	}
+
+	misc_deregister(&sf_dev->stfbcdev);
+
+	return 0;
+}
+
+static void starfive_fb_shutdown(struct platform_device *dev)
+{
+	return ;
+}
+
+static struct of_device_id starfive_fb_dt_match[] = {
+	{
+		.compatible = "starfive,vpp-lcdc",
+	},
+	{}
+};
+
+static struct platform_driver starfive_fb_driver = {
+	.probe		= starfive_fb_probe,
+	.remove		= starfive_fb_remove,
+	.shutdown	= starfive_fb_shutdown,
+	.driver		= {
+		.name	= "starfive,vpp-lcdc",
+		.of_match_table = starfive_fb_dt_match,
+	},
+};
+
+static int __init starfive_fb_init(void)
+{
+	return platform_driver_register(&starfive_fb_driver);
+}
+
+static void __exit starfive_fb_cleanup(void)
+{
+	platform_driver_unregister(&starfive_fb_driver);
+}
+
+module_init(starfive_fb_init);
+module_exit(starfive_fb_cleanup);
+
+MODULE_AUTHOR("StarFive Technology Co., Ltd.");
+MODULE_DESCRIPTION("loadable LCDC&VPP driver for StarFive");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/fbdev/starfive/starfive_fb.h b/drivers/video/fbdev/starfive/starfive_fb.h
new file mode 100755
index 000000000000..6b5fd068d1df
--- /dev/null
+++ b/drivers/video/fbdev/starfive/starfive_fb.h
@@ -0,0 +1,143 @@
+/*
+ * StarFive Vout driver
+ *
+ * Copyright 2020 StarFive Inc.
+ *
+ * Licensed under the GPL-2.
+ */
+
+#ifndef __SF_FRAMEBUFFER_H__
+#define __SF_FRAMEBUFFER_H__
+
+#include <linux/fb.h>
+#include <linux/miscdevice.h>
+
+#define FB_MEM_SIZE  0x1000000
+
+#define  H_SIZE	1920//352//1920//1280
+#define  V_SIZE	1080//288//1080//720
+#define  H_SIZE_DST	H_SIZE
+#define  V_SIZE_DST	V_SIZE
+
+#define  H_WID	40
+#define  H_BP		220
+#define  H_FP		110
+
+#define  V_WID	5
+#define  V_BP		20
+#define  V_FP		5
+
+#define VD_1080P	1080
+#define VD_720P	720
+#define VD_PAL	480
+
+#define VD_HEIGHT_1080P	VD_1080P
+#define VD_WIDTH_1080P	1920
+
+#define  RGB_OFFSET_ADDR	0//H_SIZE*(PIX_BPP+1)
+
+#define MIN_XRES		64
+#define MIN_YRES		64
+
+#define STARFIVEFB_RGB_IF	(0x00000002)
+#define STARFIVEFB_MIPI_IF	(0x00000003)
+#define STARFIVEFB_HDMI_IF	(0x00000004)
+#define STARFIVE_NAME		"starfive"
+#define BUFFER_NUMS		2
+
+//sys registers
+#define SYS_CONF_LCDC		0x00
+#define SYS_CONF_PP		0x04
+#define SYS_MAP_CONV		0x08
+
+//pp ioctl
+#define FBIOPAN_GET_PP_MODE	0x4609
+#define FBIOPAN_SET_PP_MODE	0x460a
+#define FBIOPAN_GET_PIX_FORMAT	0x460b
+#define FBIOPAN_SET_PIX_FORMAT	0x460c
+//vout clk registers
+#define CLK_LCDC_OCLK_CTRL	0x14
+
+struct res_name {
+	char name[10];
+};
+
+struct dis_panel_info {
+	const char *name;
+
+	// supported pixel format
+	int w;
+	int h;
+	int bpp;
+	int fps;
+
+	/* dpi parameters */
+	u32 dpi_pclk;
+	// pixels
+	int dpi_hsa;
+	int dpi_hbp;
+	int dpi_hfp;
+	// lines
+	int dpi_vsa;
+	int dpi_vbp;
+	int dpi_vfp;
+
+	/* dsi parameters */
+	int dphy_lanes;
+	u32 dphy_bps;
+	int dsi_burst_mode;
+	int dsi_sync_pulse;
+	// bytes
+	int dsi_hsa;
+	int dsi_hbp;
+	int dsi_hfp;
+	// lines
+	int dsi_vsa;
+	int dsi_vbp;
+	int dsi_vfp;
+};
+
+
+struct sf_fb_data {
+	struct device		*dev;
+	char			*dis_dev_name;
+	struct miscdevice	stfbcdev;
+	int 			lcdc_irq;
+	int 			vpp0_irq;
+	int 			vpp1_irq;
+	int 			vpp2_irq;
+	void __iomem		*base_clk;
+	void __iomem		*base_rst;
+	void __iomem		*base_syscfg;
+	void __iomem		*base_vpp0;
+	void __iomem		*base_vpp1;
+	void __iomem		*base_vpp2;
+	void __iomem		*base_dsitx;
+	void __iomem		*base_lcdc;
+	struct notifier_block	vin;
+	struct clk *mclk;
+
+	/*
+	 * Hardware control information
+	 */
+	struct fb_info		fb;
+	struct fb_videomode display_info;    /* reparent video mode source*/
+	struct dis_panel_info	panel_info;		/* mipi parameters for panel */
+	unsigned int		refresh_en;
+	unsigned int		pixclock;	/*lcdc_mclk*/
+	unsigned int		buf_num;	/* frame buffer number. */
+	unsigned int		buf_size;	/* frame buffer size. */
+	int		cmap_inverse;
+	int		cmap_static;
+	/* keep these registers in case we need to re-write palette */
+	u32			palette_buffer[256];
+	u32			pseudo_pal[16];
+
+	struct sf_fb_display_dev *display_dev;
+	struct pp_mode *pp;
+	int winNum;
+	int pp_conn_lcdc;
+	int ddr_format;
+};
+
+#endif
diff --git a/drivers/video/fbdev/starfive/starfive_lcdc.c b/drivers/video/fbdev/starfive/starfive_lcdc.c
new file mode 100755
index 000000000000..d43eb13f50b4
--- /dev/null
+++ b/drivers/video/fbdev/starfive/starfive_lcdc.c
@@ -0,0 +1,359 @@
+/* driver/video/starfive/starfive_lcdc.c
+**
+** This program is free software; you can redistribute it and/or modify
+** it under the terms of the GNU General Public License version 2 as
+** published by the Free Software Foundation.
+**
+** Copyright (C) 2020 StarFive, Inc.
+**
+** PURPOSE:	This files contains the driver of LCD controller.
+**
+** CHANGE HISTORY:
+**	Version		Date		Author		Description
+**	0.1.0		2020-11-03	starfive		created
+**
+*/
+
+#include <linux/module.h>
+#include "starfive_fb.h"
+#include "starfive_lcdc.h"
+#include "starfive_vpp.h"
+#include "starfive_comm_regs.h"
+
+//#define SF_LCDC_DEBUG	1
+#ifdef SF_LCDC_DEBUG
+	#define LCDC_PRT(format, args...)  printk(KERN_DEBUG "[LCDC]: " format, ## args)
+	#define LCDC_INFO(format, args...) printk(KERN_INFO "[LCDC]: " format, ## args)
+	#define LCDC_ERR(format, args...)	printk(KERN_ERR "[LCDC]: " format, ## args)
+#else
+	#define LCDC_PRT(x...)  do{} while(0)
+	#define LCDC_INFO(x...)  do{} while(0)
+	#define LCDC_ERR(x...)  do{} while(0)
+#endif
+
+static u32 sf_fb_lcdcread32(struct sf_fb_data *sf_dev, u32 reg)
+{
+	return ioread32(sf_dev->base_lcdc + reg);
+}
+
+static void sf_fb_lcdcwrite32(struct sf_fb_data *sf_dev, u32 reg, u32 val)
+{
+	iowrite32(val, sf_dev->base_lcdc + reg);
+}
+
+void lcdc_mode_cfg(struct sf_fb_data *sf_dev, uint32_t workMode, int dotEdge, int syncEdge, int r2yBypass,
+					int srcSel, int intSrc, int intFreq)
+{
+	u32 lcdcEn = 0x1;
+	u32 cfg = lcdcEn | workMode << LCDC_WORK_MODE
+				| dotEdge << LCDC_DOTCLK_P
+				| syncEdge << LCDC_HSYNC_P
+				| syncEdge << LCDC_VSYNC_P
+				| 0x0 << LCDC_DITHER_EN
+				| r2yBypass << LCDC_R2Y_BPS
+				| srcSel << LCDC_TV_LCD_PATHSEL
+				| intSrc << LCDC_INT_SEL
+				| intFreq << LCDC_INT_FREQ;
+
+	sf_fb_lcdcwrite32(sf_dev, LCDC_GCTRL, cfg);
+	LCDC_PRT("LCDC WorkMode: 0x%x, LCDC Path: %d\n", workMode, srcSel);
+}
+
+//hbk, vbk=sa+bp, hpw?
+void lcdc_timing_cfg(struct sf_fb_data *sf_dev, int vunit)
+{
+	int hpw = sf_dev->display_info.hsync_len - 1;
+	int hbk = sf_dev->display_info.hsync_len + sf_dev->display_info.left_margin;
+	int hfp = sf_dev->display_info.right_margin;
+	int vpw = sf_dev->display_info.vsync_len - 1;
+	int vbk = sf_dev->display_info.vsync_len + sf_dev->display_info.upper_margin;
+	int vfp = sf_dev->display_info.lower_margin;
+	int htiming = hbk | hfp << LCDC_RGB_HFP;
+	int vtiming = vbk | vfp << LCDC_RGB_VFP;
+	int hvwid = hpw | vpw << LCDC_RGB_VPW | vunit << LCDC_RGB_UNIT;
+
+	sf_fb_lcdcwrite32(sf_dev, LCDC_RGB_H_TMG, htiming);
+	sf_fb_lcdcwrite32(sf_dev, LCDC_RGB_V_TMG, vtiming);
+	sf_fb_lcdcwrite32(sf_dev, LCDC_RGB_W_TMG, hvwid);
+	LCDC_PRT("LCDC HPW: %d, HBK: %d, HFP: %d\n", hpw, hbk, hfp);
+	LCDC_PRT("LCDC VPW: %d, VBK: %d, VFP: %d\n", vpw, vbk, vfp);
+	LCDC_PRT("LCDC V-Unit: %d, 0-HSYNC and 1-dotClk period\n", vunit);
+}
+
+//? background size
+//lcdc_desize_cfg(sf_dev, sf_dev->display_info.xres-1, sf_dev->display_info.yres-1);
+void lcdc_desize_cfg(struct sf_fb_data *sf_dev)
+{
+	int hsize = sf_dev->display_info.xres - 1;
+	int vsize = sf_dev->display_info.yres - 1;
+	int sizecfg = hsize | vsize << LCDC_BG_VSIZE;
+
+	sf_fb_lcdcwrite32(sf_dev, LCDC_BACKGROUD, sizecfg);
+	LCDC_PRT("LCDC Dest H-Size: %d, V-Size: %d\n", hsize, vsize);
+}
+
+void lcdc_rgb_dclk_cfg(struct sf_fb_data *sf_dev, int dot_clk_sel)
+{
+	int cfg = dot_clk_sel << 16;
+
+	sf_fb_lcdcwrite32(sf_dev, LCDC_RGB_DCLK, cfg);
+	LCDC_PRT("LCDC Dot_clock_output_sel: 0x%x\n", cfg);
+}
+
+
+// color table
+//win0, no lock transfer
+//win3, no srcSel and addrMode, 0 assigned to them
+//lcdc_win_cfgA(sf_dev, winNum, sf_dev->display_info.xres-1, sf_dev->display_info.yres-1, 0x1, 0x0, 0x0, 0x1, 0x0, 0x0);
+void lcdc_win_cfgA(struct sf_fb_data *sf_dev, int winNum, int layEn,
+			int clorTab, int colorEn, int addrMode, int lock)
+{
+	int cfg;
+	int hsize = sf_dev->display_info.xres - 1;
+	int vsize = sf_dev->display_info.yres - 1;
+	int srcSel_v = 1;
+
+	if(sf_dev->pp_conn_lcdc < 0)
+		srcSel_v = 0;
+
+	cfg = hsize | vsize << LCDC_WIN_VSIZE | layEn << LCDC_WIN_EN |
+	clorTab << LCDC_CC_EN | colorEn << LCDC_CK_EN |
+	srcSel_v << LCDC_WIN_ISSEL | addrMode << LCDC_WIN_PM |
+	lock << LCDC_WIN_CLK;
+
+	sf_fb_lcdcwrite32(sf_dev, LCDC_WIN0_CFG_A + winNum * 0xC, cfg);
+	LCDC_PRT("LCDC Win%d H-Size: %d, V-Size: %d, layEn: %d, Src: %d, AddrMode: %d\n",
+		winNum, hsize, vsize, layEn, srcSel_v, addrMode);
+}
+
+static int ppfmt_to_lcdcfmt(enum COLOR_FORMAT ppfmt)
+{
+	int lcdcfmt = 0;
+
+	if(COLOR_RGB888_ARGB == ppfmt) {
+		lcdcfmt = WIN_FMT_xRGB8888;
+	} else if (COLOR_RGB888_ABGR == ppfmt) {
+		LCDC_PRT("COLOR_RGB888_ABGR(%d) not map\n", ppfmt);
+	} else if (COLOR_RGB888_RGBA == ppfmt) {
+		LCDC_PRT("COLOR_RGB888_RGBA(%d) not map\n", ppfmt);
+	} else if (COLOR_RGB888_BGRA == ppfmt) {
+		LCDC_PRT("COLOR_RGB888_BGRA(%d) not map\n", ppfmt);
+	} else if (COLOR_RGB565 == ppfmt) {
+		lcdcfmt = WIN_FMT_RGB565;
+	}
+
+	return lcdcfmt;
+}
+
+void lcdc_win_cfgB(struct sf_fb_data *sf_dev, int winNum, int xpos, int ypos, int argbOrd)
+{
+	int win_format = 0;
+	int cfg = xpos | ypos << LCDC_WIN_VPOS;
+
+	if(sf_dev->pp_conn_lcdc < 0) { //ddr -> lcdc
+		win_format = sf_dev->ddr_format;
+		LCDC_PRT("LCDC win_format: 0x%x\n",win_format);
+	} else { //ddr -> pp -> lcdc
+		win_format = ppfmt_to_lcdcfmt(sf_dev->pp[sf_dev->pp_conn_lcdc].dst.format);
+	}
+
+	if (!strcmp(sf_dev->dis_dev_name, "tda_998x_1080p"))
+		argbOrd=1;
+	if (!strcmp(sf_dev->dis_dev_name, "seeed_5_inch"))
+		argbOrd=1;
+
+	cfg |= win_format << LCDC_WIN_FMT | argbOrd << LCDC_WIN_ARGB_ORDER;
+
+	sf_fb_lcdcwrite32(sf_dev, LCDC_WIN0_CFG_B + winNum * 0xC, cfg);
+	LCDC_PRT("LCDC Win%d Xpos: %d, Ypos: %d, win_format: 0x%x, ARGB Order: 0x%x\n",
+		 winNum, xpos, ypos, win_format, argbOrd);
+}
+
+//? Color key
+void lcdc_win_cfgC(struct sf_fb_data *sf_dev, int winNum, int colorKey)
+{
+	sf_fb_lcdcwrite32(sf_dev, LCDC_WIN0_CFG_C + winNum * 0xC, colorKey);
+	LCDC_PRT("LCDC Win%d Color Key: 0x%6x\n", winNum, colorKey);
+}
+
+//? hsize
+//lcdc_win_srcSize(sf_dev, winNum, sf_dev->display_info.xres-1);
+void lcdc_win_srcSize(struct sf_fb_data *sf_dev, int winNum)
+{
+	int addr, off, winsize, preCfg, cfg;
+	int hsize = sf_dev->display_info.xres - 1;
+	switch(winNum) {
+	case 0 : {addr = LCDC_WIN01_HSIZE; off = 0xfffff000; winsize = hsize; break;}
+	case 1 : {addr = LCDC_WIN01_HSIZE; off = 0xff000fff; winsize = hsize << LCDC_IMG_HSIZE; break;}
+	case 2 : {addr = LCDC_WIN23_HSIZE; off = 0xfffff000; winsize = hsize; break;}
+	case 3 : {addr = LCDC_WIN23_HSIZE; off = 0xff000fff; winsize = hsize << LCDC_IMG_HSIZE; break;}
+	case 4 : {addr = LCDC_WIN45_HSIZE; off = 0xfffff000; winsize = hsize; break;}
+	case 5 : {addr = LCDC_WIN45_HSIZE; off = 0xff000fff; winsize = hsize << LCDC_IMG_HSIZE; break;}
+	case 6 : {addr = LCDC_WIN67_HSIZE; off = 0xfffff000; winsize = hsize; break;}
+	case 7 : {addr = LCDC_WIN67_HSIZE; off = 0xff000fff; winsize = hsize << LCDC_IMG_HSIZE; break;}
+	default: {addr = LCDC_WIN01_HSIZE; off = 0xfffff000; winsize = hsize; break;}
+	}
+	preCfg = sf_fb_lcdcread32(sf_dev, addr)  & off;
+	cfg = winsize | preCfg;
+	sf_fb_lcdcwrite32(sf_dev, addr, cfg);
+	LCDC_PRT("LCDC Win%d Src Hsize: %d\n", winNum, hsize);
+}
+
+void lcdc_alphaVal_cfg(struct sf_fb_data *sf_dev, int val1, int val2, int val3, int val4, int sel)
+{
+	int val = val1 | val2 << LCDC_ALPHA2
+			| val3 << LCDC_ALPHA3
+			| val4 << LCDC_ALPHA4
+			| sel << LCDC_01_ALPHA_SEL;
+
+	int preVal = 0xfffb0000 & sf_fb_lcdcread32(sf_dev, LCDC_ALPHA_VALUE);
+	sf_fb_lcdcwrite32(sf_dev, LCDC_ALPHA_VALUE, preVal | val);
+	LCDC_PRT("LCDC Alpha 1: %x, 2: %x, 3: %x, 4: %x\n", val1, val2, val3, val4);
+}
+
+void lcdc_panel_cfg(struct sf_fb_data *sf_dev, int buswid, int depth, int txcycle, int pixpcycle,
+		int rgb565sel, int rgb888sel)
+{
+	int cfg = buswid | depth << LCDC_COLOR_DEP
+			  | txcycle << LCDC_TCYCLES
+			  | pixpcycle << LCDC_PIXELS
+			  | rgb565sel << LCDC_565RGB_SEL
+			  | rgb888sel << LCDC_888RGB_SEL;
+
+  sf_fb_lcdcwrite32(sf_dev, LCDC_PANELDATAFMT, cfg);
+  LCDC_PRT("LCDC bus bit: :%d, pixDep: 0x%x, txCyle: %d, %dpix/cycle, RGB565 2cycle_%d, RGB888 3cycle_%d\n",
+		buswid, depth, txcycle, pixpcycle, rgb565sel, rgb888sel);
+}
+
+//winNum: 0-2
+void lcdc_win02Addr_cfg(struct sf_fb_data *sf_dev, int addr0, int addr1)
+{
+	sf_fb_lcdcwrite32(sf_dev, LCDC_WIN0STARTADDR0 + sf_dev->winNum * 0x8, addr0);
+	sf_fb_lcdcwrite32(sf_dev, LCDC_WIN0STARTADDR1 + sf_dev->winNum * 0x8, addr1);
+	LCDC_PRT("LCDC Win%d Start Addr0: 0x%8x, Addr1: 0x%8x\n", sf_dev->winNum, addr0, addr1);
+}
+
+void lcdc_enable_intr(struct sf_fb_data *sf_dev)
+{
+	int cfg;
+	cfg = ~(0x1 << LCDC_OUT_FRAME_END);
+
+	sf_fb_lcdcwrite32(sf_dev, LCDC_INT_MSK, cfg);
+}
+EXPORT_SYMBOL(lcdc_enable_intr);
+
+void lcdc_disable_intr(struct sf_fb_data *sf_dev)
+{
+	sf_fb_lcdcwrite32(sf_dev, LCDC_INT_MSK, 0xff);
+	sf_fb_lcdcwrite32(sf_dev, LCDC_INT_CLR, 0xff);
+}
+EXPORT_SYMBOL(lcdc_disable_intr);
+
+int lcdc_win_sel(struct sf_fb_data *sf_dev, enum lcdc_in_mode sel)
+{
+	int winNum = 2;
+
+	switch(sel)
+	{
+	case LCDC_IN_LCD_AXI:
+			winNum = LCDC_WIN_0;
+		break;
+	case LCDC_IN_VPP2:
+		winNum = LCDC_WIN_0;
+		break;
+	case LCDC_IN_VPP1:
+		winNum = LCDC_WIN_2;
+		break;
+	case LCDC_IN_VPP0:
+		winNum = LCDC_WIN_1;
+		mapconv_pp0_sel(sf_dev, 0x0);
+		break;
+	case LCDC_IN_MAPCONVERT:
+		winNum = LCDC_WIN_1;
+		mapconv_pp0_sel(sf_dev, 0x1);
+		break;
+	}
+
+	return winNum;
+}
+EXPORT_SYMBOL(lcdc_win_sel);
+
+void lcdc_dsi_sel(struct sf_fb_data *sf_dev)
+{
+	int temp;
+	u32 lcdcEn = 0x1;
+	u32 workMode = 0x1;
+	u32 cfg = lcdcEn | workMode << LCDC_WORK_MODE;
+
+	sf_fb_lcdcwrite32(sf_dev, LCDC_GCTRL, cfg);
+	temp = sf_fb_rstread32(sf_dev, SRST_ASSERT0);
+	temp &= ~(0x1<<BIT_RST_DSI_DPI_PIX);
+	sf_fb_rstwrite32(sf_dev, SRST_ASSERT0, temp);
+}
+EXPORT_SYMBOL(lcdc_dsi_sel);
+
+irqreturn_t lcdc_isr_handler(int this_irq, void *dev_id)
+{
+	struct sf_fb_data *sf_dev = (struct sf_fb_data *)dev_id;
+	static int count = 0;
+	u32 intr_status = 0;
+
+	intr_status = sf_fb_lcdcread32(sf_dev, LCDC_INT_STATUS);
+	sf_fb_lcdcwrite32(sf_dev, LCDC_INT_CLR, 0xffffffff);
+
+	count ++;
+	//if(0 == count % 100)
+	//LCDC_PRT("++++\n");
+		//printk("+ count = %d, intr_status = 0x%x\n", count, intr_status);
+	return IRQ_HANDLED;
+}
+EXPORT_SYMBOL(lcdc_isr_handler);
+
+void lcdc_int_cfg(struct sf_fb_data *sf_dev, int mask)
+{
+	int cfg;
+
+	if(mask==0x1)
+		cfg = 0xffffffff;
+	else
+		cfg = ~(0x1 << LCDC_OUT_FRAME_END); //only frame end interrupt mask
+	sf_fb_lcdcwrite32(sf_dev, LCDC_INT_MSK, cfg);
+}
+EXPORT_SYMBOL(lcdc_int_cfg);
+
+void lcdc_config(struct sf_fb_data *sf_dev, int winNum)
+{
+	lcdc_mode_cfg(sf_dev, 0x1, 0x1, 0x1, 0x1, 0x1, 0x1, 0x0);
+	lcdc_timing_cfg(sf_dev, 0);
+	lcdc_desize_cfg(sf_dev);
+	lcdc_rgb_dclk_cfg(sf_dev, 0x1);
+
+	if(sf_dev->pp_conn_lcdc < 0) { //ddr->lcdc
+		if (sf_dev->fb.fix.smem_start)
+			lcdc_win02Addr_cfg(sf_dev, sf_dev->fb.fix.smem_start, 0x0);
+		else {
+			lcdc_win02Addr_cfg(sf_dev, 0xfb000000, 0x0);
+			dev_err(sf_dev->dev, "smem_start is not RIGHT\n");
+		}
+	}
+
+	lcdc_win_cfgA(sf_dev, winNum, 0x1, 0x0, 0x0, 0x0, 0x0);
+	lcdc_win_cfgB(sf_dev, winNum, 0x0, 0x0, 0x0);
+	lcdc_win_cfgC(sf_dev, winNum, 0xffffff);
+	lcdc_win_srcSize(sf_dev, winNum);
+	lcdc_alphaVal_cfg(sf_dev, 0xf, 0xf, 0xf, 0xf, 0x0);
+	lcdc_panel_cfg(sf_dev, 0x3, 0x4, 0x0, 0x0, 0x0, 0x1);  //rgb888sel?
+}
+EXPORT_SYMBOL(lcdc_config);
+
+void lcdc_run(struct sf_fb_data *sf_dev, uint32_t winMode, uint32_t lcdTrig)
+{
+	uint32_t runcfg = winMode << LCDC_EN_CFG_MODE | lcdTrig;
+	sf_fb_lcdcwrite32(sf_dev, LCDC_SWITCH, runcfg);
+	LCDC_PRT("Start run LCDC\n");
+}
+EXPORT_SYMBOL(lcdc_run);
+
+MODULE_AUTHOR("StarFive Technology Co., Ltd.");
+MODULE_DESCRIPTION("loadable LCDC driver for StarFive");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/fbdev/starfive/starfive_lcdc.h b/drivers/video/fbdev/starfive/starfive_lcdc.h
new file mode 100755
index 000000000000..1bd0ad332d36
--- /dev/null
+++ b/drivers/video/fbdev/starfive/starfive_lcdc.h
@@ -0,0 +1,151 @@
+/*
+ * StarFive Vout driver
+ *
+ * Copyright 2020 StarFive Inc.
+ *
+ * Licensed under the GPL-2.
+ */
+
+#ifndef __SF_FB_LCDC_H__
+#define __SF_FB_LCDC_H__
+
+enum lcdc_in_mode{
+	LCDC_IN_LCD_AXI = 0,
+	LCDC_IN_VPP2,
+	LCDC_IN_VPP1,
+	LCDC_IN_VPP0,
+	LCDC_IN_MAPCONVERT,
+};
+
+enum lcdc_win_num{
+	LCDC_WIN_0 = 0,
+	LCDC_WIN_1,
+	LCDC_WIN_2,
+	LCDC_WIN_3,
+	LCDC_WIN_4,
+	LCDC_WIN_5,
+};
+
+enum WIN_FMT{
+	WIN_FMT_RGB565 = 4,
+	WIN_FMT_xRGB1555,
+	WIN_FMT_xRGB4444,
+	WIN_FMT_xRGB8888,
+};
+
+#define LCDC_STOP	0
+#define LCDC_RUN	1
+
+//lcdc registers
+#define LCDC_SWITCH		0x0000
+#define LCDC_GCTRL		0x0004
+#define LCDC_INT_STATUS		0x0008
+#define LCDC_INT_MSK		0x000C
+#define LCDC_INT_CLR		0x0010
+#define LCDC_RGB_H_TMG		0x0014
+#define LCDC_RGB_V_TMG		0x0018
+#define LCDC_RGB_W_TMG		0x001C
+#define LCDC_RGB_DCLK		0x0020
+#define LCDC_M_CS_CTRL		0x0024
+#define LCDC_DeltaRGB_CFG		0x0028
+#define LCDC_BACKGROUD		0x002C
+#define LCDC_WIN0_CFG_A		0x0030
+#define LCDC_WIN0_CFG_B		0x0034
+#define LCDC_WIN0_CFG_C		0x0038
+#define LCDC_WIN1_CFG_A		0x003C
+#define LCDC_WIN1_CFG_B		0x0040
+#define LCDC_WIN1_CFG_C		0x0044
+#define LCDC_WIN2_CFG_A		0x0048
+#define LCDC_WIN2_CFG_B		0x004C
+#define LCDC_WIN2_CFG_C		0x0050
+#define LCDC_WIN3_CFG_A		0x0054
+#define LCDC_WIN3_CFG_B		0x0058
+#define LCDC_WIN3_CFG_C		0x005C
+#define LCDC_WIN01_HSIZE		0x0090
+#define LCDC_WIN23_HSIZE		0x0094
+#define LCDC_WIN45_HSIZE		0x0098
+#define LCDC_WIN67_HSIZE		0x009C
+#define LCDC_ALPHA_VALUE		0x00A0
+#define LCDC_PANELDATAFMT		0x00A4
+#define LCDC_WIN0STARTADDR0		0x00B8
+#define LCDC_WIN0STARTADDR1		0x00BC
+
+/* Definition controller bit for LCDC registers */
+//for LCDC_SWITCH
+#define LCDC_DTRANS_SWITCH		0
+#define LCDC_MPU_START		1
+#define LCDC_EN_CFG_MODE		2
+//for LCDC_GCTRL
+#define LCDC_EN		0
+#define LCDC_WORK_MODE		1
+#define LCDC_A0_P		4
+#define LCDC_ENABLE_P		5
+#define LCDC_DOTCLK_P		6
+#define LCDC_HSYNC_P		7
+#define LCDC_VSYNC_P		8
+#define LCDC_DITHER_EN		9
+#define LCDC_R2Y_BPS		10
+#define LCDC_MS_SEL		11
+#define LCDC_TV_LCD_PATHSEL		12
+#define LCDC_INTERLACE		13
+#define LCDC_CBCR_ORDER		14
+#define LCDC_INT_SEL		15
+#define LCDC_INT_FREQ		24
+//for LCDC_INT_MSK
+#define LCDC_OUT_FRAME_END		5
+//for RGB_H_TMG,RGB_V_TMG,RGB_W_TMG
+#define LCDC_RGB_HBK		0
+#define LCDC_RGB_HFP		16
+#define LCDC_RGB_VBK		0
+#define LCDC_RGB_VFP		16
+#define LCDC_RGB_HPW		0
+#define LCDC_RGB_VPW		8
+#define LCDC_RGB_UNIT		16
+//for BACKGROUD
+#define LCDC_BG_HSIZE		0
+#define LCDC_BG_VSIZE		12
+//for WINx_CFG_A/B/C
+#define LCDC_WIN_HSIZE		0
+#define LCDC_WIN_VSIZE		12
+#define LCDC_WIN_EN		24
+#define LCDC_CC_EN		25
+#define LCDC_CK_EN		26
+#define LCDC_WIN_ISSEL		27
+#define LCDC_WIN_PM		28
+#define LCDC_WIN_CLK		30
+#define LCDC_WIN_HPOS		0
+#define LCDC_WIN_VPOS		12
+#define LCDC_WIN_FMT		24
+#define LCDC_WIN_ARGB_ORDER		27
+#define LCDC_WIN_CC		0
+//for WINxx_HSIZE
+#define LCDC_IMG_HSIZE		12
+//for LCDC_ALPHA_VALUE
+#define LCDC_ALPHA1		0
+#define LCDC_ALPHA2		4
+#define LCDC_ALPHA3		8
+#define LCDC_ALPHA4		12
+#define LCDC_A_GLBL_ALPHA		16
+#define LCDC_B_GLBL_ALPHA		17
+#define LCDC_01_ALPHA_SEL		18
+//for LCDC_PANELDATAFMT
+#define LCDC_BUS_W		0
+#define LCDC_TCYCLES		2
+#define LCDC_COLOR_DEP		4
+#define LCDC_PIXELS		7
+#define LCDC_332RGB_SEL		8
+#define LCDC_444RGB_SEL		9
+#define LCDC_666RGB_SEL		12
+#define LCDC_565RGB_SEL		16
+#define LCDC_888RGB_SEL		18
+
+extern void lcdc_enable_intr(struct sf_fb_data *sf_dev);
+extern void lcdc_disable_intr(struct sf_fb_data *sf_dev);
+extern irqreturn_t lcdc_isr_handler(int this_irq, void *dev_id);
+extern void lcdc_int_cfg(struct sf_fb_data *sf_dev, int mask);
+extern void lcdc_config(struct sf_fb_data *sf_dev, int winNum);
+extern int lcdc_win_sel(struct sf_fb_data *sf_dev, enum lcdc_in_mode sel);
+extern void lcdc_dsi_sel(struct sf_fb_data *sf_dev);
+extern void lcdc_run(struct sf_fb_data *sf_dev, uint32_t winMode, uint32_t lcdTrig);
+
+#endif
diff --git a/drivers/video/fbdev/starfive/starfive_mipi_tx.c b/drivers/video/fbdev/starfive/starfive_mipi_tx.c
new file mode 100755
index 000000000000..9a212cc88552
--- /dev/null
+++ b/drivers/video/fbdev/starfive/starfive_mipi_tx.c
@@ -0,0 +1,642 @@
+/* driver/video/starfive/starfive_mipi_tx.c
+**
+** This program is free software; you can redistribute it and/or modify
+** it under the terms of the GNU General Public License version 2 as
+** published by the Free Software Foundation.
+**
+** Copyright (C) 2021 StarFive, Inc.
+**
+** PURPOSE:	This files contains the driver of LCD controller.
+**
+** CHANGE HISTORY:
+**	Version		Date		Author		Description
+**	0.1.0		2021-01-06	starfive		created
+**
+*/
+
+#include <linux/module.h>
+#include <stdarg.h>
+#include <linux/delay.h>
+#include "starfive_comm_regs.h"
+#include "starfive_mipi_tx.h"
+
+//#define SF_MIPITX_DEBUG	1
+#ifdef SF_MIPITX_DEBUG
+	#define MIPITX_PRT(format, args...)  printk(KERN_DEBUG "[MIPITX]: " format, ## args)
+	#define MIPITX_INFO(format, args...) printk(KERN_INFO "[MIPITX]: " format, ## args)
+	#define MIPITX_ERR(format, args...)	printk(KERN_ERR "[MIPITX]: " format, ## args)
+#else
+	#define MIPITX_PRT(x...)  do{} while(0)
+	#define MIPITX_INFO(x...)  do{} while(0)
+	#define MIPITX_ERR(x...)  do{} while(0)
+#endif
+
+static u32 sf_fb_dsitxread32(struct sf_fb_data *sf_dev, u32 reg)
+{
+	return ioread32(sf_dev->base_dsitx + reg);
+}
+
+static void sf_fb_dsitxwrite32(struct sf_fb_data *sf_dev, u32 reg, u32 val)
+{
+	iowrite32(val, sf_dev->base_dsitx + reg);
+}
+
+static void dcs_start(struct sf_fb_data *sf_dev, u32 cmd_head, u32 cmd_size, u32 cmd_nat)
+{
+	u32 main_settings;
+	u32 cmd_long = (cmd_head == CMD_HEAD_WRITE_N);
+
+	sf_fb_dsitxwrite32(sf_dev, DIRECT_CMD_STAT_CLR_ADDR, 0xffffffff);
+	sf_fb_dsitxwrite32(sf_dev, DIRECT_CMD_STAT_CTRL_ADDR, 0xffffffff);
+
+	main_settings = (0<<25)     //trigger_val
+					|(1<<24)    //cmd_lp_en
+					|((cmd_size&0xff)<<16)
+					|(0<<14)    //cmd_id
+					|((cmd_head&0x3f)<<8)
+					|((cmd_long&0x1)<<3)
+					|(cmd_nat&0x7);
+
+	sf_fb_dsitxwrite32(sf_dev, DIRECT_CMD_MAINSET_ADDR, main_settings);
+}
+
+static void dcs_write_32(struct sf_fb_data *sf_dev, u32 val)
+{
+	sf_fb_dsitxwrite32(sf_dev, DIRECT_CMD_WRDAT_ADDR, val);
+}
+
+static void dcs_wait_finish(struct sf_fb_data *sf_dev, u32 exp_sts_mask, u32 exp_sts)
+{
+	u32 stat = 0;
+	int timeout = 100;
+	int stat_88;
+	int stat_188;
+	int stat_88_ack_val;
+
+	sf_fb_dsitxwrite32(sf_dev, DIRECT_CMD_SEND_ADDR, 0);
+
+	do {
+		stat = sf_fb_dsitxread32(sf_dev, DIRECT_CMD_STAT_ADDR);
+		if ((stat & exp_sts_mask) == exp_sts) {
+			break;
+		}
+		mdelay(10);
+	} while (--timeout);
+	if (!timeout) {
+		printk("timeout!\n");
+	}
+
+	stat_88 = sf_fb_dsitxread32(sf_dev, DIRECT_CMD_STAT_ADDR);
+	stat_188 = sf_fb_dsitxread32(sf_dev, PHY_ERR_FLAG_ADDR);
+	stat_88_ack_val = stat_88 >> 16;
+	if (stat_188 || stat_88_ack_val) {
+		MIPITX_PRT("stat: [188h] %08x, [88h] %08x\r\n", stat_188, stat_88);
+	}
+}
+
+static void mipi_tx_lxn_set(struct sf_fb_data *sf_dev, u32 reg, u32 n_hstx, u32 p_hstx)
+{
+	u32 temp = 0;
+
+	temp = n_hstx;
+	temp |= p_hstx << 5;
+	sf_fb_cfgwrite32(sf_dev, reg, temp);
+}
+
+static void dsi_csi2tx_sel(struct sf_fb_data *sf_dev, int sel)
+{
+	u32 temp = 0;
+
+	temp = sf_fb_cfgread32(sf_dev, SCFG_DSI_CSI_SEL);
+	temp &= ~(0x1);
+	temp |= (sel & 0x1);
+	sf_fb_cfgwrite32(sf_dev, SCFG_DSI_CSI_SEL, temp);
+}
+
+static void dphy_clane_hs_txready_sel(struct sf_fb_data *sf_dev, u32 ready_sel)
+{
+	sf_fb_cfgwrite32(sf_dev, SCFG_TXREADY_SRC_SEL_D, ready_sel);
+	sf_fb_cfgwrite32(sf_dev, SCFG_TXREADY_SRC_SEL_C, ready_sel);
+	sf_fb_cfgwrite32(sf_dev, SCFG_HS_PRE_ZERO_T_D, 0x30);
+	sf_fb_cfgwrite32(sf_dev, SCFG_HS_PRE_ZERO_T_C, 0x30);
+	MIPITX_PRT("DPHY ppi_c_hs_tx_ready from source %d\n", ready_sel);
+}
+
+static void dphy_config(struct sf_fb_data *sf_dev, int bit_rate)
+{
+	int pre_div,      fbk_int,       extd_cycle_sel;
+	int dhs_pre_time, dhs_zero_time, dhs_trial_time;
+	int chs_pre_time, chs_zero_time, chs_trial_time;
+	int chs_clk_pre_time, chs_clk_post_time;
+	u32 set_val = 0;
+
+	mipi_tx_lxn_set(sf_dev, SCFG_L0N_L0P_HSTX, 0x10, 0x10);
+	mipi_tx_lxn_set(sf_dev, SCFG_L1N_L1P_HSTX, 0x10, 0x10);
+	mipi_tx_lxn_set(sf_dev, SCFG_L2N_L2P_HSTX, 0x10, 0x10);
+	mipi_tx_lxn_set(sf_dev, SCFG_L3N_L3P_HSTX, 0x10, 0x10);
+	mipi_tx_lxn_set(sf_dev, SCFG_L4N_L4P_HSTX, 0x10, 0x10);
+
+	if(bit_rate == 80) {
+		pre_div=0x1,		fbk_int=2*0x33,		extd_cycle_sel=0x4,
+		dhs_pre_time=0xe,	dhs_zero_time=0x1d,	dhs_trial_time=0x15,
+		chs_pre_time=0x5,	chs_zero_time=0x2b,	chs_trial_time=0xd,
+		chs_clk_pre_time=0xf,
+		chs_clk_post_time=0x71;
+	} else if (bit_rate == 100) {
+		pre_div=0x1,		fbk_int=2*0x40,		extd_cycle_sel=0x4,
+		dhs_pre_time=0x10,	dhs_zero_time=0x21,	dhs_trial_time=0x17,
+		chs_pre_time=0x7,	chs_zero_time=0x35,	chs_trial_time=0xf,
+		chs_clk_pre_time=0xf,
+		chs_clk_post_time=0x73;
+	} else if (bit_rate == 200) {
+		pre_div=0x1,		fbk_int=2*0x40,		extd_cycle_sel=0x3;
+		dhs_pre_time=0xc,	dhs_zero_time=0x1b,	dhs_trial_time=0x13;
+		chs_pre_time=0x7,	chs_zero_time=0x35,	chs_trial_time=0xf,
+		chs_clk_pre_time=0x7,
+		chs_clk_post_time=0x3f;
+	} else if(bit_rate == 300) {
+		pre_div=0x1,		fbk_int=2*0x60, 	extd_cycle_sel=0x3,
+		dhs_pre_time=0x11,	dhs_zero_time=0x25, dhs_trial_time=0x19,
+		chs_pre_time=0xa, 	chs_zero_time=0x50, chs_trial_time=0x15,
+		chs_clk_pre_time=0x7,
+		chs_clk_post_time=0x45;
+	} else if(bit_rate == 400) {
+		pre_div=0x1,      	fbk_int=2*0x40,		extd_cycle_sel=0x2,
+		dhs_pre_time=0xa, 	dhs_zero_time=0x18,	dhs_trial_time=0x11,
+		chs_pre_time=0x7, 	chs_zero_time=0x35, chs_trial_time=0xf,
+		chs_clk_pre_time=0x3,
+		chs_clk_post_time=0x25;
+	} else if(bit_rate == 500 ) {
+		pre_div=0x1,      fbk_int=2*0x50,       extd_cycle_sel=0x2,
+		dhs_pre_time=0xc, dhs_zero_time=0x1d,	dhs_trial_time=0x14,
+		chs_pre_time=0x9, chs_zero_time=0x42,	chs_trial_time=0x12,
+		chs_clk_pre_time=0x3,
+		chs_clk_post_time=0x28;
+	} else if(bit_rate == 600 ) {
+		pre_div=0x1,      fbk_int=2*0x60,       extd_cycle_sel=0x2,
+		dhs_pre_time=0xe, dhs_zero_time=0x23,	dhs_trial_time=0x17,
+		chs_pre_time=0xa, chs_zero_time=0x50,	chs_trial_time=0x15,
+		chs_clk_pre_time=0x3,
+		chs_clk_post_time=0x2b;
+	} else if(bit_rate == 700) {
+		pre_div=0x1,      fbk_int=2*0x38,       extd_cycle_sel=0x1,
+		dhs_pre_time=0x8, dhs_zero_time=0x14,	dhs_trial_time=0xf,
+		chs_pre_time=0x6, chs_zero_time=0x2f,	chs_trial_time=0xe,
+		chs_clk_pre_time=0x1,
+		chs_clk_post_time=0x16;
+	} else if(bit_rate == 800 ) {
+		pre_div=0x1,      fbk_int=2*0x40,       extd_cycle_sel=0x1,
+		dhs_pre_time=0x9, dhs_zero_time=0x17,	dhs_trial_time=0x10,
+		chs_pre_time=0x7, chs_zero_time=0x35,	chs_trial_time=0xf,
+		chs_clk_pre_time=0x1,
+		chs_clk_post_time=0x18;
+	} else if(bit_rate == 900 ) {
+		pre_div=0x1,      fbk_int=2*0x48,       extd_cycle_sel=0x1,
+		dhs_pre_time=0xa, dhs_zero_time=0x19, 	dhs_trial_time=0x12,
+		chs_pre_time=0x8, chs_zero_time=0x3c, 	chs_trial_time=0x10,
+		chs_clk_pre_time=0x1,
+		chs_clk_post_time=0x19;
+	} else if(bit_rate == 1000) {
+		pre_div=0x1,      fbk_int=2*0x50,       extd_cycle_sel=0x1,
+		dhs_pre_time=0xb, dhs_zero_time=0x1c,	dhs_trial_time=0x13,
+		chs_pre_time=0x9, chs_zero_time=0x42,	chs_trial_time=0x12,
+		chs_clk_pre_time=0x1,
+		chs_clk_post_time=0x1b;
+	} else if(bit_rate == 1100) {
+		pre_div=0x1,      fbk_int=2*0x58,       extd_cycle_sel=0x1,
+		dhs_pre_time=0xc, dhs_zero_time=0x1e,	dhs_trial_time=0x15,
+		chs_pre_time=0x9, chs_zero_time=0x4a,	chs_trial_time=0x14,
+		chs_clk_pre_time=0x1,
+		chs_clk_post_time=0x1d;
+	} else if(bit_rate == 1200) {
+		pre_div=0x1,      fbk_int=2*0x60,       extd_cycle_sel=0x1,
+		dhs_pre_time=0xe, dhs_zero_time=0x20,	dhs_trial_time=0x16,
+		chs_pre_time=0xa, chs_zero_time=0x50,	chs_trial_time=0x15,
+		chs_clk_pre_time=0x1,
+		chs_clk_post_time=0x1e;
+	} else if(bit_rate == 1300) {
+		pre_div=0x1,      fbk_int=2*0x34,       extd_cycle_sel=0x0,
+		dhs_pre_time=0x7, dhs_zero_time=0x12,	dhs_trial_time=0xd,
+		chs_pre_time=0x5, chs_zero_time=0x2c,	chs_trial_time=0xd,
+		chs_clk_pre_time=0x0,
+		chs_clk_post_time=0xf;
+	} else if(bit_rate == 1400) {
+		pre_div=0x1,      fbk_int=2*0x38,       extd_cycle_sel=0x0,
+		dhs_pre_time=0x7, dhs_zero_time=0x14,	dhs_trial_time=0xe,
+		chs_pre_time=0x6, chs_zero_time=0x2f,	chs_trial_time=0xe,
+		chs_clk_pre_time=0x0,
+		chs_clk_post_time=0x10;
+	} else if(bit_rate == 1500) {
+		pre_div=0x1,      fbk_int=2*0x3c,       extd_cycle_sel=0x0,
+		dhs_pre_time=0x8, dhs_zero_time=0x14,	dhs_trial_time=0xf,
+		chs_pre_time=0x6, chs_zero_time=0x32,	chs_trial_time=0xe,
+		chs_clk_pre_time=0x0,
+		chs_clk_post_time=0x11;
+	} else if(bit_rate == 1600) {
+		pre_div=0x1,      fbk_int=2*0x40,       extd_cycle_sel=0x0,
+		dhs_pre_time=0x9, dhs_zero_time=0x15,	dhs_trial_time=0x10,
+		chs_pre_time=0x7, chs_zero_time=0x35,	chs_trial_time=0xf,
+		chs_clk_pre_time=0x0,
+		chs_clk_post_time=0x12;
+	} else if(bit_rate == 1700) {
+		pre_div=0x1,      fbk_int=2*0x44,       extd_cycle_sel=0x0,
+		dhs_pre_time=0x9, dhs_zero_time=0x17,	dhs_trial_time=0x10,
+		chs_pre_time=0x7, chs_zero_time=0x39,	chs_trial_time=0x10,
+		chs_clk_pre_time=0x0,
+		chs_clk_post_time=0x12;
+	} else if(bit_rate == 1800) {
+		pre_div=0x1,      fbk_int=2*0x48,       extd_cycle_sel=0x0,
+		dhs_pre_time=0xa, dhs_zero_time=0x18,	dhs_trial_time=0x11,
+		chs_pre_time=0x8, chs_zero_time=0x3c,	chs_trial_time=0x10,
+		chs_clk_pre_time=0x0,
+		chs_clk_post_time=0x13;
+	} else if(bit_rate == 1900) {
+		pre_div=0x1,      fbk_int=2*0x4c,       extd_cycle_sel=0x0,
+		dhs_pre_time=0xa, dhs_zero_time=0x1a,	dhs_trial_time=0x12,
+		chs_pre_time=0x8, chs_zero_time=0x3f,	chs_trial_time=0x11,
+		chs_clk_pre_time=0x0,
+		chs_clk_post_time=0x14;
+	} else if(bit_rate == 2000) {
+		pre_div=0x1,      fbk_int=2*0x50,       extd_cycle_sel=0x0,
+		dhs_pre_time=0xb, dhs_zero_time=0x1b,	dhs_trial_time=0x13,
+		chs_pre_time=0x9, chs_zero_time=0x42,	chs_trial_time=0x12,
+		chs_clk_pre_time=0x0,
+		chs_clk_post_time=0x15;
+	} else if(bit_rate == 2100) {
+		pre_div=0x1,      fbk_int=2*0x54,       extd_cycle_sel=0x0,
+		dhs_pre_time=0xb, dhs_zero_time=0x1c,	dhs_trial_time=0x13,
+		chs_pre_time=0x9, chs_zero_time=0x46,	chs_trial_time=0x13,
+		chs_clk_pre_time=0x0,
+		chs_clk_post_time=0x15;
+	} else if(bit_rate == 2200) {
+		pre_div=0x1,      fbk_int=2*0x5b,       extd_cycle_sel=0x0,
+		dhs_pre_time=0xc, dhs_zero_time=0x1d,	dhs_trial_time=0x14,
+		chs_pre_time=0x9, chs_zero_time=0x4a,	chs_trial_time=0x14,
+		chs_clk_pre_time=0x0,
+		chs_clk_post_time=0x16;
+	} else if(bit_rate == 2300) {
+		pre_div=0x1,      fbk_int=2*0x5c,       extd_cycle_sel=0x0,
+		dhs_pre_time=0xc, dhs_zero_time=0x1f,	dhs_trial_time=0x15,
+		chs_pre_time=0xa, chs_zero_time=0x4c,	chs_trial_time=0x14,
+		chs_clk_pre_time=0x0,
+		chs_clk_post_time=0x17;
+	} else if(bit_rate == 2400) {
+		pre_div=0x1,      fbk_int=2*0x60,       extd_cycle_sel=0x0,
+		dhs_pre_time=0xd, dhs_zero_time=0x20,	dhs_trial_time=0x16,
+		chs_pre_time=0xa, chs_zero_time=0x50,	chs_trial_time=0x15,
+		chs_clk_pre_time=0x0,
+		chs_clk_post_time=0x18;
+	} else if(bit_rate == 2500) {
+		pre_div=0x1,      fbk_int=2*0x64,       extd_cycle_sel=0x0,
+		dhs_pre_time=0xe, dhs_zero_time=0x21,	dhs_trial_time=0x16,
+		chs_pre_time=0xb, chs_zero_time=0x53,	chs_trial_time=0x16,
+		chs_clk_pre_time=0x0,
+		chs_clk_post_time=0x18;
+	} else {
+		//default bit_rate == 700
+		pre_div=0x1,      fbk_int=2*0x38,       extd_cycle_sel=0x1,
+		dhs_pre_time=0x8, dhs_zero_time=0x14,	dhs_trial_time=0xf,
+		chs_pre_time=0x6, chs_zero_time=0x2f,	chs_trial_time=0xe,
+		chs_clk_pre_time=0x1,
+		chs_clk_post_time=0x16;
+		MIPITX_ERR(" ERROR: invalid bit rate configuration!\n");
+	}
+	sf_fb_cfgwrite32(sf_dev, SCFG_REFCLK_SEL, 0x3);
+
+	set_val = 0
+			| (1 << OFFSET_CFG_L1_SWAP_SEL)
+			| (4 << OFFSET_CFG_L2_SWAP_SEL)
+			| (2 << OFFSET_CFG_L3_SWAP_SEL)
+			| (3 << OFFSET_CFG_L4_SWAP_SEL);
+	sf_fb_cfgwrite32(sf_dev, SCFG_LX_SWAP_SEL, set_val);
+
+	set_val = 0
+			| (0 << OFFSET_SCFG_PWRON_READY_N)
+			| (1 << OFFSET_RG_CDTX_PLL_FM_EN)
+			| (0 << OFFSET_SCFG_PLLSSC_EN)
+			| (1 << OFFSET_RG_CDTX_PLL_LDO_STB_X2_EN);
+	sf_fb_cfgwrite32(sf_dev, SCFG_DBUS_PW_PLL_SSC_LD0, set_val);
+
+	set_val = fbk_int
+			| (pre_div << 9);
+	sf_fb_cfgwrite32(sf_dev, SCFG_RG_CDTX_PLL_FBK_PRE, set_val);
+
+	sf_fb_cfgwrite32(sf_dev, SCFG_RG_EXTD_CYCLE_SEL, extd_cycle_sel);
+
+	set_val = chs_zero_time
+			| (dhs_pre_time << OFFSET_DHS_PRE_TIME)
+			| (dhs_trial_time << OFFSET_DHS_TRIAL_TIME)
+			| (dhs_zero_time << OFFSET_DHS_ZERO_TIME);
+	sf_fb_cfgwrite32(sf_dev, SCFG_RG_CLANE_DLANE_TIME, set_val);
+
+	set_val = chs_clk_post_time
+			| (chs_clk_pre_time << OFFSET_CHS_PRE_TIME)
+			| (chs_pre_time << OFFSET_CHS_TRIAL_TIME)
+			| (chs_trial_time << OFFSET_CHS_ZERO_TIME);
+	sf_fb_cfgwrite32(sf_dev, SCFG_RG_CLANE_HS_TIME, set_val);
+
+}
+
+void reset_dphy(struct sf_fb_data *sf_dev, int resetb)
+{
+	u32 cfg_link_enable = 0x01;//bit0
+	u32 cfg_ck2_ck3_ck_enable = 0x07;//bit0-3
+	u32 cfg_ck1_dat_enable = 0x1f<<3;//bit3-7
+	u32 cfg_dsc_enable = 0x01;//bit0
+	u32 precfg = sf_fb_dsitxread32(sf_dev, VID_MCTL_MAIN_EN) & ~cfg_ck1_dat_enable;
+	sf_fb_dsitxwrite32(sf_dev, VID_MCTL_MAIN_EN, precfg|cfg_ck1_dat_enable);
+
+	precfg = sf_fb_dsitxread32(sf_dev, VID_MCTL_MAIN_PHY_CTL) & ~cfg_ck2_ck3_ck_enable;
+	sf_fb_dsitxwrite32(sf_dev, VID_MCTL_MAIN_PHY_CTL, precfg|cfg_ck2_ck3_ck_enable);
+
+	precfg = sf_fb_dsitxread32(sf_dev, VID_MCTL_MAIN_DATA_CTL) & ~cfg_link_enable;
+	sf_fb_dsitxwrite32(sf_dev, VID_MCTL_MAIN_DATA_CTL, precfg|cfg_link_enable);
+
+	precfg = sf_fb_cfgread32(sf_dev, SCFG_PHY_RESETB);
+	precfg &= ~(cfg_dsc_enable);
+	precfg |= (resetb&cfg_dsc_enable);
+	sf_fb_cfgwrite32(sf_dev, SCFG_PHY_RESETB, precfg);
+}
+
+void polling_dphy_lock(struct sf_fb_data *sf_dev)
+{
+	int pll_unlock;
+
+	udelay(10);
+
+	do {
+		pll_unlock = sf_fb_cfgread32(sf_dev, SCFG_GRS_CDTX_PLL) >> 3;
+		pll_unlock &= 0x1;
+		MIPITX_PRT("%s check\n",__func__, __LINE__);
+	} while(pll_unlock == 0x1);
+	//udelay(10);
+}
+
+static int dsitx_phy_config(struct sf_fb_data *sf_dev)
+{
+	uint32_t bit_rate = sf_dev->panel_info.dphy_bps/1000000UL;//(1920 * 1080 * bpp / dlanes * fps / 1000000 + 99) / 100 * 100;
+
+	dphy_config(sf_dev, bit_rate);
+	reset_dphy(sf_dev, 1);
+	mdelay(10);
+	polling_dphy_lock(sf_dev);
+
+	return 0;
+}
+
+void release_txbyte_rst(struct sf_fb_data *sf_dev)
+{
+	u32 temp = sf_fb_rstread32(sf_dev, SRST_ASSERT0);
+	temp &= ~(0x1<<18);
+	temp |= (0x0&0x1)<<18;
+	sf_fb_rstwrite32(sf_dev, SRST_ASSERT0, temp);
+
+	do {
+		temp = sf_fb_rstread32(sf_dev, SRST_STATUS0) >> 18;
+		temp &= 0x1;
+		MIPITX_PRT("%s check\n",__func__, __LINE__);
+	} while (temp != 0x1 );
+	//udelay(1);
+	MIPITX_PRT("Tx byte reset released for csi2tx and dsitx\n");
+}
+
+void vid_size_cfg_update(struct sf_fb_data *sf_dev)
+{
+	int vcfg1 = sf_dev->panel_info.dsi_vsa | (sf_dev->panel_info.dsi_vbp<<6) | (sf_dev->panel_info.dsi_vfp<<12);
+
+	int hsa_len = (sf_dev->panel_info.dsi_sync_pulse==0) ? 0 : sf_dev->panel_info.dsi_hsa-14;
+	int hbp_len = (sf_dev->panel_info.dsi_sync_pulse==0) ? sf_dev->panel_info.dsi_hsa+sf_dev->panel_info.dsi_hbp-12 : sf_dev->panel_info.dsi_hbp-12;
+	int hact_len = sf_dev->panel_info.w * sf_dev->panel_info.bpp/8;
+	int hfp_len =(sf_dev->panel_info.dsi_burst_mode) ? 0x0 : (sf_dev->panel_info.dsi_hfp-6);
+	int hcfg1 = hsa_len|(hbp_len<<16);
+	int hcfg2 = hact_len|(hfp_len<<16);
+
+	hbp_len = sf_dev->panel_info.dsi_burst_mode ? (sf_dev->panel_info.dphy_lanes * (sf_dev->panel_info.dsi_hsa + sf_dev->panel_info.dsi_hbp)) - 12 + 300 : hbp_len;
+	sf_fb_dsitxwrite32(sf_dev, VID_VSIZE1_ADDR, vcfg1);
+	sf_fb_dsitxwrite32(sf_dev, VID_VSIZE2_ADDR, sf_dev->panel_info.h);
+	MIPITX_PRT("DSI VSA: %d, VBP: %d, VFP: %d, VACT: %d\n", sf_dev->panel_info.dsi_vsa, sf_dev->panel_info.dsi_vbp, sf_dev->panel_info.dsi_vfp, sf_dev->panel_info.h);
+
+	sf_fb_dsitxwrite32(sf_dev, VID_HSIZE1_ADDR, hcfg1);
+	sf_fb_dsitxwrite32(sf_dev, VID_HSIZE2_ADDR, hcfg2);
+	MIPITX_PRT("DSI HSA: %d, HBP: %d, HFP: %d, HACT: %d\n", hsa_len, hbp_len, hfp_len, hact_len);
+
+	sf_fb_dsitxwrite32(sf_dev, VID_ERR_COLOR1_ADDR, (0xcc<<12)|0xaa);
+	sf_fb_dsitxwrite32(sf_dev, VID_ERR_COLOR2_ADDR, (0xee<<12)|0x55);
+}
+
+static int div_roundup(int dived, int divsor)
+{
+	int q = ((dived-1)/divsor)+1;
+
+	return q;
+}
+
+void vid_blktime_cfg_update(struct sf_fb_data *sf_dev)
+{
+	int hsa_dsi = (sf_dev->panel_info.dsi_sync_pulse==0) ? 0 : sf_dev->panel_info.dsi_hsa- 14;
+	//int hbp_dsi = (pulse_event==0) ? (hsa+hbp)-12 : hbp - 12;
+	//int hact_dsi = hact;
+	//int hfp_dsi = (burst_en) ? 0 : hfp-6;
+	int hline_bytes = (sf_dev->panel_info.dsi_hsa + sf_dev->panel_info.dsi_hbp + sf_dev->panel_info.w * sf_dev->panel_info.bpp/8 + sf_dev->panel_info.dsi_hfp);
+	int total_line = div_roundup(hline_bytes, sf_dev->panel_info.dphy_lanes);
+	int blkline_pulse_pck = hline_bytes - 20 - hsa_dsi;
+	int pulse_reg_dura = total_line - div_roundup(hsa_dsi+14,sf_dev->panel_info.dphy_lanes);
+	int blkline_event_pck0 = hline_bytes - 10;
+	int event_reg_dura = total_line - div_roundup(8, sf_dev->panel_info.dphy_lanes);
+	int burst_blkline_pck = hline_bytes * 2 - 4;
+	int burst_reg_dura = total_line * 2 - div_roundup(4, sf_dev->panel_info.dphy_lanes);
+	//int burst_hbp = 2 * (hsa+hbp)- 12 + fifo_fill;
+	int blkeol_pck = burst_blkline_pck - (sf_dev->panel_info.w * sf_dev->panel_info.bpp/8 + 6 + sf_dev->panel_info.dsi_hbp + 6);
+	//int txbyte_cycles = div_roundup((burst_hbp + hact_dsi), nlane);
+	int blkeol_dura = div_roundup(blkeol_pck + 6, sf_dev->panel_info.dphy_lanes);
+	int blkline_event_pck = (sf_dev->panel_info.dsi_burst_mode) ? burst_blkline_pck : blkline_event_pck0;
+	int reg_line_duration = (sf_dev->panel_info.dsi_burst_mode) ? burst_reg_dura :
+					(sf_dev->panel_info.dsi_sync_pulse==0) ? event_reg_dura : pulse_reg_dura;
+	int reg_wakeup_time = DPHY_REG_WAKEUP_TIME<<17;
+	int dphy_time = reg_line_duration | reg_wakeup_time;
+	int max_line = (sf_dev->panel_info.dsi_sync_pulse) ? (blkline_pulse_pck-6)<<16 : (blkline_event_pck-6)<<16 ;
+	int exact_burst = blkeol_pck;
+
+	sf_fb_dsitxwrite32(sf_dev, VID_BLKSIZE1_ADDR, (blkeol_pck<<15)|blkline_event_pck);
+	MIPITX_PRT("DSI blkeol_pck: %d, blkline_event_pck: %d\n", blkeol_pck, blkline_event_pck);
+
+	sf_fb_dsitxwrite32(sf_dev, VID_BLKSIZE2_ADDR, blkline_pulse_pck);
+	MIPITX_PRT("DSI blkline_pulse_pck: %d\n", blkline_pulse_pck);
+
+	sf_fb_dsitxwrite32(sf_dev, VID_PCK_TIME_ADDR, blkeol_dura);
+	MIPITX_PRT("DSI blkeol_duration: %d\n", blkeol_dura);
+
+	sf_fb_dsitxwrite32(sf_dev, VID_DPHY_TIME_ADDR, dphy_time);
+	MIPITX_PRT("DSI reg line duration: %d, wakeup time: %d\n", reg_line_duration, reg_wakeup_time>>17);
+
+	sf_fb_dsitxwrite32(sf_dev, VID_VCA_SET2_ADDR, max_line|exact_burst);
+	MIPITX_PRT("DSI max_line: %d, max_burst: %d\n", max_line>>16, exact_burst);
+}
+
+void dsi_main_cfg(struct sf_fb_data *sf_dev)
+{
+	//PHY Main control
+	int hs_continous = 0x01;
+	int te = 0x00;
+	int cmdEn = 0x00;
+	int cont = hs_continous<<4;
+	int lanen = 0xF>>(4-(sf_dev->panel_info.dphy_lanes-1));
+	int write_burst = 0x3c<<8;
+	int main_phy_cfg = cont | lanen | write_burst;
+	int tvg_en = 0;
+	int lane_en = (0x1F>>(4-sf_dev->panel_info.dphy_lanes))<<3;
+	int dpi_en = 0x1<<14;
+	int main_en_cfg = lane_en | dpi_en | 0x1;
+	//main data ctrl, 0x4
+	int te_en = (te<<12) | (te<<8) | (te<<24);
+	int bta = cmdEn<<14;
+	int rden = cmdEn<<13;
+	int tvg = tvg_en<<6;
+	int vid_en = 0x1<<5;
+	int vid_if_sel = 0x1<<2;
+	int sdi_mode = 0x1<<1;
+	int link_en = 0x1;
+	int interface = 0x0;//0x3<<1;
+	int main_cfg = link_en | interface | sdi_mode | vid_if_sel| vid_en | tvg | rden | bta | te_en;
+	//VID Main Ctrl, 0xb0
+	int idle_miss_vsync=0x1<<31;
+	int recovery_mode = 0x1<<25;
+	int h_sync_pulse = sf_dev->panel_info.dsi_sync_pulse<<20;
+	int sync_active = sf_dev->panel_info.dsi_sync_pulse<<19;
+	int burst_mode = sf_dev->panel_info.dsi_burst_mode<<18;
+	int pix_mode = 0x3<<14;
+	int header = 0x3e<<8;
+	int cfg = header|pix_mode|burst_mode|sync_active|h_sync_pulse|recovery_mode|idle_miss_vsync;
+	//TVG main ctrl
+	int strp_size = 0x7<<5;
+	int tvg_md = TVG_MODE<<3;
+	int tvg_stop = 0x1<<1;
+	int start_tvg = tvg_en;
+	int tvg_cfg = start_tvg | tvg_stop | tvg_md | strp_size;
+
+	sf_fb_dsitxwrite32(sf_dev, PHY_TIMEOUT1_ADDR, 0xafffb);
+	sf_fb_dsitxwrite32(sf_dev, PHY_TIMEOUT2_ADDR, 0x3ffff);
+	sf_fb_dsitxwrite32(sf_dev, ULPOUT_TIME_ADDR, 0x3ab05);
+
+	sf_fb_dsitxwrite32(sf_dev, MAIN_PHY_CTRL_ADDR, main_phy_cfg);
+	sf_fb_dsitxwrite32(sf_dev, MAIN_EN_ADDR, main_en_cfg);
+
+	sf_fb_dsitxwrite32(sf_dev, MAIN_DATA_CTRL_ADDR, main_cfg);
+	sf_fb_dsitxwrite32(sf_dev, VID_MAIN_CTRL_ADDR, cfg);
+	sf_fb_dsitxwrite32(sf_dev, DIRECT_CMD_STAT_CTRL_ADDR, 0x80);
+	sf_fb_dsitxwrite32(sf_dev, TVG_CTRL_ADDR, tvg_cfg);
+	MIPITX_PRT("DSI TVG main ctrl 0xfc: 0x%x\n", tvg_cfg);
+}
+
+int dsitx_dcs_write(struct sf_fb_data *sf_dev, int cmd_size, ...)
+{
+	int ret = 0;
+	u32 exp_sts_mask = 0x2; // [1]write complete
+	u32 exp_sts = 0x2;
+	// transfer the sequence
+	int i;
+	struct dcs_buffer wbuf;
+	va_list ap;
+
+	// dcs cmd config
+	int cmd_head = (cmd_size < 2 ? CMD_HEAD_WRITE_0 :
+					(cmd_size < 3 ? CMD_HEAD_WRITE_1 :
+					CMD_HEAD_WRITE_N));
+	dcs_start(sf_dev, cmd_head, cmd_size, CMD_NAT_WRITE);
+
+	wbuf.len = 0;
+	wbuf.val32 = 0;
+	va_start(ap, cmd_size);
+	for (i = 0; i < cmd_size; i++) {
+		wbuf.val8[wbuf.len++] = (char)va_arg(ap, int);
+		if (((i + 1) & 0x3) == 0) {
+			dcs_write_32(sf_dev, wbuf.val32);
+			wbuf.len = 0;
+			wbuf.val32 = 0;
+		}
+	}
+	if (i & 0x3) {
+		dcs_write_32(sf_dev, wbuf.val32);
+		wbuf.len = 0;
+		wbuf.val32 = 0;
+	}
+	va_end(ap);
+
+	// wait transfer complete
+	dcs_wait_finish(sf_dev, exp_sts_mask, exp_sts);
+
+	return ret;
+}
+
+static int seeed_panel_send_cmd(struct sf_fb_data *sf_dev, u16 reg, u32 val)
+{
+	u8 msg[] = {
+		reg,
+		reg >> 8,
+		val,
+		val >> 8,
+		val >> 16,
+		val >> 24,
+	};
+
+	dsitx_dcs_write(sf_dev, 6, msg[0], msg[1], msg[2], msg[3], msg[4], msg[5]);
+
+	return 0;
+}
+
+static int seeed_panel_enable(struct sf_fb_data *sf_dev)
+{
+	seeed_panel_send_cmd(sf_dev, DSI_LANEENABLE,
+			DSI_LANEENABLE_CLOCK |
+			DSI_LANEENABLE_D0);
+	seeed_panel_send_cmd(sf_dev, PPI_D0S_CLRSIPOCOUNT, 0x05);
+	seeed_panel_send_cmd(sf_dev, PPI_D1S_CLRSIPOCOUNT, 0x05);
+	seeed_panel_send_cmd(sf_dev, PPI_D0S_ATMR, 0x00);
+	seeed_panel_send_cmd(sf_dev, PPI_D1S_ATMR, 0x00);
+	seeed_panel_send_cmd(sf_dev, PPI_LPTXTIMECNT, 0x03);
+	seeed_panel_send_cmd(sf_dev, SPICMR, 0x00);
+	seeed_panel_send_cmd(sf_dev, LCDCTRL, 0x00100150);
+	seeed_panel_send_cmd(sf_dev, SYSCTRL, 0x040f);
+	mdelay(100);
+	seeed_panel_send_cmd(sf_dev, PPI_STARTPPI, 0x01);
+	seeed_panel_send_cmd(sf_dev, DSI_STARTDSI, 0x01);
+	mdelay(100);
+
+	return 0;
+}
+
+void dpi_cfg(struct sf_fb_data *sf_dev, int int_en) {
+	sf_fb_dsitxwrite32(sf_dev, DPI_IRQ_EN_ADDR, int_en);
+}
+
+int sf_mipi_init(struct sf_fb_data *sf_dev)
+{
+	int ret = 0;
+	uint32_t dpi_fifo_int = 0;
+
+	dsi_csi2tx_sel(sf_dev, DSI_CONN_LCDC);
+	dphy_clane_hs_txready_sel(sf_dev, 0x1);
+
+	dsitx_phy_config(sf_dev);
+	release_txbyte_rst(sf_dev);
+	mdelay(100);
+
+	dpi_fifo_int = sf_fb_dsitxread32(sf_dev, DPI_IRQ_CLR_ADDR);
+	if (dpi_fifo_int) {
+		sf_fb_dsitxwrite32(sf_dev, DPI_IRQ_CLR_ADDR, 1);
+	}
+
+	vid_size_cfg_update(sf_dev);
+	vid_blktime_cfg_update(sf_dev);
+	dsi_main_cfg(sf_dev);
+	mdelay(100);
+	seeed_panel_enable(sf_dev);
+	dpi_cfg(sf_dev, 1);
+
+	return ret;
+}
+
+EXPORT_SYMBOL(sf_mipi_init);
+MODULE_AUTHOR("StarFive Technology Co., Ltd.");
+MODULE_DESCRIPTION("loadable MIPI Tx driver for StarFive");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/fbdev/starfive/starfive_mipi_tx.h b/drivers/video/fbdev/starfive/starfive_mipi_tx.h
new file mode 100755
index 000000000000..3fd6723fa2ab
--- /dev/null
+++ b/drivers/video/fbdev/starfive/starfive_mipi_tx.h
@@ -0,0 +1,202 @@
+/*
+ * StarFive Vout driver
+ *
+ * Copyright 2021 StarFive Inc.
+ *
+ * Licensed under the GPL-2.
+ */
+
+#ifndef __SF_FB_MIPI_TX_H__
+#define __SF_FB_MIPI_TX_H__
+
+//PHY timing paramter
+#define TVG_MODE 0x2
+
+#define	BIT_RATE 1500
+
+#define DSI_RATE ((BIT_RATE/100)+1)*100
+#define DPHY_REG_WAKEUP_TIME  0xE5  //0x27
+
+
+#define DSI_CONN_LCDC 0
+#define CSI_CONN_LCDC 1
+
+/* DSI PPI Layer Registers */
+#define PPI_STARTPPI        0x0104
+#define PPI_BUSYPPI     0x0108
+#define PPI_LINEINITCNT     0x0110
+#define PPI_LPTXTIMECNT     0x0114
+#define PPI_CLS_ATMR        0x0140
+#define PPI_D0S_ATMR        0x0144
+#define PPI_D1S_ATMR        0x0148
+#define PPI_D0S_CLRSIPOCOUNT    0x0164
+#define PPI_D1S_CLRSIPOCOUNT    0x0168
+#define CLS_PRE         0x0180
+#define D0S_PRE         0x0184
+#define D1S_PRE         0x0188
+#define CLS_PREP        0x01A0
+#define D0S_PREP        0x01A4
+#define D1S_PREP        0x01A8
+#define CLS_ZERO        0x01C0
+#define D0S_ZERO        0x01C4
+#define D1S_ZERO        0x01C8
+#define PPI_CLRFLG      0x01E0
+#define PPI_CLRSIPO     0x01E4
+#define HSTIMEOUT       0x01F0
+#define HSTIMEOUTENABLE     0x01F4
+
+/* DSI Protocol Layer Registers */
+#define DSI_STARTDSI        0x0204
+#define DSI_BUSYDSI     0x0208
+#define DSI_LANEENABLE      0x0210
+# define DSI_LANEENABLE_CLOCK       BIT(0)
+# define DSI_LANEENABLE_D0      BIT(1)
+# define DSI_LANEENABLE_D1      BIT(2)
+
+#define DSI_LANESTATUS0     0x0214
+#define DSI_LANESTATUS1     0x0218
+#define DSI_INTSTATUS       0x0220
+#define DSI_INTMASK     0x0224
+#define DSI_INTCLR      0x0228
+#define DSI_LPTXTO      0x0230
+#define DSI_MODE        0x0260
+#define DSI_PAYLOAD0        0x0268
+#define DSI_PAYLOAD1        0x026C
+#define DSI_SHORTPKTDAT     0x0270
+#define DSI_SHORTPKTREQ     0x0274
+#define DSI_BTASTA      0x0278
+#define DSI_BTACLR      0x027C
+
+/* LCDC/DPI Host Registers */
+#define LCDCTRL         0x0420
+#define HSR         0x0424
+#define HDISPR          0x0428
+#define VSR         0x042C
+#define VDISPR          0x0430
+#define VFUEN           0x0434
+
+/* DBI-B Host Registers */
+#define DBIBCTRL        0x0440
+
+/* SPI Master Registers */
+#define SPICMR          0x0450
+#define SPITCR          0x0454
+
+/* System Controller Registers */
+#define SYSSTAT         0x0460
+#define SYSCTRL         0x0464
+#define SYSPLL1         0x0468
+#define SYSPLL2         0x046C
+#define SYSPLL3         0x0470
+#define SYSPMCTRL       0x047C
+
+/*mipi cmd*/
+#define CMD_HEAD_WRITE_0    0x05
+#define CMD_HEAD_WRITE_1    0x15
+#define CMD_HEAD_WRITE_N    0x39
+#define CMD_HEAD_READ       0x06
+
+#define CMD_NAT_WRITE       0x00
+#define CMD_NAT_READ        0x01
+#define CMD_NAT_TE          0x04
+#define CMD_NAT_TRIGGER     0x05
+#define CMD_NAT_BTA         0x06
+
+//dsitx reg , base addr 0x12100000
+#define  MAIN_DATA_CTRL_ADDR  0x004
+#define  MAIN_EN_ADDR         0x00C
+#define  MAIN_PHY_CTRL_ADDR   0x08
+#define  MAIN_STAT_CTRL_ADDR  0x130
+#define  MAIN_STAT_ADDR       0x024
+#define  MAIN_STAT_CLR_ADDR   0x150
+#define  MAIN_STAT_FLAG_ADDR  0x170
+
+#define  PHY_CTRL_ADDR        0x008
+#define  PHY_TIMEOUT1_ADDR    0x014
+#define  PHY_TIMEOUT2_ADDR    0x018
+#define  ULPOUT_TIME_ADDR     0x01c
+#define  DPHY_ERR_ADDR        0x28
+#define  LANE_STAT_ADDR       0x2C
+#define  PHY_SKEWCAL_TIMEOUT_ADDR 0x040
+#define  PHY_ERR_CTRL1_ADDR   0x148
+#define  PHY_ERR_CTRL2_ADDR   0x14c
+#define  PHY_ERR_CLR_ADDR     0x168
+#define  PHY_ERR_FLAG_ADDR    0x188
+
+#define  CMD_MODE_CTRL_ADDR   0x70
+#define  CMD_MODE_CTRL2_ADDR  0x74
+#define  CMD_MODE_STAT_ADDR   0x78
+#define  CMD_MODE_STAT_CTRL_ADDR     0x134
+#define  CMD_MODE_STAT_CLR_ADDR      0x154
+#define  CMD_MODE_STAT_FLAG_ADDR     0x174
+
+#define  DIRECT_CMD_STAT_CTRL_ADDR   0x138
+#define  DIRECT_CMD_STAT_CTL_ADDR    0x140
+
+#define  DIRECT_CMD_STAT_CLR_ADDR    0x158
+#define  DIRECT_CMD_STAT_FLAG_ADDR   0x178
+#define  DIRECT_CMD_RDSTAT_CTRL_ADDR 0x13C
+#define  DIRECT_CMD_RDSTAT_CLR_ADDR  0x15C
+#define  DIRECT_CMD_RDSTAT_FLAG_ADDR 0x17C
+#define  DIRECT_CMD_SEND_ADDR    0x80
+#define  DIRECT_CMD_MAINSET_ADDR 0x84
+#define  DIRECT_CMD_STAT_ADDR    0x88
+#define  DIRECT_CMD_RDINIT_ADDR  0x8c
+#define  DIRECT_CMD_WRDAT_ADDR   0x90
+#define  DIRECT_CMD_FIFORST_ADDR 0x94
+#define  DIRECT_CMD_RDATA_ADDR  0xa0
+#define  DIRECT_CMD_RDPROP_ADDR 0xa4
+#define  DIRECT_CMD_RDSTAT_ADDR 0xa8
+
+//dsitx registers
+#define  VID_MCTL_MAIN_DATA_CTL	        0x04
+#define  VID_MCTL_MAIN_PHY_CTL	        0x08
+#define  VID_MCTL_MAIN_EN	            0x0c
+#define  VID_MAIN_CTRL_ADDR    0xb0
+#define  VID_VSIZE1_ADDR       0xb4
+#define  VID_VSIZE2_ADDR       0xb8
+#define  VID_HSIZE1_ADDR       0xc0
+#define  VID_HSIZE2_ADDR       0xc4
+#define  VID_BLKSIZE1_ADDR     0xCC
+#define  VID_BLKSIZE2_ADDR     0xd0
+#define  VID_PCK_TIME_ADDR     0xd8
+#define  VID_DPHY_TIME_ADDR    0xdc
+#define  VID_ERR_COLOR1_ADDR   0xe0
+#define  VID_ERR_COLOR2_ADDR   0xe4
+#define  VID_VPOS_ADDR         0xe8
+#define  VID_HPOS_ADDR         0xec
+#define  VID_MODE_STAT_ADDR    0xf0
+#define  VID_VCA_SET1_ADDR     0xf4
+#define  VID_VCA_SET2_ADDR     0xf8
+
+#define  VID_MODE_STAT_CLR_ADDR    0x160
+#define  VID_MODE_STAT_FLAG_ADDR   0x180
+
+#define  TVG_CTRL_ADDR      0x0fc
+#define  TVG_IMG_SIZE_ADDR  0x100
+#define  TVG_COLOR1_ADDR    0x104
+#define  TVG_COLOR1BIT_ADDR 0x108
+#define  TVG_COLOR2_ADDR    0x10c
+#define  TVG_COLOR2BIT_ADDR 0x110
+#define  TVG_STAT_ADDR      0x114
+#define  TVG_STAT_CTRL_ADDR 0x144
+#define  TVG_STAT_CLR_ADDR  0x164
+#define  TVG_STAT_FLAG_ADDR 0x184
+
+#define  DPI_IRQ_EN_ADDR   0x1a0
+#define  DPI_IRQ_CLR_ADDR  0x1a4
+#define  DPI_IRQ_STAT_ADDR 0x1a4
+#define  DPI_CFG_ADDR      0x1ac
+
+struct dcs_buffer {
+	u32 len;
+	union {
+		u32 val32;
+		char val8[4];
+	};
+};
+
+extern int sf_mipi_init(struct sf_fb_data *sf_dev);
+
+#endif
+
diff --git a/drivers/video/fbdev/starfive/starfive_vpp.c b/drivers/video/fbdev/starfive/starfive_vpp.c
new file mode 100755
index 000000000000..f10af48c9fb9
--- /dev/null
+++ b/drivers/video/fbdev/starfive/starfive_vpp.c
@@ -0,0 +1,588 @@
+/* driver/video/starfive/starfive_vpp.c
+**
+** This program is free software; you can redistribute it and/or modify
+** it under the terms of the GNU General Public License version 2 as
+** published by the Free Software Foundation.
+**
+** Copyright (C) 2020 StarFive, Inc.
+**
+** PURPOSE:	This files contains the driver of VPP.
+**
+** CHANGE HISTORY:
+**	Version		Date		Author		Description
+**	0.1.0		2020-10-09	starfive		created
+**
+*/
+
+#include <linux/module.h>
+
+#include "starfive_fb.h"
+#include "starfive_vpp.h"
+
+//#define SF_PP_DEBUG	1
+#ifdef SF_PP_DEBUG
+	#define PP_PRT(format, args...)  printk(KERN_DEBUG "[pp]: " format, ## args)
+	#define PP_INFO(format, args...) printk(KERN_INFO "[pp]: " format, ## args)
+	#define PP_ERR(format, args...) printk(KERN_ERR "[pp]: " format, ## args)
+#else
+	#define PP_PRT(x...)  do{} while(0)
+	#define PP_INFO(x...)  do{} while(0)
+	#define PP_ERR(x...)  do{} while(0)
+#endif
+
+static u32 sf_fb_sysread32(struct sf_fb_data *sf_dev, u32 reg)
+{
+	return ioread32(sf_dev->base_syscfg + reg);
+}
+
+static void sf_fb_syswrite32(struct sf_fb_data *sf_dev, u32 reg, u32 val)
+{
+	iowrite32(val, sf_dev->base_syscfg + reg);
+}
+
+static u32 sf_fb_vppread32(struct sf_fb_data *sf_dev, int ppNum, u32 reg)
+{
+	void __iomem	*base_vpp = 0;
+	switch(ppNum) {
+		case 0 : {base_vpp = sf_dev->base_vpp0; break;}
+		case 1 : {base_vpp = sf_dev->base_vpp1; break;}
+		case 2 : {base_vpp = sf_dev->base_vpp2; break;}
+		default: {PP_ERR("Err：invalid vpp Number!\n"); break;}
+	}
+	return ioread32(base_vpp + reg);
+}
+
+static void sf_fb_vppwrite32(struct sf_fb_data *sf_dev, int ppNum, u32 reg, u32 val)
+{
+	void __iomem	*base_vpp = 0;
+	switch(ppNum) {
+	case 0 : {base_vpp = sf_dev->base_vpp0; break;}
+	case 1 : {base_vpp = sf_dev->base_vpp1; break;}
+	case 2 : {base_vpp = sf_dev->base_vpp2; break;}
+	default: {PP_ERR("Err：invalid vpp Number!\n"); break;}
+	}
+	iowrite32(val, base_vpp + reg);
+}
+
+void mapconv_pp0_sel(struct sf_fb_data *sf_dev, int sel)
+{
+	u32 temp;
+	temp = sf_fb_sysread32(sf_dev, SYS_MAP_CONV);
+	temp &= ~(0x1);
+	temp |= (sel & 0x1);
+	sf_fb_syswrite32(sf_dev, SYS_MAP_CONV, temp);
+}
+EXPORT_SYMBOL(mapconv_pp0_sel);
+
+void pp_output_cfg(struct sf_fb_data *sf_dev, int ppNum, int outSel, int progInter, int desformat, int ptMode)
+{
+	int cfg = outSel | progInter << PP_INTERLACE
+				| desformat << PP_DES_FORMAT
+				| ptMode << PP_POINTER_MODE;
+
+	int preCfg = 0xffff8f0 & sf_fb_vppread32(sf_dev, ppNum, PP_CTRL1);
+	sf_fb_vppwrite32(sf_dev, ppNum, PP_CTRL1, cfg | preCfg);
+	PP_PRT("PP%d outSel: %d, outFormat: 0x%x, Out Interlace: %d, ptMode: %d\n",
+		ppNum, outSel, desformat, progInter, ptMode);
+}
+
+void pp_srcfmt_cfg(struct sf_fb_data *sf_dev, int ppNum, int srcformat, int yuv420Inter, int yuv422_mode,
+			int yuv420_mode, int argbOrd)
+{
+	int cfg = srcformat << PP_SRC_FORMAT_N | yuv420Inter << PP_420_ITLC
+						| yuv422_mode << PP_SRC_422_YUV_POS
+						| yuv420_mode << PP_SRC_420_YUV_POS
+						| argbOrd << PP_SRC_ARGB_ORDER;
+
+	int preCfg = 0x83ffff0f & sf_fb_vppread32(sf_dev, ppNum, PP_CTRL1);
+	sf_fb_vppwrite32(sf_dev, ppNum, PP_CTRL1, cfg | preCfg);
+	PP_PRT("PP%d Src Format: 0x%x, YUV420 Interlace: %d, YUV422: %d, YUV420: %d, ARGB Order: %d\n",
+		ppNum, srcformat,yuv420Inter,yuv422_mode,yuv420_mode, argbOrd);
+}
+
+void pp_r2yscal_bypass(struct sf_fb_data *sf_dev, int ppNum, int r2yByp, int scalByp, int y2rByp)
+{
+	int bypass = (r2yByp | scalByp<<1 | y2rByp<<2) << PP_R2Y_BPS;
+	int preCfg = 0xffff8fff & sf_fb_vppread32(sf_dev, ppNum, PP_CTRL1);
+
+	sf_fb_vppwrite32(sf_dev, ppNum, PP_CTRL1, bypass | preCfg);
+	PP_PRT("PP%d Bypass R2Y: %d, Y2R: %d, MainSacle: %d\n", ppNum, r2yByp, y2rByp, scalByp);
+}
+
+void pp_argb_alpha(struct sf_fb_data *sf_dev, int ppNum, int alpha)
+{
+	int preCfg = 0xff00ffff & sf_fb_vppread32(sf_dev, ppNum, PP_CTRL1);
+
+	sf_fb_vppwrite32(sf_dev, ppNum, PP_CTRL1, alpha << PP_ARGB_ALPHA | preCfg);
+	PP_PRT("PP%d Alpha: 0x%4x\n", ppNum, alpha);
+}
+
+//rgbNum: 1-3
+void pp_r2y_coeff(struct sf_fb_data *sf_dev, int ppNum, int coefNum, int rcoef, int gcoef, int bcoef, int off)
+{
+	int rgcoeff = rcoef | gcoef << PP_COEF_G1;
+	int bcoefoff = bcoef| off << PP_OFFSET_1;
+
+	u32 addr1 = (coefNum - 1) * 0x8 + PP_R2Y_COEF1;
+	u32 addr2 = (coefNum - 1) * 0x8 + PP_R2Y_COEF2;
+	sf_fb_vppwrite32(sf_dev, ppNum, addr1, rgcoeff);
+	sf_fb_vppwrite32(sf_dev, ppNum, addr2, bcoefoff);
+	PP_PRT("PP%d coefNum: %d, rCoef: 0x%4x, gCoef: 0x%4x, bCoef: 0x%4x, off: 0x%4x\n",
+		ppNum, coefNum, rcoef, gcoef, bcoef, off);
+}
+
+void pp_output_fmt_cfg(struct sf_fb_data *sf_dev, int ppNum, int yuv420Inter, int yuv422_mode)
+{
+	int preCfg = 0xfffffffe & sf_fb_vppread32(sf_dev, ppNum, PP_CTRL2);
+
+	preCfg = preCfg | yuv420Inter << PP_DES_420_ORDER | yuv422_mode << PP_DES_422_ORDER;
+	sf_fb_vppwrite32(sf_dev, ppNum, PP_CTRL2, preCfg);
+	PP_PRT("PP%d Lock Transfer: %d\n", ppNum, yuv422_mode);
+}
+
+void pp_lockTrans_cfg(struct sf_fb_data *sf_dev, int ppNum, int lockTrans)
+{
+	int preCfg = 0xfffffffe & sf_fb_vppread32(sf_dev, ppNum, PP_CTRL2);
+
+	sf_fb_vppwrite32(sf_dev, ppNum, PP_CTRL2, lockTrans | preCfg);
+	PP_PRT("PP%d Lock Transfer: %d\n", ppNum, lockTrans);
+}
+
+void pp_int_interval_cfg(struct sf_fb_data *sf_dev, int ppNum, int interval)
+{
+	int preCfg = 0xffff00ff & sf_fb_vppread32(sf_dev, ppNum, PP_CTRL2);
+
+	sf_fb_vppwrite32(sf_dev, ppNum, PP_CTRL2, interval << PP_INT_INTERVAL | preCfg);
+	PP_PRT("PP%d Frame Interrupt interval: %d Frames\n", ppNum, interval);
+}
+
+void pp_srcSize_cfg(struct sf_fb_data *sf_dev, int ppNum, int hsize, int vsize)
+{
+	int size = hsize | vsize << PP_SRC_VSIZE;
+
+	sf_fb_vppwrite32(sf_dev, ppNum, PP_SRC_SIZE, size);
+	PP_PRT("PP%d HSize: %d, VSize: %d\n", ppNum, hsize, vsize);
+}
+
+//0-no drop, 1-1/2, 2-1/4, down to 1/32
+void pp_drop_cfg(struct sf_fb_data *sf_dev, int ppNum, int hdrop, int vdrop)
+{
+	int drop = hdrop | vdrop << PP_DROP_VRATION;
+	sf_fb_vppwrite32(sf_dev, ppNum, PP_DROP_CTRL, drop);
+	PP_PRT("PP%d HDrop: %d, VDrop: %d\n", ppNum, hdrop, vdrop);
+}
+
+void pp_desSize_cfg(struct sf_fb_data *sf_dev, int ppNum, int hsize, int vsize)
+{
+	int size = hsize | vsize << PP_DES_VSIZE;
+	sf_fb_vppwrite32(sf_dev, ppNum, PP_DES_SIZE, size);
+	PP_PRT("PP%d HSize: %d, VSize: %d\n", ppNum, hsize, vsize);
+}
+
+void pp_desAddr_cfg(struct sf_fb_data *sf_dev, int ppNum, int yaddr, int uaddr, int vaddr)
+{
+	sf_fb_vppwrite32(sf_dev, ppNum, PP_DES_Y_SA, yaddr);
+	sf_fb_vppwrite32(sf_dev, ppNum, PP_DES_U_SA, uaddr);
+	sf_fb_vppwrite32(sf_dev, ppNum, PP_DES_V_SA, vaddr);
+	PP_PRT("PP%d des-Addr Y: 0x%8x, U: 0x%8x, V: 0x%8x\n", ppNum, yaddr, uaddr, vaddr);
+}
+
+void pp_desOffset_cfg(struct sf_fb_data *sf_dev, int ppNum, int yoff, int uoff, int voff)
+{
+	sf_fb_vppwrite32(sf_dev, ppNum, PP_DES_Y_OFS, yoff);
+	sf_fb_vppwrite32(sf_dev, ppNum, PP_DES_U_OFS, uoff);
+	sf_fb_vppwrite32(sf_dev, ppNum, PP_DES_V_OFS, voff);
+	PP_PRT("PP%d des-Offset Y: 0x%4x, U: 0x%4x, V: 0x%4x\n", ppNum, yoff, uoff, voff);
+}
+
+void pp_intcfg(struct sf_fb_data *sf_dev, int ppNum, int intMask)
+{
+	int intcfg = ~(0x1<<0);
+
+	if(intMask)
+	intcfg = 0xf;
+	sf_fb_vppwrite32(sf_dev, ppNum, PP_INT_MASK, intcfg);
+}
+EXPORT_SYMBOL(pp_intcfg);
+
+//next source frame Y/RGB start address, ?
+void pp_srcAddr_next(struct sf_fb_data *sf_dev, int ppNum, int ysa, int usa, int vsa)
+{
+	sf_fb_vppwrite32(sf_dev, ppNum, PP_SRC_Y_SA_NXT, ysa);
+	sf_fb_vppwrite32(sf_dev, ppNum, PP_SRC_U_SA_NXT, usa);
+	sf_fb_vppwrite32(sf_dev, ppNum, PP_SRC_V_SA_NXT, vsa);
+	PP_PRT("PP%d next Y startAddr: 0x%8x, U startAddr: 0x%8x, V startAddr: 0x%8x\n", ppNum, ysa, usa, vsa);
+}
+EXPORT_SYMBOL(pp_srcAddr_next);
+
+void pp_srcOffset_cfg(struct sf_fb_data *sf_dev, int ppNum, int yoff, int uoff, int voff)
+{
+	sf_fb_vppwrite32(sf_dev, ppNum, PP_SRC_Y_OFS, yoff);
+	sf_fb_vppwrite32(sf_dev, ppNum, PP_SRC_U_OFS, uoff);
+	sf_fb_vppwrite32(sf_dev, ppNum, PP_SRC_V_OFS, voff);
+	PP_PRT("PP%d src-Offset Y: 0x%4x, U: 0x%4x, V: 0x%4x\n", ppNum, yoff, uoff, voff);
+}
+EXPORT_SYMBOL(pp_srcOffset_cfg);
+
+void pp_nxtAddr_load(struct sf_fb_data *sf_dev, int ppNum, int nxtPar, int nxtPos)
+{
+	sf_fb_vppwrite32(sf_dev, ppNum, PP_LOAD_NXT_PAR, nxtPar | nxtPos);
+	PP_PRT("PP%d next addrPointer: %d, %d set Regs\n", ppNum, nxtPar, nxtPos);
+}
+EXPORT_SYMBOL(pp_nxtAddr_load);
+
+void pp_run(struct sf_fb_data *sf_dev, int ppNum, int start)
+{
+	sf_fb_vppwrite32(sf_dev, ppNum, PP_SWITCH, start);
+	//if(start)
+	//  PP_PRT("Now start the PP%d\n\n", ppNum);
+}
+EXPORT_SYMBOL(pp_run);
+
+void pp1_enable_intr(struct sf_fb_data *sf_dev)
+{
+	sf_fb_vppwrite32(sf_dev, 1, PP_INT_MASK, 0x0);
+}
+EXPORT_SYMBOL(pp1_enable_intr);
+
+void pp_enable_intr(struct sf_fb_data *sf_dev, int ppNum)
+{
+	u32 cfg = 0xfffe;
+
+	sf_fb_vppwrite32(sf_dev, ppNum, PP_INT_MASK, cfg);
+}
+EXPORT_SYMBOL(pp_enable_intr);
+
+void pp_disable_intr(struct sf_fb_data *sf_dev, int ppNum)
+{
+	sf_fb_vppwrite32(sf_dev, ppNum, PP_INT_MASK, 0xf);
+	sf_fb_vppwrite32(sf_dev, ppNum, PP_INT_CLR, 0xf);
+}
+EXPORT_SYMBOL(pp_disable_intr);
+
+static void pp_srcfmt_set(struct sf_fb_data *sf_dev, int ppNum, struct pp_video_mode *src)
+{
+	switch(src->format) {
+	case COLOR_YUV422_YVYU:
+		pp_srcfmt_cfg(sf_dev, ppNum, PP_SRC_YUV422, 0x0, COLOR_YUV422_YVYU, 0x0, 0x0);
+		break;
+	case COLOR_YUV422_VYUY:
+		pp_srcfmt_cfg(sf_dev, ppNum, PP_SRC_YUV422, 0x0, COLOR_YUV422_VYUY, 0x0, 0x0);
+		break;
+	case COLOR_YUV422_YUYV:
+		pp_srcfmt_cfg(sf_dev, ppNum, PP_SRC_YUV422, 0x0, COLOR_YUV422_YUYV, 0x0, 0x0);
+		break;
+	case COLOR_YUV422_UYVY:
+		pp_srcfmt_cfg(sf_dev, ppNum, PP_SRC_YUV422, 0x0, COLOR_YUV422_UYVY, 0x0, 0x0);
+		break;
+	case COLOR_YUV420P:
+		pp_srcfmt_cfg(sf_dev, ppNum, PP_SRC_YUV420P, 0x0, 0, 0x0, 0x0);
+		break;
+	case COLOR_YUV420_NV21:
+		pp_srcfmt_cfg(sf_dev, ppNum, PP_SRC_YUV420I, 0x1, 0, COLOR_YUV420_NV21-COLOR_YUV420_NV21, 0x0);
+		break;
+	case COLOR_YUV420_NV12:
+		pp_srcfmt_cfg(sf_dev, ppNum, PP_SRC_YUV420I, 0x1, 0, COLOR_YUV420_NV12-COLOR_YUV420_NV21, 0x0);
+		break;
+	case COLOR_RGB888_ARGB:
+		pp_srcfmt_cfg(sf_dev, ppNum, PP_SRC_GRB888, 0x0, 0x0, 0x0, COLOR_RGB888_ARGB-COLOR_RGB888_ARGB);//0x0);
+		break;
+	case COLOR_RGB888_ABGR:
+		pp_srcfmt_cfg(sf_dev, ppNum, PP_SRC_GRB888, 0x0, 0x0, 0x0, COLOR_RGB888_ABGR-COLOR_RGB888_ARGB);//0x1);
+		break;
+	case COLOR_RGB888_RGBA:
+		pp_srcfmt_cfg(sf_dev, ppNum, PP_SRC_GRB888, 0x0, 0x0, 0x0, COLOR_RGB888_RGBA-COLOR_RGB888_ARGB);//0x2);
+		break;
+	case COLOR_RGB888_BGRA:
+		pp_srcfmt_cfg(sf_dev, ppNum, PP_SRC_GRB888, 0x0, 0x0, 0x0, COLOR_RGB888_BGRA-COLOR_RGB888_ARGB);//0x3);
+		break;
+	case COLOR_RGB565:
+		pp_srcfmt_cfg(sf_dev, ppNum, PP_SRC_RGB565, 0x0, 0x0, 0x0, 0x0);
+		break;
+	}
+}
+
+static void pp_dstfmt_set(struct sf_fb_data *sf_dev, int ppNum, struct pp_video_mode *dst)
+{
+	unsigned int outsel = 1;
+
+	if(dst->addr)
+	{
+		outsel = 0;
+	}
+
+	switch(dst->format) {
+	case COLOR_YUV422_YVYU:
+		pp_output_cfg(sf_dev, ppNum, outsel, 0x0, PP_DST_YUV422, 0x0);
+		pp_output_fmt_cfg(sf_dev, ppNum, 0, COLOR_YUV422_UYVY - COLOR_YUV422_YVYU);
+		break;
+	case COLOR_YUV422_VYUY:
+		pp_output_cfg(sf_dev, ppNum, outsel, 0x0, PP_DST_YUV422, 0x0);
+		pp_output_fmt_cfg(sf_dev, ppNum, 0, COLOR_YUV422_UYVY - COLOR_YUV422_VYUY);
+		break;
+	case COLOR_YUV422_YUYV:
+		pp_output_cfg(sf_dev, ppNum, outsel, 0x0, PP_DST_YUV422, 0x0);
+		pp_output_fmt_cfg(sf_dev, ppNum, 0, COLOR_YUV422_UYVY - COLOR_YUV422_YUYV);
+		break;
+	case COLOR_YUV422_UYVY:
+		pp_output_cfg(sf_dev, ppNum, outsel, 0x0, PP_DST_YUV422, 0x0);
+		pp_output_fmt_cfg(sf_dev, ppNum, 0, COLOR_YUV422_UYVY - COLOR_YUV422_YVYU);
+		break;
+	case COLOR_YUV420P:
+		pp_output_cfg(sf_dev, ppNum, outsel, 0x0, PP_DST_YUV420P, 0x0);
+		pp_output_fmt_cfg(sf_dev, ppNum, 0, 0);
+		break;
+	case COLOR_YUV420_NV21:
+		pp_output_cfg(sf_dev, ppNum, outsel, 0x0, PP_DST_YUV420I, 0x0);
+		pp_output_fmt_cfg(sf_dev, ppNum, COLOR_YUV420_NV21 - COLOR_YUV420_NV21, 0);
+		break;
+	case COLOR_YUV420_NV12:
+		pp_output_cfg(sf_dev, ppNum, outsel, 0x0, PP_DST_YUV420I, 0x0);///0x2, 0x0);
+		//pp_output_fmt_cfg(ppNum, COLOR_YUV420_NV12 - COLOR_YUV420_NV21, 0);
+		break;
+	case COLOR_RGB888_ARGB:
+		pp_output_cfg(sf_dev, ppNum, outsel, 0x0, PP_DST_ARGB888, 0x0);
+		//pp_output_fmt_cfg(ppNum, 0, 0);
+		break;
+	case COLOR_RGB888_ABGR:
+		pp_output_cfg(sf_dev, ppNum, outsel, 0x0, PP_DST_ABGR888, 0x0);
+		pp_output_fmt_cfg(sf_dev, ppNum, 0, 0);
+		break;
+	case COLOR_RGB888_RGBA:
+		pp_output_cfg(sf_dev, ppNum, outsel, 0x0, PP_DST_RGBA888, 0x0);
+		pp_output_fmt_cfg(sf_dev, ppNum, 0, 0);
+		break;
+	case COLOR_RGB888_BGRA:
+		pp_output_cfg(sf_dev, ppNum, outsel, 0x0, PP_DST_BGRA888, 0x0);
+		pp_output_fmt_cfg(sf_dev, ppNum, 0, 0);
+		break;
+	case COLOR_RGB565:
+		pp_output_cfg(sf_dev, ppNum, outsel, 0x0, PP_DST_RGB565, 0x0);
+		pp_output_fmt_cfg(sf_dev, ppNum, 0, 0);
+		break;
+	}
+}
+
+
+void pp_format_set(struct sf_fb_data *sf_dev, int ppNum, struct pp_video_mode *src, struct pp_video_mode *dst)
+{
+
+	/* 1:bypass, 0:not bypass */
+	unsigned int scale_byp = 1;
+
+	pp_srcfmt_set(sf_dev, ppNum, src);
+	pp_dstfmt_set(sf_dev, ppNum, dst);
+	if((src->height != dst->height) || (src->width != dst->width)) {
+		scale_byp = 0;
+	}
+
+	if((src->format >= COLOR_RGB888_ARGB) && (dst->format <= COLOR_YUV420_NV12)) {
+		/* rgb -> yuv-420 */
+		pp_r2yscal_bypass(sf_dev, ppNum, NOT_BYPASS, scale_byp, BYPASS);
+		pp_r2y_coeff(sf_dev, ppNum, 1, R2Y_COEF_R1, R2Y_COEF_G1, R2Y_COEF_B1, R2Y_OFFSET1);
+		pp_r2y_coeff(sf_dev, ppNum, 2, R2Y_COEF_R2, R2Y_COEF_G2, R2Y_COEF_B2, R2Y_OFFSET2);
+		pp_r2y_coeff(sf_dev, ppNum, 3, R2Y_COEF_R3, R2Y_COEF_G3, R2Y_COEF_B3, R2Y_OFFSET3);
+	} else if ((src->format <= COLOR_YUV420_NV12) && (dst->format >= COLOR_RGB888_ARGB)) {
+	/* yuv-420 -> rgb */
+		pp_r2yscal_bypass(sf_dev, ppNum, BYPASS, scale_byp, NOT_BYPASS);
+	} else if ((src->format <= COLOR_YUV422_YVYU) && (dst->format <= COLOR_YUV420_NV12)) {
+		/* yuv422 -> yuv420 */
+		pp_r2yscal_bypass(sf_dev, ppNum, BYPASS, scale_byp, BYPASS);
+	} else {
+		/* rgb565->argb888 */
+		pp_r2yscal_bypass(sf_dev, ppNum, BYPASS, scale_byp, BYPASS);
+	} //else if((src->format >= COLOR_RGB888_ARGB) && (dst->format >= COLOR_RGB888_ARGB))
+	{
+		/* rgb -> rgb */
+		// pp_r2yscal_bypass(ppNum, BYPASS, scale_byp, BYPASS);
+	}
+	pp_argb_alpha(sf_dev, ppNum, 0xff);
+
+	if(dst->addr) {
+		pp_lockTrans_cfg(sf_dev, ppNum, SYS_BUS_OUTPUT);
+	} else {
+		pp_lockTrans_cfg(sf_dev, ppNum, FIFO_OUTPUT);
+	}
+
+	pp_int_interval_cfg(sf_dev, ppNum, 0x1);
+
+}
+
+void pp_size_set(struct sf_fb_data *sf_dev, int ppNum, struct pp_video_mode *src, struct pp_video_mode *dst)
+{
+	uint32_t srcAddr, dstaddr;
+	unsigned int size, y_rgb_ofst, uofst;
+	unsigned int v_uvofst = 0, next_y_rgb_addr = 0, next_u_addr = 0, next_v_addr = 0;
+	unsigned int i = 0;
+
+	pp_srcSize_cfg(sf_dev, ppNum, src->width - 1, src->height - 1);
+	pp_drop_cfg(sf_dev, ppNum, 0x0, 0x0);///0:no drop
+	pp_desSize_cfg(sf_dev, ppNum, dst->width - 1, dst->height - 1);
+
+	srcAddr = src->addr + (i<<30);///PP_SRC_BASE_ADDR + (i<<30);
+	size = src->width * src->height;
+
+	if(src->format >= COLOR_RGB888_ARGB) { next_y_rgb_addr = srcAddr;
+		next_u_addr = 0;
+		next_v_addr = 0;
+
+		y_rgb_ofst = 0;
+		uofst = 0;
+		v_uvofst = 0;
+
+		//pp_srcAddr_next(ppNum, srcAddr, 0, 0);
+		//pp_srcOffset_cfg(ppNum, 0x0, 0x0, 0x0);
+	} else {
+		//if((src->format == COLOR_YUV420_NV21) || (src->format == COLOR_YUV420_NV12)){
+		if(src->format == COLOR_YUV420_NV21) {    //ok
+			next_y_rgb_addr = srcAddr;
+			next_u_addr = srcAddr+size;
+			next_v_addr = srcAddr+size + 1;
+			y_rgb_ofst = 0;
+			uofst = 0;
+			v_uvofst = size;
+		} else if (src->format == COLOR_YUV420_NV12) {
+			next_y_rgb_addr = srcAddr;
+			next_u_addr = srcAddr+size;
+			next_v_addr = srcAddr+size+1;
+			y_rgb_ofst = 0;
+			uofst = 0;
+			v_uvofst = size;
+		} else if (src->format == COLOR_YUV420P) {
+			next_y_rgb_addr = srcAddr;
+			next_u_addr = srcAddr+size;
+			next_v_addr = srcAddr+size*5/4;
+			y_rgb_ofst = 0;
+			uofst = 0;
+			v_uvofst = 0;
+		} else if (src->format == COLOR_YUV422_YVYU) {   //ok
+			next_y_rgb_addr = srcAddr;
+			next_u_addr = srcAddr+1;
+			next_v_addr = srcAddr+3;
+			y_rgb_ofst = 0;
+			uofst = 0;
+			v_uvofst = 0;
+		} else if (src->format == COLOR_YUV422_VYUY) {   //ok
+			next_y_rgb_addr = srcAddr+1;
+			next_u_addr = srcAddr+2;
+			next_v_addr = srcAddr;
+			y_rgb_ofst = 0;
+			uofst = 0;
+			v_uvofst = 0;
+        } else if(src->format == COLOR_YUV422_YUYV) {   //ok
+			next_y_rgb_addr = srcAddr;
+			next_u_addr = srcAddr+1;
+			next_v_addr = srcAddr+2;
+			y_rgb_ofst = 0;
+			uofst = 0;
+			v_uvofst = 0;
+		} else if(src->format == COLOR_YUV422_UYVY) {  //ok
+			next_y_rgb_addr = srcAddr+1;
+			next_u_addr = srcAddr;
+			next_v_addr = srcAddr+2;
+			y_rgb_ofst = 0;
+			uofst = 0;
+			v_uvofst = 0;
+		}
+	}
+	pp_srcAddr_next(sf_dev, ppNum, next_y_rgb_addr, next_u_addr, next_v_addr);
+	pp_srcOffset_cfg(sf_dev, ppNum, y_rgb_ofst, uofst, v_uvofst);
+	/* source addr not change */
+	pp_nxtAddr_load(sf_dev, ppNum, 0x1, (i & 0x1));
+
+	if(dst->addr) {
+		dstaddr = dst->addr;
+		size = dst->height*dst->width;
+		if(dst->format >= COLOR_RGB888_ARGB) {
+			next_y_rgb_addr = dstaddr;
+			next_u_addr = 0;
+			next_v_addr = 0;
+			y_rgb_ofst = 0;
+			uofst = 0;
+			v_uvofst = 0;
+		} else {
+			if(dst->format == COLOR_YUV420_NV21) {
+				/* yyyyvuvuvu */
+				next_y_rgb_addr = dstaddr;
+				next_u_addr = dstaddr+size;
+				next_v_addr = 0;//dstaddr+size;
+				y_rgb_ofst = 0;
+				uofst = 0;
+				v_uvofst = 0;
+			} else if (dst->format == COLOR_YUV420_NV12){
+				/* yyyyuvuvuv */
+				next_y_rgb_addr = dstaddr;
+				next_u_addr = dstaddr+size;
+				next_v_addr = dstaddr+size+1;
+				y_rgb_ofst = 0;
+				uofst = size;
+				v_uvofst = 0;
+			} else if(dst->format == COLOR_YUV420P) {
+				next_y_rgb_addr = dstaddr;
+				next_u_addr = dstaddr+size;
+				next_v_addr = dstaddr+size*5/4;
+				y_rgb_ofst = 0;
+				uofst = 0;
+				v_uvofst = 0;
+			} else if (dst->format == COLOR_YUV422_YVYU) {
+				next_y_rgb_addr = dstaddr;
+				next_u_addr = dstaddr+1;
+				next_v_addr = dstaddr+3;
+				y_rgb_ofst = 0;
+				uofst = 0;
+				v_uvofst = 0;
+			} else if(dst->format == COLOR_YUV422_VYUY) {
+				next_y_rgb_addr = dstaddr+1;
+				next_u_addr = dstaddr+2;
+				next_v_addr = dstaddr;
+				y_rgb_ofst = 0;
+				uofst = 0;
+				v_uvofst = 0;
+			} else if(dst->format == COLOR_YUV422_YUYV) {
+				next_y_rgb_addr = dstaddr;
+				next_u_addr = dstaddr+1;
+				next_v_addr = dstaddr+2;
+				y_rgb_ofst = 0;
+				uofst = 0;
+				v_uvofst = 0;
+			} else if(dst->format == COLOR_YUV422_UYVY) {
+				next_y_rgb_addr = dstaddr+1;
+				next_u_addr = dstaddr;
+				next_v_addr = dstaddr+2;
+				y_rgb_ofst = 0;
+				uofst = 0;
+				v_uvofst = 0;
+			}
+		}
+		pp_desAddr_cfg(sf_dev, ppNum, next_y_rgb_addr, next_u_addr, next_v_addr);
+		pp_desOffset_cfg(sf_dev, ppNum, y_rgb_ofst, uofst, v_uvofst);
+	}
+
+}
+
+void pp_config(struct sf_fb_data *sf_dev, int ppNum, struct pp_video_mode *src, struct pp_video_mode *dst)
+{
+	//pp_disable_intr(sf_dev, ppNum);
+	pp_format_set(sf_dev, ppNum, src, dst);
+	pp_size_set(sf_dev, ppNum, src, dst);
+}
+EXPORT_SYMBOL(pp_config);
+
+irqreturn_t vpp1_isr_handler(int this_irq, void *dev_id)
+{
+	struct sf_fb_data *sf_dev = (struct sf_fb_data *)dev_id;
+	static int count = 0;
+	sf_fb_vppwrite32(sf_dev, 1, PP_INT_CLR, 0xf);
+
+	count ++;
+	if(0 == count % 60)
+		PP_PRT("=");
+		//printk("=");
+
+	return IRQ_HANDLED;
+}
+EXPORT_SYMBOL(vpp1_isr_handler);
+
+MODULE_AUTHOR("StarFive Technology Co., Ltd.");
+MODULE_DESCRIPTION("loadable VPP driver for StarFive");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/fbdev/starfive/starfive_vpp.h b/drivers/video/fbdev/starfive/starfive_vpp.h
new file mode 100755
index 000000000000..bb89277db54f
--- /dev/null
+++ b/drivers/video/fbdev/starfive/starfive_vpp.h
@@ -0,0 +1,191 @@
+/*
+ * StarFive Vout driver
+ *
+ * Copyright 2020 StarFive Inc.
+ *
+ * Licensed under the GPL-2.
+ */
+
+#ifndef __SF_FB_VPP_H__
+#define __SF_FB_VPP_H__
+
+#define PP_ID_0	0
+#define PP_ID_1	1
+#define PP_ID_2	2
+
+#define PP_NUM	3
+
+#define PP_STOP	0
+#define PP_RUN	1
+
+#define PP_INTR_ENABLE	1
+#define PP_INTR_DISABLE	0
+//PP coefficients
+///*
+#define R2Y_COEF_R1       77
+#define R2Y_COEF_G1       150
+#define R2Y_COEF_B1       29
+#define R2Y_OFFSET1       0
+
+#define R2Y_COEF_R2       (0x400|43)
+#define R2Y_COEF_G2       (0x400|85)
+#define R2Y_COEF_B2       128
+#define R2Y_OFFSET2       128
+
+#define R2Y_COEF_R3       128
+#define R2Y_COEF_G3       (0x400|107)
+#define R2Y_COEF_B3       (0x400|21)
+#define R2Y_OFFSET3       128
+//*/
+enum PP_LCD_PATH
+{
+	SYS_BUS_OUTPUT = 0,
+	FIFO_OUTPUT = 1,
+};
+
+enum PP_COLOR_CONVERT_SCALE
+{
+	NOT_BYPASS = 0,
+	BYPASS,
+};
+
+enum PP_SRC_FORMAT
+{
+	PP_SRC_YUV420P = 0,
+	PP_SRC_YUV422,
+	PP_SRC_YUV420I,
+	PP_RESERVED,
+	PP_SRC_GRB888,
+	PP_SRC_RGB565,
+};
+
+enum PP_DST_FORMAT
+{
+	PP_DST_YUV420P = 0,
+	PP_DST_YUV422,
+	PP_DST_YUV420I,
+	PP_DST_RGBA888,
+	PP_DST_ARGB888,
+	PP_DST_RGB565,
+	PP_DST_ABGR888,
+	PP_DST_BGRA888,
+};
+
+enum COLOR_FORMAT{
+	COLOR_YUV422_UYVY = 0,  //00={Y1,V0,Y0,U0}
+	COLOR_YUV422_VYUY = 1,  //01={Y1,U0,Y0,V0}
+	COLOR_YUV422_YUYV = 2,  //10={V0,Y1,U0,Y0}
+	COLOR_YUV422_YVYU = 3,  //11={U0,Y1,V0,Y0}
+	COLOR_YUV420P,
+	COLOR_YUV420_NV21,
+	COLOR_YUV420_NV12,
+	COLOR_RGB888_ARGB,
+	COLOR_RGB888_ABGR,
+	COLOR_RGB888_RGBA,
+	COLOR_RGB888_BGRA,
+	COLOR_RGB565,
+};
+
+struct pp_video_mode {
+	enum COLOR_FORMAT format;
+	unsigned int height;
+	unsigned int width;
+	unsigned int addr;
+};
+
+struct pp_mode {
+	char pp_id;
+	bool bus_out;	/*out to ddr*/
+	bool fifo_out;	/*out to lcdc*/
+	bool inited;
+	struct pp_video_mode src;
+	struct pp_video_mode dst;
+};
+
+//vpp registers
+#define PP_SWITCH		0x0000
+#define PP_CTRL1		0x0004
+#define PP_CTRL2		0x0008
+#define PP_SRC_SIZE		0x000C
+#define PP_DROP_CTRL		0x0010
+#define PP_DES_SIZE		0x0014
+#define PP_Scale_Hratio		0x0018
+#define PP_Scale_Vratio		0x001C
+#define PP_Scale_limit		0x0020
+#define PP_SRC_Y_SA_NXT		0x0024
+#define PP_SRC_U_SA_NXT		0x0028
+#define PP_SRC_V_SA_NXT		0x002c
+#define PP_LOAD_NXT_PAR		0x0030
+#define PP_SRC_Y_SA0		0x0034
+#define PP_SRC_U_SA0		0x0038
+#define PP_SRC_V_SA0		0x003c
+#define PP_SRC_Y_OFS		0x0040
+#define PP_SRC_U_OFS		0x0044
+#define PP_SRC_V_OFS		0x0048
+#define PP_SRC_Y_SA1		0x004C
+#define PP_SRC_U_SA1		0x0050
+#define PP_SRC_V_SA1		0x0054
+#define PP_DES_Y_SA		0x0058
+#define PP_DES_U_SA		0x005C
+#define PP_DES_V_SA		0x0060
+#define PP_DES_Y_OFS		0x0064
+#define PP_DES_U_OFS		0x0068
+#define PP_DES_V_OFS		0x006C
+#define PP_INT_MASK		0x0074
+#define PP_INT_CLR		0x0078
+#define PP_R2Y_COEF1		0x007C
+#define PP_R2Y_COEF2		0x0080
+
+/* Definition controller bit for LCDC registers */
+//for PP_SWITCH
+#define PP_TRIG		0
+//for PP_CTRL1
+#define PP_LCDPATH_EN		0
+#define PP_INTERLACE		1
+#define PP_POINTER_MODE		2
+#define PP_SRC_FORMAT_N		4
+#define PP_420_ITLC		7
+#define PP_DES_FORMAT		8
+#define PP_R2Y_BPS		12
+#define PP_MSCALE_BPS		13
+#define PP_Y2R_BPS		14
+#define PP_ARGB_ALPHA		16
+#define PP_UV_IN_ADD_128		24
+#define PP_UV_OUT_ADD_128		25
+#define PP_SRC_422_YUV_POS		26
+#define PP_SRC_420_YUV_POS		28
+#define PP_SRC_ARGB_ORDER		29
+//for PP_CTRL2
+#define PP_LOCK_EN		0
+#define PP_INT_INTERVAL		8
+#define PP_DES_422_ORDER		16
+#define PP_DES_420_ORDER		18
+//for PP_SRC_SIZE
+#define PP_SRC_HSIZE		0
+#define PP_SRC_VSIZE		16
+//for PP_DROP_CTRL
+#define PP_DROP_HRATION		0
+#define PP_DROP_VRATION		4
+//for PP_DES_SIZE
+#define PP_DES_HSIZE		0
+#define PP_DES_VSIZE		16
+//for PP_R2Y_COEF1
+#define PP_COEF_R1		0
+#define PP_COEF_G1		16
+//for PP_R2Y_COEF2
+#define PP_COEF_B1		0
+#define PP_OFFSET_1		16
+
+extern void mapconv_pp0_sel(struct sf_fb_data *sf_dev, int sel);
+extern void pp_srcAddr_next(struct sf_fb_data *sf_dev, int ppNum, int ysa, int usa, int vsa);
+extern void pp_srcOffset_cfg(struct sf_fb_data *sf_dev, int ppNum, int yoff, int uoff, int voff);
+extern void pp_nxtAddr_load(struct sf_fb_data *sf_dev, int ppNum, int nxtPar, int nxtPos);
+extern void pp_intcfg(struct sf_fb_data *sf_dev, int ppNum, int intMask);
+extern irqreturn_t vpp1_isr_handler(int this_irq, void *dev_id);
+extern void pp1_enable_intr(struct sf_fb_data *sf_dev);
+extern void pp_enable_intr(struct sf_fb_data *sf_dev, int ppNum);
+extern void pp_disable_intr(struct sf_fb_data *sf_dev, int ppNum);
+extern void pp_config(struct sf_fb_data *sf_dev, int ppNum, struct pp_video_mode *src, struct pp_video_mode *dst);
+extern void pp_run(struct sf_fb_data *sf_dev, int ppNum, int start);
+
+#endif
diff --git a/drivers/video/fbdev/starfive/sys_comm_regs.h b/drivers/video/fbdev/starfive/sys_comm_regs.h
new file mode 100755
index 000000000000..4ad4572122b5
--- /dev/null
+++ b/drivers/video/fbdev/starfive/sys_comm_regs.h
@@ -0,0 +1,1070 @@
+/*
+ * StarFive sys regs
+ *
+ * Copyright 2020 StarFive Inc.
+ *
+ * Licensed under the GPL-2.
+ */
+
+#ifndef __SYS_COMM_REGS_H__
+#define __SYS_COMM_REGS_H__
+
+#define MA_OUTW( io, val )  ({void __iomem * vir; vir = ioremap(io, 4); iowrite32((u32)val, vir);})
+#define MA_INW( io )  ({void __iomem * vir; vir = ioremap(io, 4); ioread32(vir);})
+
+#define WDT_BASE_ADDR					0x12480000
+
+#define DSITX_BASE_ADDR                 0x12100000
+#define CSI2TX_BASE_ADDR                0x12220000
+#define ISP_MIPI_CONTROLLER0_BASE_ADDR  0x19800000
+#define ISP_MIPI_CONTROLLER1_BASE_ADDR  0x19830000
+
+#define VOUT_SYS_CLKGEN_BASE_ADDR  		0x12240000
+#define VOUT_SYS_RSTGEN_BASE_ADDR  		0x12250000
+#define VOUT_SYS_SYSCON_BASE_ADDR  		0x12260000
+
+#define ISP_CLKGEN_BASE_ADDR        	0x19810000
+#define ISP_RSTGEN_BASE_ADDR       		0x19820000
+#define ISP_SYSCONTROLLER_BASE_ADDR 	0x19840000
+
+#define ISP0_AXI_SLV_BASE_ADDR    	    0x19870000
+#define ISP1_AXI_SLV_BASE_ADDR    	    0x198A0000
+
+#define CLKGEN_BASE_ADDR                0x11800000
+#define RSTGEN_BASE_ADDR                0x11840000
+#define ISP_CLKGEN_BASE_ADDR        	0x19810000
+#define ISP_RSTGEN_BASE_ADDR        	0x19820000
+#define ISP_SYSCONTROLLER_BASE_ADDR 	0x19840000
+
+#define GPIO_BASE_ADDR		            0x11910000
+#define EZGPIO_FULLMUX_BASE_ADDR		0x11910000
+#define SYSCON_IOPAD_CTRL_BASE_ADDR     0x11858000
+
+#define vout_sys_rstgen_Software_RESET_assert0_REG_ADDR  VOUT_SYS_RSTGEN_BASE_ADDR + 0x0
+#define vout_sys_rstgen_Software_RESET_status0_REG_ADDR  VOUT_SYS_RSTGEN_BASE_ADDR + 0x4
+#define rstgen_Software_RESET_assert1_REG_ADDR           RSTGEN_BASE_ADDR + 0x4
+#define rstgen_Software_RESET_status1_REG_ADDR           RSTGEN_BASE_ADDR + 0x14
+
+//#define VOUT_SYS_CLKGEN_BASE_ADDR 0x0
+#define clk_vout_apb_ctrl_REG_ADDR  VOUT_SYS_CLKGEN_BASE_ADDR + 0x0
+#define clk_mapconv_apb_ctrl_REG_ADDR  VOUT_SYS_CLKGEN_BASE_ADDR + 0x4
+#define clk_mapconv_axi_ctrl_REG_ADDR  VOUT_SYS_CLKGEN_BASE_ADDR + 0x8
+#define clk_disp0_axi_ctrl_REG_ADDR  VOUT_SYS_CLKGEN_BASE_ADDR + 0xC
+#define clk_disp1_axi_ctrl_REG_ADDR  VOUT_SYS_CLKGEN_BASE_ADDR + 0x10
+#define clk_lcdc_oclk_ctrl_REG_ADDR  VOUT_SYS_CLKGEN_BASE_ADDR + 0x14
+#define clk_lcdc_axi_ctrl_REG_ADDR  VOUT_SYS_CLKGEN_BASE_ADDR + 0x18
+#define clk_vpp0_axi_ctrl_REG_ADDR  VOUT_SYS_CLKGEN_BASE_ADDR + 0x1C
+#define clk_vpp1_axi_ctrl_REG_ADDR  VOUT_SYS_CLKGEN_BASE_ADDR + 0x20
+#define clk_vpp2_axi_ctrl_REG_ADDR  VOUT_SYS_CLKGEN_BASE_ADDR + 0x24
+#define clk_pixrawout_apb_ctrl_REG_ADDR  VOUT_SYS_CLKGEN_BASE_ADDR + 0x28
+#define clk_pixrawout_axi_ctrl_REG_ADDR  VOUT_SYS_CLKGEN_BASE_ADDR + 0x2C
+#define clk_csi2tx_strm0_pixclk_ctrl_REG_ADDR  VOUT_SYS_CLKGEN_BASE_ADDR + 0x30
+#define clk_csi2tx_strm0_apb_ctrl_REG_ADDR  VOUT_SYS_CLKGEN_BASE_ADDR + 0x34
+#define clk_dsi_sys_clk_ctrl_REG_ADDR  VOUT_SYS_CLKGEN_BASE_ADDR + 0x38
+#define clk_dsi_apb_ctrl_REG_ADDR  VOUT_SYS_CLKGEN_BASE_ADDR + 0x3C
+#define clk_ppi_tx_esc_clk_ctrl_REG_ADDR  VOUT_SYS_CLKGEN_BASE_ADDR + 0x40
+
+//#define CLKGEN_BASE_ADDR 0x0
+#define clk_cpundbus_root_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x0
+#define clk_dla_root_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x4
+#define clk_dsp_root_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x8
+#define clk_gmacusb_root_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0xC
+#define clk_perh0_root_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x10
+#define clk_perh1_root_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x14
+#define clk_vin_root_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x18
+#define clk_vout_root_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x1C
+#define clk_audio_root_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x20
+#define clk_cdechifi4_root_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x24
+#define clk_cdec_root_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x28
+#define clk_voutbus_root_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x2C
+#define clk_cpunbus_root_div_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x30
+#define clk_dsp_root_div_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x34
+#define clk_perh0_src_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x38
+#define clk_perh1_src_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x3C
+#define clk_pll0_testout_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x40
+#define clk_pll1_testout_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x44
+#define clk_pll2_testout_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x48
+#define clk_pll2_refclk_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x4C
+#define clk_cpu_core_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x50
+#define clk_cpu_axi_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x54
+#define clk_ahb_bus_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x58
+#define clk_apb1_bus_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x5C
+#define clk_apb2_bus_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x60
+#define clk_dom3ahb_bus_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x64
+#define clk_dom7ahb_bus_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x68
+#define clk_u74_core0_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x6C
+#define clk_u74_core1_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x70
+#define clk_u74_axi_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x74
+#define clk_u74rtc_toggle_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x78
+#define clk_sgdma2p_axi_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x7C
+#define clk_dma2pnoc_axi_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x80
+#define clk_sgdma2p_ahb_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x84
+#define clk_dla_bus_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x88
+#define clk_dla_axi_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x8C
+#define clk_dlanoc_axi_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x90
+#define clk_dla_apb_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x94
+#define clk_vp6_core_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x98
+#define clk_vp6bus_src_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x9C
+#define clk_vp6_axi_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0xA0
+#define clk_vcdecbus_src_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0xA4
+#define clk_vdec_bus_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0xA8
+#define clk_vdec_axi_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0xAC
+#define clk_vdecbrg_mainclk_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0xB0
+#define clk_vdec_bclk_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0xB4
+#define clk_vdec_cclk_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0xB8
+#define clk_vdec_apb_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0xBC
+#define clk_jpeg_axi_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0xC0
+#define clk_jpeg_cclk_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0xC4
+#define clk_jpeg_apb_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0xC8
+#define clk_gc300_2x_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0xCC
+#define clk_gc300_ahb_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0xD0
+#define clk_jpcgc300_axibus_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0xD4
+#define clk_gc300_axi_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0xD8
+#define clk_jpcgc300_mainclk_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0xDC
+#define clk_venc_bus_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0xE0
+#define clk_venc_axi_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0xE4
+#define clk_vencbrg_mainclk_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0xE8
+#define clk_venc_bclk_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0xEC
+#define clk_venc_cclk_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0xF0
+#define clk_venc_apb_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0xF4
+#define clk_ddrpll_div2_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0xF8
+#define clk_ddrpll_div4_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0xFC
+#define clk_ddrpll_div8_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x100
+#define clk_ddrosc_div2_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x104
+#define clk_ddrc0_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x108
+#define clk_ddrc1_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x10C
+#define clk_ddrphy_apb_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x110
+#define clk_noc_rob_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x114
+#define clk_noc_cog_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x118
+#define clk_nne_ahb_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x11C
+#define clk_nnebus_src1_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x120
+#define clk_nne_bus_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x124
+#define clk_nne_axi_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x128
+#define clk_nnenoc_axi_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x12C
+#define clk_dlaslv_axi_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x130
+#define clk_dspx2c_axi_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x134
+#define clk_hifi4_src_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x138
+#define clk_hifi4_corefree_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x13C
+#define clk_hifi4_core_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x140
+#define clk_hifi4_bus_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x144
+#define clk_hifi4_axi_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x148
+#define clk_hifi4noc_axi_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x14C
+#define clk_sgdma1p_bus_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x150
+#define clk_sgdma1p_axi_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x154
+#define clk_dma1p_axi_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x158
+#define clk_x2c_axi_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x15C
+#define clk_usb_bus_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x160
+#define clk_usb_axi_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x164
+#define clk_usbnoc_axi_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x168
+#define clk_usbphy_rootdiv_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x16C
+#define clk_usbphy_125m_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x170
+#define clk_usbphy_plldiv25m_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x174
+#define clk_usbphy_25m_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x178
+#define clk_audio_div_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x17C
+#define clk_audio_src_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x180
+#define clk_audio_12288_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x184
+#define clk_vin_src_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x188
+#define clk_isp0_bus_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x18C
+#define clk_isp0_axi_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x190
+#define clk_isp0noc_axi_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x194
+#define clk_ispslv_axi_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x198
+#define clk_isp1_bus_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x19C
+#define clk_isp1_axi_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x1A0
+#define clk_isp1noc_axi_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x1A4
+#define clk_vin_bus_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x1A8
+#define clk_vin_axi_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x1AC
+#define clk_vinnoc_axi_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x1B0
+#define clk_vout_src_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x1B4
+#define clk_dispbus_src_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x1B8
+#define clk_disp_bus_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x1BC
+#define clk_disp_axi_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x1C0
+#define clk_dispnoc_axi_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x1C4
+#define clk_sdio0_ahb_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x1C8
+#define clk_sdio0_cclkint_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x1CC
+#define clk_sdio0_cclkint_inv_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x1D0
+#define clk_sdio1_ahb_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x1D4
+#define clk_sdio1_cclkint_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x1D8
+#define clk_sdio1_cclkint_inv_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x1DC
+#define clk_gmac_ahb_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x1E0
+#define clk_gmac_root_div_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x1E4
+#define clk_gmac_ptp_refclk_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x1E8
+#define clk_gmac_gtxclk_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x1EC
+#define clk_gmac_rmii_txclk_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x1F0
+#define clk_gmac_rmii_rxclk_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x1F4
+#define clk_gmac_tx_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x1F8
+#define clk_gmac_tx_inv_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x1FC
+#define clk_gmac_rx_pre_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x200
+#define clk_gmac_rx_inv_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x204
+#define clk_gmac_rmii_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x208
+#define clk_gmac_tophyref_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x20C
+#define clk_spi2ahb_ahb_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x210
+#define clk_spi2ahb_core_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x214
+#define clk_ezmaster_ahb_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x218
+#define clk_e24_ahb_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x21C
+#define clk_e24rtc_toggle_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x220
+#define clk_qspi_ahb_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x224
+#define clk_qspi_apb_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x228
+#define clk_qspi_refclk_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x22C
+#define clk_sec_ahb_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x230
+#define clk_aes_clk_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x234
+#define clk_sha_clk_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x238
+#define clk_pka_clk_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x23C
+#define clk_trng_apb_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x240
+#define clk_otp_apb_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x244
+#define clk_uart0_apb_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x248
+#define clk_uart0_core_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x24C
+#define clk_uart1_apb_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x250
+#define clk_uart1_core_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x254
+#define clk_spi0_apb_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x258
+#define clk_spi0_core_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x25C
+#define clk_spi1_apb_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x260
+#define clk_spi1_core_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x264
+#define clk_i2c0_apb_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x268
+#define clk_i2c0_core_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x26C
+#define clk_i2c1_apb_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x270
+#define clk_i2c1_core_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x274
+#define clk_gpio_apb_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x278
+#define clk_uart2_apb_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x27C
+#define clk_uart2_core_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x280
+#define clk_uart3_apb_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x284
+#define clk_uart3_core_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x288
+#define clk_spi2_apb_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x28C
+#define clk_spi2_core_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x290
+#define clk_spi3_apb_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x294
+#define clk_spi3_core_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x298
+#define clk_i2c2_apb_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x29C
+#define clk_i2c2_core_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x2A0
+#define clk_i2c3_apb_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x2A4
+#define clk_i2c3_core_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x2A8
+#define clk_wdtimer_apb_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x2AC
+#define clk_wdt_coreclk_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x2B0
+#define clk_timer0_coreclk_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x2B4
+#define clk_timer1_coreclk_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x2B8
+#define clk_timer2_coreclk_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x2BC
+#define clk_timer3_coreclk_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x2C0
+#define clk_timer4_coreclk_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x2C4
+#define clk_timer5_coreclk_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x2C8
+#define clk_timer6_coreclk_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x2CC
+#define clk_vp6intc_apb_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x2D0
+#define clk_pwm_apb_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x2D4
+#define clk_msi_apb_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x2D8
+#define clk_temp_apb_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x2DC
+#define clk_temp_sense_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x2E0
+#define clk_syserr_apb_ctrl_REG_ADDR  CLKGEN_BASE_ADDR + 0x2E4
+
+
+#define _ENABLE_CLOCK_clk_disp0_axi_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(clk_disp0_axi_ctrl_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<31); \
+	_ezchip_macro_read_value_ |= (0x1&0x1)<<31; \
+	MA_OUTW(clk_disp0_axi_ctrl_REG_ADDR,_ezchip_macro_read_value_); \
+}
+
+#define _ENABLE_CLOCK_clk_disp1_axi_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(clk_disp1_axi_ctrl_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<31); \
+	_ezchip_macro_read_value_ |= (0x1&0x1)<<31; \
+	MA_OUTW(clk_disp1_axi_ctrl_REG_ADDR,_ezchip_macro_read_value_); \
+}
+
+#define _ENABLE_CLOCK_clk_lcdc_oclk_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(clk_lcdc_oclk_ctrl_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<31); \
+	_ezchip_macro_read_value_ |= (0x1&0x1)<<31; \
+	MA_OUTW(clk_lcdc_oclk_ctrl_REG_ADDR,_ezchip_macro_read_value_); \
+}
+
+#define _ENABLE_CLOCK_clk_lcdc_axi_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(clk_lcdc_axi_ctrl_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<31); \
+	_ezchip_macro_read_value_ |= (0x1&0x1)<<31; \
+	MA_OUTW(clk_lcdc_axi_ctrl_REG_ADDR,_ezchip_macro_read_value_); \
+}
+
+#define _ENABLE_CLOCK_clk_vpp0_axi_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(clk_vpp0_axi_ctrl_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<31); \
+	_ezchip_macro_read_value_ |= (0x1&0x1)<<31; \
+	MA_OUTW(clk_vpp0_axi_ctrl_REG_ADDR,_ezchip_macro_read_value_); \
+}
+
+
+#define _ENABLE_CLOCK_clk_vpp1_axi_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(clk_vpp1_axi_ctrl_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<31); \
+	_ezchip_macro_read_value_ |= (0x1&0x1)<<31; \
+	MA_OUTW(clk_vpp1_axi_ctrl_REG_ADDR,_ezchip_macro_read_value_); \
+}
+
+#define _ENABLE_CLOCK_clk_vpp2_axi_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(clk_vpp2_axi_ctrl_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<31); \
+	_ezchip_macro_read_value_ |= (0x1&0x1)<<31; \
+	MA_OUTW(clk_vpp2_axi_ctrl_REG_ADDR,_ezchip_macro_read_value_); \
+}
+
+#define _ENABLE_CLOCK_clk_vpp2_axi_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(clk_vpp2_axi_ctrl_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<31); \
+	_ezchip_macro_read_value_ |= (0x1&0x1)<<31; \
+	MA_OUTW(clk_vpp2_axi_ctrl_REG_ADDR,_ezchip_macro_read_value_); \
+}
+
+#define _ENABLE_CLOCK_clk_mapconv_apb_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(clk_mapconv_apb_ctrl_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<31); \
+	_ezchip_macro_read_value_ |= (0x1&0x1)<<31; \
+	MA_OUTW(clk_mapconv_apb_ctrl_REG_ADDR,_ezchip_macro_read_value_); \
+}
+
+#define _ENABLE_CLOCK_clk_mapconv_axi_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(clk_mapconv_axi_ctrl_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<31); \
+	_ezchip_macro_read_value_ |= (0x1&0x1)<<31; \
+	MA_OUTW(clk_mapconv_axi_ctrl_REG_ADDR,_ezchip_macro_read_value_); \
+}
+
+#define _ENABLE_CLOCK_clk_pixrawout_apb_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(clk_pixrawout_apb_ctrl_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<31); \
+	_ezchip_macro_read_value_ |= (0x1&0x1)<<31; \
+	MA_OUTW(clk_pixrawout_apb_ctrl_REG_ADDR,_ezchip_macro_read_value_); \
+}
+
+#define _ENABLE_CLOCK_clk_pixrawout_axi_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(clk_pixrawout_axi_ctrl_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<31); \
+	_ezchip_macro_read_value_ |= (0x1&0x1)<<31; \
+	MA_OUTW(clk_pixrawout_axi_ctrl_REG_ADDR,_ezchip_macro_read_value_); \
+}
+
+#define _ENABLE_CLOCK_clk_csi2tx_strm0_apb_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(clk_csi2tx_strm0_apb_ctrl_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<31); \
+	_ezchip_macro_read_value_ |= (0x1&0x1)<<31; \
+	MA_OUTW(clk_csi2tx_strm0_apb_ctrl_REG_ADDR,_ezchip_macro_read_value_); \
+}
+
+#define _ENABLE_CLOCK_clk_csi2tx_strm0_pixclk_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(clk_csi2tx_strm0_pixclk_ctrl_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<31); \
+	_ezchip_macro_read_value_ |= (0x1&0x1)<<31; \
+	MA_OUTW(clk_csi2tx_strm0_pixclk_ctrl_REG_ADDR,_ezchip_macro_read_value_); \
+}
+
+#define _ENABLE_CLOCK_clk_ppi_tx_esc_clk_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(clk_ppi_tx_esc_clk_ctrl_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<31); \
+	_ezchip_macro_read_value_ |= (0x1&0x1)<<31; \
+	MA_OUTW(clk_ppi_tx_esc_clk_ctrl_REG_ADDR,_ezchip_macro_read_value_); \
+}
+
+#define _ENABLE_CLOCK_clk_dsi_apb_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(clk_dsi_apb_ctrl_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<31); \
+	_ezchip_macro_read_value_ |= (0x1&0x1)<<31; \
+	MA_OUTW(clk_dsi_apb_ctrl_REG_ADDR,_ezchip_macro_read_value_); \
+}
+
+#define _ENABLE_CLOCK_clk_dsi_sys_clk_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(clk_dsi_sys_clk_ctrl_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<31); \
+	_ezchip_macro_read_value_ |= (0x1&0x1)<<31; \
+	MA_OUTW(clk_dsi_sys_clk_ctrl_REG_ADDR,_ezchip_macro_read_value_); \
+}
+
+#define _CLEAR_RESET_vout_sys_rstgen_rstn_disp0_axi_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(vout_sys_rstgen_Software_RESET_assert0_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<2); \
+	_ezchip_macro_read_value_ |= (0x0&0x1)<<2; \
+	MA_OUTW(vout_sys_rstgen_Software_RESET_assert0_REG_ADDR,_ezchip_macro_read_value_); \
+	do { \
+		_ezchip_macro_read_value_ = MA_INW(vout_sys_rstgen_Software_RESET_status0_REG_ADDR)>>2; \
+		_ezchip_macro_read_value_ &= 0x1;\
+	} while(_ezchip_macro_read_value_!=0x1); \
+}
+
+#define _CLEAR_RESET_vout_sys_rstgen_rstn_disp1_axi_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(vout_sys_rstgen_Software_RESET_assert0_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<3); \
+	_ezchip_macro_read_value_ |= (0x0&0x1)<<3; \
+	MA_OUTW(vout_sys_rstgen_Software_RESET_assert0_REG_ADDR,_ezchip_macro_read_value_); \
+	do { \
+		_ezchip_macro_read_value_ = MA_INW(vout_sys_rstgen_Software_RESET_status0_REG_ADDR)>>3; \
+		_ezchip_macro_read_value_ &= 0x1;\
+	} while(_ezchip_macro_read_value_!=0x1); \
+}
+
+#define _CLEAR_RESET_vout_sys_rstgen_rstn_lcdc_axi_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(vout_sys_rstgen_Software_RESET_assert0_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<5); \
+	_ezchip_macro_read_value_ |= (0x0&0x1)<<5; \
+	MA_OUTW(vout_sys_rstgen_Software_RESET_assert0_REG_ADDR,_ezchip_macro_read_value_); \
+	do { \
+		_ezchip_macro_read_value_ = MA_INW(vout_sys_rstgen_Software_RESET_status0_REG_ADDR)>>5; \
+		_ezchip_macro_read_value_ &= 0x1;\
+	} while(_ezchip_macro_read_value_!=0x1); \
+}
+
+#define _CLEAR_RESET_vout_sys_rstgen_rstn_vpp0_axi_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(vout_sys_rstgen_Software_RESET_assert0_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<6); \
+	_ezchip_macro_read_value_ |= (0x0&0x1)<<6; \
+	MA_OUTW(vout_sys_rstgen_Software_RESET_assert0_REG_ADDR,_ezchip_macro_read_value_); \
+	do { \
+		_ezchip_macro_read_value_ = MA_INW(vout_sys_rstgen_Software_RESET_status0_REG_ADDR)>>6; \
+		_ezchip_macro_read_value_ &= 0x1;\
+	} while(_ezchip_macro_read_value_!=0x1); \
+}
+
+#define _CLEAR_RESET_vout_sys_rstgen_rstn_vpp1_axi_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(vout_sys_rstgen_Software_RESET_assert0_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<7); \
+	_ezchip_macro_read_value_ |= (0x0&0x1)<<7; \
+	MA_OUTW(vout_sys_rstgen_Software_RESET_assert0_REG_ADDR,_ezchip_macro_read_value_); \
+	do { \
+		_ezchip_macro_read_value_ = MA_INW(vout_sys_rstgen_Software_RESET_status0_REG_ADDR)>>7; \
+		_ezchip_macro_read_value_ &= 0x1;\
+	} while(_ezchip_macro_read_value_!=0x1); \
+}
+
+#define _CLEAR_RESET_vout_sys_rstgen_rstn_vpp2_axi_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(vout_sys_rstgen_Software_RESET_assert0_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<8); \
+	_ezchip_macro_read_value_ |= (0x0&0x1)<<8; \
+	MA_OUTW(vout_sys_rstgen_Software_RESET_assert0_REG_ADDR,_ezchip_macro_read_value_); \
+	do { \
+		_ezchip_macro_read_value_ = MA_INW(vout_sys_rstgen_Software_RESET_status0_REG_ADDR)>>8; \
+		_ezchip_macro_read_value_ &= 0x1;\
+	} while(_ezchip_macro_read_value_!=0x1); \
+}
+
+#define _CLEAR_RESET_vout_sys_rstgen_rstn_mapconv_apb_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(vout_sys_rstgen_Software_RESET_assert0_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1); \
+	_ezchip_macro_read_value_ |= (0x0&0x1); \
+	MA_OUTW(vout_sys_rstgen_Software_RESET_assert0_REG_ADDR,_ezchip_macro_read_value_); \
+	do { \
+		_ezchip_macro_read_value_ = MA_INW(vout_sys_rstgen_Software_RESET_status0_REG_ADDR); \
+		_ezchip_macro_read_value_ &= 0x1;\
+	} while(_ezchip_macro_read_value_!=0x1); \
+}
+
+#define _CLEAR_RESET_vout_sys_rstgen_rstn_mapconv_axi_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(vout_sys_rstgen_Software_RESET_assert0_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<1); \
+	_ezchip_macro_read_value_ |= (0x0&0x1)<<1; \
+	MA_OUTW(vout_sys_rstgen_Software_RESET_assert0_REG_ADDR,_ezchip_macro_read_value_); \
+	do { \
+		_ezchip_macro_read_value_ = MA_INW(vout_sys_rstgen_Software_RESET_status0_REG_ADDR)>>1; \
+		_ezchip_macro_read_value_ &= 0x1;\
+	} while(_ezchip_macro_read_value_!=0x1); \
+}
+
+#define _CLEAR_RESET_vout_sys_rstgen_rstn_pixrawout_apb_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(vout_sys_rstgen_Software_RESET_assert0_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<9); \
+	_ezchip_macro_read_value_ |= (0x0&0x1)<<9; \
+	MA_OUTW(vout_sys_rstgen_Software_RESET_assert0_REG_ADDR,_ezchip_macro_read_value_); \
+	do { \
+		_ezchip_macro_read_value_ = MA_INW(vout_sys_rstgen_Software_RESET_status0_REG_ADDR)>>9; \
+		_ezchip_macro_read_value_ &= 0x1;\
+	} while(_ezchip_macro_read_value_!=0x1); \
+}
+
+#define _CLEAR_RESET_vout_sys_rstgen_rstn_pixrawout_axi_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(vout_sys_rstgen_Software_RESET_assert0_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<10); \
+	_ezchip_macro_read_value_ |= (0x0&0x1)<<10; \
+	MA_OUTW(vout_sys_rstgen_Software_RESET_assert0_REG_ADDR,_ezchip_macro_read_value_); \
+	do { \
+		_ezchip_macro_read_value_ = MA_INW(vout_sys_rstgen_Software_RESET_status0_REG_ADDR)>>10; \
+		_ezchip_macro_read_value_ &= 0x1;\
+	} while(_ezchip_macro_read_value_!=0x1); \
+}
+
+#define _CLEAR_RESET_vout_sys_rstgen_rstn_dsi_apb_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(vout_sys_rstgen_Software_RESET_assert0_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<15); \
+	_ezchip_macro_read_value_ |= (0x0&0x1)<<15; \
+	MA_OUTW(vout_sys_rstgen_Software_RESET_assert0_REG_ADDR,_ezchip_macro_read_value_); \
+	do { \
+		_ezchip_macro_read_value_ = MA_INW(vout_sys_rstgen_Software_RESET_status0_REG_ADDR)>>15; \
+		_ezchip_macro_read_value_ &= 0x1;\
+	} while(_ezchip_macro_read_value_!=0x1); \
+}
+
+#define _CLEAR_RESET_vout_sys_rstgen_rstn_dsi_sys_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(vout_sys_rstgen_Software_RESET_assert0_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<16); \
+	_ezchip_macro_read_value_ |= (0x0&0x1)<<16; \
+	MA_OUTW(vout_sys_rstgen_Software_RESET_assert0_REG_ADDR,_ezchip_macro_read_value_); \
+	do { \
+		_ezchip_macro_read_value_ = MA_INW(vout_sys_rstgen_Software_RESET_status0_REG_ADDR)>>16; \
+		_ezchip_macro_read_value_ &= 0x1;\
+	} while(_ezchip_macro_read_value_!=0x1); \
+}
+
+#define _CLEAR_RESET_vout_sys_rstgen_rstn_dsi_ppi_tx_esc_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(vout_sys_rstgen_Software_RESET_assert0_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<19); \
+	_ezchip_macro_read_value_ |= (0x0&0x1)<<19; \
+	MA_OUTW(vout_sys_rstgen_Software_RESET_assert0_REG_ADDR,_ezchip_macro_read_value_); \
+	do { \
+		_ezchip_macro_read_value_ = MA_INW(vout_sys_rstgen_Software_RESET_status0_REG_ADDR)>>19; \
+		_ezchip_macro_read_value_ &= 0x1;\
+	} while(_ezchip_macro_read_value_!=0x1); \
+}
+
+#define _CLEAR_RESET_vout_sys_rstgen_rstn_dsi_ppi_rx_esc_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(vout_sys_rstgen_Software_RESET_assert0_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<20); \
+	_ezchip_macro_read_value_ |= (0x0&0x1)<<20; \
+	MA_OUTW(vout_sys_rstgen_Software_RESET_assert0_REG_ADDR,_ezchip_macro_read_value_); \
+	do { \
+		_ezchip_macro_read_value_ = MA_INW(vout_sys_rstgen_Software_RESET_status0_REG_ADDR)>>20; \
+		_ezchip_macro_read_value_ &= 0x1;\
+	} while(_ezchip_macro_read_value_!=0x1); \
+}
+
+#define _CLEAR_RESET_vout_sys_rstgen_rstn_csi2tx_strm0_apb_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(vout_sys_rstgen_Software_RESET_assert0_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<11); \
+	_ezchip_macro_read_value_ |= (0x0&0x1)<<11; \
+	MA_OUTW(vout_sys_rstgen_Software_RESET_assert0_REG_ADDR,_ezchip_macro_read_value_); \
+	do { \
+		_ezchip_macro_read_value_ = MA_INW(vout_sys_rstgen_Software_RESET_status0_REG_ADDR)>>11; \
+		_ezchip_macro_read_value_ &= 0x1;\
+	} while(_ezchip_macro_read_value_!=0x1); \
+}
+
+#define _CLEAR_RESET_vout_sys_rstgen_rstn_csi2tx_strm0_pix_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(vout_sys_rstgen_Software_RESET_assert0_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<12); \
+	_ezchip_macro_read_value_ |= (0x0&0x1)<<12; \
+	MA_OUTW(vout_sys_rstgen_Software_RESET_assert0_REG_ADDR,_ezchip_macro_read_value_); \
+	do { \
+		_ezchip_macro_read_value_ = MA_INW(vout_sys_rstgen_Software_RESET_status0_REG_ADDR)>>12; \
+		_ezchip_macro_read_value_ &= 0x1;\
+	} while(_ezchip_macro_read_value_!=0x1); \
+}
+
+#define _CLEAR_RESET_vout_sys_rstgen_rstn_csi2tx_ppi_tx_esc_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(vout_sys_rstgen_Software_RESET_assert0_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<13); \
+	_ezchip_macro_read_value_ |= (0x0&0x1)<<13; \
+	MA_OUTW(vout_sys_rstgen_Software_RESET_assert0_REG_ADDR,_ezchip_macro_read_value_); \
+	do { \
+		_ezchip_macro_read_value_ = MA_INW(vout_sys_rstgen_Software_RESET_status0_REG_ADDR)>>13; \
+		_ezchip_macro_read_value_ &= 0x1;\
+	} while(_ezchip_macro_read_value_!=0x1); \
+}
+
+#define _ASSERT_RESET_rstgen_rstn_vout_src_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(rstgen_Software_RESET_assert1_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<23); \
+	_ezchip_macro_read_value_ |= (0x1&0x1)<<23; \
+	MA_OUTW(rstgen_Software_RESET_assert1_REG_ADDR,_ezchip_macro_read_value_); \
+	do { \
+		_ezchip_macro_read_value_ = MA_INW(rstgen_Software_RESET_status1_REG_ADDR)>>23; \
+		_ezchip_macro_read_value_ &= 0x1;\
+	} while(_ezchip_macro_read_value_!=0x0); \
+}
+
+#define _ASSERT_RESET_rstgen_rstn_disp_axi_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(rstgen_Software_RESET_assert1_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<24); \
+	_ezchip_macro_read_value_ |= (0x1&0x1)<<24; \
+	MA_OUTW(rstgen_Software_RESET_assert1_REG_ADDR,_ezchip_macro_read_value_); \
+	do { \
+		_ezchip_macro_read_value_ = MA_INW(rstgen_Software_RESET_status1_REG_ADDR)>>24; \
+		_ezchip_macro_read_value_ &= 0x1;\
+	} while(_ezchip_macro_read_value_!=0x0); \
+}
+
+#define _ENABLE_CLOCK_clk_disp_axi_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(clk_disp_axi_ctrl_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<31); \
+	_ezchip_macro_read_value_ |= (0x1&0x1)<<31; \
+	MA_OUTW(clk_disp_axi_ctrl_REG_ADDR,_ezchip_macro_read_value_); \
+}
+
+#define _ENABLE_CLOCK_clk_vout_src_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(clk_vout_src_ctrl_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<31); \
+	_ezchip_macro_read_value_ |= (0x1&0x1)<<31; \
+	MA_OUTW(clk_vout_src_ctrl_REG_ADDR,_ezchip_macro_read_value_); \
+}
+
+#define _CLEAR_RESET_rstgen_rstn_vout_src_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(rstgen_Software_RESET_assert1_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<23); \
+	_ezchip_macro_read_value_ |= (0x0&0x1)<<23; \
+	MA_OUTW(rstgen_Software_RESET_assert1_REG_ADDR,_ezchip_macro_read_value_); \
+	do { \
+		_ezchip_macro_read_value_ = MA_INW(rstgen_Software_RESET_status1_REG_ADDR)>>23; \
+		_ezchip_macro_read_value_ &= 0x1;\
+	} while(_ezchip_macro_read_value_!=0x1); \
+}
+
+#define _CLEAR_RESET_rstgen_rstn_disp_axi_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(rstgen_Software_RESET_assert1_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<24); \
+	_ezchip_macro_read_value_ |= (0x0&0x1)<<24; \
+	MA_OUTW(rstgen_Software_RESET_assert1_REG_ADDR,_ezchip_macro_read_value_); \
+	do { \
+		_ezchip_macro_read_value_ = MA_INW(rstgen_Software_RESET_status1_REG_ADDR)>>24; \
+		_ezchip_macro_read_value_ &= 0x1;\
+	} while(_ezchip_macro_read_value_!=0x1); \
+}
+
+#define _ENABLE_CLOCK_clk_vin_src_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(clk_vin_src_ctrl_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<31); \
+	_ezchip_macro_read_value_ |= (0x1&0x1)<<31; \
+	MA_OUTW(clk_vin_src_ctrl_REG_ADDR,_ezchip_macro_read_value_); \
+}
+
+#define _ENABLE_CLOCK_clk_vin_axi_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(clk_vin_axi_ctrl_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<31); \
+	_ezchip_macro_read_value_ |= (0x1&0x1)<<31; \
+	MA_OUTW(clk_vin_axi_ctrl_REG_ADDR,_ezchip_macro_read_value_); \
+}
+
+#define _ENABLE_CLOCK_clk_vinnoc_axi_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(clk_vinnoc_axi_ctrl_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<31); \
+	_ezchip_macro_read_value_ |= (0x1&0x1)<<31; \
+	MA_OUTW(clk_vinnoc_axi_ctrl_REG_ADDR,_ezchip_macro_read_value_); \
+}
+
+#define _ENABLE_CLOCK_clk_ispslv_axi_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(clk_ispslv_axi_ctrl_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<31); \
+	_ezchip_macro_read_value_ |= (0x1&0x1)<<31; \
+	MA_OUTW(clk_ispslv_axi_ctrl_REG_ADDR,_ezchip_macro_read_value_); \
+}
+
+#define _ENABLE_CLOCK_clk_isp0_axi_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(clk_isp0_axi_ctrl_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<31); \
+	_ezchip_macro_read_value_ |= (0x1&0x1)<<31; \
+	MA_OUTW(clk_isp0_axi_ctrl_REG_ADDR,_ezchip_macro_read_value_); \
+}
+
+#define _ENABLE_CLOCK_clk_isp1_axi_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(clk_isp1_axi_ctrl_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<31); \
+	_ezchip_macro_read_value_ |= (0x1&0x1)<<31; \
+	MA_OUTW(clk_isp1_axi_ctrl_REG_ADDR,_ezchip_macro_read_value_); \
+}
+
+#define _ENABLE_CLOCK_clk_isp0noc_axi_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(clk_isp0noc_axi_ctrl_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<31); \
+	_ezchip_macro_read_value_ |= (0x1&0x1)<<31; \
+	MA_OUTW(clk_isp0noc_axi_ctrl_REG_ADDR,_ezchip_macro_read_value_); \
+}
+
+#define _ENABLE_CLOCK_clk_isp1noc_axi_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(clk_isp1noc_axi_ctrl_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<31); \
+	_ezchip_macro_read_value_ |= (0x1&0x1)<<31; \
+	MA_OUTW(clk_isp1noc_axi_ctrl_REG_ADDR,_ezchip_macro_read_value_); \
+}
+
+#define _CLEAR_RESET_rstgen_rstn_vin_src_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(rstgen_Software_RESET_assert1_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<15); \
+	_ezchip_macro_read_value_ |= (0x0&0x1)<<15; \
+	MA_OUTW(rstgen_Software_RESET_assert1_REG_ADDR,_ezchip_macro_read_value_); \
+	do { \
+		_ezchip_macro_read_value_ = MA_INW(rstgen_Software_RESET_status1_REG_ADDR)>>15; \
+		_ezchip_macro_read_value_ &= 0x1;\
+	} while(_ezchip_macro_read_value_!=0x1); \
+}
+
+#define _CLEAR_RESET_rstgen_rstn_vin_axi_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(rstgen_Software_RESET_assert1_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<17); \
+	_ezchip_macro_read_value_ |= (0x0&0x1)<<17; \
+	MA_OUTW(rstgen_Software_RESET_assert1_REG_ADDR,_ezchip_macro_read_value_); \
+	do { \
+		_ezchip_macro_read_value_ = MA_INW(rstgen_Software_RESET_status1_REG_ADDR)>>17; \
+		_ezchip_macro_read_value_ &= 0x1;\
+	} while(_ezchip_macro_read_value_!=0x1); \
+}
+
+#define _CLEAR_RESET_rstgen_rstn_vinnoc_axi_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(rstgen_Software_RESET_assert1_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<18); \
+	_ezchip_macro_read_value_ |= (0x0&0x1)<<18; \
+	MA_OUTW(rstgen_Software_RESET_assert1_REG_ADDR,_ezchip_macro_read_value_); \
+	do { \
+		_ezchip_macro_read_value_ = MA_INW(rstgen_Software_RESET_status1_REG_ADDR)>>18; \
+		_ezchip_macro_read_value_ &= 0x1;\
+	} while(_ezchip_macro_read_value_!=0x1); \
+}
+
+#define _CLEAR_RESET_rstgen_rstn_ispslv_axi_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(rstgen_Software_RESET_assert1_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<16); \
+	_ezchip_macro_read_value_ |= (0x0&0x1)<<16; \
+	MA_OUTW(rstgen_Software_RESET_assert1_REG_ADDR,_ezchip_macro_read_value_); \
+	do { \
+		_ezchip_macro_read_value_ = MA_INW(rstgen_Software_RESET_status1_REG_ADDR)>>16; \
+		_ezchip_macro_read_value_ &= 0x1;\
+	} while(_ezchip_macro_read_value_!=0x1); \
+}
+
+#define _CLEAR_RESET_rstgen_rstn_isp0_axi_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(rstgen_Software_RESET_assert1_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<19); \
+	_ezchip_macro_read_value_ |= (0x0&0x1)<<19; \
+	MA_OUTW(rstgen_Software_RESET_assert1_REG_ADDR,_ezchip_macro_read_value_); \
+	do { \
+		_ezchip_macro_read_value_ = MA_INW(rstgen_Software_RESET_status1_REG_ADDR)>>19; \
+		_ezchip_macro_read_value_ &= 0x1;\
+	} while(_ezchip_macro_read_value_!=0x1); \
+}
+
+#define _CLEAR_RESET_rstgen_rstn_isp0noc_axi_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(rstgen_Software_RESET_assert1_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<20); \
+	_ezchip_macro_read_value_ |= (0x0&0x1)<<20; \
+	MA_OUTW(rstgen_Software_RESET_assert1_REG_ADDR,_ezchip_macro_read_value_); \
+	do { \
+		_ezchip_macro_read_value_ = MA_INW(rstgen_Software_RESET_status1_REG_ADDR)>>20; \
+		_ezchip_macro_read_value_ &= 0x1;\
+	} while(_ezchip_macro_read_value_!=0x1); \
+}
+
+#define _CLEAR_RESET_rstgen_rstn_isp1_axi_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(rstgen_Software_RESET_assert1_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<21); \
+	_ezchip_macro_read_value_ |= (0x0&0x1)<<21; \
+	MA_OUTW(rstgen_Software_RESET_assert1_REG_ADDR,_ezchip_macro_read_value_); \
+	do { \
+		_ezchip_macro_read_value_ = MA_INW(rstgen_Software_RESET_status1_REG_ADDR)>>21; \
+		_ezchip_macro_read_value_ &= 0x1;\
+	} while(_ezchip_macro_read_value_!=0x1); \
+}
+
+#define _CLEAR_RESET_rstgen_rstn_isp1noc_axi_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(rstgen_Software_RESET_assert1_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<22); \
+	_ezchip_macro_read_value_ |= (0x0&0x1)<<22; \
+	MA_OUTW(rstgen_Software_RESET_assert1_REG_ADDR,_ezchip_macro_read_value_); \
+	do { \
+		_ezchip_macro_read_value_ = MA_INW(rstgen_Software_RESET_status1_REG_ADDR)>>22; \
+		_ezchip_macro_read_value_ &= 0x1;\
+	} while(_ezchip_macro_read_value_!=0x1); \
+}
+
+#define _ENABLE_CLOCK_clk_c_isp0_ {}
+#define clk_isp0_ctrl_REG_ADDR  ISP_CLKGEN_BASE_ADDR + 0x3C
+#define clk_isp0_2x_ctrl_REG_ADDR  ISP_CLKGEN_BASE_ADDR + 0x40
+#define clk_isp0_mipi_ctrl_REG_ADDR  ISP_CLKGEN_BASE_ADDR + 0x44
+#define clk_isp1_ctrl_REG_ADDR  ISP_CLKGEN_BASE_ADDR + 0x48
+#define clk_isp1_2x_ctrl_REG_ADDR  ISP_CLKGEN_BASE_ADDR + 0x4C
+#define clk_isp1_mipi_ctrl_REG_ADDR  ISP_CLKGEN_BASE_ADDR + 0x50
+
+#define _ENABLE_CLOCK_clk_isp0_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(clk_isp0_ctrl_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<31); \
+	_ezchip_macro_read_value_ |= (0x1&0x1)<<31; \
+	MA_OUTW(clk_isp0_ctrl_REG_ADDR,_ezchip_macro_read_value_); \
+}
+
+#define _ENABLE_CLOCK_clk_isp0_2x_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(clk_isp0_2x_ctrl_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<31); \
+	_ezchip_macro_read_value_ |= (0x1&0x1)<<31; \
+	MA_OUTW(clk_isp0_2x_ctrl_REG_ADDR,_ezchip_macro_read_value_); \
+}
+
+#define _ENABLE_CLOCK_clk_isp0_mipi_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(clk_isp0_mipi_ctrl_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<31); \
+	_ezchip_macro_read_value_ |= (0x1&0x1)<<31; \
+	MA_OUTW(clk_isp0_mipi_ctrl_REG_ADDR,_ezchip_macro_read_value_); \
+}
+
+#define _ENABLE_CLOCK_clk_c_isp1_ {}
+
+#define _ENABLE_CLOCK_clk_isp1_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(clk_isp1_ctrl_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<31); \
+	_ezchip_macro_read_value_ |= (0x1&0x1)<<31; \
+	MA_OUTW(clk_isp1_ctrl_REG_ADDR,_ezchip_macro_read_value_); \
+}
+
+#define _ENABLE_CLOCK_clk_isp1_2x_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(clk_isp1_2x_ctrl_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<31); \
+	_ezchip_macro_read_value_ |= (0x1&0x1)<<31; \
+	MA_OUTW(clk_isp1_2x_ctrl_REG_ADDR,_ezchip_macro_read_value_); \
+}
+
+#define _ENABLE_CLOCK_clk_isp1_mipi_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(clk_isp1_mipi_ctrl_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<31); \
+	_ezchip_macro_read_value_ |= (0x1&0x1)<<31; \
+	MA_OUTW(clk_isp1_mipi_ctrl_REG_ADDR,_ezchip_macro_read_value_); \
+}
+
+#define clk_mipi_rx0_sys0_ctrl_REG_ADDR  			ISP_CLKGEN_BASE_ADDR + 0x24
+#define clk_mipi_rx1_sys1_ctrl_REG_ADDR  			ISP_CLKGEN_BASE_ADDR + 0x38
+#define isp_rstgen_Software_RESET_assert0_REG_ADDR  ISP_RSTGEN_BASE_ADDR + 0x0
+#define isp_rstgen_Software_RESET_status0_REG_ADDR  ISP_RSTGEN_BASE_ADDR + 0x4
+
+#define _ENABLE_CLOCK_clk_mipi_rx0_sys0_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(clk_mipi_rx0_sys0_ctrl_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<31); \
+	_ezchip_macro_read_value_ |= (0x1&0x1)<<31; \
+	MA_OUTW(clk_mipi_rx0_sys0_ctrl_REG_ADDR,_ezchip_macro_read_value_); \
+}
+
+#define _ENABLE_CLOCK_clk_mipi_rx1_sys1_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(clk_mipi_rx1_sys1_ctrl_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<31); \
+	_ezchip_macro_read_value_ |= (0x1&0x1)<<31; \
+	MA_OUTW(clk_mipi_rx1_sys1_ctrl_REG_ADDR,_ezchip_macro_read_value_); \
+}
+
+#define _CLEAR_RESET_isp_rstgen_rst_n_pclk_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(isp_rstgen_Software_RESET_assert0_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<1); \
+	_ezchip_macro_read_value_ |= (0x0&0x1)<<1; \
+	MA_OUTW(isp_rstgen_Software_RESET_assert0_REG_ADDR,_ezchip_macro_read_value_); \
+	do { \
+		_ezchip_macro_read_value_ = MA_INW(isp_rstgen_Software_RESET_status0_REG_ADDR)>>1; \
+		_ezchip_macro_read_value_ &= 0x1;\
+	} while(_ezchip_macro_read_value_!=0x1); \
+}
+
+#define _CLEAR_RESET_isp_rstgen_rst_n_sys_clk_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(isp_rstgen_Software_RESET_assert0_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1); \
+	_ezchip_macro_read_value_ |= (0x0&0x1); \
+	MA_OUTW(isp_rstgen_Software_RESET_assert0_REG_ADDR,_ezchip_macro_read_value_); \
+	do { \
+		_ezchip_macro_read_value_ = MA_INW(isp_rstgen_Software_RESET_status0_REG_ADDR); \
+		_ezchip_macro_read_value_ &= 0x1;\
+	} while(_ezchip_macro_read_value_!=0x1); \
+}
+
+#define _CLEAR_RESET_isp_rstgen_rst_c_isp0_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(isp_rstgen_Software_RESET_assert0_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<19); \
+	_ezchip_macro_read_value_ |= (0x0&0x1)<<19; \
+	MA_OUTW(isp_rstgen_Software_RESET_assert0_REG_ADDR,_ezchip_macro_read_value_); \
+	do { \
+		_ezchip_macro_read_value_ = MA_INW(isp_rstgen_Software_RESET_status0_REG_ADDR)>>19; \
+		_ezchip_macro_read_value_ &= 0x1;\
+	} while(_ezchip_macro_read_value_!=0x0); \
+}
+
+#define _CLEAR_RESET_isp_rstgen_rst_isp_0_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(isp_rstgen_Software_RESET_assert0_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<11); \
+	_ezchip_macro_read_value_ |= (0x0&0x1)<<11; \
+	MA_OUTW(isp_rstgen_Software_RESET_assert0_REG_ADDR,_ezchip_macro_read_value_); \
+	do { \
+		_ezchip_macro_read_value_ = MA_INW(isp_rstgen_Software_RESET_status0_REG_ADDR)>>11; \
+		_ezchip_macro_read_value_ &= 0x1;\
+	} while(_ezchip_macro_read_value_!=0x1); \
+}
+
+#define _CLEAR_RESET_isp_rstgen_rst_p_isp0_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(isp_rstgen_Software_RESET_assert0_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<15); \
+	_ezchip_macro_read_value_ |= (0x0&0x1)<<15; \
+	MA_OUTW(isp_rstgen_Software_RESET_assert0_REG_ADDR,_ezchip_macro_read_value_); \
+	do { \
+		_ezchip_macro_read_value_ = MA_INW(isp_rstgen_Software_RESET_status0_REG_ADDR)>>15; \
+		_ezchip_macro_read_value_ &= 0x1;\
+	} while(_ezchip_macro_read_value_!=0x0); \
+}
+
+#define _CLEAR_RESET_isp_rstgen_rst_n_sys_clk_1_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(isp_rstgen_Software_RESET_assert0_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<2); \
+	_ezchip_macro_read_value_ |= (0x0&0x1)<<2; \
+	MA_OUTW(isp_rstgen_Software_RESET_assert0_REG_ADDR,_ezchip_macro_read_value_); \
+	do { \
+		_ezchip_macro_read_value_ = MA_INW(isp_rstgen_Software_RESET_status0_REG_ADDR)>>2; \
+		_ezchip_macro_read_value_ &= 0x1;\
+	} while(_ezchip_macro_read_value_!=0x1); \
+}
+
+#define _CLEAR_RESET_isp_rstgen_rst_c_isp1_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(isp_rstgen_Software_RESET_assert0_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<20); \
+	_ezchip_macro_read_value_ |= (0x0&0x1)<<20; \
+	MA_OUTW(isp_rstgen_Software_RESET_assert0_REG_ADDR,_ezchip_macro_read_value_); \
+	do { \
+		_ezchip_macro_read_value_ = MA_INW(isp_rstgen_Software_RESET_status0_REG_ADDR)>>20; \
+		_ezchip_macro_read_value_ &= 0x1;\
+	} while(_ezchip_macro_read_value_!=0x0); \
+}
+
+#define _CLEAR_RESET_isp_rstgen_rst_c_isp1_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(isp_rstgen_Software_RESET_assert0_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<20); \
+	_ezchip_macro_read_value_ |= (0x0&0x1)<<20; \
+	MA_OUTW(isp_rstgen_Software_RESET_assert0_REG_ADDR,_ezchip_macro_read_value_); \
+	do { \
+		_ezchip_macro_read_value_ = MA_INW(isp_rstgen_Software_RESET_status0_REG_ADDR)>>20; \
+		_ezchip_macro_read_value_ &= 0x1;\
+	} while(_ezchip_macro_read_value_!=0x0); \
+}
+
+#define _CLEAR_RESET_isp_rstgen_rst_p_isp1_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(isp_rstgen_Software_RESET_assert0_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<16); \
+	_ezchip_macro_read_value_ |= (0x0&0x1)<<16; \
+	MA_OUTW(isp_rstgen_Software_RESET_assert0_REG_ADDR,_ezchip_macro_read_value_); \
+	do { \
+		_ezchip_macro_read_value_ = MA_INW(isp_rstgen_Software_RESET_status0_REG_ADDR)>>16; \
+		_ezchip_macro_read_value_ &= 0x1;\
+	} while(_ezchip_macro_read_value_!=0x0); \
+}
+
+#define _CLEAR_RESET_isp_rstgen_rst_p_axiwr_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(isp_rstgen_Software_RESET_assert0_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<14); \
+	_ezchip_macro_read_value_ |= (0x0&0x1)<<14; \
+	MA_OUTW(isp_rstgen_Software_RESET_assert0_REG_ADDR,_ezchip_macro_read_value_); \
+	do { \
+		_ezchip_macro_read_value_ = MA_INW(isp_rstgen_Software_RESET_status0_REG_ADDR)>>14; \
+		_ezchip_macro_read_value_ &= 0x1;\
+	} while(_ezchip_macro_read_value_!=0x1); \
+}
+
+#define _CLEAR_RESET_isp_rstgen_rst_p_axird_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(isp_rstgen_Software_RESET_assert0_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<13); \
+	_ezchip_macro_read_value_ |= (0x0&0x1)<<13; \
+	MA_OUTW(isp_rstgen_Software_RESET_assert0_REG_ADDR,_ezchip_macro_read_value_); \
+	do { \
+		_ezchip_macro_read_value_ = MA_INW(isp_rstgen_Software_RESET_status0_REG_ADDR)>>13; \
+		_ezchip_macro_read_value_ &= 0x1;\
+	} while(_ezchip_macro_read_value_!=0x1); \
+}
+
+#define _CLEAR_RESET_isp_rstgen_rst_isp_1_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(isp_rstgen_Software_RESET_assert0_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<12); \
+	_ezchip_macro_read_value_ |= (0x0&0x1)<<12; \
+	MA_OUTW(isp_rstgen_Software_RESET_assert0_REG_ADDR,_ezchip_macro_read_value_); \
+	do { \
+		_ezchip_macro_read_value_ = MA_INW(isp_rstgen_Software_RESET_status0_REG_ADDR)>>12; \
+		_ezchip_macro_read_value_ &= 0x1;\
+	} while(_ezchip_macro_read_value_!=0x1); \
+}
+
+#define _DISABLE_CLOCK_clk_disp_axi_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(clk_disp_axi_ctrl_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<31); \
+	_ezchip_macro_read_value_ |= (0x0&0x1)<<31; \
+	MA_OUTW(clk_disp_axi_ctrl_REG_ADDR,_ezchip_macro_read_value_); \
+}
+
+#define _DISABLE_CLOCK_clk_vout_src_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(clk_vout_src_ctrl_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<31); \
+	_ezchip_macro_read_value_ |= (0x0&0x1)<<31; \
+	MA_OUTW(clk_vout_src_ctrl_REG_ADDR,_ezchip_macro_read_value_); \
+}
+
+#define _ASSERT_RESET_rstgen_rstn_vin_src_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(rstgen_Software_RESET_assert1_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<15); \
+	_ezchip_macro_read_value_ |= (0x1&0x1)<<15; \
+	MA_OUTW(rstgen_Software_RESET_assert1_REG_ADDR,_ezchip_macro_read_value_); \
+	do { \
+		_ezchip_macro_read_value_ = MA_INW(rstgen_Software_RESET_status1_REG_ADDR)>>15; \
+		_ezchip_macro_read_value_ &= 0x1;\
+	} while(_ezchip_macro_read_value_!=0x0); \
+}
+
+#define _DISABLE_CLOCK_clk_vin_src_ { \
+	u32 _ezchip_macro_read_value_=MA_INW(clk_vin_src_ctrl_REG_ADDR); \
+	_ezchip_macro_read_value_ &= ~(0x1<<31); \
+	_ezchip_macro_read_value_ |= (0x0&0x1)<<31; \
+	MA_OUTW(clk_vin_src_ctrl_REG_ADDR,_ezchip_macro_read_value_); \
+}
+
+void delay(int cycles)
+{
+    int i;
+
+    for(i = 0; i < cycles; i++);
+}
+
+void vout_sys_clkrstsrc_init(int open)
+{
+	if(open==0x0) {
+		_DISABLE_CLOCK_clk_disp_axi_
+		_DISABLE_CLOCK_clk_vout_src_
+		_ASSERT_RESET_rstgen_rstn_vout_src_
+		_ASSERT_RESET_rstgen_rstn_disp_axi_
+	} else {
+		_ENABLE_CLOCK_clk_disp_axi_
+		_ENABLE_CLOCK_clk_vout_src_
+		_CLEAR_RESET_rstgen_rstn_vout_src_
+		_CLEAR_RESET_rstgen_rstn_disp_axi_
+	}
+
+	delay(1000);
+	printk("Config the clk and reset source for vout domain Finish\n");
+}
+
+void vout_sys_clkrst_init(int open){
+	if(open==0x1){
+	 _ENABLE_CLOCK_clk_disp0_axi_;
+	 _ENABLE_CLOCK_clk_disp1_axi_;
+	 _ENABLE_CLOCK_clk_lcdc_oclk_;
+	 _ENABLE_CLOCK_clk_lcdc_axi_;
+	 _ENABLE_CLOCK_clk_vpp0_axi_;
+	 _ENABLE_CLOCK_clk_vpp1_axi_;
+	 _ENABLE_CLOCK_clk_vpp2_axi_;
+	 _ENABLE_CLOCK_clk_mapconv_apb_;
+	 _ENABLE_CLOCK_clk_mapconv_axi_;
+	 _ENABLE_CLOCK_clk_pixrawout_apb_;
+	 _ENABLE_CLOCK_clk_pixrawout_axi_;
+	 _ENABLE_CLOCK_clk_csi2tx_strm0_apb_;
+	 _ENABLE_CLOCK_clk_csi2tx_strm0_pixclk_;
+	 _ENABLE_CLOCK_clk_ppi_tx_esc_clk_;
+	 _ENABLE_CLOCK_clk_dsi_apb_;
+	 _ENABLE_CLOCK_clk_dsi_sys_clk_;
+
+
+	 _CLEAR_RESET_vout_sys_rstgen_rstn_disp0_axi_;
+	 _CLEAR_RESET_vout_sys_rstgen_rstn_disp1_axi_;
+	 _CLEAR_RESET_vout_sys_rstgen_rstn_lcdc_axi_;
+	 _CLEAR_RESET_vout_sys_rstgen_rstn_vpp0_axi_;
+	 _CLEAR_RESET_vout_sys_rstgen_rstn_vpp1_axi_;
+	 _CLEAR_RESET_vout_sys_rstgen_rstn_vpp2_axi_;
+	 _CLEAR_RESET_vout_sys_rstgen_rstn_mapconv_apb_;
+	 _CLEAR_RESET_vout_sys_rstgen_rstn_mapconv_axi_;
+	 _CLEAR_RESET_vout_sys_rstgen_rstn_pixrawout_apb_;
+	 _CLEAR_RESET_vout_sys_rstgen_rstn_pixrawout_axi_;
+
+	 _CLEAR_RESET_vout_sys_rstgen_rstn_dsi_apb_;
+	 _CLEAR_RESET_vout_sys_rstgen_rstn_dsi_sys_;
+	 _CLEAR_RESET_vout_sys_rstgen_rstn_dsi_ppi_tx_esc_;
+	 _CLEAR_RESET_vout_sys_rstgen_rstn_dsi_ppi_rx_esc_;
+
+	 _CLEAR_RESET_vout_sys_rstgen_rstn_csi2tx_strm0_apb_;
+	 _CLEAR_RESET_vout_sys_rstgen_rstn_csi2tx_strm0_pix_;
+	 _CLEAR_RESET_vout_sys_rstgen_rstn_csi2tx_ppi_tx_esc_;
+	}
+	printk("Config the clk and reset for vout domain, Finish\n");
+	delay(100);
+}
+
+#endif
+
diff --git a/drivers/video/fbdev/starfive/tda998x.c b/drivers/video/fbdev/starfive/tda998x.c
new file mode 100755
index 000000000000..64faaa01123f
--- /dev/null
+++ b/drivers/video/fbdev/starfive/tda998x.c
@@ -0,0 +1,2254 @@
+/*
+ * Copyright (C) 2012 Texas Instruments
+ * Author: Rob Clark <robdclark@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/component.h>
+#include <linux/gpio/consumer.h>
+#include <linux/hdmi.h>
+#include <linux/module.h>
+#include <linux/platform_data/tda9950.h>
+#include <linux/irq.h>
+#include <sound/asoundef.h>
+#include <sound/hdmi-codec.h>
+#include <drm/drm_atomic_helper.h>
+#include <drm/drm_crtc_helper.h>
+#include <drm/drm_probe_helper.h>
+#include <drm/drm_bridge.h>
+#include <drm/drm_edid.h>
+#include <drm/drm_of.h>
+#include <drm/drm_print.h>
+#include <drm/i2c/tda998x.h>
+#include <media/cec-notifier.h>
+#include "starfive_display_dev.h"
+
+#define DBG(fmt, ...) DRM_DEBUG(fmt"\n", ##__VA_ARGS__)
+
+static DEFINE_MUTEX(tda998x_mutex);
+
+struct tda998x_audio_port {
+	u8 format;		/* AFMT_xxx */
+	u8 config;		/* AP value */
+};
+
+struct tda998x_priv {
+	struct i2c_client *cec;
+	struct i2c_client *hdmi;
+	struct mutex mutex;
+	u16 rev;
+	u8 cec_addr;
+	u8 current_page;
+	bool is_on;
+	bool supports_infoframes;
+	bool sink_has_audio;
+	u8 vip_cntrl_0;
+	u8 vip_cntrl_1;
+	u8 vip_cntrl_2;
+	unsigned long tmds_clock;
+	struct tda998x_audio_params audio_params;
+
+	struct platform_device *audio_pdev;
+	struct mutex audio_mutex;
+
+	struct mutex edid_mutex;
+	wait_queue_head_t wq_edid;
+	volatile int wq_edid_wait;
+
+	struct work_struct detect_work;
+	struct timer_list edid_delay_timer;
+	wait_queue_head_t edid_delay_waitq;
+	bool edid_delay_active;
+
+	struct drm_encoder encoder;
+	struct drm_bridge bridge;
+	struct drm_connector connector;
+
+	struct tda998x_audio_port audio_port[2];
+	struct tda9950_glue cec_glue;
+	struct gpio_desc *calib;
+	struct cec_notifier *cec_notify;
+};
+
+#define conn_to_tda998x_priv(x) \
+	container_of(x, struct tda998x_priv, connector)
+#define enc_to_tda998x_priv(x) \
+	container_of(x, struct tda998x_priv, encoder)
+#define bridge_to_tda998x_priv(x) \
+	container_of(x, struct tda998x_priv, bridge)
+
+/* The TDA9988 series of devices use a paged register scheme.. to simplify
+ * things we encode the page # in upper bits of the register #.  To read/
+ * write a given register, we need to make sure CURPAGE register is set
+ * appropriately.  Which implies reads/writes are not atomic.  Fun!
+ */
+
+#define REG(page, addr) (((page) << 8) | (addr))
+#define REG2ADDR(reg)   ((reg) & 0xff)
+#define REG2PAGE(reg)   (((reg) >> 8) & 0xff)
+
+#define REG_CURPAGE               0xff                /* write */
+
+
+/* Page 00h: General Control */
+#define REG_VERSION_LSB           REG(0x00, 0x00)     /* read */
+#define REG_MAIN_CNTRL0           REG(0x00, 0x01)     /* read/write */
+# define MAIN_CNTRL0_SR           (1 << 0)
+# define MAIN_CNTRL0_DECS         (1 << 1)
+# define MAIN_CNTRL0_DEHS         (1 << 2)
+# define MAIN_CNTRL0_CECS         (1 << 3)
+# define MAIN_CNTRL0_CEHS         (1 << 4)
+# define MAIN_CNTRL0_SCALER       (1 << 7)
+#define REG_VERSION_MSB           REG(0x00, 0x02)     /* read */
+#define REG_SOFTRESET             REG(0x00, 0x0a)     /* write */
+# define SOFTRESET_AUDIO          (1 << 0)
+# define SOFTRESET_I2C_MASTER     (1 << 1)
+#define REG_DDC_DISABLE           REG(0x00, 0x0b)     /* read/write */
+#define REG_CCLK_ON               REG(0x00, 0x0c)     /* read/write */
+#define REG_I2C_MASTER            REG(0x00, 0x0d)     /* read/write */
+# define I2C_MASTER_DIS_MM        (1 << 0)
+# define I2C_MASTER_DIS_FILT      (1 << 1)
+# define I2C_MASTER_APP_STRT_LAT  (1 << 2)
+#define REG_FEAT_POWERDOWN        REG(0x00, 0x0e)     /* read/write */
+# define FEAT_POWERDOWN_PREFILT   BIT(0)
+# define FEAT_POWERDOWN_CSC       BIT(1)
+# define FEAT_POWERDOWN_SPDIF     (1 << 3)
+#define REG_INT_FLAGS_0           REG(0x00, 0x0f)     /* read/write */
+#define REG_INT_FLAGS_1           REG(0x00, 0x10)     /* read/write */
+#define REG_INT_FLAGS_2           REG(0x00, 0x11)     /* read/write */
+# define INT_FLAGS_2_EDID_BLK_RD  (1 << 1)
+#define REG_ENA_ACLK              REG(0x00, 0x16)     /* read/write */
+#define REG_ENA_VP_0              REG(0x00, 0x18)     /* read/write */
+#define REG_ENA_VP_1              REG(0x00, 0x19)     /* read/write */
+#define REG_ENA_VP_2              REG(0x00, 0x1a)     /* read/write */
+#define REG_ENA_AP                REG(0x00, 0x1e)     /* read/write */
+#define REG_VIP_CNTRL_0           REG(0x00, 0x20)     /* write */
+# define VIP_CNTRL_0_MIRR_A       (1 << 7)
+# define VIP_CNTRL_0_SWAP_A(x)    (((x) & 7) << 4)
+# define VIP_CNTRL_0_MIRR_B       (1 << 3)
+# define VIP_CNTRL_0_SWAP_B(x)    (((x) & 7) << 0)
+#define REG_VIP_CNTRL_1           REG(0x00, 0x21)     /* write */
+# define VIP_CNTRL_1_MIRR_C       (1 << 7)
+# define VIP_CNTRL_1_SWAP_C(x)    (((x) & 7) << 4)
+# define VIP_CNTRL_1_MIRR_D       (1 << 3)
+# define VIP_CNTRL_1_SWAP_D(x)    (((x) & 7) << 0)
+#define REG_VIP_CNTRL_2           REG(0x00, 0x22)     /* write */
+# define VIP_CNTRL_2_MIRR_E       (1 << 7)
+# define VIP_CNTRL_2_SWAP_E(x)    (((x) & 7) << 4)
+# define VIP_CNTRL_2_MIRR_F       (1 << 3)
+# define VIP_CNTRL_2_SWAP_F(x)    (((x) & 7) << 0)
+#define REG_VIP_CNTRL_3           REG(0x00, 0x23)     /* write */
+# define VIP_CNTRL_3_X_TGL        (1 << 0)
+# define VIP_CNTRL_3_H_TGL        (1 << 1)
+# define VIP_CNTRL_3_V_TGL        (1 << 2)
+# define VIP_CNTRL_3_EMB          (1 << 3)
+# define VIP_CNTRL_3_SYNC_DE      (1 << 4)
+# define VIP_CNTRL_3_SYNC_HS      (1 << 5)
+# define VIP_CNTRL_3_DE_INT       (1 << 6)
+# define VIP_CNTRL_3_EDGE         (1 << 7)
+#define REG_VIP_CNTRL_4           REG(0x00, 0x24)     /* write */
+# define VIP_CNTRL_4_BLC(x)       (((x) & 3) << 0)
+# define VIP_CNTRL_4_BLANKIT(x)   (((x) & 3) << 2)
+# define VIP_CNTRL_4_CCIR656      (1 << 4)
+# define VIP_CNTRL_4_656_ALT      (1 << 5)
+# define VIP_CNTRL_4_TST_656      (1 << 6)
+# define VIP_CNTRL_4_TST_PAT      (1 << 7)
+#define REG_VIP_CNTRL_5           REG(0x00, 0x25)     /* write */
+# define VIP_CNTRL_5_CKCASE       (1 << 0)
+# define VIP_CNTRL_5_SP_CNT(x)    (((x) & 3) << 1)
+#define REG_MUX_AP                REG(0x00, 0x26)     /* read/write */
+# define MUX_AP_SELECT_I2S	  0x64
+# define MUX_AP_SELECT_SPDIF	  0x40
+#define REG_MUX_VP_VIP_OUT        REG(0x00, 0x27)     /* read/write */
+#define REG_MAT_CONTRL            REG(0x00, 0x80)     /* write */
+# define MAT_CONTRL_MAT_SC(x)     (((x) & 3) << 0)
+# define MAT_CONTRL_MAT_BP        (1 << 2)
+#define REG_VIDFORMAT             REG(0x00, 0xa0)     /* write */
+#define REG_REFPIX_MSB            REG(0x00, 0xa1)     /* write */
+#define REG_REFPIX_LSB            REG(0x00, 0xa2)     /* write */
+#define REG_REFLINE_MSB           REG(0x00, 0xa3)     /* write */
+#define REG_REFLINE_LSB           REG(0x00, 0xa4)     /* write */
+#define REG_NPIX_MSB              REG(0x00, 0xa5)     /* write */
+#define REG_NPIX_LSB              REG(0x00, 0xa6)     /* write */
+#define REG_NLINE_MSB             REG(0x00, 0xa7)     /* write */
+#define REG_NLINE_LSB             REG(0x00, 0xa8)     /* write */
+#define REG_VS_LINE_STRT_1_MSB    REG(0x00, 0xa9)     /* write */
+#define REG_VS_LINE_STRT_1_LSB    REG(0x00, 0xaa)     /* write */
+#define REG_VS_PIX_STRT_1_MSB     REG(0x00, 0xab)     /* write */
+#define REG_VS_PIX_STRT_1_LSB     REG(0x00, 0xac)     /* write */
+#define REG_VS_LINE_END_1_MSB     REG(0x00, 0xad)     /* write */
+#define REG_VS_LINE_END_1_LSB     REG(0x00, 0xae)     /* write */
+#define REG_VS_PIX_END_1_MSB      REG(0x00, 0xaf)     /* write */
+#define REG_VS_PIX_END_1_LSB      REG(0x00, 0xb0)     /* write */
+#define REG_VS_LINE_STRT_2_MSB    REG(0x00, 0xb1)     /* write */
+#define REG_VS_LINE_STRT_2_LSB    REG(0x00, 0xb2)     /* write */
+#define REG_VS_PIX_STRT_2_MSB     REG(0x00, 0xb3)     /* write */
+#define REG_VS_PIX_STRT_2_LSB     REG(0x00, 0xb4)     /* write */
+#define REG_VS_LINE_END_2_MSB     REG(0x00, 0xb5)     /* write */
+#define REG_VS_LINE_END_2_LSB     REG(0x00, 0xb6)     /* write */
+#define REG_VS_PIX_END_2_MSB      REG(0x00, 0xb7)     /* write */
+#define REG_VS_PIX_END_2_LSB      REG(0x00, 0xb8)     /* write */
+#define REG_HS_PIX_START_MSB      REG(0x00, 0xb9)     /* write */
+#define REG_HS_PIX_START_LSB      REG(0x00, 0xba)     /* write */
+#define REG_HS_PIX_STOP_MSB       REG(0x00, 0xbb)     /* write */
+#define REG_HS_PIX_STOP_LSB       REG(0x00, 0xbc)     /* write */
+#define REG_VWIN_START_1_MSB      REG(0x00, 0xbd)     /* write */
+#define REG_VWIN_START_1_LSB      REG(0x00, 0xbe)     /* write */
+#define REG_VWIN_END_1_MSB        REG(0x00, 0xbf)     /* write */
+#define REG_VWIN_END_1_LSB        REG(0x00, 0xc0)     /* write */
+#define REG_VWIN_START_2_MSB      REG(0x00, 0xc1)     /* write */
+#define REG_VWIN_START_2_LSB      REG(0x00, 0xc2)     /* write */
+#define REG_VWIN_END_2_MSB        REG(0x00, 0xc3)     /* write */
+#define REG_VWIN_END_2_LSB        REG(0x00, 0xc4)     /* write */
+#define REG_DE_START_MSB          REG(0x00, 0xc5)     /* write */
+#define REG_DE_START_LSB          REG(0x00, 0xc6)     /* write */
+#define REG_DE_STOP_MSB           REG(0x00, 0xc7)     /* write */
+#define REG_DE_STOP_LSB           REG(0x00, 0xc8)     /* write */
+#define REG_TBG_CNTRL_0           REG(0x00, 0xca)     /* write */
+# define TBG_CNTRL_0_TOP_TGL      (1 << 0)
+# define TBG_CNTRL_0_TOP_SEL      (1 << 1)
+# define TBG_CNTRL_0_DE_EXT       (1 << 2)
+# define TBG_CNTRL_0_TOP_EXT      (1 << 3)
+# define TBG_CNTRL_0_FRAME_DIS    (1 << 5)
+# define TBG_CNTRL_0_SYNC_MTHD    (1 << 6)
+# define TBG_CNTRL_0_SYNC_ONCE    (1 << 7)
+#define REG_TBG_CNTRL_1           REG(0x00, 0xcb)     /* write */
+# define TBG_CNTRL_1_H_TGL        (1 << 0)
+# define TBG_CNTRL_1_V_TGL        (1 << 1)
+# define TBG_CNTRL_1_TGL_EN       (1 << 2)
+# define TBG_CNTRL_1_X_EXT        (1 << 3)
+# define TBG_CNTRL_1_H_EXT        (1 << 4)
+# define TBG_CNTRL_1_V_EXT        (1 << 5)
+# define TBG_CNTRL_1_DWIN_DIS     (1 << 6)
+#define REG_ENABLE_SPACE          REG(0x00, 0xd6)     /* write */
+#define REG_HVF_CNTRL_0           REG(0x00, 0xe4)     /* write */
+# define HVF_CNTRL_0_SM           (1 << 7)
+# define HVF_CNTRL_0_RWB          (1 << 6)
+# define HVF_CNTRL_0_PREFIL(x)    (((x) & 3) << 2)
+# define HVF_CNTRL_0_INTPOL(x)    (((x) & 3) << 0)
+#define REG_HVF_CNTRL_1           REG(0x00, 0xe5)     /* write */
+# define HVF_CNTRL_1_FOR          (1 << 0)
+# define HVF_CNTRL_1_YUVBLK       (1 << 1)
+# define HVF_CNTRL_1_VQR(x)       (((x) & 3) << 2)
+# define HVF_CNTRL_1_PAD(x)       (((x) & 3) << 4)
+# define HVF_CNTRL_1_SEMI_PLANAR  (1 << 6)
+#define REG_RPT_CNTRL             REG(0x00, 0xf0)     /* write */
+#define REG_I2S_FORMAT            REG(0x00, 0xfc)     /* read/write */
+# define I2S_FORMAT(x)            (((x) & 3) << 0)
+#define REG_AIP_CLKSEL            REG(0x00, 0xfd)     /* write */
+# define AIP_CLKSEL_AIP_SPDIF	  (0 << 3)
+# define AIP_CLKSEL_AIP_I2S	  (1 << 3)
+# define AIP_CLKSEL_FS_ACLK	  (0 << 0)
+# define AIP_CLKSEL_FS_MCLK	  (1 << 0)
+# define AIP_CLKSEL_FS_FS64SPDIF  (2 << 0)
+
+/* Page 02h: PLL settings */
+#define REG_PLL_SERIAL_1          REG(0x02, 0x00)     /* read/write */
+# define PLL_SERIAL_1_SRL_FDN     (1 << 0)
+# define PLL_SERIAL_1_SRL_IZ(x)   (((x) & 3) << 1)
+# define PLL_SERIAL_1_SRL_MAN_IZ  (1 << 6)
+#define REG_PLL_SERIAL_2          REG(0x02, 0x01)     /* read/write */
+# define PLL_SERIAL_2_SRL_NOSC(x) ((x) << 0)
+# define PLL_SERIAL_2_SRL_PR(x)   (((x) & 0xf) << 4)
+#define REG_PLL_SERIAL_3          REG(0x02, 0x02)     /* read/write */
+# define PLL_SERIAL_3_SRL_CCIR    (1 << 0)
+# define PLL_SERIAL_3_SRL_DE      (1 << 2)
+# define PLL_SERIAL_3_SRL_PXIN_SEL (1 << 4)
+#define REG_SERIALIZER            REG(0x02, 0x03)     /* read/write */
+#define REG_BUFFER_OUT            REG(0x02, 0x04)     /* read/write */
+#define REG_PLL_SCG1              REG(0x02, 0x05)     /* read/write */
+#define REG_PLL_SCG2              REG(0x02, 0x06)     /* read/write */
+#define REG_PLL_SCGN1             REG(0x02, 0x07)     /* read/write */
+#define REG_PLL_SCGN2             REG(0x02, 0x08)     /* read/write */
+#define REG_PLL_SCGR1             REG(0x02, 0x09)     /* read/write */
+#define REG_PLL_SCGR2             REG(0x02, 0x0a)     /* read/write */
+#define REG_AUDIO_DIV             REG(0x02, 0x0e)     /* read/write */
+# define AUDIO_DIV_SERCLK_1       0
+# define AUDIO_DIV_SERCLK_2       1
+# define AUDIO_DIV_SERCLK_4       2
+# define AUDIO_DIV_SERCLK_8       3
+# define AUDIO_DIV_SERCLK_16      4
+# define AUDIO_DIV_SERCLK_32      5
+#define REG_SEL_CLK               REG(0x02, 0x11)     /* read/write */
+# define SEL_CLK_SEL_CLK1         (1 << 0)
+# define SEL_CLK_SEL_VRF_CLK(x)   (((x) & 3) << 1)
+# define SEL_CLK_ENA_SC_CLK       (1 << 3)
+#define REG_ANA_GENERAL           REG(0x02, 0x12)     /* read/write */
+
+
+/* Page 09h: EDID Control */
+#define REG_EDID_DATA_0           REG(0x09, 0x00)     /* read */
+/* next 127 successive registers are the EDID block */
+#define REG_EDID_CTRL             REG(0x09, 0xfa)     /* read/write */
+#define REG_DDC_ADDR              REG(0x09, 0xfb)     /* read/write */
+#define REG_DDC_OFFS              REG(0x09, 0xfc)     /* read/write */
+#define REG_DDC_SEGM_ADDR         REG(0x09, 0xfd)     /* read/write */
+#define REG_DDC_SEGM              REG(0x09, 0xfe)     /* read/write */
+
+
+/* Page 10h: information frames and packets */
+#define REG_IF1_HB0               REG(0x10, 0x20)     /* read/write */
+#define REG_IF2_HB0               REG(0x10, 0x40)     /* read/write */
+#define REG_IF3_HB0               REG(0x10, 0x60)     /* read/write */
+#define REG_IF4_HB0               REG(0x10, 0x80)     /* read/write */
+#define REG_IF5_HB0               REG(0x10, 0xa0)     /* read/write */
+
+
+/* Page 11h: audio settings and content info packets */
+#define REG_AIP_CNTRL_0           REG(0x11, 0x00)     /* read/write */
+# define AIP_CNTRL_0_RST_FIFO     (1 << 0)
+# define AIP_CNTRL_0_SWAP         (1 << 1)
+# define AIP_CNTRL_0_LAYOUT       (1 << 2)
+# define AIP_CNTRL_0_ACR_MAN      (1 << 5)
+# define AIP_CNTRL_0_RST_CTS      (1 << 6)
+#define REG_CA_I2S                REG(0x11, 0x01)     /* read/write */
+# define CA_I2S_CA_I2S(x)         (((x) & 31) << 0)
+# define CA_I2S_HBR_CHSTAT        (1 << 6)
+#define REG_LATENCY_RD            REG(0x11, 0x04)     /* read/write */
+#define REG_ACR_CTS_0             REG(0x11, 0x05)     /* read/write */
+#define REG_ACR_CTS_1             REG(0x11, 0x06)     /* read/write */
+#define REG_ACR_CTS_2             REG(0x11, 0x07)     /* read/write */
+#define REG_ACR_N_0               REG(0x11, 0x08)     /* read/write */
+#define REG_ACR_N_1               REG(0x11, 0x09)     /* read/write */
+#define REG_ACR_N_2               REG(0x11, 0x0a)     /* read/write */
+#define REG_CTS_N                 REG(0x11, 0x0c)     /* read/write */
+# define CTS_N_K(x)               (((x) & 7) << 0)
+# define CTS_N_M(x)               (((x) & 3) << 4)
+#define REG_ENC_CNTRL             REG(0x11, 0x0d)     /* read/write */
+# define ENC_CNTRL_RST_ENC        (1 << 0)
+# define ENC_CNTRL_RST_SEL        (1 << 1)
+# define ENC_CNTRL_CTL_CODE(x)    (((x) & 3) << 2)
+#define REG_DIP_FLAGS             REG(0x11, 0x0e)     /* read/write */
+# define DIP_FLAGS_ACR            (1 << 0)
+# define DIP_FLAGS_GC             (1 << 1)
+#define REG_DIP_IF_FLAGS          REG(0x11, 0x0f)     /* read/write */
+# define DIP_IF_FLAGS_IF1         (1 << 1)
+# define DIP_IF_FLAGS_IF2         (1 << 2)
+# define DIP_IF_FLAGS_IF3         (1 << 3)
+# define DIP_IF_FLAGS_IF4         (1 << 4)
+# define DIP_IF_FLAGS_IF5         (1 << 5)
+#define REG_CH_STAT_B(x)          REG(0x11, 0x14 + (x)) /* read/write */
+
+
+/* Page 12h: HDCP and OTP */
+#define REG_TX3                   REG(0x12, 0x9a)     /* read/write */
+#define REG_TX4                   REG(0x12, 0x9b)     /* read/write */
+# define TX4_PD_RAM               (1 << 1)
+#define REG_TX33                  REG(0x12, 0xb8)     /* read/write */
+# define TX33_HDMI                (1 << 1)
+
+
+/* Page 13h: Gamut related metadata packets */
+
+
+
+/* CEC registers: (not paged)
+ */
+#define REG_CEC_INTSTATUS	  0xee		      /* read */
+# define CEC_INTSTATUS_CEC	  (1 << 0)
+# define CEC_INTSTATUS_HDMI	  (1 << 1)
+#define REG_CEC_CAL_XOSC_CTRL1    0xf2
+# define CEC_CAL_XOSC_CTRL1_ENA_CAL	BIT(0)
+#define REG_CEC_DES_FREQ2         0xf5
+# define CEC_DES_FREQ2_DIS_AUTOCAL BIT(7)
+#define REG_CEC_CLK               0xf6
+# define CEC_CLK_FRO              0x11
+#define REG_CEC_FRO_IM_CLK_CTRL   0xfb                /* read/write */
+# define CEC_FRO_IM_CLK_CTRL_GHOST_DIS (1 << 7)
+# define CEC_FRO_IM_CLK_CTRL_ENA_OTP   (1 << 6)
+# define CEC_FRO_IM_CLK_CTRL_IMCLK_SEL (1 << 1)
+# define CEC_FRO_IM_CLK_CTRL_FRO_DIV   (1 << 0)
+#define REG_CEC_RXSHPDINTENA	  0xfc		      /* read/write */
+#define REG_CEC_RXSHPDINT	  0xfd		      /* read */
+# define CEC_RXSHPDINT_RXSENS     BIT(0)
+# define CEC_RXSHPDINT_HPD        BIT(1)
+#define REG_CEC_RXSHPDLEV         0xfe                /* read */
+# define CEC_RXSHPDLEV_RXSENS     (1 << 0)
+# define CEC_RXSHPDLEV_HPD        (1 << 1)
+
+#define REG_CEC_ENAMODS           0xff                /* read/write */
+# define CEC_ENAMODS_EN_CEC_CLK   (1 << 7)
+# define CEC_ENAMODS_DIS_FRO      (1 << 6)
+# define CEC_ENAMODS_DIS_CCLK     (1 << 5)
+# define CEC_ENAMODS_EN_RXSENS    (1 << 2)
+# define CEC_ENAMODS_EN_HDMI      (1 << 1)
+# define CEC_ENAMODS_EN_CEC       (1 << 0)
+
+
+/* Device versions: */
+#define TDA9989N2                 0x0101
+#define TDA19989                  0x0201
+#define TDA19989N2                0x0202
+#define TDA19988                  0x0301
+
+static int
+cec_write(struct tda998x_priv *priv, u16 addr, u8 val)
+{
+	u8 buf[] = {addr, val};
+	struct i2c_msg msg = {
+		.addr = priv->cec_addr,
+		.len = 2,
+		.buf = buf,
+	};
+	int ret;
+
+	ret = i2c_transfer(priv->hdmi->adapter, &msg, 1);
+	if (ret < 0)
+		dev_err(&priv->hdmi->dev, "Error %d writing to cec:0x%x\n",
+			ret, addr);
+
+	return ret;
+}
+
+static u8
+cec_read(struct tda998x_priv *priv, u8 addr)
+{
+	u8 val;
+	struct i2c_msg msg[2] = {
+		{
+			.addr = priv->cec_addr,
+			.len = 1,
+			.buf = &addr,
+		}, {
+			.addr = priv->cec_addr,
+			.flags = I2C_M_RD,
+			.len = 1,
+			.buf = &val,
+		},
+	};
+	int ret;
+
+	ret = i2c_transfer(priv->hdmi->adapter, msg, ARRAY_SIZE(msg));
+	if (ret < 0) {
+		dev_err(&priv->hdmi->dev, "Error %d reading from cec:0x%x\n",
+			ret, addr);
+		val = 0;
+	}
+
+	return val;
+}
+
+static void cec_enamods(struct tda998x_priv *priv, u8 mods, bool enable)
+{
+	int val = cec_read(priv, REG_CEC_ENAMODS);
+
+	if (val < 0)
+		return;
+
+	if (enable)
+		val |= mods;
+	else
+		val &= ~mods;
+
+	cec_write(priv, REG_CEC_ENAMODS, val);
+}
+
+static void tda998x_cec_set_calibration(struct tda998x_priv *priv, bool enable)
+{
+	if (enable) {
+		u8 val;
+
+		cec_write(priv, 0xf3, 0xc0);
+		cec_write(priv, 0xf4, 0xd4);
+
+		/* Enable automatic calibration mode */
+		val = cec_read(priv, REG_CEC_DES_FREQ2);
+		val &= ~CEC_DES_FREQ2_DIS_AUTOCAL;
+		cec_write(priv, REG_CEC_DES_FREQ2, val);
+
+		/* Enable free running oscillator */
+		cec_write(priv, REG_CEC_CLK, CEC_CLK_FRO);
+		cec_enamods(priv, CEC_ENAMODS_DIS_FRO, false);
+
+		cec_write(priv, REG_CEC_CAL_XOSC_CTRL1,
+			  CEC_CAL_XOSC_CTRL1_ENA_CAL);
+	} else {
+		cec_write(priv, REG_CEC_CAL_XOSC_CTRL1, 0);
+	}
+}
+
+/*
+ * Calibration for the internal oscillator: we need to set calibration mode,
+ * and then pulse the IRQ line low for a 10ms ± 1% period.
+ */
+static void tda998x_cec_calibration(struct tda998x_priv *priv)
+{
+	struct gpio_desc *calib = priv->calib;
+
+	mutex_lock(&priv->edid_mutex);
+	if (priv->hdmi->irq > 0)
+		disable_irq(priv->hdmi->irq);
+	gpiod_direction_output(calib, 1);
+	tda998x_cec_set_calibration(priv, true);
+
+	local_irq_disable();
+	gpiod_set_value(calib, 0);
+	mdelay(10);
+	gpiod_set_value(calib, 1);
+	local_irq_enable();
+
+	tda998x_cec_set_calibration(priv, false);
+	gpiod_direction_input(calib);
+	if (priv->hdmi->irq > 0)
+		enable_irq(priv->hdmi->irq);
+	mutex_unlock(&priv->edid_mutex);
+}
+
+static int tda998x_cec_hook_init(void *data)
+{
+	struct tda998x_priv *priv = data;
+	struct gpio_desc *calib;
+
+	calib = gpiod_get(&priv->hdmi->dev, "nxp,calib", GPIOD_ASIS);
+	if (IS_ERR(calib)) {
+		dev_warn(&priv->hdmi->dev, "failed to get calibration gpio: %ld\n",
+			 PTR_ERR(calib));
+		return PTR_ERR(calib);
+	}
+
+	priv->calib = calib;
+
+	return 0;
+}
+
+static void tda998x_cec_hook_exit(void *data)
+{
+	struct tda998x_priv *priv = data;
+
+	gpiod_put(priv->calib);
+	priv->calib = NULL;
+}
+
+static int tda998x_cec_hook_open(void *data)
+{
+	struct tda998x_priv *priv = data;
+
+	cec_enamods(priv, CEC_ENAMODS_EN_CEC_CLK | CEC_ENAMODS_EN_CEC, true);
+	tda998x_cec_calibration(priv);
+
+	return 0;
+}
+
+static void tda998x_cec_hook_release(void *data)
+{
+	struct tda998x_priv *priv = data;
+
+	cec_enamods(priv, CEC_ENAMODS_EN_CEC_CLK | CEC_ENAMODS_EN_CEC, false);
+}
+
+static int set_page(struct tda998x_priv *priv, u16 reg)
+{
+	if (REG2PAGE(reg) != priv->current_page) {
+		struct i2c_client *client = priv->hdmi;
+		u8 buf[] = {
+				REG_CURPAGE, REG2PAGE(reg)
+		};
+		int ret = i2c_master_send(client, buf, sizeof(buf));
+		if (ret < 0) {
+			dev_err(&client->dev, "%s %04x err %d\n", __func__,
+					reg, ret);
+			return ret;
+		}
+
+		priv->current_page = REG2PAGE(reg);
+	}
+
+	return 0;
+}
+
+static int
+reg_read_range(struct tda998x_priv *priv, u16 reg, char *buf, int cnt)
+{
+	struct i2c_client *client = priv->hdmi;
+	u8 addr = REG2ADDR(reg);
+	int ret;
+
+	mutex_lock(&priv->mutex);
+	ret = set_page(priv, reg);
+	if (ret < 0)
+		goto out;
+
+	ret = i2c_master_send(client, &addr, sizeof(addr));
+	if (ret < 0)
+		goto fail;
+
+	ret = i2c_master_recv(client, buf, cnt);
+	if (ret < 0)
+		goto fail;
+
+	goto out;
+
+fail:
+	dev_err(&client->dev, "Error %d reading from 0x%x\n", ret, reg);
+out:
+	mutex_unlock(&priv->mutex);
+
+	return ret;
+}
+
+#define MAX_WRITE_RANGE_BUF 32
+
+static void
+reg_write_range(struct tda998x_priv *priv, u16 reg, u8 *p, int cnt)
+{
+	struct i2c_client *client = priv->hdmi;
+	/* This is the maximum size of the buffer passed in */
+	u8 buf[MAX_WRITE_RANGE_BUF + 1];
+	int ret;
+
+	if (cnt > MAX_WRITE_RANGE_BUF) {
+		dev_err(&client->dev, "Fixed write buffer too small (%d)\n",
+				MAX_WRITE_RANGE_BUF);
+		return;
+	}
+
+	buf[0] = REG2ADDR(reg);
+	memcpy(&buf[1], p, cnt);
+
+	mutex_lock(&priv->mutex);
+	ret = set_page(priv, reg);
+	if (ret < 0)
+		goto out;
+
+	ret = i2c_master_send(client, buf, cnt + 1);
+	if (ret < 0)
+		dev_err(&client->dev, "Error %d writing to 0x%x\n", ret, reg);
+out:
+	mutex_unlock(&priv->mutex);
+}
+
+static int
+reg_read(struct tda998x_priv *priv, u16 reg)
+{
+	u8 val = 0;
+	int ret;
+
+	ret = reg_read_range(priv, reg, &val, sizeof(val));
+	if (ret < 0)
+		return ret;
+
+	return val;
+}
+
+static void
+reg_write(struct tda998x_priv *priv, u16 reg, u8 val)
+{
+	struct i2c_client *client = priv->hdmi;
+	u8 buf[] = {REG2ADDR(reg), val};
+	int ret;
+
+	mutex_lock(&priv->mutex);
+	ret = set_page(priv, reg);
+	if (ret < 0)
+		goto out;
+
+	ret = i2c_master_send(client, buf, sizeof(buf));
+	if (ret < 0)
+		dev_err(&client->dev, "Error %d writing to 0x%x\n", ret, reg);
+out:
+	mutex_unlock(&priv->mutex);
+}
+
+static void
+reg_write16(struct tda998x_priv *priv, u16 reg, u16 val)
+{
+	struct i2c_client *client = priv->hdmi;
+	u8 buf[] = {REG2ADDR(reg), val >> 8, val};
+	int ret;
+
+	mutex_lock(&priv->mutex);
+	ret = set_page(priv, reg);
+	if (ret < 0)
+		goto out;
+
+	ret = i2c_master_send(client, buf, sizeof(buf));
+	if (ret < 0)
+		dev_err(&client->dev, "Error %d writing to 0x%x\n", ret, reg);
+out:
+	mutex_unlock(&priv->mutex);
+}
+
+static void reg_set(struct tda998x_priv *priv, u16 reg, u8 val)
+{
+	int old_val;
+
+	old_val = reg_read(priv, reg);
+	if (old_val >= 0)
+		reg_write(priv, reg, old_val | val);
+}
+
+static void
+reg_clear(struct tda998x_priv *priv, u16 reg, u8 val)
+{
+	int old_val;
+
+	old_val = reg_read(priv, reg);
+	if (old_val >= 0)
+		reg_write(priv, reg, old_val & ~val);
+}
+
+static void tda998x_reset(struct tda998x_priv *priv)
+{
+	/* reset audio and i2c master: */
+	reg_write(priv, REG_SOFTRESET, SOFTRESET_AUDIO | SOFTRESET_I2C_MASTER);
+	msleep(50);
+	reg_write(priv, REG_SOFTRESET, 0);
+	msleep(50);
+
+	/* reset transmitter: */
+	reg_set(priv, REG_MAIN_CNTRL0, MAIN_CNTRL0_SR);
+	reg_clear(priv, REG_MAIN_CNTRL0, MAIN_CNTRL0_SR);
+
+	/* PLL registers common configuration */
+	reg_write(priv, REG_PLL_SERIAL_1, 0x00);
+	reg_write(priv, REG_PLL_SERIAL_2, PLL_SERIAL_2_SRL_NOSC(1));
+	reg_write(priv, REG_PLL_SERIAL_3, 0x00);
+	reg_write(priv, REG_SERIALIZER,   0x00);
+	reg_write(priv, REG_BUFFER_OUT,   0x00);
+	reg_write(priv, REG_PLL_SCG1,     0x00);
+	reg_write(priv, REG_AUDIO_DIV,    AUDIO_DIV_SERCLK_8);
+	reg_write(priv, REG_SEL_CLK,      SEL_CLK_SEL_CLK1 | SEL_CLK_ENA_SC_CLK);
+	reg_write(priv, REG_PLL_SCGN1,    0xfa);
+	reg_write(priv, REG_PLL_SCGN2,    0x00);
+	reg_write(priv, REG_PLL_SCGR1,    0x5b);
+	reg_write(priv, REG_PLL_SCGR2,    0x00);
+	reg_write(priv, REG_PLL_SCG2,     0x10);
+
+	/* Write the default value MUX register */
+	reg_write(priv, REG_MUX_VP_VIP_OUT, 0x24);
+}
+
+/*
+ * The TDA998x has a problem when trying to read the EDID close to a
+ * HPD assertion: it needs a delay of 100ms to avoid timing out while
+ * trying to read EDID data.
+ *
+ * However, tda998x_connector_get_modes() may be called at any moment
+ * after tda998x_connector_detect() indicates that we are connected, so
+ * we need to delay probing modes in tda998x_connector_get_modes() after
+ * we have seen a HPD inactive->active transition.  This code implements
+ * that delay.
+ */
+static void tda998x_edid_delay_done(struct timer_list *t)
+{
+	struct tda998x_priv *priv = from_timer(priv, t, edid_delay_timer);
+
+	priv->edid_delay_active = false;
+	wake_up(&priv->edid_delay_waitq);
+	schedule_work(&priv->detect_work);
+}
+
+static void tda998x_edid_delay_start(struct tda998x_priv *priv)
+{
+	priv->edid_delay_active = true;
+	mod_timer(&priv->edid_delay_timer, jiffies + HZ/10);
+}
+
+static int tda998x_edid_delay_wait(struct tda998x_priv *priv)
+{
+	return wait_event_killable(priv->edid_delay_waitq, !priv->edid_delay_active);
+}
+
+/*
+ * We need to run the KMS hotplug event helper outside of our threaded
+ * interrupt routine as this can call back into our get_modes method,
+ * which will want to make use of interrupts.
+ */
+static void tda998x_detect_work(struct work_struct *work)
+{
+	struct tda998x_priv *priv =
+		container_of(work, struct tda998x_priv, detect_work);
+	struct drm_device *dev = priv->connector.dev;
+
+	if (dev)
+		drm_kms_helper_hotplug_event(dev);
+}
+
+/*
+ * only 2 interrupts may occur: screen plug/unplug and EDID read
+ */
+static irqreturn_t tda998x_irq_thread(int irq, void *data)
+{
+	struct tda998x_priv *priv = data;
+	u8 sta, cec, lvl, flag0, flag1, flag2;
+	bool handled = false;
+
+	sta = cec_read(priv, REG_CEC_INTSTATUS);
+	if (sta & CEC_INTSTATUS_HDMI) {
+		cec = cec_read(priv, REG_CEC_RXSHPDINT);
+		lvl = cec_read(priv, REG_CEC_RXSHPDLEV);
+		flag0 = reg_read(priv, REG_INT_FLAGS_0);
+		flag1 = reg_read(priv, REG_INT_FLAGS_1);
+		flag2 = reg_read(priv, REG_INT_FLAGS_2);
+		DRM_DEBUG_DRIVER(
+			"tda irq sta %02x cec %02x lvl %02x f0 %02x f1 %02x f2 %02x\n",
+			sta, cec, lvl, flag0, flag1, flag2);
+
+		if (cec & CEC_RXSHPDINT_HPD) {
+			if (lvl & CEC_RXSHPDLEV_HPD) {
+				tda998x_edid_delay_start(priv);
+			} else {
+				schedule_work(&priv->detect_work);
+				cec_notifier_set_phys_addr(priv->cec_notify,
+						   CEC_PHYS_ADDR_INVALID);
+			}
+			handled = true;
+		}
+
+		if ((flag2 & INT_FLAGS_2_EDID_BLK_RD) && priv->wq_edid_wait) {
+			priv->wq_edid_wait = 0;
+			wake_up(&priv->wq_edid);
+			handled = true;
+		}
+	}
+
+	return IRQ_RETVAL(handled);
+}
+
+static void
+tda998x_write_if(struct tda998x_priv *priv, u8 bit, u16 addr,
+		 union hdmi_infoframe *frame)
+{
+	u8 buf[MAX_WRITE_RANGE_BUF];
+	ssize_t len;
+
+	len = hdmi_infoframe_pack(frame, buf, sizeof(buf));
+	if (len < 0) {
+		dev_err(&priv->hdmi->dev,
+			"hdmi_infoframe_pack() type=0x%02x failed: %zd\n",
+			frame->any.type, len);
+		return;
+	}
+
+	reg_clear(priv, REG_DIP_IF_FLAGS, bit);
+	reg_write_range(priv, addr, buf, len);
+	reg_set(priv, REG_DIP_IF_FLAGS, bit);
+}
+
+static int tda998x_write_aif(struct tda998x_priv *priv,
+				struct hdmi_audio_infoframe *cea)
+{
+	union hdmi_infoframe frame;
+
+	frame.audio = *cea;
+	tda998x_write_if(priv, DIP_IF_FLAGS_IF4, REG_IF4_HB0, &frame);
+
+	return 0;
+}
+
+static void
+tda998x_write_avi(struct tda998x_priv *priv, const struct drm_display_mode *mode)
+{
+	union hdmi_infoframe frame;
+
+	drm_hdmi_avi_infoframe_from_display_mode(&frame.avi, &priv->connector, mode);
+	frame.avi.quantization_range = HDMI_QUANTIZATION_RANGE_FULL;
+
+	tda998x_write_if(priv, DIP_IF_FLAGS_IF2, REG_IF2_HB0, &frame);
+}
+
+/* Audio support */
+
+static void tda998x_audio_mute(struct tda998x_priv *priv, bool on)
+{
+	if (on) {
+		reg_set(priv, REG_SOFTRESET, SOFTRESET_AUDIO);
+		reg_clear(priv, REG_SOFTRESET, SOFTRESET_AUDIO);
+		reg_set(priv, REG_AIP_CNTRL_0, AIP_CNTRL_0_RST_FIFO);
+	} else {
+		reg_clear(priv, REG_AIP_CNTRL_0, AIP_CNTRL_0_RST_FIFO);
+	}
+}
+
+static int tda998x_configure_audio(struct tda998x_priv *priv,
+			struct tda998x_audio_params *params)
+{
+	u8 buf[6], clksel_aip, clksel_fs, cts_n, adiv;
+	u32 n;
+
+	/* Enable audio ports */
+	reg_write(priv, REG_ENA_AP, params->config);
+
+	/* Set audio input source */
+	switch (params->format) {
+	case AFMT_SPDIF:
+		reg_write(priv, REG_ENA_ACLK, 0);
+		reg_write(priv, REG_MUX_AP, MUX_AP_SELECT_SPDIF);
+		clksel_aip = AIP_CLKSEL_AIP_SPDIF;
+		clksel_fs = AIP_CLKSEL_FS_FS64SPDIF;
+		cts_n = CTS_N_M(3) | CTS_N_K(3);
+		break;
+
+	case AFMT_I2S:
+		reg_write(priv, REG_ENA_ACLK, 1);
+		reg_write(priv, REG_MUX_AP, MUX_AP_SELECT_I2S);
+		clksel_aip = AIP_CLKSEL_AIP_I2S;
+		clksel_fs = AIP_CLKSEL_FS_ACLK;
+		switch (params->sample_width) {
+		case 16:
+			cts_n = CTS_N_M(3) | CTS_N_K(1);
+			break;
+		case 18:
+		case 20:
+		case 24:
+			cts_n = CTS_N_M(3) | CTS_N_K(2);
+			break;
+		default:
+		case 32:
+			cts_n = CTS_N_M(3) | CTS_N_K(3);
+			break;
+		}
+		break;
+
+	default:
+		dev_err(&priv->hdmi->dev, "Unsupported I2S format\n");
+		return -EINVAL;
+	}
+
+	reg_write(priv, REG_AIP_CLKSEL, clksel_aip);
+	reg_clear(priv, REG_AIP_CNTRL_0, AIP_CNTRL_0_LAYOUT |
+					AIP_CNTRL_0_ACR_MAN);	/* auto CTS */
+	reg_write(priv, REG_CTS_N, cts_n);
+
+	/*
+	 * Audio input somehow depends on HDMI line rate which is
+	 * related to pixclk. Testing showed that modes with pixclk
+	 * >100MHz need a larger divider while <40MHz need the default.
+	 * There is no detailed info in the datasheet, so we just
+	 * assume 100MHz requires larger divider.
+	 */
+	adiv = AUDIO_DIV_SERCLK_8;
+	if (priv->tmds_clock > 100000)
+		adiv++;			/* AUDIO_DIV_SERCLK_16 */
+
+	/* S/PDIF asks for a larger divider */
+	if (params->format == AFMT_SPDIF)
+		adiv++;			/* AUDIO_DIV_SERCLK_16 or _32 */
+
+	reg_write(priv, REG_AUDIO_DIV, adiv);
+
+	/*
+	 * This is the approximate value of N, which happens to be
+	 * the recommended values for non-coherent clocks.
+	 */
+	n = 128 * params->sample_rate / 1000;
+
+	/* Write the CTS and N values */
+	buf[0] = 0x44;
+	buf[1] = 0x42;
+	buf[2] = 0x01;
+	buf[3] = n;
+	buf[4] = n >> 8;
+	buf[5] = n >> 16;
+	reg_write_range(priv, REG_ACR_CTS_0, buf, 6);
+
+	/* Set CTS clock reference */
+	reg_write(priv, REG_AIP_CLKSEL, clksel_aip | clksel_fs);
+
+	/* Reset CTS generator */
+	reg_set(priv, REG_AIP_CNTRL_0, AIP_CNTRL_0_RST_CTS);
+	reg_clear(priv, REG_AIP_CNTRL_0, AIP_CNTRL_0_RST_CTS);
+
+	/* Write the channel status
+	 * The REG_CH_STAT_B-registers skip IEC958 AES2 byte, because
+	 * there is a separate register for each I2S wire.
+	 */
+	buf[0] = params->status[0];
+	buf[1] = params->status[1];
+	buf[2] = params->status[3];
+	buf[3] = params->status[4];
+	reg_write_range(priv, REG_CH_STAT_B(0), buf, 4);
+
+	tda998x_audio_mute(priv, true);
+	msleep(20);
+	tda998x_audio_mute(priv, false);
+
+	return tda998x_write_aif(priv, &params->cea);
+}
+
+static int tda998x_audio_hw_params(struct device *dev, void *data,
+				   struct hdmi_codec_daifmt *daifmt,
+				   struct hdmi_codec_params *params)
+{
+	struct tda998x_priv *priv = dev_get_drvdata(dev);
+	int i, ret;
+	struct tda998x_audio_params audio = {
+		.sample_width = params->sample_width,
+		.sample_rate = params->sample_rate,
+		.cea = params->cea,
+	};
+
+	memcpy(audio.status, params->iec.status,
+	       min(sizeof(audio.status), sizeof(params->iec.status)));
+
+	switch (daifmt->fmt) {
+	case HDMI_I2S:
+		if (daifmt->bit_clk_inv || daifmt->frame_clk_inv ||
+		    daifmt->bit_clk_master || daifmt->frame_clk_master) {
+			dev_err(dev, "%s: Bad flags %d %d %d %d\n", __func__,
+				daifmt->bit_clk_inv, daifmt->frame_clk_inv,
+				daifmt->bit_clk_master,
+				daifmt->frame_clk_master);
+			return -EINVAL;
+		}
+		for (i = 0; i < ARRAY_SIZE(priv->audio_port); i++)
+			if (priv->audio_port[i].format == AFMT_I2S)
+				audio.config = priv->audio_port[i].config;
+		audio.format = AFMT_I2S;
+		break;
+	case HDMI_SPDIF:
+		for (i = 0; i < ARRAY_SIZE(priv->audio_port); i++)
+			if (priv->audio_port[i].format == AFMT_SPDIF)
+				audio.config = priv->audio_port[i].config;
+		audio.format = AFMT_SPDIF;
+		break;
+	default:
+		dev_err(dev, "%s: Invalid format %d\n", __func__, daifmt->fmt);
+		return -EINVAL;
+	}
+
+	if (audio.config == 0) {
+		dev_err(dev, "%s: No audio configuration found\n", __func__);
+		return -EINVAL;
+	}
+
+	mutex_lock(&priv->audio_mutex);
+	if (priv->supports_infoframes && priv->sink_has_audio)
+		ret = tda998x_configure_audio(priv, &audio);
+	else
+		ret = 0;
+
+	if (ret == 0)
+		priv->audio_params = audio;
+	mutex_unlock(&priv->audio_mutex);
+
+	return ret;
+}
+
+static void tda998x_audio_shutdown(struct device *dev, void *data)
+{
+	struct tda998x_priv *priv = dev_get_drvdata(dev);
+
+	mutex_lock(&priv->audio_mutex);
+	reg_write(priv, REG_ENA_AP, 0);
+	priv->audio_params.format = AFMT_UNUSED;
+	mutex_unlock(&priv->audio_mutex);
+}
+
+int tda998x_audio_mute_stream(struct device *dev, void *data, bool enable,
+				int direction)
+{
+	struct tda998x_priv *priv = dev_get_drvdata(dev);
+
+	mutex_lock(&priv->audio_mutex);
+	tda998x_audio_mute(priv, enable);
+	mutex_unlock(&priv->audio_mutex);
+
+	return 0;
+}
+
+static int tda998x_audio_get_eld(struct device *dev, void *data,
+				 uint8_t *buf, size_t len)
+{
+	struct tda998x_priv *priv = dev_get_drvdata(dev);
+
+	mutex_lock(&priv->audio_mutex);
+	memcpy(buf, priv->connector.eld, min(sizeof(priv->connector.eld), len));
+	mutex_unlock(&priv->audio_mutex);
+
+	return 0;
+}
+
+static const struct hdmi_codec_ops audio_codec_ops = {
+	.hw_params = tda998x_audio_hw_params,
+	.audio_shutdown = tda998x_audio_shutdown,
+	.mute_stream = tda998x_audio_mute_stream,
+	.get_eld = tda998x_audio_get_eld,
+	.no_capture_mute = 1,
+};
+
+static int tda998x_audio_codec_init(struct tda998x_priv *priv,
+					struct device *dev)
+{
+	struct hdmi_codec_pdata codec_data = {
+		.ops = &audio_codec_ops,
+		.max_i2s_channels = 2,
+	};
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(priv->audio_port); i++) {
+		if (priv->audio_port[i].format == AFMT_I2S &&
+			priv->audio_port[i].config != 0)
+			codec_data.i2s = 1;
+		if (priv->audio_port[i].format == AFMT_SPDIF &&
+			priv->audio_port[i].config != 0)
+			codec_data.spdif = 1;
+	}
+
+	priv->audio_pdev = platform_device_register_data(
+		dev, HDMI_CODEC_DRV_NAME, PLATFORM_DEVID_AUTO,
+		&codec_data, sizeof(codec_data));
+
+	return PTR_ERR_OR_ZERO(priv->audio_pdev);
+}
+
+/* DRM connector functions */
+
+static enum drm_connector_status
+tda998x_connector_detect(struct drm_connector *connector, bool force)
+{
+	struct tda998x_priv *priv = conn_to_tda998x_priv(connector);
+	u8 val = cec_read(priv, REG_CEC_RXSHPDLEV);
+
+	return (val & CEC_RXSHPDLEV_HPD) ? connector_status_connected :
+			connector_status_disconnected;
+}
+
+static void tda998x_connector_destroy(struct drm_connector *connector)
+{
+	drm_connector_cleanup(connector);
+}
+
+static const struct drm_connector_funcs tda998x_connector_funcs = {
+	.dpms = drm_helper_connector_dpms,
+	.reset = drm_atomic_helper_connector_reset,
+	.fill_modes = drm_helper_probe_single_connector_modes,
+	.detect = tda998x_connector_detect,
+	.destroy = tda998x_connector_destroy,
+	.atomic_duplicate_state = drm_atomic_helper_connector_duplicate_state,
+	.atomic_destroy_state = drm_atomic_helper_connector_destroy_state,
+};
+
+static int read_edid_block(void *data, u8 *buf, unsigned int blk, size_t length)
+{
+	struct tda998x_priv *priv = data;
+	u8 offset, segptr;
+	int ret, i;
+
+	offset = (blk & 1) ? 128 : 0;
+	segptr = blk / 2;
+
+	mutex_lock(&priv->edid_mutex);
+
+	reg_write(priv, REG_DDC_ADDR, 0xa0);
+	reg_write(priv, REG_DDC_OFFS, offset);
+	reg_write(priv, REG_DDC_SEGM_ADDR, 0x60);
+	reg_write(priv, REG_DDC_SEGM, segptr);
+
+	/* enable reading EDID: */
+	priv->wq_edid_wait = 1;
+	reg_write(priv, REG_EDID_CTRL, 0x1);
+
+	/* flag must be cleared by sw: */
+	reg_write(priv, REG_EDID_CTRL, 0x0);
+
+	/* wait for block read to complete: */
+	if (priv->hdmi->irq) {
+		i = wait_event_timeout(priv->wq_edid,
+					!priv->wq_edid_wait,
+					msecs_to_jiffies(100));
+		if (i < 0) {
+			dev_err(&priv->hdmi->dev, "read edid wait err %d\n", i);
+			ret = i;
+			goto failed;
+		}
+	} else {
+		for (i = 100; i > 0; i--) {
+			msleep(1);
+			ret = reg_read(priv, REG_INT_FLAGS_2);
+			if (ret < 0)
+				goto failed;
+			if (ret & INT_FLAGS_2_EDID_BLK_RD)
+				break;
+		}
+	}
+
+	if (i == 0) {
+		dev_err(&priv->hdmi->dev, "read edid timeout\n");
+		ret = -ETIMEDOUT;
+		goto failed;
+	}
+
+	ret = reg_read_range(priv, REG_EDID_DATA_0, buf, length);
+	if (ret != length) {
+		dev_err(&priv->hdmi->dev, "failed to read edid block %d: %d\n",
+			blk, ret);
+		goto failed;
+	}
+
+	ret = 0;
+
+ failed:
+	mutex_unlock(&priv->edid_mutex);
+	return ret;
+}
+
+static int tda998x_connector_get_modes(struct drm_connector *connector)
+{
+	struct tda998x_priv *priv = conn_to_tda998x_priv(connector);
+	struct edid *edid;
+	int n;
+
+	/*
+	 * If we get killed while waiting for the HPD timeout, return
+	 * no modes found: we are not in a restartable path, so we
+	 * can't handle signals gracefully.
+	 */
+	if (tda998x_edid_delay_wait(priv))
+		return 0;
+
+	if (priv->rev == TDA19988)
+		reg_clear(priv, REG_TX4, TX4_PD_RAM);
+
+	edid = drm_do_get_edid(connector, read_edid_block, priv);
+
+	if (priv->rev == TDA19988)
+		reg_set(priv, REG_TX4, TX4_PD_RAM);
+
+	if (!edid) {
+		dev_warn(&priv->hdmi->dev, "failed to read EDID\n");
+		return 0;
+	}
+
+	drm_connector_update_edid_property(connector, edid);
+	cec_notifier_set_phys_addr_from_edid(priv->cec_notify, edid);
+
+	mutex_lock(&priv->audio_mutex);
+	n = drm_add_edid_modes(connector, edid);
+	priv->sink_has_audio = drm_detect_monitor_audio(edid);
+	mutex_unlock(&priv->audio_mutex);
+
+	kfree(edid);
+
+	return n;
+}
+
+static struct drm_encoder *
+tda998x_connector_best_encoder(struct drm_connector *connector)
+{
+	struct tda998x_priv *priv = conn_to_tda998x_priv(connector);
+
+	return priv->bridge.encoder;
+}
+
+static
+const struct drm_connector_helper_funcs tda998x_connector_helper_funcs = {
+	.get_modes = tda998x_connector_get_modes,
+	.best_encoder = tda998x_connector_best_encoder,
+};
+
+static int tda998x_connector_init(struct tda998x_priv *priv,
+				  struct drm_device *drm)
+{
+	struct drm_connector *connector = &priv->connector;
+	int ret;
+
+	connector->interlace_allowed = 1;
+
+	if (priv->hdmi->irq)
+		connector->polled = DRM_CONNECTOR_POLL_HPD;
+	else
+		connector->polled = DRM_CONNECTOR_POLL_CONNECT |
+			DRM_CONNECTOR_POLL_DISCONNECT;
+
+	drm_connector_helper_add(connector, &tda998x_connector_helper_funcs);
+	ret = drm_connector_init(drm, connector, &tda998x_connector_funcs,
+				 DRM_MODE_CONNECTOR_HDMIA);
+	if (ret)
+		return ret;
+
+	drm_connector_attach_encoder(&priv->connector, priv->bridge.encoder);
+
+	return 0;
+}
+
+/* DRM bridge functions */
+
+static int tda998x_bridge_attach(struct drm_bridge *bridge,
+				enum drm_bridge_attach_flags flags)
+{
+	struct tda998x_priv *priv = bridge_to_tda998x_priv(bridge);
+
+	if (flags & DRM_BRIDGE_ATTACH_NO_CONNECTOR) {
+		DRM_ERROR("Fix bridge driver to make connector optional!");
+		return -EINVAL;
+	}
+
+	return tda998x_connector_init(priv, bridge->dev);
+}
+
+static void tda998x_bridge_detach(struct drm_bridge *bridge)
+{
+	struct tda998x_priv *priv = bridge_to_tda998x_priv(bridge);
+
+	drm_connector_cleanup(&priv->connector);
+}
+
+static enum drm_mode_status tda998x_bridge_mode_valid(struct drm_bridge *bridge,
+				const struct drm_display_info *info,
+				const struct drm_display_mode *mode)
+{
+	/* TDA19988 dotclock can go up to 165MHz */
+	struct tda998x_priv *priv = bridge_to_tda998x_priv(bridge);
+
+	if (mode->clock > ((priv->rev == TDA19988) ? 165000 : 150000))
+		return MODE_CLOCK_HIGH;
+	if (mode->htotal >= BIT(13))
+		return MODE_BAD_HVALUE;
+	if (mode->vtotal >= BIT(11))
+		return MODE_BAD_VVALUE;
+	return MODE_OK;
+}
+
+static void tda998x_enable(struct device *dev)
+{
+	struct tda998x_priv *priv = dev_get_drvdata(dev);
+
+	if(NULL == priv) {
+		return ;
+	}
+
+	/* enable video ports, audio will be enabled later */
+	reg_write(priv, REG_ENA_VP_0, 0xff);
+	reg_write(priv, REG_ENA_VP_1, 0xff);
+	reg_write(priv, REG_ENA_VP_2, 0xff);
+	/* set muxing after enabling ports: */
+	reg_write(priv, REG_VIP_CNTRL_0, priv->vip_cntrl_0);
+	reg_write(priv, REG_VIP_CNTRL_1, priv->vip_cntrl_1);
+	reg_write(priv, REG_VIP_CNTRL_2, priv->vip_cntrl_2);
+
+	priv->is_on = true;
+
+}
+
+static void tda998x_bridge_enable(struct drm_bridge *bridge)
+{
+	struct tda998x_priv *priv = bridge_to_tda998x_priv(bridge);
+
+	if (!priv->is_on) {
+		/* enable video ports, audio will be enabled later */
+		reg_write(priv, REG_ENA_VP_0, 0xff);
+		reg_write(priv, REG_ENA_VP_1, 0xff);
+		reg_write(priv, REG_ENA_VP_2, 0xff);
+		/* set muxing after enabling ports: */
+		reg_write(priv, REG_VIP_CNTRL_0, priv->vip_cntrl_0);
+		reg_write(priv, REG_VIP_CNTRL_1, priv->vip_cntrl_1);
+		reg_write(priv, REG_VIP_CNTRL_2, priv->vip_cntrl_2);
+		priv->is_on = true;
+	}
+}
+
+static void tda998x_bridge_disable(struct drm_bridge *bridge)
+{
+	struct tda998x_priv *priv = bridge_to_tda998x_priv(bridge);
+
+	if (priv->is_on) {
+		/* disable video ports */
+		reg_write(priv, REG_ENA_VP_0, 0x00);
+		reg_write(priv, REG_ENA_VP_1, 0x00);
+		reg_write(priv, REG_ENA_VP_2, 0x00);
+		priv->is_on = false;
+	}
+}
+
+static void tda998x_mode_set(struct device *dev, struct sf_fb_display_dev *display_dev)
+{
+	struct tda998x_priv *priv = dev_get_drvdata(dev);
+//	unsigned long tmds_clock;
+	u16 ref_pix, ref_line, n_pix, n_line;
+	u16 hs_pix_s, hs_pix_e;
+	u16 vs1_pix_s, vs1_pix_e, vs1_line_s, vs1_line_e;
+	u16 vs2_pix_s, vs2_pix_e, vs2_line_s, vs2_line_e;
+	u16 vwin1_line_s, vwin1_line_e;
+	u16 vwin2_line_s, vwin2_line_e;
+	u16 de_pix_s, de_pix_e;
+	u8 reg, div, rep;
+
+	/*
+	 * Internally TDA998x is using ITU-R BT.656 style sync but
+	 * we get VESA style sync. TDA998x is using a reference pixel
+	 * relative to ITU to sync to the input frame and for output
+	 * sync generation. Currently, we are using reference detection
+	 * from HS/VS, i.e. REFPIX/REFLINE denote frame start sync point
+	 * which is position of rising VS with coincident rising HS.
+	 *
+	 * Now there is some issues to take care of:
+	 * - HDMI data islands require sync-before-active
+	 * - TDA998x register values must be > 0 to be enabled
+	 * - REFLINE needs an additional offset of +1
+	 * - REFPIX needs an addtional offset of +1 for UYUV and +3 for RGB
+	 *
+	 * So we add +1 to all horizontal and vertical register values,
+	 * plus an additional +3 for REFPIX as we are using RGB input only.
+	 */
+
+	struct drm_display_mode *mode = NULL;
+	int h_acive = display_dev->xres;
+	int h_front_porch = display_dev->timing.rgb.videomode_info.hfp;
+	int h_sync = display_dev->timing.rgb.videomode_info.hsync;
+	int h_back_porch = display_dev->timing.rgb.videomode_info.hbp;
+	int v_acive = display_dev->yres;
+	int v_front_porch = display_dev->timing.rgb.videomode_info.vfp;
+	int v_sync = display_dev->timing.rgb.videomode_info.vsync;
+	int v_back_porch = display_dev->timing.rgb.videomode_info.vbp;
+
+	mode = kzalloc(sizeof(struct drm_display_mode), GFP_KERNEL);
+	if(NULL == mode || NULL == priv) {
+		dev_err(dev, "%s: malloc fail\n", __func__);
+		return ;
+	}
+
+	mode->hdisplay = h_acive;
+	mode->hsync_start = h_acive + h_front_porch;
+	mode->hsync_end = h_acive + h_front_porch + h_sync;
+	mode->htotal = h_acive + h_front_porch + h_sync + h_back_porch;
+
+	mode->vdisplay = v_acive;
+	mode->vsync_start = v_acive + v_front_porch;
+	mode->vsync_end = v_acive + v_front_porch + v_sync;
+	mode->vtotal = v_acive + v_front_porch + v_sync + v_back_porch;
+
+	n_pix        = mode->htotal;
+	n_line       = mode->vtotal;
+
+	hs_pix_e     = mode->hsync_end - mode->hdisplay;
+	hs_pix_s     = mode->hsync_start - mode->hdisplay;
+	de_pix_e     = mode->htotal;
+	de_pix_s     = mode->htotal - mode->hdisplay;
+	ref_pix      = 3 + hs_pix_s;
+
+	/*
+	 * Attached LCD controllers may generate broken sync. Allow
+	 * those to adjust the position of the rising VS edge by adding
+	 * HSKEW to ref_pix.
+	 */
+	//if (adjusted_mode->flags & DRM_MODE_FLAG_HSKEW)
+	//	ref_pix += adjusted_mode->hskew;
+
+	mode->flags = 0;
+
+	if ((mode->flags & DRM_MODE_FLAG_INTERLACE) == 0) {
+		ref_line     = 1 + mode->vsync_start - mode->vdisplay;
+		vwin1_line_s = mode->vtotal - mode->vdisplay - 1;
+		vwin1_line_e = vwin1_line_s + mode->vdisplay;
+		vs1_pix_s    = vs1_pix_e = hs_pix_s;
+		vs1_line_s   = mode->vsync_start - mode->vdisplay;
+		vs1_line_e   = vs1_line_s +
+			       mode->vsync_end - mode->vsync_start;
+		vwin2_line_s = vwin2_line_e = 0;
+		vs2_pix_s    = vs2_pix_e  = 0;
+		vs2_line_s   = vs2_line_e = 0;
+	} else {
+		ref_line     = 1 + (mode->vsync_start - mode->vdisplay)/2;
+		vwin1_line_s = (mode->vtotal - mode->vdisplay)/2;
+		vwin1_line_e = vwin1_line_s + mode->vdisplay/2;
+		vs1_pix_s    = vs1_pix_e = hs_pix_s;
+		vs1_line_s   = (mode->vsync_start - mode->vdisplay)/2;
+		vs1_line_e   = vs1_line_s +
+			       (mode->vsync_end - mode->vsync_start)/2;
+		vwin2_line_s = vwin1_line_s + mode->vtotal/2;
+		vwin2_line_e = vwin2_line_s + mode->vdisplay/2;
+		vs2_pix_s    = vs2_pix_e = hs_pix_s + mode->htotal/2;
+		vs2_line_s   = vs1_line_s + mode->vtotal/2 ;
+		vs2_line_e   = vs2_line_s +
+			       (mode->vsync_end - mode->vsync_start)/2;
+	}
+
+	//tmds_clock = mode->clock;
+
+	/*
+	 * The divisor is power-of-2. The TDA9983B datasheet gives
+	 * this as ranges of Msample/s, which is 10x the TMDS clock:
+	 *   0 - 800 to 1500 Msample/s
+	 *   1 - 400 to 800 Msample/s
+	 *   2 - 200 to 400 Msample/s
+	 *   3 - as 2 above
+	 */
+#if 0
+	for (div = 0; div < 3; div++)
+		if (80000 >> div <= tmds_clock)
+			break;
+#endif
+
+	div = 0;
+
+	mutex_lock(&tda998x_mutex);
+
+	/* mute the audio FIFO: */
+	reg_set(priv, REG_AIP_CNTRL_0, AIP_CNTRL_0_RST_FIFO);
+
+	/* set HDMI HDCP mode off: */
+	reg_write(priv, REG_TBG_CNTRL_1, TBG_CNTRL_1_DWIN_DIS);
+	reg_clear(priv, REG_TX33, TX33_HDMI);
+	reg_write(priv, REG_ENC_CNTRL, ENC_CNTRL_CTL_CODE(0));
+
+	/* no pre-filter or interpolator: */
+	reg_write(priv, REG_HVF_CNTRL_0, HVF_CNTRL_0_PREFIL(0) |
+			HVF_CNTRL_0_INTPOL(0));
+	reg_set(priv, REG_FEAT_POWERDOWN, FEAT_POWERDOWN_PREFILT);
+	reg_write(priv, REG_VIP_CNTRL_5, VIP_CNTRL_5_SP_CNT(0));
+	reg_write(priv, REG_VIP_CNTRL_4, VIP_CNTRL_4_BLANKIT(0) |
+			VIP_CNTRL_4_BLC(0));
+
+	reg_clear(priv, REG_PLL_SERIAL_1, PLL_SERIAL_1_SRL_MAN_IZ);
+	reg_clear(priv, REG_PLL_SERIAL_3, PLL_SERIAL_3_SRL_CCIR |
+					  PLL_SERIAL_3_SRL_DE);
+	reg_write(priv, REG_SERIALIZER, 0);
+	reg_write(priv, REG_HVF_CNTRL_1, HVF_CNTRL_1_VQR(0));
+
+	/* TODO enable pixel repeat for pixel rates less than 25Msamp/s */
+	rep = 0;
+	reg_write(priv, REG_RPT_CNTRL, 0);
+	reg_write(priv, REG_SEL_CLK, SEL_CLK_SEL_VRF_CLK(0) |
+			SEL_CLK_SEL_CLK1 | SEL_CLK_ENA_SC_CLK);
+
+	reg_write(priv, REG_PLL_SERIAL_2, PLL_SERIAL_2_SRL_NOSC(div) |
+			PLL_SERIAL_2_SRL_PR(rep));
+
+	/* set color matrix bypass flag: */
+	reg_write(priv, REG_MAT_CONTRL, MAT_CONTRL_MAT_BP |
+				MAT_CONTRL_MAT_SC(1));
+	reg_set(priv, REG_FEAT_POWERDOWN, FEAT_POWERDOWN_CSC);
+
+	/* set BIAS tmds value: */
+	reg_write(priv, REG_ANA_GENERAL, 0x09);
+
+	/*
+	 * Sync on rising HSYNC/VSYNC
+	 */
+	reg = VIP_CNTRL_3_SYNC_HS;
+
+	/*
+	 * TDA19988 requires high-active sync at input stage,
+	 * so invert low-active sync provided by master encoder here
+	 */
+	if (mode->flags & DRM_MODE_FLAG_NHSYNC)
+		reg |= VIP_CNTRL_3_H_TGL;
+	if (mode->flags & DRM_MODE_FLAG_NVSYNC)
+		reg |= VIP_CNTRL_3_V_TGL;
+	//reg_write(priv, REG_VIP_CNTRL_3, reg);
+	reg_write(priv, REG_VIP_CNTRL_3, 0x26);
+	reg_write(priv, REG_VIDFORMAT, 0x06);
+
+	reg_write16(priv, REG_REFPIX_MSB, ref_pix);
+	reg_write16(priv, REG_REFLINE_MSB, ref_line);
+	reg_write16(priv, REG_NPIX_MSB, n_pix);
+	reg_write16(priv, REG_NLINE_MSB, n_line);
+	reg_write16(priv, REG_VS_LINE_STRT_1_MSB, vs1_line_s);
+	reg_write16(priv, REG_VS_PIX_STRT_1_MSB, vs1_pix_s);
+	reg_write16(priv, REG_VS_LINE_END_1_MSB, vs1_line_e);
+	reg_write16(priv, REG_VS_PIX_END_1_MSB, vs1_pix_e);
+	reg_write16(priv, REG_VS_LINE_STRT_2_MSB, vs2_line_s);
+	reg_write16(priv, REG_VS_PIX_STRT_2_MSB, vs2_pix_s);
+	reg_write16(priv, REG_VS_LINE_END_2_MSB, vs2_line_e);
+	reg_write16(priv, REG_VS_PIX_END_2_MSB, vs2_pix_e);
+	reg_write16(priv, REG_HS_PIX_START_MSB, hs_pix_s);
+	reg_write16(priv, REG_HS_PIX_STOP_MSB, hs_pix_e);
+	reg_write16(priv, REG_VWIN_START_1_MSB, vwin1_line_s);
+	reg_write16(priv, REG_VWIN_END_1_MSB, vwin1_line_e);
+	reg_write16(priv, REG_VWIN_START_2_MSB, vwin2_line_s);
+	reg_write16(priv, REG_VWIN_END_2_MSB, vwin2_line_e);
+	reg_write16(priv, REG_DE_START_MSB, de_pix_s);
+	reg_write16(priv, REG_DE_STOP_MSB, de_pix_e);
+
+	if (priv->rev == TDA19988) {
+		/* let incoming pixels fill the active space (if any) */
+		reg_write(priv, REG_ENABLE_SPACE, 0x00);
+	}
+
+	/*
+	 * Always generate sync polarity relative to input sync and
+	 * revert input stage toggled sync at output stage
+	 */
+	reg = TBG_CNTRL_1_DWIN_DIS | TBG_CNTRL_1_TGL_EN;
+	if (mode->flags & DRM_MODE_FLAG_NHSYNC)
+		reg |= TBG_CNTRL_1_H_TGL;
+	if (mode->flags & DRM_MODE_FLAG_NVSYNC)
+		reg |= TBG_CNTRL_1_V_TGL;
+	//reg_write(priv, REG_TBG_CNTRL_1, reg);
+	reg_write(priv, REG_TBG_CNTRL_1, 0x46);
+
+	/* must be last register set: */
+	reg_write(priv, REG_TBG_CNTRL_0, 0);
+
+#if 0
+	priv->tmds_clock = adjusted_mode->clock;
+
+	/* CEA-861B section 6 says that:
+	 * CEA version 1 (CEA-861) has no support for infoframes.
+	 * CEA version 2 (CEA-861A) supports version 1 AVI infoframes,
+	 * and optional basic audio.
+	 * CEA version 3 (CEA-861B) supports version 1 and 2 AVI infoframes,
+	 * and optional digital audio, with audio infoframes.
+	 *
+	 * Since we only support generation of version 2 AVI infoframes,
+	 * ignore CEA version 2 and below (iow, behave as if we're a
+	 * CEA-861 source.)
+	 */
+	priv->supports_infoframes = priv->connector.display_info.cea_rev >= 3;
+
+	if (priv->supports_infoframes) {
+		/* We need to turn HDMI HDCP stuff on to get audio through */
+		reg &= ~TBG_CNTRL_1_DWIN_DIS;
+		reg_write(priv, REG_TBG_CNTRL_1, reg);
+		reg_write(priv, REG_ENC_CNTRL, ENC_CNTRL_CTL_CODE(1));
+		reg_set(priv, REG_TX33, TX33_HDMI);
+
+		tda998x_write_avi(priv, adjusted_mode);
+
+		if (priv->audio_params.format != AFMT_UNUSED &&
+		    priv->sink_has_audio)
+			tda998x_configure_audio(priv, &priv->audio_params);
+	}
+#endif
+
+	mutex_unlock(&tda998x_mutex);
+}
+
+
+
+static void tda998x_bridge_mode_set(struct drm_bridge *bridge,
+					const struct drm_display_mode *mode,
+					const struct drm_display_mode *adjusted_mode)
+{
+	struct tda998x_priv *priv = bridge_to_tda998x_priv(bridge);
+	unsigned long tmds_clock;
+	u16 ref_pix, ref_line, n_pix, n_line;
+	u16 hs_pix_s, hs_pix_e;
+	u16 vs1_pix_s, vs1_pix_e, vs1_line_s, vs1_line_e;
+	u16 vs2_pix_s, vs2_pix_e, vs2_line_s, vs2_line_e;
+	u16 vwin1_line_s, vwin1_line_e;
+	u16 vwin2_line_s, vwin2_line_e;
+	u16 de_pix_s, de_pix_e;
+	u8 reg, div, rep;
+
+	/*
+	 * Internally TDA998x is using ITU-R BT.656 style sync but
+	 * we get VESA style sync. TDA998x is using a reference pixel
+	 * relative to ITU to sync to the input frame and for output
+	 * sync generation. Currently, we are using reference detection
+	 * from HS/VS, i.e. REFPIX/REFLINE denote frame start sync point
+	 * which is position of rising VS with coincident rising HS.
+	 *
+	 * Now there is some issues to take care of:
+	 * - HDMI data islands require sync-before-active
+	 * - TDA998x register values must be > 0 to be enabled
+	 * - REFLINE needs an additional offset of +1
+	 * - REFPIX needs an addtional offset of +1 for UYUV and +3 for RGB
+	 *
+	 * So we add +1 to all horizontal and vertical register values,
+	 * plus an additional +3 for REFPIX as we are using RGB input only.
+	 */
+	n_pix        = mode->htotal;
+	n_line       = mode->vtotal;
+
+	hs_pix_e     = mode->hsync_end - mode->hdisplay;
+	hs_pix_s     = mode->hsync_start - mode->hdisplay;
+	de_pix_e     = mode->htotal;
+	de_pix_s     = mode->htotal - mode->hdisplay;
+	ref_pix      = 3 + hs_pix_s;
+
+	/*
+	 * Attached LCD controllers may generate broken sync. Allow
+	 * those to adjust the position of the rising VS edge by adding
+	 * HSKEW to ref_pix.
+	 */
+	if (adjusted_mode->flags & DRM_MODE_FLAG_HSKEW)
+		ref_pix += adjusted_mode->hskew;
+
+	if ((mode->flags & DRM_MODE_FLAG_INTERLACE) == 0) {
+		ref_line     = 1 + mode->vsync_start - mode->vdisplay;
+		vwin1_line_s = mode->vtotal - mode->vdisplay - 1;
+		vwin1_line_e = vwin1_line_s + mode->vdisplay;
+		vs1_pix_s    = vs1_pix_e = hs_pix_s;
+		vs1_line_s   = mode->vsync_start - mode->vdisplay;
+		vs1_line_e   = vs1_line_s + mode->vsync_end - mode->vsync_start;
+		vwin2_line_s = vwin2_line_e = 0;
+		vs2_pix_s    = vs2_pix_e  = 0;
+		vs2_line_s   = vs2_line_e = 0;
+	} else {
+		ref_line     = 1 + (mode->vsync_start - mode->vdisplay)/2;
+		vwin1_line_s = (mode->vtotal - mode->vdisplay)/2;
+		vwin1_line_e = vwin1_line_s + mode->vdisplay/2;
+		vs1_pix_s    = vs1_pix_e = hs_pix_s;
+		vs1_line_s   = (mode->vsync_start - mode->vdisplay)/2;
+		vs1_line_e   = vs1_line_s + (mode->vsync_end - mode->vsync_start)/2;
+		vwin2_line_s = vwin1_line_s + mode->vtotal/2;
+		vwin2_line_e = vwin2_line_s + mode->vdisplay/2;
+		vs2_pix_s    = vs2_pix_e = hs_pix_s + mode->htotal/2;
+		vs2_line_s   = vs1_line_s + mode->vtotal/2 ;
+		vs2_line_e   = vs2_line_s + (mode->vsync_end - mode->vsync_start)/2;
+	}
+
+	tmds_clock = mode->clock;
+
+	/*
+	 * The divisor is power-of-2. The TDA9983B datasheet gives
+	 * this as ranges of Msample/s, which is 10x the TMDS clock:
+	 *   0 - 800 to 1500 Msample/s
+	 *   1 - 400 to 800 Msample/s
+	 *   2 - 200 to 400 Msample/s
+	 *   3 - as 2 above
+	 */
+	for (div = 0; div < 3; div++)
+		if (80000 >> div <= tmds_clock)
+			break;
+
+	mutex_lock(&priv->audio_mutex);
+
+	/* mute the audio FIFO: */
+	reg_set(priv, REG_AIP_CNTRL_0, AIP_CNTRL_0_RST_FIFO);
+
+	/* set HDMI HDCP mode off: */
+	reg_write(priv, REG_TBG_CNTRL_1, TBG_CNTRL_1_DWIN_DIS);
+	reg_clear(priv, REG_TX33, TX33_HDMI);
+	reg_write(priv, REG_ENC_CNTRL, ENC_CNTRL_CTL_CODE(0));
+
+	/* no pre-filter or interpolator: */
+	reg_write(priv, REG_HVF_CNTRL_0, HVF_CNTRL_0_PREFIL(0) |
+			HVF_CNTRL_0_INTPOL(0));
+	reg_set(priv, REG_FEAT_POWERDOWN, FEAT_POWERDOWN_PREFILT);
+	reg_write(priv, REG_VIP_CNTRL_5, VIP_CNTRL_5_SP_CNT(0));
+	reg_write(priv, REG_VIP_CNTRL_4, VIP_CNTRL_4_BLANKIT(0) |
+			VIP_CNTRL_4_BLC(0));
+
+	reg_clear(priv, REG_PLL_SERIAL_1, PLL_SERIAL_1_SRL_MAN_IZ);
+	reg_clear(priv, REG_PLL_SERIAL_3, PLL_SERIAL_3_SRL_CCIR |
+			PLL_SERIAL_3_SRL_DE);
+	reg_write(priv, REG_SERIALIZER, 0);
+	reg_write(priv, REG_HVF_CNTRL_1, HVF_CNTRL_1_VQR(0));
+
+	/* TODO enable pixel repeat for pixel rates less than 25Msamp/s */
+	rep = 0;
+	reg_write(priv, REG_RPT_CNTRL, 0);
+	reg_write(priv, REG_SEL_CLK, SEL_CLK_SEL_VRF_CLK(0) |
+			SEL_CLK_SEL_CLK1 | SEL_CLK_ENA_SC_CLK);
+
+	reg_write(priv, REG_PLL_SERIAL_2, PLL_SERIAL_2_SRL_NOSC(div) |
+			PLL_SERIAL_2_SRL_PR(rep));
+
+	/* set color matrix bypass flag: */
+	reg_write(priv, REG_MAT_CONTRL, MAT_CONTRL_MAT_BP |
+			MAT_CONTRL_MAT_SC(1));
+	reg_set(priv, REG_FEAT_POWERDOWN, FEAT_POWERDOWN_CSC);
+
+	/* set BIAS tmds value: */
+	reg_write(priv, REG_ANA_GENERAL, 0x09);
+
+	/*
+	 * Sync on rising HSYNC/VSYNC
+	 */
+	reg = VIP_CNTRL_3_SYNC_HS;
+
+	/*
+	 * TDA19988 requires high-active sync at input stage,
+	 * so invert low-active sync provided by master encoder here
+	 */
+	if (mode->flags & DRM_MODE_FLAG_NHSYNC)
+		reg |= VIP_CNTRL_3_H_TGL;
+	if (mode->flags & DRM_MODE_FLAG_NVSYNC)
+		reg |= VIP_CNTRL_3_V_TGL;
+	reg_write(priv, REG_VIP_CNTRL_3, reg);
+
+	reg_write(priv, REG_VIDFORMAT, 0x00);
+	reg_write16(priv, REG_REFPIX_MSB, ref_pix);
+	reg_write16(priv, REG_REFLINE_MSB, ref_line);
+	reg_write16(priv, REG_NPIX_MSB, n_pix);
+	reg_write16(priv, REG_NLINE_MSB, n_line);
+	reg_write16(priv, REG_VS_LINE_STRT_1_MSB, vs1_line_s);
+	reg_write16(priv, REG_VS_PIX_STRT_1_MSB, vs1_pix_s);
+	reg_write16(priv, REG_VS_LINE_END_1_MSB, vs1_line_e);
+	reg_write16(priv, REG_VS_PIX_END_1_MSB, vs1_pix_e);
+	reg_write16(priv, REG_VS_LINE_STRT_2_MSB, vs2_line_s);
+	reg_write16(priv, REG_VS_PIX_STRT_2_MSB, vs2_pix_s);
+	reg_write16(priv, REG_VS_LINE_END_2_MSB, vs2_line_e);
+	reg_write16(priv, REG_VS_PIX_END_2_MSB, vs2_pix_e);
+	reg_write16(priv, REG_HS_PIX_START_MSB, hs_pix_s);
+	reg_write16(priv, REG_HS_PIX_STOP_MSB, hs_pix_e);
+	reg_write16(priv, REG_VWIN_START_1_MSB, vwin1_line_s);
+	reg_write16(priv, REG_VWIN_END_1_MSB, vwin1_line_e);
+	reg_write16(priv, REG_VWIN_START_2_MSB, vwin2_line_s);
+	reg_write16(priv, REG_VWIN_END_2_MSB, vwin2_line_e);
+	reg_write16(priv, REG_DE_START_MSB, de_pix_s);
+	reg_write16(priv, REG_DE_STOP_MSB, de_pix_e);
+
+	if (priv->rev == TDA19988) {
+		/* let incoming pixels fill the active space (if any) */
+		reg_write(priv, REG_ENABLE_SPACE, 0x00);
+	}
+
+	/*
+	 * Always generate sync polarity relative to input sync and
+	 * revert input stage toggled sync at output stage
+	 */
+	reg = TBG_CNTRL_1_DWIN_DIS | TBG_CNTRL_1_TGL_EN;
+	if (mode->flags & DRM_MODE_FLAG_NHSYNC)
+		reg |= TBG_CNTRL_1_H_TGL;
+	if (mode->flags & DRM_MODE_FLAG_NVSYNC)
+		reg |= TBG_CNTRL_1_V_TGL;
+	reg_write(priv, REG_TBG_CNTRL_1, reg);
+
+	/* must be last register set: */
+	reg_write(priv, REG_TBG_CNTRL_0, 0);
+
+	priv->tmds_clock = adjusted_mode->clock;
+
+	/* CEA-861B section 6 says that:
+	 * CEA version 1 (CEA-861) has no support for infoframes.
+	 * CEA version 2 (CEA-861A) supports version 1 AVI infoframes,
+	 * and optional basic audio.
+	 * CEA version 3 (CEA-861B) supports version 1 and 2 AVI infoframes,
+	 * and optional digital audio, with audio infoframes.
+	 *
+	 * Since we only support generation of version 2 AVI infoframes,
+	 * ignore CEA version 2 and below (iow, behave as if we're a
+	 * CEA-861 source.)
+	 */
+	priv->supports_infoframes = priv->connector.display_info.cea_rev >= 3;
+
+	if (priv->supports_infoframes) {
+		/* We need to turn HDMI HDCP stuff on to get audio through */
+		reg &= ~TBG_CNTRL_1_DWIN_DIS;
+		reg_write(priv, REG_TBG_CNTRL_1, reg);
+		reg_write(priv, REG_ENC_CNTRL, ENC_CNTRL_CTL_CODE(1));
+		reg_set(priv, REG_TX33, TX33_HDMI);
+
+		tda998x_write_avi(priv, adjusted_mode);
+
+		if (priv->audio_params.format != AFMT_UNUSED && priv->sink_has_audio)
+			tda998x_configure_audio(priv, &priv->audio_params);
+	}
+
+	mutex_unlock(&priv->audio_mutex);
+}
+
+static const struct drm_bridge_funcs tda998x_bridge_funcs = {
+	.attach = tda998x_bridge_attach,
+	.detach = tda998x_bridge_detach,
+	.mode_valid = tda998x_bridge_mode_valid,
+	.disable = tda998x_bridge_disable,
+	.mode_set = tda998x_bridge_mode_set,
+	.enable = tda998x_bridge_enable,
+};
+
+/* I2C driver functions */
+
+static int tda998x_get_audio_ports(struct tda998x_priv *priv,
+			struct device_node *np)
+{
+	const u32 *port_data;
+	u32 size;
+	int i;
+
+	port_data = of_get_property(np, "audio-ports", &size);
+	if (!port_data)
+		return 0;
+
+	size /= sizeof(u32);
+	if (size > 2 * ARRAY_SIZE(priv->audio_port) || size % 2 != 0) {
+		dev_err(&priv->hdmi->dev,
+			"Bad number of elements in audio-ports dt-property\n");
+		return -EINVAL;
+	}
+
+	size /= 2;
+
+	for (i = 0; i < size; i++) {
+		u8 afmt = be32_to_cpup(&port_data[2*i]);
+		u8 ena_ap = be32_to_cpup(&port_data[2*i+1]);
+
+		if (afmt != AFMT_SPDIF && afmt != AFMT_I2S) {
+			dev_err(&priv->hdmi->dev,
+				"Bad audio format %u\n", afmt);
+			return -EINVAL;
+		}
+
+		priv->audio_port[i].format = afmt;
+		priv->audio_port[i].config = ena_ap;
+	}
+
+	if (priv->audio_port[0].format == priv->audio_port[1].format) {
+		dev_err(&priv->hdmi->dev,
+			"There can only be on I2S port and one SPDIF port\n");
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static void tda998x_set_config(struct tda998x_priv *priv,
+				const struct tda998x_encoder_params *p)
+{
+	priv->vip_cntrl_0 = VIP_CNTRL_0_SWAP_A(p->swap_a) |
+			(p->mirr_a ? VIP_CNTRL_0_MIRR_A : 0) |
+			VIP_CNTRL_0_SWAP_B(p->swap_b) |
+			(p->mirr_b ? VIP_CNTRL_0_MIRR_B : 0);
+	priv->vip_cntrl_1 = VIP_CNTRL_1_SWAP_C(p->swap_c) |
+			(p->mirr_c ? VIP_CNTRL_1_MIRR_C : 0) |
+			VIP_CNTRL_1_SWAP_D(p->swap_d) |
+			(p->mirr_d ? VIP_CNTRL_1_MIRR_D : 0);
+	priv->vip_cntrl_2 = VIP_CNTRL_2_SWAP_E(p->swap_e) |
+			(p->mirr_e ? VIP_CNTRL_2_MIRR_E : 0) |
+			VIP_CNTRL_2_SWAP_F(p->swap_f) |
+			(p->mirr_f ? VIP_CNTRL_2_MIRR_F : 0);
+	priv->audio_params = p->audio_params;
+}
+
+static void tda998x_destroy(struct device *dev)
+{
+	struct tda998x_priv *priv = dev_get_drvdata(dev);
+
+	drm_bridge_remove(&priv->bridge);
+
+	/* disable all IRQs and free the IRQ handler */
+	cec_write(priv, REG_CEC_RXSHPDINTENA, 0);
+	reg_clear(priv, REG_INT_FLAGS_2, INT_FLAGS_2_EDID_BLK_RD);
+
+	if (priv->audio_pdev)
+		platform_device_unregister(priv->audio_pdev);
+
+	if (priv->hdmi->irq)
+		free_irq(priv->hdmi->irq, priv);
+
+	del_timer_sync(&priv->edid_delay_timer);
+	cancel_work_sync(&priv->detect_work);
+
+	i2c_unregister_device(priv->cec);
+
+	cec_notifier_conn_unregister(priv->cec_notify);
+}
+
+static int tda998x_create(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct device_node *np = client->dev.of_node;
+	struct i2c_board_info cec_info;
+	struct tda998x_priv *priv;
+	u32 video;
+	int rev_lo, rev_hi, ret;
+
+	priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	dev_set_drvdata(dev, priv);
+
+	mutex_init(&priv->mutex);	/* protect the page access */
+	mutex_init(&priv->audio_mutex); /* protect access from audio thread */
+	mutex_init(&priv->edid_mutex);
+	INIT_LIST_HEAD(&priv->bridge.list);
+	init_waitqueue_head(&priv->edid_delay_waitq);
+	timer_setup(&priv->edid_delay_timer, tda998x_edid_delay_done, 0);
+	INIT_WORK(&priv->detect_work, tda998x_detect_work);
+
+	priv->vip_cntrl_0 = VIP_CNTRL_0_SWAP_A(2) | VIP_CNTRL_0_SWAP_B(3);
+	priv->vip_cntrl_1 = VIP_CNTRL_1_SWAP_C(0) | VIP_CNTRL_1_SWAP_D(1);
+	priv->vip_cntrl_2 = VIP_CNTRL_2_SWAP_E(4) | VIP_CNTRL_2_SWAP_F(5);
+
+	/* CEC I2C address bound to TDA998x I2C addr by configuration pins */
+	priv->cec_addr = 0x34 + (client->addr & 0x03);
+	priv->current_page = 0xff;
+	priv->hdmi = client;
+
+	/* wake up the device: */
+	ret = cec_write(priv, REG_CEC_ENAMODS,
+			CEC_ENAMODS_EN_RXSENS | CEC_ENAMODS_EN_HDMI);
+	if (ret < 0)
+		return ret;
+
+	tda998x_reset(priv);
+
+	/* read version: */
+	rev_lo = reg_read(priv, REG_VERSION_LSB);
+	if (rev_lo < 0) {
+		dev_err(dev, "failed to read version: %d\n", rev_lo);
+		return rev_lo;
+	}
+
+	rev_hi = reg_read(priv, REG_VERSION_MSB);
+	if (rev_hi < 0) {
+		dev_err(dev, "failed to read version: %d\n", rev_hi);
+		return rev_hi;
+	}
+
+	priv->rev = rev_lo | rev_hi << 8;
+
+	/* mask off feature bits: */
+	priv->rev &= ~0x30; /* not-hdcp and not-scalar bit */
+
+	switch (priv->rev) {
+	case TDA9989N2:
+		dev_info(dev, "found TDA9989 n2");
+		break;
+	case TDA19989:
+		dev_info(dev, "found TDA19989");
+		break;
+	case TDA19989N2:
+		dev_info(dev, "found TDA19989 n2");
+		break;
+	case TDA19988:
+		dev_info(dev, "found TDA19988");
+		break;
+	default:
+		dev_err(dev, "found unsupported device: %04x\n", priv->rev);
+		return -ENXIO;
+	}
+
+	/* after reset, enable DDC: */
+	reg_write(priv, REG_DDC_DISABLE, 0x00);
+
+	/* set clock on DDC channel: */
+	reg_write(priv, REG_TX3, 39);
+
+	/* if necessary, disable multi-master: */
+	if (priv->rev == TDA19989)
+		reg_set(priv, REG_I2C_MASTER, I2C_MASTER_DIS_MM);
+
+	cec_write(priv, REG_CEC_FRO_IM_CLK_CTRL,
+			CEC_FRO_IM_CLK_CTRL_GHOST_DIS | CEC_FRO_IM_CLK_CTRL_IMCLK_SEL);
+
+	/* ensure interrupts are disabled */
+	cec_write(priv, REG_CEC_RXSHPDINTENA, 0);
+
+	/* clear pending interrupts */
+	cec_read(priv, REG_CEC_RXSHPDINT);
+	reg_read(priv, REG_INT_FLAGS_0);
+	reg_read(priv, REG_INT_FLAGS_1);
+	reg_read(priv, REG_INT_FLAGS_2);
+
+	/* initialize the optional IRQ */
+	if (client->irq) {
+		unsigned long irq_flags;
+
+		/* init read EDID waitqueue and HDP work */
+		init_waitqueue_head(&priv->wq_edid);
+
+		irq_flags =
+			irqd_get_trigger_type(irq_get_irq_data(client->irq));
+
+		priv->cec_glue.irq_flags = irq_flags;
+
+		irq_flags |= IRQF_SHARED | IRQF_ONESHOT;
+		ret = request_threaded_irq(client->irq, NULL, tda998x_irq_thread,
+				irq_flags, "tda998x", priv);
+		if (ret) {
+			dev_err(dev, "failed to request IRQ#%u: %d\n", client->irq, ret);
+			goto err_irq;
+		}
+
+		/* enable HPD irq */
+		cec_write(priv, REG_CEC_RXSHPDINTENA, CEC_RXSHPDLEV_HPD);
+	}
+
+	priv->cec_notify = cec_notifier_conn_register(dev, NULL, NULL);
+	if (!priv->cec_notify) {
+		ret = -ENOMEM;
+		goto fail;
+	}
+
+	priv->cec_glue.parent = dev;
+	priv->cec_glue.data = priv;
+	priv->cec_glue.init = tda998x_cec_hook_init;
+	priv->cec_glue.exit = tda998x_cec_hook_exit;
+	priv->cec_glue.open = tda998x_cec_hook_open;
+	priv->cec_glue.release = tda998x_cec_hook_release;
+
+	/*
+	 * Some TDA998x are actually two I2C devices merged onto one piece
+	 * of silicon: TDA9989 and TDA19989 combine the HDMI transmitter
+	 * with a slightly modified TDA9950 CEC device.  The CEC device
+	 * is at the TDA9950 address, with the address pins strapped across
+	 * to the TDA998x address pins.  Hence, it always has the same
+	 * offset.
+	 */
+	memset(&cec_info, 0, sizeof(cec_info));
+	strlcpy(cec_info.type, "tda9950", sizeof(cec_info.type));
+	cec_info.addr = priv->cec_addr;
+	cec_info.platform_data = &priv->cec_glue;
+	cec_info.irq = client->irq;
+
+	priv->cec = i2c_new_client_device(client->adapter, &cec_info);
+	if (!priv->cec) {
+		ret = -ENODEV;
+		goto fail;
+	}
+
+	/* enable EDID read irq: */
+	reg_set(priv, REG_INT_FLAGS_2, INT_FLAGS_2_EDID_BLK_RD);
+
+	if (np) {
+		/* get the device tree parameters */
+		ret = of_property_read_u32(np, "video-ports", &video);
+		if (ret == 0) {
+			priv->vip_cntrl_0 = video >> 16;
+			priv->vip_cntrl_1 = video >> 8;
+			priv->vip_cntrl_2 = video;
+		}
+
+		ret = tda998x_get_audio_ports(priv, np);
+		if (ret)
+			goto fail;
+
+		if (priv->audio_port[0].format != AFMT_UNUSED)
+			tda998x_audio_codec_init(priv, &client->dev);
+	} else if (dev->platform_data) {
+		tda998x_set_config(priv, dev->platform_data);
+	}
+
+	priv->bridge.funcs = &tda998x_bridge_funcs;
+#ifdef CONFIG_OF
+	priv->bridge.of_node = dev->of_node;
+#endif
+
+	drm_bridge_add(&priv->bridge);
+
+	return 0;
+
+fail:
+	tda998x_destroy(dev);
+err_irq:
+	return ret;
+}
+
+/* DRM encoder functions */
+
+static void tda998x_encoder_destroy(struct drm_encoder *encoder)
+{
+	drm_encoder_cleanup(encoder);
+}
+
+static const struct drm_encoder_funcs tda998x_encoder_funcs = {
+	.destroy = tda998x_encoder_destroy,
+};
+
+static int tda998x_encoder_init(struct device *dev, struct drm_device *drm)
+{
+	struct tda998x_priv *priv = dev_get_drvdata(dev);
+	u32 crtcs = 0;
+	int ret;
+
+	if (dev->of_node)
+		crtcs = drm_of_find_possible_crtcs(drm, dev->of_node);
+
+	/* If no CRTCs were found, fall back to our old behaviour */
+	if (crtcs == 0) {
+		dev_warn(dev, "Falling back to first CRTC\n");
+		crtcs = 1 << 0;
+	}
+
+	priv->encoder.possible_crtcs = crtcs;
+
+	ret = drm_encoder_init(drm, &priv->encoder, &tda998x_encoder_funcs,
+			       DRM_MODE_ENCODER_TMDS, NULL);
+	if (ret)
+		goto err_encoder;
+
+	ret = drm_bridge_attach(&priv->encoder, &priv->bridge, NULL, 0);
+	if (ret)
+		goto err_bridge;
+
+	return 0;
+
+err_bridge:
+	drm_encoder_cleanup(&priv->encoder);
+err_encoder:
+	return ret;
+}
+
+static int tda998x_bind(struct device *dev, struct device *master, void *data)
+{
+	struct drm_device *drm = data;
+
+	return tda998x_encoder_init(dev, drm);
+}
+
+static void tda998x_unbind(struct device *dev, struct device *master, void *data)
+{
+	struct tda998x_priv *priv = dev_get_drvdata(dev);
+
+	drm_encoder_cleanup(&priv->encoder);
+}
+
+static const struct component_ops tda998x_ops = {
+	.bind = tda998x_bind,
+	.unbind = tda998x_unbind,
+};
+
+static int tda998x_probe(struct i2c_client *client, const struct i2c_device_id *id)
+{
+	int ret;
+	struct sf_fb_display_dev *display_dev = NULL;
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		dev_warn(&client->dev, "adapter does not support I2C\n");
+		return -EIO;
+	}
+
+	display_dev = sf_fb_display_dev_get_by_name("tda_998x_1080p");
+	if(NULL == display_dev) {
+		dev_err(&client->dev, "can not get dev information\n");
+		return -ENODEV;
+	}
+
+	ret = tda998x_create(&client->dev);
+	if (ret) {
+		dev_err(&client->dev, "tda998x is not ready!\n");
+		return 0;
+	}
+
+	ret = component_add(&client->dev, &tda998x_ops);
+	if (ret)
+		tda998x_destroy(&client->dev);
+
+	tda998x_mode_set(&client->dev, display_dev);
+	tda998x_enable(&client->dev);
+
+	return ret;
+}
+
+static int tda998x_remove(struct i2c_client *client)
+{
+	component_del(&client->dev, &tda998x_ops);
+	tda998x_destroy(&client->dev);
+
+	return 0;
+}
+
+#ifdef CONFIG_OF
+static const struct of_device_id tda998x_dt_ids[] = {
+	{ .compatible = "nxp,tda998x", },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, tda998x_dt_ids);
+#endif
+
+static const struct i2c_device_id tda998x_ids[] = {
+	{ "tda998x", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, tda998x_ids);
+
+static struct i2c_driver tda998x_driver = {
+	.probe = tda998x_probe,
+	.remove = tda998x_remove,
+	.driver = {
+		.name = "tda998x",
+		.of_match_table = of_match_ptr(tda998x_dt_ids),
+	},
+	.id_table = tda998x_ids,
+};
+
+module_i2c_driver(tda998x_driver);
+MODULE_AUTHOR("StarFive Technology Co., Ltd.");
+MODULE_DESCRIPTION("loadable tda998x driver for StarFive");
+MODULE_LICENSE("GPL");
diff --git a/include/dt-bindings/starfive_fb.h b/include/dt-bindings/starfive_fb.h
new file mode 100644
index 000000000000..a7e014d61b29
--- /dev/null
+++ b/include/dt-bindings/starfive_fb.h
@@ -0,0 +1,47 @@
+#ifndef __STARFIVE_FB_H
+#define __STARFIVE_FB_H
+
+/*color code*/
+#define COLOR_CODE_16BIT_CONFIG1	0	//PACKET RGB565
+#define	COLOR_CODE_16BIT_CONFIG2	1	//UNPACKET RGB565
+#define	COLOR_CODE_16BIT_CONFIG3	2	//UNPACKET RGB565
+#define	COLOR_CODE_18BIT_CONFIG1	3	//PACKET RGB666
+#define	COLOR_CODE_18BIT_CONFIG2	4	//UNPACKET RGB666
+#define	COLOR_CODE_24BIT		5	//PACKET RGB888
+#define	COLOR_CODE_MAX			6
+
+/*command code*/
+#define	DCS_CMD				02
+#define	GEN_CMD				03
+#define	SW_PACK0			04
+#define	SW_PACK1			05
+#define	SW_PACK2			06
+#define	LW_PACK				07
+#define	SHUTDOWN_SW_PACK		08
+
+/*color format, need match to enum COLOR_FORMAT in starfive_vpp.h*/
+#define	COLOR_YUV422_UYVY	0
+#define	COLOR_YUV422_VYUY	1
+#define	COLOR_YUV422_YUYV	2
+#define	COLOR_YUV422_YVYU	3
+#define	COLOR_YUV420P	4
+#define	COLOR_YUV420_NV21	5
+#define	COLOR_YUV420_NV12	6
+#define	COLOR_RGB888_ARGB	7
+#define	COLOR_RGB888_ABGR	8
+#define	COLOR_RGB888_RGBA	9
+#define	COLOR_RGB888_BGRA	10
+#define	COLOR_RGB565	11
+
+#define SRC_COLORBAR_VIN_ISP  0
+#define SRC_DVP_SENSOR_VIN    1
+#define SRC_DVP_SENSOR_VIN_ISP 2
+#define SRC_CSI2RX_VIN_ISP    3
+#define SRC_DVP_SENSOR_VIN_OV5640 4
+
+#define    WIN_FMT_RGB565		4
+#define    WIN_FMT_xRGB1555		5
+#define    WIN_FMT_xRGB4444		6
+#define    WIN_FMT_xRGB8888		7
+
+#endif
diff --git a/include/video/stf-vin.h b/include/video/stf-vin.h
new file mode 100644
index 000000000000..e96fedef82f9
--- /dev/null
+++ b/include/video/stf-vin.h
@@ -0,0 +1,307 @@
+/* include/video/stf-vin.h
+ *
+ * Copyright 2020 starfive tech.
+ *	Eric Tang <eric.tang@starfivetech.com>
+ *
+ * Generic vin notifier interface
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+*/
+#ifndef _VIDEO_VIN_H
+#define _VIDEO_VIN_H
+
+#include <linux/cdev.h>
+
+#define DRV_NAME "stf-vin"
+#define FB_FIRST_ADDR      0xf9000000
+#define FB_SECOND_ADDR     0xf97e9000
+
+#define RESERVED_MEM_SIZE  0x1000000
+
+#define VIN_MIPI_CONTROLLER0_OFFSET 0x00000
+#define VIN_CLKGEN_OFFSET           0x10000
+#define VIN_RSTGEN_OFFSET           0x20000
+#define VIN_MIPI_CONTROLLER1_OFFSET 0x30000
+#define VIN_SYSCONTROLLER_OFFSET    0x40000
+
+#define VD_1080P    1080
+#define VD_720P     720
+#define VD_PAL      480
+
+#define VD_HEIGHT_1080P     VD_1080P
+#define VD_WIDTH_1080P      1920
+
+#define VD_HEIGHT_720P      VD_720P
+#define VD_WIDTH_720P       1080
+
+#define VD_HEIGHT_480       480
+#define VD_WIDTH_640        640
+
+#define SEEED_WIDTH_800       800
+#define SEEED_HIGH_480        480
+
+#define VIN_TOP_CLKGEN_BASE_ADDR	    0x11800000
+#define VIN_TOP_RSTGEN_BASE_ADDR	    0x11840000
+#define VIN_TOP_IOPAD_BASE_ADDR	        0x11858000
+
+#define ISP_BASE_MIPI0_ADDR             0x19800000
+#define ISP_BASE_CLKGEN_ADDR            0x19810000
+#define ISP_BASE_RSTGEN_ADDR            0x19820000
+#define ISP_BASE_MIPI1_ADDR             0x19830000
+#define ISP_BASE_SYSCTRL_ADDR	        0x19840000
+#define ISP_BASE_ISP0_ADDR	            0x19870000
+#define ISP_BASE_ISP1_ADDR	            0x198a0000
+
+
+//vin clk registers
+#define CLK_VIN_SRC_CTRL		    0x188
+#define CLK_ISP0_AXI_CTRL		    0x190
+#define CLK_ISP0NOC_AXI_CTRL	    0x194
+#define CLK_ISPSLV_AXI_CTRL		    0x198
+#define CLK_ISP1_AXI_CTRL		    0x1A0
+#define CLK_ISP1NOC_AXI_CTRL	    0x1A4
+#define CLK_VIN_AXI		            0x1AC
+#define CLK_VINNOC_AXI		        0x1B0
+
+//isp clk registers
+#define CLK_DPHY_CFGCLK_ISPCORE_2X_CTRL    0x00
+#define CLK_DPHY_REFCLK_ISPCORE_2X_CTRL    0x04
+#define CLK_DPHY_TXCLKESC_IN_CTRL          0x08
+#define CLK_MIPI_RX0_PXL_CTRL       0x0c
+#define CLK_MIPI_RX1_PXL_CTRL       0x10
+#define CLK_MIPI_RX0_PXL_0_CTRL     0X14
+#define CLK_MIPI_RX0_PXL_1_CTRL     0X18
+#define CLK_MIPI_RX0_PXL_2_CTRL     0X1C
+#define CLK_MIPI_RX0_PXL_3_CTRL     0X20
+#define CLK_MIPI_RX0_SYS0_CTRL      0x24
+#define CLK_MIPI_RX1_PXL_0_CTRL     0X28
+#define CLK_MIPI_RX1_PXL_1_CTRL     0X2C
+#define CLK_MIPI_RX1_PXL_2_CTRL     0X30
+#define CLK_MIPI_RX1_PXL_3_CTRL     0X34
+#define CLK_MIPI_RX1_SYS1_CTRL      0x38
+#define CLK_ISP0_CTRL               0x3c
+#define CLK_ISP0_2X_CTRL            0x40
+#define CLK_ISP0_MIPI_CTRL          0x44
+#define CLK_C_ISP0_CTRL             0x64
+#define CLK_ISP1_CTRL               0x48
+#define CLK_ISP1_2X_CTRL            0x4C
+#define CLK_ISP1_MIPI_CTRL          0x50
+#define CLK_C_ISP1_CTRL             0x68
+#define CLK_CSI2RX0_APB_CTRL        0x58
+
+
+#define CLK_VIN_AXI_WR_CTRL         0x5C
+
+#define SOFTWARE_RESET_ASSERT0		0x0
+#define SOFTWARE_RESET_ASSERT1		0x4
+#define SOFTWARE_RESET_STATUS		0x4
+
+#define IOPAD_REG81		        0x144
+#define IOPAD_REG82		        0x148
+#define IOPAD_REG83		        0x14C
+#define IOPAD_REG84		        0x150
+#define IOPAD_REG85		        0x154
+#define IOPAD_REG86		        0x158
+#define IOPAD_REG87	            0x15C
+#define IOPAD_REG88	            0x160
+#define IOPAD_REG89	            0x164
+
+//sys control REG DEFINE
+#define SYSCTRL_REG4	            0x10
+#define SYSCTRL_DPHY_CTRL	        0x14
+#define SYSCTRL_VIN_AXI_CTRL	    0x18
+#define SYSCTRL_VIN_WR_START_ADDR	0x28
+#define SYSCTRL_VIN_RD_END_ADDR	    0x2C
+#define SYSCTRL_VIN_WR_PIX_TOTAL	0x30
+#define SYSCTRL_VIN_RD_PIX_TOTAL	0x34
+#define SYSCTRL_VIN_RW_CTRL	        0x38
+#define SYSCTRL_VIN_SRC_CHAN_SEL	0x3C
+#define SYSCTRL_VIN_SRC_DW_SEL	    0x40
+#define SYSCTRL_VIN_RD_VBLANK	    0x44
+#define SYSCTRL_VIN_RD_VEND	        0x48
+#define SYSCTRL_VIN_RD_HBLANK	    0x4C
+#define SYSCTRL_VIN_RD_HEND	        0x50
+#define SYSCTRL_VIN_INTP_CTRL	    0x54
+
+#define ISP_NO_SCALE_ENABLE     (0x1<<20)
+#define ISP_MULTI_FRAME_ENABLE  (0x1<<17)
+#define ISP_SS0_ENABLE          (0x1<<11)
+#define ISP_SS1_ENABLE          (0x1<<12)
+#define ISP_RESET               (0x1<<1)
+#define ISP_ENBALE              (0x1)
+
+
+
+ //ISP REG DEFINE
+#define ISP_REG_DVP_POLARITY_CFG            0x00000014
+#define ISP_REG_RAW_FORMAT_CFG              0x00000018
+#define ISP_REG_CFA_MODE                    0x00000A1C
+#define ISP_REG_PIC_CAPTURE_START_CFG       0x0000001C
+#define ISP_REG_PIC_CAPTURE_END_CFG         0x00000020
+#define ISP_REG_PIPELINE_XY_SIZE            0x00000A0C
+#define ISP_REG_Y_PLANE_START_ADDR          0x00000A80
+#define ISP_REG_UV_PLANE_START_ADDR         0x00000A84
+#define ISP_REG_STRIDE                      0x00000A88
+#define ISP_REG_PIXEL_COORDINATE_GEN        0x00000A8C
+#define ISP_REG_PIXEL_AXI_CONTROL           0x00000A90
+#define ISP_REG_SS_AXI_CONTROL              0x00000AC4
+#define ISP_REG_RGB_TO_YUV_COVERSION0       0x00000E40
+#define ISP_REG_RGB_TO_YUV_COVERSION1       0x00000E44
+#define ISP_REG_RGB_TO_YUV_COVERSION2       0x00000E48
+#define ISP_REG_RGB_TO_YUV_COVERSION3       0x00000E4C
+#define ISP_REG_RGB_TO_YUV_COVERSION4       0x00000E50
+#define ISP_REG_RGB_TO_YUV_COVERSION5       0x00000E54
+#define ISP_REG_RGB_TO_YUV_COVERSION6       0x00000E58
+#define ISP_REG_RGB_TO_YUV_COVERSION7       0x00000E5C
+#define ISP_REG_RGB_TO_YUV_COVERSION8       0x00000E60
+#define ISP_REG_CIS_MODULE_CFG              0x00000010
+#define ISP_REG_ISP_CTRL_1                  0x00000A08
+#define ISP_REG_ISP_CTRL_0                  0x00000A00
+#define ISP_REG_DC_AXI_ID                   0x00000044
+#define ISP_REG_CSI_INPUT_EN_AND_STATUS     0x00000000
+
+//CSI registers
+#define DEVICE_CONFIG           0x00
+#define SOFT_RESET              0x04
+#define STATIC_CFG              0x08
+#define ERROR_BYPASS_CFG        0x10
+#define MONITOR_IRQS            0x18
+#define MONITOR_IRQS_MASK_CFG   0x1c
+#define INFO_IRQS               0x20
+#define INFO_IRQS_MASK_CFG      0x24
+#define ERROR_IRQS              0x28
+#define ERROR_IRQS_MASK_CFG     0x2c
+#define DPHY_LANE_CONTROL       0x40
+#define DPHY_STATUS             0x48
+#define DPHY_ERR_STATUS_IRQ     0x4C
+#define DPHY_ERR_IRQ_MASK_CFG   0x50
+#define INTEGRATION_DEBUG       0x60
+#define ERROR_DEBUG             0x74
+
+#define STREAM0_CTRL            0x100
+#define STREAM0_STATUS          0x104
+#define STREAM0_DATA_CFG        0x108
+#define STREAM0_CFG             0x10c
+#define STREAM0_MONITOR_CTRL    0x110
+#define STREAM0_MONITOR_FRAME   0x114
+#define STREAM0_MONITOR_LB      0x118
+#define STREAM0_TIMER           0x11c
+#define STREAM0_FCC_CFG         0x120
+#define STREAM0_FCC_CTRL        0x124
+#define STREAM0_FIFO_FILL_LVL   0x128
+
+typedef enum
+{
+    DT_RAW6  = 0x28,
+    DT_RAW7  = 0x29,
+    DT_RAW8  = 0x2a,
+    DT_RAW10 = 0x2b,
+    DT_RAW12 = 0x2c,
+    DT_RAW14 = 0x2d,
+} mipicam_data_type_t;
+
+
+enum VIN_SOURCE_FORMAT {
+	SRC_COLORBAR_VIN_ISP = 0,
+	SRC_DVP_SENSOR_VIN,
+	SRC_DVP_SENSOR_VIN_ISP,//need replace sensor
+	SRC_CSI2RX_VIN_ISP,
+	SRC_DVP_SENSOR_VIN_OV5640,
+};
+
+struct reg_name {
+	char name[10];
+};
+
+typedef struct
+{
+    int dlane_nb;
+    int dlane_map[4];
+    int dlane_en[4];
+    int dlane_pn_swap[4];
+    int clane_nb;
+    int clane_map[2];
+    int clane_pn_swap[2];
+} csi2rx_dphy_cfg_t;
+
+typedef struct
+{
+    int lane_nb;
+    int dlane_map[4];
+    int dt;
+    int hsize;
+    int vsize;
+} csi2rx_cfg_t;
+
+
+typedef struct
+{
+    int mipi_id, w, h, dt, bpp, fps,lane;
+	u8  clane_swap;
+    u8  clane_pn_swap;
+    u8  dlane_swap[4];
+    u8  dlane_pn_swap[4];
+} csi_format;
+
+struct vin_params {
+	void *paddr;
+	unsigned long size;
+};
+
+struct vin_buf {
+	void *vaddr;
+	dma_addr_t paddr;
+	u32 size;
+};
+
+struct vin_framesize {
+	u32 width;
+	u32 height;
+};
+
+struct vin_format {
+	enum VIN_SOURCE_FORMAT format;
+	u8 fps;
+};
+
+struct stf_vin_dev {
+	/* Protects the access of variables shared within the interrupt */
+	spinlock_t irqlock;
+	int irq;
+	struct device *dev;
+	struct cdev vin_cdev;
+	void __iomem *base;
+	void __iomem *mipi0_base;
+	void __iomem *clkgen_base;
+	void __iomem *rstgen_base;
+	void __iomem *mipi1_base;
+	void __iomem *sysctrl_base;
+	void __iomem *isp_isp0_base;
+	void __iomem *isp_isp1_base;
+	void __iomem *vin_top_clkgen_base;
+	void __iomem *vin_top_rstgen_base;
+	void __iomem *vin_top_iopad_base;
+
+	struct vin_framesize frame;
+	struct vin_format format;
+	bool isp0;
+	bool isp1;
+	int isp0_irq;
+	int isp1_irq;
+	u32 major;
+	struct vin_buf buf;
+
+	wait_queue_head_t wq;
+	bool condition;
+	int odd;
+
+	csi_format csi_fmt;
+};
+
+extern int vin_notifier_register(struct notifier_block *nb);
+extern void vin_notifier_unregister(struct notifier_block *nb);
+extern int vin_notifier_call(unsigned long e, void *v);
+#endif
author	Hal Feng <hal.feng@starfivetech.com>	2021-12-22 11:21:21 +0300
committer	MichaelZhuxx <michael.zhu@starfivetech.com>	2022-01-04 14:47:09 +0300
commit	016794a1040c8e1ab002f3aa41659589385eb63b (patch)
tree	8573c4cfb423ac4e799438a11aa07a8403a36c6b
parent	32f645aaf3b1fb8c47ae9dfb50a6ab642b6fa5b4 (diff)
download	linux-016794a1040c8e1ab002f3aa41659589385eb63b.tar.xz