linux:add ov5640 sensor support base evb

28 files changed, 5896 insertions, 379 deletions

diff --git a/drivers/i2c/busses/i2c-designware-core.h b/drivers/i2c/busses/i2c-designware-core.h
index eb5ef4d0f463..b74cdbf78c34 100644
--- a/drivers/i2c/busses/i2c-designware-core.h
+++ b/drivers/i2c/busses/i2c-designware-core.h
@@ -80,6 +80,8 @@
 #define DW_IC_SDA_HOLD_MIN_VERS	0x3131312A
 #define DW_IC_COMP_TYPE		0xfc
 #define DW_IC_COMP_TYPE_VALUE	0x44570140
+#define DW_IC_FS_SPKLEN	0xa0
+#define DW_IC_HS_SPKLEN	0xa4
 
 #define DW_IC_INTR_RX_UNDER	0x001
 #define DW_IC_INTR_RX_OVER	0x002
@@ -179,6 +181,15 @@ struct clk;
 struct device;
 struct reset_control;
 
+#define CONFIG_I2C_DW_CLOCK_SPEED 55//20//55//20
+
+#define I2C_STD_HCNT		(CONFIG_I2C_DW_CLOCK_SPEED * 4)//0x6//
+#define I2C_STD_LCNT		(CONFIG_I2C_DW_CLOCK_SPEED * 5)//0xf//
+#define I2C_FS_HCNT			((CONFIG_I2C_DW_CLOCK_SPEED * 6) / 8)
+#define I2C_FS_LCNT			((CONFIG_I2C_DW_CLOCK_SPEED * 7) / 8)
+#define I2C_HS_HCNT			((CONFIG_I2C_DW_CLOCK_SPEED * 6) / 8)
+#define I2C_HS_LCNT			((CONFIG_I2C_DW_CLOCK_SPEED * 7) / 8)
+
 /**
  * struct dw_i2c_dev - private i2c-designware data
  * @dev: driver model device node
@@ -284,6 +295,9 @@ struct dw_i2c_dev {
 	int			(*init)(struct dw_i2c_dev *dev);
 	int			(*set_sda_hold_time)(struct dw_i2c_dev *dev);
 	int			mode;
+	int			scl_gpio;
+	int			sda_gpio;
+	bool		auto_calc_lhcnt;
 	struct i2c_bus_recovery_info rinfo;
 	bool			suspended;
 };
diff --git a/drivers/i2c/busses/i2c-designware-master.c b/drivers/i2c/busses/i2c-designware-master.c
index 99d74ddf1313..fd1850a6b328 100644
--- a/drivers/i2c/busses/i2c-designware-master.c
+++ b/drivers/i2c/busses/i2c-designware-master.c
@@ -21,6 +21,7 @@
 #include <linux/regmap.h>
 #include <linux/reset.h>
 #include <linux/gpio-starfive-vic.h>
+//#include <linux/gpio-vic.h>
 
 #include "i2c-designware-core.h"
 
@@ -172,6 +173,166 @@ out:
 	return ret;
 }
 
+static int i2c_dw_configure_gpio(struct dw_i2c_dev *dev)
+{
+	if((dev->scl_gpio > 0) && (dev->sda_gpio > 0))
+	{
+		switch(dev->adapter.nr) {
+		case 0:
+			SET_GPIO_dout_LOW(dev->scl_gpio);
+			SET_GPIO_dout_LOW(dev->sda_gpio);
+			SET_GPIO_doen_reverse_(dev->scl_gpio,1);
+			SET_GPIO_doen_reverse_(dev->sda_gpio,1);
+			SET_GPIO_doen_i2c0_pad_sck_oe(dev->scl_gpio);
+			SET_GPIO_doen_i2c0_pad_sda_oe(dev->sda_gpio);
+			SET_GPIO_i2c0_pad_sck_in(dev->scl_gpio);
+			SET_GPIO_i2c0_pad_sda_in(dev->sda_gpio);
+			break;
+		case 1:
+			SET_GPIO_dout_LOW(dev->scl_gpio);
+			SET_GPIO_dout_LOW(dev->sda_gpio);
+			SET_GPIO_doen_reverse_(dev->scl_gpio,1);
+			SET_GPIO_doen_reverse_(dev->sda_gpio,1);
+			SET_GPIO_doen_i2c1_pad_sck_oe(dev->scl_gpio);
+			SET_GPIO_doen_i2c1_pad_sda_oe(dev->sda_gpio);
+			SET_GPIO_i2c1_pad_sck_in(dev->scl_gpio);
+			SET_GPIO_i2c1_pad_sda_in(dev->sda_gpio);
+			break;
+		case 2:
+			SET_GPIO_dout_LOW(dev->scl_gpio);
+			SET_GPIO_dout_LOW(dev->sda_gpio);
+			SET_GPIO_doen_reverse_(dev->scl_gpio,1);
+			SET_GPIO_doen_reverse_(dev->sda_gpio,1);
+			SET_GPIO_doen_i2c2_pad_sck_oe(dev->scl_gpio);
+			SET_GPIO_doen_i2c2_pad_sda_oe(dev->sda_gpio);
+			SET_GPIO_i2c2_pad_sck_in(dev->scl_gpio);
+			SET_GPIO_i2c2_pad_sda_in(dev->sda_gpio);
+			break;
+		case 3:
+			SET_GPIO_dout_LOW(dev->scl_gpio);
+			SET_GPIO_dout_LOW(dev->sda_gpio);
+			SET_GPIO_doen_reverse_(dev->scl_gpio,1);
+			SET_GPIO_doen_reverse_(dev->sda_gpio,1);
+			SET_GPIO_doen_i2c3_pad_sck_oe(dev->scl_gpio);
+			SET_GPIO_doen_i2c3_pad_sda_oe(dev->sda_gpio);
+			SET_GPIO_i2c3_pad_sck_in(dev->scl_gpio);
+			SET_GPIO_i2c3_pad_sda_in(dev->sda_gpio);
+			break;
+		default:
+			dev_warn(dev->dev, "NOTE: i2c adapter number is invalid\n");
+		}
+	}
+	else
+		dev_warn(dev->dev, "NOTE: scl/sda gpio number is invalid !\n");
+	return 0;
+}
+
+
+static int i2c_dw_cal_scl_lhcnt(struct dw_i2c_dev *dev)
+{
+	u32	value = 0U;
+	u32	rc = 0U;
+    int ret;
+	/* Make sure we have a supported speed for the DesignWare model */
+	/* and have setup the clock frequency and speed mode */
+
+	switch (i2c_dw_clk_rate(dev)) {
+	case 100:
+		/* Following the directions on DW spec page 59, IC_SS_SCL_LCNT
+		 * must have register values larger than IC_FS_SPKLEN + 7
+		 */
+		ret = regmap_read(dev->map, DW_IC_FS_SPKLEN, &value);
+		if (I2C_STD_LCNT <= (value + 7)) {
+			regmap_read(dev->map, DW_IC_FS_SPKLEN, &value);
+			value = value + 8;
+		} else {
+			value = I2C_STD_LCNT;
+		}
+
+		dev->ss_lcnt = value;
+
+		/* Following the directions on DW spec page 59, IC_SS_SCL_HCNT
+		 * must have register values larger than IC_FS_SPKLEN + 5
+		 */
+
+		ret = regmap_read(dev->map, DW_IC_FS_SPKLEN, &value);
+		if (I2C_STD_HCNT <= (value + 5)) {
+			regmap_read(dev->map, DW_IC_FS_SPKLEN, &value);
+			value = value + 6;
+		} else {
+			value = I2C_STD_HCNT;
+		}
+
+		dev->ss_hcnt = value;
+		break;
+		/* fall through */
+	case 400:
+	case 1000:
+		/*
+		 * Following the directions on DW spec page 59, IC_FS_SCL_LCNT
+		 * must have register values larger than IC_FS_SPKLEN + 7
+		 */
+
+	    ret = regmap_read(dev->map, DW_IC_FS_SPKLEN, &value);
+		if (I2C_FS_LCNT <= (value + 7)) {
+			regmap_read(dev->map, DW_IC_FS_SPKLEN, &value);
+			value = value + 8;
+
+		} else {
+			value = I2C_FS_LCNT;
+		}
+
+		dev->fs_lcnt = value;
+		dev->fp_lcnt = value;
+
+		/*
+		 * Following the directions on DW spec page 59, IC_FS_SCL_HCNT
+		 * must have register values larger than IC_FS_SPKLEN + 5
+		 */
+		ret = regmap_read(dev->map, DW_IC_FS_SPKLEN, &value);
+		if (I2C_FS_HCNT <= (value + 5)) {
+			regmap_read(dev->map, DW_IC_FS_SPKLEN, &value);
+			value = value + 6;
+
+		} else {
+			value = I2C_FS_HCNT;
+		}
+
+		dev->fs_hcnt = value;
+		dev->fp_hcnt = value;
+		break;
+	case 3400:
+		ret = regmap_read(dev->map, DW_IC_HS_SPKLEN, &value);
+		if (I2C_HS_LCNT <= (value + 7)) {
+			regmap_read(dev->map, DW_IC_HS_SPKLEN, &value);
+			value = value + 8;
+		} else {
+			value = I2C_HS_LCNT;
+		}
+
+		dev->hs_lcnt = value;
+		ret = regmap_read(dev->map, DW_IC_HS_SPKLEN, &value);
+		if (I2C_HS_HCNT <= (value + 5)) {
+		    regmap_read(dev->map, DW_IC_HS_SPKLEN, &value);
+			value = value + 6;
+		} else {
+			value = I2C_HS_HCNT;
+		}
+
+		dev->hs_hcnt = value;
+		break;
+	default:
+		/* TODO change */
+		rc = -1;
+	}
+
+	/*
+	 * Clear any interrupts currently waiting in the controller
+	 */
+	return rc;
+}
+
+
 /**
  * i2c_dw_init() - Initialize the designware I2C master hardware
  * @dev: device private data
@@ -191,6 +352,9 @@ static int i2c_dw_init_master(struct dw_i2c_dev *dev)
 	/* Disable the adapter */
 	__i2c_dw_disable(dev);
 
+	if (true == dev->auto_calc_lhcnt)
+		i2c_dw_cal_scl_lhcnt(dev);
+
 	/* Write standard speed timing parameters */
 	regmap_write(dev->map, DW_IC_SS_SCL_HCNT, dev->ss_hcnt);
 	regmap_write(dev->map, DW_IC_SS_SCL_LCNT, dev->ss_lcnt);
@@ -807,6 +971,7 @@ int i2c_dw_probe_master(struct dw_i2c_dev *dev)
 	if (ret)
 		dev_err(dev->dev, "failure adding adapter: %d\n", ret);
 	pm_runtime_put_noidle(dev->dev);
+	i2c_dw_configure_gpio(dev);
 
 	return ret;
 }
