diff --git a/arch/arm/mach-rockchip/Kconfig b/arch/arm/mach-rockchip/Kconfig
index 6313fec3ddb9..d042bf289f99 100644
--- a/arch/arm/mach-rockchip/Kconfig
+++ b/arch/arm/mach-rockchip/Kconfig
@@ -19,6 +19,7 @@ config ARCH_ROCKCHIP
 	select CLKSRC_ARM_GLOBAL_TIMER_SCHED_CLOCK if (CPU_RK30XX || CPU_RK3188)
 	select ZONE_DMA if ARM_LPAE
 	select PM
+	select ARM_ERRATA_814220 if (CPU_RK3036 || CPU_RK312X || CPU_RK322X || CPU_RV1103B || CPU_RV1106 || CPU_RV1108 || CPU_RV1126 || CPU_RK3506)
 	help
 	  Support for Rockchip's Cortex-A9 Single-to-Quad-Core-SoCs
 	  containing the RK2928, RK30xx and RK31xx series.
diff --git a/drivers/gpu/drm/rockchip/rockchip_drm_drv.h b/drivers/gpu/drm/rockchip/rockchip_drm_drv.h
index ae4d56f04049..6a07fd285386 100644
--- a/drivers/gpu/drm/rockchip/rockchip_drm_drv.h
+++ b/drivers/gpu/drm/rockchip/rockchip_drm_drv.h
@@ -55,14 +55,6 @@ struct iommu_domain;
 #define VOP_OUTPUT_IF_HDMI1	BIT(12)
 #define VOP_OUTPUT_IF_DP2	BIT(13)
 
-#ifndef DRM_FORMAT_NV20
-#define DRM_FORMAT_NV20		fourcc_code('N', 'V', '2', '0') /* 2x1 subsampled Cr:Cb plane */
-#endif
-
-#ifndef DRM_FORMAT_NV30
-#define DRM_FORMAT_NV30		fourcc_code('N', 'V', '3', '0') /* non-subsampled Cr:Cb plane */
-#endif
-
 #define RK_IF_PROP_COLOR_DEPTH		"color_depth"
 #define RK_IF_PROP_COLOR_FORMAT		"color_format"
 #define RK_IF_PROP_COLOR_DEPTH_CAPS	"color_depth_caps"
diff --git a/drivers/iio/imu/inv_icm42670/inv_icm42670.h b/drivers/iio/imu/inv_icm42670/inv_icm42670.h
index 3aee90fafed4..20211fb02a08 100644
--- a/drivers/iio/imu/inv_icm42670/inv_icm42670.h
+++ b/drivers/iio/imu/inv_icm42670/inv_icm42670.h
@@ -502,6 +502,7 @@ struct icm42670_data {
 	s64 period_max;		 // Maximum interrupt period deviation in nanoseconds
 	int period_divider;
 	int interrupt_regval;
+	bool enable_fifo;
 	u8  data_buff[ICM42670_DATA_BUFF_SIZE];
 };
 
@@ -532,6 +533,7 @@ enum icm42670_sensor_type {
 // fifo
 #define ICM42670_OUTPUT_DATA_SIZE 24 // align 8, last 8 for timestamp
 #define ICM42670_OUTPUT_DATA_SIZE_PULS_ONE 25
+#define ICM42670_ACCEL_GYRO_SIZE	12
 #define ICM42670_FIFO_DATUM			   16
 #define ICM42670_BYTES_PER_3AXIS_SENSOR   6
 #define ICM42670_FIFO_COUNT_BYTE		  2
@@ -545,6 +547,8 @@ extern const struct dev_pm_ops icm42670_pm_ops;
 
 irqreturn_t icm42670_read_fifo(int irq, void *p);
 int icm42670_reset_fifo(struct iio_dev *indio_dev);
+irqreturn_t icm42670_read_data(int irq, void *p);
+int icm42670_reset_data(struct iio_dev *indio_dev);
 int icm42670_core_probe(struct regmap *regmap, int irq, const char *name,
 						int chip_type, icm42670_bus_setup bus_setup);
 void icm42670_core_remove(struct device *dev);
diff --git a/drivers/iio/imu/inv_icm42670/inv_icm42670_core.c b/drivers/iio/imu/inv_icm42670/inv_icm42670_core.c
index 901ca0d66a6d..cd8623768bbf 100644
--- a/drivers/iio/imu/inv_icm42670/inv_icm42670_core.c
+++ b/drivers/iio/imu/inv_icm42670/inv_icm42670_core.c
@@ -43,6 +43,10 @@ static int icm42670_set_odr(struct icm42670_data *data,
 					enum icm42670_sensor_type t,
 					int odr);
 
+static int icm42670_temp_read_raw(struct iio_dev *indio_dev,
+			       struct iio_chan_spec const *chan,
+			       int *val, int *val2, long mask);
+
 #define ICM42670_CHANNEL(_type, _axis, _index) {		\
 	.type = _type,						\
 	.modified = 1,						\
@@ -59,10 +63,14 @@ static int icm42670_set_odr(struct icm42670_data *data,
 	},							\
 }
 
-#define ICM42670_TEMP_CHANNEL(_type, _index) {			\
+#define ICM42670_TEMP_CHANNEL_8BIT(_type, _index) {			\
 	.type = _type,						\
-	.channel = -1,						\
-	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),		\
+	.modified = 1,						\
+	.channel2 = 1,					\
+	.info_mask_separate =					\
+		BIT(IIO_CHAN_INFO_RAW) |			\
+		BIT(IIO_CHAN_INFO_OFFSET) |			\
+		BIT(IIO_CHAN_INFO_SCALE),			\
 	.scan_index = _index,					\
 	.scan_type = {						\
 		.sign = 's',					\
@@ -72,6 +80,34 @@ static int icm42670_set_odr(struct icm42670_data *data,
 	},							\
 }
 