diff --git a/drivers/i2c/busses/i2c-designware-platdrv.c b/drivers/i2c/busses/i2c-designware-platdrv.c
index 0dff28515ae7..cd34bcb58a89 100644
--- a/drivers/i2c/busses/i2c-designware-platdrv.c
+++ b/drivers/i2c/busses/i2c-designware-platdrv.c
@@ -224,6 +224,7 @@ static const struct dmi_system_id dw_i2c_hwmon_class_dmi[] = {
 static int dw_i2c_plat_probe(struct platform_device *pdev)
 {
 	struct dw_i2c_platform_data *pdata = dev_get_platdata(&pdev->dev);
+	struct device_node *np = pdev->dev.of_node;
 	struct i2c_adapter *adap;
 	struct dw_i2c_dev *dev;
 	struct i2c_timings *t;
@@ -240,6 +241,9 @@ static int dw_i2c_plat_probe(struct platform_device *pdev)
 	dev->flags = (uintptr_t)device_get_match_data(&pdev->dev);
 	dev->dev = &pdev->dev;
 	dev->irq = irq;
+	dev->scl_gpio = of_get_named_gpio(np, "scl-gpio", 0);
+	dev->sda_gpio = of_get_named_gpio(np, "sda-gpio", 0);
+	dev->auto_calc_lhcnt = of_property_read_bool(np, "auto_calc_scl_lhcnt");
 	platform_set_drvdata(pdev, dev);
 
 	ret = dw_i2c_plat_request_regs(dev);
diff --git a/drivers/media/platform/starfive/Makefile b/drivers/media/platform/starfive/Makefile
index 469c18c3f8ee..cabb4d874b14 100644
--- a/drivers/media/platform/starfive/Makefile
+++ b/drivers/media/platform/starfive/Makefile
@@ -5,4 +5,4 @@
 
 obj-$(CONFIG_VIN_SENSOR_OV5640) += ov5640_dvp.o
 obj-$(CONFIG_VIN_SENSOR_SC2235) += sc2235.o
-obj-$(CONFIG_VIDEO_STF_VIN) += stf_vin.o stf_event.o stf_isp.o
+obj-$(CONFIG_VIDEO_STF_VIN) += stf_vin.o stf_event.o stf_isp.o stf_csi.o
diff --git a/drivers/media/platform/starfive/imx219_mipi.c b/drivers/media/platform/starfive/imx219_mipi.c
new file mode 100644
index 000000000000..6004bbbaecc9
--- /dev/null
+++ b/drivers/media/platform/starfive/imx219_mipi.c
@@ -0,0 +1,424 @@
+/*
+ * Copyright (C) 2011-2013 StarFive Technology Co., Ltd. All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/ctype.h>
+#include <linux/types.h>
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/gpio-starfive-vic.h>
+#include <linux/gpio.h>
+#include <linux/delay.h>
+#include <video/stf-vin.h>
+#include <linux/of.h>
+#include <linux/of_gpio.h>
+
+#define SF_MIPI_DEBUG
+
+#ifdef SF_MIPI_DEBUG
+	#define MIPI_PRT(format, args...)    printk(KERN_DEBUG "[MIPI]: " format, ## args)
+	#define MIPI_INFO(format, args...)   printk(KERN_INFO "[MIPI]: " format, ## args)
+	#define MIPI_ERR(format, args...)	 printk(KERN_ERR "[MIPI]: " format, ## args)
+#else
+	#define MIPI_PRT(x...)  do{} while(0)
+	#define MIPI_INFO(x...)  do{} while(0)
+	#define MIPI_ERR(x...)  do{} while(0)
+#endif
+
+#define MCLK_HZ             24000000UL
+#define MCLK_INTERVAL_US   (MCLK_HZ/1000000UL)
+#define REGVALLIST(list) (list), sizeof(list)/sizeof((list)[0])
+#define IMX_CSI0_RST_PIN        58 //20
+#define IMX_CSI1_RST_PIN        57 //21
+/* Chip ID */
+#define IMX219_REG_CHIP_ID              0x0000
+#define IMX219_CHIP_ID                  0x0219
+
+#define IMX219_RESET_PIN_NAME	"imx219-reset-pin"
+
+typedef struct
+{
+    u16 addr;
+    u8 val;
+} regval_t;
+
+typedef enum
+{
+    MODE_1080P_30FPS_4D,
+    MODE_1080P_60FPS_4D,
+    MODE_1080P_30FPS_2D,
+    MODE_4K_30FPS_4D,
+} mipicam_mode_t;
+
+static const regval_t S_RASPI_CAM_V2_1080P_30FPS_2D[] =
+{
+	{0x0103, 0x01},
+	{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, 0x57},
+	{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},
+	{0x0162, 0x0d},
+	{0x0163, 0x78},
+};
+
+
+const struct {
+		mipicam_mode_t mode;
+		const regval_t * regval;
+		int regval_num;
+} imx219_mode_params[] = {
+		{ MODE_1080P_30FPS_2D, REGVALLIST(S_RASPI_CAM_V2_1080P_30FPS_2D) },
+	};
+
+struct reg_value {
+	u16 u16RegAddr;
+	u8 u8Val;
+	u8 u8Mask;
+	u32 u32Delay_ms;
+};
+
+struct imx219 {
+	struct i2c_client *i2c_client;
+	struct device *dev;
+
+	int reset_pin;
+	mipicam_mode_t mode;
+
+	struct mutex lock; /* lock to protect power state, ctrls and mode */
+	bool power_on;
+
+	struct gpio_desc *enable_gpio;
+};
+
+static int imx219_write_reg(struct i2c_client *client,u16 reg, u8 val)
+{
+	u8 au8Buf[3] = {0};
+
+	au8Buf[0] = reg >> 8;
+	au8Buf[1] = reg & 0xff;
+	au8Buf[2] = val;
+
+	if (i2c_master_send(client, au8Buf, 3) < 0) {
+		pr_err("%s:write reg error:reg=%x,val=%x\n",
+			__func__, reg, val);
+		return -1;
+	}
+
+	mdelay(5);
+
+	return 0;
+}
+
+static int imx219_read_reg(struct i2c_client *client,u16 reg, u8 *val)
+{
+	u8 au8RegBuf[2] = {0};
+	u8 u8RdVal = 0;
+	int valrtn = 0;
+
+	au8RegBuf[0] = reg >> 8;
+	au8RegBuf[1] = reg & 0xff;
+
+    valrtn = i2c_master_send(client, au8RegBuf, 2);
+	if (2 != valrtn) {
+		pr_err("%s:write reg error:reg=%x,valrtn=%d\n",
+				__func__, reg,valrtn);
+		return -1;
+	}
+	
+	if (1 != i2c_master_recv(client, &u8RdVal, 1)) {
+		pr_err("%s:read reg error:reg=%x,val=%x\n",
+				__func__, reg, u8RdVal);
+		return -1;
+	}
+
+	*val = u8RdVal;
+
+	return u8RdVal;
+}
+
+static u16 imx219_cam_v2_read_16(struct i2c_client *client, u16 addr, u16 mask)
+{
+    u16 val = 0;
+    u8 temp;
+
+    imx219_read_reg(client, addr, &temp);
+    val = (temp & (mask>>8)) << 8;
+    imx219_read_reg(client, addr+1, &temp);
+    val |= (temp & (mask&0xff));
+
+    return val;
+}
+
+
+static int imx219_gpio_reserve(int pin, int dir,const char *name)
+{
+	int ret = -ENODEV;
+	if (!gpio_is_valid(pin))
+		return ret;
+
+	ret = gpio_request(pin, name);
+	if (ret) {
+		return ret;
+	}
+
+	ret = gpio_direction_output(pin, dir);
+	if (ret) {
+		gpio_free(pin);
+		return ret;
+	}
+
+	return 0;
+}
+
+static void imx219_gpio_release(int pin, const char *name)
+{
+	if (gpio_is_valid(pin)) {
+		gpio_free(pin);
+	}
+}
+static int imx219_power_down(struct imx219 *imx219)
+{
+    // software standby
+    imx219_write_reg(imx219->i2c_client,0x100, 0);
+    
+    imx219_gpio_reserve(imx219->reset_pin, 0, IMX219_RESET_PIN_NAME);
+    return 0;
+}
+
+static int imx219_power_up(struct imx219 *imx219)
+{
+	int ret;
+	
+	/* Power configuration */
+	ret = imx219_gpio_reserve(imx219->reset_pin, 0, IMX219_RESET_PIN_NAME);
+	if (ret)
+		goto disable;
+
+	if (gpio_is_valid(imx219->reset_pin)) {
+	    //gpio_direction_output(dev->reset_pin, 1);
+	    gpio_set_value(imx219->reset_pin, 1);
+	}
+	udelay(200 + 32000*MCLK_INTERVAL_US); 
+
+	u16 chip_id = imx219_cam_v2_read_16(imx219->i2c_client, IMX219_REG_CHIP_ID, 0xffff);
+	if (chip_id != IMX219_CHIP_ID) {
+		MIPI_INFO("raspi_cam_v2 failed to power up!\n");
+		return -EINVAL;
+	} else {
+		MIPI_INFO("raspi_cam_v2 powered up!\n");
+	}
+    return 0;
+disable:
+	return ret;
+}
+
+static int imx219_set_power_on(struct imx219 *imx219)
+{
+
+    return imx219_power_up(imx219);
+}
+
+static int imx219_start(struct imx219 *imx219)
+{
+    int result = imx219_write_reg(imx219->i2c_client,0x100, 1);
+    mdelay(200);
+    return result;
+}
+
+static int imx219_stop(struct imx219 *imx219)
+{
+    int result = imx219_write_reg(imx219->i2c_client,0x100, 0);
+    mdelay(200);
+    return result;
+}
+
+static int imx219_set_mode(struct imx219 *imx219)
+{
+	int i, j;
+	for (i = 0; i < sizeof(imx219_mode_params)/sizeof(imx219_mode_params[0]); i++) {
+		if (imx219->mode == imx219_mode_params[i].mode) {
+			for (j = 0; j < imx219_mode_params[i].regval_num; j++) {
+				imx219_write_reg(imx219->i2c_client,imx219_mode_params[i].regval[j].addr, imx219_mode_params[i].regval[j].val);
+			}
+			return 0;
+		}
+	}
+	
+	return -EINVAL;
+}
+
+int mipicam_set_mode(struct imx219 *imx219)
+{
+	imx219_stop(imx219);
+
+    return imx219_set_mode(imx219);
+}
+
+static void imx219_set_power_down(struct imx219 *imx219)
+{
+    imx219_stop(imx219);
+    imx219_power_down(imx219);
+}
+
+/*!
+ * ov4689 I2C probe function
+ *
+ * @param adapter            struct i2c_adapter *
+ * @return  Error code indicating success or failure
+ */
+static int imx219_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	struct device *dev = &client->dev;
+	struct imx219 *imx219;
+	struct device_node *np = client->dev.of_node;
+	int ret;
+	MIPI_INFO("imx219_probe enter\n");
+
+	imx219 = devm_kzalloc(dev, sizeof(struct imx219), GFP_KERNEL);
+	if (!imx219)
+		return -ENOMEM;
+		
+    imx219->i2c_client = client;
+	imx219->dev = dev;
+	imx219->mode = MODE_1080P_30FPS_2D;
+	
+	imx219->reset_pin = of_get_named_gpio(np, "reset-gpio", 0);
+
+	i2c_set_clientdata(client, imx219);
+	
+    ret = imx219_set_power_on(imx219);
+	if (ret) {
+	   MIPI_ERR("failed to open sensor\n");
+	   goto release_gpios;
+	}
+
+	if (mipicam_set_mode(imx219)) {
+		   MIPI_ERR("mode %d not supported by sensor[i2c_bus 0]\n", imx219->mode);
+		   goto EXIT;
+	}
+
+	imx219_write_reg(imx219->i2c_client, 0x0157, 0x5f);
+	imx219_write_reg(imx219->i2c_client, 0x015a, 0x09);
+	imx219_write_reg(imx219->i2c_client, 0x015b, 0x60);
+	imx219_write_reg(imx219->i2c_client, 0x0165, 0xa8);
+	imx219_write_reg(imx219->i2c_client, 0x0167, 0x27);
+
+    imx219_start(imx219);
+    return 0;
+
+   EXIT:
+	   /* 8. stop and close sensor */
+	   imx219_set_power_down(imx219);
+	   return 0;
+   release_gpios:
+	   imx219_gpio_release(imx219->reset_pin, IMX219_RESET_PIN_NAME);
+       return 0;
+}
+
+static int imx219_remove(struct i2c_client *client)
+{
+	struct imx219 *imx219 = i2c_get_clientdata(client);
+
+	imx219_set_power_down(imx219);
+	imx219_gpio_release(imx219->reset_pin, IMX219_RESET_PIN_NAME);
+	return 0;
+}
+
+static const struct i2c_device_id imx219_id[] = {
+	{"imx219", 0},
+	{},
+};
+
+MODULE_DEVICE_TABLE(i2c, imx219_id);
+
+static struct i2c_driver imx219_i2c_driver = {
+	.driver = {
+		  .owner = THIS_MODULE,
+		  .name  = "imx219_mipi",
+		  },
+	.probe  = imx219_probe,
+	.remove = imx219_remove,
+	.id_table = imx219_id,
+};
+
+static __init int init_imx219(void)
+{ 
+    int err;
+
+	err = i2c_add_driver(&imx219_i2c_driver);
+    if (err != 0)
+		printk("i2c driver registration failed, error=%d\n", err);
+
+	return err;
+}
+
+static __exit void exit_imx219(void)
+{
+	i2c_del_driver(&imx219_i2c_driver);
+}
+
+fs_initcall(init_imx219);
+module_exit(exit_imx219);
+
+MODULE_AUTHOR("StarFive Technology Co., Ltd.");
+MODULE_DESCRIPTION("imx219 Mipi Camera Driver");
+MODULE_LICENSE("GPL");
+MODULE_VERSION("1.0");
+MODULE_ALIAS("DVP");
diff --git a/drivers/media/platform/starfive/ov5640_dvp.c b/drivers/media/platform/starfive/ov5640_dvp.c
index acc524a3af0b..e90cbfbfdfac 100755
--- a/drivers/media/platform/starfive/ov5640_dvp.c
+++ b/drivers/media/platform/starfive/ov5640_dvp.c
@@ -415,6 +415,7 @@ static int ov5640_probe(struct i2c_client *client,
 	ArySize = ARRAY_SIZE(ov5640_setting_30fps_1080P_1920_1080);
 	retval = ov5640_download_firmware(pModeSetting, ArySize);
 
+	printk("ov5640_probe done\n");
     return 0;
 }
 
diff --git a/drivers/media/platform/starfive/stf_csi.c b/drivers/media/platform/starfive/stf_csi.c
new file mode 100644
index 000000000000..57516f0d3681
--- /dev/null
+++ b/drivers/media/platform/starfive/stf_csi.c
@@ -0,0 +1,210 @@
+/* /drivers/media/platform/starfive/stf_csi.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-12-09	starfive		created
+**
+*/
+#include <asm/io.h>
+#include <linux/fb.h>
+#include <linux/module.h>
+#include <video/stf-vin.h>
+#include <linux/delay.h>
+#include "stf_csi.h"
+
+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 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 void reg_clear_highest_bit(void __iomem * base, u32 reg)
+{
+    u32 val;
+	val = ioread32(base + reg);
+	val &= ~(0x1 << 31);
+	val |= (0x0 & 0x1) << 31;
+	iowrite32(val, base + reg);
+}
+*/
+int csi2rx_dphy_config(struct stf_vin_dev *vin,const csi2rx_dphy_cfg_t *cfg)
+{
+    union dphy_lane_swap dphy_lane_swap = {
+        .bits = {
+            .rx_1c2c_sel        = cfg->clane_nb - 1, // 0 - 1clk, 1 - 2clks
+            .lane_swap_clk      = cfg->clane_map[0], //mipi-rx0 bind to clk0
+            .lane_swap_clk1     = cfg->clane_map[1], //mipi-rx1 bind to clk1
+            .lane_swap_lan0     = cfg->dlane_map[0],
+            .lane_swap_lan1     = cfg->dlane_map[1],
+            .lane_swap_lan2     = cfg->dlane_map[2],
+            .lane_swap_lan3     = cfg->dlane_map[3],
+            .dpdn_swap_clk      = cfg->clane_pn_swap[0],
+            .dpdn_swap_clk1     = cfg->clane_pn_swap[1],
+            .dpdn_swap_lan0     = cfg->dlane_pn_swap[0],
+            .dpdn_swap_lan1     = cfg->dlane_pn_swap[1],
+            .dpdn_swap_lan2     = cfg->dlane_pn_swap[2],
+            .dpdn_swap_lan3     = cfg->dlane_pn_swap[3],
+            .hs_freq_chang_clk0 = 0,
+            .hs_freq_chang_clk1 = 0,
+            .reserved           = 0,
+        }
+    };
+		
+	union dphy_lane_en dphy_lane_en = {
+        .bits = {
+            .gpio_en          = 0,
+            .mp_test_mode_sel = 0,
+            .mp_test_en       = 0,
+            .dphy_enable_lan0 = cfg->dlane_en[0],
+            .dphy_enable_lan1 = cfg->dlane_en[1],
+            .dphy_enable_lan2 = cfg->dlane_en[2],
+            .dphy_enable_lan3 = cfg->dlane_en[3],
+            .rsvd_0           = 0,
+        }
+    };
+    // evb, ov4689, c0-0,d0-1,d1-2,d2-3,d3-4,c1-5
+    //write_reg(ISP_SYSCONTROLLER_BASE_ADDR, 0x10, 0x000468d0);
+    reg_write(vin->sysctrl_base, SYSCTRL_REG4, dphy_lane_swap.raw);    
+    reg_write(vin->sysctrl_base, SYSCTRL_DPHY_CTRL, dphy_lane_en.raw); 
+    reg_write(vin->clkgen_base, CLK_DPHY_CFGCLK_ISPCORE_2X_CTRL, 0x80000008);
+    reg_write(vin->clkgen_base, CLK_DPHY_REFCLK_ISPCORE_2X_CTRL, 0x80000010);
+    reg_write(vin->clkgen_base, CLK_DPHY_TXCLKESC_IN_CTRL, 0x80000028);
+
+    return 0;
+}
+EXPORT_SYMBOL(csi2rx_dphy_config);
+
+static int csi2rx_reset(struct stf_vin_dev *vin,int id)
+{
+	reg_set_highest_bit(vin->clkgen_base,CLK_CSI2RX0_APB_CTRL);
+
+    if (id == 0) {
+		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_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 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);  //Frame Capture Counter enable, vc0
+}
+
+int csi2rx_config(struct stf_vin_dev *vin,int id, const csi2rx_cfg_t *cfg)
+{
+    int s_stream0_fifo_mode = 0;
+    int s_stream0_fifo_fill = 0;
+	
+    union static_config config;
+    union dphy_lane_ctrl dphy_lane_ctrl;
+    union stream_cfg stream0_cfg = {
+        .fifo_fill      = 0, // fifo depth is 2048 pixels
+        .bpp_bypass     = 0,
+        .fifo_mode      = 0, // 0 full line; 1 large buffer
+        .num_pixels     = 0, // 0 - 1 pixel per clock (default), 1 - 2 pixel
+        .ls_le_mode     = 0,
+        .interface_mode = 0,
+    };
+
+    void *reg_base = NULL;
+    if(id == 0)
+        reg_base= vin->mipi0_base;
+    else if (id == 1) {
+        reg_base= vin->mipi1_base;
+    }else {
+        return 0;
+    }
+    
+    csi2rx_reset(vin,id);
+
+    // 0x08 STATIC_CFG
+    config.raw = 0;
+    config.bits.lane_nb = cfg->lane_nb;
+    config.bits.dl0_map = cfg->dlane_map[0];
+    config.bits.dl1_map = cfg->dlane_map[1];
+    config.bits.dl2_map = cfg->dlane_map[2];
+    config.bits.dl3_map = cfg->dlane_map[3];
+	
+    reg_write(reg_base, STATIC_CFG, config.raw);
+
+    // 0x40 DPHY_LANE_CONTROL
+    dphy_lane_ctrl.raw = 0;
+    dphy_lane_ctrl.bits.dl0_en = dphy_lane_ctrl.bits.dl0_reset = (cfg->lane_nb > 0);
+    dphy_lane_ctrl.bits.dl1_en = dphy_lane_ctrl.bits.dl1_reset = (cfg->lane_nb > 1);
+    dphy_lane_ctrl.bits.dl2_en = dphy_lane_ctrl.bits.dl2_reset = (cfg->lane_nb > 2);
+    dphy_lane_ctrl.bits.dl3_en = dphy_lane_ctrl.bits.dl3_reset = (cfg->lane_nb > 3);
+    dphy_lane_ctrl.bits.cl_en = dphy_lane_ctrl.bits.cl_reset = 1;
+	
+    reg_write(reg_base, DPHY_LANE_CONTROL, dphy_lane_ctrl.raw);
+
+    csi2rx_debug_config(reg_base, cfg->vsize);
+
+    reg_write(reg_base, STREAM0_DATA_CFG, 0x00000080|(cfg->dt&0x3f));  // all vc, dt0 enabled
+
+    stream0_cfg.fifo_mode = s_stream0_fifo_mode & 0x3;
+    stream0_cfg.fifo_fill = (stream0_cfg.fifo_mode == 1) ? s_stream0_fifo_fill : 0;
+    reg_write(reg_base, STREAM0_CFG, stream0_cfg.value);
+    reg_write(reg_base, STREAM0_CTRL, 0x00000010);      // soft_rst
+    mdelay(100);
+    reg_write(reg_base, STREAM0_CTRL, 0x00000001);      // start
+
+    return 0;
+}
+EXPORT_SYMBOL(csi2rx_config);
+
+MODULE_AUTHOR("StarFive Technology Co., Ltd.");
+MODULE_DESCRIPTION("loadable CSI driver for StarFive");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/platform/starfive/stf_csi.h b/drivers/media/platform/starfive/stf_csi.h
new file mode 100644
index 000000000000..7ba1f7ea1996
--- /dev/null
+++ b/drivers/media/platform/starfive/stf_csi.h
@@ -0,0 +1,135 @@
+/*
+ * StarFive isp driver
+ *
+ * Copyright 2020 StarFive Inc.
+ *
+ * Licensed under the GPL-2.
+ */
+
+#ifndef __SF_CSI_H__
+#define __SF_CSI_H__
+
+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;
+};
+
+extern int csi2rx_dphy_config(struct stf_vin_dev *vin,const csi2rx_dphy_cfg_t *cfg);
+extern int csi2rx_config(struct stf_vin_dev *vin,int id, const csi2rx_cfg_t *cfg);
+
+
+#endif
+
diff --git a/drivers/media/platform/starfive/stf_isp.c b/drivers/media/platform/starfive/stf_isp.c
index 67da513cdffd..2ee3bd402eea 100644
--- a/drivers/media/platform/starfive/stf_isp.c
+++ b/drivers/media/platform/starfive/stf_isp.c
@@ -17,6 +17,19 @@
 #include <linux/fb.h>
 #include <linux/module.h>
 #include <video/stf-vin.h>
+#include <linux/delay.h>
+
+#define SF_ISP_DEBUG
+#ifdef SF_ISP_DEBUG
+	#define ISP_PRT(format, args...)    printk(KERN_DEBUG "[ISP]: " format, ## args)
+	#define ISP_INFO(format, args...)   printk(KERN_INFO "[ISP]: " format, ## args)
+	#define ISP_ERR(format, args...)	printk(KERN_ERR "[ISP]: " format, ## args)
+#else
+	#define ISP_PRT(x...)  do{} while(0)
+	#define ISP_INFO(x...)  do{} while(0)
+	#define ISP_ERR(x...)  do{} while(0)
+#endif
+
 
 static inline u32 reg_read(void __iomem * base, u32 reg)
 {
@@ -30,112 +43,150 @@ static inline void reg_write(void __iomem * base, u32 reg, u32 val)
 
 void isp_ddr_format_config(struct stf_vin_dev *vin)
 {
+	void __iomem *ispbase;
+
+	if(vin->isp0)
+		ispbase = vin->isp_isp0_base;
+	else if(vin->isp1)
+	    ispbase = vin->isp_isp1_base;
+	else
+		return;
+
 	switch (vin->format.format) {
 	case SRC_COLORBAR_VIN_ISP:
-	    reg_write(vin->base_isp, ISP_REG_DVP_POLARITY_CFG, 0xd);
+	    reg_write(ispbase, ISP_REG_DVP_POLARITY_CFG, 0xd);
 		break;
 
 	case SRC_DVP_SENSOR_VIN_ISP:
-	    reg_write(vin->base_isp, ISP_REG_DVP_POLARITY_CFG, 0x08);
+	    reg_write(ispbase, ISP_REG_DVP_POLARITY_CFG, 0x08);
 		break;
 
 	default:
 		pr_err("unknown format\n");
 		return;
 	}
-	reg_write(vin->base_isp, ISP_REG_RAW_FORMAT_CFG, 0x000011BB);	// sym_order = [0+:16]
-	reg_write(vin->base_isp, ISP_REG_CFA_MODE, 0x00000030);
-	reg_write(vin->base_isp, ISP_REG_PIC_CAPTURE_START_CFG, 0x00000000); // cro_hstart = [0+:16], cro_vstart = [16+:16]
+
+	reg_write(ispbase, ISP_REG_RAW_FORMAT_CFG, 0x000011BB);	// sym_order = [0+:16]
+	reg_write(ispbase, ISP_REG_CFA_MODE, 0x00000030);
+	reg_write(ispbase, ISP_REG_PIC_CAPTURE_START_CFG, 0x00000000); // cro_hstart = [0+:16], cro_vstart = [16+:16]
 }
 
 void isp_ddr_resolution_config(struct stf_vin_dev *vin)
 {
 	u32 val = 0;
+	void __iomem *ispbase;
+
+	if(vin->isp0)
+		ispbase = vin->isp_isp0_base;
+	else if(vin->isp1)
+	    ispbase = vin->isp_isp1_base;
+	else
+		return;
 	val = (vin->frame.width-1) + ((vin->frame.height-1)<<16);
-	
-	reg_write(vin->base_isp, ISP_REG_PIC_CAPTURE_END_CFG, val);	// cro_hend = [0+:16], cro_vend = [16+:16]
+
+	reg_write(ispbase, ISP_REG_PIC_CAPTURE_END_CFG, val);	// cro_hend = [0+:16], cro_vend = [16+:16]
 	val = (vin->frame.width) + ((vin->frame.height)<<16);
-	reg_write(vin->base_isp, ISP_REG_PIPELINE_XY_SIZE, val);	// ISP pipeline width[0+:16], height[16+:16]
+	reg_write(ispbase, ISP_REG_PIPELINE_XY_SIZE, val);	// ISP pipeline width[0+:16], height[16+:16]
 
-	reg_write(vin->base_isp, ISP_REG_Y_PLANE_START_ADDR, FB_FIRST_ADDR);	// Unscaled Output Image Y Plane Start Address Register
-	reg_write(vin->base_isp, ISP_REG_UV_PLANE_START_ADDR, FB_FIRST_ADDR+(vin->frame.width*vin->frame.height));	// Unscaled Output Image UV Plane Start Address Register
+	reg_write(ispbase, ISP_REG_Y_PLANE_START_ADDR, vin->buf.paddr);	// Unscaled Output Image Y Plane Start Address Register
+	reg_write(ispbase, ISP_REG_UV_PLANE_START_ADDR,
+		  vin->buf.paddr + (vin->frame.width * vin->frame.height));	// Unscaled Output Image UV Plane Start Address Register
 
-	reg_write(vin->base_isp, ISP_REG_STRIDE, vin->frame.width);	// Unscaled Output Image Stride Register
+	reg_write(ispbase, ISP_REG_STRIDE, vin->frame.width);	// Unscaled Output Image Stride Register
 
-	reg_write(vin->base_isp, ISP_REG_PIXEL_COORDINATE_GEN, 0x00000010);	// Unscaled Output Pixel Coordinate Generator Mode Register
-	
-	reg_write(vin->base_isp, ISP_REG_PIXEL_AXI_CONTROL, 0x00000000);
-	reg_write(vin->base_isp, ISP_REG_SS_AXI_CONTROL, 0x00000000);
-
-	reg_write(vin->base_isp, ISP_REG_RGB_TO_YUV_COVERSION0, 0x0000004D);	// ICCONV_0
-	reg_write(vin->base_isp, ISP_REG_RGB_TO_YUV_COVERSION1, 0x00000096);	// ICCONV_1
-	reg_write(vin->base_isp, ISP_REG_RGB_TO_YUV_COVERSION2, 0x0000001D);	// ICCONV_2
-	reg_write(vin->base_isp, ISP_REG_RGB_TO_YUV_COVERSION3, 0x000001DA);	// ICCONV_3
-	reg_write(vin->base_isp, ISP_REG_RGB_TO_YUV_COVERSION4, 0x000001B6);	// ICCONV_4
-	reg_write(vin->base_isp, ISP_REG_RGB_TO_YUV_COVERSION5, 0x00000070);	// ICCONV_5
-	reg_write(vin->base_isp, ISP_REG_RGB_TO_YUV_COVERSION6, 0x0000009D);	// ICCONV_6
-	reg_write(vin->base_isp, ISP_REG_RGB_TO_YUV_COVERSION7, 0x0000017C);	// ICCONV_7
-	reg_write(vin->base_isp, ISP_REG_RGB_TO_YUV_COVERSION8, 0x000001E6);	// ICCONV_8
-	
-	reg_write(vin->base_isp, ISP_REG_CIS_MODULE_CFG, 0x00000000);
-	reg_write(vin->base_isp, ISP_REG_ISP_CTRL_1, 0x10000022);	//0x30000022);//
-	reg_write(vin->base_isp, ISP_REG_DC_AXI_ID, 0x00000000);
-	reg_write(vin->base_isp, 0x00000008, 0x00010005);//this reg can not be found in document
+	reg_write(ispbase, ISP_REG_PIXEL_COORDINATE_GEN, 0x00000010);	// Unscaled Output Pixel Coordinate Generator Mode Register
+	reg_write(ispbase, ISP_REG_PIXEL_AXI_CONTROL, 0x00000000);
+	reg_write(ispbase, ISP_REG_SS_AXI_CONTROL, 0x00000000);
+
+	reg_write(ispbase, ISP_REG_RGB_TO_YUV_COVERSION0, 0x0000004D);	// ICCONV_0
+	reg_write(ispbase, ISP_REG_RGB_TO_YUV_COVERSION1, 0x00000096);	// ICCONV_1
+	reg_write(ispbase, ISP_REG_RGB_TO_YUV_COVERSION2, 0x0000001D);	// ICCONV_2
+	reg_write(ispbase, ISP_REG_RGB_TO_YUV_COVERSION3, 0x000001DA);	// ICCONV_3
+	reg_write(ispbase, ISP_REG_RGB_TO_YUV_COVERSION4, 0x000001B6);	// ICCONV_4
+	reg_write(ispbase, ISP_REG_RGB_TO_YUV_COVERSION5, 0x00000070);	// ICCONV_5
+	reg_write(ispbase, ISP_REG_RGB_TO_YUV_COVERSION6, 0x0000009D);	// ICCONV_6
+	reg_write(ispbase, ISP_REG_RGB_TO_YUV_COVERSION7, 0x0000017C);	// ICCONV_7
+	reg_write(ispbase, ISP_REG_RGB_TO_YUV_COVERSION8, 0x000001E6);	// ICCONV_8
+
+	reg_write(ispbase, ISP_REG_CIS_MODULE_CFG, 0x00000000);
+	reg_write(ispbase, ISP_REG_ISP_CTRL_1, 0x10000022);	//0x30000022)
+	reg_write(ispbase, ISP_REG_DC_AXI_ID, 0x00000000);
+	reg_write(ispbase, 0x00000008, 0x00010005);//this reg can not be found in document
 }
 
 void isp_reset(struct stf_vin_dev *vin)
 {
+	void __iomem *ispbase;
 	u32 isp_enable = ISP_NO_SCALE_ENABLE | ISP_MULTI_FRAME_ENABLE;
-	reg_write(vin->base_isp, ISP_REG_ISP_CTRL_0, (isp_enable | ISP_RESET) /*0x00120002 */ );	// isp_rst = [1]
-	reg_write(vin->base_isp, ISP_REG_ISP_CTRL_0, isp_enable /*0x00120000 */ );	// isp_rst = [1]
+	if(vin->isp0)
+		ispbase = vin->isp_isp0_base;
+	else if(vin->isp1)
+	    ispbase = vin->isp_isp1_base;
+	else
+		return;
+
+	reg_write(ispbase, ISP_REG_ISP_CTRL_0, (isp_enable | ISP_RESET) /*0x00120002 */ );	// isp_rst = [1]
+	reg_write(ispbase, ISP_REG_ISP_CTRL_0, isp_enable /*0x00120000 */ );	// isp_rst = [1]
 }
 
 void isp_enable(struct stf_vin_dev *vin)
 {
 	u32 isp_enable = ISP_NO_SCALE_ENABLE | ISP_MULTI_FRAME_ENABLE;
+	void __iomem *ispbase;
 
-	reg_write(vin->base_isp, ISP_REG_ISP_CTRL_0, (isp_enable | ISP_ENBALE) /*0x00120001 */ );	// isp_en = [0]
-	reg_write(vin->base_isp, 0x00000008, 0x00010004);	// CSI immed shadow update
-	reg_write(vin->base_isp, ISP_REG_CSI_INPUT_EN_AND_STATUS, 0x00000001);	// csi_en = [0]
+	if(vin->isp0)
+		ispbase = vin->isp_isp0_base;
+	else if(vin->isp1)
+	    ispbase = vin->isp_isp1_base;
+	else
+		return;
+
+	reg_write(ispbase, ISP_REG_ISP_CTRL_0, (isp_enable | ISP_ENBALE) /*0x00120001 */ );	// isp_en = [0]
+	reg_write(ispbase, 0x00000008, 0x00010004);	// CSI immed shadow update
+	reg_write(ispbase, ISP_REG_CSI_INPUT_EN_AND_STATUS, 0x00000001);	// csi_en = [0]
 }
 
 void isp_dvp_2ndframe_config(struct stf_vin_dev *vin)
 {
-	reg_write(vin->base_isp, ISP_REG_Y_PLANE_START_ADDR, FB_SECOND_ADDR);	// Unscaled Output Image Y Plane Start Address Register
-	reg_write(vin->base_isp, ISP_REG_UV_PLANE_START_ADDR, FB_SECOND_ADDR+vin->frame.width*vin->frame.height);	// Unscaled Output Image UV Plane Start Address Register
-	reg_write(vin->base_isp, ISP_REG_STRIDE, vin->frame.width);	// Unscaled Output Image Stride Register
+	void __iomem *ispbase;
+	if(vin->isp0)
+		ispbase = vin->isp_isp0_base;
+	else if(vin->isp1)
+	    ispbase = vin->isp_isp1_base;
+	else
+		return;
+
+	reg_write(ispbase, ISP_REG_Y_PLANE_START_ADDR, FB_SECOND_ADDR);	// Unscaled Output Image Y Plane Start Address Register
+	reg_write(ispbase, ISP_REG_UV_PLANE_START_ADDR, FB_SECOND_ADDR+vin->frame.width*vin->frame.height);	// Unscaled Output Image UV Plane Start Address Register
+	reg_write(ispbase, ISP_REG_STRIDE, vin->frame.width);	// Unscaled Output Image Stride Register
 }
 
 void isp_clk_set(struct stf_vin_dev *vin)
 {
-	void __iomem *clkgen_base = vin->base + VIN_CLKGEN_OFFSET;
-
 	if (vin->isp0) {
 		/* enable isp0 clk */
-		reg_write(clkgen_base, CLK_ISP0_CTRL, 0x80000002);	//ISP0-CLK
-		reg_write(clkgen_base, CLK_ISP0_2X_CTRL, 0x80000000);
-		reg_write(clkgen_base, CLK_ISP0_MIPI_CTRL, 0x80000000);
-
-		reg_write(clkgen_base, CLK_MIPI_RX0_PXL_CTRL, 0x00000008);	//0x00000010);colorbar
+		reg_write(vin->clkgen_base, CLK_ISP0_CTRL, 0x80000002);	//ISP0-CLK
+		reg_write(vin->clkgen_base, CLK_ISP0_2X_CTRL, 0x80000000);
+		reg_write(vin->clkgen_base, CLK_ISP0_MIPI_CTRL, 0x80000000);
+		reg_write(vin->clkgen_base, CLK_MIPI_RX0_PXL_CTRL, 0x00000008);	//0x00000010);colorbar
 
 		if (vin->format.format == SRC_COLORBAR_VIN_ISP)
-			reg_write(clkgen_base, CLK_C_ISP0_CTRL, 0x00000000);
+			reg_write(vin->clkgen_base, CLK_C_ISP0_CTRL, 0x00000000);
 		else
-			reg_write(clkgen_base, CLK_C_ISP0_CTRL, 0x02000000);
+			reg_write(vin->clkgen_base, CLK_C_ISP0_CTRL, 0x02000000);
 	}
 
 	if (vin->isp1) {
 		/* enable isp1 clk */
-		reg_write(clkgen_base, CLK_ISP1_CTRL, 0x80000002);	//ISP1-CLK
-		reg_write(clkgen_base, CLK_ISP1_2X_CTRL, 0x80000000);
-		reg_write(clkgen_base, CLK_ISP1_MIPI_CTRL, 0x80000000);
-
-		reg_write(clkgen_base, CLK_MIPI_RX1_PXL_CTRL, 0x00000008);	//0x00000010);colorbar
+		reg_write(vin->clkgen_base, CLK_ISP1_CTRL, 0x80000002);	//ISP1-CLK
+		reg_write(vin->clkgen_base, CLK_ISP1_2X_CTRL, 0x80000000);
+		reg_write(vin->clkgen_base, CLK_ISP1_MIPI_CTRL, 0x80000000);
+		reg_write(vin->clkgen_base, CLK_MIPI_RX1_PXL_CTRL, 0x00000008);	//0x00000010);colorbar
 
 		if (vin->format.format == SRC_COLORBAR_VIN_ISP)
-			reg_write(clkgen_base, CLK_C_ISP1_CTRL, 0x00000000);
+			reg_write(vin->clkgen_base, CLK_C_ISP1_CTRL, 0x00000000);
 		else
-			reg_write(clkgen_base, CLK_C_ISP1_CTRL, 0x02000000);
+			reg_write(vin->clkgen_base, CLK_C_ISP1_CTRL, 0x02000000);
 	}
 }
 EXPORT_SYMBOL(isp_clk_set);
@@ -153,17 +204,237 @@ void isp_ddr_config(struct stf_vin_dev *vin)
 }
 EXPORT_SYMBOL(isp_ddr_config);
 