+#define ICM42670_TEMP_CHANNEL_16BIT(_type, _index) {			\
+	.type = _type,						\
+	.modified = 1,						\
+	.channel2 = 1,					\
+	.info_mask_separate =					\
+		BIT(IIO_CHAN_INFO_RAW) |			\
+		BIT(IIO_CHAN_INFO_OFFSET) |			\
+		BIT(IIO_CHAN_INFO_SCALE),			\
+	.scan_index = _index,					\
+	.scan_type = {						\
+		.sign = 's',					\
+		.realbits = 16,					\
+		.storagebits = 16,				\
+		.endianness = IIO_BE,				\
+	},							\
+}
+
+static const struct iio_chan_spec icm42670_fifo_channels[] = {
+	ICM42670_CHANNEL(IIO_ACCEL, IIO_MOD_X, ICM42670_SCAN_ACCEL_X),
+	ICM42670_CHANNEL(IIO_ACCEL, IIO_MOD_Y, ICM42670_SCAN_ACCEL_Y),
+	ICM42670_CHANNEL(IIO_ACCEL, IIO_MOD_Z, ICM42670_SCAN_ACCEL_Z),
+	ICM42670_CHANNEL(IIO_ANGL_VEL, IIO_MOD_X, ICM42670_SCAN_GYRO_X),
+	ICM42670_CHANNEL(IIO_ANGL_VEL, IIO_MOD_Y, ICM42670_SCAN_GYRO_Y),
+	ICM42670_CHANNEL(IIO_ANGL_VEL, IIO_MOD_Z, ICM42670_SCAN_GYRO_Z),
+	ICM42670_TEMP_CHANNEL_8BIT(IIO_TEMP, ICM42670_SCAN_TEMP),
+	IIO_CHAN_SOFT_TIMESTAMP(ICM42670_SCAN_TIMESTAMP),
+};
+
 static const struct iio_chan_spec icm42670_channels[] = {
 	ICM42670_CHANNEL(IIO_ACCEL, IIO_MOD_X, ICM42670_SCAN_ACCEL_X),
 	ICM42670_CHANNEL(IIO_ACCEL, IIO_MOD_Y, ICM42670_SCAN_ACCEL_Y),
@@ -79,7 +115,7 @@ static const struct iio_chan_spec icm42670_channels[] = {
 	ICM42670_CHANNEL(IIO_ANGL_VEL, IIO_MOD_X, ICM42670_SCAN_GYRO_X),
 	ICM42670_CHANNEL(IIO_ANGL_VEL, IIO_MOD_Y, ICM42670_SCAN_GYRO_Y),
 	ICM42670_CHANNEL(IIO_ANGL_VEL, IIO_MOD_Z, ICM42670_SCAN_GYRO_Z),
-	ICM42670_TEMP_CHANNEL(IIO_TEMP, ICM42670_SCAN_TEMP),
+	ICM42670_TEMP_CHANNEL_16BIT(IIO_TEMP, ICM42670_SCAN_TEMP),
 	IIO_CHAN_SOFT_TIMESTAMP(ICM42670_SCAN_TIMESTAMP),
 };
 
@@ -134,9 +170,9 @@ static struct icm42670_regs icm42670_regs[] = {
 };
 
 static IIO_CONST_ATTR(in_accel_sampling_frequency_available,
-			"1 3 6 12 25 50 100 200 400 800");
+			"1 3 6 12 25 50 100 200 400 800 1600");
 static IIO_CONST_ATTR(in_anglvel_sampling_frequency_available,
-			"12 25 50 100 200 400 800");
+			"12 25 50 100 200 400 800 1600");
 static IIO_CONST_ATTR(in_accel_scale_available,
 			"0.000598 0.001196 0.002393 0.004785");
 static IIO_CONST_ATTR(in_anglvel_scale_available,
@@ -558,6 +594,16 @@ static int icm42670_read_raw(struct iio_dev *indio_dev,
 	int ret;
 	struct icm42670_data *data = iio_priv(indio_dev);
 
+	switch (chan->type) {
+	case IIO_ANGL_VEL:
+	case IIO_ACCEL:
+		break;
+	case IIO_TEMP:
+		return icm42670_temp_read_raw(indio_dev, chan, val, val2, mask);
+	default:
+		return -EINVAL;
+	}
+
 	switch (mask) {
 	case IIO_CHAN_INFO_RAW:
 		ret = iio_device_claim_direct_mode(indio_dev);
@@ -666,6 +712,36 @@ static void icm42670_disable_vdd_reg(void *_data)
 		dev_err(dev, "failed to disable vdd error %d\n", ret);
 }
 
+static int icm42670_temp_read_raw(struct iio_dev *indio_dev,
+				struct iio_chan_spec const *chan,
+				int *val, int *val2, long mask)
+{
+	struct icm42670_data *data = iio_priv(indio_dev);
+	int ret = 0;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		ret = iio_device_claim_direct_mode(indio_dev);
+		if (ret)
+			return ret;
+		ret = icm42670_get_data(data, icm42670_to_sensor(chan->type), chan->channel2, val);
+		iio_device_release_direct_mode(indio_dev);
+		return ret ? ret : IIO_VAL_INT;
+
+	case IIO_CHAN_INFO_SCALE:
+		*val = 0;
+		*val2 = data->enable_fifo ? 500000 : 7812500;
+		return data->enable_fifo ? IIO_VAL_INT_PLUS_MICRO : IIO_VAL_INT_PLUS_NANO;
+
+	case IIO_CHAN_INFO_OFFSET:
+		*val = data->enable_fifo ? 50 : 3200;
+		return IIO_VAL_INT;
+
+	default:
+		return -EINVAL;
+	}
+}
+
 static int __icm42670_set_fsr(struct icm42670_data *data,
 							 enum icm42670_sensor_type t, int sel)
 {
@@ -846,26 +922,30 @@ static int icm42670_chip_init(struct icm42670_data *data, icm42670_bus_setup bus
 		return ret;
 	}
 
-	/* FIFO count is reported in records (1 record = 16 bytes for gyro+accel+tempsensor data) */
-	ret = regmap_update_bits(data->regmap, REG_INTF_CONFIG0,
-				BIT_FIFO_COUNT_REC_FIFO_COUNT_REC,
-					BIT_FIFO_COUNT_REC_FIFO_COUNT_REC);
-	if (ret)
-		return ret;
+	if (true == data->enable_fifo) {
+		/* FIFO count is reported in records
+		 * (1 record = 16 bytes for gyro+accel+tempsensor data)
+		 */
+		ret = regmap_update_bits(data->regmap, REG_INTF_CONFIG0,
+					BIT_FIFO_COUNT_REC_FIFO_COUNT_REC,
+						BIT_FIFO_COUNT_REC_FIFO_COUNT_REC);
+		if (ret)
+			return ret;
 
-	/* FIFO data in big-endian (default) */
-	ret = regmap_update_bits(data->regmap, REG_INTF_CONFIG0,
-				BIT_FIFO_COUNT_REC_FIFO_COUNT_ENDIAN,
-				BIT_FIFO_COUNT_REC_FIFO_COUNT_ENDIAN);
-	if (ret)
-		return ret;
+		/* FIFO data in big-endian (default) */
+		ret = regmap_update_bits(data->regmap, REG_INTF_CONFIG0,
+					BIT_FIFO_COUNT_REC_FIFO_COUNT_ENDIAN,
+					BIT_FIFO_COUNT_REC_FIFO_COUNT_ENDIAN);
+		if (ret)
+			return ret;
 
-	/* sensor data in big-endian (default) */
-	ret = regmap_update_bits(data->regmap, REG_INTF_CONFIG0,
-				BIT_FIFO_COUNT_REC_SENSOR_DATA_ENDIAN,
-				BIT_FIFO_COUNT_REC_SENSOR_DATA_ENDIAN);
-	if (ret)
-		return ret;
+		/* sensor data in big-endian (default) */
+		ret = regmap_update_bits(data->regmap, REG_INTF_CONFIG0,
+					BIT_FIFO_COUNT_REC_SENSOR_DATA_ENDIAN,
+					BIT_FIFO_COUNT_REC_SENSOR_DATA_ENDIAN);
+		if (ret)
+			return ret;
+	}
 
 	ret = icm42670_set_mode(data, ICM42670_ACCEL, true);
 	if (ret < 0) {
@@ -951,6 +1031,10 @@ int icm42670_core_probe(struct regmap *regmap,
 		return -EINVAL;
 	}
 
+	data->enable_fifo = of_property_read_bool(data->node, "enable-fifo");
+	if (true != data->enable_fifo)
+		dev_info(dev, "Not using FIFO!\n");
+
 	desc = irq_get_irq_data(irq);
 	if (!desc) {
 		dev_err(dev, "Could not find IRQ %d\n", irq);
@@ -1000,15 +1084,19 @@ int icm42670_core_probe(struct regmap *regmap,
 		name = icm42670_match_acpi_device(dev);
 
 	indio_dev->dev.parent = dev;
-	indio_dev->channels = icm42670_channels;
-	indio_dev->num_channels = ARRAY_SIZE(icm42670_channels);
+	indio_dev->channels =
+		data->enable_fifo ? icm42670_fifo_channels : icm42670_channels;
+	indio_dev->num_channels =
+		data->enable_fifo ?
+		ARRAY_SIZE(icm42670_fifo_channels) : ARRAY_SIZE(icm42670_channels);
 	indio_dev->available_scan_masks = icm42670_scan_masks;
 	indio_dev->name = name;
 	indio_dev->info = &icm42670_info;
 	indio_dev->modes = INDIO_BUFFER_TRIGGERED;
 
-	ret = devm_iio_triggered_buffer_setup(dev, indio_dev, iio_pollfunc_store_time,
-						icm42670_read_fifo, NULL);
+	ret = devm_iio_triggered_buffer_setup(dev, indio_dev,
+		iio_pollfunc_store_time,
+		data->enable_fifo ? icm42670_read_fifo : icm42670_read_data, NULL);
 	if (ret < 0)
 		goto uninit;
 
diff --git a/drivers/iio/imu/inv_icm42670/inv_icm42670_ring.c b/drivers/iio/imu/inv_icm42670/inv_icm42670_ring.c
index 1dbddb85f217..14280b51a67e 100644
--- a/drivers/iio/imu/inv_icm42670/inv_icm42670_ring.c
+++ b/drivers/iio/imu/inv_icm42670/inv_icm42670_ring.c
@@ -242,3 +242,94 @@ flush_fifo:
 
 	return IRQ_HANDLED;
 }
+
+int icm42670_reset_data(struct iio_dev *indio_dev)
+{
+	int ret;
+	struct icm42670_data  *st = iio_priv(indio_dev);
+	const struct device *dev = regmap_get_device(st->regmap);
+
+	/* reset it timestamp validation */
+	st->it_timestamp = 0;
+
+	ret = regmap_write(st->regmap, REG_INT_SOURCE0, BIT_INT_MODE_OFF);
+	if (ret) {
+		dev_err(dev, "int_enable failed %d\n",
+			ret);
+		goto reset_fail;
+	}
+
+	/* disable the sensor output to FIFO */
+	ret = regmap_write(st->regmap, REG_FIFO_CONFIG1, BIT_FIFO_MODE_BYPASS);
+	if (ret) {
+		dev_err(dev, "Failed to write to REG_FIFO_CONFIG1 register!\n");
+		goto reset_fail;
+	}
+
+	ret = regmap_write(st->regmap, REG_INT_SOURCE0,
+			BIT_INT_DRDY_INT_EN);
+	if (ret) {
+		dev_err(dev, "Failed to write to REG_INT_SOURCE0 register!\n");
+		goto reset_fail;
+	}
+
+	msleep(50);
+
+	st->interrupt_period = st->standard_period;
+
+	return 0;
+
+reset_fail:
+	dev_err(regmap_get_device(st->regmap), "%s :reset fifo failed %d\n", __func__, ret);
+
+	return ret;
+}
+
+irqreturn_t icm42670_read_data(int irq, void *p)
+{
+	struct iio_poll_func *pf = p;
+	struct iio_dev *indio_dev = pf->indio_dev;
+	struct icm42670_data *st = iio_priv(indio_dev);
+	const struct device *dev = regmap_get_device(st->regmap);
+	int ret, int_drdy;
+	u8 data[ICM42670_OUTPUT_DATA_SIZE_PULS_ONE];
+
+	mutex_lock(&st->lock);
+
+	ret = regmap_read(st->regmap, REG_INT_STATUS_DRDY, &int_drdy);
+	if (ret) {
+		dev_err(dev, "failed to ack interrupt\n");
+		goto read_fail;
+	}
+
+	if (!(int_drdy & (BIT_INT_STATUS_DRDY)))
+		goto read_fail;
+
+	ret = regmap_bulk_read(st->regmap, REG_ACCEL_DATA_X0_UI,
+		(u8 *)data, ICM42670_ACCEL_GYRO_SIZE);
+	if (ret) {
+		dev_err(dev, "regmap_bulk_read accel+gyro failed\n");
+		goto read_fail;
+	}
+
+	ret = regmap_bulk_read(st->regmap, REG_TEMP_DATA0_UI,
+		(u8 *)&data[ICM42670_ACCEL_GYRO_SIZE], 2);
+	if (ret) {
+		dev_err(dev, "regmap_bulk_read temperature failed\n");
+		goto read_fail;
+	}
+
+	iio_push_to_buffers_with_timestamp(indio_dev, &(data[0]), pf->timestamp);
+
+	mutex_unlock(&st->lock);
+	iio_trigger_notify_done(indio_dev->trig);
+
+	return IRQ_HANDLED;
+
+read_fail:
+	dev_info(dev, "read data fail\n");
+	mutex_unlock(&st->lock);
+	iio_trigger_notify_done(indio_dev->trig);
+
+	return IRQ_HANDLED;
+}
diff --git a/drivers/iio/imu/inv_icm42670/inv_icm42670_trigger.c b/drivers/iio/imu/inv_icm42670/inv_icm42670_trigger.c
index 8b52eca60738..e1364454c891 100644
--- a/drivers/iio/imu/inv_icm42670/inv_icm42670_trigger.c
+++ b/drivers/iio/imu/inv_icm42670/inv_icm42670_trigger.c
@@ -26,9 +26,15 @@ int icm42670_set_enable(struct iio_dev *indio_dev, bool enable)
 		if (ret)
 			goto error_accl_off;
 
-		ret = icm42670_reset_fifo(indio_dev);
-		if (ret)
-			goto error_gyro_off;
+		if (true == data->enable_fifo) {
+			ret = icm42670_reset_fifo(indio_dev);
+			if (ret)
+				goto error_gyro_off;
+		} else {
+			ret = icm42670_reset_data(indio_dev);
+			if (ret)
+				goto error_gyro_off;
+		}
 
 		/* Theoretical interrupt period */
 		data->standard_period = data->period_divider > 0 ?
@@ -43,20 +49,25 @@ int icm42670_set_enable(struct iio_dev *indio_dev, bool enable)
 		data->period_max = div_s64((data->standard_period * (100 +
 					ICM42670_TS_PERIOD_JITTER)), 100);
 	} else {
+		ret = regmap_write(data->regmap, REG_INT_SOURCE0, BIT_INT_MODE_OFF);
+		if (ret) {
+			dev_err(dev, "int_enable failed %d\n", ret);
+			goto error_gyro_off;
+		}
 		ret = regmap_write(data->regmap, REG_FIFO_CONFIG1, BIT_FIFO_MODE_BYPASS);
 		if (ret) {
 			dev_err(dev, "set REG_FIFO_CONFIG1 fail: %d\n", ret);
 			goto error_gyro_off;
 		}
-		ret = icm42670_set_mode(data, ICM42670_GYRO, false);
-		if (ret)
-			goto error_gyro_off;
 
 		ret = icm42670_set_mode(data, ICM42670_ACCEL, false);
 		if (ret)
 			goto error_accl_off;
+
+		ret = icm42670_set_mode(data, ICM42670_GYRO, false);
+		if (ret)
+			goto error_gyro_off;
 	}
-	dev_info(dev, "set fifo mode end\n");
 	return ret;
 
 error_gyro_off:
diff --git a/drivers/media/i2c/Kconfig b/drivers/media/i2c/Kconfig
index d524ae2e1489..d3c0bc171f65 100644
--- a/drivers/media/i2c/Kconfig
+++ b/drivers/media/i2c/Kconfig
@@ -2903,6 +2903,18 @@ config VIDEO_THCV244
 	  To compile this driver as a module, choose M here: the
 	  module will be called thcv244.
 
+config VIDEO_TP2855
+	tristate "NEXTCHIP tp2855 driver support"
+	depends on I2C && VIDEO_DEV
+	select MEDIA_CONTROLLER
+	select VIDEO_V4L2_SUBDEV_API
+	help
+	  Support for the Nextchip TP2855 multi channels digital decode to
+	  MIPI CSI-2 bridge.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called tp2855.
+
 config VIDEO_TVP514X
 	tristate "Texas Instruments TVP514x video decoder"
 	depends on VIDEO_DEV && I2C
diff --git a/drivers/media/i2c/Makefile b/drivers/media/i2c/Makefile
index d1a935b5768a..6a293413fb71 100644
--- a/drivers/media/i2c/Makefile
+++ b/drivers/media/i2c/Makefile
@@ -270,6 +270,7 @@ obj-$(CONFIG_VIDEO_THCV244) += thcv244.o
 obj-$(CONFIG_VIDEO_THS7303) += ths7303.o
 obj-$(CONFIG_VIDEO_THS8200) += ths8200.o
 obj-$(CONFIG_VIDEO_TLV320AIC23B) += tlv320aic23b.o
+obj-$(CONFIG_VIDEO_TP2855) += tp2855.o
 obj-$(CONFIG_VIDEO_TVAUDIO) += tvaudio.o
 obj-$(CONFIG_VIDEO_TVP514X) += tvp514x.o
 obj-$(CONFIG_VIDEO_TVP5150) += tvp5150.o
diff --git a/drivers/media/i2c/tp2855.c b/drivers/media/i2c/tp2855.c
index 32578ebfd7d4..0a10569bba86 100644
--- a/drivers/media/i2c/tp2855.c
+++ b/drivers/media/i2c/tp2855.c
@@ -37,8 +37,9 @@
 
 #include <linux/platform_device.h>
 #include <linux/input.h>
+#include "tp2855.h"
 
-#define DRIVER_VERSION				KERNEL_VERSION(0, 0x01, 0x0)
+#define DRIVER_VERSION				KERNEL_VERSION(0, 0x01, 0x1)
 #define TP2855_TEST_PATTERN 		0
 #define TP2855_XVCLK_FREQ			27000000
 #define TP2855_LINK_FREQ_297M		(297000000UL >> 1)
@@ -49,13 +50,20 @@
 #define OF_CAMERA_PINCTRL_STATE_DEFAULT		"rockchip,camera_default"
 #define OF_CAMERA_PINCTRL_STATE_SLEEP		"rockchip,camera_sleep"
 
+static const char *const tp2855_supply_names[] = {
+	"dovdd",		/* Digital I/O power */
+	"avdd",			/* Analog power */
+	"dvdd",			/* Digital power */
+};
+#define TP2855_NUM_SUPPLIES ARRAY_SIZE(tp2855_supply_names)
+
 enum{
-	CH_1=0,
-	CH_2=1,
-	CH_3=2,
-	CH_4=3,
-	CH_ALL=4,
-	MIPI_PAGE=8,
+	CH_1 = 0,
+	CH_2 = 1,
+	CH_3 = 2,
+	CH_4 = 3,
+	CH_ALL = 4,
+	MIPI_PAGE = 8,
 };
 
 enum{
@@ -75,8 +83,10 @@ struct tp2855_mode {
 	struct v4l2_fract max_fps;
 	u32 mipi_freq_idx;
 	u32 bpp;
-	const struct regval *global_reg_list;
-	const struct regval *reg_list;
+	const struct regval *common_reg_list;
+	const struct regval *mipi_reg_list;
+	int common_reg_size;
+	int mipi_reg_size;
 	u32 hdr_mode;
 	u32 vc[PAD_MAX];
 	u32 channel_reso[PAD_MAX];
@@ -91,6 +101,7 @@ struct tp2855 {
 	struct pinctrl		*pinctrl;
 	struct pinctrl_state	*pins_default;
 	struct pinctrl_state	*pins_sleep;
+	struct regulator_bulk_data supplies[TP2855_NUM_SUPPLIES];
 
 	struct v4l2_subdev	subdev;
 	struct media_pad	pad;
@@ -107,17 +118,16 @@ struct tp2855 {
 	const char		*module_name;
 	const char		*len_name;
 	bool			lost_video_status;
-	struct task_struct *detect_thread;
-
-	int streaming;
+	struct task_struct	*detect_thread;
+	u8			detect_status[PAD_MAX];
+	bool			do_reset;
+	int			streaming;
 };
 
 
 #define to_tp2855(sd) container_of(sd, struct tp2855, subdev)
 
 static __maybe_unused const struct regval common_setting_297M_720p_25fps_regs[] = {
-	{0x40, 0x04},
-	{0xf5, 0x00},
 	{0x02, 0x42},
 	{0x07, 0xc0},
 	{0x0b, 0xc0},
@@ -135,9 +145,6 @@ static __maybe_unused const struct regval common_setting_297M_720p_25fps_regs[]
 	{0x21, 0x84},
 	{0x22, 0x36},
 	{0x23, 0x3c},
-#if TP2855_TEST_PATTERN
-	{0x2a, 0x3c}, //vi test
-#endif
 	{0x2b, 0x60},
 	{0x2c, 0x0a},
 	{0x2d, 0x30},
@@ -149,7 +156,26 @@ static __maybe_unused const struct regval common_setting_297M_720p_25fps_regs[]
 	{0x35, 0x25},
 	{0x38, 0x00},
 	{0x39, 0x18},
+	//STD_HDA
+	{0x02, 0x46},
+	{0x0d, 0x71},
+	{0x18, 0x1b},
+	{0x20, 0x40},
+	{0x21, 0x46},
+	{0x25, 0xfe},
+	{0x26, 0x01},
+	{0x2c, 0x3a},
+	{0x2d, 0x5a},
+	{0x2e, 0x40},
+	{0x30, 0x9e},
+	{0x31, 0x20},
+	{0x32, 0x10},
+	{0x33, 0x90},
+	// {0x23, 0x02}, //vi test ok
+	// {0x23, 0x00},
+};
 
+static __maybe_unused const struct regval mipi_setting_297M_4ch_4lane_regs[] = {
 	{0x40, 0x08},
 	{0x01, 0xf0},
 	{0x02, 0x01},
@@ -162,18 +188,34 @@ static __maybe_unused const struct regval common_setting_297M_720p_25fps_regs[]
 	{0x26, 0x03},
 	{0x27, 0x09},
 	{0x29, 0x02},
-	{0x33, 0x07},
+	{0x33, 0x0f},
 	{0x33, 0x00},
 	{0x14, 0xc4},
 	{0x14, 0x44},
+};
 
+static __maybe_unused const struct regval mipi_setting_594M_4ch_4lane_regs[] = {
+	{0x40, 0x08},
+	{0x01, 0xf0},
+	{0x02, 0x01},
+	{0x08, 0x0f},
+	{0x20, 0x44},
+	{0x34, 0xe4},
+	{0x15, 0x0C},
+	{0x25, 0x08},
+	{0x26, 0x06},
+	{0x27, 0x11},
+	{0x29, 0x0a},
+	{0x33, 0x0f},
+	{0x33, 0x00},
+	{0x14, 0x33},
+	{0x14, 0xb3},
+	{0x14, 0x33},
 	// {0x23, 0x02}, //vi test ok
 	// {0x23, 0x00},
 };
 
 static __maybe_unused const struct regval common_setting_594M_1080p_25fps_regs[] = {
-	{0x40, 0x04},
-	{0xf5, 0x00},
 	{0x02, 0x40},
 	{0x07, 0xc0},
 	{0x0b, 0xc0},
@@ -191,9 +233,6 @@ static __maybe_unused const struct regval common_setting_594M_1080p_25fps_regs[]
 	{0x21, 0x84},
 	{0x22, 0x36},
 	{0x23, 0x3c},
-#if TP2855_TEST_PATTERN
-	{0x2a, 0x3c}, //vi test
-#endif
 	{0x2b, 0x60},
 	{0x2c, 0x0a},
 	{0x2d, 0x30},
@@ -205,25 +244,22 @@ static __maybe_unused const struct regval common_setting_594M_1080p_25fps_regs[]
 	{0x35, 0x05},
 	{0x38, 0x00},
 	{0x39, 0x1C},
-
-	{0x40, 0x08},
-	{0x01, 0xf0},
-	{0x02, 0x01},
-	{0x08, 0x0f},
-	{0x20, 0x44},
-	{0x34, 0xe4},
-	{0x15, 0x0C},
-	{0x25, 0x08},
-	{0x26, 0x06},
-	{0x27, 0x11},
-	{0x29, 0x0a},
-	{0x33, 0x07},
-	{0x33, 0x00},
-	{0x14, 0x33},
-	{0x14, 0xb3},
-	{0x14, 0x33},
-	// {0x23, 0x02}, //vi test ok
-	// {0x23, 0x00},
+	//STD_HDA
+	{0x02, 0x44},
+	{0x0d, 0x73},
+	{0x15, 0x01},
+	{0x16, 0xf0},
+	{0x20, 0x3c},
+	{0x21, 0x46},
+	{0x25, 0xfe},
+	{0x26, 0x0d},
+	{0x2c, 0x3a},
+	{0x2d, 0x54},
+	{0x2e, 0x40},
+	{0x30, 0xa5},
+	{0x31, 0x86},
+	{0x32, 0xfb},
+	{0x33, 0x60},
 };
 
 static struct tp2855_mode supported_modes[] = {
@@ -235,7 +271,10 @@ static struct tp2855_mode supported_modes[] = {
 			.numerator = 10000,
 			.denominator = 250000,
 		},
-		.global_reg_list = common_setting_594M_1080p_25fps_regs,
+		.common_reg_list = common_setting_594M_1080p_25fps_regs,
+		.common_reg_size = ARRAY_SIZE(common_setting_594M_1080p_25fps_regs),
+		.mipi_reg_list = mipi_setting_594M_4ch_4lane_regs,
+		.mipi_reg_size = ARRAY_SIZE(mipi_setting_594M_4ch_4lane_regs),
 		.mipi_freq_idx = 0,
 		.bpp = 8,
 		.vc[PAD0] = 0,
@@ -251,7 +290,10 @@ static struct tp2855_mode supported_modes[] = {
 			.numerator = 10000,
 			.denominator = 250000,
 		},
-		.global_reg_list = common_setting_297M_720p_25fps_regs,
+		.common_reg_list = common_setting_297M_720p_25fps_regs,
+		.common_reg_size = ARRAY_SIZE(common_setting_297M_720p_25fps_regs),
+		.mipi_reg_list = mipi_setting_297M_4ch_4lane_regs,
+		.mipi_reg_size = ARRAY_SIZE(mipi_setting_297M_4ch_4lane_regs),
 		.mipi_freq_idx = 1,
 		.bpp = 8,
 		.vc[PAD0] = 0,
@@ -427,7 +469,7 @@ static int tp2855_get_fmt(struct v4l2_subdev *sd,
 		fmt->format.height = mode->height;
 		fmt->format.code = mode->bus_fmt;
 		fmt->format.field = V4L2_FIELD_NONE;
-		if (fmt->pad < PAD_MAX && fmt->pad >= PAD0)
+		if (fmt->pad < PAD_MAX && fmt->pad > PAD0)
 			fmt->reserved[0] = mode->vc[fmt->pad];
 		else
 			fmt->reserved[0] = mode->vc[PAD0];
@@ -481,7 +523,18 @@ static int tp2855_g_mbus_config(struct v4l2_subdev *sd, unsigned int pad_id,
 				 struct v4l2_mbus_config *cfg)
 {
 	cfg->type = V4L2_MBUS_CSI2_DPHY;
-	cfg->bus.mipi_csi2.num_data_lanes = 4;
+	cfg->bus.mipi_csi2.num_data_lanes = TP2855_LANES;
+
+	return 0;
+}
+
+static int tp2855_g_frame_interval(struct v4l2_subdev *sd,
+				    struct v4l2_subdev_frame_interval *fi)
+{
+	struct tp2855 *tp2855 = to_tp2855(sd);
+	const struct tp2855_mode *mode = tp2855->cur_mode;
+
+	fi->interval = mode->max_fps;
 
 	return 0;
 }
@@ -531,8 +584,11 @@ static long tp2855_compat_ioctl32(struct v4l2_subdev *sd,
 		}
 
 		ret = tp2855_ioctl(sd, cmd, inf);
-		if (!ret)
+		if (!ret) {
 			ret = copy_to_user(up, inf, sizeof(*inf));
+			if (ret)
+				ret = -EFAULT;
+		}
 		kfree(inf);
 		break;
 	case RKMODULE_AWB_CFG:
@@ -545,6 +601,8 @@ static long tp2855_compat_ioctl32(struct v4l2_subdev *sd,
 		ret = copy_from_user(cfg, up, sizeof(*cfg));
 		if (!ret)
 			ret = tp2855_ioctl(sd, cmd, cfg);
+		else
+			ret = -EFAULT;
 		kfree(cfg);
 		break;
 	default:
@@ -556,29 +614,86 @@ static long tp2855_compat_ioctl32(struct v4l2_subdev *sd,
 }
 #endif
 
+static int tp2855_get_channel_input_status(struct tp2855 *tp2855, u8 ch)
+{
+	struct i2c_client *client = tp2855->client;
+	u8 val = 0;
+
+	mutex_lock(&tp2855->mutex);
+	tp2855_write_reg(client, 0x40, ch);
+	tp2855_read_reg(client, 0x01, &val);
+	mutex_unlock(&tp2855->mutex);
+	dev_dbg(&client->dev, "input_status ch %d : %x\n", ch, val);
+
+	return (val & 0x80) ? 0 : 1;
+}
+
+static int tp2855_get_all_input_status(struct tp2855 *tp2855, u8 *detect_status)
+{
+	struct i2c_client *client = tp2855->client;
+	u8 val = 0, i;
+
+	for (i = 0; i < PAD_MAX; i++) {
+		tp2855_write_reg(client, 0x40, i);
+		tp2855_read_reg(client, 0x01, &val);
+		detect_status[i] = tp2855_get_channel_input_status(tp2855, i);
+	}
+
+	return 0;
+}
+
+static int tp2855_set_decoder_mode(struct i2c_client *client, int ch, int status)
+{
+	u8 val = 0;
+
+	tp2855_write_reg(client, 0x40, ch);
+	tp2855_read_reg(client, 0x26, &val);
+	if (status)
+		val |= 0x1;
+	else
+		val &= ~0x1;
+	tp2855_write_reg(client, 0x26, val);
+
+	return 0;
+}
+
 static int detect_thread_function(void *data)
 {
 	struct tp2855 *tp2855 = (struct tp2855 *) data;
 	struct i2c_client *client = tp2855->client;
-	u8 detect_status = 0, reg26_val = 0;
-	bool lost_video = false;
-	tp2855->lost_video_status = true;
+	u8 detect_status = 0;
+	int i, need_reset_wait = -1;
+
+	if (tp2855->power_on) {
+		tp2855_get_all_input_status(tp2855, tp2855->detect_status);
+		for (i = 0; i < PAD_MAX; i++)
+			tp2855_set_decoder_mode(client, i, tp2855->detect_status[i]);
+		tp2855->do_reset = 0;
+	}
+
 	while (!kthread_should_stop()) {
 		if (tp2855->power_on) {
-			tp2855_write_reg(client, 0x40, 0x04);
-			tp2855_read_reg(client, 0x01, &detect_status);
-			tp2855_read_reg(client, 0x26, &reg26_val);
-			lost_video = (detect_status & 0x80) ? true : false;
-			if (tp2855->lost_video_status != lost_video) {
-				if (lost_video) {
-					tp2855_write_reg(client, 0x26, (reg26_val & 0xfe));
-				} else {
-					tp2855_write_reg(client, 0x26, (reg26_val | 0x01));
+			for (i = 0; i < PAD_MAX; i++) {
+				detect_status = tp2855_get_channel_input_status(tp2855, i);
+				if (detect_status != tp2855->detect_status[i]) {
+					if (!detect_status)
+						dev_info(&client->dev,
+							"detect channel %d video plug out\n", i);
+					else
+						dev_info(&client->dev,
+							"detect channel %d video plug in\n", i);
+					tp2855->detect_status[i] = detect_status;
+					tp2855_set_decoder_mode(client, i, detect_status);
+					need_reset_wait = 5;
 				}
-				tp2855->lost_video_status = lost_video;
-				tp2855_read_reg(client, 0x26, &reg26_val);
-				dev_err(&client->dev, "tp2855 detect video lost status %d reg26_val %x\n",
-						lost_video, reg26_val);
+			}
+			if (need_reset_wait > 0) {
+				need_reset_wait--;
+			} else if (need_reset_wait == 0) {
+				need_reset_wait = -1;
+				tp2855->do_reset = 1;
+				dev_info(&client->dev,
+					"trigger reset time up\n");
 			}
 		}
 		set_current_state(TASK_INTERRUPTIBLE);
@@ -609,24 +724,110 @@ static int __maybe_unused detect_thread_stop(struct tp2855 *tp2855) {
 	return 0;
 }
 
+static int tp2855_set_channel_reso(struct tp2855 *tp2855, int ch,
+			    enum tp2855_support_reso reso)
+{
+	int val = reso;
+	u8 tmp = 0;
+	const unsigned char SYS_MODE[5] = { 0x01, 0x02, 0x04, 0x08, 0x0f };
+
+	tp2855_write_reg(tp2855->client, 0x40, ch);
+
+	switch (val) {
+	case TP2855_CVSTD_1080P_25:
+		dev_info(&tp2855->client->dev, "set channel %d 1080P_25\n", ch);
+		tp2855_read_reg(tp2855->client, 0xf5, &tmp);
+		tmp &= ~SYS_MODE[ch];
+		tp2855_write_reg(tp2855->client, 0xf5, tmp);
+		tp2855_write_array(tp2855->client, common_setting_594M_1080p_25fps_regs,
+					ARRAY_SIZE(common_setting_594M_1080p_25fps_regs));
+		break;
+	case TP2855_CVSTD_720P_25:
+		dev_info(&tp2855->client->dev, "set channel %d 720P_25\n", ch);
+		tp2855_read_reg(tp2855->client, 0xf5, &tmp);
+		tmp |= SYS_MODE[ch];
+		tp2855_write_reg(tp2855->client, 0xf5, tmp);
+		tp2855_write_array(tp2855->client, common_setting_297M_720p_25fps_regs,
+					ARRAY_SIZE(common_setting_297M_720p_25fps_regs));
+		break;
+	default:
+		dev_info(&tp2855->client->dev,
+			"set channel %d is not supported, default 1080P_25\n", ch);
+		tp2855_read_reg(tp2855->client, 0xf5, &tmp);
+		tmp &= ~SYS_MODE[ch];
+		tp2855_write_reg(tp2855->client, 0xf5, tmp);
+		tp2855_write_array(tp2855->client, common_setting_594M_1080p_25fps_regs,
+					ARRAY_SIZE(common_setting_594M_1080p_25fps_regs));
+		break;
+	}
+
+#if TP2855_TEST_PATTERN
+	tp2855_write_reg(tp2855->client, 0x2a, 0x3c);
+#endif
+
+	return 0;
+}
+
+static int tp2855_get_channel_reso(struct tp2855 *tp2855, int ch)
+{
+	u8 detect_fmt = 0xff;
+	u8 reso = 0xff;
+
+	tp2855_write_reg(tp2855->client, 0x40, ch);
+	tp2855_read_reg(tp2855->client, 0x03, &detect_fmt);
+	reso = detect_fmt & 0x7;
+
+	switch (reso) {
+	case TP2855_CVSTD_1080P_30:
+		dev_info(&tp2855->client->dev, "detect channel %d 1080P_30\n", ch);
+		break;
+	case TP2855_CVSTD_1080P_25:
+		dev_info(&tp2855->client->dev, "detect channel %d 1080P_25\n", ch);
+		tp2855->cur_mode = &supported_modes[0];
+		break;
+	case TP2855_CVSTD_720P_30:
+		dev_info(&tp2855->client->dev, "detect channel %d 720P_30\n", ch);
+		break;
+	case TP2855_CVSTD_720P_25:
+		dev_info(&tp2855->client->dev, "detect channel %d 720P_25\n", ch);
+		tp2855->cur_mode = &supported_modes[1];
+		break;
+	case TP2855_CVSTD_SD:
+		dev_info(&tp2855->client->dev, "detect channel %d SD\n", ch);
+		break;
+	default:
+		dev_info(&tp2855->client->dev,
+			"detect channel %d is not supported, default 1080P_25\n", ch);
+		reso = TP2855_CVSTD_1080P_25;
+		tp2855->cur_mode = &supported_modes[0];
+	}
+
+	return reso;
+}
+
+static __maybe_unused int auto_detect_channel_fmt(struct tp2855 *tp2855)
+{
+	int ch = 0;
+	enum tp2855_support_reso reso = 0xff;
+
+	for (ch = 0; ch < PAD_MAX; ch++) {
+		reso = tp2855_get_channel_reso(tp2855, ch);
+		tp2855_set_channel_reso(tp2855, ch, reso);
+	}
+
+	return 0;
+}
+
 static int __tp2855_start_stream(struct tp2855 *tp2855)
 {
 	int ret;
-	int array_size = 0;
 	struct i2c_client *client = tp2855->client;
 
-    if (tp2855->cur_mode->global_reg_list == common_setting_594M_1080p_25fps_regs) {
-		array_size = ARRAY_SIZE(common_setting_594M_1080p_25fps_regs);
-	} else if (tp2855->cur_mode->global_reg_list == common_setting_297M_720p_25fps_regs) {
-		array_size = ARRAY_SIZE(common_setting_297M_720p_25fps_regs);
-	} else {
-		return -1;
-	}
-
+	auto_detect_channel_fmt(tp2855);
 	ret = tp2855_write_array(tp2855->client,
-		tp2855->cur_mode->global_reg_list, array_size);
+		tp2855->cur_mode->mipi_reg_list, tp2855->cur_mode->mipi_reg_size);
 	if (ret) {
-		dev_err(&client->dev, "__tp2855_start_stream global_reg_list faild");
+		dev_err(&client->dev, "__tp2855_start_stream mipi_reg_list failed");
 		return ret;
 	}
 
@@ -637,10 +838,6 @@ static int __tp2855_start_stream(struct tp2855 *tp2855)
 
 static int __tp2855_stop_stream(struct tp2855 *tp2855)
 {
-	struct i2c_client *client = tp2855->client;
-
-	tp2855_write_reg(client, 0x40, 0x08);
-	tp2855_write_reg(client, 0x23, 0x02);
 	detect_thread_stop(tp2855);
 
 	return 0;
@@ -738,6 +935,12 @@ static int __tp2855_power_on(struct tp2855 *tp2855)
 		goto err_clk;
 	}
 
+	ret = regulator_bulk_enable(TP2855_NUM_SUPPLIES, tp2855->supplies);
+	if (ret < 0) {
+		dev_err(dev, "Failed to enable regulators\n");
+		goto disable_clk;
+	}
+
 	if (!IS_ERR(tp2855->reset_gpio)) {
 		gpiod_set_value_cansleep(tp2855->reset_gpio, 1);
 		usleep_range(10*1000, 20*1000);
@@ -749,6 +952,9 @@ static int __tp2855_power_on(struct tp2855 *tp2855)
 
 	return 0;
 
+disable_clk:
+	clk_disable_unprepare(tp2855->xvclk);
+
 err_clk:
 	if (!IS_ERR(tp2855->reset_gpio))
 		gpiod_set_value_cansleep(tp2855->reset_gpio, 0);
@@ -779,6 +985,7 @@ static void __tp2855_power_off(struct tp2855 *tp2855)
 
 	if (!IS_ERR(tp2855->power_gpio))
 		gpiod_set_value_cansleep(tp2855->power_gpio, 0);
+	regulator_bulk_disable(TP2855_NUM_SUPPLIES, tp2855->supplies);
 }
 
 static int tp2855_initialize_controls(struct tp2855 *tp2855)
@@ -877,6 +1084,7 @@ static const struct v4l2_subdev_internal_ops tp2855_internal_ops = {
 
 static const struct v4l2_subdev_video_ops tp2855_video_ops = {
 	.s_stream = tp2855_stream,
+	.g_frame_interval = tp2855_g_frame_interval,
 };
 
 static const struct v4l2_subdev_pad_ops tp2855_subdev_pad_ops = {
@@ -901,17 +1109,39 @@ static const struct v4l2_subdev_ops tp2855_subdev_ops = {
 	.pad   = &tp2855_subdev_pad_ops,
 };
 
-static int check_chip_id(struct i2c_client *client){
+static int check_chip_id(struct i2c_client *client)
+{
+	int ret = 0;
 	struct device *dev = &client->dev;
-	unsigned char chip_id = 0xFF;
-	tp2855_read_reg(client, 0x34, &chip_id);
-	dev_err(dev, "chip_id : 0x%2x\n", chip_id);
-	if (chip_id != 0x0) {
-		return -1;
+	u8 chip_id_h = 0, chip_id_l = 0;
+
+	tp2855_write_reg(client, 0x40, 0x0);
+	tp2855_read_reg(client, 0xFE, &chip_id_h);
+	tp2855_read_reg(client, 0xFF, &chip_id_l);
+
+	if (chip_id_h != 0x28 || chip_id_l != 0x55) {
+		dev_err(dev, "expected 0x2855, detected: 0x%0x%0x\n", chip_id_h, chip_id_l);
+		ret = -EINVAL;
+	} else {
+		dev_info(dev, "Found TP2855 sensor, chip id: 0x%0x%0x\n", chip_id_h, chip_id_l);
 	}
-	return 0;
+
+	return ret;
 }
 
+static int tp2855_configure_regulators(struct tp2855 *tp2855)
+{
+	unsigned int i;
+
+	for (i = 0; i < TP2855_NUM_SUPPLIES; i++)
+		tp2855->supplies[i].supply = tp2855_supply_names[i];
+
+	return devm_regulator_bulk_get(&tp2855->client->dev,
+				       TP2855_NUM_SUPPLIES,
+				       tp2855->supplies);
+}
+
+
 static int tp2855_probe(struct i2c_client *client,
 			 const struct i2c_device_id *id)
 {
@@ -961,6 +1191,12 @@ static int tp2855_probe(struct i2c_client *client,
 	if (IS_ERR(tp2855->power_gpio))
 		dev_warn(dev, "Failed to get power-gpios\n");
 
+	ret = tp2855_configure_regulators(tp2855);
+	if (ret) {
+		dev_err(dev, "Failed to get power regulators\n");
+		return ret;
+	}
+
 	tp2855->pinctrl = devm_pinctrl_get(dev);
 	if (!IS_ERR(tp2855->pinctrl)) {
 		tp2855->pins_default =
@@ -1075,6 +1311,7 @@ static const struct dev_pm_ops tp2855_pm_ops = {
 #if IS_ENABLED(CONFIG_OF)
 static const struct of_device_id tp2855_of_match[] = {
 	{ .compatible = "tp2855" },
+	{ .compatible = "tp2815" },
 	{},
 };
 MODULE_DEVICE_TABLE(of, tp2855_of_match);
@@ -1082,6 +1319,7 @@ MODULE_DEVICE_TABLE(of, tp2855_of_match);
 
 static const struct i2c_device_id tp2855_match_id[] = {
 	{ "tp2855", 0 },
+	{ "tp2815", 0 },
 	{ },
 };
 
@@ -1096,18 +1334,21 @@ static struct i2c_driver tp2855_i2c_driver = {
 	.id_table	= tp2855_match_id,
 };
 
-static int __init sensor_mod_init(void)
+int tp2855_sensor_mod_init(void)
 {
 	return i2c_add_driver(&tp2855_i2c_driver);
 }
 
-static void __exit sensor_mod_exit(void)
+#ifndef CONFIG_VIDEO_REVERSE_IMAGE
+device_initcall_sync(tp2855_sensor_mod_init);
+#endif
+
+static void __exit tp2855_sensor_mod_exit(void)
 {
 	i2c_del_driver(&tp2855_i2c_driver);
 }
 
-device_initcall_sync(sensor_mod_init);
-module_exit(sensor_mod_exit);
+module_exit(tp2855_sensor_mod_exit);
 
 MODULE_AUTHOR("Vicent Chi <vicent.chi@rock-chips.com>");
 MODULE_DESCRIPTION("tp2855 sensor driver");
diff --git a/drivers/media/i2c/tp2855.h b/drivers/media/i2c/tp2855.h
new file mode 100644
index 000000000000..5d6280c43503
--- /dev/null
+++ b/drivers/media/i2c/tp2855.h
@@ -0,0 +1,24 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * tp2855 driver
+ *
+ * Copyright (C) 2024 Rockchip Electronics Co., Ltd.
+ *
+ */
+#ifndef _TP2855_H_
+#define _TP2855_H_
+
+enum tp2855_support_reso {
+	TP2855_CVSTD_720P_60 = 0,
+	TP2855_CVSTD_720P_50,
+	TP2855_CVSTD_1080P_30,
+	TP2855_CVSTD_1080P_25,
+	TP2855_CVSTD_720P_30,
+	TP2855_CVSTD_720P_25,
+	TP2855_CVSTD_SD,
+	TP2855_CVSTD_OTHER,
+};
+
+int tp2855_sensor_mod_init(void);
+
+#endif
diff --git a/drivers/media/platform/rockchip/cif/capture.c b/drivers/media/platform/rockchip/cif/capture.c
index a62be1f558f5..f97e9e7289a5 100644
--- a/drivers/media/platform/rockchip/cif/capture.c
+++ b/drivers/media/platform/rockchip/cif/capture.c
@@ -4577,7 +4577,8 @@ static int rkcif_csi_channel_set_v1(struct rkcif_stream *stream,
 				CSI_START_INTEN(channel->id) :
 				CSI_START_INTEN_RK3576(channel->id));
 
-		if (priv && priv->mode.rdbk_mode >= RKISP_VICAP_RDBK_AIQ && detect_stream->is_line_wake_up) {
+		if ((!priv || (priv && priv->mode.rdbk_mode >= RKISP_VICAP_RDBK_AIQ)) &&
+		    detect_stream->is_line_wake_up) {
 			rkcif_write_register_or(dev, CIF_REG_MIPI_LVDS_INTEN,
 						CSI_LINE_INTEN_RK3588(channel->id));
 			wait_line = dev->wait_line;
diff --git a/drivers/video/rockchip/vehicle/Kconfig b/drivers/video/rockchip/vehicle/Kconfig
index e61c864842f2..e772c0625537 100644
--- a/drivers/video/rockchip/vehicle/Kconfig
+++ b/drivers/video/rockchip/vehicle/Kconfig
@@ -8,6 +8,16 @@ config VIDEO_REVERSE_IMAGE
 
 if VIDEO_REVERSE_IMAGE
 
+config VIDEO_REVERSE_TP2815
+	bool "tp2815 for reverse sensor"
+	help
+	  Say y if use tp2815.
+
+config VIDEO_REVERSE_TP2855
+	bool "tp2855 for reverse sensor"
+	help
+	  Say y if use tp2855.
+
 config VIDEO_REVERSE_NVP6324
 	bool "nvp6324 for reverse sensor"
 	help
@@ -28,11 +38,6 @@ config VIDEO_REVERSE_GC2145
 	help
 	  Say y if use gc2145.
 
-config VIDEO_REVERSE_TP2825
-	bool "tp2825 for reverse sensor"
-	help
-	  Say y if use tp2825.
-
 config VIDEO_REVERSE_AD7181
 	bool "ad7181 for reverse sensor"
 	help
diff --git a/drivers/video/rockchip/vehicle/Makefile b/drivers/video/rockchip/vehicle/Makefile
index 7da28b30548a..b88f826080a2 100644
--- a/drivers/video/rockchip/vehicle/Makefile
+++ b/drivers/video/rockchip/vehicle/Makefile
@@ -22,5 +22,8 @@ video_rkvehicle-$(CONFIG_VIDEO_REVERSE_GC2145) += \
 video_rkvehicle-$(CONFIG_VIDEO_REVERSE_TP2825) += \
 			vehicle_ad_tp2825.o
 
+video_rkvehicle-$(CONFIG_VIDEO_REVERSE_TP2855) += \
+			vehicle_ad_tp2855.o
+
 video_rkvehicle-$(CONFIG_VIDEO_REVERSE_AD7181) += \
 			vehicle_ad_7181.o
diff --git a/drivers/video/rockchip/vehicle/vehicle_ad_tp2855.c b/drivers/video/rockchip/vehicle/vehicle_ad_tp2855.c
new file mode 100644
index 000000000000..8348a3c4772f
--- /dev/null
+++ b/drivers/video/rockchip/vehicle/vehicle_ad_tp2855.c
@@ -0,0 +1,843 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * vehicle sensor tp2855
+ *
+ * Copyright (C) 2024 Rockchip Electronics Co., Ltd.
+ * Authors:
+ *
+ *      Jianwei Fan <jianwei.fan@rock-chips.com>
+ *
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/delay.h>
+#include <linux/sched.h>
+#include <linux/errno.h>
+#include <linux/sysctl.h>
+#include <linux/interrupt.h>
+#include <linux/platform_device.h>
+#include <linux/proc_fs.h>
+#include <linux/suspend.h>
+#include <linux/delay.h>
+#include <linux/io.h>
+#include <linux/irq.h>
+#include <linux/uaccess.h>
+#include <linux/of_gpio.h>
+#include <linux/of_irq.h>
+#include "vehicle_cfg.h"
+#include "vehicle_main.h"
+#include "vehicle_ad.h"
+#include "vehicle_ad_tp2855.h"
+
+enum {
+	CVSTD_720P60 = 0,
+	CVSTD_720P50,
+	CVSTD_1080P30,
+	CVSTD_1080P25,
+	CVSTD_720P30,
+	CVSTD_720P25,
+	CVSTD_SVGAP30,
+	CVSTD_SD,
+	CVSTD_NTSC,
+	CVSTD_PAL
+};
+
+enum {
+	FORCE_PAL_WIDTH = 960,
+	FORCE_PAL_HEIGHT = 576,
+	FORCE_NTSC_WIDTH = 960,
+	FORCE_NTSC_HEIGHT = 480,
+	FORCE_SVGA_WIDTH = 800,
+	FORCE_SVGA_HEIGHT = 600,
+	FORCE_720P_WIDTH = 1280,
+	FORCE_720P_HEIGHT = 720,
+	FORCE_1080P_WIDTH = 1920,
+	FORCE_1080P_HEIGHT = 1080,
+	FORCE_CIF_OUTPUT_FORMAT = CIF_OUTPUT_FORMAT_420,
+};
+
+enum {
+	VIDEO_UNPLUG,
+	VIDEO_IN,
+	VIDEO_LOCKED,
+	VIDEO_UNLOCK
+};
+
+#define TP2855_LINK_FREQ_297M			(297000000UL >> 1)
+#define TP2855_LINK_FREQ_594M			(594000000UL >> 1)
+#define TP2855_XVCLK_FREQ			27000000
+#define TP2855_TEST_PATTERN			0
+
+static struct vehicle_ad_dev *tp2855_g_addev;
+static int cvstd_mode = CVSTD_1080P25;
+static int cvstd_old = CVSTD_720P25;
+static int cvstd_state = VIDEO_UNPLUG;
+// static int cvstd_old_state = VIDEO_UNLOCK;
+
+static bool g_tp2855_streaming;
+
+enum tp2855_support_reso {
+	TP2855_CVSTD_720P_60 = 0,
+	TP2855_CVSTD_720P_50,
+	TP2855_CVSTD_1080P_30,
+	TP2855_CVSTD_1080P_25,
+	TP2855_CVSTD_720P_30,
+	TP2855_CVSTD_720P_25,
+	TP2855_CVSTD_SD,
+	TP2855_CVSTD_OTHER,
+};
+
+struct regval {
+	u8 addr;
+	u8 val;
+};
+
+static __maybe_unused const struct regval common_setting_297M_720p_25fps_regs[] = {
+	{0x02, 0x42},
+	{0x07, 0xc0},
+	{0x0b, 0xc0},
+	{0x0c, 0x13},
+	{0x0d, 0x50},
+	{0x15, 0x13},
+	{0x16, 0x15},
+	{0x17, 0x00},
+	{0x18, 0x19},
+	{0x19, 0xd0},
+	{0x1a, 0x25},
+	{0x1c, 0x07},
+	{0x1d, 0xbc},
+	{0x20, 0x30},
+	{0x21, 0x84},
+	{0x22, 0x36},
+	{0x23, 0x3c},
+	{0x2b, 0x60},
+	{0x2c, 0x0a},
+	{0x2d, 0x30},
+	{0x2e, 0x70},
+	{0x30, 0x48},
+	{0x31, 0xbb},
+	{0x32, 0x2e},
+	{0x33, 0x90},
+	{0x35, 0x25},
+	{0x38, 0x00},
+	{0x39, 0x18},
+
+	{0x02, 0x46},
+	{0x0d, 0x71},
+	{0x18, 0x1b},
+	{0x20, 0x40},
+	{0x21, 0x46},
+	{0x25, 0xfe},
+	{0x26, 0x01},
+	{0x2c, 0x3a},
+	{0x2d, 0x5a},
+	{0x2e, 0x40},
+	{0x30, 0x9e},
+	{0x31, 0x20},
+	{0x32, 0x10},
+	{0x33, 0x90},
+	// {0x23, 0x02}, //vi test ok
+	// {0x23, 0x00},
+};
+
+static __maybe_unused const struct regval mipi_setting_297M_4ch_4lane_regs[] = {
+	{0x40, 0x08},
+	{0x01, 0xf0},
+	{0x02, 0x01},
+	{0x08, 0x0f},
+	{0x20, 0x44},
+	{0x34, 0xe4},
+	{0x14, 0x44},
+	{0x15, 0x0d},
+	{0x25, 0x04},
+	{0x26, 0x03},
+	{0x27, 0x09},
+	{0x29, 0x02},
+	{0x33, 0x0f},
+	{0x33, 0x00},
+	{0x14, 0xc4},
+	{0x14, 0x44},
+};
+
+static __maybe_unused const struct regval mipi_setting_594M_4ch_4lane_regs[] = {
+	{0x40, 0x08},
+	{0x01, 0xf0},
+	{0x02, 0x01},
+	{0x08, 0x0f},
+	{0x20, 0x44},
+	{0x34, 0xe4},
+	{0x15, 0x0C},
+	{0x25, 0x08},
+	{0x26, 0x06},
+	{0x27, 0x11},
+	{0x29, 0x0a},
+	{0x33, 0x0f},
+	{0x33, 0x00},
+	{0x14, 0x33},
+	{0x14, 0xb3},
+	{0x14, 0x33},
+	// {0x23, 0x02}, //vi test ok
+	// {0x23, 0x00},
+};
+
+static __maybe_unused const struct regval common_setting_594M_1080p_25fps_regs[] = {
+	{0x02, 0x40},
+	{0x07, 0xc0},
+	{0x0b, 0xc0},
+	{0x0c, 0x03},
+	{0x0d, 0x50},
+	{0x15, 0x03},
+	{0x16, 0xd2},
+	{0x17, 0x80},
+	{0x18, 0x29},
+	{0x19, 0x38},
+	{0x1a, 0x47},
+	{0x1c, 0x0a},
+	{0x1d, 0x50},
+	{0x20, 0x30},
+	{0x21, 0x84},
+	{0x22, 0x36},
+	{0x23, 0x3c},
+	{0x2b, 0x60},
+	{0x2c, 0x0a},
+	{0x2d, 0x30},
+	{0x2e, 0x70},
+	{0x30, 0x48},
+	{0x31, 0xbb},
+	{0x32, 0x2e},
+	{0x33, 0x90},
+	{0x35, 0x05},
+	{0x38, 0x00},
+	{0x39, 0x1C},
+
+	{0x02, 0x44},
+	{0x0d, 0x73},
+	{0x15, 0x01},
+	{0x16, 0xf0},
+	{0x20, 0x3c},
+	{0x21, 0x46},
+	{0x25, 0xfe},
+	{0x26, 0x0d},
+	{0x2c, 0x3a},
+	{0x2d, 0x54},
+	{0x2e, 0x40},
+	{0x30, 0xa5},
+	{0x31, 0x86},
+	{0x32, 0xfb},
+	{0x33, 0x60},
+};
+
+static void tp2855_reinit_parameter(struct vehicle_ad_dev *ad, unsigned char cvstd)
+{
+	int i = 0;
+
+	switch (cvstd) {
+	case CVSTD_1080P25:
+		ad->cfg.width = 1920;
+		ad->cfg.height = 1080;
+		ad->cfg.start_x = 0;
+		ad->cfg.start_y = 0;
+		ad->cfg.input_format = CIF_INPUT_FORMAT_YUV;
+		ad->cfg.output_format = FORCE_CIF_OUTPUT_FORMAT;
+		ad->cfg.field_order = 0;
+		ad->cfg.yuv_order = 0;/*00 - UYVY*/
+		ad->cfg.href = 0;
+		ad->cfg.vsync = 0;
+		ad->cfg.frame_rate = 25;
+		ad->cfg.mipi_freq = TP2855_LINK_FREQ_594M;
+		break;
+	case CVSTD_720P25:
+		ad->cfg.width = 1280;
+		ad->cfg.height = 720;
+		ad->cfg.start_x = 0;
+		ad->cfg.start_y = 0;
+		ad->cfg.input_format = CIF_INPUT_FORMAT_YUV;
+		ad->cfg.output_format = FORCE_CIF_OUTPUT_FORMAT;
+		ad->cfg.field_order = 0;
+		ad->cfg.yuv_order = 0;/*00 - UYVY*/
+		ad->cfg.href = 0;
+		ad->cfg.vsync = 0;
+		ad->cfg.frame_rate = 25;
+		ad->cfg.mipi_freq = TP2855_LINK_FREQ_297M;
+		break;
+	default:
+		ad->cfg.width = 1920;
+		ad->cfg.height = 1080;
+		ad->cfg.start_x = 0;
+		ad->cfg.start_y = 0;
+		ad->cfg.input_format = CIF_INPUT_FORMAT_YUV;
+		ad->cfg.output_format = FORCE_CIF_OUTPUT_FORMAT;
+		ad->cfg.field_order = 0;
+		ad->cfg.yuv_order = 0;/*00 - UYVY*/
+		ad->cfg.href = 0;
+		ad->cfg.vsync = 0;
+		ad->cfg.frame_rate = 25;
+		ad->cfg.mipi_freq = TP2855_LINK_FREQ_594M;
+		break;
+	}
+	ad->cfg.type = V4L2_MBUS_CSI2_DPHY;
+	ad->cfg.mbus_flags = V4L2_MBUS_CSI2_4_LANE | V4L2_MBUS_CSI2_CONTINUOUS_CLOCK |
+			 V4L2_MBUS_CSI2_CHANNELS;
+	ad->cfg.mbus_code = MEDIA_BUS_FMT_UYVY8_2X8;
+
+	switch (ad->cfg.mbus_flags & V4L2_MBUS_CSI2_LANES) {
+	case V4L2_MBUS_CSI2_1_LANE:
+		ad->cfg.lanes = 1;
+		break;
+	case V4L2_MBUS_CSI2_2_LANE:
+		ad->cfg.lanes = 2;
+		break;
+	case V4L2_MBUS_CSI2_3_LANE:
+		ad->cfg.lanes = 3;
+		break;
+	case V4L2_MBUS_CSI2_4_LANE:
+		ad->cfg.lanes = 4;
+		break;
+	default:
+		ad->cfg.lanes = 1;
+		break;
+	}
+
+	/* fix crop info from dts config */
+	for (i = 0; i < 4; i++) {
+		if ((ad->defrects[i].width == ad->cfg.width) &&
+		    (ad->defrects[i].height == ad->cfg.height)) {
+			ad->cfg.start_x = ad->defrects[i].crop_x;
+			ad->cfg.start_y = ad->defrects[i].crop_y;
+			ad->cfg.width = ad->defrects[i].crop_width;
+			ad->cfg.height = ad->defrects[i].crop_height;
+		}
+	}
+}
+
+/* sensor register write */
+static int tp2855_write_reg(struct vehicle_ad_dev *ad, u8 reg, u8 val)
+{
+	struct i2c_msg msg;
+	u8 buf[2];
+	int ret;
+
+	buf[0] = reg & 0xFF;
+	buf[1] = val;
+
+	msg.addr = ad->i2c_add;
+	msg.flags = 0;
+	msg.buf = buf;
+	msg.len = sizeof(buf);
+
+	ret = i2c_transfer(ad->adapter, &msg, 1);
+	if (ret >= 0) {
+		usleep_range(300, 400);
+		return 0;
+	}
+
+	VEHICLE_DGERR("tp2855 write reg(0x%x val:0x%x) failed !\n", reg, val);
+
+	return ret;
+}
+
+static int tp2855_write_array(struct vehicle_ad_dev *ad,
+			       const struct regval *regs, int size)
+{
+	int i, ret = 0;
+
+	i = 0;
+	while (i < size) {
+		ret = tp2855_write_reg(ad, regs[i].addr, regs[i].val);
+		if (ret) {
+			VEHICLE_DGERR("%s failed !\n", __func__);
+			break;
+		}
+		i++;
+	}
+
+	return ret;
+}
+
+/* sensor register read */
+static int tp2855_read_reg(struct vehicle_ad_dev *ad, u8 reg, u8 *val)
+{
+	struct i2c_msg msg[2];
+	u8 buf[1];
+	int ret;
+
+	buf[0] = reg & 0xFF;
+
+	msg[0].addr = ad->i2c_add;
+	msg[0].flags = 0;
+	msg[0].buf = buf;
+	msg[0].len = sizeof(buf);
+
+	msg[1].addr = ad->i2c_add;
+	msg[1].flags = 0 | I2C_M_RD;
+	msg[1].buf = buf;
+	msg[1].len = 1;
+
+	ret = i2c_transfer(ad->adapter, msg, 2);
+	if (ret >= 0) {
+		*val = buf[0];
+		return 0;
+	}
+
+	VEHICLE_DGERR("tp2855 read reg(0x%x) failed !\n", reg);
+
+	return ret;
+}
+
+void tp2855_channel_set(struct vehicle_ad_dev *ad, int channel)
+{
+	ad->ad_chl = channel;
+	VEHICLE_DG("%s, channel set(%d)", __func__, ad->ad_chl);
+}
+
+int tp2855_ad_get_cfg(struct vehicle_cfg **cfg)
+{
+	if (!tp2855_g_addev)
+		return -1;
+
+	switch (cvstd_state) {
+	case VIDEO_UNPLUG:
+		tp2855_g_addev->cfg.ad_ready = false;
+		break;
+	case VIDEO_LOCKED:
+		tp2855_g_addev->cfg.ad_ready = true;
+		break;
+	case VIDEO_IN:
+		tp2855_g_addev->cfg.ad_ready = false;
+		break;
+	}
+
+	tp2855_g_addev->cfg.ad_ready = true;
+
+	*cfg = &tp2855_g_addev->cfg;
+
+	return 0;
+}
+
+void tp2855_ad_check_cif_error(struct vehicle_ad_dev *ad, int last_line)
+{
+	VEHICLE_INFO("%s, last_line %d\n", __func__, last_line);
+
+	if (last_line < 1)
+		return;
+
+	ad->cif_error_last_line = last_line;
+	if (cvstd_mode == CVSTD_PAL) {
+		if (last_line == FORCE_NTSC_HEIGHT) {
+			if (ad->state_check_work.state_check_wq)
+				queue_delayed_work(
+					ad->state_check_work.state_check_wq,
+					&ad->state_check_work.work,
+					msecs_to_jiffies(0));
+		}
+	} else if (cvstd_mode == CVSTD_NTSC) {
+		if (last_line == FORCE_PAL_HEIGHT) {
+			if (ad->state_check_work.state_check_wq)
+				queue_delayed_work(
+					ad->state_check_work.state_check_wq,
+					&ad->state_check_work.work,
+					msecs_to_jiffies(0));
+		}
+	} else if (cvstd_mode == CVSTD_1080P25) {
+		if (last_line == FORCE_1080P_HEIGHT) {
+			if (ad->state_check_work.state_check_wq)
+				queue_delayed_work(
+					ad->state_check_work.state_check_wq,
+					&ad->state_check_work.work,
+					msecs_to_jiffies(0));
+		}
+	} else if (cvstd_mode == CVSTD_720P25) {
+		if (last_line == FORCE_720P_HEIGHT) {
+			if (ad->state_check_work.state_check_wq)
+				queue_delayed_work(
+					ad->state_check_work.state_check_wq,
+					&ad->state_check_work.work,
+					msecs_to_jiffies(0));
+		}
+	}
+}
+
+static int tp2855_set_channel_reso(struct vehicle_ad_dev *ad, int ch,
+						enum tp2855_support_reso reso)
+{
+	int val = reso;
+	u8 tmp = 0;
+	const unsigned char SYS_MODE[5] = { 0x01, 0x02, 0x04, 0x08, 0x0f };
+
+	tp2855_write_reg(ad, 0x40, ch);
+
+	switch (val) {
+	case TP2855_CVSTD_1080P_25:
+		VEHICLE_INFO("set channel %d 1080P_25\n", ch);
+		tp2855_read_reg(ad, 0xf5, &tmp);
+		tmp &= ~SYS_MODE[ch];
+		tp2855_write_reg(ad, 0xf5, tmp);
+		tp2855_write_array(ad, common_setting_594M_1080p_25fps_regs,
+				ARRAY_SIZE(common_setting_594M_1080p_25fps_regs));
+		break;
+	case TP2855_CVSTD_720P_25:
+		VEHICLE_INFO("set channel %d 720P_25\n", ch);
+		tp2855_read_reg(ad, 0xf5, &tmp);
+		tmp |= SYS_MODE[ch];
+		tp2855_write_reg(ad, 0xf5, tmp);
+		tp2855_write_array(ad, common_setting_297M_720p_25fps_regs,
+				ARRAY_SIZE(common_setting_297M_720p_25fps_regs));
+		break;
+	default:
+		VEHICLE_INFO(
+			"set channel %d is not supported, default 1080P_25\n", ch);
+		tp2855_read_reg(ad, 0xf5, &tmp);
+		tmp &= ~SYS_MODE[ch];
+		tp2855_write_reg(ad, 0xf5, tmp);
+		tp2855_write_array(ad, common_setting_594M_1080p_25fps_regs,
+				ARRAY_SIZE(common_setting_594M_1080p_25fps_regs));
+		break;
+	}
+
+	//set color bar
+	if (!ad->last_detect_status)
+		tp2855_write_reg(ad, 0x2a, 0x3c);
+	else
+		tp2855_write_reg(ad, 0x2a, 0x30);
+
+	return 0;
+}
+
+static int tp2855_get_channel_reso(struct vehicle_ad_dev *ad, int ch)
+{
+	u8 detect_fmt = 0xff;
+	u8 reso = 0xff;
+
+	tp2855_write_reg(ad, 0x40, ch);
+	tp2855_read_reg(ad, 0x03, &detect_fmt);
+	reso = detect_fmt & 0x7;
+
+	switch (reso) {
+	case TP2855_CVSTD_1080P_30:
+		VEHICLE_DG("detect channel %d 1080P_30\n", ch);
+		cvstd_mode = CVSTD_1080P30;
+		break;
+	case TP2855_CVSTD_1080P_25:
+		VEHICLE_DG("detect channel %d 1080P_25\n", ch);
+		cvstd_mode = CVSTD_1080P25;
+		break;
+	case TP2855_CVSTD_720P_30:
+		VEHICLE_DG("detect channel %d 720P_30\n", ch);
+		cvstd_mode = CVSTD_720P30;
+		break;
+	case TP2855_CVSTD_720P_25:
+		VEHICLE_DG("detect channel %d 720P_25\n", ch);
+		cvstd_mode = CVSTD_720P25;
+		break;
+	case TP2855_CVSTD_SD:
+		VEHICLE_DG("detect channel %d SD\n", ch);
+		cvstd_mode = CVSTD_SD;
+		break;
+	default:
+		VEHICLE_DG(
+			"detect channel %d is not supported, default 1080P_25\n", ch);
+		reso = TP2855_CVSTD_1080P_25;
+		cvstd_mode = CVSTD_1080P25;
+	}
+
+	return reso;
+}
+
+static __maybe_unused int auto_detect_channel_fmt(struct vehicle_ad_dev *ad)
+{
+	int ch = 0;
+	enum tp2855_support_reso reso = 0xff;
+
+	for (ch = 0; ch < 4; ch++) {
+		reso = tp2855_get_channel_reso(ad, ch);
+		tp2855_set_channel_reso(ad, ch, reso);
+	}
+
+	return 0;
+}
+
+int tp2855_check_id(struct vehicle_ad_dev *ad)
+{
+	int ret = 0;
+	u8 chip_id_h = 0, chip_id_l = 0;
+
+	tp2855_write_reg(ad, 0x40, 0x0);
+	ret = tp2855_read_reg(ad, 0xFE, &chip_id_h);
+	ret |= tp2855_read_reg(ad, 0xFF, &chip_id_l);
+	if (ret)
+		return ret;
+
+	if (chip_id_h != 0x28 || chip_id_l != 0x55) {
+		VEHICLE_DGERR("%s: expected 0x2855, detected: 0x%0x%0x !",
+			ad->ad_name, chip_id_h, chip_id_l);
+		ret = -EINVAL;
+	} else {
+		VEHICLE_INFO("%s Found TP2855 sensor: id(0x%x%x) !\n",
+						__func__, chip_id_h, chip_id_l);
+	}
+
+	return ret;
+}
+
+static int tp2855_get_channel_input_status(struct vehicle_ad_dev *ad, u8 ch)
+{
+	u8 val = 0;
+
+	tp2855_write_reg(ad, 0x40, ch);
+	tp2855_read_reg(ad, 0x01, &val);
+	VEHICLE_DG("input_status ch %d : %x\n", ch, val);
+
+	return (val & 0x80) ? 0 : 1;
+}
+
+static int tp2855_get_all_input_status(struct vehicle_ad_dev *ad, u8 *detect_status)
+{
+	u8 val = 0, i;
+
+	for (i = 0; i < 4; i++) {
+		tp2855_write_reg(ad, 0x40, i);
+		tp2855_read_reg(ad, 0x01, &val);
+		detect_status[i] = tp2855_get_channel_input_status(ad, i);
+	}
+
+	return 0;
+}
+
+static int tp2855_set_decoder_mode(struct vehicle_ad_dev *ad, int ch, int status)
+{
+	u8 val = 0;
+
+	tp2855_write_reg(ad, 0x40, ch);
+	tp2855_read_reg(ad, 0x26, &val);
+	if (status)
+		val |= 0x1;
+	else
+		val &= ~0x1;
+	tp2855_write_reg(ad, 0x26, val);
+
+	return 0;
+}
+
+static int tp2855_detect(struct vehicle_ad_dev *ad)
+{
+	int i;
+	u8 detect_status[4];
+
+	tp2855_get_all_input_status(ad, detect_status);
+	for (i = 0; i < 4; i++)
+		tp2855_set_decoder_mode(ad, i, detect_status[i]);
+
+	return 0;
+}
+
+static int __tp2855_start_stream(struct vehicle_ad_dev *ad)
+{
+	int ret = 0;
+	int array_size = 0;
+
+	auto_detect_channel_fmt(ad);
+	switch (cvstd_mode) {
+	case CVSTD_1080P25:
+		array_size = ARRAY_SIZE(mipi_setting_594M_4ch_4lane_regs);
+		ret = tp2855_write_array(ad, mipi_setting_594M_4ch_4lane_regs, array_size);
+		break;
+	case CVSTD_720P25:
+		array_size = ARRAY_SIZE(mipi_setting_297M_4ch_4lane_regs);
+		ret = tp2855_write_array(ad, mipi_setting_297M_4ch_4lane_regs, array_size);
+		break;
+	}
+	if (ret) {
+		VEHICLE_INFO(" tp2855 start stream: wrote mipi reg failed");
+		return ret;
+	}
+	tp2855_detect(ad);
+
+	msleep(50);
+
+	return 0;
+}
+
+static int __tp2855_stop_stream(struct vehicle_ad_dev *ad)
+{
+	return 0;
+}
+
+static int tp2855_check_cvstd(struct vehicle_ad_dev *ad, bool activate_check)
+{
+	tp2855_get_channel_reso(ad, ad->ad_chl);
+
+	return 0;
+}
+
+int tp2855_stream(struct vehicle_ad_dev *ad, int enable)
+{
+	VEHICLE_INFO("%s on(%d)\n", __func__, enable);
+
+	g_tp2855_streaming = (enable != 0);
+	if (g_tp2855_streaming) {
+		__tp2855_start_stream(ad);
+		if (ad->state_check_work.state_check_wq)
+			queue_delayed_work(ad->state_check_work.state_check_wq,
+				&ad->state_check_work.work, msecs_to_jiffies(200));
+	} else {
+		__tp2855_stop_stream(ad);
+		if (ad->state_check_work.state_check_wq)
+			cancel_delayed_work_sync(&ad->state_check_work.work);
+		VEHICLE_DG("%s(%d): cancel_queue_delayed_work!\n", __func__, __LINE__);
+	}
+
+	return 0;
+}
+
+static void tp2855_power_on(struct vehicle_ad_dev *ad)
+{
+	if (gpio_is_valid(ad->power)) {
+		gpio_request(ad->power, "tp2855_power");
+		gpio_direction_output(ad->power, ad->pwr_active);
+		/* gpio_set_value(ad->power, ad->pwr_active); */
+	}
+
+	if (gpio_is_valid(ad->powerdown)) {
+		gpio_request(ad->powerdown, "tp2855_pwd");
+		gpio_direction_output(ad->powerdown, 1);
+		/* gpio_set_value(ad->powerdown, !ad->pwdn_active); */
+	}
+
+	if (gpio_is_valid(ad->reset)) {
+		gpio_request(ad->reset, "tp2855_rst");
+		gpio_direction_output(ad->reset, 0);
+		usleep_range(1500, 2000);
+		gpio_direction_output(ad->reset, 1);
+	}
+	mdelay(100);
+}
+
+static void tp2855_power_off(struct vehicle_ad_dev *ad)
+{
+	if (gpio_is_valid(ad->reset))
+		gpio_free(ad->reset);
+	if (gpio_is_valid(ad->power))
+		gpio_free(ad->power);
+	if (gpio_is_valid(ad->powerdown))
+		gpio_free(ad->powerdown);
+}
+
+static __maybe_unused int tp2855_auto_detect_hotplug(struct vehicle_ad_dev *ad)
+{
+	u8 detect_status = 0;
+
+	tp2855_write_reg(ad, 0x40, 0x04);
+	tp2855_read_reg(ad, 0x01, &detect_status);
+
+	ad->detect_status = (detect_status & 0x80) ? 0 : 1;
+	VEHICLE_DG("input_status %x\n", ad->detect_status);
+
+	return 0;
+}
+
+static void tp2855_check_state_work(struct work_struct *work)
+{
+	struct vehicle_ad_dev *ad;
+
+	ad = tp2855_g_addev;
+	tp2855_auto_detect_hotplug(ad);
+	tp2855_check_cvstd(ad, true);
+	if (ad->detect_status != ad->last_detect_status || cvstd_old != cvstd_mode) {
+		ad->last_detect_status = ad->detect_status;
+		cvstd_old = cvstd_mode;
+		vehicle_ad_stat_change_notify();
+	}
+
+	if (g_tp2855_streaming)
+		queue_delayed_work(ad->state_check_work.state_check_wq,
+				   &ad->state_check_work.work, msecs_to_jiffies(100));
+}
+
+int tp2855_ad_deinit(void)
+{
+	struct vehicle_ad_dev *ad;
+
+	ad = tp2855_g_addev;
+
+	if (!ad)
+		return -1;
+
+	if (ad->state_check_work.state_check_wq) {
+		cancel_delayed_work_sync(&ad->state_check_work.work);
+		flush_delayed_work(&ad->state_check_work.work);
+		flush_workqueue(ad->state_check_work.state_check_wq);
+		destroy_workqueue(ad->state_check_work.state_check_wq);
+	}
+
+	tp2855_power_off(ad);
+
+	return 0;
+}
+
+static __maybe_unused int get_ad_mode_from_fix_format(int fix_format)
+{
+	int mode = -1;
+
+	switch (fix_format) {
+	case AD_FIX_FORMAT_PAL:
+	case AD_FIX_FORMAT_NTSC:
+	case AD_FIX_FORMAT_720P_50FPS:
+	case AD_FIX_FORMAT_720P_30FPS:
+	case AD_FIX_FORMAT_720P_25FPS:
+		mode = CVSTD_720P25;
+		break;
+	case AD_FIX_FORMAT_1080P_30FPS:
+	case AD_FIX_FORMAT_1080P_25FPS:
+
+	default:
+		mode = CVSTD_720P25;
+		break;
+	}
+
+	return mode;
+}
+
+int tp2855_ad_init(struct vehicle_ad_dev *ad)
+{
+	int val;
+	int i = 0;
+
+	tp2855_g_addev = ad;
+
+	/*  1. i2c init */
+	while (ad->adapter == NULL) {
+		ad->adapter = i2c_get_adapter(ad->i2c_chl);
+		usleep_range(10000, 12000);
+	}
+	if (ad->adapter == NULL)
+		return -ENODEV;
+
+	if (!i2c_check_functionality(ad->adapter, I2C_FUNC_I2C))
+		return -EIO;
+
+	/*  2. ad power on sequence */
+	tp2855_power_on(ad);
+
+	while (++i < 5) {
+		usleep_range(1000, 1200);
+		val = vehicle_generic_sensor_read(ad, 0xf0);
+		if (val != 0xff)
+			break;
+		VEHICLE_INFO("tp2855_init i2c_reg_read fail\n");
+	}
+
+	tp2855_reinit_parameter(ad, cvstd_mode);
+	ad->last_detect_status = false;
+
+	/*  create workqueue to detect signal change */
+	INIT_DELAYED_WORK(&ad->state_check_work.work, tp2855_check_state_work);
+	ad->state_check_work.state_check_wq =
+		create_singlethread_workqueue("vehicle-ad-tp2855");
+
+	queue_delayed_work(ad->state_check_work.state_check_wq,
+			   &ad->state_check_work.work, msecs_to_jiffies(100));
+
+	return 0;
+}
diff --git a/drivers/video/rockchip/vehicle/vehicle_ad_tp2855.h b/drivers/video/rockchip/vehicle/vehicle_ad_tp2855.h
new file mode 100644
index 000000000000..52332263d1ca
--- /dev/null
+++ b/drivers/video/rockchip/vehicle/vehicle_ad_tp2855.h
@@ -0,0 +1,18 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Rockchip Vehicle driver
+ *
+ * Copyright (C) 2024 Rockchip Electronics Co., Ltd.
+ */
+#ifndef __VEHICLE_AD_TP2855_H__
+#define __VEHICLE_AD_TP2855_H__
+
+int tp2855_ad_init(struct vehicle_ad_dev *ad);
+int tp2855_ad_deinit(void);
+int tp2855_ad_get_cfg(struct vehicle_cfg **cfg);
+void tp2855_ad_check_cif_error(struct vehicle_ad_dev *ad, int last_line);
+int tp2855_check_id(struct vehicle_ad_dev *ad);
+int tp2855_stream(struct vehicle_ad_dev *ad, int enable);
+void tp2855_channel_set(struct vehicle_ad_dev *ad, int channel);
+
+#endif
diff --git a/drivers/video/rockchip/vehicle/vehicle_generic_sensor.c b/drivers/video/rockchip/vehicle/vehicle_generic_sensor.c
index 6f1ab9c42215..32102d7cdcb0 100644
--- a/drivers/video/rockchip/vehicle/vehicle_generic_sensor.c
+++ b/drivers/video/rockchip/vehicle/vehicle_generic_sensor.c
@@ -14,6 +14,7 @@
 #include <linux/of_gpio.h>
 #include "vehicle_ad.h"
 #include "vehicle_ad_7181.h"
+#include "vehicle_ad_tp2855.h"
 #include "vehicle_ad_tp2825.h"
 #include "vehicle_ad_gc2145.h"
 #include "vehicle_ad_nvp6324.h"
@@ -23,6 +24,7 @@
 #include "../../../../drivers/media/i2c/jaguar1_drv/jaguar1_v4l2.h"
 #include "../../../../drivers/media/i2c/nvp6188.h"
 #include "../../../../drivers/media/i2c/max96714.h"
+#include "../../../../drivers/media/i2c/tp2855.h"
 
 struct vehicle_sensor_ops {
 	const char *name;
@@ -117,6 +119,21 @@ static struct vehicle_sensor_ops sensor_cb_series[] = {
 #ifdef CONFIG_VIDEO_NVP6188
 		.sensor_mod_init = nvp6188_sensor_mod_init
 #endif
+#endif
+	},
+	{
+		.name = "tp2855",
+#ifdef CONFIG_VIDEO_REVERSE_TP2855
+		.sensor_init = tp2855_ad_init,
+		.sensor_deinit = tp2855_ad_deinit,
+		.sensor_stream = tp2855_stream,
+		.sensor_get_cfg = tp2855_ad_get_cfg,
+		.sensor_check_cif_error = tp2855_ad_check_cif_error,
+		.sensor_check_id_cb = tp2855_check_id,
+		.sensor_set_channel = tp2855_channel_set,
+#ifdef CONFIG_VIDEO_TP2855
+		.sensor_mod_init = tp2855_sensor_mod_init
+#endif
 #endif
 	}
 };
@@ -367,7 +384,7 @@ int vehicle_parse_sensor(struct vehicle_ad_dev *ad)
 
 void vehicle_ad_channel_set(struct vehicle_ad_dev *ad, int channel)
 {
-	if (sensor_cb->sensor_set_channel)
+	if (sensor_cb && sensor_cb->sensor_set_channel)
 		sensor_cb->sensor_set_channel(ad, channel);
 }
 
@@ -377,7 +394,7 @@ int vehicle_ad_init(struct vehicle_ad_dev *ad)
 	//WARN_ON(1);
 	VEHICLE_DGERR("%s(%d) ad_name:%s!", __func__, __LINE__, ad->ad_name);
 
-	if (sensor_cb->sensor_init) {
+	if (sensor_cb && sensor_cb->sensor_init) {
 		ret = sensor_cb->sensor_init(ad);
 		if (ret < 0) {
 			VEHICLE_DGERR("%s sensor_init failed!\n", ad->ad_name);
@@ -389,7 +406,7 @@ int vehicle_ad_init(struct vehicle_ad_dev *ad)
 		goto end;
 	}
 
-	if (sensor_cb->sensor_check_id_cb) {
+	if (sensor_cb && sensor_cb->sensor_check_id_cb) {
 		ret = sensor_cb->sensor_check_id_cb(ad);
 		if (ret < 0)
 			VEHICLE_DGERR("%s check id failed!\n", ad->ad_name);
@@ -403,7 +420,7 @@ int vehicle_ad_deinit(void)
 {
 	int ret = 0;
 
-	if (sensor_cb->sensor_deinit)
+	if (sensor_cb && sensor_cb->sensor_deinit)
 		ret = sensor_cb->sensor_deinit();
 	else
 		ret = -EINVAL;
@@ -428,7 +445,7 @@ struct vehicle_cfg *vehicle_ad_get_vehicle_cfg(void)
 {
 	struct vehicle_cfg *cfg = NULL;
 
-	if (sensor_cb->sensor_get_cfg)
+	if (sensor_cb && sensor_cb->sensor_get_cfg)
 		sensor_cb->sensor_get_cfg(&cfg);
 
 	return cfg;
@@ -436,6 +453,6 @@ struct vehicle_cfg *vehicle_ad_get_vehicle_cfg(void)
 
 void vehicle_ad_check_cif_error(struct vehicle_ad_dev *ad, int last_line)
 {
-	if (sensor_cb->sensor_get_cfg)
+	if (sensor_cb && sensor_cb->sensor_get_cfg)
 		sensor_cb->sensor_check_cif_error(ad, last_line);
 }