-void isp_base_addr_config(struct stf_vin_dev *vin)
+#define REGVALLIST(list) (list), sizeof(list)/sizeof((list)[0])
+
+typedef struct
+{
+	u32 addr;
+	u32 val;
+} regval_t;
+
+static const regval_t isp_800_480_reg_config_list[] = {
+	{0x00000014, 0x0000000D},
+	{0x00000018, 0x000011BB},
+	{0x00000A1C, 0x00000032},
+	{0x0000001C, 0x00000000},
+	{0x00000020, 0x01df031f},
+	{0x00000A0C, 0x01e00320},
+	{0x00000A80, 0xF9000000},
+	{0x00000A84, 0xF905DC00},
+	{0x00000A88, 0x00000320},
+	{0x00000A8C, 0x00000000},
+	{0x00000A90, 0x00000000},
+	{0x00000E40, 0x0000004C},
+	{0x00000E44, 0x00000097},
+	{0x00000E48, 0x0000001D},
+	{0x00000E4C, 0x000001D5},
+	{0x00000E50, 0x000001AC},
+	{0x00000E54, 0x00000080},
+	{0x00000E58, 0x00000080},
+	{0x00000E5C, 0x00000194},
+	{0x00000E60, 0x000001EC},
+	{0x00000280, 0x00000000},
+	{0x00000284, 0x00000000},
+	{0x00000288, 0x00000000},
+	{0x0000028C, 0x00000000},
+	{0x00000290, 0x00000000},
+	{0x00000294, 0x00000000},
+	{0x00000298, 0x00000000},
+	{0x0000029C, 0x00000000},
+	{0x000002A0, 0x00000000},
+	{0x000002A4, 0x00000000},
+	{0x000002A8, 0x00000000},
+	{0x000002AC, 0x00000000},
+	{0x000002B0, 0x00000000},
+	{0x000002B4, 0x00000000},
+	{0x000002B8, 0x00000000},
+	{0x000002BC, 0x00000000},
+	{0x000002C0, 0x00F000F0},
+	{0x000002C4, 0x00F000F0},
+	{0x000002C8, 0x00800080},
+	{0x000002CC, 0x00800080},
+	{0x000002D0, 0x00800080},
+	{0x000002D4, 0x00800080},
+	{0x000002D8, 0x00B000B0},
+	{0x000002DC, 0x00B000B0},
+	{0x00000E00, 0x24000000},
+	{0x00000E04, 0x159500A5},
+	{0x00000E08, 0x0F9900EE},
+	{0x00000E0C, 0x0CE40127},
+	{0x00000E10, 0x0B410157},
+	{0x00000E14, 0x0A210181},
+	{0x00000E18, 0x094B01A8},
+	{0x00000E1C, 0x08A401CC},
+	{0x00000E20, 0x081D01EE},
+	{0x00000E24, 0x06B20263},
+	{0x00000E28, 0x05D802C7},
+	{0x00000E2C, 0x05420320},
+	{0x00000E30, 0x04D30370},
+	{0x00000E34, 0x047C03BB},
+	{0x00000E38, 0x043703FF},
+	{0x00000010, 0x00000080},
+	{0x00000A08, 0x10000032},
+	{0x00000A00, 0x00120002},
+	{0x00000A00, 0x00120000},
+	{0x00000A50, 0x00000002},
+	{0x00000A00, 0x00120001},
+	{0x00000008, 0x00010000},
+	{0x00000008, 0x0002000A},
+	{0x00000000, 0x00000001},
+};
+
+static const regval_t isp_1080p_reg_config_list[] = {
+	{0x00000014, 0x0000000D},
+	{0x00000018, 0x000011BB},
+	{0x00000A1C, 0x00000032},
+	{0x0000001C, 0x00000000},
+	{0x00000020, 0x0437077F},
+	{0x00000A0C, 0x04380780},
+	{0x00000A80, 0xF9000000},
+	{0x00000A84, 0xF91FA400},
+	{0x00000A88, 0x00000780},
+	{0x00000A8C, 0x00000000},
+	{0x00000A90, 0x00000000},
+	{0x00000E40, 0x0000004C},
+	{0x00000E44, 0x00000097},
+	{0x00000E48, 0x0000001D},
+	{0x00000E4C, 0x000001D5},
+	{0x00000E50, 0x000001AC},
+	{0x00000E54, 0x00000080},
+	{0x00000E58, 0x00000080},
+	{0x00000E5C, 0x00000194},
+	{0x00000E60, 0x000001EC},
+	{0x00000280, 0x00000000},
+	{0x00000284, 0x00000000},
+	{0x00000288, 0x00000000},
+	{0x0000028C, 0x00000000},
+	{0x00000290, 0x00000000},
+	{0x00000294, 0x00000000},
+	{0x00000298, 0x00000000},
+	{0x0000029C, 0x00000000},
+	{0x000002A0, 0x00000000},
+	{0x000002A4, 0x00000000},
+	{0x000002A8, 0x00000000},
+	{0x000002AC, 0x00000000},
+	{0x000002B0, 0x00000000},
+	{0x000002B4, 0x00000000},
+	{0x000002B8, 0x00000000},
+	{0x000002BC, 0x00000000},
+	{0x000002C0, 0x00F000F0},
+	{0x000002C4, 0x00F000F0},
+	{0x000002C8, 0x00800080},
+	{0x000002CC, 0x00800080},
+	{0x000002D0, 0x00800080},
+	{0x000002D4, 0x00800080},
+	{0x000002D8, 0x00B000B0},
+	{0x000002DC, 0x00B000B0},
+	{0x00000E00, 0x24000000},
+	{0x00000E04, 0x159500A5},
+	{0x00000E08, 0x0F9900EE},
+	{0x00000E0C, 0x0CE40127},
+	{0x00000E10, 0x0B410157},
+	{0x00000E14, 0x0A210181},
+	{0x00000E18, 0x094B01A8},
+	{0x00000E1C, 0x08A401CC},
+	{0x00000E20, 0x081D01EE},
+	{0x00000E24, 0x06B20263},
+	{0x00000E28, 0x05D802C7},
+	{0x00000E2C, 0x05420320},
+	{0x00000E30, 0x04D30370},
+	{0x00000E34, 0x047C03BB},
+	{0x00000E38, 0x043703FF},
+	{0x00000010, 0x00000080},
+	{0x00000A08, 0x10000032},
+	{0x00000A00, 0x00120002},
+	{0x00000A00, 0x00120000},
+	{0x00000A50, 0x00000002},
+	{0x00000A00, 0x00120001},
+	{0x00000008, 0x00010000},
+	{0x00000008, 0x0002000A},
+	{0x00000000, 0x00000001},
+};
+
+const struct {
+	const regval_t * regval;
+	int regval_num;
+} isp_1920_1080_settings[] = {
+	{REGVALLIST(isp_1080p_reg_config_list)},
+};
+
+const struct {
+	const regval_t * regval;
+	int regval_num;
+} isp_800_480_settings[] = {
+	{REGVALLIST(isp_800_480_reg_config_list)},
+};
+
+void isp_config(struct stf_vin_dev *vin,int isp_id)
 {
+	void __iomem *ispbase;
+	int j;
+    u32 mipi_vc = 0;
+	u32 mipi_channel_sel,vin_src_chan_sel;
+	u32 y_start = vin->buf.paddr;
+	u32 uv_start = y_start + vin->frame.width*vin->frame.height;
+	ISP_INFO("  y_start 0x%08x, uv_start 0x%08x\n", y_start, uv_start);
+	ISP_INFO("  config isp %d <-- mipi %d:\n", isp_id, vin->csi_fmt.mipi_id);
+	ISP_INFO("  h_size %d, v_size %d\n", vin->frame.width, vin->frame.height);
 	if(vin->isp0)
-		vin->base_isp = ioremap(VIN_ISP0_BASE_ADDR, 0x30000);
+		ispbase = vin->isp_isp0_base;
 	else if(vin->isp1)
-		vin->base_isp = ioremap(VIN_ISP1_BASE_ADDR, 0x30000);
-    else{
+	    ispbase = vin->isp_isp1_base;
+	else
 		return;
+
+    if(vin->csi_fmt.mipi_id==0)
+		reg_write(vin->clkgen_base, CLK_MIPI_RX0_PXL_CTRL, 0x3);
+	else
+		reg_write(vin->clkgen_base, CLK_MIPI_RX1_PXL_CTRL, 0x3);
+	
+    if(isp_id==0){
+	    reg_write(vin->clkgen_base, CLK_ISP0_MIPI_CTRL, 0x80000000|(vin->csi_fmt.mipi_id<<24));
+	    reg_write(vin->clkgen_base, CLK_C_ISP0_CTRL, vin->csi_fmt.mipi_id<<24);
+    }
+	else{
+	    reg_write(vin->clkgen_base, CLK_ISP1_MIPI_CTRL, 0x80000000|(vin->csi_fmt.mipi_id<<24));
+	    reg_write(vin->clkgen_base, CLK_C_ISP1_CTRL, vin->csi_fmt.mipi_id<<24);
     }
+    mipi_channel_sel = vin->csi_fmt.mipi_id*4+mipi_vc;
+    vin_src_chan_sel = reg_read(vin->sysctrl_base, SYSCTRL_VIN_SRC_CHAN_SEL);
+
+    if (isp_id == 0) {
+        vin_src_chan_sel &= ~0xf;
+        vin_src_chan_sel |= mipi_channel_sel & 0xf;
+    } else {
+        vin_src_chan_sel &= ~0xf0;
+        vin_src_chan_sel |= (mipi_channel_sel & 0xf) << 4;
+    }
+    reg_write(vin->sysctrl_base, SYSCTRL_VIN_SRC_CHAN_SEL, vin_src_chan_sel);
+
+    // vin padding mipi output from raw10 to raw12
+    if (vin->csi_fmt.dt == DT_RAW10) {
+        uint32_t vin_src_dw_sel = reg_read(vin->sysctrl_base, SYSCTRL_VIN_SRC_DW_SEL);
+        vin_src_dw_sel |= 1<<(isp_id==0?4:5);
+        reg_write(vin->sysctrl_base, SYSCTRL_VIN_SRC_DW_SEL, vin_src_dw_sel);
+    }
+
+	if (VD_WIDTH_1080P == vin->frame.width) {
+		for (j = 0; j < isp_1920_1080_settings[0].regval_num; j++) {
+			reg_write(ispbase,
+				  isp_1920_1080_settings[0].regval[j].addr,
+				  isp_1920_1080_settings[0].regval[j].val);
+			mdelay(5);
+		}
+	} else if (SEEED_WIDTH_800 == vin->frame.width) {
+		for (j = 0; j < isp_800_480_settings[0].regval_num; j++) {
+			reg_write(ispbase,
+				  isp_800_480_settings[0].regval[j].addr,
+				  isp_800_480_settings[0].regval[j].val);
+			mdelay(5);
+		}
+	}
 }
-EXPORT_SYMBOL(isp_base_addr_config);
+EXPORT_SYMBOL(isp_config);
 
 MODULE_AUTHOR("StarFive Technology Co., Ltd.");
 MODULE_DESCRIPTION("loadable ISP driver for StarFive");
diff --git a/drivers/media/platform/starfive/stf_isp.h b/drivers/media/platform/starfive/stf_isp.h
index 6fc8cafd4777..b918dded372a 100644
--- a/drivers/media/platform/starfive/stf_isp.h
+++ b/drivers/media/platform/starfive/stf_isp.h
@@ -11,6 +11,6 @@
 
 extern void isp_clk_set(struct stf_vin_dev *vin);
 extern void isp_ddr_config(struct stf_vin_dev *vin);
-extern void isp_base_addr_config(struct stf_vin_dev *vin);
+extern void isp_config(struct stf_vin_dev *vin,int isp_id);
 
 #endif
diff --git a/drivers/media/platform/starfive/stf_vin.c b/drivers/media/platform/starfive/stf_vin.c
index 6fe19d20376a..bb86e6a5cb2e 100644
--- a/drivers/media/platform/starfive/stf_vin.c
+++ b/drivers/media/platform/starfive/stf_vin.c
@@ -19,6 +19,7 @@
 #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>
@@ -28,9 +29,25 @@
 #include <linux/uaccess.h>
 #include <video/stf-vin.h>
 #include "stf_isp.h"
+#include "stf_csi.h"
+
+
+static const struct reg_name mem_reg_name[] = {
+	{"mipi0"},
+	{"vclk"},
+	{"vrst"},
+	{"mipi1"},
+	{"sctrl"},
+	{"isp0"},
+	{"isp1"},
+	{"tclk"},
+	{"trst"},
+	{"iopad"}
+};
 
 static DEFINE_MUTEX(vin_mutex);
-static void __iomem *isp0_base;
+
+
 
 static inline u32 reg_read(void __iomem * base, u32 reg)
 {
@@ -79,7 +96,7 @@ static int vin_open(struct inode *inode, struct file *file)
 	dev=container_of(inode->i_cdev, struct stf_vin_dev, vin_cdev);
 
 	file->private_data = dev;
-out:
+//out:
 	mutex_unlock(&vin_mutex);
 	return ret;
 }
@@ -156,6 +173,49 @@ static const struct file_operations vin_fops = {
 	.mmap = vin_mmap,
 };
 
+static int vin_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 < sizeof(mem_reg_name)/sizeof(struct 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 {
+			dev_err(&pdev->dev, "Could not match resource name\n");
+		}
+	}
+
+	return 0;
+}
+
 /*-------------------------------------------------------*/
 /*
  * NOTE:
@@ -164,38 +224,50 @@ static const struct file_operations vin_fops = {
  * TODO: vic clk driver
  *
  */
-static int vin_clk_reset(void)
+static int vin_clk_reset(struct stf_vin_dev *vin)
 {
-	void __iomem *vin_clk_gen_base
-	    = ioremap(VIN_CLKGEN_BASE_ADDR, 0x1000);
 	//disable clk
-    reg_clear_highest_bit(vin_clk_gen_base,CLK_VIN_SRC_CTRL);
-    reg_clear_highest_bit(vin_clk_gen_base,CLK_ISP0_AXI_CTRL);
-    reg_clear_highest_bit(vin_clk_gen_base,CLK_ISP0NOC_AXI_CTRL);
-    reg_clear_highest_bit(vin_clk_gen_base,CLK_ISPSLV_AXI_CTRL);
-    reg_clear_highest_bit(vin_clk_gen_base,CLK_ISP1_AXI_CTRL);
-	reg_clear_highest_bit(vin_clk_gen_base,CLK_ISP1NOC_AXI_CTRL);
-    reg_clear_highest_bit(vin_clk_gen_base,CLK_VIN_AXI);
-    reg_clear_highest_bit(vin_clk_gen_base,CLK_VINNOC_AXI);
+	reg_clear_highest_bit(vin->vin_top_clkgen_base,CLK_VIN_SRC_CTRL);
+	reg_clear_highest_bit(vin->vin_top_clkgen_base,CLK_ISP0_AXI_CTRL);
+	reg_clear_highest_bit(vin->vin_top_clkgen_base,CLK_ISP0NOC_AXI_CTRL);
+	reg_clear_highest_bit(vin->vin_top_clkgen_base,CLK_ISPSLV_AXI_CTRL);
+	reg_clear_highest_bit(vin->vin_top_clkgen_base,CLK_ISP1_AXI_CTRL);
+	reg_clear_highest_bit(vin->vin_top_clkgen_base,CLK_ISP1NOC_AXI_CTRL);
+	reg_clear_highest_bit(vin->vin_top_clkgen_base,CLK_VIN_AXI);
+	reg_clear_highest_bit(vin->vin_top_clkgen_base,CLK_VINNOC_AXI);
 
 	//enable clk
-    reg_set_highest_bit(vin_clk_gen_base,CLK_VIN_SRC_CTRL);
-    reg_set_highest_bit(vin_clk_gen_base,CLK_ISP0_AXI_CTRL);
-    reg_set_highest_bit(vin_clk_gen_base,CLK_ISP0NOC_AXI_CTRL);
-    reg_set_highest_bit(vin_clk_gen_base,CLK_ISPSLV_AXI_CTRL);
-    reg_set_highest_bit(vin_clk_gen_base,CLK_ISP1_AXI_CTRL);
-	reg_set_highest_bit(vin_clk_gen_base,CLK_ISP1NOC_AXI_CTRL);
-    reg_set_highest_bit(vin_clk_gen_base,CLK_VIN_AXI);
-    reg_set_highest_bit(vin_clk_gen_base,CLK_VINNOC_AXI);
+	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);
+	return 0;
+}
+
+static int isp_rstgen_assert_reset(struct stf_vin_dev *vin)
+{
+	 u32 val;
+	 u32 val_reg_reset_config = 0x19f807;
+
+	 val = ioread32(vin->rstgen_base + SOFTWARE_RESET_ASSERT0);
+	 val |= val_reg_reset_config;
+	 iowrite32(val, vin->rstgen_base + SOFTWARE_RESET_ASSERT0);
+
+	 val = ioread32(vin->rstgen_base + SOFTWARE_RESET_ASSERT0);
+	 val &= ~(val_reg_reset_config);
+
+	 iowrite32(val, vin->rstgen_base + SOFTWARE_RESET_ASSERT0);
 
 	return 0;
 }
 
-static int vin_rstgen_assert_reset(void)
+static int vin_rstgen_assert_reset(struct stf_vin_dev *vin)
 {
 	u32 val;
-	void __iomem *vin_rst_gen_base
-	    = ioremap(VIN_RSTGEN_BASE_ADDR, 0x1000);
 	/*
 	 *      Software_RESET_assert1 (0x11840004)
 	 *      ------------------------------------
@@ -211,60 +283,94 @@ static int vin_rstgen_assert_reset(void)
 	 */
 	u32 val_reg_reset_config = 0x7f8000;
 
-	val = ioread32(vin_rst_gen_base + SOFTWARE_RESET_ASSERT1);
+	val = ioread32(vin->vin_top_rstgen_base + SOFTWARE_RESET_ASSERT1);
 	val |= val_reg_reset_config;
-	iowrite32(val, vin_rst_gen_base + SOFTWARE_RESET_ASSERT1);
+	iowrite32(val, vin->vin_top_rstgen_base + SOFTWARE_RESET_ASSERT1);
 
-	val = ioread32(vin_rst_gen_base + SOFTWARE_RESET_ASSERT1);
+	val = ioread32(vin->vin_top_rstgen_base + SOFTWARE_RESET_ASSERT1);
 	val &= ~(val_reg_reset_config);
 
-	iowrite32(val, vin_rst_gen_base + SOFTWARE_RESET_ASSERT1);
+	iowrite32(val, vin->vin_top_rstgen_base + SOFTWARE_RESET_ASSERT1);
 
 	return 0;
 }
 
 /*
- * TODO: should replace with DTS pinctrl
+ * vin_get_pixel_size:
  *
+ * size = (width * height * 4)/(cnfg_axiwr_pix_ct / 2)
  */
+static int vin_get_axiwr_pixel_size(struct stf_vin_dev *vin)
+{
+	u32 value;
+	int cnfg_axiwr_pix_ct;
+
+	value = reg_read(vin->sysctrl_base, SYSCTRL_VIN_RW_CTRL) & 0x3;
+	if (value == 0)
+		cnfg_axiwr_pix_ct = 2;
+	else if (value == 1)
+		cnfg_axiwr_pix_ct = 4;
+	else if (value == 2)
+		cnfg_axiwr_pix_ct = 8;
+	else
+		return 0;
+
+	return (vin->frame.height * vin->frame.width * 4) / (cnfg_axiwr_pix_ct /
+							     2);
+}
+
+static int vin_update_buf_size(struct stf_vin_dev *vin)
+{
+	u32 size;
+	if (vin->format.format == SRC_DVP_SENSOR_VIN_OV5640) //ov5640 out rgb565
+		size = vin->frame.height*vin->frame.width*2;
+	else
+		size = vin_get_axiwr_pixel_size(vin); //out raw data
+
+	if (size == 0) {
+		dev_err(vin->dev, "size is 0");
+		return -EINVAL;
+	}
+
+	vin->buf.size = size;
+
+	return 0;
+}
+
 
-static void dvp_io_pad_func_shared_config(void)
+static void dvp_io_pad_func_shared_config(struct stf_vin_dev *vin)
 {
-	void __iomem *vin_rst_gen_base
-	    = ioremap(VIN_IOPAD_BASE_ADDR, 0x1000);
 	/*
 	 * pin: 49 ~ 57
 	 * offset: 0x144 ~ 0x164
 	 * SCFG_funcshare_pad_ctrl
 	 */
 	u32 val_scfg_funcshare_config = 0x800080;
-	iowrite32(val_scfg_funcshare_config, vin_rst_gen_base + IOPAD_REG81);
-	iowrite32(val_scfg_funcshare_config, vin_rst_gen_base + IOPAD_REG82);
-	iowrite32(val_scfg_funcshare_config, vin_rst_gen_base + IOPAD_REG83);
-	iowrite32(val_scfg_funcshare_config, vin_rst_gen_base + IOPAD_REG84);
-	iowrite32(val_scfg_funcshare_config, vin_rst_gen_base + IOPAD_REG85);
-	iowrite32(val_scfg_funcshare_config, vin_rst_gen_base + IOPAD_REG86);
-	iowrite32(val_scfg_funcshare_config, vin_rst_gen_base + IOPAD_REG87);
-	iowrite32(val_scfg_funcshare_config, vin_rst_gen_base + IOPAD_REG88);
-	iowrite32(val_scfg_funcshare_config, vin_rst_gen_base + IOPAD_REG89);
+	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 vin_rstgen_clkgen(struct stf_vin_dev *vin)
 {
-	void __iomem *rstgen_base = vin->base + VIN_RSTGEN_OFFSET;
-	void __iomem *clkgen_base = vin->base + VIN_CLKGEN_OFFSET;
 	u32 val_vin_axi_wr_ctrl_reg = 0x02000000;
 
 	/* rst disable */
-	reg_write(rstgen_base, SOFTWARE_RESET_ASSERT0, 0xFFFFFFFF);
+	reg_write(vin->rstgen_base, SOFTWARE_RESET_ASSERT0, 0xFFFFFFFF);
 
 	/* rst enable */
-	reg_write(rstgen_base, SOFTWARE_RESET_ASSERT0, 0x0);
+	reg_write(vin->rstgen_base, SOFTWARE_RESET_ASSERT0, 0x0);
 
 	switch (vin->format.format) {
 	case SRC_DVP_SENSOR_VIN_OV5640:
 	case SRC_DVP_SENSOR_VIN:
-		reg_write(clkgen_base, CLK_VIN_AXI_WR_CTRL, val_vin_axi_wr_ctrl_reg);
+		reg_write(vin->clkgen_base, CLK_VIN_AXI_WR_CTRL, val_vin_axi_wr_ctrl_reg);
 		break;
 
 	case SRC_COLORBAR_VIN_ISP:
@@ -283,11 +389,10 @@ static int vin_rstgen_clkgen(struct stf_vin_dev *vin)
 
 static int get_vin_axiwr_pix_ct(struct stf_vin_dev *vin)
 {
-	void __iomem *sysctrl_base = vin->base + VIN_SYSCONTROLLER_OFFSET;
 	u32 value;
 	int cnfg_axiwr_pix_ct = 0;
 
-	value = reg_read(sysctrl_base, SYSCTRL_REG14) & 0x3;
+	value = reg_read(vin->sysctrl_base, SYSCTRL_VIN_RW_CTRL) & 0x3;
 	if (value == 0)
 		cnfg_axiwr_pix_ct = 2;
 	else if (value == 1)
@@ -302,48 +407,46 @@ static int get_vin_axiwr_pix_ct(struct stf_vin_dev *vin)
 
 static void set_vin_wr_pix_total(struct stf_vin_dev *vin)
 {
-	void __iomem *sysctrl_base = vin->base + VIN_SYSCONTROLLER_OFFSET;
 	int val, pix_ct;
 	pix_ct = get_vin_axiwr_pix_ct(vin);
 	val = (vin->frame.width / pix_ct) - 1;
-	reg_write(sysctrl_base, SYSCTRL_REG12, val);
-}
-
-static int stf_vin_clk_init(struct stf_vin_dev *vin)
-{
-	int ret=0;
-	ret = vin_clk_reset();
-	ret |= vin_rstgen_assert_reset();
-	ret |= vin_rstgen_clkgen(vin);
-	return ret;
+	reg_write(vin->sysctrl_base, SYSCTRL_VIN_WR_PIX_TOTAL, val);
 }
 
 static void dvp_vin_ddr_addr_config(struct stf_vin_dev *vin)
 {
-	void __iomem *sysctrl_base = vin->base + VIN_SYSCONTROLLER_OFFSET;
 	u32 val_vin_axi_ctrl_reg = 0x00000003;
 	dev_dbg(vin->dev, "%d: addr: 0x%lx, size: 0x%lx\n", __LINE__,
 		(long)vin->buf.paddr, (long)(vin->buf.paddr + vin->buf.size));
 	/* set the start address of vin and enable */
-	reg_write(sysctrl_base, SYSCTRL_REG10, (long)vin->buf.paddr);
-	reg_write(sysctrl_base, SYSCTRL_REG11, (long)(vin->buf.paddr + vin->buf.size));
-	reg_write(sysctrl_base, SYSCTRL_REG6, val_vin_axi_ctrl_reg);
+	reg_write(vin->sysctrl_base, SYSCTRL_VIN_WR_START_ADDR, (long)vin->buf.paddr);
+	reg_write(vin->sysctrl_base, SYSCTRL_VIN_RD_END_ADDR, (long)(vin->buf.paddr + vin->buf.size));
+	reg_write(vin->sysctrl_base, SYSCTRL_VIN_AXI_CTRL, val_vin_axi_ctrl_reg);
 }
 
 static int stf_vin_config_set(struct stf_vin_dev *vin)
 {
-	void __iomem *sysctrl_base = vin->base + VIN_SYSCONTROLLER_OFFSET;
-	u32 val_vin_rd_pix_total_reg ;
-	u32 val_vin_rd_vblank_reg ;
-	u32 val_vin_rd_vend_reg ;
-	u32 val_vin_rd_hblank_reg ;
-	u32 val_vin_rd_hend_reg ;
-	u32 val_vin_rw_ctrl_reg ;
-	u32 val_vin_src_channel_reg ;
-	u32 val_vin_axi_ctrl_reg ;
-	u32 val_vin_rw_start_addr_reg ;
-	u32 val_vin_rd_end_addr_reg ;
-	u32 val_vin_wr_pix_reg ;
+	u32  val_vin_rd_pix_total_reg,val_vin_rd_vblank_reg,val_vin_rd_vend_reg;
+	u32  val_vin_rd_hblank_reg,val_vin_rd_hend_reg,val_vin_rw_ctrl_reg;
+	u32  val_vin_src_channel_reg,val_vin_axi_ctrl_reg,val_vin_rw_start_addr_reg;
+	u32  val_vin_rd_end_addr_reg,val_vin_wr_pix_reg,reset_val;
+//	int ret;
+	csi2rx_dphy_cfg_t dphy_rx_cfg = {
+			.dlane_nb		= vin->csi_fmt.lane,
+			.dlane_map		= {vin->csi_fmt.dlane_swap[0],	  vin->csi_fmt.dlane_swap[1],	 vin->csi_fmt.dlane_swap[2],	vin->csi_fmt.dlane_swap[3]},
+			.dlane_pn_swap	= {vin->csi_fmt.dlane_pn_swap[0], vin->csi_fmt.dlane_pn_swap[1], vin->csi_fmt.dlane_pn_swap[2], vin->csi_fmt.dlane_pn_swap[3]},
+			.dlane_en		= {vin->csi_fmt.lane>0,   vin->csi_fmt.lane>1,   vin->csi_fmt.lane>2, 	vin->csi_fmt.lane>3},
+			.clane_nb		= 1,
+			.clane_map		= {vin->csi_fmt.clane_swap, 	5},
+			.clane_pn_swap	= {vin->csi_fmt.clane_pn_swap,	0},
+	};
+	csi2rx_cfg_t csi2rx_cfg = {
+				.lane_nb	= vin->csi_fmt.lane,//lane count
+				.dlane_map	= {1,2,3,4},
+				.dt 		= vin->csi_fmt.dt,
+				.hsize		= vin->csi_fmt.w,
+				.vsize		= vin->csi_fmt.h,
+	};
 
 	switch (vin->format.format) {
 	case SRC_COLORBAR_VIN_ISP:
@@ -357,21 +460,21 @@ static int stf_vin_config_set(struct stf_vin_dev *vin)
 		val_vin_src_channel_reg = 0x00000088;
 		val_vin_axi_ctrl_reg = 0x00000004;
 
-		reg_write(sysctrl_base, SYSCTRL_REG13, val_vin_rd_pix_total_reg);
-		reg_write(sysctrl_base, SYSCTRL_REG17, val_vin_rd_vblank_reg);
-		reg_write(sysctrl_base, SYSCTRL_REG18, val_vin_rd_vend_reg);
-		reg_write(sysctrl_base, SYSCTRL_REG19, val_vin_rd_hblank_reg);
-		reg_write(sysctrl_base, SYSCTRL_REG20, val_vin_rd_hend_reg);
-		reg_write(sysctrl_base, SYSCTRL_REG14, val_vin_rw_ctrl_reg);
-		reg_write(sysctrl_base, SYSCTRL_REG15, val_vin_src_channel_reg);
-		reg_write(sysctrl_base, SYSCTRL_REG6, val_vin_axi_ctrl_reg);
-		isp_base_addr_config(vin);
+		reg_write(vin->sysctrl_base, SYSCTRL_VIN_RD_PIX_TOTAL, val_vin_rd_pix_total_reg);
+		reg_write(vin->sysctrl_base, SYSCTRL_VIN_RD_VBLANK, val_vin_rd_vblank_reg);
+		reg_write(vin->sysctrl_base, SYSCTRL_VIN_RD_VEND, val_vin_rd_vend_reg);
+		reg_write(vin->sysctrl_base, SYSCTRL_VIN_RD_HBLANK, val_vin_rd_hblank_reg);
+		reg_write(vin->sysctrl_base, SYSCTRL_VIN_RD_HEND, val_vin_rd_hend_reg);
+		reg_write(vin->sysctrl_base, SYSCTRL_VIN_RW_CTRL, val_vin_rw_ctrl_reg);
+		reg_write(vin->sysctrl_base, SYSCTRL_VIN_SRC_CHAN_SEL, val_vin_src_channel_reg);
+		reg_write(vin->sysctrl_base, SYSCTRL_VIN_AXI_CTRL, val_vin_axi_ctrl_reg);
+
 		if(vin->isp0||vin->isp1)
 			isp_ddr_config(vin);
 		break;
 
 	case SRC_DVP_SENSOR_VIN:
-		dvp_io_pad_func_shared_config();
+		dvp_io_pad_func_shared_config(vin);
 		/*
 		 * NOTE:
 		 * 0x38: [13:12]
@@ -379,37 +482,42 @@ static int stf_vin_config_set(struct stf_vin_dev *vin)
 		 * 01: pix_data[9:2]
 		 */
 	    val_vin_rw_ctrl_reg = 0x00010301;
-		reg_write(sysctrl_base, SYSCTRL_REG14, val_vin_rw_ctrl_reg);
+		reg_write(vin->sysctrl_base, SYSCTRL_VIN_RW_CTRL, val_vin_rw_ctrl_reg);
 
 		set_vin_wr_pix_total(vin);
 	break;
 
 	case SRC_DVP_SENSOR_VIN_OV5640:
-		dvp_io_pad_func_shared_config();
-	    val_vin_rw_start_addr_reg = FB_FIRST_ADDR;
-		val_vin_rd_end_addr_reg = FB_FIRST_ADDR+0x3f4800;
+		dvp_io_pad_func_shared_config(vin);
+	    val_vin_rw_start_addr_reg = vin->buf.paddr;
+		val_vin_rd_end_addr_reg = vin->buf.paddr+vin->frame.width*vin->frame.height*2;
 		val_vin_wr_pix_reg = 0x000001df;
 		val_vin_rw_ctrl_reg = 0x00001202;
 		val_vin_axi_ctrl_reg = 0x00000003;
-		reg_write(sysctrl_base, SYSCTRL_REG10, val_vin_rw_start_addr_reg);	// First frame address
-		reg_write(sysctrl_base, SYSCTRL_REG11, val_vin_rd_end_addr_reg);	//4bytes 0x887e9000 2bytes 0x883f4800, 1byte 0x881FA400
-		reg_write(sysctrl_base, SYSCTRL_REG12, val_vin_wr_pix_reg);	//       0x000003bf        0x000001df        0x000000ef
-		reg_write(sysctrl_base, SYSCTRL_REG14, val_vin_rw_ctrl_reg);
-		reg_write(sysctrl_base, SYSCTRL_REG6,  val_vin_axi_ctrl_reg);
-		break;
+		reg_write(vin->sysctrl_base, SYSCTRL_VIN_WR_START_ADDR, val_vin_rw_start_addr_reg);	// First frame address
+		reg_write(vin->sysctrl_base, SYSCTRL_VIN_RD_END_ADDR, val_vin_rd_end_addr_reg);	//4bytes 0x887e9000 2bytes 0x883f4800, 1byte 0x881FA400
+		reg_write(vin->sysctrl_base, SYSCTRL_VIN_WR_PIX_TOTAL, val_vin_wr_pix_reg);	// 0x000003bf        0x000001df        0x000000ef
+		reg_write(vin->sysctrl_base, SYSCTRL_VIN_RW_CTRL, val_vin_rw_ctrl_reg);
+		reg_write(vin->sysctrl_base, SYSCTRL_VIN_AXI_CTRL,  val_vin_axi_ctrl_reg);
+	break;
 
 	case SRC_DVP_SENSOR_VIN_ISP:
-		dvp_io_pad_func_shared_config();
+		dvp_io_pad_func_shared_config(vin);
 	    val_vin_rw_ctrl_reg = 0x00011300;
 	    val_vin_src_channel_reg = 0x00001100;
-		reg_write(sysctrl_base, SYSCTRL_REG14, val_vin_rw_ctrl_reg);
-		reg_write(sysctrl_base, SYSCTRL_REG15, val_vin_src_channel_reg);
-		isp_base_addr_config(vin);
+		reg_write(vin->sysctrl_base, SYSCTRL_VIN_RW_CTRL, val_vin_rw_ctrl_reg);
+		reg_write(vin->sysctrl_base, SYSCTRL_VIN_SRC_CHAN_SEL, val_vin_src_channel_reg);
 		if(vin->isp0||vin->isp1)
 			isp_ddr_config(vin);
 	break;
 
 	case SRC_CSI2RX_VIN_ISP:
+		isp_config(vin, 0);
+		csi2rx_dphy_config(vin,&dphy_rx_cfg);
+		csi2rx_config(vin,0, &csi2rx_cfg);
+		reset_val = reg_read(vin->rstgen_base, 0x00);
+		reset_val &= ~(1<<17);
+		reg_write(vin->rstgen_base, 0x00, reset_val);
 	break;
 
 	default:
@@ -420,65 +528,12 @@ static int stf_vin_config_set(struct stf_vin_dev *vin)
 	return 0;
 }
 
-/*
- * vin_get_pixel_size:
- *
- * size = (width * height * 4)/(cnfg_axiwr_pix_ct / 2)
- */
-static int vin_get_axiwr_pixel_size(struct stf_vin_dev *vin)
-{
-	void __iomem *sysctrl_base = vin->base + VIN_SYSCONTROLLER_OFFSET;
-	u32 value;
-	int cnfg_axiwr_pix_ct;
-
-	value = reg_read(sysctrl_base, SYSCTRL_REG14) & 0x3;
-
-	if (value == 0)
-		cnfg_axiwr_pix_ct = 2;
-	else if (value == 1)
-		cnfg_axiwr_pix_ct = 4;
-	else if (value == 2)
-		cnfg_axiwr_pix_ct = 8;
-	else
-		return 0;
-
-	return (vin->frame.height * vin->frame.width * 4) / (cnfg_axiwr_pix_ct /
-							     2);
-}
-
-static int vin_alloc_buf(struct stf_vin_dev *vin)
-{
-	struct vin_buf *buf = &vin->buf;
-	u32 size;
-	if (vin->format.format == SRC_DVP_SENSOR_VIN_OV5640)
-		size = vin->frame.height*vin->frame.width*2;
-	else
-		size = vin_get_axiwr_pixel_size(vin);
-
-	if (size == 0) {
-		dev_err(vin->dev, "size is 0");
-		return -EINVAL;
-	}
-
-	buf->vaddr = dma_alloc_coherent(vin->dev, 2 * size, &buf->paddr,
-					GFP_KERNEL);
-	if (!buf->vaddr) {
-		dev_err(vin->dev,
-			"Failed to allocate buffer of size 0x%x\n", size);
-		return -ENOMEM;
-	}
-
-	buf->size = size;
-
-	return 0;
-}
 
 static void vin_free_buf(struct stf_vin_dev *vin)
 {
 	struct vin_buf *buf = &vin->buf;
 
 	if (buf->vaddr) {
-		dma_free_coherent(vin->dev, buf->size, buf->vaddr, buf->paddr);
 		buf->vaddr = NULL;
 		buf->size = 0;
 	}
@@ -486,8 +541,8 @@ static void vin_free_buf(struct stf_vin_dev *vin)
 
 static void vin_intr_clear(void __iomem * sysctrl_base)
 {
-	reg_write(sysctrl_base, SYSCTRL_REG21, 0x1);
-	reg_write(sysctrl_base, SYSCTRL_REG21, 0x0);
+	reg_write(sysctrl_base, SYSCTRL_VIN_INTP_CTRL, 0x1);
+	reg_write(sysctrl_base, SYSCTRL_VIN_INTP_CTRL, 0x0);
 }
 
 static irqreturn_t vin_wr_irq_handler(int irq, void *priv)
@@ -496,10 +551,7 @@ static irqreturn_t vin_wr_irq_handler(int irq, void *priv)
 	static int wtimes = 0;
 	struct stf_vin_dev *vin = priv;
 
-	void __iomem *sysctrl_base = vin->base + VIN_SYSCONTROLLER_OFFSET;
-
-	/*clear interrupt */
-	vin_intr_clear(sysctrl_base);
+	vin_intr_clear(vin->sysctrl_base);
 
 	wtimes = wtimes % 2;
 	if (wtimes == 0)
@@ -519,11 +571,11 @@ static irqreturn_t vin_wr_irq_handler(int irq, void *priv)
 	return IRQ_HANDLED;
 }
 
-void vin_isp0_intr_clear(void)
+void vin_isp0_intr_clear(struct stf_vin_dev *vin)
 {
 	u32 value;
-	value = reg_read(isp0_base, 0xA00);
-	reg_write(isp0_base, 0xA00, value);
+	value = reg_read(vin->isp_isp0_base, 0xA00);
+	reg_write(vin->isp_isp0_base, 0xA00, value);
 }
 
 static irqreturn_t vin_isp0_irq_handler(int irq, void *priv)
@@ -533,19 +585,19 @@ static irqreturn_t vin_isp0_irq_handler(int irq, void *priv)
 	struct stf_vin_dev *vin = priv;
 
 	/*clear interrupt */
-	vin_isp0_intr_clear();
+	vin_isp0_intr_clear(vin);
 
 	wtimes = wtimes % 2;
-	if (wtimes == 0) {
-		params.paddr = (void *)FB_FIRST_ADDR;
-		reg_write(isp0_base, ISP_REG_Y_PLANE_START_ADDR,  FB_FIRST_ADDR);	// Unscaled Output Image Y Plane Start Address Register
-		reg_write(isp0_base, ISP_REG_UV_PLANE_START_ADDR, FB_FIRST_ADDR+(vin->frame.width*vin->frame.height));	// Unscaled Output Image UV Plane Start Address Register
-		reg_write(isp0_base, ISP_REG_STRIDE, vin->frame.width);	// Unscaled Output Image Stride Register
+	if (wtimes == 1) {
+		params.paddr = (void *)vin->buf.paddr+vin->buf.size;
+		reg_write(vin->isp_isp0_base, ISP_REG_Y_PLANE_START_ADDR,  vin->buf.paddr);	// Unscaled Output Image Y Plane Start Address Register
+		reg_write(vin->isp_isp0_base, ISP_REG_UV_PLANE_START_ADDR, vin->buf.paddr+(vin->frame.width*vin->frame.height));	// Unscaled Output Image UV Plane Start Address Register
+		reg_write(vin->isp_isp0_base, ISP_REG_STRIDE, vin->frame.width);	// Unscaled Output Image Stride Register
 	} else {
-		params.paddr = (void *)FB_SECOND_ADDR;
-		reg_write(isp0_base, ISP_REG_Y_PLANE_START_ADDR,  FB_SECOND_ADDR);	// Unscaled Output Image Y Plane Start Address Register
-		reg_write(isp0_base, ISP_REG_UV_PLANE_START_ADDR, FB_SECOND_ADDR+(vin->frame.width*vin->frame.height));	// Unscaled Output Image UV Plane Start Address Register
-		reg_write(isp0_base, ISP_REG_STRIDE, vin->frame.width);	// Unscaled Output Image Stride Register
+		params.paddr = (void *)vin->buf.paddr;
+		reg_write(vin->isp_isp0_base, ISP_REG_Y_PLANE_START_ADDR,  vin->buf.paddr+vin->buf.size);	// Unscaled Output Image Y Plane Start Address Register
+		reg_write(vin->isp_isp0_base, ISP_REG_UV_PLANE_START_ADDR, vin->buf.paddr+vin->buf.size+(vin->frame.width*vin->frame.height));	// Unscaled Output Image UV Plane Start Address Register
+		reg_write(vin->isp_isp0_base, ISP_REG_STRIDE, vin->frame.width);	// Unscaled Output Image Stride Register
 	}
 
 	params.size = vin->buf.size;
@@ -564,38 +616,156 @@ static void vin_irq_disable(struct stf_vin_dev *vin)
 {
 	unsigned int mask_value = 0, value = 0;
 
-	void __iomem *sysctrl_base = vin->base + VIN_SYSCONTROLLER_OFFSET;
-
 	/* mask and clear vin interrupt */
 	mask_value = (0x1 << 4) | (0x1 << 20);
 
-	value = 0x1 | (0x1 << 0x16) | mask_value;
-	reg_write(sysctrl_base, SYSCTRL_REG21, value);
+	value = 0x1 | (0x1 << 16) | mask_value;
+	reg_write(vin->sysctrl_base, SYSCTRL_VIN_INTP_CTRL, value);
 
 	value = mask_value;
-	reg_write(sysctrl_base, SYSCTRL_REG21, value);
+	reg_write(vin->sysctrl_base, SYSCTRL_VIN_INTP_CTRL, value);
 }
 
 static void vin_irq_enable(struct stf_vin_dev *vin)
 {
 	unsigned int value = 0;
 
-	void __iomem *sysctrl_base = vin->base + VIN_SYSCONTROLLER_OFFSET;
-
 	value = ~((0x1 << 4) | (0x1 << 20));
 
-	reg_write(sysctrl_base, SYSCTRL_REG21, value);
+	reg_write(vin->sysctrl_base, SYSCTRL_VIN_INTP_CTRL, value);
+}
+
+static int vin_sys_init(struct stf_vin_dev *vin)
+{
+	u32 val;
+
+	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 {
+        printk("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);
+
+	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);
+
+	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);
+	reg_set_highest_bit(vin->clkgen_base,CLK_MIPI_RX0_SYS0_CTRL);
+	reg_set_highest_bit(vin->clkgen_base,CLK_MIPI_RX1_SYS1_CTRL);
+
+	isp_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);
+    return 0;
+}
+
+static 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;
+
+	if (of_property_read_u32(np, "format", &vin->format.format)) {
+		dev_err(dev,"Missing format property in the DT.\n");
+		ret = -EINVAL;
+	}
+	dev_info(dev,"vin->format.format = %d ,in the DT.\n",vin->format.format);
+
+	if (of_property_read_u32(np, "frame-width", &vin->frame.width)) {
+		dev_err(dev,"Missing frame-width property in the DT.\n");
+		ret = -EINVAL;
+	}
+
+	if (of_property_read_u32(np, "frame-height", &vin->frame.height)) {
+		dev_err(dev,"Missing frame.height property in the DT.\n");
+		ret = -EINVAL;
+	}
+
+	vin->isp0 = of_property_read_bool(np, "isp0_enable");
+	vin->isp1 = of_property_read_bool(np, "isp1_enable");
+
+	of_property_read_u8_array(np, "csi-dlane-swaps", vin->csi_fmt.dlane_swap, 4);
+
+	of_property_read_u8_array(np, "csi-dlane-pn-swaps", vin->csi_fmt.dlane_pn_swap, 4);
+
+	of_property_read_u8(np, "csi-clane-swap", &vin->csi_fmt.clane_swap);
+
+	of_property_read_u8(np, "csi-clane-pn-swap", &vin->csi_fmt.clane_pn_swap);
+
+	of_property_read_u32(np, "csi-mipiID", &vin->csi_fmt.mipi_id);
+
+	of_property_read_u32(np, "csi-width", &vin->csi_fmt.w);
+
+	of_property_read_u32(np, "csi-height", &vin->csi_fmt.h);
+
+	of_property_read_u32(np, "csi-dt", &vin->csi_fmt.dt);
+
+	of_property_read_u32(np, "csi-lane", &vin->csi_fmt.lane);
+
+	return ret;
+}
+
+static int stf_vin_clk_init(struct stf_vin_dev *vin)
+{
+	int ret=0;
+	switch (vin->format.format) {
+	case SRC_COLORBAR_VIN_ISP:
+	case SRC_DVP_SENSOR_VIN:
+	case SRC_DVP_SENSOR_VIN_OV5640:
+	case SRC_DVP_SENSOR_VIN_ISP:
+		 ret =  vin_clk_reset(vin);
+	     ret |= vin_rstgen_assert_reset(vin);
+	     ret |= vin_rstgen_clkgen(vin);
+	break;
+
+	case SRC_CSI2RX_VIN_ISP:
+		 ret =vin_sys_init(vin);
+	break;
+
+	default:
+		pr_err("unknown format\n");
+		return -EINVAL;
+	}
+
+	return ret;
 }
 
 static int stf_vin_probe(struct platform_device *pdev)
 {
 	struct stf_vin_dev *vin;
-	struct resource *mem;
 	int irq;
 	dev_t devid;
 	struct class *vin_cls;
 	int major = 0;
 	int ret = 0;
+	struct device_node *node;
+	struct resource res_mem;
+
 	dev_info(&pdev->dev, "vin probe enter!\n");
 
 	vin = devm_kzalloc(&pdev->dev, sizeof(struct stf_vin_dev), GFP_KERNEL);
@@ -614,23 +784,10 @@ static int stf_vin_probe(struct platform_device *pdev)
 		return -ENODEV;
 	}
 
-	isp0_base = ioremap(VIN_ISP0_BASE_ADDR, 0x30000);
-	if (IS_ERR(isp0_base)) {
-		dev_err(&pdev->dev, "Could not map registers\n");
-		return PTR_ERR(isp0_base);
-	}
-
-	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!mem) {
-		dev_err(&pdev->dev, "Could not get resource\n");
-		return -ENODEV;
-	}
-
-	vin->base = devm_ioremap(&pdev->dev, mem->start,
-					 resource_size(mem));
-	if (IS_ERR(vin->base)) {
-		dev_err(&pdev->dev, "Could not map registers\n");
-		return PTR_ERR(vin->base);
+	ret = vin_get_mem_res(pdev,vin);
+	if (ret) {
+			dev_err(&pdev->dev, "Could not map registers\n");
+			goto out;
 	}
 
 	spin_lock_init(&vin->irqlock);
@@ -654,18 +811,21 @@ static int stf_vin_probe(struct platform_device *pdev)
 		goto out;
 	}
 
-	ret = of_reserved_mem_device_init(&pdev->dev);
-	if (ret) {
+	node = of_parse_phandle(pdev->dev.of_node, "memory-region", 0);
+	if (node) {
+		of_address_to_resource(node, 0, &res_mem);
+		vin->buf.paddr = res_mem.start;
+	} else {
 		dev_err(&pdev->dev, "Could not get reserved memory\n");
-		goto out;
+		return -ENOMEM;
 	}
 
-	/* default config */
-	vin->format.format = SRC_DVP_SENSOR_VIN_OV5640;
-	vin->frame.height = VD_HEIGHT_1080P;
-	vin->frame.width = VD_WIDTH_1080P;
-	vin->isp0 = false;
-	vin->isp1 = false;
+	vin->buf.vaddr = devm_ioremap_resource(&pdev->dev, &res_mem);
+	memset(vin->buf.vaddr, 0, RESERVED_MEM_SIZE);
+
+	ret = vin_parse_dt(&pdev->dev, vin);
+	if (ret)
+		goto out;
 
 	vin->condition = false;
 	init_waitqueue_head(&vin->wq);
@@ -687,7 +847,7 @@ static int stf_vin_probe(struct platform_device *pdev)
 		goto out;
 	}
 
-	ret = vin_alloc_buf(vin);
+	ret = vin_update_buf_size(vin);
 	if (ret)
 		goto out;
 
@@ -747,6 +907,7 @@ static struct platform_driver stf_vin_driver = {
 
 static int __init stf_vin_init(void)
 {
+    printk("stf_vin_init vin vin !!!!!!!\n");
 	return platform_driver_register(&stf_vin_driver);
 }
 
diff --git a/drivers/video/fbdev/starfive/Kconfig b/drivers/video/fbdev/starfive/Kconfig
index cb617708752a..8c6ed2b925f0 100644
--- a/drivers/video/fbdev/starfive/Kconfig
+++ b/drivers/video/fbdev/starfive/Kconfig
@@ -1,12 +1,35 @@
 #
 # Video TX driver configuration
 #
-menu "Video TX 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"
-	depends on FB_STARFIVE
 	help
 	  Say Y here if you want to have support for ADV7513 displayer
 
+config FB_STARFIVE_HDMI_TDA998X
+	bool "HDMI TDA998X displayer support"
+	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
+	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
index 80daf1a41b4e..da6c6f8fb2a9 100644
--- a/drivers/video/fbdev/starfive/Makefile
+++ b/drivers/video/fbdev/starfive/Makefile
@@ -4,6 +4,8 @@
 #
 
 obj-$(CONFIG_FB_STARFIVE)		+= starfive_fb.o starfive_lcdc.o starfive_vpp.o \
-					starfive_display_dev.o starfive_displayer.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
index bb4dd0c375fa..7900a4e8d094 100644
--- a/drivers/video/fbdev/starfive/adv7513.c
+++ b/drivers/video/fbdev/starfive/adv7513.c
@@ -146,6 +146,7 @@ static int adv7513_probe(struct i2c_client *client, const struct i2c_device_id *
 	struct device *dev = &client->dev;
     u8 value;
 	int ret = 0;
+	dev_info(dev,"adv7513_probe enter \n");
 
 	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
 		dev_warn(&client->dev,
@@ -166,7 +167,10 @@ static int adv7513_probe(struct i2c_client *client, const struct i2c_device_id *
 	i2c_set_clientdata(client, adv7513);
 
 	adv7513_read(client, 0x00, &value);
-	dev_info(&client->dev, "%s[%d],version = 0x%x\n",__func__,__LINE__,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);
@@ -209,6 +213,7 @@ static int adv7513_probe(struct i2c_client *client, const struct i2c_device_id *
 		default:
 			dev_err(dev,"not support width %d \n",adv7513->def_width);
 	}
+	dev_info(dev,"adv7513_probe done \n");
 
 	return ret;
 }
diff --git a/drivers/video/fbdev/starfive/seeed5inch.c b/drivers/video/fbdev/starfive/seeed5inch.c
new file mode 100644
index 000000000000..3f6fb7543936
--- /dev/null
+++ b/drivers/video/fbdev/starfive/seeed5inch.c
@@ -0,0 +1,242 @@
+/*
+ ******************************************************************************
+ * @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-starfive-vic.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..4c19c58d9c3f
--- /dev/null
+++ b/drivers/video/fbdev/starfive/starfive_comm_regs.h
@@ -0,0 +1,95 @@
+/*
+ * StarFive Vout driver
+ *
+ * Copyright 2020 StarFive Inc.
+ *
+ * Licensed under the GPL-2.
+ */
+
+#ifndef __SF_COMM_REGS_H__
+#define __SF_COMM_REGS_H__
+
+#include <video/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 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 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
index 2997a0721621..85c7b41139e9 100644
--- a/drivers/video/fbdev/starfive/starfive_display_dev.c
+++ b/drivers/video/fbdev/starfive/starfive_display_dev.c
@@ -77,6 +77,33 @@ static int build_dev_list(struct sf_fb_data *fb_data)
 	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;
@@ -101,6 +128,8 @@ struct sf_fb_display_dev* sf_fb_display_dev_get(struct sf_fb_data *fb_data)
 			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);
diff --git a/drivers/video/fbdev/starfive/starfive_display_dev.h b/drivers/video/fbdev/starfive/starfive_display_dev.h
index 13348226e421..8cf65a423d65 100644
--- a/drivers/video/fbdev/starfive/starfive_display_dev.h
+++ b/drivers/video/fbdev/starfive/starfive_display_dev.h
@@ -12,13 +12,12 @@
 #include <linux/fb.h>
 #include <linux/delay.h>
 #include <linux/gpio.h>
-
-#include "starfive_fb.h"
+#include <video/starfive_fb.h>
 
 #define RST_SEQ_LEN		16
 
-#define COMIPFB_HSYNC_HIGH_ACT		(0x03)
-#define COMIPFB_VSYNC_HIGH_ACT		(0x04)
+#define FB_HSYNC_HIGH_ACT		(0x03)
+#define FB_VSYNC_HIGH_ACT		(0x04)
 
 #define MIPI_VIDEO_MODE 		(0x05)
 #define MIPI_COMMAND_MODE 		(0x06)
@@ -172,6 +171,16 @@ struct te_info {
 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;
@@ -184,6 +193,10 @@ struct sf_fb_timing_mipi {
 	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;
@@ -224,7 +237,9 @@ struct sf_fb_display_dev {
 	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 */
-	union {
+
+	struct {
+		struct sf_fb_timing_rgb rgb;
 		//struct comipfb_dev_timing_rgb rgb;
 		struct sf_fb_timing_mipi mipi;
 	} timing;
@@ -252,6 +267,7 @@ struct sf_fb_display_dev {
 
 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 /*__COMIPFB_DEV_H_*/
+#endif /*__SF_FB_DISPLAY_DEV_H_*/
diff --git a/drivers/video/fbdev/starfive/starfive_displayer.c b/drivers/video/fbdev/starfive/starfive_displayer.c
index e23e1ff6ce9d..4945f81a52f3 100644
--- a/drivers/video/fbdev/starfive/starfive_displayer.c
+++ b/drivers/video/fbdev/starfive/starfive_displayer.c
@@ -24,11 +24,15 @@
 #include <linux/errno.h>
 #include <linux/platform_device.h>
 #include <linux/err.h>
+#include <video/starfive_fb.h>
 
-#include "starfive_fb.h"
 #include "starfive_display_dev.h"
 
+#ifdef _ALIGN_UP
+#undef _ALIGN_UP
 #define _ALIGN_UP(addr, size) (((addr)+((size)-1))&(~((typeof(addr))(size)-1)))
+#endif
+
 #define DSI_CMD_LEN(hdr)	(sizeof(*hdr) + (hdr)->dlen)
 
 static int sf_displayer_reset(struct sf_fb_data *fbi)
@@ -115,13 +119,13 @@ static int of_parse_video_mode(struct device_node *np,
 	}
 	videomode_info->vfp = temp_val;
 
-	videomode_info->sync_pol = COMIPFB_VSYNC_HIGH_ACT;
+	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 = COMIPFB_VSYNC_HIGH_ACT;
+			videomode_info->sync_pol = FB_VSYNC_HIGH_ACT;
 		else if (!strcmp(data, "hsync_high_act"))
-			videomode_info->sync_pol = COMIPFB_HSYNC_HIGH_ACT;
+			videomode_info->sync_pol = FB_HSYNC_HIGH_ACT;
 	}
 
 	videomode_info->lp_cmd_en = of_property_read_bool(np,
@@ -380,6 +384,86 @@ static int of_parse_mipi_timing(struct device_node *np,
 	}
 	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);
@@ -392,7 +476,7 @@ static int of_parse_mipi_timing(struct device_node *np,
 
 	mipi_timing->auto_stop_clklane_en = of_property_read_bool(np,
 							"auto_stop_clklane_en");
-	mipi_timing->auto_stop_clklane_en = of_property_read_bool(np,
+	mipi_timing->im_pin_val = of_property_read_bool(np,
 							"im_pin_val");
 
 	rc = of_property_read_u32(np, "color_bits", &temp_val);
@@ -422,6 +506,20 @@ static int of_parse_mipi_timing(struct device_node *np,
 	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)
 {
@@ -715,8 +813,11 @@ static int sf_displayer_parse_dt(struct device *dev,
 		pandev->flags |= RESUME_WITH_CE;
 
 	pandev->init_last = of_property_read_bool(np, "init_last");
-	/*mipi info parse*/
-	of_parse_mipi_timing(np, &pandev->timing.mipi);
+
+	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))
diff --git a/drivers/video/fbdev/starfive/starfive_fb.c b/drivers/video/fbdev/starfive/starfive_fb.c
index 4a8287aed0fe..93624430160d 100644
--- a/drivers/video/fbdev/starfive/starfive_fb.c
+++ b/drivers/video/fbdev/starfive/starfive_fb.c
@@ -37,14 +37,17 @@
 #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 <video/starfive_fb.h>
 
-#include "starfive_fb.h"
 #include "starfive_lcdc.h"
 #include "starfive_vpp.h"
 #include "starfive_display_dev.h"
+#include "starfive_mipi_tx.h"
 
 static struct sf_fb_data *stf_dev = NULL;
 
@@ -63,6 +66,7 @@ static DEFINE_MUTEX(stf_mutex);
 
 static const struct res_name mem_res_name[] = {
 	{"lcdc"},
+	{"dsitx"},
 	{"vpp0"},
 	{"vpp1"},
 	{"vpp2"},
@@ -90,6 +94,12 @@ static int sf_fb_lcdc_clk_cfg(struct sf_fb_data *sf_dev)
 		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 1280:
 			dev_warn(sf_dev->dev, "1280 do nothing! need to set clk\n");
 			break;
@@ -107,6 +117,7 @@ static int sf_fb_lcdc_clk_cfg(struct sf_fb_data *sf_dev)
 	return ret;
 }
 
+#if defined(CONFIG_VIDEO_STF_VIN)
 static int vin_frame_complete_notify(struct notifier_block *nb,
 				      unsigned long val, void *v)
 {
@@ -115,16 +126,15 @@ static int vin_frame_complete_notify(struct notifier_block *nb,
 	unsigned int address;
 	unsigned int u_addr, v_addr, size;
 	unsigned int y_rgb_offset, u_offset, v_offset;
-	int i = 0;
 
-	address = psy->paddr;
+	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__);
+		//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;
@@ -133,11 +143,12 @@ static int vin_frame_complete_notify(struct notifier_block *nb,
 	        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;
+			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;
@@ -149,6 +160,7 @@ static int vin_frame_complete_notify(struct notifier_block *nb,
 
 	return NOTIFY_OK;
 }
+#endif
 
 static int sf_get_mem_res(struct platform_device *pdev, struct sf_fb_data *sf_dev)
 {
@@ -167,6 +179,8 @@ static int sf_get_mem_res(struct platform_device *pdev, struct sf_fb_data *sf_de
 
 		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")) {
@@ -178,7 +192,7 @@ static int sf_get_mem_res(struct platform_device *pdev, struct sf_fb_data *sf_de
 		} else if (!strcmp(name, "rst")) {
 			sf_dev->base_rst = regs;
 		} else if (!strcmp(name, "sys")) {
-			sf_dev->base_sys = regs;
+			sf_dev->base_syscfg = regs;
 		} else {
 			dev_err(&pdev->dev, "Could not match resource name\n");
 		}
@@ -192,6 +206,12 @@ static void sf_fb_get_var(struct fb_var_screeninfo *var, struct sf_fb_data *sf_d
 	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;
@@ -276,6 +296,13 @@ static int sf_fb_set_par(struct fb_info *info)
 			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;
 }
 
@@ -307,7 +334,7 @@ static int sf_fb_release(struct fb_info *info, int user)
 
 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);
+//	struct sf_fb_data *sf_dev = container_of(info, struct sf_fb_data, fb);
 
 	FB_PRT("%s,%d\n",__func__, __LINE__);
 	return 0;
@@ -374,6 +401,13 @@ static int sf_fb_check_var(struct fb_var_screeninfo *var, struct fb_info *info)
 			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;
 }
 
@@ -474,8 +508,15 @@ static int sf_fb_set_addr(struct sf_fb_data *sf_dev, struct fb_var_screeninfo *v
 			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);
+			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);
@@ -544,7 +585,7 @@ static int sf_fb_mmap(struct fb_info *info, struct vm_area_struct *vma)
 //	vma->vm_flags |= VM_IO | VM_RESERVED;
 	vma->vm_flags |= VM_IO;
 
-//	if (!(layer->parent->pdata->flags & COMIPFB_CACHED_BUFFER))
+//	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,
@@ -597,18 +638,21 @@ static struct fb_ops sf_fb_ops = {
 
 static int sf_fb_map_video_memory(struct sf_fb_data *sf_dev)
 {
-	int  ret;
-	dma_addr_t fb_phys_addr = 0;
-
-	ret = of_reserved_mem_device_init(sf_dev->dev);
-	if(ret) {
-		dev_err(sf_dev->dev, "Could not get reserved memory\n");
-		return ret;
+	struct resource res_mem;
+	struct device_node *node;
+
+	node = of_parse_phandle(sf_dev->dev->of_node, "memory-region", 0);
+	if(node) {
+		of_address_to_resource(node, 0, &res_mem);
+		sf_dev->fb.screen_size = resource_size(&res_mem);
+		sf_dev->fb.fix.smem_start = res_mem.start;
+	} else {
+		dev_err(sf_dev->dev, "Could not get reserved memory.\n");
+		return -ENOMEM;
 	}
-	sf_dev->fb.screen_base = dma_alloc_coherent(sf_dev->dev, 0x1000000, &fb_phys_addr, GFP_KERNEL);
-	sf_dev->fb.screen_size = 0x1000000;
 
-	sf_dev->fb.fix.smem_start = fb_phys_addr;
+	sf_dev->fb.screen_base = devm_ioremap_resource(sf_dev->dev, &res_mem);
+	memset(sf_dev->fb.screen_base, 0, sf_dev->fb.screen_size);
 
 	return 0;
 }
@@ -676,7 +720,6 @@ static int sf_fb_init(struct sf_fb_data *sf_dev)
 	return 0;
 }
 
-
 static int sf_fbinfo_init(struct device *dev, struct sf_fb_data *sf_dev)
 {
 	struct sf_fb_display_dev *display_dev = NULL;
@@ -709,10 +752,33 @@ static int sf_fbinfo_init(struct device *dev, struct sf_fb_data *sf_dev)
 				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 == COMIPFB_HSYNC_HIGH_ACT)
+				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 == COMIPFB_VSYNC_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;
@@ -728,6 +794,23 @@ static int sf_fbinfo_init(struct device *dev, struct sf_fb_data *sf_dev)
 			}
 			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;
@@ -778,7 +861,7 @@ static const struct vm_operations_struct mmap_mem_ops = {
 
 static int stfb_mmap(struct file *file, struct vm_area_struct *vma)
 {
-	struct sf_fb_data *sf_dev = file->private_data;
+//	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;
@@ -849,7 +932,11 @@ static int sf_fb_lcdc_init(struct sf_fb_data *sf_dev) {
 
 	pp_id = sf_dev->pp_conn_lcdc;
 	if (pp_id < 0) {
-		dev_warn(sf_dev->dev, "NO pp connect to LCDC\n");
+		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 {
 		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);
@@ -873,7 +960,6 @@ static int sf_fb_pp_video_mode_init(struct sf_fb_data *sf_dev, struct pp_video_m
 		src->width = sf_dev->pp[pp_id].src.width;
 		src->height = sf_dev->pp[pp_id].src.height;
 		src->addr = 0xf9000000;
-
 		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;
@@ -905,6 +991,19 @@ static int sf_fb_pp_init(struct sf_fb_data *sf_dev) {
 	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;
@@ -920,6 +1019,15 @@ static int sf_fb_parse_dt(struct device *dev, struct sf_fb_data *sf_dev) {
 		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");
@@ -962,8 +1070,6 @@ static int sf_fb_parse_dt(struct device *dev, struct sf_fb_data *sf_dev) {
 	return ret;
 }
 
-//#define FB_BUFF_VIN
-
 static int starfive_fb_probe(struct platform_device *pdev)
 {
 	struct device *dev = &pdev->dev;
@@ -983,14 +1089,25 @@ static int starfive_fb_probe(struct platform_device *pdev)
 
 	ret = sf_fb_parse_dt(dev, sf_dev);
 
+#if defined(CONFIG_VIDEO_STF_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;
@@ -999,11 +1116,12 @@ static int starfive_fb_probe(struct platform_device *pdev)
 		return -ENODEV;
 	}
 
-#ifdef FB_BUFF_VIN
-	/*the address 0xf9000000 is required in CMA modem by VIN,
-	*the case used to check VIN image data path only
-	*is not normal application.
-	*/
+#if defined(CONFIG_FB_STARFIVE_VIDEO)
+/* the address 0xf9000000 is required by VIN,
+ * the case used to check VIN image data path only
+ * is not normal application.
+ */
+    printk("defined CONFIG_FB_STARFIVE_VIDEO\n");
 	sf_dev->fb.fix.smem_start = 0xf9000000;
 #endif
 
@@ -1042,12 +1160,19 @@ static int starfive_fb_probe(struct platform_device *pdev)
 		return ret;
 	}
 
+
+    if(STARFIVEFB_MIPI_IF == sf_dev->display_dev->interface_info){
+		printk("STARFIVEFB_MIPI_IF interface_info\n");
+		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_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");
@@ -1077,6 +1202,7 @@ static int starfive_fb_probe(struct platform_device *pdev)
 	ret = misc_register(&sf_dev->stfbcdev);
 	if (ret) {
 		dev_err(dev, "creare stfbcdev FAIL!\n");
+		return ret;
 	}
 
 	lcdc_enable_intr(sf_dev);
@@ -1085,6 +1211,19 @@ static int starfive_fb_probe(struct platform_device *pdev)
 	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 ;
@@ -1099,7 +1238,7 @@ static struct of_device_id starfive_fb_dt_match[] = {
 
 static struct platform_driver starfive_fb_driver = {
 	.probe		= starfive_fb_probe,
-	//.remove		= comipfb_remove,
+	.remove		= starfive_fb_remove,
 	.shutdown	= starfive_fb_shutdown,
 	.driver		= {
 		.name	= "starfive,vpp-lcdc",
diff --git a/drivers/video/fbdev/starfive/starfive_lcdc.c b/drivers/video/fbdev/starfive/starfive_lcdc.c
index 1626c3cf8fbb..fcb564badec1 100644
--- a/drivers/video/fbdev/starfive/starfive_lcdc.c
+++ b/drivers/video/fbdev/starfive/starfive_lcdc.c
@@ -15,10 +15,11 @@
 */
 
 #include <linux/module.h>
+#include <video/starfive_fb.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
@@ -108,31 +109,68 @@ void lcdc_rgb_dclk_cfg(struct sf_fb_data *sf_dev, int dot_clk_sel)
 //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 srcSel, int addrMode, int lock)
+			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 cfg = hsize | vsize << LCDC_WIN_VSIZE
-					| layEn << LCDC_WIN_EN
-					| clorTab << LCDC_CC_EN
-					| colorEn << LCDC_CK_EN
-					| srcSel << LCDC_WIN_ISSEL
-					| addrMode << LCDC_WIN_PM
-					| lock << LCDC_WIN_CLK;
+   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, addrMode);
 }
 
-void lcdc_win_cfgB(struct sf_fb_data *sf_dev, int winNum, int xpos, int ypos, int pixbpp, int argbOrd)
+static int ppfmt_to_lcdcfmt(enum COLOR_FORMAT ppfmt)
 {
-  int cfg = xpos | ypos << LCDC_WIN_VPOS
-					| pixbpp << LCDC_WIN_FMT
-					| argbOrd << LCDC_WIN_ARGB_ORDER;
+	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;
+}
 
-  sf_fb_lcdcwrite32(sf_dev, LCDC_WIN0_CFG_B + winNum * 0xC, cfg);
-  LCDC_PRT("LCDC Win%d Xpos: %d, Ypos: %d, pixBpp: 0x%x, ARGB Order: 0x%x\n",winNum, xpos, ypos, pixbpp, argbOrd);
+void lcdc_win_cfgB(struct sf_fb_data *sf_dev, int winNum, int xpos, int ypos, int argbOrd)
+{
+	int win_format = 0;
+
+	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=0;
+	if (!strcmp(sf_dev->dis_dev_name, "seeed_5_inch"))
+		argbOrd=1;
+	//argbOrd=3;
+
+	int cfg = xpos | ypos << LCDC_WIN_VPOS | 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
@@ -191,6 +229,14 @@ void lcdc_panel_cfg(struct sf_fb_data *sf_dev, int buswid, int depth, int txcycl
 		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;
@@ -214,6 +260,7 @@ int lcdc_win_sel(struct sf_fb_data *sf_dev, enum lcdc_in_mode sel)
 	switch(sel)
 	{
 	case LCDC_IN_LCD_AXI:
+		winNum = LCDC_WIN_0;
 		break;
 	case LCDC_IN_VPP2:
 		winNum = LCDC_WIN_0;
@@ -235,6 +282,21 @@ int lcdc_win_sel(struct sf_fb_data *sf_dev, enum lcdc_in_mode sel)
 }
 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;
@@ -245,9 +307,8 @@ irqreturn_t lcdc_isr_handler(int this_irq, void *dev_id)
 	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);
+	if(0 == count % 100)
+		;//printk("lcdc count = %d, intr_status = 0x%x\n", count, intr_status);
 	return IRQ_HANDLED;
 }
 EXPORT_SYMBOL(lcdc_isr_handler);
@@ -271,8 +332,17 @@ void lcdc_config(struct sf_fb_data *sf_dev, int winNum)
 	lcdc_desize_cfg(sf_dev);
 	lcdc_rgb_dclk_cfg(sf_dev, 0x1);
 
-	lcdc_win_cfgA(sf_dev, winNum, 0x1, 0x0, 0x0, 0x1, 0x0, 0x0);
-	lcdc_win_cfgB(sf_dev, winNum, 0x0, 0x0, 0x7, 0x0);
+	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);
diff --git a/drivers/video/fbdev/starfive/starfive_lcdc.h b/drivers/video/fbdev/starfive/starfive_lcdc.h
index 2dcb44f6ec0d..f07bc793d22a 100644
--- a/drivers/video/fbdev/starfive/starfive_lcdc.h
+++ b/drivers/video/fbdev/starfive/starfive_lcdc.h
@@ -26,6 +26,13 @@ enum lcdc_win_num{
 	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
 
@@ -60,6 +67,9 @@ enum lcdc_win_num{
 #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
@@ -136,6 +146,7 @@ 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 100644
index 000000000000..6ccc9780507e
--- /dev/null
+++ b/drivers/video/fbdev/starfive/starfive_mipi_tx.c
@@ -0,0 +1,665 @@
+/* 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 n, ...)
+{
+    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_size = n;
+    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 100644
index 000000000000..cac15d5b16e6
--- /dev/null
+++ b/drivers/video/fbdev/starfive/starfive_mipi_tx.h
@@ -0,0 +1,203 @@
+/*
+ * 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
index 2097782a9fdc..ddad9e0d9327 100644
--- a/drivers/video/fbdev/starfive/starfive_vpp.c
+++ b/drivers/video/fbdev/starfive/starfive_vpp.c
@@ -15,15 +15,15 @@
 */
 
 #include <linux/module.h>
+#include <video/starfive_fb.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)
+	#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)
@@ -32,12 +32,12 @@
 
 static u32 sf_fb_sysread32(struct sf_fb_data *sf_dev, u32 reg)
 {
-	return ioread32(sf_dev->base_sys + 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_sys + reg);
+	iowrite32(val, sf_dev->base_syscfg + reg);
 }
 
 static u32 sf_fb_vppread32(struct sf_fb_data *sf_dev, int ppNum, u32 reg)
@@ -285,13 +285,13 @@ static void pp_srcfmt_set(struct sf_fb_data *sf_dev, int ppNum, struct pp_video_
             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_ARGB-COLOR_RGB888_ABGR);//0x1);
+            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_ARGB-COLOR_RGB888_RGBA);//0x2);
+            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_ARGB-COLOR_RGB888_BGRA);//0x3);
+            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);
@@ -436,16 +436,11 @@ void pp_size_set(struct sf_fb_data *sf_dev, int ppNum, struct pp_video_mode *src
         if(src->format == COLOR_YUV420_NV21) {    //ok
             next_y_rgb_addr = srcAddr;
             next_u_addr = srcAddr+size+1;
-            next_v_addr = srcAddr+size;//0;
-
+            next_v_addr = srcAddr+size;
             y_rgb_ofst = 0;
             uofst = 0;
             v_uvofst = size;
-            //pp_srcAddr_next(ppNum, next_y_rgb_addr, next_u_addr, next_v_addr);
-            //pp_srcOffset_cfg(ppNum, y_rgb_ofst, uofst, v_uvofst);
-        }
-
-        if(src->format == COLOR_YUV420_NV12) {    //ok
+        } else if (src->format == COLOR_YUV420_NV12) {
             next_y_rgb_addr = srcAddr;
             next_u_addr = srcAddr+size;
             next_v_addr = srcAddr+size+1;
@@ -580,9 +575,8 @@ irqreturn_t vpp1_isr_handler(int this_irq, void *dev_id)
 	sf_fb_vppwrite32(sf_dev, 1, PP_INT_CLR, 0xf);
 
 	count ++;
-	if(0 == count % 60)
-		PP_PRT("=");
-		//printk("=");
+	if(0 == count % 100)
+		;//PP_PRT("=");
 
 	return IRQ_HANDLED;
 }
diff --git a/drivers/video/fbdev/starfive/tda998x.c b/drivers/video/fbdev/starfive/tda998x.c
new file mode 100644
index 000000000000..61ad31d58de9
--- /dev/null
+++ b/drivers/video/fbdev/starfive/tda998x.c
@@ -0,0 +1,2279 @@
+/*
+ * 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/video/starfive_fb.h b/include/video/starfive_fb.h
new file mode 100644
index 000000000000..ca742b09ba44
--- /dev/null
+++ b/include/video/starfive_fb.h
@@ -0,0 +1,138 @@
+/*
+ * 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
+
+//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/include/video/stf-vin.h b/include/video/stf-vin.h
index 292e0dd9e995..e96fedef82f9 100755
--- a/include/video/stf-vin.h
+++ b/include/video/stf-vin.h
@@ -19,6 +19,8 @@
 #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
@@ -38,9 +40,21 @@
 #define VD_HEIGHT_480       480
 #define VD_WIDTH_640        640
 
-#define VIN_CLKGEN_BASE_ADDR	0x11800000
-#define VIN_RSTGEN_BASE_ADDR	0x11840000
-#define VIN_IOPAD_BASE_ADDR	    0x11858000
+#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
@@ -53,7 +67,21 @@
 #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
@@ -61,12 +89,15 @@
 #define CLK_ISP1_CTRL               0x48
 #define CLK_ISP1_2X_CTRL            0x4C
 #define CLK_ISP1_MIPI_CTRL          0x50
-#define CLK_MIPI_RX1_PXL_CTRL       0x10
 #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
@@ -79,18 +110,21 @@
 #define IOPAD_REG89	            0x164
 
 //sys control REG DEFINE
-#define SYSCTRL_REG6	            0x18
-#define SYSCTRL_REG10	            0x28
-#define SYSCTRL_REG11	            0x2C
-#define SYSCTRL_REG12	            0x30
-#define SYSCTRL_REG13	            0x34
-#define SYSCTRL_REG14	            0x38
-#define SYSCTRL_REG15	            0x3C
-#define SYSCTRL_REG17	            0x44
-#define SYSCTRL_REG18	            0x48
-#define SYSCTRL_REG19	            0x4C
-#define SYSCTRL_REG20	            0x50
-#define SYSCTRL_REG21	            0x54
+#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)
@@ -99,8 +133,7 @@
 #define ISP_RESET               (0x1<<1)
 #define ISP_ENBALE              (0x1)
 
-#define VIN_ISP0_BASE_ADDR	0x19870000
-#define VIN_ISP1_BASE_ADDR	0x198a0000
+
 
  //ISP REG DEFINE
 #define ISP_REG_DVP_POLARITY_CFG            0x00000014
@@ -130,6 +163,47 @@
 #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,
@@ -138,6 +212,40 @@ enum VIN_SOURCE_FORMAT {
 	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;
@@ -166,7 +274,17 @@ struct stf_vin_dev {
 	struct device *dev;
 	struct cdev vin_cdev;
 	void __iomem *base;
-	void __iomem *base_isp;
+	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;
@@ -179,6 +297,8 @@ struct stf_vin_dev {
 	wait_queue_head_t wq;
 	bool condition;
 	int odd;
+
+	csi_format csi_fmt;
 };
 
 extern int vin_notifier_register(struct notifier_block *nb);