commit e05386da9e3d134b00f8493c7b1f08e75112a139
Author: zhaoxc0502 <zhaoxc0502@thundersoft.com>
Date:   Thu Jun 16 17:22:35 2022 +0800

    linux_drivers_media
    
    Change-Id: I049bfa49539911e2f2699823b3f446166db22bbe

diff --git a/drivers/media/Kconfig b/drivers/media/Kconfig
index a6d073f2e..3faba1011 100644
--- a/drivers/media/Kconfig
+++ b/drivers/media/Kconfig
@@ -43,7 +43,7 @@ config MEDIA_SUBDRV_AUTOSELECT
 	depends on HAS_IOMEM
 	select I2C
 	select I2C_MUX
-	default y if MEDIA_SUPPORT_FILTER
+	default n
 	help
 	  By default, a media driver auto-selects all possible ancillary
 	  devices such as tuners, sensors, video encoders/decoders and
diff --git a/drivers/media/i2c/ov5640.c b/drivers/media/i2c/ov5640.c
index 8f0812e85..1b2b947d7 100644
--- a/drivers/media/i2c/ov5640.c
+++ b/drivers/media/i2c/ov5640.c
@@ -98,7 +98,8 @@
 #define OV5640_REG_AVG_READOUT		0x56a1
 
 enum ov5640_mode_id {
-	OV5640_MODE_QCIF_176_144 = 0,
+	OV5640_MODE_QQVGA_160_120 = 0,
+	OV5640_MODE_QCIF_176_144,
 	OV5640_MODE_QVGA_320_240,
 	OV5640_MODE_VGA_640_480,
 	OV5640_MODE_NTSC_720_480,
@@ -111,7 +112,8 @@ enum ov5640_mode_id {
 };
 
 enum ov5640_frame_rate {
-	OV5640_15_FPS = 0,
+	OV5640_08_FPS = 0,
+	OV5640_15_FPS,
 	OV5640_30_FPS,
 	OV5640_60_FPS,
 	OV5640_NUM_FRAMERATES,
@@ -153,6 +155,7 @@ MODULE_PARM_DESC(virtual_channel,
 		 "MIPI CSI-2 virtual channel (0..3), default 0");
 
 static const int ov5640_framerates[] = {
+	[OV5640_08_FPS] = 8,
 	[OV5640_15_FPS] = 15,
 	[OV5640_30_FPS] = 30,
 	[OV5640_60_FPS] = 60,
@@ -356,10 +359,12 @@ static const struct reg_value ov5640_init_setting_30fps_VGA[] = {
 	{0x583b, 0x28, 0, 0}, {0x583c, 0x42, 0, 0}, {0x583d, 0xce, 0, 0},
 	{0x5025, 0x00, 0, 0}, {0x3a0f, 0x30, 0, 0}, {0x3a10, 0x28, 0, 0},
 	{0x3a1b, 0x30, 0, 0}, {0x3a1e, 0x26, 0, 0}, {0x3a11, 0x60, 0, 0},
-	{0x3a1f, 0x14, 0, 0}, {0x3008, 0x02, 0, 0}, {0x3c00, 0x04, 0, 300},
+	{0x3a1f, 0x14, 0, 0}, {0x3008, 0x42, 0, 0}, {0x3c00, 0x04, 0, 300},
+	{0x302c, 0xc2, 0, 0},
 };
 
 static const struct reg_value ov5640_setting_VGA_640_480[] = {
+	{0x3008, 0x42, 0, 0},
 	{0x3c07, 0x08, 0, 0},
 	{0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
 	{0x3814, 0x31, 0, 0},
@@ -376,9 +381,11 @@ static const struct reg_value ov5640_setting_VGA_640_480[] = {
 	{0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0},
 	{0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
 	{0x3824, 0x02, 0, 0}, {0x5001, 0xa3, 0, 0},
+	{0x3008, 0x02, 0, 10},
 };
 
 static const struct reg_value ov5640_setting_XGA_1024_768[] = {
+	{0x3008, 0x42, 0, 0},
 	{0x3c07, 0x08, 0, 0},
 	{0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
 	{0x3814, 0x31, 0, 0},
@@ -393,11 +400,13 @@ static const struct reg_value ov5640_setting_XGA_1024_768[] = {
 	{0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
 	{0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
 	{0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0},
-	{0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
+	{0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x20, 0, 0},
 	{0x3824, 0x02, 0, 0}, {0x5001, 0xa3, 0, 0},
+	{0x3008, 0x02, 0, 10}
 };
 
 static const struct reg_value ov5640_setting_QVGA_320_240[] = {
+	{0x3008, 0x42, 0, 0},
 	{0x3c07, 0x08, 0, 0},
 	{0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
 	{0x3814, 0x31, 0, 0},
@@ -414,9 +423,11 @@ static const struct reg_value ov5640_setting_QVGA_320_240[] = {
 	{0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0},
 	{0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
 	{0x3824, 0x02, 0, 0}, {0x5001, 0xa3, 0, 0},
+	{0x3008, 0x02, 0, 10},
 };
 
 static const struct reg_value ov5640_setting_QCIF_176_144[] = {
+	{0x3008, 0x42, 0, 0},
 	{0x3c07, 0x08, 0, 0},
 	{0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
 	{0x3814, 0x31, 0, 0},
@@ -433,9 +444,11 @@ static const struct reg_value ov5640_setting_QCIF_176_144[] = {
 	{0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0},
 	{0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
 	{0x3824, 0x02, 0, 0}, {0x5001, 0xa3, 0, 0},
+	{0x3008, 0x02, 0, 10},
 };
 
 static const struct reg_value ov5640_setting_NTSC_720_480[] = {
+	{0x3008, 0x42, 0, 0},
 	{0x3c07, 0x08, 0, 0},
 	{0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
 	{0x3814, 0x31, 0, 0},
@@ -452,9 +465,11 @@ static const struct reg_value ov5640_setting_NTSC_720_480[] = {
 	{0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0},
 	{0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
 	{0x3824, 0x02, 0, 0}, {0x5001, 0xa3, 0, 0},
+	{0x3008, 0x02, 0, 10},
 };
 
 static const struct reg_value ov5640_setting_PAL_720_576[] = {
+	{0x3008, 0x42, 0, 0},
 	{0x3c07, 0x08, 0, 0},
 	{0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
 	{0x3814, 0x31, 0, 0},
@@ -471,9 +486,11 @@ static const struct reg_value ov5640_setting_PAL_720_576[] = {
 	{0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0},
 	{0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
 	{0x3824, 0x02, 0, 0}, {0x5001, 0xa3, 0, 0},
+	{0x3008, 0x02, 0, 10},
 };
 
 static const struct reg_value ov5640_setting_720P_1280_720[] = {
+	{0x3008, 0x42, 0, 0},
 	{0x3c07, 0x07, 0, 0},
 	{0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
 	{0x3814, 0x31, 0, 0},
@@ -490,6 +507,7 @@ static const struct reg_value ov5640_setting_720P_1280_720[] = {
 	{0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0},
 	{0x4407, 0x04, 0, 0}, {0x460b, 0x37, 0, 0}, {0x460c, 0x20, 0, 0},
 	{0x3824, 0x04, 0, 0}, {0x5001, 0x83, 0, 0},
+	{0x3008, 0x02, 0, 15},
 };
 
 static const struct reg_value ov5640_setting_1080P_1920_1080[] = {
@@ -520,10 +538,11 @@ static const struct reg_value ov5640_setting_1080P_1920_1080[] = {
 	{0x3a0e, 0x03, 0, 0}, {0x3a0d, 0x04, 0, 0}, {0x3a14, 0x04, 0, 0},
 	{0x3a15, 0x60, 0, 0}, {0x4407, 0x04, 0, 0},
 	{0x460b, 0x37, 0, 0}, {0x460c, 0x20, 0, 0}, {0x3824, 0x04, 0, 0},
-	{0x4005, 0x1a, 0, 0},
+	{0x4005, 0x1a, 0, 0}, {0x3008, 0x02, 0, 10},
 };
 
 static const struct reg_value ov5640_setting_QSXGA_2592_1944[] = {
+	{0x3008, 0x42, 0, 0},
 	{0x3c07, 0x08, 0, 0},
 	{0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
 	{0x3814, 0x11, 0, 0},
@@ -538,8 +557,9 @@ static const struct reg_value ov5640_setting_QSXGA_2592_1944[] = {
 	{0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
 	{0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
 	{0x4001, 0x02, 0, 0}, {0x4004, 0x06, 0, 0},
-	{0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
+	{0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x20, 0, 0},
 	{0x3824, 0x02, 0, 0}, {0x5001, 0x83, 0, 70},
+	{0x3008, 0x02, 0, 20},
 };
 
 /* power-on sensor init reg table */
@@ -916,6 +936,52 @@ static unsigned long ov5640_calc_sys_clk(struct ov5640_dev *sensor,
 	return best;
 }
 
+static int ov5640_check_valid_mode(struct ov5640_dev *sensor,
+				   const struct ov5640_mode_info *mode,
+				   enum ov5640_frame_rate rate)
+{
+	struct i2c_client *client = sensor->i2c_client;
+	int ret = 0;
+
+	switch (mode->id) {
+	case OV5640_MODE_QQVGA_160_120:
+	case OV5640_MODE_QCIF_176_144:
+	case OV5640_MODE_QVGA_320_240:
+	case OV5640_MODE_NTSC_720_480:
+	case OV5640_MODE_PAL_720_576 :
+	case OV5640_MODE_XGA_1024_768:
+	case OV5640_MODE_720P_1280_720:
+		if ((rate != OV5640_15_FPS) &&
+		    (rate != OV5640_30_FPS))
+			ret = -EINVAL;
+		break;
+	case OV5640_MODE_VGA_640_480:
+		if ((rate != OV5640_15_FPS) &&
+		    (rate != OV5640_30_FPS))
+			ret = -EINVAL;
+		break;
+	case OV5640_MODE_1080P_1920_1080:
+		if (sensor->ep.bus_type == V4L2_MBUS_CSI2_DPHY) {
+			if ((rate != OV5640_15_FPS) &&
+			    (rate != OV5640_30_FPS))
+				ret = -EINVAL;
+		 } else {
+			if ((rate != OV5640_15_FPS))
+				ret = -EINVAL;
+		 }
+		break;
+	case OV5640_MODE_QSXGA_2592_1944:
+		if (rate != OV5640_08_FPS)
+			ret = -EINVAL;
+		break;
+	default:
+		dev_err(&client->dev, "Invalid mode (%d)\n", mode->id);
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
 /*
  * ov5640_set_mipi_pclk() - Calculate the clock tree configuration values
  *			    for the MIPI CSI-2 output.
@@ -1025,7 +1091,7 @@ static int ov5640_set_dvp_pclk(struct ov5640_dev *sensor, unsigned long rate)
 			 &bit_div, &pclk_div);
 
 	if (bit_div == 2)
-		bit_div = 8;
+		bit_div = 0xA;
 
 	ret = ov5640_mod_reg(sensor, OV5640_REG_SC_PLL_CTRL0,
 			     0x0f, bit_div);
@@ -1225,8 +1291,18 @@ static int ov5640_set_stream_dvp(struct ov5640_dev *sensor, bool on)
 
 static int ov5640_set_stream_mipi(struct ov5640_dev *sensor, bool on)
 {
+	const struct ov5640_mode_info *mode;
+	u8 line_sync;
 	int ret;
 
+	mode = sensor->current_mode;
+	line_sync = (mode->id == OV5640_MODE_XGA_1024_768 ||
+		     mode->id == OV5640_MODE_QSXGA_2592_1944) ? 0 : 1;
+	ret = ov5640_write_reg(sensor, OV5640_REG_MIPI_CTRL00,
+			       0x24 | (line_sync << 4));
+	if (ret)
+		return ret;
+
 	/*
 	 * Enable/disable the MIPI interface
 	 *
@@ -1249,8 +1325,18 @@ static int ov5640_set_stream_mipi(struct ov5640_dev *sensor, bool on)
 	if (ret)
 		return ret;
 
-	return ov5640_write_reg(sensor, OV5640_REG_FRAME_CTRL01,
+	ret = ov5640_write_reg(sensor, OV5640_REG_FRAME_CTRL01,
 				on ? 0x00 : 0x0f);
+	if (ret)
+		return ret;
+
+	ret = ov5640_write_reg(sensor, OV5640_REG_SYS_CTRL0,
+				on ? 0x02 : 0x42);
+	if (ret)
+		return ret;
+
+	msleep(100);
+	return ret;
 }
 
 static int ov5640_get_sysclk(struct ov5640_dev *sensor)
@@ -1535,12 +1621,9 @@ ov5640_find_mode(struct ov5640_dev *sensor, enum ov5640_frame_rate fr,
 				      width, height);
 
 	if (!mode ||
-	    (!nearest && (mode->hact != width || mode->vact != height)))
-		return NULL;
-
-	/* Check to see if the current mode exceeds the max frame rate */
-	if (ov5640_framerates[fr] > ov5640_framerates[mode->max_fps])
+	    (!nearest && (mode->hact != width || mode->vact != height))) {
 		return NULL;
+	}
 
 	return mode;
 }
@@ -1920,6 +2003,7 @@ static void ov5640_set_power_off(struct ov5640_dev *sensor)
 	ov5640_power(sensor, false);
 	regulator_bulk_disable(OV5640_NUM_SUPPLIES, sensor->supplies);
 	clk_disable_unprepare(sensor->xclk);
+	sensor->streaming = false;
 }
 
 static int ov5640_set_power_mipi(struct ov5640_dev *sensor, bool on)
@@ -1928,7 +2012,7 @@ static int ov5640_set_power_mipi(struct ov5640_dev *sensor, bool on)
 
 	if (!on) {
 		/* Reset MIPI bus settings to their default values. */
-		ov5640_write_reg(sensor, OV5640_REG_IO_MIPI_CTRL00, 0x58);
+		ov5640_write_reg(sensor, OV5640_REG_IO_MIPI_CTRL00, 0x40);
 		ov5640_write_reg(sensor, OV5640_REG_MIPI_CTRL00, 0x04);
 		ov5640_write_reg(sensor, OV5640_REG_PAD_OUTPUT00, 0x00);
 		return 0;
@@ -1944,7 +2028,7 @@ static int ov5640_set_power_mipi(struct ov5640_dev *sensor, bool on)
 	 * [3] = 0	: Power up MIPI LS Rx
 	 * [2] = 0	: MIPI interface disabled
 	 */
-	ret = ov5640_write_reg(sensor, OV5640_REG_IO_MIPI_CTRL00, 0x40);
+	ret = ov5640_write_reg(sensor, OV5640_REG_IO_MIPI_CTRL00, 0x45);
 	if (ret)
 		return ret;
 
@@ -1953,9 +2037,10 @@ static int ov5640_set_power_mipi(struct ov5640_dev *sensor, bool on)
 	 *
 	 * 0x4800 = 0x24
 	 * [5] = 1	: Gate clock when 'no packets'
+	 * [4] = 1	: Line sync enable
 	 * [2] = 1	: MIPI bus in LP11 when 'no packets'
 	 */
-	ret = ov5640_write_reg(sensor, OV5640_REG_MIPI_CTRL00, 0x24);
+	ret = ov5640_write_reg(sensor, OV5640_REG_MIPI_CTRL00, 0x34);
 	if (ret)
 		return ret;
 
@@ -2070,7 +2155,7 @@ static int ov5640_set_power_dvp(struct ov5640_dev *sensor, bool on)
 	 * [3] = 1	: Power down MIPI LS Rx
 	 * [2] = 0	: DVP enable (MIPI disable)
 	 */
-	ret = ov5640_write_reg(sensor, OV5640_REG_IO_MIPI_CTRL00, 0x18);
+	ret = ov5640_write_reg(sensor, OV5640_REG_IO_MIPI_CTRL00, 0x58);
 	if (ret)
 		return ret;
 
@@ -2167,11 +2252,11 @@ static int ov5640_try_frame_interval(struct ov5640_dev *sensor,
 				     u32 width, u32 height)
 {
 	const struct ov5640_mode_info *mode;
-	enum ov5640_frame_rate rate = OV5640_15_FPS;
+	enum ov5640_frame_rate rate = OV5640_08_FPS;
 	int minfps, maxfps, best_fps, fps;
 	int i;
 
-	minfps = ov5640_framerates[OV5640_15_FPS];
+	minfps = ov5640_framerates[OV5640_08_FPS];
 	maxfps = ov5640_framerates[OV5640_60_FPS];
 
 	if (fi->numerator == 0) {
@@ -2220,10 +2305,10 @@ static int ov5640_get_fmt(struct v4l2_subdev *sd,
 	else
 		fmt = &sensor->fmt;
 
+	fmt->reserved[1] = (sensor->current_fr == OV5640_30_FPS) ? 30 : 15;
 	format->format = *fmt;
 
 	mutex_unlock(&sensor->lock);
-
 	return 0;
 }
 
@@ -2241,6 +2326,7 @@ static int ov5640_try_fmt_internal(struct v4l2_subdev *sd,
 		return -EINVAL;
 	fmt->width = mode->hact;
 	fmt->height = mode->vact;
+	memset(fmt->reserved, 0, sizeof(fmt->reserved));
 
 	if (new_mode)
 		*new_mode = mode;
@@ -2301,6 +2387,9 @@ static int ov5640_set_fmt(struct v4l2_subdev *sd,
 
 	__v4l2_ctrl_s_ctrl_int64(sensor->ctrls.pixel_rate,
 				 ov5640_calc_pixel_rate(sensor));
+
+	if (sensor->pending_mode_change || sensor->pending_fmt_change)
+		sensor->fmt = *mbus_fmt;
 out:
 	mutex_unlock(&sensor->lock);
 	return ret;
@@ -2818,24 +2907,36 @@ static int ov5640_enum_frame_interval(
 	struct v4l2_subdev_frame_interval_enum *fie)
 {
 	struct ov5640_dev *sensor = to_ov5640_dev(sd);
-	struct v4l2_fract tpf;
-	int ret;
+	int i, j, count;
 
 	if (fie->pad != 0)
 		return -EINVAL;
 	if (fie->index >= OV5640_NUM_FRAMERATES)
 		return -EINVAL;
 
-	tpf.numerator = 1;
-	tpf.denominator = ov5640_framerates[fie->index];
-
-	ret = ov5640_try_frame_interval(sensor, &tpf,
-					fie->width, fie->height);
-	if (ret < 0)
+	if (fie->width == 0 || fie->height == 0 || fie->code == 0) {
+		pr_warn("Please assign pixel format, width and height.\n");
 		return -EINVAL;
+	}
 
-	fie->interval = tpf;
-	return 0;
+	fie->interval.numerator = 1;
+
+	count = 0;
+	for (i = 0; i < OV5640_NUM_FRAMERATES; i++) {
+		for (j = 0; j < OV5640_NUM_MODES; j++) {
+			if (fie->width  == ov5640_mode_data[j].hact &&
+			    fie->height == ov5640_mode_data[j].vact &&
+			    !ov5640_check_valid_mode(sensor, &ov5640_mode_data[j], i))
+				count++;
+
+			if (fie->index == (count - 1)) {
+				fie->interval.denominator = ov5640_framerates[i];
+				return 0;
+			}
+		}
+	}
+
+	return -EINVAL;
 }
 
 static int ov5640_g_frame_interval(struct v4l2_subdev *sd,
@@ -2915,11 +3016,23 @@ static int ov5640_enum_mbus_code(struct v4l2_subdev *sd,
 static int ov5640_s_stream(struct v4l2_subdev *sd, int enable)
 {
 	struct ov5640_dev *sensor = to_ov5640_dev(sd);
+	struct i2c_client *client = sensor->i2c_client;
 	int ret = 0;
 
 	mutex_lock(&sensor->lock);
 
 	if (sensor->streaming == !enable) {
+		ret = ov5640_check_valid_mode(sensor,
+					      sensor->current_mode,
+					      sensor->current_fr);
+		if (ret) {
+			dev_err(&client->dev, "Not support WxH@fps=%dx%d@%d\n",
+				sensor->current_mode->hact,
+				sensor->current_mode->vact,
+				ov5640_framerates[sensor->current_fr]);
+			goto out;
+		}
+
 		if (enable && sensor->pending_mode_change) {
 			ret = ov5640_set_mode(sensor);
 			if (ret)
@@ -2973,6 +3086,17 @@ static const struct v4l2_subdev_ops ov5640_subdev_ops = {
 	.pad = &ov5640_pad_ops,
 };
 
+static int ov5640_link_setup(struct media_entity *entity,
+			   const struct media_pad *local,
+			   const struct media_pad *remote, u32 flags)
+{
+	return 0;
+}
+
+static const struct media_entity_operations ov5640_sd_media_ops = {
+	.link_setup = ov5640_link_setup,
+};
+
 static int ov5640_get_regulators(struct ov5640_dev *sensor)
 {
 	int i;
@@ -3116,9 +3240,9 @@ static int ov5640_probe(struct i2c_client *client)
 
 	v4l2_i2c_subdev_init(&sensor->sd, client, &ov5640_subdev_ops);
 
-	sensor->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
-			    V4L2_SUBDEV_FL_HAS_EVENTS;
+	sensor->sd.flags |= V4L2_SUBDEV_FL_HAS_EVENTS;
 	sensor->pad.flags = MEDIA_PAD_FL_SOURCE;
+	sensor->sd.entity.ops = &ov5640_sd_media_ops;
 	sensor->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
 	ret = media_entity_pads_init(&sensor->sd.entity, 1, &sensor->pad);
 	if (ret)
diff --git a/drivers/media/platform/Kconfig b/drivers/media/platform/Kconfig
index 7e152bbb4..2d24e467c 100644
--- a/drivers/media/platform/Kconfig
+++ b/drivers/media/platform/Kconfig
@@ -147,6 +147,21 @@ config VIDEO_RENESAS_CEU
 	help
 	  This is a v4l2 driver for the Renesas CEU Interface
 
+config VIDEO_MXC_CAPTURE
+	tristate "MXC Video For Linux Video Capture"
+	depends on VIDEO_V4L2
+	help
+	  This is the video4linux2 capture driver based on i.MX video-in module.
+
+config VIDEO_MX8_CAPTURE
+	tristate "MX8 Video For Linux Video Capture"
+	depends on VIDEO_V4L2
+	help
+	  This is the video4linux2 capture driver based on i.MX8 module.
+
+
+source "drivers/media/platform/mxc/capture/Kconfig"
+source "drivers/media/platform/imx8/Kconfig"
 source "drivers/media/platform/exynos4-is/Kconfig"
 source "drivers/media/platform/am437x/Kconfig"
 source "drivers/media/platform/xilinx/Kconfig"
diff --git a/drivers/media/platform/Makefile b/drivers/media/platform/Makefile
index 62b6cdc8c..78b73126e 100644
--- a/drivers/media/platform/Makefile
+++ b/drivers/media/platform/Makefile
@@ -67,6 +67,8 @@ obj-$(CONFIG_VIDEO_ATMEL_ISI)		+= atmel/
 
 obj-$(CONFIG_VIDEO_STM32_DCMI)		+= stm32/
 
+obj-$(CONFIG_VIDEO_MX8_CAPTURE)		+= imx8/
+obj-$(CONFIG_VIDEO_MXC_CAPTURE)		+= mxc/capture/
 obj-$(CONFIG_VIDEO_MEDIATEK_VPU)	+= mtk-vpu/
 
 obj-$(CONFIG_VIDEO_MEDIATEK_VCODEC)	+= mtk-vcodec/
diff --git a/drivers/media/platform/imx8/Kconfig b/drivers/media/platform/imx8/Kconfig
new file mode 100644
index 000000000..9e33a590b
--- /dev/null
+++ b/drivers/media/platform/imx8/Kconfig
@@ -0,0 +1,17 @@
+if VIDEO_MX8_CAPTURE
+menu "IMX8 Camera ISI/MIPI Features support"
+
+config IMX8_MIPI_CSI2_YAV
+	tristate "IMX8 MIPI CSI2 Controller Yet Another Version"
+	default y
+
+config IMX8_JPEG
+	tristate "IMX8 JPEG Encoder/Decoder"
+	depends on VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API
+	select V4L2_MEM2MEM_DEV
+	select VIDEOBUF2_DMA_CONTIG
+	default m
+
+endmenu
+endif #VIDEO_MX8_CAPTURE
+
diff --git a/drivers/media/platform/imx8/Makefile b/drivers/media/platform/imx8/Makefile
new file mode 100644
index 000000000..15259cf9c
--- /dev/null
+++ b/drivers/media/platform/imx8/Makefile
@@ -0,0 +1,3 @@
+obj-$(CONFIG_IMX8_MIPI_CSI2_YAV) += mxc-mipi-csi2_yav.o
+mxc-jpeg-encdec-objs := mxc-jpeg-hw.o mxc-jpeg.o
+obj-$(CONFIG_IMX8_JPEG) += mxc-jpeg-encdec.o
diff --git a/drivers/media/platform/imx8/mxc-jpeg-hw.c b/drivers/media/platform/imx8/mxc-jpeg-hw.c
new file mode 100644
index 000000000..6ff1bf792
--- /dev/null
+++ b/drivers/media/platform/imx8/mxc-jpeg-hw.c
@@ -0,0 +1,248 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * i.MX8QXP/i.MX8QM JPEG encoder/decoder v4l2 driver
+ *
+ * Copyright 2018-2019 NXP
+ */
+
+#include <linux/delay.h>
+#include <media/videobuf2-core.h>
+#include "mxc-jpeg-hw.h"
+
+#define print_wrapper_reg(dev, base_address, reg_offset)\
+		internal_print_wrapper_reg(dev, (base_address), #reg_offset,\
+					   (reg_offset))
+#define internal_print_wrapper_reg(dev, base_address, reg_name, reg_offset) {\
+		int val;\
+		val = readl((base_address) + (reg_offset));\
+		dev_dbg(dev, "Wrapper reg %s = 0x%x\n", reg_name, val);\
+}
+
+void print_descriptor_info(struct device *dev, struct mxc_jpeg_desc *desc)
+{
+	dev_dbg(dev, " MXC JPEG NEXT_DESCPT_PTR 0x%x\n",
+		desc->next_descpt_ptr);
+	dev_dbg(dev, " MXC JPEG BUF_BASE0 0x%x\n", desc->buf_base0);
+	dev_dbg(dev, " MXC JPEG BUF_BASE1 0x%x\n", desc->buf_base1);
+	dev_dbg(dev, " MXC JPEG LINE_PITCH %d\n", desc->line_pitch);
+	dev_dbg(dev, " MXC JPEG STM_BUFBASE 0x%x\n", desc->stm_bufbase);
+	dev_dbg(dev, " MXC JPEG STM_BUFSIZE %d\n", desc->stm_bufsize);
+	dev_dbg(dev, " MXC JPEG IMGSIZE %x (%d x %d)\n", desc->imgsize,
+		desc->imgsize >> 16, desc->imgsize & 0xFFFF);
+	dev_dbg(dev, " MXC JPEG STM_CTRL 0x%x\n", desc->stm_ctrl);
+}
+
+void print_cast_status(struct device *dev, void __iomem *reg,
+		       unsigned int mode)
+{
+	dev_dbg(dev, "CAST IP status regs:\n");
+	print_wrapper_reg(dev, reg, CAST_STATUS0);
+	print_wrapper_reg(dev, reg, CAST_STATUS1);
+	print_wrapper_reg(dev, reg, CAST_STATUS2);
+	print_wrapper_reg(dev, reg, CAST_STATUS3);
+	print_wrapper_reg(dev, reg, CAST_STATUS4);
+	print_wrapper_reg(dev, reg, CAST_STATUS5);
+	print_wrapper_reg(dev, reg, CAST_STATUS6);
+	print_wrapper_reg(dev, reg, CAST_STATUS7);
+	print_wrapper_reg(dev, reg, CAST_STATUS8);
+	print_wrapper_reg(dev, reg, CAST_STATUS9);
+	print_wrapper_reg(dev, reg, CAST_STATUS10);
+	print_wrapper_reg(dev, reg, CAST_STATUS11);
+	print_wrapper_reg(dev, reg, CAST_STATUS12);
+	print_wrapper_reg(dev, reg, CAST_STATUS13);
+	if (mode == MXC_JPEG_DECODE)
+		return;
+	print_wrapper_reg(dev, reg, CAST_STATUS14);
+	print_wrapper_reg(dev, reg, CAST_STATUS15);
+	print_wrapper_reg(dev, reg, CAST_STATUS16);
+	print_wrapper_reg(dev, reg, CAST_STATUS17);
+	print_wrapper_reg(dev, reg, CAST_STATUS18);
+	print_wrapper_reg(dev, reg, CAST_STATUS19);
+}
+
+void print_wrapper_info(struct device *dev, void __iomem *reg)
+{
+	dev_dbg(dev, "Wrapper regs:\n");
+	print_wrapper_reg(dev, reg, GLB_CTRL);
+	print_wrapper_reg(dev, reg, COM_STATUS);
+	print_wrapper_reg(dev, reg, BUF_BASE0);
+	print_wrapper_reg(dev, reg, BUF_BASE1);
+	print_wrapper_reg(dev, reg, LINE_PITCH);
+	print_wrapper_reg(dev, reg, STM_BUFBASE);
+	print_wrapper_reg(dev, reg, STM_BUFSIZE);
+	print_wrapper_reg(dev, reg, IMGSIZE);
+	print_wrapper_reg(dev, reg, STM_CTRL);
+}
+
+void mxc_jpeg_enable_irq(void __iomem *reg, int slot)
+{
+	writel(0xFFFFFFFF, reg + MXC_SLOT_OFFSET(slot, SLOT_IRQ_EN));
+}
+
+void mxc_jpeg_sw_reset(void __iomem *reg)
+{
+	/*
+	 * engine soft reset, internal state machine reset
+	 * this will not reset registers, however, it seems
+	 * the registers may remain inconsistent with the internal state
+	 * so, on purpose, at least let GLB_CTRL bits clear after this reset
+	 */
+	writel(GLB_CTRL_SFT_RST, reg + GLB_CTRL);
+}
+
+u32 mxc_jpeg_get_offset(void __iomem *reg, int slot)
+{
+	return readl(reg + MXC_SLOT_OFFSET(slot, SLOT_BUF_PTR));
+}
+
+void mxc_jpeg_enc_mode_conf(struct device *dev, void __iomem *reg)
+{
+	dev_dbg(dev, "CAST Encoder CONFIG...\n");
+	/*
+	 * "Config_Mode" enabled, "Config_Mode auto clear enabled",
+	 */
+	writel(0xa0, reg + CAST_MODE);
+
+	/* all markers and segments */
+	writel(0x3ff, reg + CAST_CFG_MODE);
+
+	/* quality factor */
+	writel(0x4b, reg + CAST_QUALITY);
+}
+
+void mxc_jpeg_enc_mode_go(struct device *dev, void __iomem *reg)
+{
+	dev_dbg(dev, "CAST Encoder GO...\n");
+	/*
+	 * "GO" enabled, "GO bit auto clear" enabled
+	 */
+	writel(0x140, reg + CAST_MODE);
+}
+
+void wait_frmdone(struct device *dev, void __iomem *reg)
+{
+	u32 regval = 0;
+
+	do {
+		regval = readl(reg + MXC_SLOT_OFFSET(0, SLOT_STATUS));
+	} while (!(regval & SLOTa_STATUS_FRMDONE));
+
+	writel(regval, reg + MXC_SLOT_OFFSET(0, SLOT_STATUS)); /* w1c */
+
+	dev_dbg(dev, "Received FRMDONE\n");
+	if (regval & SLOTa_STATUS_ENC_CONFIG_ERR)
+		dev_info(dev, "SLOTa_STATUS_ENC_CONFIG_ERR\n");
+}
+
+int mxc_jpeg_enable(void __iomem *reg)
+{
+	u32 regval;
+
+	writel(GLB_CTRL_JPG_EN, reg + GLB_CTRL);
+	regval = readl(reg);
+	return regval;
+}
+
+void mxc_jpeg_dec_mode_go(struct device *dev, void __iomem *reg)
+{
+	dev_dbg(dev, "CAST Decoder GO...\n");
+	writel(MXC_DEC_EXIT_IDLE_MODE, reg + CAST_CTRL);
+}
+
+int mxc_jpeg_get_slot(void __iomem *reg)
+{
+	int slot_val;
+	int i = 0;
+	int tmp = GLB_CTRL_SLOT_EN(0);
+
+	/* currently enabled slots */
+	slot_val = readl(reg + GLB_CTRL) & 0xF0;
+
+	for (; tmp != tmp << 4; tmp = tmp << 1) {
+		if ((slot_val & tmp) == 0)
+			/* first free slot */
+			return i;
+		++i;
+	}
+	return -EINVAL;
+}
+
+void mxc_jpeg_enable_slot(void __iomem *reg, int slot)
+{
+	u32 regval;
+
+	regval = readl(reg + GLB_CTRL);
+	writel(GLB_CTRL_SLOT_EN(slot) | regval, reg + GLB_CTRL);
+}
+
+void mxc_jpeg_set_l_endian(void __iomem *reg, int le)
+{
+	u32 regval;
+
+	regval = readl(reg + GLB_CTRL);
+	regval &= ~GLB_CTRL_L_ENDIAN(1); /* clear */
+	writel(GLB_CTRL_L_ENDIAN(le) | regval, reg + GLB_CTRL); /* set */
+}
+
+void mxc_jpeg_set_config_mode(void __iomem *reg, int config_mode)
+{
+	u32 regval;
+
+	regval = readl(reg + STM_CTRL);
+	regval &= ~STM_CTRL_CONFIG_MOD(1);
+	writel(STM_CTRL_CONFIG_MOD(config_mode) | regval, reg + STM_CTRL);
+}
+
+int mxc_jpeg_set_params(struct mxc_jpeg_desc *desc,  u32 bufsize,
+			 u16 out_pitch, u32 format)
+{
+	desc->line_pitch = out_pitch;
+	desc->stm_bufsize = bufsize;
+	switch (format) {
+	case V4L2_PIX_FMT_YUV32:
+		desc->stm_ctrl |= MXC_JPEG_YUV444 << 3;
+		break;
+	case V4L2_PIX_FMT_YUYV:
+		desc->stm_ctrl |= MXC_JPEG_YUV422 << 3;
+		break;
+	case V4L2_PIX_FMT_RGB32:
+		desc->stm_ctrl |= MXC_JPEG_RGB << 3;
+		break;
+	default:
+		return -1;
+	}
+	return 0;
+}
+
+void mxc_jpeg_set_bufsize(struct mxc_jpeg_desc *desc,  u32 bufsize)
+{
+	desc->stm_bufsize = bufsize;
+}
+
+void mxc_jpeg_set_res(struct mxc_jpeg_desc *desc, u16 w, u16 h)
+{
+	desc->imgsize = w << 16 | h;
+}
+
+
+void mxc_jpeg_set_line_pitch(struct mxc_jpeg_desc *desc, u32 line_pitch)
+{
+	desc->line_pitch = line_pitch;
+}
+
+void mxc_jpeg_set_desc(u32 desc, void __iomem *reg, int slot)
+{
+	writel(desc | MXC_NXT_DESCPT_EN,
+	       reg + MXC_SLOT_OFFSET(slot, SLOT_NXT_DESCPT_PTR));
+}
+
+void mxc_jpeg_set_regs_from_desc(struct mxc_jpeg_desc *desc, void __iomem *reg)
+{
+	writel(desc->buf_base0, reg + BUF_BASE0);
+	writel(desc->buf_base1, reg + BUF_BASE1);
+	writel(desc->line_pitch, reg + LINE_PITCH);
+	writel(desc->stm_bufbase, reg + STM_BUFBASE);
+	writel(desc->stm_bufsize, reg + STM_BUFSIZE);
+	writel(desc->imgsize, reg + IMGSIZE);
+	writel(desc->stm_ctrl, reg + STM_CTRL);
+}
diff --git a/drivers/media/platform/imx8/mxc-jpeg-hw.h b/drivers/media/platform/imx8/mxc-jpeg-hw.h
new file mode 100644
index 000000000..6aafd8f10
--- /dev/null
+++ b/drivers/media/platform/imx8/mxc-jpeg-hw.h
@@ -0,0 +1,144 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * i.MX8QXP/i.MX8QM JPEG encoder/decoder v4l2 driver
+ *
+ * Copyright 2018-2019 NXP
+ */
+
+#ifndef _MXC_JPEG_HW_H
+#define _MXC_JPEG_HW_H
+
+#define MXC_JPEG_DECODE			0
+#define MXC_JPEG_ENCODE			1
+
+/* JPEG Decoder/Encoder Wrapper Register Map */
+#define GLB_CTRL			0x0
+#define COM_STATUS			0x4
+#define BUF_BASE0			0x14
+#define BUF_BASE1			0x18
+#define LINE_PITCH			0x1C
+#define STM_BUFBASE			0x20
+#define STM_BUFSIZE			0x24
+#define IMGSIZE				0x28
+#define STM_CTRL			0x2C
+
+/* CAST JPEG-Decoder/Encoder Status Register Map (read-only)*/
+#define CAST_STATUS0			0x100
+#define CAST_STATUS1			0x104
+#define CAST_STATUS2			0x108
+#define CAST_STATUS3			0x10c
+#define CAST_STATUS4			0x110
+#define CAST_STATUS5			0x114
+#define CAST_STATUS6			0x118
+#define CAST_STATUS7			0x11c
+#define CAST_STATUS8			0x120
+#define CAST_STATUS9			0x124
+#define CAST_STATUS10			0x128
+#define CAST_STATUS11			0x12c
+#define CAST_STATUS12			0x130
+#define CAST_STATUS13			0x134
+/* the following are for encoder only */
+#define CAST_STATUS14		0x138
+#define CAST_STATUS15		0x13c
+#define CAST_STATUS16		0x140
+#define CAST_STATUS17		0x144
+#define CAST_STATUS18		0x148
+#define CAST_STATUS19		0x14c
+
+/* CAST JPEG-Decoder Control Register Map (write-only) */
+#define CAST_CTRL			CAST_STATUS13
+
+/* CAST JPEG-Encoder Control Register Map (write-only) */
+#define CAST_MODE			CAST_STATUS0
+#define CAST_CFG_MODE			CAST_STATUS1
+#define CAST_QUALITY			CAST_STATUS2
+#define CAST_RSVD			CAST_STATUS3
+#define CAST_REC_REGS_SEL		CAST_STATUS4
+#define CAST_LUMTH			CAST_STATUS5
+#define CAST_CHRTH			CAST_STATUS6
+#define CAST_NOMFRSIZE_LO		CAST_STATUS7
+#define CAST_NOMFRSIZE_HI		CAST_STATUS8
+#define CAST_OFBSIZE_LO			CAST_STATUS9
+#define CAST_OFBSIZE_HI			CAST_STATUS10
+
+#define MXC_MAX_SLOTS	1 /* TODO use all 4 slots*/
+/* JPEG-Decoder Wrapper Slot Registers 0..3 */
+#define SLOT_BASE			0x10000
+#define SLOT_STATUS			0x0
+#define SLOT_IRQ_EN			0x4
+#define SLOT_BUF_PTR			0x8
+#define SLOT_CUR_DESCPT_PTR		0xC
+#define SLOT_NXT_DESCPT_PTR		0x10
+#define MXC_SLOT_OFFSET(slot, offset)	((SLOT_BASE * (slot + 1)) + offset)
+
+/* GLB_CTRL fields */
+#define GLB_CTRL_JPG_EN					0x1
+#define GLB_CTRL_SFT_RST				(0x1 << 1)
+#define GLB_CTRL_DEC_GO					(0x1 << 2)
+#define GLB_CTRL_L_ENDIAN(le)				((le) << 3)
+#define GLB_CTRL_SLOT_EN(slot)				(0x1 << (slot + 4))
+
+/* COM_STAUS fields */
+#define COM_STATUS_DEC_ONGOING(r)		(((r) & (1 << 31)) >> 31)
+#define COM_STATUS_CUR_SLOT(r)			(((r) & (0x3 << 29)) >> 29)
+
+/* STM_CTRL fields */
+#define STM_CTRL_PIXEL_PRECISION		(0x1 << 2)
+#define STM_CTRL_IMAGE_FORMAT(img_fmt)		((img_fmt) << 3)
+#define STM_CTRL_IMAGE_FORMAT_MASK		(0xF << 3)
+#define STM_CTRL_BITBUF_PTR_CLR(clr)		((clr) << 7)
+#define STM_CTRL_AUTO_START(go)			((go) << 8)
+#define STM_CTRL_CONFIG_MOD(mod)		((mod) << 9)
+
+/* SLOTa_STATUS fields TBD */
+#define SLOTa_STATUS_FRMDONE			(0x1 << 3)
+#define SLOTa_STATUS_ENC_CONFIG_ERR		(0x1 << 8)
+
+/* SLOTa_IRQ_EN fields TBD */
+
+#define MXC_NXT_DESCPT_EN			0x1
+#define MXC_DEC_EXIT_IDLE_MODE			0x4
+
+/* JPEG-Decoder Wrapper - STM_CTRL Register Fields */
+#define MXC_PIXEL_PRECISION(precision) ((precision)/8 << 2)
+enum mxc_jpeg_image_format {
+	MXC_JPEG_INVALID = -1,
+	MXC_JPEG_YUV420 = 0x0, /* 2 Plannar, Y=1st plane UV=2nd plane */
+	MXC_JPEG_YUV422 = 0x1, /* 1 Plannar, YUYV sequence */
+	MXC_JPEG_RGB	= 0x2, /* RGBRGB packed format */
+	MXC_JPEG_YUV444	= 0x3, /* 1 Plannar, YUVYUV sequence */
+	MXC_JPEG_GRAY = 0x4, /* Y8 or Y12 or Single Component */
+	MXC_JPEG_RESERVED = 0x5,
+	MXC_JPEG_ARGB	= 0x6,
+};
+
+
+#include "mxc-jpeg.h"
+void print_descriptor_info(struct device *dev, struct mxc_jpeg_desc *desc);
+void print_cast_status(struct device *dev, void __iomem *reg,
+		       unsigned int mode);
+void print_wrapper_info(struct device *dev, void __iomem *reg);
+void mxc_jpeg_sw_reset(void __iomem *reg);
+int mxc_jpeg_enable(void __iomem *reg);
+void wait_frmdone(struct device *dev, void __iomem *reg);
+void mxc_jpeg_enc_mode_conf(struct device *dev, void __iomem *reg);
+void mxc_jpeg_enc_mode_go(struct device *dev, void __iomem *reg);
+void mxc_jpeg_dec_mode_go(struct device *dev, void __iomem *reg);
+int mxc_jpeg_get_slot(void __iomem *reg);
+u32 mxc_jpeg_get_offset(void __iomem *reg, int slot);
+void mxc_jpeg_enable_slot(void __iomem *reg, int slot);
+void mxc_jpeg_set_l_endian(void __iomem *reg, int le);
+void mxc_jpeg_enable_irq(void __iomem *reg, int slot);
+int mxc_jpeg_set_input(void __iomem *reg, u32 in_buf, u32 bufsize);
+int mxc_jpeg_set_output(void __iomem *reg, u16 out_pitch, u32 out_buf,
+			u16 w, u16 h);
+void mxc_jpeg_set_config_mode(void __iomem *reg, int config_mode);
+int mxc_jpeg_set_params(struct mxc_jpeg_desc *desc,  u32 bufsize, u16
+			out_pitch, u32 format);
+void mxc_jpeg_set_bufsize(struct mxc_jpeg_desc *desc,  u32 bufsize);
+void mxc_jpeg_set_res(struct mxc_jpeg_desc *desc, u16 w, u16 h);
+void mxc_jpeg_set_line_pitch(struct mxc_jpeg_desc *desc, u32 line_pitch);
+void mxc_jpeg_set_desc(u32 desc, void __iomem *reg, int slot);
+void mxc_jpeg_set_regs_from_desc(struct mxc_jpeg_desc *desc,
+				 void __iomem *reg);
+#endif
diff --git a/drivers/media/platform/imx8/mxc-jpeg.c b/drivers/media/platform/imx8/mxc-jpeg.c
new file mode 100644
index 000000000..a55a30396
--- /dev/null
+++ b/drivers/media/platform/imx8/mxc-jpeg.c
@@ -0,0 +1,2311 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * i.MX8QXP/i.MX8QM JPEG encoder/decoder v4l2 driver
+ *
+ * Copyright 2018-2019 NXP
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/io.h>
+#include <linux/clk.h>
+#include <linux/of_platform.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/irqreturn.h>
+#include <linux/interrupt.h>
+#include <linux/pm_runtime.h>
+#include <linux/pm_domain.h>
+#include <linux/string.h>
+#include <linux/timekeeping.h>
+
+#include <media/v4l2-mem2mem.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-event.h>
+#include <media/videobuf2-dma-contig.h>
+
+#include "mxc-jpeg-hw.h"
+#include "mxc-jpeg.h"
+
+static struct mxc_jpeg_fmt mxc_formats[] = {
+	{
+		.name		= "JPEG",
+		.fourcc		= V4L2_PIX_FMT_JPEG,
+		.colplanes	= 1,
+		.flags		= MXC_JPEG_FMT_TYPE_ENC,
+	},
+	{
+		.name		= "RGB", /*RGBRGB packed format*/
+		.fourcc		= V4L2_PIX_FMT_RGB24,
+		.depth		= 24,
+		.colplanes	= 1,
+		.h_align	= 3,
+		.v_align	= 3,
+		.flags		= MXC_JPEG_FMT_TYPE_RAW,
+	},
+	{
+		.name		= "ARGB", /* ARGBARGB packed format */
+		.fourcc		= V4L2_PIX_FMT_ARGB32,
+		.depth		= 32,
+		.colplanes	= 1,
+		.h_align	= 3,
+		.v_align	= 3,
+		.flags		= MXC_JPEG_FMT_TYPE_RAW,
+	},
+	{
+		.name		= "YUV420", /* 1st plane = Y, 2nd plane = UV */
+		.fourcc		= V4L2_PIX_FMT_NV12,
+		.depth		= 12, /* 6 bytes (4Y + UV) for 4 pixels */
+		.colplanes	= 2, /* 1 plane Y, 1 plane UV interleaved */
+		.h_align	= 4,
+		.v_align	= 4,
+		.flags		= MXC_JPEG_FMT_TYPE_RAW,
+	},
+	{
+		.name		= "YUV422", /* YUYV */
+		.fourcc		= V4L2_PIX_FMT_YUYV,
+		.depth		= 16,
+		.colplanes	= 1,
+		.h_align	= 4,
+		.v_align	= 3,
+		.flags		= MXC_JPEG_FMT_TYPE_RAW,
+	},
+	{
+		.name		= "YUV444", /* YUVYUV */
+		.fourcc		= V4L2_PIX_FMT_YUV24,
+		.depth		= 24,
+		.colplanes	= 1,
+		.h_align	= 3,
+		.v_align	= 3,
+		.flags		= MXC_JPEG_FMT_TYPE_RAW,
+	},
+	{
+		.name		= "Gray", /* Gray (Y8/Y12) or Single Comp */
+		.fourcc		= V4L2_PIX_FMT_GREY,
+		.depth		= 8,
+		.colplanes	= 1,
+		.h_align	= 3,
+		.v_align	= 3,
+		.flags		= MXC_JPEG_FMT_TYPE_RAW,
+	},
+};
+#define MXC_JPEG_NUM_FORMATS ARRAY_SIZE(mxc_formats)
+
+static const struct of_device_id mxc_jpeg_match[] = {
+	{
+		.compatible = "fsl,imx8-jpgdec",
+		.data       = (void *)MXC_JPEG_DECODE,
+	},
+	{
+		.compatible = "fsl,imx8-jpgenc",
+		.data       = (void *)MXC_JPEG_ENCODE,
+	},
+	{ },
+};
+
+/* default configuration stream 64x64 yuv422 */
+static const unsigned char jpeg_soi[] = {0xFF, 0xD8};
+static const unsigned char jpeg_app0[] = {0xFF, 0xE0, 0x00, 0x10, 0x4A,
+0x46, 0x49, 0x46, 0x00, 0x01, 0x01, 0x00,
+0x00, 0x01, 0x00, 0x01, 0x00, 0x00};
+static const unsigned char jpeg_app14[] = {
+0xFF, 0xEE, 0x00, 0x0E, 0x41, 0x64, 0x6F, 0x62,
+0x65, 0x00, 0x64, 0x00, 0x00, 0x00, 0x00, 0x00};
+static const unsigned char jpeg_dqt[] = {0xFF, 0xDB,
+0x00, 0x84, 0x00, 0x10, 0x0B, 0x0C,
+0x0E, 0x0C, 0x0A, 0x10, 0x0E, 0x0D, 0x0E,
+0x12, 0x11, 0x10, 0x13, 0x18, 0x28, 0x1A,
+0x18, 0x16, 0x16, 0x18, 0x31, 0x23, 0x25,
+0x1D, 0x28, 0x3A, 0x33, 0x3D, 0x3C, 0x39,
+0x33, 0x38, 0x37, 0x40, 0x48, 0x5C, 0x4E,
+0x40, 0x44, 0x57, 0x45, 0x37, 0x38, 0x50,
+0x6D, 0x51, 0x57, 0x5F, 0x62, 0x67, 0x68,
+0x67, 0x3E, 0x4D, 0x71, 0x79, 0x70, 0x64,
+0x78, 0x5C, 0x65, 0x67, 0x63, 0x01, 0x11,
+0x12, 0x12, 0x18, 0x15, 0x18, 0x2F, 0x1A,
+0x1A, 0x2F, 0x63, 0x42, 0x38, 0x42, 0x63,
+0x63, 0x63, 0x63, 0x63, 0x63, 0x63, 0x63,
+0x63, 0x63, 0x63, 0x63, 0x63, 0x63, 0x63,
+0x63, 0x63, 0x63, 0x63, 0x63, 0x63, 0x63,
+0x63, 0x63, 0x63, 0x63, 0x63, 0x63, 0x63,
+0x63, 0x63, 0x63, 0x63, 0x63, 0x63, 0x63,
+0x63, 0x63, 0x63, 0x63, 0x63, 0x63, 0x63,
+0x63, 0x63, 0x63, 0x63, 0x63, 0x63, 0x63};
+static const unsigned char jpeg_sof_maximal[] = {0xFF, 0xC0,
+0x00, 0x14, 0x08, 0x00, 0x40,
+0x00, 0x40, 0x04, 0x01, 0x11, 0x00, 0x02,
+0x11, 0x01, 0x03, 0x11, 0x01, 0x04, 0x11, 0x01};
+static const unsigned char jpeg_dht[] = {0xFF, 0xC4,
+0x01, 0xA2, 0x00, 0x00, 0x01, 0x05, 0x01,
+0x01, 0x01, 0x01, 0x01, 0x01, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01,
+0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08,
+0x09, 0x0A, 0x0B, 0x10, 0x00, 0x02, 0x01,
+0x03, 0x03, 0x02, 0x04, 0x03, 0x05, 0x05,
+0x04, 0x04, 0x00, 0x00, 0x01, 0x7D, 0x01,
+0x02, 0x03, 0x00, 0x04, 0x11, 0x05, 0x12,
+0x21, 0x31, 0x41, 0x06, 0x13, 0x51, 0x61,
+0x07, 0x22, 0x71, 0x14, 0x32, 0x81, 0x91,
+0xA1, 0x08, 0x23, 0x42, 0xB1, 0xC1, 0x15,
+0x52, 0xD1, 0xF0, 0x24, 0x33, 0x62, 0x72,
+0x82, 0x09, 0x0A, 0x16, 0x17, 0x18, 0x19,
+0x1A, 0x25, 0x26, 0x27, 0x28, 0x29, 0x2A,
+0x34, 0x35, 0x36, 0x37, 0x38, 0x39, 0x3A,
+0x43, 0x44, 0x45, 0x46, 0x47, 0x48, 0x49,
+0x4A, 0x53, 0x54, 0x55, 0x56, 0x57, 0x58,
+0x59, 0x5A, 0x63, 0x64, 0x65, 0x66, 0x67,
+0x68, 0x69, 0x6A, 0x73, 0x74, 0x75, 0x76,
+0x77, 0x78, 0x79, 0x7A, 0x83, 0x84, 0x85,
+0x86, 0x87, 0x88, 0x89, 0x8A, 0x92, 0x93,
+0x94, 0x95, 0x96, 0x97, 0x98, 0x99, 0x9A,
+0xA2, 0xA3, 0xA4, 0xA5, 0xA6, 0xA7, 0xA8,
+0xA9, 0xAA, 0xB2, 0xB3, 0xB4, 0xB5, 0xB6,
+0xB7, 0xB8, 0xB9, 0xBA, 0xC2, 0xC3, 0xC4,
+0xC5, 0xC6, 0xC7, 0xC8, 0xC9, 0xCA, 0xD2,
+0xD3, 0xD4, 0xD5, 0xD6, 0xD7, 0xD8, 0xD9,
+0xDA, 0xE1, 0xE2, 0xE3, 0xE4, 0xE5, 0xE6,
+0xE7, 0xE8, 0xE9, 0xEA, 0xF1, 0xF2, 0xF3,
+0xF4, 0xF5, 0xF6, 0xF7, 0xF8, 0xF9, 0xFA,
+0x01, 0x00, 0x03, 0x01, 0x01, 0x01, 0x01,
+0x01, 0x01, 0x01, 0x01, 0x01, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x01, 0x02, 0x03,
+0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0A,
+0x0B, 0x11, 0x00, 0x02, 0x01, 0x02, 0x04,
+0x04, 0x03, 0x04, 0x07, 0x05, 0x04, 0x04,
+0x00, 0x01, 0x02, 0x77, 0x00, 0x01, 0x02,
+0x03, 0x11, 0x04, 0x05, 0x21, 0x31, 0x06,
+0x12, 0x41, 0x51, 0x07, 0x61, 0x71, 0x13,
+0x22, 0x32, 0x81, 0x08, 0x14, 0x42, 0x91,
+0xA1, 0xB1, 0xC1, 0x09, 0x23, 0x33, 0x52,
+0xF0, 0x15, 0x62, 0x72, 0xD1, 0x0A, 0x16,
+0x24, 0x34, 0xE1, 0x25, 0xF1, 0x17, 0x18,
+0x19, 0x1A, 0x26, 0x27, 0x28, 0x29, 0x2A,
+0x35, 0x36, 0x37, 0x38, 0x39, 0x3A, 0x43,
+0x44, 0x45, 0x46, 0x47, 0x48, 0x49, 0x4A,
+0x53, 0x54, 0x55, 0x56, 0x57, 0x58, 0x59,
+0x5A, 0x63, 0x64, 0x65, 0x66, 0x67, 0x68,
+0x69, 0x6A, 0x73, 0x74, 0x75, 0x76, 0x77,
+0x78, 0x79, 0x7A, 0x82, 0x83, 0x84, 0x85,
+0x86, 0x87, 0x88, 0x89, 0x8A, 0x92, 0x93,
+0x94, 0x95, 0x96, 0x97, 0x98, 0x99, 0x9A,
+0xA2, 0xA3, 0xA4, 0xA5, 0xA6, 0xA7, 0xA8,
+0xA9, 0xAA, 0xB2, 0xB3, 0xB4, 0xB5, 0xB6,
+0xB7, 0xB8, 0xB9, 0xBA, 0xC2, 0xC3, 0xC4,
+0xC5, 0xC6, 0xC7, 0xC8, 0xC9, 0xCA, 0xD2,
+0xD3, 0xD4, 0xD5, 0xD6, 0xD7, 0xD8, 0xD9,
+0xDA, 0xE2, 0xE3, 0xE4, 0xE5, 0xE6, 0xE7,
+0xE8, 0xE9, 0xEA, 0xF2, 0xF3, 0xF4, 0xF5,
+0xF6, 0xF7, 0xF8, 0xF9, 0xFA};
+static const unsigned char jpeg_dri[] = {0xFF, 0xDD,
+0x00, 0x04, 0x00, 0x20};
+static const unsigned char jpeg_sos_maximal[] = {0xFF, 0xDA,
+0x00, 0x0C, 0x04, 0x01, 0x00, 0x02, 0x11, 0x03,
+0x11, 0x04, 0x11, 0x00, 0x3F, 0x00,};
+static const unsigned char jpeg_eoi[] = {0xFF, 0xD9};
+
+struct mxc_jpeg_src_buf {
+	/* common v4l buffer stuff -- must be first */
+	struct vb2_v4l2_buffer	b;
+	struct list_head	list;
+
+	/* mxc-jpeg specific */
+	bool			dht_needed;
+	bool			jpeg_parse_error;
+};
+
+static inline struct mxc_jpeg_src_buf *vb2_to_mxc_buf(struct vb2_buffer *vb)
+{
+	return container_of(to_vb2_v4l2_buffer(vb), struct mxc_jpeg_src_buf, b);
+}
+
+/*  Print Four-character-code (FOURCC) */
+static char *fourcc_to_str(u32 format)
+{
+	char *buf = kmalloc(32, GFP_KERNEL);
+
+	snprintf(buf, 32,
+		 "%c%c%c%c",
+		 format & 0xff,
+		 (format >> 8) & 0xff,
+		 (format >> 16) & 0xff,
+		 (format >> 24) & 0x7f);
+	return buf;
+}
+
+static unsigned int mxc_jpeg_tracing;
+module_param_named(jpeg_tracing, mxc_jpeg_tracing, int, 0600);
+MODULE_PARM_DESC(jpeg_tracing, "Enable detailed debug messages");
+
+static void _bswap16(u16 *a)
+{
+	*a = ((*a & 0x00FF) << 8) | ((*a & 0xFF00) >> 8);
+}
+
+static void print_buf_preview(struct device *dev, struct vb2_buffer *buf)
+{
+	unsigned char *data;
+	u32 dma_addr;
+
+	if (!mxc_jpeg_tracing)
+		return;
+
+	dma_addr = vb2_dma_contig_plane_dma_addr(buf, 0);
+	data = (char *)vb2_plane_vaddr(buf, 0);
+
+	/* print just the first 4 bytes from the beginning of the buffer */
+	dev_dbg(dev, "vaddr=%p dma_addr=%x: %x %x %x %x ...\n",
+		data, dma_addr,
+		data[0], data[1], data[2], data[3]);
+}
+
+static void print_nbuf_to_eoi(struct device *dev, struct vb2_buffer *buf,
+			      int n)
+{
+	unsigned char *data;
+	u32 dma_addr;
+	int i;
+	int items_per_line = 22;
+	char *bufstr, *bufptr;
+
+	if (!mxc_jpeg_tracing)
+		return;
+
+	if (n == 0)
+		n = buf->planes[0].bytesused;
+
+	dma_addr = vb2_dma_contig_plane_dma_addr(buf, 0);
+	data = (unsigned char *)vb2_plane_vaddr(buf, 0);
+
+	dev_dbg(dev, "vaddr=%p dma_addr=%x bytesused=%d:",
+		 data, dma_addr, n);
+
+	bufstr = kmalloc(items_per_line * 6, GFP_ATOMIC);
+	bufptr = bufstr;
+	for (i = 0; i < n; i++) {
+		snprintf(bufptr, 6, "0x%02x,", data[i]);
+		bufptr += 5;
+		if ((i+1) % items_per_line == 0) {
+			/* print the current line */
+			dev_dbg(dev, "%s", bufstr);
+			/* back to buffer start */
+			bufptr = bufstr;
+		}
+		/* stop at End of Image (EOI) marker*/
+		if (i > 0 && data[i-1] == 0xFF && data[i] == 0xD9)
+			break;
+	}
+	if (bufptr != bufstr)
+		dev_dbg(dev, "%s", bufstr);
+	dev_dbg(dev, "buffer size = %d", i);
+	kfree(bufstr);
+}
+
+static inline struct mxc_jpeg_ctx *mxc_jpeg_fh_to_ctx(struct v4l2_fh *fh)
+{
+	return container_of(fh, struct mxc_jpeg_ctx, fh);
+}
+
+static int enum_fmt(struct mxc_jpeg_fmt *mxc_formats, int n,
+		    struct v4l2_fmtdesc *f, u32 type)
+{
+	int i, num = 0;
+
+	for (i = 0; i < n; ++i) {
+		if (mxc_formats[i].flags == type) {
+			/* index-th format of type type found ? */
+			if (num == f->index)
+				break;
+			/* Correct type but haven't reached our index yet,
+			 * just increment per-type index
+			 */
+			++num;
+		}
+	}
+
+	/* Format not found */
+	if (i >= n)
+		return -EINVAL;
+
+	strscpy(f->description, mxc_formats[i].name, sizeof(f->description));
+	f->pixelformat = mxc_formats[i].fourcc;
+
+	return 0;
+}
+
+static struct mxc_jpeg_fmt *mxc_jpeg_find_format(struct mxc_jpeg_ctx *ctx,
+						 u32 pixelformat)
+{
+	unsigned int k;
+
+	for (k = 0; k < MXC_JPEG_NUM_FORMATS; k++) {
+		struct mxc_jpeg_fmt *fmt = &mxc_formats[k];
+
+		if (fmt->fourcc == pixelformat)
+			return fmt;
+	}
+	return NULL;
+}
+
+static enum mxc_jpeg_image_format mxc_jpeg_fourcc_to_imgfmt(
+	u32	fourcc)
+{
+	switch (fourcc) {
+	case V4L2_PIX_FMT_GREY:
+		return MXC_JPEG_GRAY;
+	case V4L2_PIX_FMT_YUYV:
+		return MXC_JPEG_YUV422;
+	case V4L2_PIX_FMT_NV12:
+		return MXC_JPEG_YUV420;
+	case V4L2_PIX_FMT_YUV24:
+		return MXC_JPEG_YUV444;
+	case V4L2_PIX_FMT_RGB24:
+		return MXC_JPEG_RGB;
+	case V4L2_PIX_FMT_ARGB32:
+		return MXC_JPEG_ARGB;
+	default:
+		return MXC_JPEG_INVALID;
+	}
+}
+
+static int mxc_jpeg_imgfmt_to_fourcc(enum mxc_jpeg_image_format imgfmt,
+				  u32 *fourcc)
+{
+	switch (imgfmt) {
+	case MXC_JPEG_GRAY:
+		*fourcc = V4L2_PIX_FMT_GREY;
+		return 0;
+	case MXC_JPEG_YUV422:
+		*fourcc = V4L2_PIX_FMT_YUYV;
+		return 0;
+	case MXC_JPEG_YUV420:
+		*fourcc =  V4L2_PIX_FMT_NV12;
+		return 0;
+	case MXC_JPEG_YUV444:
+		*fourcc =  V4L2_PIX_FMT_YUV24;
+		return 0;
+	case MXC_JPEG_RGB:
+		*fourcc =  V4L2_PIX_FMT_RGB24;
+		return 0;
+	case MXC_JPEG_ARGB:
+		*fourcc =  V4L2_PIX_FMT_ARGB32;
+		return 0;
+	default:
+		return 1;
+	}
+}
+
+static struct mxc_jpeg_q_data *mxc_jpeg_get_q_data(struct mxc_jpeg_ctx *ctx,
+						   enum v4l2_buf_type type)
+{
+	if (V4L2_TYPE_IS_OUTPUT(type))
+		return &ctx->out_q;
+	return &ctx->cap_q;
+}
+
+static void mxc_jpeg_addrs(struct mxc_jpeg_desc *desc,
+			   struct vb2_buffer *raw_buf,
+			   struct vb2_buffer *jpeg_buf, int offset)
+{
+	int img_fmt = desc->stm_ctrl & STM_CTRL_IMAGE_FORMAT_MASK;
+
+	desc->buf_base0 = vb2_dma_contig_plane_dma_addr(raw_buf, 0);
+	desc->buf_base1 = 0;
+	if (img_fmt == STM_CTRL_IMAGE_FORMAT(MXC_JPEG_YUV420)) {
+		WARN_ON(raw_buf->num_planes < 2);
+		desc->buf_base1 = vb2_dma_contig_plane_dma_addr(raw_buf, 1);
+	}
+	desc->stm_bufbase = vb2_dma_contig_plane_dma_addr(jpeg_buf, 0) +
+		offset;
+}
+
+static void notify_eos(struct mxc_jpeg_ctx *ctx)
+{
+	const struct v4l2_event ev = {
+		.type = V4L2_EVENT_EOS
+	};
+
+	dev_dbg(ctx->mxc_jpeg->dev, "Notify app event EOS reached");
+	v4l2_event_queue_fh(&ctx->fh, &ev);
+}
+
+static void notify_src_chg(struct mxc_jpeg_ctx *ctx)
+{
+	const struct v4l2_event ev = {
+			.type = V4L2_EVENT_SOURCE_CHANGE,
+			.u.src_change.changes =
+			V4L2_EVENT_SRC_CH_RESOLUTION,
+		};
+
+	dev_dbg(ctx->mxc_jpeg->dev, "Notify app event SRC_CH_RESOLUTION");
+	v4l2_event_queue_fh(&ctx->fh, &ev);
+}
+
+static int mxc_get_free_slot(struct mxc_jpeg_slot_data slot_data[], int n)
+{
+	int free_slot = 0;
+
+	while (slot_data[free_slot].used && free_slot < n)
+		free_slot++;
+
+	return free_slot; /* >=n when there are no more free slots */
+}
+
+static bool mxc_jpeg_alloc_slot_data(struct mxc_jpeg_dev *jpeg,
+				     unsigned int slot)
+{
+	if (jpeg->slot_data[slot].desc)
+		goto skip_alloc; /* already allocated, reuse it */
+	/* allocate descriptor for decoding/encoding phase */
+	jpeg->slot_data[slot].desc = dma_alloc_coherent(jpeg->dev,
+		sizeof(struct mxc_jpeg_desc),
+		&jpeg->slot_data[slot].desc_handle, GFP_ATOMIC);
+	if (!jpeg->slot_data[slot].desc)
+		goto err;
+
+	/* allocate descriptor for configuration phase (encoder only) */
+	jpeg->slot_data[slot].cfg_desc = dma_alloc_coherent(jpeg->dev,
+		sizeof(struct mxc_jpeg_desc),
+		&jpeg->slot_data[slot].cfg_desc_handle, GFP_ATOMIC);
+	if (!jpeg->slot_data[slot].cfg_desc)
+		goto err;
+
+	/* allocate configuration stream */
+	jpeg->slot_data[slot].cfg_stream_vaddr = dma_alloc_coherent(
+		jpeg->dev,
+		MXC_JPEG_MAX_CFG_STREAM,
+		&jpeg->slot_data[slot].cfg_stream_handle, GFP_ATOMIC);
+	if (!jpeg->slot_data[slot].cfg_stream_vaddr)
+		goto err;
+skip_alloc:
+	jpeg->slot_data[slot].used = true;
+
+	return true;
+err:
+	dev_err(jpeg->dev, "Could not allocate descriptors for slot %d", slot);
+	return false;
+}
+
+static void mxc_jpeg_free_slot_data(struct mxc_jpeg_dev *jpeg,
+				    unsigned int slot)
+{
+	if (slot >= MXC_MAX_SLOTS) {
+		dev_err(jpeg->dev, "Invalid slot %d, nothing to free.", slot);
+		return;
+	}
+
+	/* free descriptor for decoding/encoding phase */
+	dma_free_coherent(jpeg->dev, sizeof(struct mxc_jpeg_desc),
+			  jpeg->slot_data[slot].desc,
+			  jpeg->slot_data[slot].desc_handle);
+
+	/* free descriptor for encoder configuration phase / decoder DHT */
+	dma_free_coherent(jpeg->dev, sizeof(struct mxc_jpeg_desc),
+			  jpeg->slot_data[slot].cfg_desc,
+			  jpeg->slot_data[slot].cfg_desc_handle);
+
+	/* free configuration stream */
+	dma_free_coherent(jpeg->dev, MXC_JPEG_MAX_CFG_STREAM,
+			  jpeg->slot_data[slot].cfg_stream_vaddr,
+			  jpeg->slot_data[slot].cfg_stream_handle);
+
+	jpeg->slot_data[slot].used = false;
+}
+
+static irqreturn_t mxc_jpeg_dec_irq(int irq, void *priv)
+{
+	struct mxc_jpeg_dev *jpeg = priv;
+	struct mxc_jpeg_ctx *ctx;
+	void __iomem *reg = jpeg->base_reg;
+	struct device *dev = jpeg->dev;
+	struct vb2_v4l2_buffer *src_buf, *dst_buf;
+	struct mxc_jpeg_src_buf *jpeg_src_buf;
+	enum vb2_buffer_state buf_state;
+	u32 dec_ret, com_status;
+	unsigned long payload;
+	struct mxc_jpeg_q_data *q_data;
+	enum v4l2_buf_type cap_type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+	unsigned int slot;
+
+	spin_lock(&jpeg->hw_lock);
+
+	com_status = readl(reg + COM_STATUS);
+	slot = COM_STATUS_CUR_SLOT(com_status);
+	dev_dbg(dev, "Irq %d on slot %d.\n", irq, slot);
+
+	ctx = v4l2_m2m_get_curr_priv(jpeg->m2m_dev);
+	if (!ctx) {
+		dev_err(dev,
+			"Instance released before the end of transaction.\n");
+		/* soft reset only resets internal state, not registers */
+		mxc_jpeg_sw_reset(reg);
+		/* clear all interrupts */
+		writel(0xFFFFFFFF, reg + MXC_SLOT_OFFSET(slot, SLOT_STATUS));
+		goto job_unlock;
+	}
+
+	if (slot != ctx->slot) {
+		/* TODO investigate after all power domains work together */
+		dev_warn(dev, "IRQ slot %d != context slot %d.\n",
+			 slot, ctx->slot);
+		goto job_unlock;
+	}
+
+	dec_ret = readl(reg + MXC_SLOT_OFFSET(slot, SLOT_STATUS));
+	writel(dec_ret, reg + MXC_SLOT_OFFSET(slot, SLOT_STATUS)); /* w1c */
+
+	dst_buf = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
+	src_buf = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
+	if (!dst_buf || !src_buf) {
+		dev_err(dev, "No source or destination buffer.\n");
+		goto job_unlock;
+	}
+	jpeg_src_buf = vb2_to_mxc_buf(&src_buf->vb2_buf);
+
+	if (ctx->aborting) {
+		dev_warn(dev, "Aborting current job\n");
+		mxc_jpeg_sw_reset(reg);
+		buf_state = VB2_BUF_STATE_ERROR;
+		goto buffers_done;
+	}
+
+	if (dec_ret & SLOTa_STATUS_ENC_CONFIG_ERR) {
+		u32 ret = readl(reg + CAST_STATUS12);
+
+		dev_err(dev, "Encoder/decoder error, status=0x%08x", ret);
+		mxc_jpeg_sw_reset(reg);
+		buf_state = VB2_BUF_STATE_ERROR;
+		goto buffers_done;
+	}
+
+	if (!(dec_ret & SLOTa_STATUS_FRMDONE))
+		goto job_unlock;
+
+	if (ctx->mode == MXC_JPEG_ENCODE
+	    && ctx->enc_state == MXC_JPEG_ENC_CONF) {
+		ctx->enc_state = MXC_JPEG_ENC_DONE;
+		dev_dbg(dev, "Encoder config finished. Start encoding...\n");
+		mxc_jpeg_enc_mode_go(dev, reg);
+		goto job_unlock;
+	}
+	if (ctx->mode == MXC_JPEG_DECODE && jpeg_src_buf->dht_needed) {
+		jpeg_src_buf->dht_needed = false;
+		dev_dbg(dev, "Decoder DHT cfg finished. Start decoding...\n");
+		goto job_unlock;
+	}
+	if (ctx->mode == MXC_JPEG_ENCODE) {
+		payload = readl(reg + MXC_SLOT_OFFSET(slot, SLOT_BUF_PTR));
+		vb2_set_plane_payload(&dst_buf->vb2_buf, 0, payload);
+		dev_dbg(dev, "Encoding finished, payload size: %ld\n",
+			payload);
+	} else {
+		q_data = mxc_jpeg_get_q_data(ctx, cap_type);
+		payload = q_data->sizeimage[0];
+		vb2_set_plane_payload(&dst_buf->vb2_buf, 0, payload);
+		vb2_set_plane_payload(&dst_buf->vb2_buf, 1, 0);
+		if (q_data->fmt->colplanes == 2) {
+			payload = q_data->sizeimage[1];
+			vb2_set_plane_payload(&dst_buf->vb2_buf, 1, payload);
+		}
+		dev_dbg(dev, "Decoding finished, payload size: %ld + %ld\n",
+			vb2_get_plane_payload(&dst_buf->vb2_buf, 0),
+			vb2_get_plane_payload(&dst_buf->vb2_buf, 1));
+	}
+
+	dst_buf->vb2_buf.timestamp = ktime_get_ns();
+
+	/* short preview of the results */
+	dev_dbg(dev, "src_buf preview: ");
+	print_buf_preview(dev, &src_buf->vb2_buf);
+	dev_dbg(dev, "dst_buf preview: ");
+	print_buf_preview(dev, &dst_buf->vb2_buf);
+	buf_state = VB2_BUF_STATE_DONE;
+
+buffers_done:
+	jpeg->slot_data[slot].used = false; /* unused, but don't free */
+	v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
+	v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
+	v4l2_m2m_buf_done(src_buf, buf_state);
+	v4l2_m2m_buf_done(dst_buf, buf_state);
+	spin_unlock(&jpeg->hw_lock);
+	v4l2_m2m_job_finish(jpeg->m2m_dev, ctx->fh.m2m_ctx);
+	return IRQ_HANDLED;
+job_unlock:
+	spin_unlock(&jpeg->hw_lock);
+	return IRQ_HANDLED;
+}
+
+static int mxc_jpeg_fixup_sof(struct mxc_jpeg_sof *sof,
+			      u32 fourcc,
+			      u16 w, u16 h)
+{
+	int sof_length;
+
+	sof->precision = 8; /* TODO allow 8/12 bit precision*/
+	sof->height = h;
+	_bswap16(&sof->height);
+	sof->width = w;
+	_bswap16(&sof->width);
+
+	switch (fourcc) {
+	case V4L2_PIX_FMT_NV12:
+		sof->components_no = 3;
+		sof->comp[0].v = 0x2; sof->comp[0].h = 0x2;
+		break;
+	case V4L2_PIX_FMT_YUYV:
+		sof->components_no = 3;
+		sof->comp[0].v = 0x1; sof->comp[0].h = 0x2;
+		break;
+	case V4L2_PIX_FMT_YUV24:
+	case V4L2_PIX_FMT_RGB24:
+	default:
+		sof->components_no = 3;
+		break;
+	case V4L2_PIX_FMT_ARGB32:
+		sof->components_no = 4;
+		break;
+	case V4L2_PIX_FMT_GREY:
+		sof->components_no = 1;
+		break;
+	}
+	sof_length = 8 + 3 * sof->components_no;
+	sof->length = sof_length;
+	_bswap16(&sof->length);
+
+	return sof_length; /* not swaped */
+}
+
+static int mxc_jpeg_fixup_sos(struct mxc_jpeg_sos *sos,
+			      u32 fourcc)
+{
+	int sos_length;
+	u8 *sof_u8 = (u8 *)sos;
+
+	switch (fourcc) {
+	case V4L2_PIX_FMT_NV12:
+		sos->components_no = 3;
+		break;
+	case V4L2_PIX_FMT_YUYV:
+		sos->components_no = 3;
+		break;
+	case V4L2_PIX_FMT_YUV24:
+	case V4L2_PIX_FMT_RGB24:
+	default:
+		sos->components_no = 3;
+		break;
+	case V4L2_PIX_FMT_ARGB32:
+		sos->components_no = 4;
+		break;
+	case V4L2_PIX_FMT_GREY:
+		sos->components_no = 1;
+		break;
+	}
+	sos_length = 6 + 2 * sos->components_no;
+	sos->length = sos_length;
+	_bswap16(&sos->length);
+
+	/* SOS ignorable bytes, not so ignorable after all */
+	sof_u8[sos_length - 1] = 0x0;
+	sof_u8[sos_length - 2] = 0x3f;
+	sof_u8[sos_length - 3] = 0x0;
+
+	return sos_length; /* not swaped */
+}
+
+static unsigned int mxc_jpeg_setup_cfg_stream(void *cfg_stream_vaddr,
+					      u32 fourcc,
+					      u16 w, u16 h)
+{
+	unsigned int offset = 0;
+	u8 *cfg = (u8 *)cfg_stream_vaddr;
+	struct mxc_jpeg_sof *sof;
+	struct mxc_jpeg_sos *sos;
+
+	memcpy(cfg + offset, jpeg_soi, ARRAY_SIZE(jpeg_soi));
+	offset += ARRAY_SIZE(jpeg_soi);
+
+	if (fourcc == V4L2_PIX_FMT_RGB24 ||
+	    fourcc == V4L2_PIX_FMT_ARGB32) {
+		memcpy(cfg + offset, jpeg_app14, sizeof(jpeg_app14));
+		offset += sizeof(jpeg_app14);
+	} else {
+		memcpy(cfg + offset, jpeg_app0, sizeof(jpeg_app0));
+		offset += sizeof(jpeg_app0);
+	}
+
+	memcpy(cfg + offset, jpeg_dqt, sizeof(jpeg_dqt));
+	offset += sizeof(jpeg_dqt);
+
+	memcpy(cfg + offset, jpeg_sof_maximal, sizeof(jpeg_sof_maximal));
+	offset += 2; /* skip marker ID */
+	sof = (struct mxc_jpeg_sof *)(cfg + offset);
+	offset += mxc_jpeg_fixup_sof(sof, fourcc, w, h);
+
+	memcpy(cfg + offset, jpeg_dht, sizeof(jpeg_dht));
+	offset += sizeof(jpeg_dht);
+
+	memcpy(cfg + offset, jpeg_dri, sizeof(jpeg_dri));
+	offset += sizeof(jpeg_dri);
+
+	memcpy(cfg + offset, jpeg_sos_maximal, sizeof(jpeg_sos_maximal));
+	offset += 2; /* skip marker ID */
+	sos = (struct mxc_jpeg_sos *)(cfg + offset);
+	offset += mxc_jpeg_fixup_sos(sos, fourcc);
+
+	memcpy(cfg + offset, jpeg_eoi, sizeof(jpeg_eoi));
+	offset += sizeof(jpeg_eoi);
+
+	return offset;
+}
+
+static void mxc_jpeg_config_dec_desc(struct vb2_buffer *out_buf,
+				     struct mxc_jpeg_ctx *ctx,
+				     struct vb2_buffer *src_buf,
+				     struct vb2_buffer *dst_buf)
+{
+	enum v4l2_buf_type cap_type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+	struct mxc_jpeg_q_data *q_data_cap;
+	enum mxc_jpeg_image_format img_fmt;
+	struct mxc_jpeg_dev *jpeg = ctx->mxc_jpeg;
+	void __iomem *reg = jpeg->base_reg;
+	unsigned int slot = ctx->slot;
+	struct mxc_jpeg_desc *desc = jpeg->slot_data[slot].desc;
+	struct mxc_jpeg_desc *cfg_desc = jpeg->slot_data[slot].cfg_desc;
+	dma_addr_t desc_handle = jpeg->slot_data[slot].desc_handle;
+	dma_addr_t cfg_desc_handle = jpeg->slot_data[slot].cfg_desc_handle;
+	dma_addr_t cfg_stream_handle = jpeg->slot_data[slot].cfg_stream_handle;
+	unsigned int *cfg_size = &jpeg->slot_data[slot].cfg_stream_size;
+	void *cfg_stream_vaddr = jpeg->slot_data[slot].cfg_stream_vaddr;
+	struct mxc_jpeg_src_buf *jpeg_src_buf;
+
+	jpeg_src_buf = vb2_to_mxc_buf(src_buf);
+
+	/* setup the decoding descriptor */
+	desc->next_descpt_ptr = 0; /* end of chain */
+	q_data_cap = mxc_jpeg_get_q_data(ctx, cap_type);
+	desc->imgsize = q_data_cap->w_adjusted << 16 | q_data_cap->h_adjusted;
+	img_fmt = mxc_jpeg_fourcc_to_imgfmt(q_data_cap->fmt->fourcc);
+	desc->stm_ctrl &= ~STM_CTRL_IMAGE_FORMAT(0xF); /* clear image format */
+	desc->stm_ctrl |= STM_CTRL_IMAGE_FORMAT(img_fmt);
+	desc->line_pitch = q_data_cap->bytesperline[0];
+	mxc_jpeg_addrs(desc, dst_buf, src_buf, 0);
+	mxc_jpeg_set_bufsize(desc, ALIGN(vb2_plane_size(src_buf, 0), 1024));
+	print_descriptor_info(jpeg->dev, desc);
+
+	if (!jpeg_src_buf->dht_needed) {
+		/* validate the decoding descriptor */
+		mxc_jpeg_set_desc(desc_handle, reg, slot);
+		return;
+	}
+
+	/*
+	 * if a default huffman table is needed, use the config descriptor to
+	 * inject a DHT, by chaining it before the decoding descriptor
+	 */
+	*cfg_size = mxc_jpeg_setup_cfg_stream(cfg_stream_vaddr,
+					      V4L2_PIX_FMT_YUYV,
+					      MXC_JPEG_MIN_WIDTH,
+					      MXC_JPEG_MIN_HEIGHT);
+	cfg_desc->next_descpt_ptr = desc_handle | MXC_NXT_DESCPT_EN;
+	cfg_desc->buf_base0 = vb2_dma_contig_plane_dma_addr(dst_buf, 0);
+	cfg_desc->buf_base1 = 0;
+	cfg_desc->imgsize = MXC_JPEG_MIN_WIDTH << 16;
+	cfg_desc->imgsize |= MXC_JPEG_MIN_HEIGHT;
+	cfg_desc->line_pitch = MXC_JPEG_MIN_WIDTH * 2;
+	cfg_desc->stm_ctrl = STM_CTRL_IMAGE_FORMAT(MXC_JPEG_YUV422);
+	cfg_desc->stm_bufbase = cfg_stream_handle;
+	cfg_desc->stm_bufsize = ALIGN(*cfg_size, 1024);
+	print_descriptor_info(jpeg->dev, cfg_desc);
+
+	/* validate the configuration descriptor */
+	mxc_jpeg_set_desc(cfg_desc_handle, reg, slot);
+}
+
+static void mxc_jpeg_config_enc_desc(struct vb2_buffer *out_buf,
+				     struct mxc_jpeg_ctx *ctx,
+				     struct vb2_buffer *src_buf,
+				     struct vb2_buffer *dst_buf)
+{
+	struct mxc_jpeg_dev *jpeg = ctx->mxc_jpeg;
+	void __iomem *reg = jpeg->base_reg;
+	unsigned int slot = ctx->slot;
+	struct mxc_jpeg_desc *desc = jpeg->slot_data[slot].desc;
+	struct mxc_jpeg_desc *cfg_desc = jpeg->slot_data[slot].cfg_desc;
+	dma_addr_t desc_handle = jpeg->slot_data[slot].desc_handle;
+	dma_addr_t cfg_desc_handle = jpeg->slot_data[slot].cfg_desc_handle;
+	void *cfg_stream_vaddr = jpeg->slot_data[slot].cfg_stream_vaddr;
+	struct mxc_jpeg_q_data *q_data;
+	enum mxc_jpeg_image_format img_fmt;
+	int w, h;
+
+	q_data = mxc_jpeg_get_q_data(ctx, src_buf->vb2_queue->type);
+
+	jpeg->slot_data[slot].cfg_stream_size =
+			mxc_jpeg_setup_cfg_stream(cfg_stream_vaddr,
+						  q_data->fmt->fourcc,
+						  q_data->w_adjusted,
+						  q_data->h_adjusted);
+
+	/* chain the config descriptor with the encoding descriptor */
+	cfg_desc->next_descpt_ptr = desc_handle | MXC_NXT_DESCPT_EN;
+
+	cfg_desc->buf_base0 = jpeg->slot_data[slot].cfg_stream_handle;
+	cfg_desc->buf_base1 = 0;
+	cfg_desc->line_pitch = 0;
+	cfg_desc->stm_bufbase = 0;
+	cfg_desc->stm_bufsize = 0x2000;
+	cfg_desc->imgsize = 0;
+	cfg_desc->stm_ctrl = STM_CTRL_CONFIG_MOD(1);
+
+	desc->next_descpt_ptr = 0; /* end of chain */
+
+	/* use adjusted resolution for CAST IP job */
+	w = q_data->w_adjusted;
+	h = q_data->h_adjusted;
+	mxc_jpeg_set_res(desc, w, h);
+	mxc_jpeg_set_line_pitch(desc, w * (q_data->fmt->depth / 8));
+	mxc_jpeg_set_bufsize(desc, desc->line_pitch * h);
+	img_fmt = mxc_jpeg_fourcc_to_imgfmt(q_data->fmt->fourcc);
+	if (img_fmt == MXC_JPEG_INVALID)
+		dev_err(jpeg->dev, "No valid image format detected\n");
+	desc->stm_ctrl = STM_CTRL_CONFIG_MOD(0) |
+			 STM_CTRL_IMAGE_FORMAT(img_fmt);
+	mxc_jpeg_addrs(desc, src_buf, dst_buf, 0);
+	dev_dbg(jpeg->dev, "cfg_desc - 0x%llx:\n", cfg_desc_handle);
+	print_descriptor_info(jpeg->dev, cfg_desc);
+	dev_dbg(jpeg->dev, "enc desc - 0x%llx:\n", desc_handle);
+	print_descriptor_info(jpeg->dev, desc);
+	print_wrapper_info(jpeg->dev, reg);
+	print_cast_status(jpeg->dev, reg, MXC_JPEG_ENCODE);
+
+	/* validate the configuration descriptor */
+	mxc_jpeg_set_desc(cfg_desc_handle, reg, slot);
+}
+
+static void mxc_jpeg_device_run(void *priv)
+{
+	struct mxc_jpeg_ctx *ctx = priv;
+	struct mxc_jpeg_dev *jpeg = ctx->mxc_jpeg;
+	void __iomem *reg = jpeg->base_reg;
+	struct device *dev = jpeg->dev;
+	struct vb2_v4l2_buffer *src_buf, *dst_buf;
+	unsigned long flags;
+	struct mxc_jpeg_src_buf *jpeg_src_buf;
+
+	spin_lock_irqsave(&ctx->mxc_jpeg->hw_lock, flags);
+	src_buf = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
+	dst_buf = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
+	if (!src_buf || !dst_buf) {
+		dev_err(dev, "Null src or dst buf\n");
+		goto end;
+	}
+
+	jpeg_src_buf = vb2_to_mxc_buf(&src_buf->vb2_buf);
+	if (jpeg_src_buf->jpeg_parse_error) {
+		jpeg->slot_data[ctx->slot].used = false;
+		v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
+		v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
+		v4l2_m2m_buf_done(src_buf, VB2_BUF_STATE_ERROR);
+		v4l2_m2m_buf_done(dst_buf, VB2_BUF_STATE_ERROR);
+		spin_unlock_irqrestore(&ctx->mxc_jpeg->hw_lock, flags);
+		v4l2_m2m_job_finish(jpeg->m2m_dev, ctx->fh.m2m_ctx);
+
+		return;
+	}
+
+	/*
+	 * TODO: this reset should be removed, once we figure out
+	 * how to overcome hardware issues both on encoder and decoder
+	 */
+	mxc_jpeg_sw_reset(reg);
+	mxc_jpeg_enable(reg);
+	mxc_jpeg_set_l_endian(reg, 1);
+
+	ctx->slot = mxc_get_free_slot(jpeg->slot_data, MXC_MAX_SLOTS);
+	if (ctx->slot >= MXC_MAX_SLOTS) {
+		dev_err(dev, "No more free slots\n");
+		goto end;
+	}
+	if (!mxc_jpeg_alloc_slot_data(jpeg, ctx->slot)) {
+		dev_err(dev, "Canot allocate slot data\n");
+		goto end;
+	}
+
+	mxc_jpeg_enable_slot(reg, ctx->slot);
+	mxc_jpeg_enable_irq(reg, ctx->slot);
+
+	src_buf->vb2_buf.timestamp = ktime_get_ns();
+
+	if (ctx->mode == MXC_JPEG_ENCODE) {
+		dev_dbg(dev, "Encoding on slot %d\n", ctx->slot);
+		ctx->enc_state = MXC_JPEG_ENC_CONF;
+		mxc_jpeg_config_enc_desc(&dst_buf->vb2_buf, ctx,
+					 &src_buf->vb2_buf, &dst_buf->vb2_buf);
+		mxc_jpeg_enc_mode_conf(dev, reg);
+	} else {
+		dev_dbg(dev, "Decoding on slot %d\n", ctx->slot);
+		print_nbuf_to_eoi(dev, &src_buf->vb2_buf, 0);
+		mxc_jpeg_config_dec_desc(&dst_buf->vb2_buf, ctx,
+					 &src_buf->vb2_buf, &dst_buf->vb2_buf);
+		mxc_jpeg_dec_mode_go(dev, reg);
+	}
+end:
+	spin_unlock_irqrestore(&ctx->mxc_jpeg->hw_lock, flags);
+}
+
+static int mxc_jpeg_try_decoder_cmd(struct file *file, void *fh,
+				    struct v4l2_decoder_cmd *c)
+{
+	if (c->cmd != V4L2_DEC_CMD_STOP && c->cmd != V4L2_DEC_CMD_START)
+		return -EINVAL;
+
+	c->flags = 0;
+
+	if (c->cmd == V4L2_DEC_CMD_START) {
+		c->start.speed = 0;
+		c->start.format = 0;
+	}
+
+	return 0;
+}
+
+static int mxc_jpeg_decoder_cmd(struct file *file, void *priv,
+				struct v4l2_decoder_cmd *cmd)
+{
+	struct v4l2_fh *fh = file->private_data;
+	struct mxc_jpeg_ctx *ctx = mxc_jpeg_fh_to_ctx(fh);
+	struct device *dev = ctx->mxc_jpeg->dev;
+	int ret;
+
+	ret = mxc_jpeg_try_decoder_cmd(file, fh, cmd);
+	if (ret < 0)
+		return ret;
+
+	/* Ignore decoder command silently in encoder context */
+	if (ctx->mode != MXC_JPEG_DECODE)
+		return 0;
+
+	if (cmd->cmd == V4L2_DEC_CMD_STOP) {
+		dev_dbg(dev, "Received V4L2_DEC_CMD_STOP");
+		if (v4l2_m2m_num_src_bufs_ready(fh->m2m_ctx) == 0) {
+			/* No more src bufs, notify app EOS */
+			notify_eos(ctx);
+		} else {
+			/* will send EOS later*/
+			ctx->stopping = 1;
+		}
+	}
+
+	return 0;
+}
+
+static int mxc_jpeg_try_encoder_cmd(struct file *file, void *fh,
+				    struct v4l2_encoder_cmd *c)
+{
+	if (c->cmd != V4L2_ENC_CMD_STOP && c->cmd != V4L2_ENC_CMD_START)
+		return -EINVAL;
+
+	c->flags = 0;
+
+	return 0;
+}
+
+static int mxc_jpeg_encoder_cmd(struct file *file, void *priv,
+				struct v4l2_encoder_cmd *cmd)
+{
+	struct v4l2_fh *fh = file->private_data;
+	struct mxc_jpeg_ctx *ctx = mxc_jpeg_fh_to_ctx(fh);
+	struct device *dev = ctx->mxc_jpeg->dev;
+	int ret;
+
+	ret = mxc_jpeg_try_encoder_cmd(file, fh, cmd);
+	if (ret < 0)
+		return ret;
+
+	/* Ignore encoder command silently in decoder context */
+	if (ctx->mode != MXC_JPEG_ENCODE)
+		return 0;
+
+	if (cmd->cmd == V4L2_ENC_CMD_STOP) {
+		dev_dbg(dev, "Received V4L2_ENC_CMD_STOP");
+		if (v4l2_m2m_num_src_bufs_ready(fh->m2m_ctx) == 0) {
+			/* No more src bufs, notify app EOS */
+			notify_eos(ctx);
+		} else {
+			/* will send EOS later*/
+			ctx->stopping = 1;
+		}
+	}
+
+	return 0;
+}
+
+static int mxc_jpeg_job_ready(void *priv)
+{
+	struct mxc_jpeg_ctx *ctx = priv;
+	unsigned int num_src_bufs_ready;
+	unsigned int num_dst_bufs_ready;
+	unsigned long flags;
+
+	spin_lock_irqsave(&ctx->mxc_jpeg->hw_lock, flags);
+
+	num_src_bufs_ready = v4l2_m2m_num_src_bufs_ready(ctx->fh.m2m_ctx);
+	num_dst_bufs_ready = v4l2_m2m_num_dst_bufs_ready(ctx->fh.m2m_ctx);
+
+	spin_unlock_irqrestore(&ctx->mxc_jpeg->hw_lock, flags);
+
+	if (num_src_bufs_ready >= 1 && num_dst_bufs_ready >= 1)
+		return 1;
+	return 0;
+}
+static void mxc_jpeg_job_abort(void *priv)
+{
+	struct mxc_jpeg_ctx *ctx = priv;
+
+	ctx->aborting = 1;
+	dev_dbg(ctx->mxc_jpeg->dev, "Abort requested\n");
+}
+
+static int mxc_jpeg_queue_setup(struct vb2_queue *q,
+				unsigned int *nbuffers,
+				unsigned int *nplanes,
+				unsigned int sizes[],
+				struct device *alloc_ctxs[])
+{
+	struct mxc_jpeg_ctx *ctx = vb2_get_drv_priv(q);
+	struct mxc_jpeg_q_data *q_data = NULL;
+	int i;
+
+	q_data = mxc_jpeg_get_q_data(ctx, q->type);
+	if (!q_data)
+		return -EINVAL;
+
+	/* Handle CREATE_BUFS situation - *nplanes != 0 */
+	if (*nplanes) {
+		for (i = 0; i < *nplanes; i++) {
+			if (sizes[i] < q_data->sizeimage[i])
+				return -EINVAL;
+		}
+	}
+
+	/* Handle REQBUFS situation */
+	*nplanes = q_data->fmt->colplanes;
+	for (i = 0; i < *nplanes; i++)
+		sizes[i] = q_data->sizeimage[i];
+
+	return 0;
+}
+static int mxc_jpeg_start_streaming(struct vb2_queue *q, unsigned int count)
+{
+	struct mxc_jpeg_ctx *ctx = vb2_get_drv_priv(q);
+	int ret;
+
+	dev_dbg(ctx->mxc_jpeg->dev, "Start streaming ctx=%p", ctx);
+	ret = pm_runtime_get_sync(ctx->mxc_jpeg->dev);
+	return ret > 0 ? 0 : ret;
+}
+
+static void release_active_buffers(struct vb2_queue *q,
+				   enum vb2_buffer_state s)
+{
+	struct vb2_buffer *vb;
+
+	if (!list_empty(&q->queued_list))
+		list_for_each_entry(vb, &q->queued_list, queued_entry) {
+			if (vb->state == VB2_BUF_STATE_ACTIVE)
+				vb2_buffer_done(vb, s);
+		}
+}
+
+static void mxc_jpeg_stop_streaming(struct vb2_queue *q)
+{
+	struct mxc_jpeg_ctx *ctx = vb2_get_drv_priv(q);
+
+	dev_dbg(ctx->mxc_jpeg->dev, "Stop streaming ctx=%p", ctx);
+
+	/* Release all active buffers */
+	release_active_buffers(q, VB2_BUF_STATE_ERROR);
+
+	pm_runtime_put_sync(&ctx->mxc_jpeg->pdev->dev);
+}
+struct mxc_jpeg_stream {
+	u8 *addr;
+	u32 loc;
+	u32 end;
+};
+
+/* returns a value that fits into u8, or negative error */
+static int get_byte(struct mxc_jpeg_stream *stream)
+{
+	int ret;
+
+	if (stream->loc >= stream->end)
+		return -1;
+	ret = stream->addr[stream->loc];
+	stream->loc++;
+	return ret;
+}
+
+static int get_sof(struct device *dev,
+	struct mxc_jpeg_stream *stream,
+	struct mxc_jpeg_sof *sof)
+{
+	int i;
+
+	if (stream->loc + sizeof(struct mxc_jpeg_sof) >= stream->end)
+		return -1;
+	memcpy(sof, &stream->addr[stream->loc], sizeof(struct mxc_jpeg_sof));
+	_bswap16(&sof->length);
+	_bswap16(&sof->height);
+	_bswap16(&sof->width);
+	dev_dbg(dev, "JPEG SOF: precision=%d\n", sof->precision);
+	dev_dbg(dev, "JPEG SOF: height=%d, width=%d\n",
+		sof->height, sof->width);
+	for (i = 0; i < sof->components_no; i++) {
+		dev_dbg(dev, "JPEG SOF: comp_id=%d, H=0x%x, V=0x%x\n",
+			sof->comp[i].id, sof->comp[i].v, sof->comp[i].h);
+	}
+	return 0;
+}
+
+static int mxc_jpeg_valid_comp_id(
+	struct device *dev,
+	struct mxc_jpeg_sof *sof,
+	struct mxc_jpeg_sos *sos)
+{
+	int valid = 1;
+	int i;
+
+	for (i = 0; i < sof->components_no; i++)
+		if (sof->comp[i].id > MXC_JPEG_MAX_COMPONENTS) {
+			valid = 0;
+			dev_err(dev, "Component %d has invalid ID: %d",
+				i, sof->comp[i].id);
+		}
+	if (!valid)
+		/* patch all comp IDs if at least one is invalid */
+		for (i = 0; i < sof->components_no; i++) {
+			dev_warn(dev, "Component %d ID patched to: %d",
+				 i, i + 1);
+			sof->comp[i].id = i + 1;
+			sos->comp[i].id = i + 1;
+		}
+
+	return valid;
+}
+
+static enum mxc_jpeg_image_format mxc_jpeg_get_image_format(
+	struct device *dev,
+	const struct mxc_jpeg_sof *sof)
+{
+	if (sof->components_no == 1) {
+		dev_dbg(dev, "IMAGE_FORMAT is: MXC_JPEG_GRAY\n");
+		return MXC_JPEG_GRAY;
+	}
+	if (sof->components_no == 3) {
+		if (sof->comp[0].h == 2 && sof->comp[0].v == 2 &&
+		    sof->comp[1].h == 1 && sof->comp[1].v == 1 &&
+		    sof->comp[2].h == 1 && sof->comp[2].v == 1){
+			dev_dbg(dev, "IMAGE_FORMAT is: MXC_JPEG_YUV420\n");
+			return MXC_JPEG_YUV420;
+		}
+		if (sof->comp[0].h == 2 && sof->comp[0].v == 1 &&
+		    sof->comp[1].h == 1 && sof->comp[1].v == 1 &&
+		    sof->comp[2].h == 1 && sof->comp[2].v == 1){
+			dev_dbg(dev, "IMAGE_FORMAT is: MXC_JPEG_YUV422\n");
+			return MXC_JPEG_YUV422;
+		}
+		if (sof->comp[0].h == 1 && sof->comp[0].v == 1 &&
+		    sof->comp[1].h == 1 && sof->comp[1].v == 1 &&
+		    sof->comp[2].h == 1 && sof->comp[2].v == 1){
+			dev_dbg(dev, "IMAGE_FORMAT is: MXC_JPEG_YUV444\n");
+			return MXC_JPEG_YUV444;
+		}
+	}
+	if (sof->components_no == 4) {
+		if (sof->comp[0].h == 1 && sof->comp[0].v == 1 &&
+		    sof->comp[1].h == 1 && sof->comp[1].v == 1 &&
+		    sof->comp[2].h == 1 && sof->comp[2].v == 1 &&
+		    sof->comp[3].h == 1 && sof->comp[3].v == 1){
+			/* this is not tested */
+			dev_dbg(dev, "IMAGE_FORMAT is: MXC_JPEG_ARGB\n");
+			return MXC_JPEG_ARGB;
+		}
+	}
+	dev_err(dev, "Could not identify image format\n");
+	return MXC_JPEG_INVALID;
+}
+
+static void mxc_jpeg_bytesperline(struct mxc_jpeg_q_data *q,
+				  u32 precision)
+{
+	/* Bytes distance between the leftmost pixels in two adjacent lines */
+	if (q->fmt->fourcc == V4L2_PIX_FMT_JPEG) {
+		/* bytesperline unused for compressed formats */
+		q->bytesperline[0] = 0;
+		q->bytesperline[1] = 0;
+	} else if (q->fmt->fourcc == V4L2_PIX_FMT_NV12) {
+		/* When the image format is planar the bytesperline value
+		 * applies to the first plane and is divided by the same factor
+		 * as the width field for the other planes
+		 */
+		q->bytesperline[0] = q->w * (precision / 8) *
+				     (q->fmt->depth / 8);
+		q->bytesperline[1] = q->bytesperline[0];
+	} else {
+		/* single plane formats */
+		q->bytesperline[0] = q->w * (precision / 8) *
+				     (q->fmt->depth / 8);
+		q->bytesperline[1] = 0;
+	}
+}
+
+static void mxc_jpeg_sizeimage(struct mxc_jpeg_q_data *q)
+{
+	if (q->fmt->fourcc == V4L2_PIX_FMT_JPEG) {
+		/* if no sizeimage from user, assume worst jpeg compression */
+		if (!q->sizeimage[0])
+			q->sizeimage[0] = 6 * q->w * q->h;
+		q->sizeimage[1] = 0;
+
+		if (q->sizeimage[0] > MXC_JPEG_MAX_SIZEIMAGE)
+			q->sizeimage[0] = MXC_JPEG_MAX_SIZEIMAGE;
+
+		/* jpeg stream size must be multiple of 1K */
+		q->sizeimage[0] = ALIGN(q->sizeimage[0], 1024);
+	} else {
+		q->sizeimage[0] = q->bytesperline[0] * q->h;
+		q->sizeimage[1] = 0;
+		if (q->fmt->fourcc == V4L2_PIX_FMT_NV12)
+			q->sizeimage[1] = q->sizeimage[0] / 2;
+	}
+}
+
+static int mxc_jpeg_parse(struct mxc_jpeg_ctx *ctx,
+	u8 *src_addr, u32 size, bool *dht_needed)
+{
+	struct device *dev = ctx->mxc_jpeg->dev;
+	struct mxc_jpeg_q_data *q_data_out, *q_data_cap;
+	enum v4l2_buf_type cap_type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+	struct mxc_jpeg_stream stream;
+	bool notfound = true;
+	bool app14 = false;
+	bool src_chg = false;
+	u8 app14_transform = 0;
+	struct mxc_jpeg_sof sof, *psof = NULL;
+	struct mxc_jpeg_sos *psos = NULL;
+	int byte;
+	u8 *next = NULL;
+	enum mxc_jpeg_image_format img_fmt;
+	u32 fourcc;
+
+	memset(&sof, 0, sizeof(struct mxc_jpeg_sof));
+	stream.addr = src_addr;
+	stream.end = size;
+	stream.loc = 0;
+	*dht_needed = true;
+	while (notfound) {
+		byte = get_byte(&stream);
+		if (byte == -1)
+			return -EINVAL;
+		if (byte != 0xff)
+			continue;
+		do {
+			byte = get_byte(&stream);
+		} while (byte == 0xff);
+		if (byte == -1)
+			return -EINVAL;
+		if (byte == 0)
+			continue;
+		switch (byte) {
+		case DHT:
+			/* DHT marker present, no need to inject default one */
+			*dht_needed = false;
+			break;
+		case SOF2: /* Progressive DCF frame definition */
+			dev_err(dev,
+				"Progressive JPEG not supported by hardware");
+			return -EINVAL;
+		case SOF1: /* Extended sequential DCF frame definition */
+		case SOF0: /* Baseline sequential DCF frame definition */
+			if (get_sof(dev, &stream, &sof) == -1)
+				break;
+			next = stream.addr + stream.loc;
+			psof = (struct mxc_jpeg_sof *)next;
+			break;
+		case SOS:
+			next = stream.addr + stream.loc;
+			psos = (struct mxc_jpeg_sos *)next;
+			notfound = false;
+			break;
+		case APP14:
+			app14 = true;
+			/*
+			 * Application Data Syntax is:
+			 * 2 bytes(APPn:0xFF,0xEE), 2 bytes(Lp), Ap1...ApLp-2
+			 * The transform flag is in Ap12
+			 * stream.loc is now on APPn-0xEE byte
+			 */
+			app14_transform = *(stream.addr + stream.loc + 12 + 1);
+			break;
+		default:
+			notfound = true;
+		}
+	}
+	q_data_out = mxc_jpeg_get_q_data(ctx,
+					 V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE);
+	if (q_data_out->w == 0 && q_data_out->h == 0) {
+		dev_warn(dev, "Invalid user resolution 0x0");
+		dev_warn(dev, "Keeping resolution from JPEG: %dx%d",
+			 sof.width, sof.height);
+		 q_data_out->w = sof.width;
+		 q_data_out->h = sof.height;
+	} else if (sof.width != q_data_out->w || sof.height != q_data_out->h) {
+		dev_err(dev,
+			"Resolution mismatch: %dx%d (JPEG) versus %dx%d(user)",
+			sof.width, sof.height, q_data_out->w, q_data_out->h);
+		return -EINVAL;
+	}
+	if (sof.width % 8 != 0 || sof.height % 8 != 0) {
+		dev_err(dev, "JPEG width or height not multiple of 8: %dx%d\n",
+			sof.width, sof.height);
+		return -EINVAL;
+	}
+	if (sof.width > MXC_JPEG_MAX_WIDTH ||
+	    sof.height > MXC_JPEG_MAX_HEIGHT) {
+		dev_err(dev, "JPEG width or height should be <= 8192: %dx%d\n",
+			sof.width, sof.height);
+		return -EINVAL;
+	}
+	if (sof.width < MXC_JPEG_MIN_WIDTH ||
+	    sof.height < MXC_JPEG_MIN_HEIGHT) {
+		dev_err(dev, "JPEG width or height should be > 64: %dx%d\n",
+			sof.width, sof.height);
+		return -EINVAL;
+	}
+	if (sof.components_no > MXC_JPEG_MAX_COMPONENTS) {
+		dev_err(dev, "JPEG number of components should be <=%d",
+			MXC_JPEG_MAX_COMPONENTS);
+		return -EINVAL;
+	}
+	/* check and, if necessary, patch component IDs*/
+	if (!mxc_jpeg_valid_comp_id(dev, psof, psos))
+		dev_warn(dev, "JPEG component ids should be 0-3 or 1-4");
+
+	img_fmt = mxc_jpeg_get_image_format(dev, &sof);
+	if (img_fmt == MXC_JPEG_INVALID)
+		return -EINVAL;
+
+	/*
+	 * If the transform flag from APP14 marker is 0, images that are
+	 * encoded with 3 components have RGB colorspace, see Recommendation
+	 * ITU-T T.872 chapter 6.5.3 APP14 marker segment for colour encoding
+	 */
+	if ((img_fmt == MXC_JPEG_YUV444) && app14 && (app14_transform == 0))
+		img_fmt = MXC_JPEG_RGB;
+
+	if (mxc_jpeg_imgfmt_to_fourcc(img_fmt, &fourcc)) {
+		dev_err(dev, "Fourcc not found for %d", img_fmt);
+		return -EINVAL;
+	}
+
+	/*
+	 * set-up the capture queue with the pixelformat and resolution
+	 * detected from the jpeg output stream
+	 */
+	q_data_cap = mxc_jpeg_get_q_data(ctx, cap_type);
+	if (q_data_cap->w != sof.width || q_data_cap->h != sof.height)
+		src_chg = true;
+	q_data_cap->w = sof.width;
+	q_data_cap->h = sof.height;
+	q_data_cap->fmt = mxc_jpeg_find_format(ctx, fourcc);
+	q_data_cap->w_adjusted = q_data_cap->w;
+	q_data_cap->h_adjusted = q_data_cap->h;
+	/*
+	 * align up the resolution for CAST IP,
+	 * but leave the buffer resolution unchanged
+	 */
+	v4l_bound_align_image(&q_data_cap->w_adjusted,
+			      q_data_cap->w_adjusted,  /* adjust up */
+			      MXC_JPEG_MAX_WIDTH,
+			      q_data_cap->fmt->h_align,
+			      &q_data_cap->h_adjusted,
+			      q_data_cap->h_adjusted, /* adjust up */
+			      MXC_JPEG_MAX_HEIGHT,
+			      q_data_cap->fmt->v_align,
+			      0);
+	{
+		char *jpeg_format_name = fourcc_to_str(fourcc);
+
+		dev_dbg(dev, "Detected jpeg res=(%dx%d)->(%dx%d), pixfmt=%s\n",
+			q_data_cap->w, q_data_cap->h,
+			q_data_cap->w_adjusted, q_data_cap->h_adjusted,
+			jpeg_format_name);
+		kfree(jpeg_format_name);
+	}
+
+	/* setup bytesperline/sizeimage for capture queue */
+	mxc_jpeg_bytesperline(q_data_cap, sof.precision);
+	mxc_jpeg_sizeimage(q_data_cap);
+
+	/*
+	 * if the CAPTURE format was updated with new values, regardless of
+	 * whether they match the values set by the client or not, signal
+	 * a source change event
+	 */
+	if (src_chg)
+		notify_src_chg(ctx);
+
+	return 0;
+}
+
+static void mxc_jpeg_buf_queue(struct vb2_buffer *vb)
+{
+	int ret;
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct mxc_jpeg_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+	struct mxc_jpeg_src_buf *jpeg_src_buf;
+
+	if (vb->vb2_queue->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+		goto end;
+
+	/* for V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE */
+	if (ctx->mode != MXC_JPEG_DECODE)
+		goto end;
+	jpeg_src_buf = vb2_to_mxc_buf(vb);
+	jpeg_src_buf->jpeg_parse_error = false;
+	ret = mxc_jpeg_parse(ctx,
+			(u8 *)vb2_plane_vaddr(vb, 0),
+			vb2_get_plane_payload(vb, 0),
+			&jpeg_src_buf->dht_needed);
+	if (ret != 0) {
+		v4l2_err(&ctx->mxc_jpeg->v4l2_dev,
+			 "error parsing jpeg headers\n");
+		jpeg_src_buf->jpeg_parse_error = true;
+	}
+end:
+	v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vbuf);
+}
+static int mxc_jpeg_buf_prepare(struct vb2_buffer *vb)
+{
+	struct mxc_jpeg_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+	struct mxc_jpeg_q_data *q_data = NULL;
+	struct device *dev = ctx->mxc_jpeg->dev;
+	unsigned long sizeimage;
+	int i;
+
+	q_data = mxc_jpeg_get_q_data(ctx, vb->vb2_queue->type);
+	if (!q_data)
+		return -EINVAL;
+	for (i = 0; i < q_data->fmt->colplanes; i++) {
+		sizeimage = q_data->sizeimage[i];
+		if (vb2_plane_size(vb, i) < sizeimage) {
+			dev_err(dev, "plane %d too small (%lu < %lu)",
+				i, vb2_plane_size(vb, i), sizeimage);
+			return -EINVAL;
+		}
+		vb2_set_plane_payload(vb, i, sizeimage);
+	}
+	return 0;
+}
+
+static void mxc_jpeg_buf_clean(struct vb2_buffer *vb)
+{
+}
+
+static const struct vb2_ops mxc_jpeg_qops = {
+	.queue_setup		= mxc_jpeg_queue_setup,
+	.wait_prepare		= vb2_ops_wait_prepare,
+	.wait_finish		= vb2_ops_wait_finish,
+	.buf_prepare		= mxc_jpeg_buf_prepare,
+	.buf_cleanup		= mxc_jpeg_buf_clean,
+	.start_streaming	= mxc_jpeg_start_streaming,
+	.stop_streaming		= mxc_jpeg_stop_streaming,
+	.buf_queue		= mxc_jpeg_buf_queue,
+};
+static int mxc_jpeg_queue_init(void *priv, struct vb2_queue *src_vq,
+			       struct vb2_queue *dst_vq)
+{
+	struct mxc_jpeg_ctx *ctx = priv;
+	int ret;
+
+	src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
+	src_vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
+	src_vq->drv_priv = ctx;
+	src_vq->buf_struct_size = sizeof(struct mxc_jpeg_src_buf);
+	src_vq->ops = &mxc_jpeg_qops;
+	src_vq->mem_ops = &vb2_dma_contig_memops;
+	src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	src_vq->lock = &ctx->mxc_jpeg->lock;
+	src_vq->dev = ctx->mxc_jpeg->dev;
+	src_vq->allow_zero_bytesused = 1; /* keep old userspace apps working */
+
+	ret = vb2_queue_init(src_vq);
+	if (ret)
+		return ret;
+
+	dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+	dst_vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
+	dst_vq->drv_priv = ctx;
+	dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
+	dst_vq->ops = &mxc_jpeg_qops;
+	dst_vq->mem_ops = &vb2_dma_contig_memops;
+	dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	dst_vq->lock = &ctx->mxc_jpeg->lock;
+	dst_vq->dev = ctx->mxc_jpeg->dev;
+
+	ret = vb2_queue_init(dst_vq);
+	return ret;
+}
+
+static void mxc_jpeg_set_default_params(struct mxc_jpeg_ctx *ctx)
+{
+	struct mxc_jpeg_q_data *out_q = &ctx->out_q;
+	struct mxc_jpeg_q_data *cap_q = &ctx->cap_q;
+	struct mxc_jpeg_q_data *q[2] = {out_q, cap_q};
+	int i;
+
+	ctx->colorspace = V4L2_COLORSPACE_JPEG,
+	ctx->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
+	ctx->quantization = V4L2_QUANTIZATION_DEFAULT;
+	ctx->xfer_func = V4L2_XFER_FUNC_DEFAULT;
+
+	if (ctx->mode == MXC_JPEG_ENCODE) {
+		out_q->fmt = mxc_jpeg_find_format(ctx, MXC_JPEG_DEFAULT_PFMT);
+		cap_q->fmt = mxc_jpeg_find_format(ctx, V4L2_PIX_FMT_JPEG);
+	} else {
+		out_q->fmt = mxc_jpeg_find_format(ctx, V4L2_PIX_FMT_JPEG);
+		cap_q->fmt = mxc_jpeg_find_format(ctx, MXC_JPEG_DEFAULT_PFMT);
+	}
+
+	for (i = 0; i < 2; i++) {
+		q[i]->w = MXC_JPEG_DEFAULT_WIDTH;
+		q[i]->h = MXC_JPEG_DEFAULT_HEIGHT;
+		mxc_jpeg_bytesperline(q[i], 8);
+		mxc_jpeg_sizeimage(q[i]);
+	}
+}
+
+static int mxc_jpeg_open(struct file *file)
+{
+	struct mxc_jpeg_dev *mxc_jpeg = video_drvdata(file);
+	struct video_device *mxc_vfd = video_devdata(file);
+	struct device *dev = mxc_jpeg->dev;
+	struct mxc_jpeg_ctx *ctx;
+	int ret = 0;
+
+	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
+	if (!ctx)
+		return -ENOMEM;
+
+	if (mutex_lock_interruptible(&mxc_jpeg->lock)) {
+		ret = -ERESTARTSYS;
+		goto free;
+	}
+
+	pm_runtime_get_sync(mxc_jpeg->dev);
+
+	v4l2_fh_init(&ctx->fh, mxc_vfd);
+	file->private_data = &ctx->fh;
+	v4l2_fh_add(&ctx->fh);
+
+	ctx->mxc_jpeg = mxc_jpeg;
+	ctx->mode = mxc_jpeg->mode;
+
+	ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(mxc_jpeg->m2m_dev, ctx,
+					    mxc_jpeg_queue_init);
+
+	if (IS_ERR(ctx->fh.m2m_ctx)) {
+		ret = PTR_ERR(ctx->fh.m2m_ctx);
+		goto error;
+	}
+
+	mxc_jpeg_set_default_params(ctx);
+	ctx->slot = MXC_MAX_SLOTS; /* slot not allocated yet */
+
+	if (mxc_jpeg->mode == MXC_JPEG_DECODE)
+		dev_dbg(dev, "Opened JPEG decoder instance %p\n", ctx);
+	else
+		dev_dbg(dev, "Opened JPEG encoder instance %p\n", ctx);
+	mutex_unlock(&mxc_jpeg->lock);
+	return 0;
+
+error:
+	v4l2_fh_del(&ctx->fh);
+	v4l2_fh_exit(&ctx->fh);
+	mutex_unlock(&mxc_jpeg->lock);
+free:
+	kfree(ctx);
+	return ret;
+}
+
+static int mxc_jpeg_querycap(struct file *file, void *priv,
+			     struct v4l2_capability *cap)
+{
+	struct mxc_jpeg_dev *mxc_jpeg = video_drvdata(file);
+
+	strscpy(cap->driver, MXC_JPEG_NAME " decoder", sizeof(cap->driver));
+	strscpy(cap->card, MXC_JPEG_NAME " decoder", sizeof(cap->card));
+	snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s",
+		 dev_name(mxc_jpeg->dev));
+	cap->device_caps = V4L2_CAP_STREAMING | V4L2_CAP_VIDEO_M2M_MPLANE;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+
+	return 0;
+}
+static int mxc_jpeg_enum_fmt_vid_cap(struct file *file, void *priv,
+				     struct v4l2_fmtdesc *f)
+{
+	struct mxc_jpeg_ctx *ctx = mxc_jpeg_fh_to_ctx(priv);
+
+	if (ctx->mode == MXC_JPEG_ENCODE)
+		return enum_fmt(mxc_formats, MXC_JPEG_NUM_FORMATS, f,
+			MXC_IN_FORMAT);
+	else
+		return enum_fmt(mxc_formats, MXC_JPEG_NUM_FORMATS, f,
+			MXC_OUT_FORMAT);
+}
+
+static int mxc_jpeg_enum_fmt_vid_out(struct file *file, void *priv,
+				     struct v4l2_fmtdesc *f)
+{
+	struct mxc_jpeg_ctx *ctx = mxc_jpeg_fh_to_ctx(priv);
+
+	if (ctx->mode == MXC_JPEG_DECODE)
+		return enum_fmt(mxc_formats, MXC_JPEG_NUM_FORMATS, f,
+				MXC_IN_FORMAT);
+	else
+		return enum_fmt(mxc_formats, MXC_JPEG_NUM_FORMATS, f,
+				MXC_OUT_FORMAT);
+}
+
+static int mxc_jpeg_try_fmt(struct v4l2_format *f, struct mxc_jpeg_fmt *fmt,
+			    struct mxc_jpeg_ctx *ctx, int q_type)
+{
+	struct v4l2_pix_format_mplane *pix_mp = &f->fmt.pix_mp;
+	struct v4l2_plane_pix_format *pfmt;
+	u32 w = (pix_mp->width < MXC_JPEG_MAX_WIDTH) ?
+		 pix_mp->width : MXC_JPEG_MAX_WIDTH;
+	u32 h = (pix_mp->height < MXC_JPEG_MAX_HEIGHT) ?
+		 pix_mp->height : MXC_JPEG_MAX_HEIGHT;
+	int i;
+	struct mxc_jpeg_q_data tmp_q;
+
+	memset(pix_mp->reserved, 0, sizeof(pix_mp->reserved));
+	pix_mp->field = V4L2_FIELD_NONE;
+	pix_mp->num_planes = fmt->colplanes;
+	pix_mp->pixelformat = fmt->fourcc;
+
+	/*
+	 * use MXC_JPEG_H_ALIGN instead of fmt->v_align, for vertical
+	 * alignment, to loosen up the alignment to multiple of 8,
+	 * otherwise NV12-1080p fails as 1080 is not a multiple of 16
+	 */
+	v4l_bound_align_image(&w,
+			      MXC_JPEG_MIN_WIDTH,
+			      w, /* adjust downwards*/
+			      fmt->h_align,
+			      &h,
+			      MXC_JPEG_MIN_HEIGHT,
+			      h, /* adjust downwards*/
+			      MXC_JPEG_H_ALIGN,
+			      0);
+	pix_mp->width = w; /* negotiate the width */
+	pix_mp->height = h; /* negotiate the height */
+
+	/* get user input into the tmp_q */
+	tmp_q.w = w;
+	tmp_q.h = h;
+	tmp_q.fmt = fmt;
+	for (i = 0; i < pix_mp->num_planes; i++) {
+		pfmt = &pix_mp->plane_fmt[i];
+		tmp_q.bytesperline[i] = pfmt->bytesperline;
+		tmp_q.sizeimage[i] = pfmt->sizeimage;
+	}
+
+	/* calculate bytesperline & sizeimage into the tmp_q */
+	mxc_jpeg_bytesperline(&tmp_q, 8);
+	mxc_jpeg_sizeimage(&tmp_q);
+
+	/* adjust user format according to our calculations */
+	for (i = 0; i < pix_mp->num_planes; i++) {
+		pfmt = &pix_mp->plane_fmt[i];
+		memset(pfmt->reserved, 0, sizeof(pfmt->reserved));
+		pfmt->bytesperline = tmp_q.bytesperline[i];
+		pfmt->sizeimage = tmp_q.sizeimage[i];
+	}
+
+	return 0;
+}
+static int mxc_jpeg_try_fmt_vid_cap(struct file *file, void *priv,
+				struct v4l2_format *f)
+{
+	struct mxc_jpeg_ctx *ctx = mxc_jpeg_fh_to_ctx(priv);
+	struct mxc_jpeg_dev *jpeg = ctx->mxc_jpeg;
+	struct device *dev = jpeg->dev;
+	struct mxc_jpeg_fmt *fmt;
+	u32 fourcc = f->fmt.pix_mp.pixelformat;
+
+	int q_type = (ctx->mode == MXC_JPEG_DECODE) ?
+		     MXC_JPEG_FMT_TYPE_RAW : MXC_JPEG_FMT_TYPE_ENC;
+
+	if (!V4L2_TYPE_IS_MULTIPLANAR(f->type)) {
+		dev_err(dev, "TRY_FMT with Invalid type: %d\n", f->type);
+		return -EINVAL;
+	}
+
+	fmt = mxc_jpeg_find_format(ctx, f->fmt.pix_mp.pixelformat);
+	if (!fmt || (fmt->flags != q_type)) {
+		char *format_name = fourcc_to_str(fourcc);
+
+		dev_warn(dev, "Format not supported: %s, use the default.\n",
+			 format_name);
+		kfree(format_name);
+		f->fmt.pix_mp.pixelformat = (ctx->mode == MXC_JPEG_DECODE) ?
+				MXC_JPEG_DEFAULT_PFMT : V4L2_PIX_FMT_JPEG;
+		fmt = mxc_jpeg_find_format(ctx, f->fmt.pix_mp.pixelformat);
+	}
+	return mxc_jpeg_try_fmt(f, fmt, ctx, q_type);
+}
+static int mxc_jpeg_try_fmt_vid_out(struct file *file, void *priv,
+				struct v4l2_format *f)
+{
+	struct mxc_jpeg_ctx *ctx = mxc_jpeg_fh_to_ctx(priv);
+	struct mxc_jpeg_dev *jpeg = ctx->mxc_jpeg;
+	struct device *dev = jpeg->dev;
+	struct mxc_jpeg_fmt *fmt;
+	u32 fourcc = f->fmt.pix_mp.pixelformat;
+
+	int q_type = (ctx->mode == MXC_JPEG_ENCODE) ?
+		     MXC_JPEG_FMT_TYPE_RAW : MXC_JPEG_FMT_TYPE_ENC;
+
+	if (!V4L2_TYPE_IS_MULTIPLANAR(f->type)) {
+		dev_err(dev, "TRY_FMT with Invalid type: %d\n", f->type);
+		return -EINVAL;
+	}
+
+	fmt = mxc_jpeg_find_format(ctx, fourcc);
+	if (!fmt || (fmt->flags != q_type)) {
+		char *format_name = fourcc_to_str(fourcc);
+
+		dev_warn(dev, "Format not supported: %s, use the default.\n",
+			 format_name);
+		kfree(format_name);
+		f->fmt.pix_mp.pixelformat = (ctx->mode == MXC_JPEG_ENCODE) ?
+				MXC_JPEG_DEFAULT_PFMT : V4L2_PIX_FMT_JPEG;
+		fmt = mxc_jpeg_find_format(ctx, f->fmt.pix_mp.pixelformat);
+	}
+	return mxc_jpeg_try_fmt(f, fmt, ctx, q_type);
+}
+static int mxc_jpeg_s_fmt(struct mxc_jpeg_ctx *ctx,
+			  struct v4l2_format *f)
+{
+	struct vb2_queue *vq;
+	struct mxc_jpeg_q_data *q_data = NULL;
+	struct v4l2_pix_format_mplane *pix_mp = &f->fmt.pix_mp;
+	struct mxc_jpeg_dev *jpeg = ctx->mxc_jpeg;
+	int i;
+
+	vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
+	if (!vq)
+		return -EINVAL;
+
+	q_data = mxc_jpeg_get_q_data(ctx, f->type);
+
+	if (vb2_is_busy(vq)) {
+		v4l2_err(&jpeg->v4l2_dev, "queue busy\n");
+		return -EBUSY;
+	}
+
+	q_data->fmt = mxc_jpeg_find_format(ctx, pix_mp->pixelformat);
+	q_data->w = pix_mp->width;
+	q_data->h = pix_mp->height;
+
+	q_data->w_adjusted = q_data->w;
+	q_data->h_adjusted = q_data->h;
+	if (ctx->mode == MXC_JPEG_DECODE) {
+		/*
+		 * align up the resolution for CAST IP,
+		 * but leave the buffer resolution unchanged
+		 * TODO check if CAST IP will write past the buffer
+		 */
+		v4l_bound_align_image(&q_data->w_adjusted,
+				      q_data->w_adjusted,  /* adjust upwards */
+				      MXC_JPEG_MAX_WIDTH,
+				      q_data->fmt->h_align,
+				      &q_data->h_adjusted,
+				      q_data->h_adjusted, /* adjust upwards */
+				      MXC_JPEG_MAX_HEIGHT,
+				      q_data->fmt->v_align,
+				      0);
+	} else {
+		/*
+		 * align down the resolution for CAST IP,
+		 * but leave the buffer resolution unchanged
+		 */
+		v4l_bound_align_image(&q_data->w_adjusted,
+				      MXC_JPEG_MIN_WIDTH,
+				      q_data->w_adjusted, /* adjust downwards*/
+				      q_data->fmt->h_align,
+				      &q_data->h_adjusted,
+				      MXC_JPEG_MIN_HEIGHT,
+				      q_data->h_adjusted, /* adjust downwards*/
+				      q_data->fmt->v_align,
+				      0);
+	}
+
+	ctx->colorspace = pix_mp->colorspace;
+	ctx->ycbcr_enc = pix_mp->ycbcr_enc;
+	ctx->xfer_func = pix_mp->xfer_func;
+	ctx->quantization = pix_mp->quantization;
+
+	for (i = 0; i < pix_mp->num_planes; i++) {
+		q_data->bytesperline[i] = pix_mp->plane_fmt[i].bytesperline;
+		q_data->sizeimage[i] = pix_mp->plane_fmt[i].sizeimage;
+	}
+
+	return 0;
+}
+static int mxc_jpeg_s_fmt_vid_cap(struct file *file, void *priv,
+				struct v4l2_format *f)
+{
+	int ret;
+
+	ret = mxc_jpeg_try_fmt_vid_cap(file, priv, f);
+	if (ret)
+		return ret;
+
+	return mxc_jpeg_s_fmt(mxc_jpeg_fh_to_ctx(priv), f);
+}
+static int mxc_jpeg_s_fmt_vid_out(struct file *file, void *priv,
+				struct v4l2_format *f)
+{
+	int ret;
+
+	ret = mxc_jpeg_try_fmt_vid_out(file, priv, f);
+	if (ret)
+		return ret;
+
+	return mxc_jpeg_s_fmt(mxc_jpeg_fh_to_ctx(priv), f);
+}
+
+static int mxc_jpeg_g_fmt_vid(struct file *file, void *priv,
+			      struct v4l2_format *f)
+{
+	struct mxc_jpeg_ctx *ctx = mxc_jpeg_fh_to_ctx(priv);
+	struct mxc_jpeg_dev *jpeg = ctx->mxc_jpeg;
+	struct device *dev = jpeg->dev;
+	struct v4l2_pix_format_mplane   *pix_mp = &f->fmt.pix_mp;
+	struct mxc_jpeg_q_data *q_data = mxc_jpeg_get_q_data(ctx, f->type);
+	int i;
+
+	if (!V4L2_TYPE_IS_MULTIPLANAR(f->type)) {
+		dev_err(dev, "G_FMT with Invalid type: %d\n", f->type);
+		return -EINVAL;
+	}
+
+	pix_mp->pixelformat = q_data->fmt->fourcc;
+	pix_mp->width = q_data->w;
+	pix_mp->height = q_data->h;
+	pix_mp->field = V4L2_FIELD_NONE;
+	pix_mp->colorspace = ctx->colorspace;
+	pix_mp->ycbcr_enc = ctx->ycbcr_enc;
+	pix_mp->xfer_func = ctx->xfer_func;
+	pix_mp->quantization = ctx->quantization;
+	pix_mp->num_planes = q_data->fmt->colplanes;
+	for (i = 0; i < pix_mp->num_planes; i++) {
+		pix_mp->plane_fmt[i].bytesperline = q_data->bytesperline[i];
+		pix_mp->plane_fmt[i].sizeimage = q_data->sizeimage[i];
+	}
+
+	return 0;
+}
+
+static int mxc_jpeg_subscribe_event(struct v4l2_fh *fh,
+		const struct v4l2_event_subscription *sub)
+{
+	switch (sub->type) {
+	case V4L2_EVENT_EOS:
+		return v4l2_event_subscribe(fh, sub, 0, NULL);
+	case V4L2_EVENT_SOURCE_CHANGE:
+		return v4l2_src_change_event_subscribe(fh, sub);
+	default:
+		return -EINVAL;
+	}
+}
+
+static int mxc_jpeg_qbuf(struct file *file, void *priv,
+			 struct v4l2_buffer *buf)
+{
+	struct v4l2_fh *fh = file->private_data;
+	struct mxc_jpeg_ctx *ctx = mxc_jpeg_fh_to_ctx(priv);
+	struct device *dev = ctx->mxc_jpeg->dev;
+	struct vb2_queue *vq;
+
+	dev_dbg(dev, "QBUF type=%d, index=%d", buf->type, buf->index);
+	vq = v4l2_m2m_get_vq(fh->m2m_ctx, buf->type);
+	if (buf->index >= vq->num_buffers) {
+		dev_err(dev, "buffer index out of range\n");
+		return -EINVAL;
+	}
+
+	return v4l2_m2m_qbuf(file, fh->m2m_ctx, buf);
+}
+
+static int mxc_jpeg_dqbuf(struct file *file, void *priv,
+			  struct v4l2_buffer *buf)
+{
+	struct v4l2_fh *fh = file->private_data;
+	struct mxc_jpeg_ctx *ctx = mxc_jpeg_fh_to_ctx(priv);
+	struct device *dev = ctx->mxc_jpeg->dev;
+	int num_src_ready = v4l2_m2m_num_src_bufs_ready(fh->m2m_ctx);
+	int ret;
+
+	dev_dbg(dev, "DQBUF type=%d, index=%d", buf->type, buf->index);
+	if (ctx->stopping == 1	&& num_src_ready == 0) {
+		/* No more src bufs, notify app EOS */
+		notify_eos(ctx);
+		ctx->stopping = 0;
+	}
+
+	ret = v4l2_m2m_dqbuf(file, fh->m2m_ctx, buf);
+	buf->field = V4L2_FIELD_NONE;
+
+	return ret;
+}
+static const struct v4l2_ioctl_ops mxc_jpeg_ioctl_ops = {
+	.vidioc_querycap		= mxc_jpeg_querycap,
+	.vidioc_enum_fmt_vid_cap	= mxc_jpeg_enum_fmt_vid_cap,
+	.vidioc_enum_fmt_vid_out	= mxc_jpeg_enum_fmt_vid_out,
+
+	.vidioc_try_fmt_vid_cap_mplane		= mxc_jpeg_try_fmt_vid_cap,
+	.vidioc_try_fmt_vid_out_mplane		= mxc_jpeg_try_fmt_vid_out,
+
+	.vidioc_s_fmt_vid_cap_mplane		= mxc_jpeg_s_fmt_vid_cap,
+	.vidioc_s_fmt_vid_out_mplane		= mxc_jpeg_s_fmt_vid_out,
+
+	.vidioc_g_fmt_vid_cap_mplane		= mxc_jpeg_g_fmt_vid,
+	.vidioc_g_fmt_vid_out_mplane		= mxc_jpeg_g_fmt_vid,
+
+	.vidioc_subscribe_event		= mxc_jpeg_subscribe_event,
+	.vidioc_unsubscribe_event	= v4l2_event_unsubscribe,
+	.vidioc_try_decoder_cmd		= mxc_jpeg_try_decoder_cmd,
+	.vidioc_decoder_cmd		= mxc_jpeg_decoder_cmd,
+	.vidioc_try_encoder_cmd		= mxc_jpeg_try_encoder_cmd,
+	.vidioc_encoder_cmd		= mxc_jpeg_encoder_cmd,
+
+	.vidioc_qbuf			= mxc_jpeg_qbuf,
+	.vidioc_dqbuf                   = mxc_jpeg_dqbuf,
+
+	.vidioc_create_bufs		= v4l2_m2m_ioctl_create_bufs,
+	.vidioc_prepare_buf		= v4l2_m2m_ioctl_prepare_buf,
+	.vidioc_reqbufs                 = v4l2_m2m_ioctl_reqbufs,
+	.vidioc_querybuf                = v4l2_m2m_ioctl_querybuf,
+	.vidioc_expbuf                  = v4l2_m2m_ioctl_expbuf,
+	.vidioc_streamon                = v4l2_m2m_ioctl_streamon,
+	.vidioc_streamoff               = v4l2_m2m_ioctl_streamoff,
+};
+
+static int mxc_jpeg_release(struct file *file)
+{
+	struct mxc_jpeg_dev *mxc_jpeg = video_drvdata(file);
+	struct mxc_jpeg_ctx *ctx = mxc_jpeg_fh_to_ctx(file->private_data);
+	struct device *dev = mxc_jpeg->dev;
+
+	mutex_lock(&mxc_jpeg->lock);
+	if (mxc_jpeg->mode == MXC_JPEG_DECODE)
+		dev_dbg(dev, "Release JPEG decoder instance on slot %d.",
+			ctx->slot);
+	else
+		dev_dbg(dev, "Release JPEG encoder instance on slot %d.",
+			ctx->slot);
+	v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
+	v4l2_fh_del(&ctx->fh);
+	v4l2_fh_exit(&ctx->fh);
+	kfree(ctx);
+	mutex_unlock(&mxc_jpeg->lock);
+
+	pm_runtime_put_sync(mxc_jpeg->dev);
+	return 0;
+}
+
+static const struct v4l2_file_operations mxc_jpeg_fops = {
+	.owner		= THIS_MODULE,
+	.open		= mxc_jpeg_open,
+	.release	= mxc_jpeg_release,
+	.poll		= v4l2_m2m_fop_poll,
+	.unlocked_ioctl	= video_ioctl2,
+	.mmap		= v4l2_m2m_fop_mmap,
+};
+static struct v4l2_m2m_ops mxc_jpeg_m2m_ops = {
+	.device_run	= mxc_jpeg_device_run,
+	.job_ready	= mxc_jpeg_job_ready,
+	.job_abort	= mxc_jpeg_job_abort,
+};
+
+static void mxc_jpeg_detach_pm_domains(struct mxc_jpeg_dev *jpeg)
+{
+	int i;
+
+	for (i = 0; i < MXC_JPEG_NUM_PD; i++) {
+		if (jpeg->pd_link[i] && !IS_ERR(jpeg->pd_link[i]))
+			device_link_del(jpeg->pd_link[i]);
+		if (jpeg->pd_dev[i] && !IS_ERR(jpeg->pd_dev[i]))
+			dev_pm_domain_detach(jpeg->pd_dev[i], true);
+		jpeg->pd_dev[i] = NULL;
+		jpeg->pd_link[i] = NULL;
+	}
+}
+
+static int mxc_jpeg_attach_pm_domains(struct mxc_jpeg_dev *jpeg)
+{
+	struct device *dev = jpeg->dev;
+	struct device_node *np = jpeg->pdev->dev.of_node;
+	int i, num_domains;
+	int ret;
+
+	num_domains = of_count_phandle_with_args(np, "power-domains",
+						 "#power-domain-cells");
+	if (num_domains != MXC_JPEG_NUM_PD) {
+		dev_err(dev, "Expecting %d power domains, got %d\n",
+			MXC_JPEG_NUM_PD, num_domains);
+		return -EINVAL;
+	}
+
+	for (i = 0; i < MXC_JPEG_NUM_PD; i++) {
+		jpeg->pd_dev[i] = dev_pm_domain_attach_by_id(dev, i);
+		if (IS_ERR(jpeg->pd_dev[i])) {
+			ret = PTR_ERR(jpeg->pd_dev[i]);
+			goto fail;
+		}
+
+		jpeg->pd_link[i] = device_link_add(dev, jpeg->pd_dev[i],
+			DL_FLAG_STATELESS |
+			DL_FLAG_PM_RUNTIME |
+			DL_FLAG_RPM_ACTIVE);
+		if (IS_ERR(jpeg->pd_link[i])) {
+			ret = PTR_ERR(jpeg->pd_link[i]);
+			goto fail;
+		}
+	}
+
+	return 0;
+fail:
+	mxc_jpeg_detach_pm_domains(jpeg);
+	return ret;
+}
+
+static int mxc_jpeg_probe(struct platform_device *pdev)
+{
+	struct mxc_jpeg_dev *jpeg;
+	struct device *dev = &pdev->dev;
+	struct resource *res;
+	int dec_irq;
+	int ret;
+	int mode;
+	const struct of_device_id *of_id;
+	unsigned int slot;
+
+	of_id = of_match_node(mxc_jpeg_match, dev->of_node);
+	mode = (int)(u64) of_id->data;
+
+	jpeg = devm_kzalloc(dev, sizeof(struct mxc_jpeg_dev), GFP_KERNEL);
+	if (!jpeg)
+		return -ENOMEM;
+
+	mutex_init(&jpeg->lock);
+	spin_lock_init(&jpeg->hw_lock);
+
+	ret = dma_set_mask_and_coherent(dev, DMA_BIT_MASK(32));
+	if (ret) {
+		dev_err(&pdev->dev, "No suitable DMA available.\n");
+		goto err_irq;
+	}
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	jpeg->base_reg = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(jpeg->base_reg))
+		return PTR_ERR(jpeg->base_reg);
+
+	for (slot = 0; slot < MXC_MAX_SLOTS; slot++) {
+		dec_irq = platform_get_irq(pdev, slot);
+		if (dec_irq < 0) {
+			dev_err(&pdev->dev, "Failed to get irq %d\n", dec_irq);
+			ret = dec_irq;
+			goto err_irq;
+		}
+		ret = devm_request_irq(&pdev->dev, dec_irq, mxc_jpeg_dec_irq,
+				       0, pdev->name, jpeg);
+		if (ret) {
+			dev_err(&pdev->dev, "Failed to request irq %d (%d)\n",
+				dec_irq, ret);
+			goto err_irq;
+		}
+	}
+
+	jpeg->pdev = pdev;
+	jpeg->dev = dev;
+	jpeg->mode = mode;
+
+	/* Start clock */
+	jpeg->clk_ipg = devm_clk_get(dev, "ipg");
+	if (IS_ERR(jpeg->clk_ipg)) {
+		dev_err(dev, "failed to get clock: ipg\n");
+		goto err_clk;
+	}
+
+	jpeg->clk_per = devm_clk_get(dev, "per");
+	if (IS_ERR(jpeg->clk_per)) {
+		dev_err(dev, "failed to get clock: per\n");
+		goto err_clk;
+	}
+
+	ret = mxc_jpeg_attach_pm_domains(jpeg);
+	if (ret < 0) {
+		dev_err(dev, "failed to attach power domains %d\n", ret);
+		return ret;
+	}
+
+	/* v4l2 */
+	ret = v4l2_device_register(dev, &jpeg->v4l2_dev);
+	if (ret) {
+		dev_err(dev, "failed to register v4l2 device\n");
+		goto err_register;
+	}
+	jpeg->m2m_dev = v4l2_m2m_init(&mxc_jpeg_m2m_ops);
+	if (IS_ERR(jpeg->m2m_dev)) {
+		dev_err(dev, "failed to register v4l2 device\n");
+		goto err_m2m;
+	}
+
+	jpeg->dec_vdev = video_device_alloc();
+	if (!jpeg->dec_vdev) {
+		dev_err(dev, "failed to register v4l2 device\n");
+		goto err_vdev_alloc;
+	}
+	if (mode == MXC_JPEG_ENCODE)
+		snprintf(jpeg->dec_vdev->name,
+				sizeof(jpeg->dec_vdev->name),
+				"%s-enc", MXC_JPEG_M2M_NAME);
+	else
+		snprintf(jpeg->dec_vdev->name,
+				sizeof(jpeg->dec_vdev->name),
+				"%s-dec", MXC_JPEG_M2M_NAME);
+
+	jpeg->dec_vdev->fops = &mxc_jpeg_fops;
+	jpeg->dec_vdev->ioctl_ops = &mxc_jpeg_ioctl_ops;
+	jpeg->dec_vdev->minor = -1;
+	jpeg->dec_vdev->release = video_device_release;
+	jpeg->dec_vdev->lock = &jpeg->lock; /* lock for ioctl serialization*/
+	jpeg->dec_vdev->v4l2_dev = &jpeg->v4l2_dev;
+	jpeg->dec_vdev->vfl_dir = VFL_DIR_M2M;
+	jpeg->dec_vdev->device_caps = V4L2_CAP_STREAMING |
+					V4L2_CAP_VIDEO_M2M_MPLANE;
+
+	ret = video_register_device(jpeg->dec_vdev, VFL_TYPE_VIDEO, -1);
+	if (ret) {
+		dev_err(dev, "failed to register video device\n");
+		goto err_vdev_register;
+	}
+	video_set_drvdata(jpeg->dec_vdev, jpeg);
+	if (mode == MXC_JPEG_ENCODE)
+		v4l2_info(&jpeg->v4l2_dev,
+			"encoder device registered as /dev/video%d (%d,%d)\n",
+			jpeg->dec_vdev->num, VIDEO_MAJOR,
+			jpeg->dec_vdev->minor);
+	else
+		v4l2_info(&jpeg->v4l2_dev,
+			"decoder device registered as /dev/video%d (%d,%d)\n",
+			jpeg->dec_vdev->num, VIDEO_MAJOR,
+			jpeg->dec_vdev->minor);
+
+	platform_set_drvdata(pdev, jpeg);
+	pm_runtime_enable(dev);
+	return 0;
+
+err_vdev_register:
+	video_device_release(jpeg->dec_vdev);
+
+err_vdev_alloc:
+	v4l2_m2m_release(jpeg->m2m_dev);
+
+err_m2m:
+	v4l2_device_unregister(&jpeg->v4l2_dev);
+
+err_register:
+err_irq:
+err_clk:
+	return ret;
+}
+
+#ifdef CONFIG_PM
+static int mxc_jpeg_runtime_resume(struct device *dev)
+{
+	struct mxc_jpeg_dev *jpeg = dev_get_drvdata(dev);
+	int ret;
+
+	ret = clk_prepare_enable(jpeg->clk_ipg);
+	if (ret < 0) {
+		dev_err(dev, "failed to enable clock: ipg\n");
+		goto err_clk;
+	}
+
+	ret = clk_prepare_enable(jpeg->clk_per);
+	if (ret < 0) {
+		dev_err(dev, "failed to enable clock: per\n");
+		goto err_clk;
+	}
+
+	return 0;
+
+err_clk:
+	return ret;
+}
+
+static int mxc_jpeg_runtime_suspend(struct device *dev)
+{
+	struct mxc_jpeg_dev *jpeg = dev_get_drvdata(dev);
+
+	clk_disable_unprepare(jpeg->clk_ipg);
+	clk_disable_unprepare(jpeg->clk_per);
+
+	return 0;
+}
+#endif
+
+static const struct dev_pm_ops	mxc_jpeg_pm_ops = {
+	SET_RUNTIME_PM_OPS(mxc_jpeg_runtime_suspend,
+			   mxc_jpeg_runtime_resume, NULL)
+};
+
+static int mxc_jpeg_remove(struct platform_device *pdev)
+{
+	unsigned int slot;
+	struct mxc_jpeg_dev *jpeg = platform_get_drvdata(pdev);
+
+	for (slot = 0; slot < MXC_MAX_SLOTS; slot++)
+		mxc_jpeg_free_slot_data(jpeg, slot);
+
+	pm_runtime_disable(&pdev->dev);
+	video_unregister_device(jpeg->dec_vdev);
+	video_device_release(jpeg->dec_vdev);
+	v4l2_m2m_release(jpeg->m2m_dev);
+	v4l2_device_unregister(&jpeg->v4l2_dev);
+	mxc_jpeg_detach_pm_domains(jpeg);
+
+	return 0;
+}
+
+MODULE_DEVICE_TABLE(of, mxc_jpeg_match);
+
+static struct platform_driver mxc_jpeg_driver = {
+	.probe = mxc_jpeg_probe,
+	.remove = mxc_jpeg_remove,
+	.driver = {
+		.name = "mxc-jpeg",
+		.of_match_table = mxc_jpeg_match,
+		.pm = &mxc_jpeg_pm_ops,
+	},
+};
+module_platform_driver(mxc_jpeg_driver);
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/platform/imx8/mxc-jpeg.h b/drivers/media/platform/imx8/mxc-jpeg.h
new file mode 100644
index 000000000..35db0ca84
--- /dev/null
+++ b/drivers/media/platform/imx8/mxc-jpeg.h
@@ -0,0 +1,170 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * i.MX8QXP/i.MX8QM JPEG encoder/decoder v4l2 driver
+ *
+ * Copyright 2018-2019 NXP
+ */
+
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-fh.h>
+
+#ifndef _MXC_JPEG_CORE_H
+#define _MXC_JPEG_CORE_H
+
+#define MXC_JPEG_M2M_NAME		"mxc-jpeg"
+#define MXC_JPEG_NAME			"mxc-jpeg"
+#define MXC_IN_FORMAT			0
+#define MXC_OUT_FORMAT			1
+#define MXC_JPEG_FMT_TYPE_ENC		0
+#define MXC_JPEG_FMT_TYPE_RAW		1
+#define MXC_JPEG_DEFAULT_WIDTH		1280
+#define MXC_JPEG_DEFAULT_HEIGHT		720
+#define MXC_JPEG_DEFAULT_PFMT		V4L2_PIX_FMT_RGB24
+#define MXC_JPEG_MIN_WIDTH		64
+#define MXC_JPEG_MIN_HEIGHT		64
+#define MXC_JPEG_MAX_WIDTH		0x2000
+#define MXC_JPEG_MAX_HEIGHT		0x2000
+#define MXC_JPEG_MAX_CFG_STREAM		0x1000
+#define MXC_JPEG_H_ALIGN		3
+#define MXC_JPEG_W_ALIGN		3
+#define MXC_JPEG_DEFAULT_SIZEIMAGE	(6 * MXC_JPEG_DEFAULT_WIDTH * \
+					 MXC_JPEG_DEFAULT_HEIGHT)
+#define MXC_JPEG_MAX_SIZEIMAGE		0xFFFFFC00
+#define MXC_JPEG_ENC_CONF		1
+#define MXC_JPEG_ENC_DONE		0
+#define SOF0				0xC0
+#define SOF1				0xC1
+#define SOF2				0xC2
+#define SOS				0xDA
+#define DHT				0xC4
+#define APP14				0xEE
+#define MXC_JPEG_ENC_CONF_DONE		1
+#define MXC_JPEG_MAX_PLANES		2
+#define MXC_JPEG_NUM_PD			6
+
+
+/**
+ * struct jpeg_fmt - driver's internal color format data
+ * @name:	format description
+ * @fourcc:	fourcc code, 0 if not applicable
+ * @depth:	number of bits per pixel
+ * @colplanes:	number of color planes (1 for packed formats)
+ * @h_align:	horizontal alignment order (align to 2^h_align)
+ * @v_align:	vertical alignment order (align to 2^v_align)
+ * @flags:	flags describing format applicability
+ */
+struct mxc_jpeg_fmt {
+	char	*name;
+	u32	fourcc;
+	int	depth;
+	int	colplanes;
+	int	memplanes;
+	int	h_align;
+	int	v_align;
+	int	subsampling;
+	u32	flags;
+};
+struct mxc_jpeg_desc {
+	u32 next_descpt_ptr;
+	u32 buf_base0;
+	u32 buf_base1;
+	u32 line_pitch;
+	u32 stm_bufbase;
+	u32 stm_bufsize;
+	u32 imgsize;
+	u32 stm_ctrl;
+} __packed;
+
+struct mxc_jpeg_q_data {
+	struct mxc_jpeg_fmt	*fmt;
+	u32			sizeimage[MXC_JPEG_MAX_PLANES];
+	u32			bytesperline[MXC_JPEG_MAX_PLANES];
+	int w;
+	int w_adjusted;
+	int h;
+	int h_adjusted;
+};
+struct mxc_jpeg_ctx {
+	struct mxc_jpeg_dev		*mxc_jpeg;
+	struct mxc_jpeg_q_data		out_q;
+	struct mxc_jpeg_q_data		cap_q;
+	struct v4l2_rect		crop_rect;
+	unsigned long			state;
+	struct v4l2_fh			fh;
+	unsigned int			mode;
+	unsigned int			enc_state;
+	unsigned int			aborting;
+	unsigned int			stopping;
+	unsigned int			dht_needed;
+	unsigned int			slot;
+	enum v4l2_colorspace colorspace;
+	enum v4l2_ycbcr_encoding ycbcr_enc;
+	enum v4l2_quantization quantization;
+	enum v4l2_xfer_func xfer_func;
+};
+
+struct mxc_jpeg_slot_data {
+	bool used;
+	struct mxc_jpeg_desc *desc; // enc/dec descriptor
+	struct mxc_jpeg_desc *cfg_desc; // configuration descriptor
+	void *cfg_stream_vaddr; // configuration bitstream virtual address
+	unsigned int cfg_stream_size;
+	int flags;
+	dma_addr_t desc_handle;
+	dma_addr_t cfg_desc_handle; // configuration descriptor dma address
+	dma_addr_t cfg_stream_handle; // configuration bitstream dma address
+};
+
+struct mxc_jpeg_dev {
+	spinlock_t			hw_lock;
+	unsigned int			mode;
+	struct mutex			lock;
+	bool				enc;
+	bool				dec;
+	struct clk			*clk_ipg;
+	struct clk			*clk_per;
+	struct platform_device		*pdev;
+	struct device			*dev;
+	void __iomem			*base_reg;
+	void __iomem			*enc_reg;
+	struct v4l2_device		v4l2_dev;
+	struct v4l2_m2m_dev		*m2m_dev;
+	struct video_device		*dec_vdev;
+	unsigned int			irq;
+	int				id;
+	struct mxc_jpeg_slot_data	slot_data[MXC_MAX_SLOTS];
+	struct device			*pd_dev[MXC_JPEG_NUM_PD];
+	struct device_link		*pd_link[MXC_JPEG_NUM_PD];
+};
+
+#define MXC_JPEG_MAX_COMPONENTS 4
+/* JPEG Start Of Frame marker fields*/
+struct mxc_jpeg_sof_comp {
+	u8 id; /*component id*/
+	u8 v :4; /* vertical sampling*/
+	u8 h :4; /* horizontal sampling*/
+	u8 quantization_table_no;
+} __packed;
+
+struct mxc_jpeg_sof {
+	u16 length;
+	u8 precision;
+	u16 height, width;
+	u8 components_no;
+	struct mxc_jpeg_sof_comp comp[MXC_JPEG_MAX_COMPONENTS];
+} __packed;
+
+/* JPEG Start Of Scan marker fields*/
+struct mxc_jpeg_sos_comp {
+	u8 id; /*component id*/
+	u8 huffman_table_no;
+} __packed;
+struct mxc_jpeg_sos {
+	u16 length;
+	u8 components_no;
+	struct mxc_jpeg_sos_comp comp[MXC_JPEG_MAX_COMPONENTS];
+	u8 ignorable_bytes[3];
+} __packed;
+
+#endif
diff --git a/drivers/media/platform/imx8/mxc-mipi-csi2.h b/drivers/media/platform/imx8/mxc-mipi-csi2.h
new file mode 100644
index 000000000..cc53325b7
--- /dev/null
+++ b/drivers/media/platform/imx8/mxc-mipi-csi2.h
@@ -0,0 +1,272 @@
+/* SPDX-License-Identifier: GPL-2.0-or-later */
+/*
+ * Copyright 2017 NXP
+ */
+
+#ifndef MXC_MIPI_CSI2_H_
+#define MXC_MIPI_CSI2_H_
+
+#include <media/v4l2-device.h>
+
+#define MXC_MIPI_CSI2_DRIVER_NAME	"mxc-mipi-csi2"
+#define MXC_MIPI_CSI2_SUBDEV_NAME	MXC_MIPI_CSI2_DRIVER_NAME
+#define MXC_MIPI_CSI2_MAX_DEVS		2
+#define MXC_MIPI_CSI2_MAX_LANES		4
+
+#define MIPI_CSI2_OF_NODE_NAME	"csi"
+
+#define MXC_MIPI_CSI2_VC0_PAD_SINK		0
+#define MXC_MIPI_CSI2_VC1_PAD_SINK		1
+#define MXC_MIPI_CSI2_VC2_PAD_SINK		2
+#define MXC_MIPI_CSI2_VC3_PAD_SINK		3
+
+#define MXC_MIPI_CSI2_VC0_PAD_SOURCE	4
+#define MXC_MIPI_CSI2_VC1_PAD_SOURCE	5
+#define MXC_MIPI_CSI2_VC2_PAD_SOURCE	6
+#define MXC_MIPI_CSI2_VC3_PAD_SOURCE	7
+#define MXC_MIPI_CSI2_VCX_PADS_NUM		8
+
+/* Subsystem CSR */
+#define CSI2SS_BASE_OFFSET					0x0
+
+#define CSI2SS_PLM_CTRL						(CSI2SS_BASE_OFFSET + 0x0)
+#define CSI2SS_PLM_CTRL_PL_CLK_RUN			0x80000000
+#define CSI2SS_PLM_CTRL_VSYNC_OVERRIDE		0x200
+#define CSI2SS_PLM_CTRL_HSYNC_OVERRIDE		0x400
+#define CSI2SS_PLM_CTRL_VALID_OVERRIDE		0x800
+#define CSI2SS_PLM_CTRL_POLARITY_MASK		0x1000
+#define CSI2SS_PLM_CTRL_POLARITY_HIGH		0x1000
+#define CSI2SS_PLM_CTRL_POLARITY_LOW		0x0
+#define CSI2SS_PLM_CTRL_ENABLE_PL			1
+#define CSI2SS_PLM_CTRL_ENABLE_PL_OFFSET	0
+#define CSI2SS_PLM_CTRL_ENABLE_PL_MASK		1
+
+#define CSI2SS_PHY_CTRL						(CSI2SS_BASE_OFFSET + 0x4)
+#define CSI2SS_PHY_CTRL_PD					1
+#define CSI2SS_PHY_CTRL_PD_OFFSET			22
+#define CSI2SS_PHY_CTRL_PD_MASK				0x400000
+#define CSI2SS_PHY_CTRL_RTERM_SEL			1
+#define CSI2SS_PHY_CTRL_RTERM_SEL_OFFSET	21
+#define CSI2SS_PHY_CTRL_RTERM_SEL_MASK		0x200000
+#define CSI2SS_PHY_CTRL_RX_HS_SETTLE_OFFSET	4
+#define CSI2SS_PHY_CTRL_RX_HS_SETTLE_MASK	0x3F0
+#define CSI2SS_PHY_CTRL_CONT_CLK_MODE		1
+#define CSI2SS_PHY_CTRL_CONT_CLK_MODE_OFFSET	3
+#define CSI2SS_PHY_CTRL_CONT_CLK_MODE_MASK	0x8
+#define CSI2SS_PHY_CTRL_DDRCLK_EN			1
+#define CSI2SS_PHY_CTRL_DDRCLK_EN_OFFSET	2
+#define CSI2SS_PHY_CTRL_DDRCLK_EN_MASK		0x4
+#define CSI2SS_PHY_CTRL_AUTO_PD_EN			1
+#define CSI2SS_PHY_CTRL_AUTO_PD_EN_OFFSET	1
+#define CSI2SS_PHY_CTRL_AUTO_PD_EN_MASK		0x2
+#define CSI2SS_PHY_CTRL_RX_ENABLE			1
+#define CSI2SS_PHY_CTRL_RX_ENABLE_OFFSET	0
+#define CSI2SS_PHY_CTRL_RX_ENABLE_MASK		0x1
+
+#define CSI2SS_PHY_STATUS					(CSI2SS_BASE_OFFSET + 0x8)
+#define CSI2SS_PHY_TEST_STATUS				(CSI2SS_BASE_OFFSET + 0x10)
+#define CSI2SS_PHY_TEST_STATUS_D0			(CSI2SS_BASE_OFFSET + 0x14)
+#define CSI2SS_PHY_TEST_STATUS_D1			(CSI2SS_BASE_OFFSET + 0x18)
+#define CSI2SS_PHY_TEST_STATUS_D2			(CSI2SS_BASE_OFFSET + 0x1C)
+#define CSI2SS_PHY_TEST_STATUS_D3			(CSI2SS_BASE_OFFSET + 0x20)
+
+#define CSI2SS_VC_INTERLACED				(CSI2SS_BASE_OFFSET + 0x30)
+#define CSI2SS_VC_INTERLACED_VC0			1
+#define CSI2SS_VC_INTERLACED_VC1			2
+#define CSI2SS_VC_INTERLACED_VC2			4
+#define CSI2SS_VC_INTERLACED_VC3			8
+#define CSI2SS_VC_INTERLACED_OFFSET			0
+#define CSI2SS_VC_INTERLACED_MASK			0xF
+
+#define CSI2SS_DATA_TYPE						(CSI2SS_BASE_OFFSET + 0x38)
+#define CSI2SS_DATA_TYPE_LEGACY_YUV420_8BIT		(1 << 2)
+#define CSI2SS_DATA_TYPE_YUV422_8BIT       		(1 << 6)
+#define CSI2SS_DATA_TYPE_YUV422_10BIT      		(1 << 7)
+#define CSI2SS_DATA_TYPE_RGB444					(1 << 8)
+#define CSI2SS_DATA_TYPE_RGB555					(1 << 9)
+#define CSI2SS_DATA_TYPE_RGB565					(1 << 10)
+#define CSI2SS_DATA_TYPE_RGB666					(1 << 11)
+#define CSI2SS_DATA_TYPE_RGB888					(1 << 12)
+#define CSI2SS_DATA_TYPE_RAW6					(1 << 16)
+#define CSI2SS_DATA_TYPE_RAW8					(1 << 18)
+#define CSI2SS_DATA_TYPE_RAW10					(1 << 19)
+#define CSI2SS_DATA_TYPE_RAW12					(1 << 20)
+#define CSI2SS_DATA_TYPE_RAW14					(1 << 21)
+
+#define CSI2SS_YUV420_1ST_LINE_DATA_TYPE		(CSI2SS_BASE_OFFSET + 0x40)
+#define CSI2SS_YUV420_1ST_LINE_DATA_TYPE_ODD	0
+#define CSI2SS_YUV420_1ST_LINE_DATA_TYPE_EVEN	1
+#define CSI2SS_YUV420_1ST_LINE_DATA_TYPE_OFFSET	0
+#define CSI2SS_YUV420_1ST_LINE_DATA_TYPE_MASK	1
+
+#define CSI2SS_CTRL_CLK_RESET					(CSI2SS_BASE_OFFSET + 0x44)
+#define CSI2SS_CTRL_CLK_RESET_EN				1
+#define CSI2SS_CTRL_CLK_RESET_OFFSET			0
+#define CSI2SS_CTRL_CLK_RESET_MASK				1
+#define CSI2SS_CTRL_CLK_RESET_CLK_OFF			1
+#define CSI2SS_CTRL_CLK_RESET_CLK_OFFSET		1
+#define CSI2SS_CTRL_CLK_RESET_CLK_MASK			0x1
+
+#define CSI2SS_STREAM_FENCE_CTRL				(CSI2SS_BASE_OFFSET + 0x48)
+#define CSI2SS_STREAM_FENCE_VC0					1
+#define CSI2SS_STREAM_FENCE_VC1					2
+#define CSI2SS_STREAM_FENCE_VC2					4
+#define CSI2SS_STREAM_FENCE_VC3					8
+#define CSI2SS_STREAM_FENCE_CTRL_OFFSET			0
+#define CSI2SS_STREAM_FENCE_CTRL_MASK			0xF
+
+#define CSI2SS_STREAM_FENCE_STATUS				(CSI2SS_BASE_OFFSET + 0x4C)
+
+/* CSI-2 controller CSR */
+#define CSI2RX_BASE_OFFSET						(0x100)
+
+#define CSI2RX_CFG_NUM_LANES					(CSI2RX_BASE_OFFSET + 0x0)
+#define CSI2RX_CFG_NUM_LANES_OFFSET				0
+#define CSI2RX_CFG_NUM_LANES_MASK				0x3
+
+#define CSI2RX_CFG_DISABLE_DATA_LANES			(CSI2RX_BASE_OFFSET + 0x4)
+#define CSI2RX_CFG_DISABLE_DATA_LANES_3			8
+#define CSI2RX_CFG_DISABLE_DATA_LANES_2			4
+#define CSI2RX_CFG_DISABLE_DATA_LANES_1			2
+#define CSI2RX_CFG_DISABLE_DATA_LANES_0			1
+#define CSI2RX_CFG_DISABLE_DATA_LANES_OFFSET	0
+#define CSI2RX_CFG_DISABLE_DATA_LANES_MASK		0xF
+
+#define CSI2RX_BIT_ERR								(CSI2RX_BASE_OFFSET + 0x8)
+
+#define CSI2RX_IRQ_STATUS							(CSI2RX_BASE_OFFSET + 0xC)
+#define CSI2RX_IRQ_STATUS_CRC_ERROR					0x1
+#define CSI2RX_IRQ_STATUS_1BIT_CRC_ERROR			0x2
+#define CSI2RX_IRQ_STATUS_2BIT_CRC_ERROR			0x4
+#define CSI2RX_IRQ_STATUS_ULPS_CHANGE				0x8
+#define CSI2RX_IRQ_STATUS_DPHY_ERRSOTHS				0x10
+#define CSI2RX_IRQ_STATUS_DPHY_ERRSOTSYNC_HS		0x20
+#define CSI2RX_IRQ_STATUS_DPHY_ERRESC				0x40
+#define CSI2RX_IRQ_STATUS_DPHY_ERRSYNCESC			0x80
+#define CSI2RX_IRQ_STATUS_DPHY_ERRCTRL				0x100
+
+#define CSI2RX_IRQ_MASK								(CSI2RX_BASE_OFFSET + 0x10)
+#define CSI2RX_IRQ_MASK_CRC_ERROR					0x1
+#define CSI2RX_IRQ_MASK_1BIT_CRC_ERROR				0x2
+#define CSI2RX_IRQ_MASK_2BIT_CRC_ERROR				0x4
+#define CSI2RX_IRQ_MASK_ULPS_CHANGE					0x8
+#define CSI2RX_IRQ_MASK_DPHY_ERRSOTHS				0x10
+#define CSI2RX_IRQ_MASK_DPHY_ERRSOTSYNC_HS			0x20
+#define CSI2RX_IRQ_MASK_DPHY_ERRESC					0x40
+#define CSI2RX_IRQ_MASK_DPHY_ERRSYNCESC				0x80
+#define CSI2RX_IRQ_MASK_DPHY_ERRCTRL				0x100
+
+#define CSI2RX_ULPS_STATUS							(CSI2RX_BASE_OFFSET + 0x14)
+#define CSI2RX_ULPS_STATUS_CLK_LANE_ULPS			0x1
+#define CSI2RX_ULPS_STATUS_DAT_LANE0_ULPS			0x2
+#define CSI2RX_ULPS_STATUS_DAT_LANE1_ULPS			0x4
+#define CSI2RX_ULPS_STATUS_DAT_LANE2_ULPS			0x8
+#define CSI2RX_ULPS_STATUS_DAT_LANE3_ULPS			0x10
+#define CSI2RX_ULPS_STATUS_CLK_LANE_MARK			0x20
+#define CSI2RX_ULPS_STATUS_DAT_LANE0_MARK			0x40
+#define CSI2RX_ULPS_STATUS_DAT_LANE1_MARK			0x80
+#define CSI2RX_ULPS_STATUS_DAT_LANE2_MARK			0x100
+#define CSI2RX_ULPS_STATUS_DAT_LANE3_MARK			0x200
+
+#define CSI2RX_PPI_ERRSOT_HS						(CSI2RX_BASE_OFFSET + 0x18)
+#define CSI2RX_PPI_ERRSOT_HS_DAT_LANE0				0x1
+#define CSI2RX_PPI_ERRSOT_HS_DAT_LANE1				0x2
+#define CSI2RX_PPI_ERRSOT_HS_DAT_LANE2				0x4
+#define CSI2RX_PPI_ERRSOT_HS_DAT_LANE3				0x8
+
+#define CSI2RX_PPI_ERRSOTSYNC_HS					(CSI2RX_BASE_OFFSET + 0x1C)
+#define CSI2RX_PPI_ERRSOTSYNC_HS_DAT_LANE0			0x1
+#define CSI2RX_PPI_ERRSOTSYNC_HS_DAT_LANE1			0x2
+#define CSI2RX_PPI_ERRSOTSYNC_HS_DAT_LANE2			0x4
+#define CSI2RX_PPI_ERRSOTSYNC_HS_DAT_LANE3			0x8
+
+#define CSI2RX_PPI_ERRESC				   		 	(CSI2RX_BASE_OFFSET + 0x20)
+#define CSI2RX_PPI_ERRESC_DAT_LANE0		   		 	0x1
+#define CSI2RX_PPI_ERRESC_DAT_LANE1		   		 	0x2
+#define CSI2RX_PPI_ERRESC_DAT_LANE2		   		 	0x4
+#define CSI2RX_PPI_ERRESC_DAT_LANE3		   		 	0x8
+
+#define CSI2RX_PPI_ERRSYNCESC			   		 	(CSI2RX_BASE_OFFSET + 0x24)
+#define CSI2RX_PPI_ERRSYNCESC_DAT_LANE0	   		 	0x1
+#define CSI2RX_PPI_ERRSYNCESC_DAT_LANE1	   		 	0x2
+#define CSI2RX_PPI_ERRSYNCESC_DAT_LANE2	   		 	0x4
+#define CSI2RX_PPI_ERRSYNCESC_DAT_LANE3	   		 	0x8
+
+#define CSI2RX_PPI_ERRCONTROL			   		 	(CSI2RX_BASE_OFFSET + 0x28)
+#define CSI2RX_PPI_ERRCONTROL_DAT_LANE0	   		 	0x1
+#define CSI2RX_PPI_ERRCONTROL_DAT_LANE1	   		 	0x2
+#define CSI2RX_PPI_ERRCONTROL_DAT_LANE2	   		 	0x4
+#define CSI2RX_PPI_ERRCONTROL_DAT_LANE3	   		 	0x8
+
+#define CSI2RX_CFG_DISABLE_PAYLOAD_0							(CSI2RX_BASE_OFFSET + 0x2C)
+#define CSI2RX_CFG_DISABLE_PAYLOAD_TYPE_LEGACY_YUV420_8BIT		(1 << 10)
+#define CSI2RX_CFG_DISABLE_PAYLOAD_TYPE_YUV422_8BIT				(1 << 14)
+#define CSI2RX_CFG_DISABLE_PAYLOAD_TYPE_YUV422_10BIT			(1 << 15)
+#define CSI2RX_CFG_DISABLE_PAYLOAD_TYPE_RGB444					(1 << 16)
+#define CSI2RX_CFG_DISABLE_PAYLOAD_TYPE_RGB555					(1 << 17)
+#define CSI2RX_CFG_DISABLE_PAYLOAD_TYPE_RGB565					(1 << 18)
+#define CSI2RX_CFG_DISABLE_PAYLOAD_TYPE_RGB666					(1 << 19)
+#define CSI2RX_CFG_DISABLE_PAYLOAD_TYPE_RGB888					(1 << 20)
+#define CSI2RX_CFG_DISABLE_PAYLOAD_TYPE_RAW6					(1 << 24)
+#define CSI2RX_CFG_DISABLE_PAYLOAD_TYPE_RAW7					(1 << 25)
+#define CSI2RX_CFG_DISABLE_PAYLOAD_TYPE_RAW8					(1 << 26)
+#define CSI2RX_CFG_DISABLE_PAYLOAD_TYPE_RAW10					(1 << 27)
+#define CSI2RX_CFG_DISABLE_PAYLOAD_TYPE_RAW12					(1 << 28)
+#define CSI2RX_CFG_DISABLE_PAYLOAD_TYPE_RAW14					(1 << 29)
+
+#define CSI2RX_CFG_DISABLE_PAYLOAD_1	(CSI2RX_BASE_OFFSET + 0x30)
+
+struct csis_hw_reset {
+	struct regmap *src;
+	u8 req_src;
+	u8 rst_val;
+};
+struct csis_phy_gpr {
+	struct regmap *gpr;
+	u8 req_src;
+};
+
+struct mxc_mipi_csi2_dev {
+	struct v4l2_device		v4l2_dev;
+	struct v4l2_subdev		sd;
+	struct v4l2_subdev		*sensor_sd;
+
+	struct media_pad pads[MXC_MIPI_CSI2_VCX_PADS_NUM];
+	struct v4l2_mbus_framefmt format;
+
+	void __iomem *csr_regs;
+	void __iomem *base_regs;
+	struct platform_device *pdev;
+	u32 flags;
+	int irq;
+
+	struct clk *clk_apb;
+	struct clk *clk_core;
+	struct clk *clk_esc;
+	struct clk *clk_pxl;
+
+	struct csis_hw_reset		hw_reset;
+	struct csis_phy_gpr		phy_gpr;
+
+	struct v4l2_async_subdev	asd;
+	struct v4l2_async_notifier	subdev_notifier;
+	struct v4l2_async_subdev	*async_subdevs[2];
+
+	struct mutex lock;
+
+	int	 id;
+	u32 hs_settle;
+	u32 send_level;
+	u32 num_lanes;
+	u8 data_lanes[4];
+	u8 vchannel;
+	u8 running;
+};
+
+enum mxc_mipi_csi2_pm_state {
+	MXC_MIPI_CSI2_PM_POWERED	= 0x1,
+	MXC_MIPI_CSI2_PM_SUSPENDED	= 0x2,
+	MXC_MIPI_CSI2_RUNTIME_SUSPENDED	= 0x4,
+};
+
+#endif
diff --git a/drivers/media/platform/imx8/mxc-mipi-csi2_yav.c b/drivers/media/platform/imx8/mxc-mipi-csi2_yav.c
new file mode 100644
index 000000000..090a0c465
--- /dev/null
+++ b/drivers/media/platform/imx8/mxc-mipi-csi2_yav.c
@@ -0,0 +1,734 @@
+/* SPDX-License-Identifier: GPL-2.0-or-later */
+/*
+ * Copyright 2017 NXP
+ */
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/errno.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/irq.h>
+#include <linux/kernel.h>
+#include <linux/memory.h>
+#include <linux/mfd/syscon.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_graph.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/regmap.h>
+#include <linux/regulator/consumer.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-subdev.h>
+#include <media/v4l2-device.h>
+
+#include "mxc-mipi-csi2.h"
+static int debug;
+module_param(debug, int, 0644);
+MODULE_PARM_DESC(debug, "Debug level (0-2)");
+
+#define MXC_MIPI_CSI2_YAV_DRIVER_NAME	"mxc-mipi-csi2_yav"
+#define MXC_MIPI_CSI2_YAV_SUBDEV_NAME	MXC_MIPI_CSI2_DRIVER_NAME
+
+#define	GPR_CSI2_1_RX_ENABLE		BIT(13)
+#define	GPR_CSI2_1_VID_INTFC_ENB	BIT(12)
+#define	GPR_CSI2_1_PD_RX 		BIT(11)
+#define	GPR_CSI2_1_HSEL			BIT(10)
+#define	GPR_CSI2_1_AUTO_PD_EN 		BIT(9)
+#define	GPR_CSI2_1_CONT_CLK_MODE 	BIT(8)
+#define	GPR_CSI2_1_S_PRG_RXHS_SETTLE(x)	(((x) & 0x3F) << 2)
+#define	GPR_CSI2_1_RX_RCAL		(3)
+
+static u8 rxhs_settle[2] = { 0x14, 0x9 };
+
+static struct mxc_mipi_csi2_dev *sd_to_mxc_mipi_csi2_dev(struct v4l2_subdev
+							 *sdev)
+{
+	return container_of(sdev, struct mxc_mipi_csi2_dev, sd);
+}
+
+#ifdef debug
+static void mxc_mipi_csi2_reg_dump(struct mxc_mipi_csi2_dev *csi2dev)
+{
+	printk("MIPI CSI2 HC register dump, mipi csi%d\n", csi2dev->id);
+	printk("MIPI CSI2 HC num of lanes     0x100 = 0x%x\n",
+	       readl(csi2dev->base_regs + 0x100));
+	printk("MIPI CSI2 HC dis lanes        0x104 = 0x%x\n",
+	       readl(csi2dev->base_regs + 0x104));
+	printk("MIPI CSI2 HC BIT ERR          0x108 = 0x%x\n",
+	       readl(csi2dev->base_regs + 0x108));
+	printk("MIPI CSI2 HC IRQ STATUS       0x10C = 0x%x\n",
+	       readl(csi2dev->base_regs + 0x10C));
+	printk("MIPI CSI2 HC IRQ MASK         0x110 = 0x%x\n",
+	       readl(csi2dev->base_regs + 0x110));
+	printk("MIPI CSI2 HC ULPS STATUS      0x114 = 0x%x\n",
+	       readl(csi2dev->base_regs + 0x114));
+	printk("MIPI CSI2 HC DPHY ErrSotHS    0x118 = 0x%x\n",
+	       readl(csi2dev->base_regs + 0x118));
+	printk("MIPI CSI2 HC DPHY ErrSotSync  0x11c = 0x%x\n",
+	       readl(csi2dev->base_regs + 0x11c));
+	printk("MIPI CSI2 HC DPHY ErrEsc      0x120 = 0x%x\n",
+	       readl(csi2dev->base_regs + 0x120));
+	printk("MIPI CSI2 HC DPHY ErrSyncEsc  0x124 = 0x%x\n",
+	       readl(csi2dev->base_regs + 0x124));
+	printk("MIPI CSI2 HC DPHY ErrControl  0x128 = 0x%x\n",
+	       readl(csi2dev->base_regs + 0x128));
+	printk("MIPI CSI2 HC DISABLE_PAYLOAD  0x12C = 0x%x\n",
+	       readl(csi2dev->base_regs + 0x12C));
+	printk("MIPI CSI2 HC DISABLE_PAYLOAD  0x130 = 0x%x\n",
+	       readl(csi2dev->base_regs + 0x130));
+	printk("MIPI CSI2 HC IGNORE_VC        0x180 = 0x%x\n",
+	       readl(csi2dev->base_regs + 0x180));
+	printk("MIPI CSI2 HC VID_VC           0x184 = 0x%x\n",
+	       readl(csi2dev->base_regs + 0x184));
+	printk("MIPI CSI2 HC FIFO_SEND_LEVEL  0x188 = 0x%x\n",
+	       readl(csi2dev->base_regs + 0x188));
+	printk("MIPI CSI2 HC VID_VSYNC        0x18C = 0x%x\n",
+	       readl(csi2dev->base_regs + 0x18C));
+	printk("MIPI CSI2 HC VID_SYNC_FP      0x190 = 0x%x\n",
+	       readl(csi2dev->base_regs + 0x190));
+	printk("MIPI CSI2 HC VID_HSYNC        0x194 = 0x%x\n",
+	       readl(csi2dev->base_regs + 0x194));
+	printk("MIPI CSI2 HC VID_HSYNC_BP     0x198 = 0x%x\n",
+	       readl(csi2dev->base_regs + 0x198));
+}
+#else
+static void mxc_mipi_csi2_reg_dump(struct mxc_mipi_csi2_dev *csi2dev)
+{
+}
+#endif
+
+static int mxc_mipi_csi2_phy_reset(struct mxc_mipi_csi2_dev *csi2dev)
+{
+	struct device *dev = &csi2dev->pdev->dev;
+	struct device_node *np = dev->of_node;
+	struct device_node *node;
+	phandle phandle;
+	u32 out_val[3];
+	int ret;
+
+	ret = of_property_read_u32_array(np, "csis-phy-reset", out_val, 3);
+	if (ret) {
+		dev_info(dev, "no csis-hw-reset property found\n");
+	} else {
+		phandle = *out_val;
+
+		node = of_find_node_by_phandle(phandle);
+		if (!node) {
+			dev_dbg(dev, "not find src node by phandle\n");
+			ret = PTR_ERR(node);
+		}
+		csi2dev->hw_reset.src = syscon_node_to_regmap(node);
+		if (IS_ERR(csi2dev->hw_reset.src)) {
+			dev_err(dev, "failed to get src regmap\n");
+			ret = PTR_ERR(csi2dev->hw_reset.src);
+		}
+		of_node_put(node);
+		if (ret < 0)
+			return ret;
+
+		csi2dev->hw_reset.req_src = out_val[1];
+		csi2dev->hw_reset.rst_val = out_val[2];
+
+		/* reset mipi phy */
+		regmap_update_bits(csi2dev->hw_reset.src,
+				   csi2dev->hw_reset.req_src,
+				   csi2dev->hw_reset.rst_val,
+				   csi2dev->hw_reset.rst_val);
+		msleep(20);
+	}
+
+	return ret;
+}
+
+static int mxc_mipi_csi2_phy_gpr(struct mxc_mipi_csi2_dev *csi2dev)
+{
+	struct device *dev = &csi2dev->pdev->dev;
+	struct device_node *np = dev->of_node;
+	struct device_node *node;
+	phandle phandle;
+	u32 out_val[2];
+	int ret;
+
+	ret = of_property_read_u32_array(np, "phy-gpr", out_val, 2);
+	if (ret) {
+		dev_dbg(dev, "no phy-gpr property found\n");
+	} else {
+		phandle = *out_val;
+
+		node = of_find_node_by_phandle(phandle);
+		if (!node) {
+			dev_dbg(dev, "not find gpr node by phandle\n");
+			ret = PTR_ERR(node);
+		}
+		csi2dev->phy_gpr.gpr = syscon_node_to_regmap(node);
+		if (IS_ERR(csi2dev->phy_gpr.gpr)) {
+			dev_err(dev, "failed to get gpr regmap\n");
+			ret = PTR_ERR(csi2dev->phy_gpr.gpr);
+		}
+		of_node_put(node);
+		if (ret < 0)
+			return ret;
+
+		csi2dev->phy_gpr.req_src = out_val[1];
+
+		regmap_update_bits(csi2dev->phy_gpr.gpr,
+				   csi2dev->phy_gpr.req_src,
+				   0x3FFF,
+				   GPR_CSI2_1_RX_ENABLE |
+				   GPR_CSI2_1_VID_INTFC_ENB |
+				   GPR_CSI2_1_HSEL |
+				   GPR_CSI2_1_CONT_CLK_MODE |
+				   GPR_CSI2_1_S_PRG_RXHS_SETTLE(csi2dev->
+								hs_settle));
+	}
+
+	return ret;
+}
+
+static void mxc_mipi_csi2_enable(struct mxc_mipi_csi2_dev *csi2dev)
+{
+	mxc_mipi_csi2_phy_gpr(csi2dev);
+}
+
+static void mxc_mipi_csi2_disable(struct mxc_mipi_csi2_dev *csi2dev)
+{
+	/* Disable Data lanes */
+	writel(0xf, csi2dev->base_regs + CSI2RX_CFG_DISABLE_DATA_LANES);
+}
+
+static void mxc_mipi_csi2_hc_config(struct mxc_mipi_csi2_dev *csi2dev)
+{
+	u32 val0, val1;
+	u32 i;
+
+	val0 = 0;
+
+	/* Lanes */
+	writel(csi2dev->num_lanes - 1,
+	       csi2dev->base_regs + CSI2RX_CFG_NUM_LANES);
+
+	for (i = 0; i < csi2dev->num_lanes; i++)
+		val0 |= (1 << (csi2dev->data_lanes[i] - 1));
+
+	val1 = 0xF & ~val0;
+	writel(val1, csi2dev->base_regs + CSI2RX_CFG_DISABLE_DATA_LANES);
+
+	/* Mask interrupt */
+	writel(0x1FF, csi2dev->base_regs + CSI2RX_IRQ_MASK);
+
+	writel(1, csi2dev->base_regs + 0x180);
+	/* vid_vc */
+	writel(1, csi2dev->base_regs + 0x184);
+	writel(csi2dev->send_level, csi2dev->base_regs + 0x188);
+}
+
+static int mipi_csi2_clk_init(struct mxc_mipi_csi2_dev *csi2dev)
+{
+	struct device *dev = &csi2dev->pdev->dev;
+
+	csi2dev->clk_core = devm_clk_get(dev, "clk_core");
+	if (IS_ERR(csi2dev->clk_core)) {
+		dev_err(dev, "failed to get csi core clk\n");
+		return PTR_ERR(csi2dev->clk_core);
+	}
+
+	csi2dev->clk_esc = devm_clk_get(dev, "clk_esc");
+	if (IS_ERR(csi2dev->clk_esc)) {
+		dev_err(dev, "failed to get csi esc clk\n");
+		return PTR_ERR(csi2dev->clk_esc);
+	}
+
+	csi2dev->clk_pxl = devm_clk_get(dev, "clk_pxl");
+	if (IS_ERR(csi2dev->clk_pxl)) {
+		dev_err(dev, "failed to get csi pixel link clk\n");
+		return PTR_ERR(csi2dev->clk_pxl);
+	}
+
+	return 0;
+}
+
+static int mipi_csi2_clk_enable(struct mxc_mipi_csi2_dev *csi2dev)
+{
+	struct device *dev = &csi2dev->pdev->dev;
+	int ret;
+
+	ret = clk_prepare_enable(csi2dev->clk_core);
+	if (ret < 0) {
+		dev_err(dev, "%s, pre clk_core error\n", __func__);
+		return ret;
+	}
+	ret = clk_prepare_enable(csi2dev->clk_esc);
+	if (ret < 0) {
+		dev_err(dev, "%s, prepare clk_esc error\n", __func__);
+		return ret;
+	}
+	ret = clk_prepare_enable(csi2dev->clk_pxl);
+	if (ret < 0) {
+		dev_err(dev, "%s, prepare clk_pxl error\n", __func__);
+		return ret;
+	}
+	return ret;
+}
+
+static void mipi_csi2_clk_disable(struct mxc_mipi_csi2_dev *csi2dev)
+{
+	clk_disable_unprepare(csi2dev->clk_core);
+	clk_disable_unprepare(csi2dev->clk_esc);
+	clk_disable_unprepare(csi2dev->clk_pxl);
+}
+
+static int mipi_csi2_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	return 0;
+}
+
+/*
+ * V4L2 subdev operations
+ */
+static int mipi_csi2_s_power(struct v4l2_subdev *sd, int on)
+{
+	return 0;
+}
+
+static int mipi_csi2_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct mxc_mipi_csi2_dev *csi2dev = sd_to_mxc_mipi_csi2_dev(sd);
+	struct device *dev = &csi2dev->pdev->dev;
+	struct v4l2_subdev *sensor_sd = csi2dev->sensor_sd;
+	int ret = 0;
+
+	dev_dbg(&csi2dev->pdev->dev, "%s: %d, csi2dev: 0x%x\n",
+		__func__, enable, csi2dev->flags);
+
+	if (enable) {
+		if (!csi2dev->running) {
+			pm_runtime_get_sync(dev);
+			mxc_mipi_csi2_phy_reset(csi2dev);
+			mxc_mipi_csi2_hc_config(csi2dev);
+			mxc_mipi_csi2_enable(csi2dev);
+			mxc_mipi_csi2_reg_dump(csi2dev);
+		}
+		v4l2_subdev_call(sensor_sd, video, s_stream, true);
+		csi2dev->running++;
+
+	} else {
+
+		v4l2_subdev_call(sensor_sd, video, s_stream, false);
+		csi2dev->running--;
+		if (!csi2dev->running) {
+			pm_runtime_put(dev);
+			mxc_mipi_csi2_disable(csi2dev);
+		}
+	}
+
+	return ret;
+}
+
+static int mipi_csis_enum_framesizes(struct v4l2_subdev *sd,
+				     struct v4l2_subdev_pad_config *cfg,
+				     struct v4l2_subdev_frame_size_enum *fse)
+{
+	struct mxc_mipi_csi2_dev *csi2dev = sd_to_mxc_mipi_csi2_dev(sd);
+	struct v4l2_subdev *sensor_sd = csi2dev->sensor_sd;
+
+	return v4l2_subdev_call(sensor_sd, pad, enum_frame_size, NULL, fse);
+}
+
+static int mipi_csis_enum_frameintervals(struct v4l2_subdev *sd,
+					 struct v4l2_subdev_pad_config *cfg,
+					 struct v4l2_subdev_frame_interval_enum
+					 *fie)
+{
+	struct mxc_mipi_csi2_dev *csi2dev = sd_to_mxc_mipi_csi2_dev(sd);
+	struct v4l2_subdev *sensor_sd = csi2dev->sensor_sd;
+
+	return v4l2_subdev_call(sensor_sd, pad, enum_frame_interval, NULL, fie);
+}
+
+static int mipi_csis_enum_mbus_code(struct v4l2_subdev *sd,
+				    struct v4l2_subdev_pad_config *cfg,
+				    struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct mxc_mipi_csi2_dev *csi2dev = sd_to_mxc_mipi_csi2_dev(sd);
+	struct v4l2_subdev *sensor_sd = csi2dev->sensor_sd;
+
+	return v4l2_subdev_call(sensor_sd, pad, enum_mbus_code, NULL, code);
+}
+
+static int mipi_csi2_get_fmt(struct v4l2_subdev *sd,
+			     struct v4l2_subdev_pad_config *cfg,
+			     struct v4l2_subdev_format *fmt)
+{
+	struct mxc_mipi_csi2_dev *csi2dev = sd_to_mxc_mipi_csi2_dev(sd);
+	struct v4l2_subdev *sensor_sd = csi2dev->sensor_sd;
+
+	if (fmt->pad)
+		return -EINVAL;
+
+	return v4l2_subdev_call(sensor_sd, pad, get_fmt, NULL, fmt);
+}
+
+static int mipi_csi2_set_fmt(struct v4l2_subdev *sd,
+			     struct v4l2_subdev_pad_config *cfg,
+			     struct v4l2_subdev_format *fmt)
+{
+	struct mxc_mipi_csi2_dev *csi2dev = sd_to_mxc_mipi_csi2_dev(sd);
+	struct v4l2_subdev *sensor_sd = csi2dev->sensor_sd;
+
+	if (fmt->pad)
+		return -EINVAL;
+
+	if (fmt->format.width * fmt->format.height > 720 * 480) {
+		csi2dev->hs_settle = rxhs_settle[1];
+	} else {
+		csi2dev->hs_settle = rxhs_settle[0];
+	}
+	csi2dev->send_level = 64;
+
+	return v4l2_subdev_call(sensor_sd, pad, set_fmt, NULL, fmt);
+}
+
+static int mipi_csis_s_parm(struct v4l2_subdev *sd, struct v4l2_streamparm *a)
+{
+	struct mxc_mipi_csi2_dev *csi2dev = sd_to_mxc_mipi_csi2_dev(sd);
+	struct v4l2_subdev *sensor_sd = csi2dev->sensor_sd;
+
+	return v4l2_subdev_call(sensor_sd, video, s_parm, a);
+}
+
+static int mipi_csis_g_parm(struct v4l2_subdev *sd, struct v4l2_streamparm *a)
+{
+	struct mxc_mipi_csi2_dev *csi2dev = sd_to_mxc_mipi_csi2_dev(sd);
+	struct v4l2_subdev *sensor_sd = csi2dev->sensor_sd;
+
+	return v4l2_subdev_call(sensor_sd, video, g_parm, a);
+}
+
+static const struct v4l2_subdev_internal_ops mipi_csi2_sd_internal_ops = {
+	.open = mipi_csi2_open,
+};
+
+static struct v4l2_subdev_pad_ops mipi_csi2_pad_ops = {
+	.enum_frame_size = mipi_csis_enum_framesizes,
+	.enum_frame_interval = mipi_csis_enum_frameintervals,
+	.enum_mbus_code = mipi_csis_enum_mbus_code,
+	.get_fmt = mipi_csi2_get_fmt,
+	.set_fmt = mipi_csi2_set_fmt,
+};
+
+static struct v4l2_subdev_core_ops mipi_csi2_core_ops = {
+	.s_power = mipi_csi2_s_power,
+};
+
+static struct v4l2_subdev_video_ops mipi_csi2_video_ops = {
+	.s_stream = mipi_csi2_s_stream,
+
+	.s_parm = mipi_csis_s_parm,
+	.g_parm = mipi_csis_g_parm,
+};
+
+static struct v4l2_subdev_ops mipi_csi2_subdev_ops = {
+	.core = &mipi_csi2_core_ops,
+	.video = &mipi_csi2_video_ops,
+	.pad = &mipi_csi2_pad_ops,
+};
+
+static int mipi_csi2_parse_dt(struct mxc_mipi_csi2_dev *csi2dev)
+{
+	struct device *dev = &csi2dev->pdev->dev;
+	struct device_node *node = dev->of_node;
+	struct v4l2_fwnode_endpoint endpoint;
+	u32 i;
+
+	csi2dev->id = of_alias_get_id(node, "csi");
+
+	csi2dev->vchannel = of_property_read_bool(node, "virtual-channel");
+
+	node = of_graph_get_next_endpoint(node, NULL);
+	if (!node) {
+		dev_err(dev, "No port node\n");
+		return -EINVAL;
+	}
+
+	/* Get port node */
+	memset(&endpoint, 0x0, sizeof(endpoint));
+	v4l2_fwnode_endpoint_parse(of_fwnode_handle(node), &endpoint);
+
+	csi2dev->num_lanes = endpoint.bus.mipi_csi2.num_data_lanes;
+	for (i = 0; i < csi2dev->num_lanes; i++)
+		csi2dev->data_lanes[i] = endpoint.bus.mipi_csi2.data_lanes[i];
+
+	of_node_put(node);
+
+	return 0;
+}
+
+static inline struct mxc_mipi_csi2_dev
+*notifier_to_mipi_dev(struct v4l2_async_notifier *n)
+{
+	return container_of(n, struct mxc_mipi_csi2_dev, subdev_notifier);
+}
+
+static int subdev_notifier_bound(struct v4l2_async_notifier *notifier,
+				 struct v4l2_subdev *subdev,
+				 struct v4l2_async_subdev *asd)
+{
+	struct mxc_mipi_csi2_dev *csi2dev = notifier_to_mipi_dev(notifier);
+
+	/* Find platform data for this sensor subdev */
+	if (csi2dev->asd.match.fwnode == of_fwnode_handle(subdev->dev->of_node))
+		csi2dev->sensor_sd = subdev;
+
+	if (subdev == NULL)
+		return -EINVAL;
+
+	v4l2_info(&csi2dev->v4l2_dev, "Registered sensor subdevice: %s\n",
+		  subdev->name);
+
+	return 0;
+}
+
+static const struct v4l2_async_notifier_operations subdev_notifier_ops = {
+	.bound = subdev_notifier_bound,
+};
+
+static int mipi_csis_subdev_host(struct mxc_mipi_csi2_dev *csi2dev)
+{
+	struct device *dev = &csi2dev->pdev->dev;
+	struct device_node *parent = dev->of_node;
+	struct device_node *node, *port, *rem;
+	int ret;
+
+	v4l2_async_notifier_init(&csi2dev->subdev_notifier);
+
+	/* Attach sensors linked to csi receivers */
+	for_each_available_child_of_node(parent, node) {
+		if (of_node_cmp(node->name, "port"))
+			continue;
+
+		/* The csi node can have only port subnode. */
+		port = of_get_next_child(node, NULL);
+		if (!port)
+			continue;
+		rem = of_graph_get_remote_port_parent(port);
+		of_node_put(port);
+		if (rem == NULL) {
+			v4l2_info(&csi2dev->v4l2_dev,
+				  "Remote device at %s not found\n",
+				  port->full_name);
+			return -1;
+		} else
+			v4l2_info(&csi2dev->v4l2_dev,
+				  "Remote device at %s XXX found\n",
+				  port->full_name);
+
+		INIT_LIST_HEAD(&csi2dev->asd.list);
+		csi2dev->asd.match_type = V4L2_ASYNC_MATCH_FWNODE;
+		csi2dev->asd.match.fwnode = of_fwnode_handle(rem);
+		csi2dev->async_subdevs[0] = &csi2dev->asd;
+
+		of_node_put(rem);
+		break;
+	}
+
+	ret = v4l2_async_notifier_add_subdev(&csi2dev->subdev_notifier,
+					&csi2dev->asd);
+	if (ret) {
+		dev_err(dev, "failed to add subdev to a notifier\n");
+		return ret;
+	}
+
+	csi2dev->subdev_notifier.v4l2_dev = &csi2dev->v4l2_dev;
+	csi2dev->subdev_notifier.ops = &subdev_notifier_ops;
+
+	ret = v4l2_async_notifier_register(&csi2dev->v4l2_dev,
+					   &csi2dev->subdev_notifier);
+	if (ret)
+		dev_err(dev,
+			"Error register async notifier regoster, ret %d\n",
+			ret);
+
+	return ret;
+}
+
+static int mipi_csi2_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct resource *mem_res;
+	struct mxc_mipi_csi2_dev *csi2dev;
+	int ret = -ENOMEM;
+
+	dev_info(&pdev->dev, "%s\n", __func__);
+	csi2dev = devm_kzalloc(dev, sizeof(*csi2dev), GFP_KERNEL);
+	if (!csi2dev)
+		return -ENOMEM;
+
+	csi2dev->pdev = pdev;
+
+	ret = mipi_csi2_parse_dt(csi2dev);
+	if (ret < 0)
+		return -EINVAL;
+
+	mem_res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	csi2dev->base_regs = devm_ioremap_resource(dev, mem_res);
+	if (IS_ERR(csi2dev->base_regs)) {
+		dev_err(dev, "Failed to get mipi csi2 HC register\n");
+		return PTR_ERR(csi2dev->base_regs);
+	}
+
+	ret = mipi_csi2_clk_init(csi2dev);
+	if (ret < 0)
+		return -EINVAL;
+
+	v4l2_subdev_init(&csi2dev->sd, &mipi_csi2_subdev_ops);
+
+	csi2dev->sd.owner = THIS_MODULE;
+	snprintf(csi2dev->sd.name, sizeof(csi2dev->sd.name), "%s.%d",
+		 MXC_MIPI_CSI2_YAV_SUBDEV_NAME, csi2dev->id);
+
+	csi2dev->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	csi2dev->sd.dev = &pdev->dev;
+
+	/* First register a v4l2 device */
+	ret = v4l2_device_register(dev, &csi2dev->v4l2_dev);
+	if (ret) {
+		dev_err(&pdev->dev, "Unable to register v4l2 device.\n");
+		return -EINVAL;
+	}
+	ret = v4l2_async_register_subdev(&csi2dev->sd);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "%s--Async register faialed, ret=%d\n",
+			__func__, ret);
+		goto e_v4l_dev;
+	}
+
+	ret = mipi_csis_subdev_host(csi2dev);
+	if (ret < 0)
+		goto e_clkdis;
+
+	/* This allows to retrieve the platform device id by the host driver */
+	v4l2_set_subdevdata(&csi2dev->sd, pdev);
+
+	/* .. and a pointer to the subdev. */
+	platform_set_drvdata(pdev, csi2dev);
+
+	ret = mipi_csi2_clk_enable(csi2dev);
+	if (ret < 0)
+		goto e_clkdis;
+
+	dev_info(&pdev->dev, "lanes: %d, name: %s\n",
+		 csi2dev->num_lanes, csi2dev->sd.name);
+
+	csi2dev->running = 0;
+	csi2dev->flags = MXC_MIPI_CSI2_PM_POWERED;
+	pm_runtime_enable(&pdev->dev);
+	mipi_csi2_clk_disable(csi2dev);
+
+	return 0;
+
+e_clkdis:
+	v4l2_async_unregister_subdev(&csi2dev->sd);
+e_v4l_dev:
+	v4l2_device_unregister(&csi2dev->v4l2_dev);
+	return ret;
+}
+
+static int mipi_csi2_remove(struct platform_device *pdev)
+{
+	struct v4l2_subdev *sd = platform_get_drvdata(pdev);
+	struct mxc_mipi_csi2_dev *csi2dev = sd_to_mxc_mipi_csi2_dev(sd);
+
+	mipi_csi2_clk_disable(csi2dev);
+	pm_runtime_disable(&pdev->dev);
+
+	return 0;
+}
+
+static int __maybe_unused mipi_csi2_pm_runtime_resume(struct device *dev)
+{
+	struct mxc_mipi_csi2_dev *csi2dev = dev_get_drvdata(dev);
+	int ret;
+
+	ret = mipi_csi2_clk_enable(csi2dev);
+	if (ret < 0) {
+		dev_info(dev, "%s:%d fail\n", __func__, __LINE__);
+		return -EAGAIN;
+	}
+
+	return 0;
+}
+
+static int __maybe_unused mipi_csi2_runtime_pm_suspend(struct device *dev)
+{
+	struct mxc_mipi_csi2_dev *csi2dev = dev_get_drvdata(dev);
+
+	mipi_csi2_clk_disable(csi2dev);
+
+	return 0;
+}
+
+static int __maybe_unused mipi_csi2_pm_suspend(struct device *dev)
+{
+	struct mxc_mipi_csi2_dev *csi2dev = dev_get_drvdata(dev);
+
+	if (csi2dev->flags & MXC_MIPI_CSI2_PM_SUSPENDED)
+		return 0;
+
+	if (csi2dev->running) {
+		dev_warn(dev, "running, prevent entering suspend.\n");
+		return -EAGAIN;
+	}
+	csi2dev->flags &= ~MXC_MIPI_CSI2_PM_POWERED;
+	csi2dev->flags |= MXC_MIPI_CSI2_PM_SUSPENDED;
+
+	return 0;
+}
+
+static int __maybe_unused mipi_csi2_pm_resume(struct device *dev)
+{
+	struct mxc_mipi_csi2_dev *csi2dev = dev_get_drvdata(dev);
+
+	if (csi2dev->flags & MXC_MIPI_CSI2_PM_POWERED)
+		return 0;
+
+	csi2dev->flags |= MXC_MIPI_CSI2_PM_POWERED;
+	csi2dev->flags &= ~MXC_MIPI_CSI2_PM_SUSPENDED;
+
+	return 0;
+}
+
+static const struct dev_pm_ops mipi_csi_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(mipi_csi2_pm_suspend, mipi_csi2_pm_resume)
+	SET_RUNTIME_PM_OPS(mipi_csi2_runtime_pm_suspend,
+				mipi_csi2_pm_runtime_resume,
+				NULL)
+};
+
+static const struct of_device_id mipi_csi2_of_match[] = {
+	{.compatible = "fsl,mxc-mipi-csi2_yav",},
+	{ /* sentinel */ },
+};
+
+MODULE_DEVICE_TABLE(of, mipi_csi2_of_match);
+
+static struct platform_driver mipi_csi2_driver = {
+	.driver = {
+		   .name = MXC_MIPI_CSI2_YAV_DRIVER_NAME,
+		   .of_match_table = mipi_csi2_of_match,
+		   .pm = &mipi_csi_pm_ops,
+		   },
+	.probe = mipi_csi2_probe,
+	.remove = mipi_csi2_remove,
+};
+
+module_platform_driver(mipi_csi2_driver);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("MXC MIPI CSI2 driver");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:" MXC_MIPI_CSI2_YAV_DRIVER_NAME);
diff --git a/drivers/media/platform/mxc/capture/Kconfig b/drivers/media/platform/mxc/capture/Kconfig
new file mode 100644
index 000000000..1fca68bfa
--- /dev/null
+++ b/drivers/media/platform/mxc/capture/Kconfig
@@ -0,0 +1,22 @@
+if VIDEO_MXC_CAPTURE
+
+config VIDEO_MXC_CSI_CAMERA
+	tristate "CSI camera support"
+	depends on VIDEO_MXC_CAPTURE && VIDEO_V4L2
+	help
+		This is the video4linux2 capture driver based on CSI module.
+
+config MXC_MIPI_CSI
+	tristate "mxc mipi csi driver"
+	depends on VIDEO_MXC_CAPTURE && VIDEO_V4L2
+	help
+		This is a V4L2 driver for i.MX7D SoC MIPI-CSI2 receiver devices.
+
+config MXC_CAMERA_OV5640_MIPI_V2
+	tristate "OmniVision ov5640 camera support using mipi"
+	depends on MXC_MIPI_CSI && I2C
+	help
+	  If you plan to use the ov5640 Camera with mipi interface in your MXC system, say Y here.
+
+
+endif
diff --git a/drivers/media/platform/mxc/capture/Makefile b/drivers/media/platform/mxc/capture/Makefile
new file mode 100644
index 000000000..4398efd32
--- /dev/null
+++ b/drivers/media/platform/mxc/capture/Makefile
@@ -0,0 +1,5 @@
+obj-$(CONFIG_VIDEO_MXC_CSI_CAMERA) += mx6s_capture.o
+obj-$(CONFIG_MXC_MIPI_CSI) += mxc_mipi_csi.o
+# Used for iMX 7D
+ov5640_camera_mipi_v2-objs := ov5640_mipi_v2.o
+obj-$(CONFIG_MXC_CAMERA_OV5640_MIPI_V2) += ov5640_camera_mipi_v2.o
diff --git a/drivers/media/platform/mxc/capture/mx6s_capture.c b/drivers/media/platform/mxc/capture/mx6s_capture.c
new file mode 100644
index 000000000..1203f1b0c
--- /dev/null
+++ b/drivers/media/platform/mxc/capture/mx6s_capture.c
@@ -0,0 +1,2042 @@
+/* SPDX-License-Identifier: GPL-2.0-or-later */
+/*
+ * Copyright (C) 2014-2016 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2019 NXP
+ */
+
+/*!
+ * @file mx6s_csi.c
+ *
+ * @brief mx6sx CMOS Sensor interface functions
+ *
+ * @ingroup CSI
+ */
+#include <asm/dma.h>
+#include <linux/busfreq-imx.h>
+#include <linux/clk.h>
+#include <linux/dma-mapping.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/errno.h>
+#include <linux/fs.h>
+#include <linux/gcd.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/math64.h>
+#include <linux/mfd/syscon.h>
+#include <linux/mm.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/of_graph.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/regmap.h>
+#include <linux/slab.h>
+#include <linux/time.h>
+#include <linux/media-bus-format.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-dev.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ioctl.h>
+#include <media/videobuf2-core.h>
+#include <media/videobuf2-dma-contig.h>
+
+#define MX6S_CAM_DRV_NAME "mx6s-csi"
+#define MX6S_CAM_VERSION "0.0.1"
+#define MX6S_CAM_DRIVER_DESCRIPTION "i.MX6S_CSI"
+
+#define MAX_VIDEO_MEM 64
+
+/* reset values */
+#define CSICR1_RESET_VAL	0x40000800
+#define CSICR2_RESET_VAL	0x0
+#define CSICR3_RESET_VAL	0x0
+
+/* csi control reg 1 */
+#define BIT_SWAP16_EN		(0x1 << 31)
+#define BIT_EXT_VSYNC		(0x1 << 30)
+#define BIT_EOF_INT_EN		(0x1 << 29)
+#define BIT_PRP_IF_EN		(0x1 << 28)
+#define BIT_CCIR_MODE		(0x1 << 27)
+#define BIT_COF_INT_EN		(0x1 << 26)
+#define BIT_SF_OR_INTEN		(0x1 << 25)
+#define BIT_RF_OR_INTEN		(0x1 << 24)
+#define BIT_SFF_DMA_DONE_INTEN  (0x1 << 22)
+#define BIT_STATFF_INTEN	(0x1 << 21)
+#define BIT_FB2_DMA_DONE_INTEN  (0x1 << 20)
+#define BIT_FB1_DMA_DONE_INTEN  (0x1 << 19)
+#define BIT_RXFF_INTEN		(0x1 << 18)
+#define BIT_SOF_POL		(0x1 << 17)
+#define BIT_SOF_INTEN		(0x1 << 16)
+#define BIT_MCLKDIV		(0xF << 12)
+#define BIT_HSYNC_POL		(0x1 << 11)
+#define BIT_CCIR_EN		(0x1 << 10)
+#define BIT_MCLKEN		(0x1 << 9)
+#define BIT_FCC			(0x1 << 8)
+#define BIT_PACK_DIR		(0x1 << 7)
+#define BIT_CLR_STATFIFO	(0x1 << 6)
+#define BIT_CLR_RXFIFO		(0x1 << 5)
+#define BIT_GCLK_MODE		(0x1 << 4)
+#define BIT_INV_DATA		(0x1 << 3)
+#define BIT_INV_PCLK		(0x1 << 2)
+#define BIT_REDGE		(0x1 << 1)
+#define BIT_PIXEL_BIT		(0x1 << 0)
+
+#define SHIFT_MCLKDIV		12
+
+/* control reg 3 */
+#define BIT_FRMCNT		(0xFFFF << 16)
+#define BIT_FRMCNT_RST		(0x1 << 15)
+#define BIT_DMA_REFLASH_RFF	(0x1 << 14)
+#define BIT_DMA_REFLASH_SFF	(0x1 << 13)
+#define BIT_DMA_REQ_EN_RFF	(0x1 << 12)
+#define BIT_DMA_REQ_EN_SFF	(0x1 << 11)
+#define BIT_STATFF_LEVEL	(0x7 << 8)
+#define BIT_HRESP_ERR_EN	(0x1 << 7)
+#define BIT_RXFF_LEVEL		(0x7 << 4)
+#define BIT_TWO_8BIT_SENSOR	(0x1 << 3)
+#define BIT_ZERO_PACK_EN	(0x1 << 2)
+#define BIT_ECC_INT_EN		(0x1 << 1)
+#define BIT_ECC_AUTO_EN		(0x1 << 0)
+
+#define SHIFT_FRMCNT		16
+#define SHIFT_RXFIFO_LEVEL	4
+
+/* csi status reg */
+#define BIT_ADDR_CH_ERR_INT (0x1 << 28)
+#define BIT_FIELD0_INT      (0x1 << 27)
+#define BIT_FIELD1_INT      (0x1 << 26)
+#define BIT_SFF_OR_INT		(0x1 << 25)
+#define BIT_RFF_OR_INT		(0x1 << 24)
+#define BIT_DMA_TSF_DONE_SFF	(0x1 << 22)
+#define BIT_STATFF_INT		(0x1 << 21)
+#define BIT_DMA_TSF_DONE_FB2	(0x1 << 20)
+#define BIT_DMA_TSF_DONE_FB1	(0x1 << 19)
+#define BIT_RXFF_INT		(0x1 << 18)
+#define BIT_EOF_INT		(0x1 << 17)
+#define BIT_SOF_INT		(0x1 << 16)
+#define BIT_F2_INT		(0x1 << 15)
+#define BIT_F1_INT		(0x1 << 14)
+#define BIT_COF_INT		(0x1 << 13)
+#define BIT_HRESP_ERR_INT	(0x1 << 7)
+#define BIT_ECC_INT		(0x1 << 1)
+#define BIT_DRDY		(0x1 << 0)
+
+/* csi control reg 18 */
+#define BIT_CSI_ENABLE			(0x1 << 31)
+#define BIT_MIPI_DATA_FORMAT_RAW8		(0x2a << 25)
+#define BIT_MIPI_DATA_FORMAT_RAW10		(0x2b << 25)
+#define BIT_MIPI_DATA_FORMAT_YUV422_8B	(0x1e << 25)
+#define BIT_MIPI_DATA_FORMAT_MASK	(0x3F << 25)
+#define BIT_MIPI_DATA_FORMAT_OFFSET	25
+#define BIT_DATA_FROM_MIPI		(0x1 << 22)
+#define BIT_MIPI_YU_SWAP		(0x1 << 21)
+#define BIT_MIPI_DOUBLE_CMPNT	(0x1 << 20)
+#define BIT_BASEADDR_CHG_ERR_EN	(0x1 << 9)
+#define BIT_BASEADDR_SWITCH_SEL	(0x1 << 5)
+#define BIT_BASEADDR_SWITCH_EN	(0x1 << 4)
+#define BIT_PARALLEL24_EN		(0x1 << 3)
+#define BIT_DEINTERLACE_EN		(0x1 << 2)
+#define BIT_TVDECODER_IN_EN		(0x1 << 1)
+#define BIT_NTSC_EN				(0x1 << 0)
+
+#define CSI_MCLK_VF		1
+#define CSI_MCLK_ENC		2
+#define CSI_MCLK_RAW		4
+#define CSI_MCLK_I2C		8
+
+#define CSI_CSICR1		0x0
+#define CSI_CSICR2		0x4
+#define CSI_CSICR3		0x8
+#define CSI_STATFIFO		0xC
+#define CSI_CSIRXFIFO		0x10
+#define CSI_CSIRXCNT		0x14
+#define CSI_CSISR		0x18
+
+#define CSI_CSIDBG		0x1C
+#define CSI_CSIDMASA_STATFIFO	0x20
+#define CSI_CSIDMATS_STATFIFO	0x24
+#define CSI_CSIDMASA_FB1	0x28
+#define CSI_CSIDMASA_FB2	0x2C
+#define CSI_CSIFBUF_PARA	0x30
+#define CSI_CSIIMAG_PARA	0x34
+
+#define CSI_CSICR18		0x48
+#define CSI_CSICR19		0x4c
+
+#define NUM_FORMATS ARRAY_SIZE(formats)
+#define MX6SX_MAX_SENSORS    1
+
+struct csi_signal_cfg_t {
+	unsigned data_width:3;
+	unsigned clk_mode:2;
+	unsigned ext_vsync:1;
+	unsigned Vsync_pol:1;
+	unsigned Hsync_pol:1;
+	unsigned pixclk_pol:1;
+	unsigned data_pol:1;
+	unsigned sens_clksrc:1;
+};
+
+struct csi_config_t {
+	/* control reg 1 */
+	unsigned int swap16_en:1;
+	unsigned int ext_vsync:1;
+	unsigned int eof_int_en:1;
+	unsigned int prp_if_en:1;
+	unsigned int ccir_mode:1;
+	unsigned int cof_int_en:1;
+	unsigned int sf_or_inten:1;
+	unsigned int rf_or_inten:1;
+	unsigned int sff_dma_done_inten:1;
+	unsigned int statff_inten:1;
+	unsigned int fb2_dma_done_inten:1;
+	unsigned int fb1_dma_done_inten:1;
+	unsigned int rxff_inten:1;
+	unsigned int sof_pol:1;
+	unsigned int sof_inten:1;
+	unsigned int mclkdiv:4;
+	unsigned int hsync_pol:1;
+	unsigned int ccir_en:1;
+	unsigned int mclken:1;
+	unsigned int fcc:1;
+	unsigned int pack_dir:1;
+	unsigned int gclk_mode:1;
+	unsigned int inv_data:1;
+	unsigned int inv_pclk:1;
+	unsigned int redge:1;
+	unsigned int pixel_bit:1;
+
+	/* control reg 3 */
+	unsigned int frmcnt:16;
+	unsigned int frame_reset:1;
+	unsigned int dma_reflash_rff:1;
+	unsigned int dma_reflash_sff:1;
+	unsigned int dma_req_en_rff:1;
+	unsigned int dma_req_en_sff:1;
+	unsigned int statff_level:3;
+	unsigned int hresp_err_en:1;
+	unsigned int rxff_level:3;
+	unsigned int two_8bit_sensor:1;
+	unsigned int zero_pack_en:1;
+	unsigned int ecc_int_en:1;
+	unsigned int ecc_auto_en:1;
+	/* fifo counter */
+	unsigned int rxcnt;
+};
+
+/*
+ * Basic structures
+ */
+struct mx6s_fmt {
+	char  name[32];
+	u32   fourcc;		/* v4l2 format id */
+	u32   pixelformat;
+	u32   mbus_code;
+	int   bpp;
+};
+
+static struct mx6s_fmt formats[] = {
+	{
+		.name		= "UYVY-16",
+		.fourcc		= V4L2_PIX_FMT_UYVY,
+		.pixelformat	= V4L2_PIX_FMT_UYVY,
+		.mbus_code	= MEDIA_BUS_FMT_UYVY8_2X8,
+		.bpp		= 2,
+	}, {
+		.name		= "YUYV-16",
+		.fourcc		= V4L2_PIX_FMT_YUYV,
+		.pixelformat	= V4L2_PIX_FMT_YUYV,
+		.mbus_code	= MEDIA_BUS_FMT_YUYV8_2X8,
+		.bpp		= 2,
+	}, {
+		.name		= "YUV32 (X-Y-U-V)",
+		.fourcc		= V4L2_PIX_FMT_YUV32,
+		.pixelformat	= V4L2_PIX_FMT_YUV32,
+		.mbus_code	= MEDIA_BUS_FMT_AYUV8_1X32,
+		.bpp		= 4,
+	}, {
+		.name		= "RAWRGB8 (SBGGR8)",
+		.fourcc		= V4L2_PIX_FMT_SBGGR8,
+		.pixelformat	= V4L2_PIX_FMT_SBGGR8,
+		.mbus_code	= MEDIA_BUS_FMT_SBGGR8_1X8,
+		.bpp		= 1,
+	}
+};
+
+struct mx6s_buf_internal {
+	struct list_head	queue;
+	int					bufnum;
+	bool				discard;
+};
+
+/* buffer for one video frame */
+struct mx6s_buffer {
+	/* common v4l buffer stuff -- must be first */
+	struct vb2_v4l2_buffer			vb;
+	struct mx6s_buf_internal	internal;
+};
+
+struct mx6s_csi_mux {
+	struct regmap *gpr;
+	u8 req_gpr;
+	u8 req_bit;
+};
+
+struct mx6s_csi_soc {
+	bool rx_fifo_rst;
+	int baseaddr_switch;
+};
+
+struct mx6s_csi_dev {
+	struct device		*dev;
+	struct video_device *vdev;
+	struct v4l2_subdev	*sd;
+	struct v4l2_device	v4l2_dev;
+
+	struct vb2_queue			vb2_vidq;
+	struct v4l2_ctrl_handler	ctrl_handler;
+
+	struct mutex		lock;
+	spinlock_t			slock;
+
+	int open_count;
+
+	/* clock */
+	struct clk	*clk_disp_axi;
+	struct clk	*clk_disp_dcic;
+	struct clk	*clk_csi_mclk;
+
+	void __iomem *regbase;
+	int irq;
+
+	u32      nextfb;
+	u32      skipframe;
+	u32	 type;
+	u32 bytesperline;
+	v4l2_std_id std;
+	struct mx6s_fmt		*fmt;
+	struct v4l2_pix_format pix;
+	u32 mbus_code;
+
+	unsigned int frame_count;
+
+	struct list_head	capture;
+	struct list_head	active_bufs;
+	struct list_head	discard;
+
+	void						*discard_buffer;
+	dma_addr_t					discard_buffer_dma;
+	size_t						discard_size;
+	struct mx6s_buf_internal	buf_discard[2];
+
+	struct v4l2_async_subdev	asd;
+	struct v4l2_async_notifier	subdev_notifier;
+
+	bool csi_mipi_mode;
+	bool csi_two_8bit_sensor_mode;
+	const struct mx6s_csi_soc *soc;
+	struct mx6s_csi_mux csi_mux;
+};
+
+static const struct of_device_id mx6s_csi_dt_ids[];
+
+static inline int csi_read(struct mx6s_csi_dev *csi, unsigned int offset)
+{
+	return __raw_readl(csi->regbase + offset);
+}
+static inline void csi_write(struct mx6s_csi_dev *csi, unsigned int value,
+			     unsigned int offset)
+{
+	__raw_writel(value, csi->regbase + offset);
+}
+
+static inline struct mx6s_csi_dev
+				*notifier_to_mx6s_dev(struct v4l2_async_notifier *n)
+{
+	return container_of(n, struct mx6s_csi_dev, subdev_notifier);
+}
+
+struct mx6s_fmt *format_by_fourcc(int fourcc)
+{
+	int i;
+
+	for (i = 0; i < NUM_FORMATS; i++) {
+		if (formats[i].pixelformat == fourcc)
+			return formats + i;
+	}
+
+	pr_err("unknown pixelformat:'%4.4s'\n", (char *)&fourcc);
+	return NULL;
+}
+
+struct mx6s_fmt *format_by_mbus(u32 code)
+{
+	int i;
+
+	for (i = 0; i < NUM_FORMATS; i++) {
+		if (formats[i].mbus_code == code)
+			return formats + i;
+	}
+
+	pr_err("unknown mbus:0x%x\n", code);
+	return NULL;
+}
+
+static struct mx6s_buffer *mx6s_ibuf_to_buf(struct mx6s_buf_internal *int_buf)
+{
+	return container_of(int_buf, struct mx6s_buffer, internal);
+}
+
+void csi_clk_enable(struct mx6s_csi_dev *csi_dev)
+{
+	clk_prepare_enable(csi_dev->clk_disp_axi);
+	clk_prepare_enable(csi_dev->clk_disp_dcic);
+	clk_prepare_enable(csi_dev->clk_csi_mclk);
+}
+
+void csi_clk_disable(struct mx6s_csi_dev *csi_dev)
+{
+	clk_disable_unprepare(csi_dev->clk_csi_mclk);
+	clk_disable_unprepare(csi_dev->clk_disp_dcic);
+	clk_disable_unprepare(csi_dev->clk_disp_axi);
+}
+
+static void csihw_reset(struct mx6s_csi_dev *csi_dev)
+{
+	__raw_writel(__raw_readl(csi_dev->regbase + CSI_CSICR3)
+			| BIT_FRMCNT_RST,
+			csi_dev->regbase + CSI_CSICR3);
+
+	__raw_writel(CSICR1_RESET_VAL, csi_dev->regbase + CSI_CSICR1);
+	__raw_writel(CSICR2_RESET_VAL, csi_dev->regbase + CSI_CSICR2);
+	__raw_writel(CSICR3_RESET_VAL, csi_dev->regbase + CSI_CSICR3);
+}
+
+static void csisw_reset(struct mx6s_csi_dev *csi_dev)
+{
+	int cr1, cr3, cr18, isr;
+
+	/* Disable csi  */
+	cr18 = csi_read(csi_dev, CSI_CSICR18);
+	cr18 &= ~BIT_CSI_ENABLE;
+	csi_write(csi_dev, cr18, CSI_CSICR18);
+
+	/* Clear RX FIFO */
+	cr1 = csi_read(csi_dev, CSI_CSICR1);
+	csi_write(csi_dev, cr1 & ~BIT_FCC, CSI_CSICR1);
+	cr1 = csi_read(csi_dev, CSI_CSICR1);
+	csi_write(csi_dev, cr1 | BIT_CLR_RXFIFO, CSI_CSICR1);
+
+	/* DMA reflash */
+	cr3 = csi_read(csi_dev, CSI_CSICR3);
+	cr3 |= BIT_DMA_REFLASH_RFF | BIT_FRMCNT_RST;
+	csi_write(csi_dev, cr3, CSI_CSICR3);
+
+	msleep(2);
+
+	cr1 = csi_read(csi_dev, CSI_CSICR1);
+	csi_write(csi_dev, cr1 | BIT_FCC, CSI_CSICR1);
+
+	isr = csi_read(csi_dev, CSI_CSISR);
+	csi_write(csi_dev, isr, CSI_CSISR);
+
+	cr18 |= csi_dev->soc->baseaddr_switch;
+
+	/* Enable csi  */
+	cr18 |= BIT_CSI_ENABLE;
+	csi_write(csi_dev, cr18, CSI_CSICR18);
+}
+
+/*!
+ * csi_init_interface
+ *    Init csi interface
+ */
+static void csi_init_interface(struct mx6s_csi_dev *csi_dev)
+{
+	unsigned int val = 0;
+	unsigned int imag_para;
+
+	val |= BIT_SOF_POL;
+	val |= BIT_REDGE;
+	val |= BIT_GCLK_MODE;
+	val |= BIT_HSYNC_POL;
+	val |= BIT_FCC;
+	val |= 1 << SHIFT_MCLKDIV;
+	val |= BIT_MCLKEN;
+	__raw_writel(val, csi_dev->regbase + CSI_CSICR1);
+
+	imag_para = (640 << 16) | 960;
+	__raw_writel(imag_para, csi_dev->regbase + CSI_CSIIMAG_PARA);
+
+	val = BIT_DMA_REFLASH_RFF;
+	__raw_writel(val, csi_dev->regbase + CSI_CSICR3);
+}
+
+static void csi_enable_int(struct mx6s_csi_dev *csi_dev, int arg)
+{
+	unsigned long cr1 = __raw_readl(csi_dev->regbase + CSI_CSICR1);
+
+	cr1 |= BIT_SOF_INTEN;
+	cr1 |= BIT_RFF_OR_INT;
+	if (arg == 1) {
+		/* still capture needs DMA intterrupt */
+		cr1 |= BIT_FB1_DMA_DONE_INTEN;
+		cr1 |= BIT_FB2_DMA_DONE_INTEN;
+	}
+	__raw_writel(cr1, csi_dev->regbase + CSI_CSICR1);
+}
+
+static void csi_disable_int(struct mx6s_csi_dev *csi_dev)
+{
+	unsigned long cr1 = __raw_readl(csi_dev->regbase + CSI_CSICR1);
+
+	cr1 &= ~BIT_SOF_INTEN;
+	cr1 &= ~BIT_RFF_OR_INT;
+	cr1 &= ~BIT_FB1_DMA_DONE_INTEN;
+	cr1 &= ~BIT_FB2_DMA_DONE_INTEN;
+	__raw_writel(cr1, csi_dev->regbase + CSI_CSICR1);
+}
+
+static void csi_enable(struct mx6s_csi_dev *csi_dev, int arg)
+{
+	unsigned long cr = __raw_readl(csi_dev->regbase + CSI_CSICR18);
+
+	if (arg == 1)
+		cr |= BIT_CSI_ENABLE;
+	else
+		cr &= ~BIT_CSI_ENABLE;
+	__raw_writel(cr, csi_dev->regbase + CSI_CSICR18);
+}
+
+static void csi_buf_stride_set(struct mx6s_csi_dev *csi_dev, u32 stride)
+{
+	__raw_writel(stride, csi_dev->regbase + CSI_CSIFBUF_PARA);
+}
+
+static void csi_deinterlace_enable(struct mx6s_csi_dev *csi_dev, bool enable)
+{
+	unsigned long cr18 = __raw_readl(csi_dev->regbase + CSI_CSICR18);
+
+	if (enable == true)
+		cr18 |= BIT_DEINTERLACE_EN;
+	else
+		cr18 &= ~BIT_DEINTERLACE_EN;
+
+	__raw_writel(cr18, csi_dev->regbase + CSI_CSICR18);
+}
+
+static void csi_deinterlace_mode(struct mx6s_csi_dev *csi_dev, int mode)
+{
+	unsigned long cr18 = __raw_readl(csi_dev->regbase + CSI_CSICR18);
+
+	if (mode == V4L2_STD_NTSC)
+		cr18 |= BIT_NTSC_EN;
+	else
+		cr18 &= ~BIT_NTSC_EN;
+
+	__raw_writel(cr18, csi_dev->regbase + CSI_CSICR18);
+}
+
+static void csi_tvdec_enable(struct mx6s_csi_dev *csi_dev, bool enable)
+{
+	unsigned long cr18 = __raw_readl(csi_dev->regbase + CSI_CSICR18);
+	unsigned long cr1 = __raw_readl(csi_dev->regbase + CSI_CSICR1);
+
+	if (enable == true) {
+		cr18 |= (BIT_TVDECODER_IN_EN |
+				BIT_BASEADDR_SWITCH_EN |
+				BIT_BASEADDR_SWITCH_SEL |
+				BIT_BASEADDR_CHG_ERR_EN);
+		cr1 |= BIT_CCIR_MODE;
+		cr1 &= ~(BIT_SOF_POL | BIT_REDGE);
+	} else {
+		cr18 &= ~(BIT_TVDECODER_IN_EN |
+				BIT_BASEADDR_SWITCH_EN |
+				BIT_BASEADDR_SWITCH_SEL |
+				BIT_BASEADDR_CHG_ERR_EN);
+		cr1 &= ~BIT_CCIR_MODE;
+		cr1 |= BIT_SOF_POL | BIT_REDGE;
+	}
+
+	__raw_writel(cr18, csi_dev->regbase + CSI_CSICR18);
+	__raw_writel(cr1, csi_dev->regbase + CSI_CSICR1);
+}
+
+static void csi_dmareq_rff_enable(struct mx6s_csi_dev *csi_dev)
+{
+	unsigned long cr3 = __raw_readl(csi_dev->regbase + CSI_CSICR3);
+	unsigned long cr2 = __raw_readl(csi_dev->regbase + CSI_CSICR2);
+
+	/* Burst Type of DMA Transfer from RxFIFO. INCR16 */
+	cr2 |= 0xC0000000;
+
+	cr3 |= BIT_DMA_REQ_EN_RFF;
+	cr3 |= BIT_HRESP_ERR_EN;
+	cr3 &= ~BIT_RXFF_LEVEL;
+	cr3 |= 0x2 << 4;
+	if (csi_dev->csi_two_8bit_sensor_mode)
+		cr3 |= BIT_TWO_8BIT_SENSOR;
+
+	__raw_writel(cr3, csi_dev->regbase + CSI_CSICR3);
+	__raw_writel(cr2, csi_dev->regbase + CSI_CSICR2);
+}
+
+static void csi_dmareq_rff_disable(struct mx6s_csi_dev *csi_dev)
+{
+	unsigned long cr3 = __raw_readl(csi_dev->regbase + CSI_CSICR3);
+
+	cr3 &= ~BIT_DMA_REQ_EN_RFF;
+	cr3 &= ~BIT_HRESP_ERR_EN;
+	__raw_writel(cr3, csi_dev->regbase + CSI_CSICR3);
+}
+
+static void csi_set_imagpara(struct mx6s_csi_dev *csi,
+					int width, int height)
+{
+	int imag_para = 0;
+	unsigned long cr3 = __raw_readl(csi->regbase + CSI_CSICR3);
+
+	imag_para = (width << 16) | height;
+	__raw_writel(imag_para, csi->regbase + CSI_CSIIMAG_PARA);
+
+	/* reflash the embeded DMA controller */
+	__raw_writel(cr3 | BIT_DMA_REFLASH_RFF, csi->regbase + CSI_CSICR3);
+}
+
+static void csi_error_recovery(struct mx6s_csi_dev *csi_dev)
+{
+	u32 cr1, cr3, cr18;
+	/* software reset */
+
+	/* Disable csi  */
+	cr18 = csi_read(csi_dev, CSI_CSICR18);
+	cr18 &= ~BIT_CSI_ENABLE;
+	csi_write(csi_dev, cr18, CSI_CSICR18);
+
+	/* Clear RX FIFO */
+	cr1 = csi_read(csi_dev, CSI_CSICR1);
+	csi_write(csi_dev, cr1 & ~BIT_FCC, CSI_CSICR1);
+	cr1 = csi_read(csi_dev, CSI_CSICR1);
+	csi_write(csi_dev, cr1 | BIT_CLR_RXFIFO, CSI_CSICR1);
+
+	cr1 = csi_read(csi_dev, CSI_CSICR1);
+	csi_write(csi_dev, cr1 | BIT_FCC, CSI_CSICR1);
+
+	/* DMA reflash */
+	cr3 = csi_read(csi_dev, CSI_CSICR3);
+	cr3 |= BIT_DMA_REFLASH_RFF;
+	csi_write(csi_dev, cr3, CSI_CSICR3);
+
+	/* Ensable csi  */
+	cr18 |= BIT_CSI_ENABLE;
+	csi_write(csi_dev, cr18, CSI_CSICR18);
+}
+
+/*
+ *  Videobuf operations
+ */
+static int mx6s_videobuf_setup(struct vb2_queue *vq,
+			unsigned int *count, unsigned int *num_planes,
+			unsigned int sizes[], struct device *alloc_devs[])
+{
+	struct mx6s_csi_dev *csi_dev = vb2_get_drv_priv(vq);
+
+	dev_dbg(csi_dev->dev, "count=%d, size=%d\n", *count, sizes[0]);
+
+	alloc_devs[0] = csi_dev->dev;
+
+	sizes[0] = csi_dev->pix.sizeimage;
+
+	pr_debug("size=%d\n", sizes[0]);
+	if (0 == *count)
+		*count = 32;
+	if (!*num_planes &&
+	    sizes[0] * *count > MAX_VIDEO_MEM * 1024 * 1024)
+		*count = (MAX_VIDEO_MEM * 1024 * 1024) / sizes[0];
+
+	*num_planes = 1;
+
+	return 0;
+}
+
+static int mx6s_videobuf_prepare(struct vb2_buffer *vb)
+{
+	struct mx6s_csi_dev *csi_dev = vb2_get_drv_priv(vb->vb2_queue);
+	int ret = 0;
+
+	dev_dbg(csi_dev->dev, "%s (vb=0x%p) 0x%p %lu\n", __func__,
+		vb, vb2_plane_vaddr(vb, 0), vb2_get_plane_payload(vb, 0));
+
+#ifdef DEBUG
+	/*
+	 * This can be useful if you want to see if we actually fill
+	 * the buffer with something
+	 */
+	if (vb2_plane_vaddr(vb, 0))
+		memset((void *)vb2_plane_vaddr(vb, 0),
+		       0xaa, vb2_get_plane_payload(vb, 0));
+#endif
+
+	vb2_set_plane_payload(vb, 0, csi_dev->pix.sizeimage);
+	if (vb2_plane_vaddr(vb, 0) &&
+	    vb2_get_plane_payload(vb, 0) > vb2_plane_size(vb, 0)) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	return 0;
+
+out:
+	return ret;
+}
+
+static void mx6s_videobuf_queue(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct mx6s_csi_dev *csi_dev = vb2_get_drv_priv(vb->vb2_queue);
+	struct mx6s_buffer *buf = container_of(vbuf, struct mx6s_buffer, vb);
+	unsigned long flags;
+
+	dev_dbg(csi_dev->dev, "%s (vb=0x%p) 0x%p %lu\n", __func__,
+		vb, vb2_plane_vaddr(vb, 0), vb2_get_plane_payload(vb, 0));
+
+	spin_lock_irqsave(&csi_dev->slock, flags);
+
+	list_add_tail(&buf->internal.queue, &csi_dev->capture);
+
+	spin_unlock_irqrestore(&csi_dev->slock, flags);
+}
+
+static void mx6s_update_csi_buf(struct mx6s_csi_dev *csi_dev,
+				 unsigned long phys, int bufnum)
+{
+	if (bufnum == 1)
+		csi_write(csi_dev, phys, CSI_CSIDMASA_FB2);
+	else
+		csi_write(csi_dev, phys, CSI_CSIDMASA_FB1);
+}
+
+static void mx6s_csi_init(struct mx6s_csi_dev *csi_dev)
+{
+	csi_clk_enable(csi_dev);
+	csihw_reset(csi_dev);
+	csi_init_interface(csi_dev);
+	csi_dmareq_rff_disable(csi_dev);
+}
+
+static void mx6s_csi_deinit(struct mx6s_csi_dev *csi_dev)
+{
+	csihw_reset(csi_dev);
+	csi_init_interface(csi_dev);
+	csi_dmareq_rff_disable(csi_dev);
+	csi_clk_disable(csi_dev);
+}
+
+static int mx6s_csi_enable(struct mx6s_csi_dev *csi_dev)
+{
+	struct v4l2_pix_format *pix = &csi_dev->pix;
+	unsigned long flags;
+	unsigned long val;
+	int timeout, timeout2;
+
+	csi_dev->skipframe = 0;
+	csisw_reset(csi_dev);
+
+	if (pix->field == V4L2_FIELD_INTERLACED)
+		csi_tvdec_enable(csi_dev, true);
+
+	/* For mipi csi input only */
+	if (csi_dev->csi_mipi_mode == true) {
+		csi_dmareq_rff_enable(csi_dev);
+		csi_enable_int(csi_dev, 1);
+		csi_enable(csi_dev, 1);
+		return 0;
+	}
+
+	local_irq_save(flags);
+	for (timeout = 10000000; timeout > 0; timeout--) {
+		if (csi_read(csi_dev, CSI_CSISR) & BIT_SOF_INT) {
+			val = csi_read(csi_dev, CSI_CSICR3);
+			csi_write(csi_dev, val | BIT_DMA_REFLASH_RFF,
+					CSI_CSICR3);
+			/* Wait DMA reflash done */
+			for (timeout2 = 1000000; timeout2 > 0; timeout2--) {
+				if (csi_read(csi_dev, CSI_CSICR3) &
+					BIT_DMA_REFLASH_RFF)
+					cpu_relax();
+				else
+					break;
+			}
+			if (timeout2 <= 0) {
+				pr_err("timeout when wait for reflash done.\n");
+				local_irq_restore(flags);
+				return -ETIME;
+			}
+			/* For imx6sl csi, DMA FIFO will auto start when sensor ready to work,
+			 * so DMA should enable right after FIFO reset, otherwise dma will lost data
+			 * and image will split.
+			 */
+			csi_dmareq_rff_enable(csi_dev);
+			csi_enable_int(csi_dev, 1);
+			csi_enable(csi_dev, 1);
+			break;
+		} else
+			cpu_relax();
+	}
+	if (timeout <= 0) {
+		pr_err("timeout when wait for SOF\n");
+		local_irq_restore(flags);
+		return -ETIME;
+	}
+	local_irq_restore(flags);
+
+	return 0;
+}
+
+static void mx6s_csi_disable(struct mx6s_csi_dev *csi_dev)
+{
+	struct v4l2_pix_format *pix = &csi_dev->pix;
+
+	csi_dmareq_rff_disable(csi_dev);
+	csi_disable_int(csi_dev);
+
+	/* set CSI_CSIDMASA_FB1 and CSI_CSIDMASA_FB2 to default value */
+	csi_write(csi_dev, 0, CSI_CSIDMASA_FB1);
+	csi_write(csi_dev, 0, CSI_CSIDMASA_FB2);
+
+	csi_buf_stride_set(csi_dev, 0);
+
+	if (pix->field == V4L2_FIELD_INTERLACED) {
+		csi_deinterlace_enable(csi_dev, false);
+		csi_tvdec_enable(csi_dev, false);
+	}
+
+	csi_enable(csi_dev, 0);
+}
+
+static int mx6s_configure_csi(struct mx6s_csi_dev *csi_dev)
+{
+	struct v4l2_pix_format *pix = &csi_dev->pix;
+	u32 cr1, cr18;
+	u32 width;
+
+	if (pix->field == V4L2_FIELD_INTERLACED) {
+		csi_deinterlace_enable(csi_dev, true);
+		csi_buf_stride_set(csi_dev, csi_dev->pix.width);
+		csi_deinterlace_mode(csi_dev, csi_dev->std);
+	} else {
+		csi_deinterlace_enable(csi_dev, false);
+		csi_buf_stride_set(csi_dev, 0);
+	}
+
+	switch (csi_dev->fmt->pixelformat) {
+	case V4L2_PIX_FMT_YUV32:
+	case V4L2_PIX_FMT_SBGGR8:
+		width = pix->width;
+		break;
+	case V4L2_PIX_FMT_UYVY:
+	case V4L2_PIX_FMT_YUYV:
+		if (csi_dev->csi_mipi_mode == true)
+			width = pix->width;
+		else
+			/* For parallel 8-bit sensor input */
+			width = pix->width * 2;
+		break;
+	default:
+		pr_debug("   case not supported\n");
+		return -EINVAL;
+	}
+	csi_set_imagpara(csi_dev, width, pix->height);
+
+	if (csi_dev->csi_mipi_mode == true) {
+		cr1 = csi_read(csi_dev, CSI_CSICR1);
+		cr1 &= ~BIT_GCLK_MODE;
+		csi_write(csi_dev, cr1, CSI_CSICR1);
+
+		cr18 = csi_read(csi_dev, CSI_CSICR18);
+		cr18 &= ~BIT_MIPI_DATA_FORMAT_MASK;
+		cr18 |= BIT_DATA_FROM_MIPI;
+
+		switch (csi_dev->fmt->pixelformat) {
+		case V4L2_PIX_FMT_UYVY:
+		case V4L2_PIX_FMT_YUYV:
+			cr18 |= BIT_MIPI_DATA_FORMAT_YUV422_8B;
+			break;
+		case V4L2_PIX_FMT_SBGGR8:
+			cr18 |= BIT_MIPI_DATA_FORMAT_RAW8;
+			break;
+		default:
+			pr_debug("   fmt not supported\n");
+			return -EINVAL;
+		}
+
+		csi_write(csi_dev, cr18, CSI_CSICR18);
+	}
+	return 0;
+}
+
+static int mx6s_start_streaming(struct vb2_queue *vq, unsigned int count)
+{
+	struct mx6s_csi_dev *csi_dev = vb2_get_drv_priv(vq);
+	struct vb2_buffer *vb;
+	struct mx6s_buffer *buf;
+	unsigned long phys;
+	unsigned long flags;
+
+	if (count < 2)
+		return -ENOBUFS;
+
+	/*
+	 * I didn't manage to properly enable/disable
+	 * a per frame basis during running transfers,
+	 * thus we allocate a buffer here and use it to
+	 * discard frames when no buffer is available.
+	 * Feel free to work on this ;)
+	 */
+	csi_dev->discard_size = csi_dev->pix.sizeimage;
+	csi_dev->discard_buffer = dma_alloc_coherent(csi_dev->v4l2_dev.dev,
+					PAGE_ALIGN(csi_dev->discard_size),
+					&csi_dev->discard_buffer_dma,
+					GFP_DMA | GFP_KERNEL);
+	if (!csi_dev->discard_buffer)
+		return -ENOMEM;
+
+	spin_lock_irqsave(&csi_dev->slock, flags);
+
+	csi_dev->buf_discard[0].discard = true;
+	list_add_tail(&csi_dev->buf_discard[0].queue,
+		      &csi_dev->discard);
+
+	csi_dev->buf_discard[1].discard = true;
+	list_add_tail(&csi_dev->buf_discard[1].queue,
+		      &csi_dev->discard);
+
+	/* csi buf 0 */
+	buf = list_first_entry(&csi_dev->capture, struct mx6s_buffer,
+			       internal.queue);
+	buf->internal.bufnum = 0;
+	vb = &buf->vb.vb2_buf;
+	vb->state = VB2_BUF_STATE_ACTIVE;
+
+	phys = vb2_dma_contig_plane_dma_addr(vb, 0);
+
+	mx6s_update_csi_buf(csi_dev, phys, buf->internal.bufnum);
+	list_move_tail(csi_dev->capture.next, &csi_dev->active_bufs);
+
+	/* csi buf 1 */
+	buf = list_first_entry(&csi_dev->capture, struct mx6s_buffer,
+			       internal.queue);
+	buf->internal.bufnum = 1;
+	vb = &buf->vb.vb2_buf;
+	vb->state = VB2_BUF_STATE_ACTIVE;
+
+	phys = vb2_dma_contig_plane_dma_addr(vb, 0);
+	mx6s_update_csi_buf(csi_dev, phys, buf->internal.bufnum);
+	list_move_tail(csi_dev->capture.next, &csi_dev->active_bufs);
+
+	csi_dev->nextfb = 0;
+
+	spin_unlock_irqrestore(&csi_dev->slock, flags);
+
+	return mx6s_csi_enable(csi_dev);
+}
+
+static void mx6s_stop_streaming(struct vb2_queue *vq)
+{
+	struct mx6s_csi_dev *csi_dev = vb2_get_drv_priv(vq);
+	unsigned long flags;
+	struct mx6s_buffer *buf, *tmp;
+	void *b;
+
+	mx6s_csi_disable(csi_dev);
+
+	spin_lock_irqsave(&csi_dev->slock, flags);
+
+	list_for_each_entry_safe(buf, tmp,
+				&csi_dev->active_bufs, internal.queue) {
+		list_del_init(&buf->internal.queue);
+		if (buf->internal.discard == false &&
+		    buf->vb.vb2_buf.state == VB2_BUF_STATE_ACTIVE)
+			vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
+	}
+
+	list_for_each_entry_safe(buf, tmp,
+				&csi_dev->capture, internal.queue) {
+		list_del_init(&buf->internal.queue);
+		if (buf->vb.vb2_buf.state == VB2_BUF_STATE_ACTIVE)
+			vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
+	}
+
+	INIT_LIST_HEAD(&csi_dev->capture);
+	INIT_LIST_HEAD(&csi_dev->active_bufs);
+	INIT_LIST_HEAD(&csi_dev->discard);
+
+	b = csi_dev->discard_buffer;
+	csi_dev->discard_buffer = NULL;
+
+	spin_unlock_irqrestore(&csi_dev->slock, flags);
+
+	dma_free_coherent(csi_dev->v4l2_dev.dev,
+				csi_dev->discard_size, b,
+				csi_dev->discard_buffer_dma);
+}
+
+static struct vb2_ops mx6s_videobuf_ops = {
+	.queue_setup     = mx6s_videobuf_setup,
+	.buf_prepare     = mx6s_videobuf_prepare,
+	.buf_queue       = mx6s_videobuf_queue,
+	.wait_prepare    = vb2_ops_wait_prepare,
+	.wait_finish     = vb2_ops_wait_finish,
+	.start_streaming = mx6s_start_streaming,
+	.stop_streaming	 = mx6s_stop_streaming,
+};
+
+static void mx6s_csi_frame_done(struct mx6s_csi_dev *csi_dev,
+		int bufnum, bool err)
+{
+	struct mx6s_buf_internal *ibuf;
+	struct mx6s_buffer *buf;
+	struct vb2_buffer *vb;
+	unsigned long phys;
+	unsigned int phys_fb1;
+	unsigned int phys_fb2;
+
+	ibuf = list_first_entry(&csi_dev->active_bufs, struct mx6s_buf_internal,
+			       queue);
+
+	if (ibuf->discard) {
+		/*
+		 * Discard buffer must not be returned to user space.
+		 * Just return it to the discard queue.
+		 */
+		list_move_tail(csi_dev->active_bufs.next, &csi_dev->discard);
+	} else {
+		buf = mx6s_ibuf_to_buf(ibuf);
+
+		vb = &buf->vb.vb2_buf;
+		phys = vb2_dma_contig_plane_dma_addr(vb, 0);
+		if (bufnum == 1) {
+			phys_fb2 = csi_read(csi_dev, CSI_CSIDMASA_FB2);
+			if (phys_fb2 != (u32)phys) {
+				dev_err(csi_dev->dev, "%lx != %x\n", phys,
+					phys_fb2);
+			}
+		} else {
+			phys_fb1 = csi_read(csi_dev, CSI_CSIDMASA_FB1);
+			if (phys_fb1 != (u32)phys) {
+				dev_err(csi_dev->dev, "%lx != %x\n", phys,
+					phys_fb1);
+			}
+		}
+		dev_dbg(csi_dev->dev, "%s (vb=0x%p) 0x%p %lu\n", __func__, vb,
+				vb2_plane_vaddr(vb, 0),
+				vb2_get_plane_payload(vb, 0));
+
+		list_del_init(&buf->internal.queue);
+		vb->timestamp =ktime_get_ns();
+		to_vb2_v4l2_buffer(vb)->sequence = csi_dev->frame_count;
+		if (err)
+			vb2_buffer_done(vb, VB2_BUF_STATE_ERROR);
+		else
+			vb2_buffer_done(vb, VB2_BUF_STATE_DONE);
+	}
+
+	csi_dev->frame_count++;
+	csi_dev->nextfb = (bufnum == 0 ? 1 : 0);
+
+	/* Config discard buffer to active_bufs */
+	if (list_empty(&csi_dev->capture)) {
+		if (list_empty(&csi_dev->discard)) {
+			dev_warn(csi_dev->dev,
+					"%s: trying to access empty discard list\n",
+					__func__);
+			return;
+		}
+
+		ibuf = list_first_entry(&csi_dev->discard,
+					struct mx6s_buf_internal, queue);
+		ibuf->bufnum = bufnum;
+
+		list_move_tail(csi_dev->discard.next, &csi_dev->active_bufs);
+
+		mx6s_update_csi_buf(csi_dev,
+					csi_dev->discard_buffer_dma, bufnum);
+		return;
+	}
+
+	buf = list_first_entry(&csi_dev->capture, struct mx6s_buffer,
+			       internal.queue);
+
+	buf->internal.bufnum = bufnum;
+
+	list_move_tail(csi_dev->capture.next, &csi_dev->active_bufs);
+
+	vb = &buf->vb.vb2_buf;
+	vb->state = VB2_BUF_STATE_ACTIVE;
+
+	phys = vb2_dma_contig_plane_dma_addr(vb, 0);
+	mx6s_update_csi_buf(csi_dev, phys, bufnum);
+}
+
+static irqreturn_t mx6s_csi_irq_handler(int irq, void *data)
+{
+	struct mx6s_csi_dev *csi_dev =  data;
+	unsigned long status;
+	u32 cr3, cr18;
+
+	spin_lock(&csi_dev->slock);
+
+	status = csi_read(csi_dev, CSI_CSISR);
+	csi_write(csi_dev, status, CSI_CSISR);
+
+	if (list_empty(&csi_dev->active_bufs)) {
+		dev_warn(csi_dev->dev,
+				"%s: called while active list is empty\n",
+				__func__);
+
+		spin_unlock(&csi_dev->slock);
+		return IRQ_HANDLED;
+	}
+
+	if (status & BIT_RFF_OR_INT) {
+		dev_warn(csi_dev->dev, "%s Rx fifo overflow\n", __func__);
+		if (csi_dev->soc->rx_fifo_rst)
+			csi_error_recovery(csi_dev);
+	}
+
+	if (status & BIT_HRESP_ERR_INT) {
+		dev_warn(csi_dev->dev, "%s Hresponse error detected\n",
+			__func__);
+		csi_error_recovery(csi_dev);
+	}
+
+	if (status & BIT_ADDR_CH_ERR_INT) {
+		/* Disable csi  */
+		cr18 = csi_read(csi_dev, CSI_CSICR18);
+		cr18 &= ~BIT_CSI_ENABLE;
+		csi_write(csi_dev, cr18, CSI_CSICR18);
+
+		/* DMA reflash */
+		cr3 = csi_read(csi_dev, CSI_CSICR3);
+		cr3 |= BIT_DMA_REFLASH_RFF;
+		csi_write(csi_dev, cr3, CSI_CSICR3);
+
+		/* Ensable csi  */
+		cr18 |= BIT_CSI_ENABLE;
+		csi_write(csi_dev, cr18, CSI_CSICR18);
+
+		csi_dev->skipframe = 1;
+		pr_debug("base address switching Change Err.\n");
+	}
+
+	if ((status & BIT_DMA_TSF_DONE_FB1) &&
+		(status & BIT_DMA_TSF_DONE_FB2)) {
+		/* For both FB1 and FB2 interrupter bits set case,
+		 * CSI DMA is work in one of FB1 and FB2 buffer,
+		 * but software can not know the state.
+		 * Skip it to avoid base address updated
+		 * when csi work in field0 and field1 will write to
+		 * new base address.
+		 * PDM TKT230775 */
+		pr_debug("Skip two frames\n");
+	} else if (status & BIT_DMA_TSF_DONE_FB1) {
+		if (csi_dev->nextfb == 0) {
+			if (csi_dev->skipframe > 0)
+				csi_dev->skipframe--;
+			else
+				mx6s_csi_frame_done(csi_dev, 0, false);
+		} else
+			pr_warn("skip frame 0\n");
+
+	} else if (status & BIT_DMA_TSF_DONE_FB2) {
+		if (csi_dev->nextfb == 1) {
+			if (csi_dev->skipframe > 0)
+				csi_dev->skipframe--;
+			else
+				mx6s_csi_frame_done(csi_dev, 1, false);
+		} else
+			pr_warn("skip frame 1\n");
+	}
+
+	spin_unlock(&csi_dev->slock);
+
+	return IRQ_HANDLED;
+}
+
+/*
+ * File operations for the device
+ */
+static int mx6s_csi_open(struct file *file)
+{
+	struct mx6s_csi_dev *csi_dev = video_drvdata(file);
+	struct v4l2_subdev *sd = csi_dev->sd;
+	struct vb2_queue *q = &csi_dev->vb2_vidq;
+	int ret = 0;
+
+	file->private_data = csi_dev;
+
+	if (mutex_lock_interruptible(&csi_dev->lock))
+		return -ERESTARTSYS;
+
+	if (csi_dev->open_count++ == 0) {
+		q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+		q->io_modes = VB2_MMAP | VB2_USERPTR;
+		q->drv_priv = csi_dev;
+		q->ops = &mx6s_videobuf_ops;
+		q->mem_ops = &vb2_dma_contig_memops;
+		q->buf_struct_size = sizeof(struct mx6s_buffer);
+		q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+		q->lock = &csi_dev->lock;
+
+		ret = vb2_queue_init(q);
+		if (ret < 0)
+			goto unlock;
+
+		pm_runtime_get_sync(csi_dev->dev);
+
+		request_bus_freq(BUS_FREQ_HIGH);
+
+		v4l2_subdev_call(sd, core, s_power, 1);
+		mx6s_csi_init(csi_dev);
+
+	}
+	mutex_unlock(&csi_dev->lock);
+
+	return ret;
+unlock:
+	mutex_unlock(&csi_dev->lock);
+	return ret;
+}
+
+static int mx6s_csi_close(struct file *file)
+{
+	struct mx6s_csi_dev *csi_dev = video_drvdata(file);
+	struct v4l2_subdev *sd = csi_dev->sd;
+
+	mutex_lock(&csi_dev->lock);
+
+	if (--csi_dev->open_count == 0) {
+		vb2_queue_release(&csi_dev->vb2_vidq);
+
+		mx6s_csi_deinit(csi_dev);
+		v4l2_subdev_call(sd, core, s_power, 0);
+
+		file->private_data = NULL;
+
+		release_bus_freq(BUS_FREQ_HIGH);
+
+		pm_runtime_put_sync_suspend(csi_dev->dev);
+	}
+	mutex_unlock(&csi_dev->lock);
+
+	return 0;
+}
+
+static ssize_t mx6s_csi_read(struct file *file, char __user *buf,
+			       size_t count, loff_t *ppos)
+{
+	struct mx6s_csi_dev *csi_dev = video_drvdata(file);
+	int ret;
+
+	dev_dbg(csi_dev->dev, "read called, buf %p\n", buf);
+
+	mutex_lock(&csi_dev->lock);
+	ret = vb2_read(&csi_dev->vb2_vidq, buf, count, ppos,
+				file->f_flags & O_NONBLOCK);
+	mutex_unlock(&csi_dev->lock);
+	return ret;
+}
+
+static struct v4l2_file_operations mx6s_csi_fops = {
+	.owner		= THIS_MODULE,
+	.open		= mx6s_csi_open,
+	.release	= mx6s_csi_close,
+	.read		= mx6s_csi_read,
+	.poll		= vb2_fop_poll,
+	.unlocked_ioctl	= video_ioctl2, /* V4L2 ioctl handler */
+	.mmap		= vb2_fop_mmap,
+};
+
+/*
+ * Video node IOCTLs
+ */
+static int mx6s_vidioc_enum_input(struct file *file, void *priv,
+				 struct v4l2_input *inp)
+{
+	if (inp->index != 0)
+		return -EINVAL;
+
+	/* default is camera */
+	inp->type = V4L2_INPUT_TYPE_CAMERA;
+	strcpy(inp->name, "Camera");
+
+	return 0;
+}
+
+static int mx6s_vidioc_g_input(struct file *file, void *priv, unsigned int *i)
+{
+	*i = 0;
+
+	return 0;
+}
+
+static int mx6s_vidioc_s_input(struct file *file, void *priv, unsigned int i)
+{
+	if (i > 0)
+		return -EINVAL;
+
+	return 0;
+}
+
+static int mx6s_vidioc_querystd(struct file *file, void *priv, v4l2_std_id *a)
+{
+	struct mx6s_csi_dev *csi_dev = video_drvdata(file);
+	struct v4l2_subdev *sd = csi_dev->sd;
+
+	return v4l2_subdev_call(sd, video, querystd, a);
+}
+
+static int mx6s_vidioc_s_std(struct file *file, void *priv, v4l2_std_id a)
+{
+	struct mx6s_csi_dev *csi_dev = video_drvdata(file);
+	struct v4l2_subdev *sd = csi_dev->sd;
+
+	return v4l2_subdev_call(sd, video, s_std, a);
+}
+
+static int mx6s_vidioc_g_std(struct file *file, void *priv, v4l2_std_id *a)
+{
+	struct mx6s_csi_dev *csi_dev = video_drvdata(file);
+	struct v4l2_subdev *sd = csi_dev->sd;
+
+	return v4l2_subdev_call(sd, video, g_std, a);
+}
+
+static int mx6s_vidioc_reqbufs(struct file *file, void *priv,
+			      struct v4l2_requestbuffers *p)
+{
+	struct mx6s_csi_dev *csi_dev = video_drvdata(file);
+
+	WARN_ON(priv != file->private_data);
+
+	return vb2_reqbufs(&csi_dev->vb2_vidq, p);
+}
+
+static int mx6s_vidioc_querybuf(struct file *file, void *priv,
+			       struct v4l2_buffer *p)
+{
+	struct mx6s_csi_dev *csi_dev = video_drvdata(file);
+	int ret;
+
+	WARN_ON(priv != file->private_data);
+
+	ret = vb2_querybuf(&csi_dev->vb2_vidq, p);
+
+	if (!ret) {
+		/* return physical address */
+		struct vb2_buffer *vb = csi_dev->vb2_vidq.bufs[p->index];
+		if (p->flags & V4L2_BUF_FLAG_MAPPED)
+			p->m.offset = vb2_dma_contig_plane_dma_addr(vb, 0);
+	}
+	return ret;
+}
+
+static int mx6s_vidioc_qbuf(struct file *file, void *priv,
+			   struct v4l2_buffer *p)
+{
+	struct mx6s_csi_dev *csi_dev = video_drvdata(file);
+
+	WARN_ON(priv != file->private_data);
+
+	return vb2_qbuf(&csi_dev->vb2_vidq, NULL, p);
+}
+
+static int mx6s_vidioc_dqbuf(struct file *file, void *priv,
+			    struct v4l2_buffer *p)
+{
+	struct mx6s_csi_dev *csi_dev = video_drvdata(file);
+
+	WARN_ON(priv != file->private_data);
+
+	return vb2_dqbuf(&csi_dev->vb2_vidq, p, file->f_flags & O_NONBLOCK);
+}
+
+static int mx6s_vidioc_enum_fmt_vid_cap(struct file *file, void  *priv,
+				       struct v4l2_fmtdesc *f)
+{
+	struct mx6s_csi_dev *csi_dev = video_drvdata(file);
+	struct v4l2_subdev *sd = csi_dev->sd;
+	struct v4l2_subdev_mbus_code_enum code = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+		.index = f->index,
+	};
+	struct mx6s_fmt *fmt;
+	int ret;
+
+	WARN_ON(priv != file->private_data);
+
+	ret = v4l2_subdev_call(sd, pad, enum_mbus_code, NULL, &code);
+	if (ret < 0) {
+		/* no more formats */
+		dev_dbg(csi_dev->dev, "No more fmt\n");
+		return -EINVAL;
+	}
+
+	fmt = format_by_mbus(code.code);
+	if (!fmt) {
+		dev_err(csi_dev->dev, "mbus (0x%08x) invalid.\n", code.code);
+		return -EINVAL;
+	}
+
+	strlcpy(f->description, fmt->name, sizeof(f->description));
+	f->pixelformat = fmt->pixelformat;
+
+	return 0;
+}
+
+static int mx6s_vidioc_try_fmt_vid_cap(struct file *file, void *priv,
+				      struct v4l2_format *f)
+{
+	struct mx6s_csi_dev *csi_dev = video_drvdata(file);
+	struct v4l2_subdev *sd = csi_dev->sd;
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	struct mx6s_fmt *fmt;
+	int ret;
+
+	fmt = format_by_fourcc(f->fmt.pix.pixelformat);
+	if (!fmt) {
+		dev_err(csi_dev->dev, "Fourcc format (0x%08x) invalid.",
+			f->fmt.pix.pixelformat);
+		return -EINVAL;
+	}
+
+	if (f->fmt.pix.width == 0 || f->fmt.pix.height == 0) {
+		dev_err(csi_dev->dev, "width %d, height %d is too small.\n",
+			f->fmt.pix.width, f->fmt.pix.height);
+		return -EINVAL;
+	}
+
+	v4l2_fill_mbus_format(&format.format, pix, fmt->mbus_code);
+	ret = v4l2_subdev_call(sd, pad, set_fmt, NULL, &format);
+	v4l2_fill_pix_format(pix, &format.format);
+
+	if (pix->field != V4L2_FIELD_INTERLACED)
+		pix->field = V4L2_FIELD_NONE;
+
+	pix->sizeimage = fmt->bpp * pix->height * pix->width;
+	pix->bytesperline = fmt->bpp * pix->width;
+
+	pix->colorspace = V4L2_COLORSPACE_SRGB;
+	pix->ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(pix->colorspace);
+	pix->quantization = V4L2_QUANTIZATION_FULL_RANGE;
+
+	return ret;
+}
+
+/*
+ * The real work of figuring out a workable format.
+ */
+
+static int mx6s_vidioc_s_fmt_vid_cap(struct file *file, void *priv,
+				    struct v4l2_format *f)
+{
+	struct mx6s_csi_dev *csi_dev = video_drvdata(file);
+	int ret;
+
+	ret = mx6s_vidioc_try_fmt_vid_cap(file, csi_dev, f);
+	if (ret < 0)
+		return ret;
+
+	csi_dev->fmt           = format_by_fourcc(f->fmt.pix.pixelformat);
+	csi_dev->mbus_code     = csi_dev->fmt->mbus_code;
+	csi_dev->pix.width     = f->fmt.pix.width;
+	csi_dev->pix.height    = f->fmt.pix.height;
+	csi_dev->pix.sizeimage = f->fmt.pix.sizeimage;
+	csi_dev->pix.field     = f->fmt.pix.field;
+	csi_dev->type          = f->type;
+	dev_dbg(csi_dev->dev, "set to pixelformat '%4.6s'\n",
+			(char *)&csi_dev->fmt->name);
+
+	/* Config csi */
+	mx6s_configure_csi(csi_dev);
+
+	return 0;
+}
+
+static int mx6s_vidioc_g_fmt_vid_cap(struct file *file, void *priv,
+				    struct v4l2_format *f)
+{
+	struct mx6s_csi_dev *csi_dev = video_drvdata(file);
+
+	WARN_ON(priv != file->private_data);
+
+	f->fmt.pix = csi_dev->pix;
+
+	return 0;
+}
+
+static int mx6s_vidioc_querycap(struct file *file, void  *priv,
+			       struct v4l2_capability *cap)
+{
+	struct mx6s_csi_dev *csi_dev = video_drvdata(file);
+
+	WARN_ON(priv != file->private_data);
+
+	/* cap->name is set by the friendly caller:-> */
+	strlcpy(cap->driver, MX6S_CAM_DRV_NAME, sizeof(cap->driver));
+	strlcpy(cap->card, MX6S_CAM_DRIVER_DESCRIPTION, sizeof(cap->card));
+	snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s",
+		 dev_name(csi_dev->dev));
+
+	cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+	return 0;
+}
+
+static int mx6s_vidioc_expbuf(struct file *file, void *priv,
+			     struct v4l2_exportbuffer *eb)
+{
+	struct mx6s_csi_dev *csi_dev = video_drvdata(file);
+
+	if (eb->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return vb2_expbuf(&csi_dev->vb2_vidq, eb);
+
+	return -EINVAL;
+}
+
+static int mx6s_vidioc_streamon(struct file *file, void *priv,
+			       enum v4l2_buf_type i)
+{
+	struct mx6s_csi_dev *csi_dev = video_drvdata(file);
+	struct v4l2_subdev *sd = csi_dev->sd;
+	int ret;
+
+	WARN_ON(priv != file->private_data);
+
+	if (i != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+    v4l2_subdev_call(sd, video, s_stream, 1);
+	ret = vb2_streamon(&csi_dev->vb2_vidq, i);
+
+	return ret;
+}
+
+static int mx6s_vidioc_streamoff(struct file *file, void *priv,
+				enum v4l2_buf_type i)
+{
+	struct mx6s_csi_dev *csi_dev = video_drvdata(file);
+	struct v4l2_subdev *sd = csi_dev->sd;
+	int ret;
+
+	WARN_ON(priv != file->private_data);
+
+	if (i != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	/*
+	 * This calls buf_release from host driver's videobuf_queue_ops for all
+	 * remaining buffers. When the last buffer is freed, stop capture
+	 */
+	ret = vb2_streamoff(&csi_dev->vb2_vidq, i);
+	if (!ret)
+		v4l2_subdev_call(sd, video, s_stream, 0);
+
+	return ret;
+}
+
+static int mx6s_vidioc_g_pixelaspect(struct file *file, void *fh,
+			       int type, struct v4l2_fract *f)
+{
+	struct mx6s_csi_dev *csi_dev = video_drvdata(file);
+
+	if (type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+	dev_dbg(csi_dev->dev, "G_PIXELASPECT not implemented\n");
+
+	return 0;
+}
+
+static int mx6s_vidioc_g_selection(struct file *file, void *priv,
+			     struct v4l2_selection *s)
+{
+	struct mx6s_csi_dev *csi_dev = video_drvdata(file);
+
+	if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+	dev_dbg(csi_dev->dev, "VIDIOC_G_SELECTION not implemented\n");
+
+	return 0;
+}
+
+static int mx6s_vidioc_s_selection(struct file *file, void *priv,
+			     struct v4l2_selection *s)
+{
+	struct mx6s_csi_dev *csi_dev = video_drvdata(file);
+
+	if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	dev_dbg(csi_dev->dev, "VIDIOC_S_SELECTION not implemented\n");
+
+	return 0;
+}
+
+static int mx6s_vidioc_g_parm(struct file *file, void *priv,
+			     struct v4l2_streamparm *a)
+{
+	struct mx6s_csi_dev *csi_dev = video_drvdata(file);
+	struct v4l2_subdev *sd = csi_dev->sd;
+
+	return v4l2_subdev_call(sd, video, g_parm, a);
+}
+
+static int mx6s_vidioc_s_parm(struct file *file, void *priv,
+				struct v4l2_streamparm *a)
+{
+	struct mx6s_csi_dev *csi_dev = video_drvdata(file);
+	struct v4l2_subdev *sd = csi_dev->sd;
+
+	return v4l2_subdev_call(sd, video, s_parm, a);
+}
+
+static int mx6s_vidioc_enum_framesizes(struct file *file, void *priv,
+					 struct v4l2_frmsizeenum *fsize)
+{
+	struct mx6s_csi_dev *csi_dev = video_drvdata(file);
+	struct v4l2_subdev *sd = csi_dev->sd;
+	struct mx6s_fmt *fmt;
+	struct v4l2_subdev_frame_size_enum fse = {
+		.index = fsize->index,
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	int ret;
+
+	fmt = format_by_fourcc(fsize->pixel_format);
+	if (fmt->pixelformat != fsize->pixel_format)
+		return -EINVAL;
+	fse.code = fmt->mbus_code;
+
+	ret = v4l2_subdev_call(sd, pad, enum_frame_size, NULL, &fse);
+	if (ret)
+		return ret;
+
+	if (fse.min_width == fse.max_width &&
+	    fse.min_height == fse.max_height) {
+		fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
+		fsize->discrete.width = fse.min_width;
+		fsize->discrete.height = fse.min_height;
+		return 0;
+	}
+
+	fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
+	fsize->stepwise.min_width = fse.min_width;
+	fsize->stepwise.max_width = fse.max_width;
+	fsize->stepwise.min_height = fse.min_height;
+	fsize->stepwise.max_height = fse.max_height;
+	fsize->stepwise.step_width = 1;
+	fsize->stepwise.step_height = 1;
+
+	return 0;
+}
+
+static int mx6s_vidioc_enum_frameintervals(struct file *file, void *priv,
+		struct v4l2_frmivalenum *interval)
+{
+	struct mx6s_csi_dev *csi_dev = video_drvdata(file);
+	struct v4l2_subdev *sd = csi_dev->sd;
+	struct mx6s_fmt *fmt;
+	struct v4l2_subdev_frame_interval_enum fie = {
+		.index = interval->index,
+		.width = interval->width,
+		.height = interval->height,
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	int ret;
+
+	fmt = format_by_fourcc(interval->pixel_format);
+	if (fmt->pixelformat != interval->pixel_format)
+		return -EINVAL;
+	fie.code = fmt->mbus_code;
+
+	ret = v4l2_subdev_call(sd, pad, enum_frame_interval, NULL, &fie);
+	if (ret)
+		return ret;
+	interval->type = V4L2_FRMIVAL_TYPE_DISCRETE;
+	interval->discrete = fie.interval;
+	return 0;
+}
+
+static const struct v4l2_ioctl_ops mx6s_csi_ioctl_ops = {
+	.vidioc_querycap          = mx6s_vidioc_querycap,
+	.vidioc_enum_fmt_vid_cap  = mx6s_vidioc_enum_fmt_vid_cap,
+	.vidioc_try_fmt_vid_cap   = mx6s_vidioc_try_fmt_vid_cap,
+	.vidioc_g_fmt_vid_cap     = mx6s_vidioc_g_fmt_vid_cap,
+	.vidioc_s_fmt_vid_cap     = mx6s_vidioc_s_fmt_vid_cap,
+	.vidioc_g_pixelaspect     = mx6s_vidioc_g_pixelaspect,
+	.vidioc_s_selection   = mx6s_vidioc_s_selection,
+	.vidioc_g_selection   = mx6s_vidioc_g_selection,
+	.vidioc_reqbufs       = mx6s_vidioc_reqbufs,
+	.vidioc_querybuf      = mx6s_vidioc_querybuf,
+	.vidioc_qbuf          = mx6s_vidioc_qbuf,
+	.vidioc_dqbuf         = mx6s_vidioc_dqbuf,
+	.vidioc_g_std         = mx6s_vidioc_g_std,
+	.vidioc_s_std         = mx6s_vidioc_s_std,
+	.vidioc_querystd      = mx6s_vidioc_querystd,
+	.vidioc_enum_input    = mx6s_vidioc_enum_input,
+	.vidioc_g_input       = mx6s_vidioc_g_input,
+	.vidioc_s_input       = mx6s_vidioc_s_input,
+	.vidioc_expbuf        = mx6s_vidioc_expbuf,
+	.vidioc_streamon      = mx6s_vidioc_streamon,
+	.vidioc_streamoff     = mx6s_vidioc_streamoff,
+	.vidioc_g_parm        = mx6s_vidioc_g_parm,
+	.vidioc_s_parm        = mx6s_vidioc_s_parm,
+	.vidioc_enum_framesizes = mx6s_vidioc_enum_framesizes,
+	.vidioc_enum_frameintervals = mx6s_vidioc_enum_frameintervals,
+};
+
+static int subdev_notifier_bound(struct v4l2_async_notifier *notifier,
+			    struct v4l2_subdev *subdev,
+			    struct v4l2_async_subdev *asd)
+{
+	struct mx6s_csi_dev *csi_dev = notifier_to_mx6s_dev(notifier);
+
+	if (subdev == NULL)
+		return -EINVAL;
+
+	/* Find platform data for this sensor subdev */
+	if (csi_dev->asd.match.fwnode == dev_fwnode(subdev->dev))
+		csi_dev->sd = subdev;
+
+	v4l2_info(&csi_dev->v4l2_dev, "Registered sensor subdevice: %s\n",
+		  subdev->name);
+
+	return 0;
+}
+
+static int mx6s_csi_mode_sel(struct mx6s_csi_dev *csi_dev)
+{
+	struct device_node *np = csi_dev->dev->of_node;
+	struct device_node *node;
+	phandle phandle;
+	u32 out_val[3];
+	int ret = 0;
+
+	if (of_get_property(np, "fsl,mipi-mode", NULL))
+		csi_dev->csi_mipi_mode = true;
+	else {
+		csi_dev->csi_mipi_mode = false;
+		return ret;
+	}
+
+	ret = of_property_read_u32_array(np, "csi-mux-mipi", out_val, 3);
+	if (ret) {
+		dev_dbg(csi_dev->dev, "no csi-mux-mipi property found\n");
+	} else {
+		phandle = *out_val;
+
+		node = of_find_node_by_phandle(phandle);
+		if (!node) {
+			dev_dbg(csi_dev->dev, "not find gpr node by phandle\n");
+			ret = PTR_ERR(node);
+		}
+		csi_dev->csi_mux.gpr = syscon_node_to_regmap(node);
+		if (IS_ERR(csi_dev->csi_mux.gpr)) {
+			dev_err(csi_dev->dev, "failed to get gpr regmap\n");
+			ret = PTR_ERR(csi_dev->csi_mux.gpr);
+		}
+		of_node_put(node);
+		if (ret < 0)
+			return ret;
+
+		csi_dev->csi_mux.req_gpr = out_val[1];
+		csi_dev->csi_mux.req_bit = out_val[2];
+
+		regmap_update_bits(csi_dev->csi_mux.gpr, csi_dev->csi_mux.req_gpr,
+			1 << csi_dev->csi_mux.req_bit, 1 << csi_dev->csi_mux.req_bit);
+	}
+	return ret;
+}
+
+static const struct v4l2_async_notifier_operations mx6s_capture_async_ops = {
+	.bound = subdev_notifier_bound,
+};
+
+static int mx6s_csi_two_8bit_sensor_mode_sel(struct mx6s_csi_dev *csi_dev)
+{
+	struct device_node *np = csi_dev->dev->of_node;
+
+	if (of_get_property(np, "fsl,two-8bit-sensor-mode", NULL))
+		csi_dev->csi_two_8bit_sensor_mode = true;
+	else {
+		csi_dev->csi_two_8bit_sensor_mode = false;
+	}
+
+	return 0;
+}
+
+static int mx6sx_register_subdevs(struct mx6s_csi_dev *csi_dev)
+{
+	struct device_node *parent = csi_dev->dev->of_node;
+	struct device_node *node, *port, *rem;
+	int ret;
+
+	v4l2_async_notifier_init(&csi_dev->subdev_notifier);
+
+	/* Attach sensors linked to csi receivers */
+	for_each_available_child_of_node(parent, node) {
+		if (of_node_cmp(node->name, "port"))
+			continue;
+
+		/* The csi node can have only port subnode. */
+		port = of_get_next_child(node, NULL);
+		if (!port)
+			continue;
+		rem = of_graph_get_remote_port_parent(port);
+		of_node_put(port);
+		if (rem == NULL) {
+			v4l2_info(&csi_dev->v4l2_dev,
+						"Remote device at %s not found\n",
+						port->full_name);
+			return -1;
+		}
+
+		csi_dev->asd.match_type = V4L2_ASYNC_MATCH_FWNODE;
+		csi_dev->asd.match.fwnode = of_fwnode_handle(rem);
+
+		ret = v4l2_async_notifier_add_subdev(&csi_dev->subdev_notifier,
+						&csi_dev->asd);
+		if (ret) {
+			of_node_put(rem);
+		}
+
+		of_node_put(rem);
+		break;
+	}
+
+	csi_dev->subdev_notifier.ops = &mx6s_capture_async_ops;
+
+	ret = v4l2_async_notifier_register(&csi_dev->v4l2_dev,
+					&csi_dev->subdev_notifier);
+	if (ret)
+		dev_err(csi_dev->dev,
+					"Error register async notifier regoster\n");
+
+	return ret;
+}
+
+static int mx6s_csi_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	const struct of_device_id *of_id;
+	struct mx6s_csi_dev *csi_dev;
+	struct video_device *vdev;
+	struct resource *res;
+	int ret = 0;
+
+	dev_info(dev, "initialising\n");
+
+	/* Prepare our private structure */
+	csi_dev = devm_kzalloc(dev, sizeof(struct mx6s_csi_dev), GFP_ATOMIC);
+	if (!csi_dev) {
+		dev_err(dev, "Can't allocate private structure\n");
+		return -ENODEV;
+	}
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	csi_dev->irq = platform_get_irq(pdev, 0);
+	if (res == NULL || csi_dev->irq < 0) {
+		dev_err(dev, "Missing platform resources data\n");
+		return -ENODEV;
+	}
+
+	csi_dev->regbase = devm_ioremap_resource(dev, res);
+	if (IS_ERR(csi_dev->regbase)) {
+		dev_err(dev, "Failed platform resources map\n");
+		return -ENODEV;
+	}
+
+	/* init video dma queues */
+	INIT_LIST_HEAD(&csi_dev->capture);
+	INIT_LIST_HEAD(&csi_dev->active_bufs);
+	INIT_LIST_HEAD(&csi_dev->discard);
+
+	csi_dev->clk_disp_axi = devm_clk_get(dev, "disp-axi");
+	if (IS_ERR(csi_dev->clk_disp_axi)) {
+		dev_err(dev, "Could not get csi axi clock\n");
+		return -ENODEV;
+	}
+
+	csi_dev->clk_disp_dcic = devm_clk_get(dev, "disp_dcic");
+	if (IS_ERR(csi_dev->clk_disp_dcic)) {
+		dev_err(dev, "Could not get disp dcic clock\n");
+		return -ENODEV;
+	}
+
+	csi_dev->clk_csi_mclk = devm_clk_get(dev, "csi_mclk");
+	if (IS_ERR(csi_dev->clk_csi_mclk)) {
+		dev_err(dev, "Could not get csi mclk clock\n");
+		return -ENODEV;
+	}
+
+	csi_dev->dev = dev;
+
+	mx6s_csi_mode_sel(csi_dev);
+	mx6s_csi_two_8bit_sensor_mode_sel(csi_dev);
+
+	of_id = of_match_node(mx6s_csi_dt_ids, csi_dev->dev->of_node);
+	if (!of_id)
+		return -EINVAL;
+	csi_dev->soc = of_id->data;
+
+	snprintf(csi_dev->v4l2_dev.name,
+		 sizeof(csi_dev->v4l2_dev.name), "CSI");
+
+	ret = v4l2_device_register(dev, &csi_dev->v4l2_dev);
+	if (ret < 0) {
+		dev_err(dev, "v4l2_device_register() failed: %d\n", ret);
+		return -ENODEV;
+	}
+
+	/* initialize locks */
+	mutex_init(&csi_dev->lock);
+	spin_lock_init(&csi_dev->slock);
+
+	/* Allocate memory for video device */
+	vdev = video_device_alloc();
+	if (vdev == NULL) {
+		ret = -ENOMEM;
+		goto err_vdev;
+	}
+
+	snprintf(vdev->name, sizeof(vdev->name), "mx6s-csi");
+
+	vdev->v4l2_dev		= &csi_dev->v4l2_dev;
+	vdev->fops			= &mx6s_csi_fops;
+	vdev->ioctl_ops		= &mx6s_csi_ioctl_ops;
+	vdev->release		= video_device_release;
+	vdev->lock			= &csi_dev->lock;
+	vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
+
+	vdev->queue = &csi_dev->vb2_vidq;
+
+	csi_dev->vdev = vdev;
+
+	video_set_drvdata(csi_dev->vdev, csi_dev);
+	mutex_lock(&csi_dev->lock);
+
+	ret = video_register_device(csi_dev->vdev, VFL_TYPE_VIDEO, -1);
+	if (ret < 0) {
+		video_device_release(csi_dev->vdev);
+		mutex_unlock(&csi_dev->lock);
+		goto err_vdev;
+	}
+
+	/* install interrupt handler */
+	if (devm_request_irq(dev, csi_dev->irq, mx6s_csi_irq_handler,
+				0, "csi", (void *)csi_dev)) {
+		mutex_unlock(&csi_dev->lock);
+		dev_err(dev, "Request CSI IRQ failed.\n");
+		ret = -ENODEV;
+		goto err_irq;
+	}
+
+	mutex_unlock(&csi_dev->lock);
+
+	ret = mx6sx_register_subdevs(csi_dev);
+	if (ret < 0)
+		goto err_irq;
+
+	pm_runtime_enable(csi_dev->dev);
+	return 0;
+
+err_irq:
+	video_unregister_device(csi_dev->vdev);
+err_vdev:
+	v4l2_device_unregister(&csi_dev->v4l2_dev);
+	return ret;
+}
+
+static int mx6s_csi_remove(struct platform_device *pdev)
+{
+	struct v4l2_device *v4l2_dev = dev_get_drvdata(&pdev->dev);
+	struct mx6s_csi_dev *csi_dev =
+				container_of(v4l2_dev, struct mx6s_csi_dev, v4l2_dev);
+
+	v4l2_async_notifier_unregister(&csi_dev->subdev_notifier);
+
+	video_unregister_device(csi_dev->vdev);
+	v4l2_device_unregister(&csi_dev->v4l2_dev);
+
+	pm_runtime_disable(csi_dev->dev);
+	return 0;
+}
+
+static int mx6s_csi_runtime_suspend(struct device *dev)
+{
+	dev_dbg(dev, "csi v4l2 busfreq high release.\n");
+	return 0;
+}
+
+static int mx6s_csi_runtime_resume(struct device *dev)
+{
+	dev_dbg(dev, "csi v4l2 busfreq high request.\n");
+	return 0;
+}
+
+static const struct dev_pm_ops mx6s_csi_pm_ops = {
+	SET_RUNTIME_PM_OPS(mx6s_csi_runtime_suspend, mx6s_csi_runtime_resume, NULL)
+};
+
+static const struct mx6s_csi_soc mx6s_soc = {
+	.rx_fifo_rst = true,
+	.baseaddr_switch = 0,
+};
+static const struct mx6s_csi_soc mx6sl_soc = {
+	.rx_fifo_rst = false,
+	.baseaddr_switch = 0,
+};
+static const struct mx6s_csi_soc mx8mq_soc = {
+	.rx_fifo_rst = true,
+	.baseaddr_switch = 0x80030,
+};
+
+static const struct of_device_id mx6s_csi_dt_ids[] = {
+	{ .compatible = "fsl,imx6s-csi",
+	  .data = &mx6s_soc,
+	},
+	{ .compatible = "fsl,imx6sl-csi",
+	  .data = &mx6sl_soc,
+	},
+	{ .compatible = "fsl,imx8mq-csi",
+	  .data = &mx8mq_soc,
+	},
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, mx6s_csi_dt_ids);
+
+static struct platform_driver mx6s_csi_driver = {
+	.driver		= {
+		.name	= MX6S_CAM_DRV_NAME,
+		.of_match_table = of_match_ptr(mx6s_csi_dt_ids),
+		.pm = &mx6s_csi_pm_ops,
+	},
+	.probe	= mx6s_csi_probe,
+	.remove	= mx6s_csi_remove,
+};
+
+module_platform_driver(mx6s_csi_driver);
+
+MODULE_DESCRIPTION("i.MX6Sx SoC Camera Host driver");
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_LICENSE("GPL");
+MODULE_VERSION(MX6S_CAM_VERSION);
diff --git a/drivers/media/platform/mxc/capture/mxc_mipi_csi.c b/drivers/media/platform/mxc/capture/mxc_mipi_csi.c
new file mode 100644
index 000000000..069a0b9bb
--- /dev/null
+++ b/drivers/media/platform/mxc/capture/mxc_mipi_csi.c
@@ -0,0 +1,1316 @@
+/* SPDX-License-Identifier: GPL-2.0-or-later */
+/*
+ * Freescale i.MX7 SoC series MIPI-CSI V3.3 receiver driver
+ *
+ * Copyright (C) 2015-2016 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2019 NXP
+ */
+/*
+ * Samsung S5P/EXYNOS SoC series MIPI-CSI receiver driver
+ *
+ * Copyright (C) 2011 - 2013 Samsung Electronics Co., Ltd.
+ * Author: Sylwester Nawrocki <s.nawrocki@samsung.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.
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/errno.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/irq.h>
+#include <linux/kernel.h>
+#include <linux/mfd/syscon.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_graph.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/regmap.h>
+#include <linux/regulator/consumer.h>
+#include <linux/reset.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-subdev.h>
+#include <media/v4l2-device.h>
+
+static int debug;
+module_param(debug, int, 0644);
+MODULE_PARM_DESC(debug, "Debug level (0-2)");
+
+#define CSIS_DRIVER_NAME	"mxc_mipi-csi"
+#define CSIS_SUBDEV_NAME	CSIS_DRIVER_NAME
+#define CSIS_MAX_ENTITIES	2
+#define CSIS0_MAX_LANES		4
+#define CSIS1_MAX_LANES		2
+
+#define MIPI_CSIS_DEF_PIX_WIDTH	640
+#define MIPI_CSIS_DEF_PIX_HEIGHT	480
+
+/* Register map definition */
+
+/* CSIS version */
+#define MIPI_CSIS_VERSION		0x00
+
+/* CSIS common control */
+#define MIPI_CSIS_CMN_CTRL			0x04
+#define MIPI_CSIS_CMN_CTRL_UPDATE_SHADOW	(1 << 16)
+#define MIPI_CSIS_CMN_CTRL_INTER_MODE		(1 << 10)
+#define MIPI_CSIS_CMN_CTRL_LANE_NR_OFFSET	8
+#define MIPI_CSIS_CMN_CTRL_LANE_NR_MASK		(3 << 8)
+#define MIPI_CSIS_CMN_CTRL_UPDATE_SHADOW_CTRL	(1 << 2)
+#define MIPI_CSIS_CMN_CTRL_RESET		(1 << 1)
+#define MIPI_CSIS_CMN_CTRL_ENABLE		(1 << 0)
+
+/* CSIS clock control */
+#define MIPI_CSIS_CLK_CTRL		0x08
+#define MIPI_CSIS_CLK_CTRL_CLKGATE_TRAIL_CH3(x)	(x << 28)
+#define MIPI_CSIS_CLK_CTRL_CLKGATE_TRAIL_CH2(x)	(x << 24)
+#define MIPI_CSIS_CLK_CTRL_CLKGATE_TRAIL_CH1(x)	(x << 20)
+#define MIPI_CSIS_CLK_CTRL_CLKGATE_TRAIL_CH0(x)	(x << 16)
+#define MIPI_CSIS_CLK_CTRL_CLKGATE_EN_MSK	(0xf << 4)
+#define MIPI_CSIS_CLK_CTRL_WCLK_SRC		(1 << 0)
+
+/* CSIS Interrupt mask */
+#define MIPI_CSIS_INTMSK			0x10
+#define MIPI_CSIS_INTMSK_EVEN_BEFORE	(1 << 31)
+#define MIPI_CSIS_INTMSK_EVEN_AFTER	(1 << 30)
+#define MIPI_CSIS_INTMSK_ODD_BEFORE	(1 << 29)
+#define MIPI_CSIS_INTMSK_ODD_AFTER	(1 << 28)
+#define MIPI_CSIS_INTMSK_FRAME_START	(1 << 24)
+#define MIPI_CSIS_INTMSK_FRAME_END	(1 << 20)
+#define MIPI_CSIS_INTMSK_ERR_SOT_HS	(1 << 16)
+#define MIPI_CSIS_INTMSK_ERR_LOST_FS	(1 << 12)
+#define MIPI_CSIS_INTMSK_ERR_LOST_FE	(1 << 8)
+#define MIPI_CSIS_INTMSK_ERR_OVER		(1 << 4)
+#define MIPI_CSIS_INTMSK_ERR_WRONG_CFG	(1 << 3)
+#define MIPI_CSIS_INTMSK_ERR_ECC		(1 << 2)
+#define MIPI_CSIS_INTMSK_ERR_CRC		(1 << 1)
+#define MIPI_CSIS_INTMSK_ERR_UNKNOWN	(1 << 0)
+
+/* CSIS Interrupt source */
+#define MIPI_CSIS_INTSRC			0x14
+#define MIPI_CSIS_INTSRC_EVEN_BEFORE	(1 << 31)
+#define MIPI_CSIS_INTSRC_EVEN_AFTER	(1 << 30)
+#define MIPI_CSIS_INTSRC_EVEN		(0x3 << 30)
+#define MIPI_CSIS_INTSRC_ODD_BEFORE	(1 << 29)
+#define MIPI_CSIS_INTSRC_ODD_AFTER	(1 << 28)
+#define MIPI_CSIS_INTSRC_ODD			(0x3 << 28)
+#define MIPI_CSIS_INTSRC_NON_IMAGE_DATA	(0xf << 28)
+#define MIPI_CSIS_INTSRC_FRAME_START	(1 << 24)
+#define MIPI_CSIS_INTSRC_FRAME_END	(1 << 20)
+#define MIPI_CSIS_INTSRC_ERR_SOT_HS	(1 << 16)
+#define MIPI_CSIS_INTSRC_ERR_LOST_FS	(1 << 12)
+#define MIPI_CSIS_INTSRC_ERR_LOST_FE	(1 << 8)
+#define MIPI_CSIS_INTSRC_ERR_OVER		(1 << 4)
+#define MIPI_CSIS_INTSRC_ERR_WRONG_CFG	(1 << 3)
+#define MIPI_CSIS_INTSRC_ERR_ECC		(1 << 2)
+#define MIPI_CSIS_INTSRC_ERR_CRC		(1 << 1)
+#define MIPI_CSIS_INTSRC_ERR_UNKNOWN	(1 << 0)
+#define MIPI_CSIS_INTSRC_ERRORS		0xfffff
+
+/* D-PHY status control */
+#define MIPI_CSIS_DPHYSTATUS		0x20
+#define MIPI_CSIS_DPHYSTATUS_ULPS_DAT			(1 << 8)
+#define MIPI_CSIS_DPHYSTATUS_STOPSTATE_DAT		(1 << 4)
+#define MIPI_CSIS_DPHYSTATUS_ULPS_CLK			(1 << 1)
+#define MIPI_CSIS_DPHYSTATUS_STOPSTATE_CLK		(1 << 0)
+
+/* D-PHY common control */
+#define MIPI_CSIS_DPHYCTRL		0x24
+#define MIPI_CSIS_DPHYCTRL_HSS_MASK			(0xff << 24)
+#define MIPI_CSIS_DPHYCTRL_HSS_OFFSET		24
+#define MIPI_CSIS_DPHYCTRL_SCLKS_MASK		(0x3 << 22)
+#define MIPI_CSIS_DPHYCTRL_SCLKS_OFFSET		22
+#define MIPI_CSIS_DPHYCTRL_DPDN_SWAP_CLK	(1 << 6)
+#define MIPI_CSIS_DPHYCTRL_DPDN_SWAP_DAT	(1 << 5)
+#define MIPI_CSIS_DPHYCTRL_ENABLE_DAT		(1 << 1)
+#define MIPI_CSIS_DPHYCTRL_ENABLE_CLK		(1 << 0)
+#define MIPI_CSIS_DPHYCTRL_ENABLE			(0x1f << 0)
+
+/* D-PHY Master and Slave Control register Low */
+#define MIPI_CSIS_DPHYBCTRL_L		0x30
+/* D-PHY Master and Slave Control register High */
+#define MIPI_CSIS_DPHYBCTRL_H		0x34
+/* D-PHY Slave Control register Low */
+#define MIPI_CSIS_DPHYSCTRL_L		0x38
+/* D-PHY Slave Control register High */
+#define MIPI_CSIS_DPHYSCTRL_H		0x3c
+
+
+/* ISP Configuration register */
+#define MIPI_CSIS_ISPCONFIG_CH0			0x40
+#define MIPI_CSIS_ISPCONFIG_CH1			0x50
+#define MIPI_CSIS_ISPCONFIG_CH2			0x60
+#define MIPI_CSIS_ISPCONFIG_CH3			0x70
+
+#define MIPI_CSIS_ISPCFG_MEM_FULL_GAP_MSK    (0xff << 24)
+#define MIPI_CSIS_ISPCFG_MEM_FULL_GAP(x)     (x << 24)
+#define MIPI_CSIS_ISPCFG_DOUBLE_CMPNT        (1 << 12)
+#define MIPI_CSIS_ISPCFG_ALIGN_32BIT         (1 << 11)
+#define MIPI_CSIS_ISPCFG_FMT_YCBCR422_8BIT   (0x1e << 2)
+#define MIPI_CSIS_ISPCFG_FMT_RAW8		(0x2a << 2)
+#define MIPI_CSIS_ISPCFG_FMT_RAW10		(0x2b << 2)
+#define MIPI_CSIS_ISPCFG_FMT_RAW12		(0x2c << 2)
+/* User defined formats, x = 1...4 */
+#define MIPI_CSIS_ISPCFG_FMT_USER(x)	((0x30 + x - 1) << 2)
+#define MIPI_CSIS_ISPCFG_FMT_MASK		(0x3f << 2)
+
+/* ISP Image Resolution register */
+#define MIPI_CSIS_ISPRESOL_CH0			0x44
+#define MIPI_CSIS_ISPRESOL_CH1			0x54
+#define MIPI_CSIS_ISPRESOL_CH2			0x64
+#define MIPI_CSIS_ISPRESOL_CH3			0x74
+#define CSIS_MAX_PIX_WIDTH		0xffff
+#define CSIS_MAX_PIX_HEIGHT		0xffff
+
+/* ISP SYNC register */
+#define MIPI_CSIS_ISPSYNC_CH0			0x48
+#define MIPI_CSIS_ISPSYNC_CH1			0x58
+#define MIPI_CSIS_ISPSYNC_CH2			0x68
+#define MIPI_CSIS_ISPSYNC_CH3			0x78
+
+#define MIPI_CSIS_ISPSYNC_HSYNC_LINTV_OFFSET	18
+#define MIPI_CSIS_ISPSYNC_VSYNC_SINTV_OFFSET 	12
+#define MIPI_CSIS_ISPSYNC_VSYNC_EINTV_OFFSET	0
+
+/* Non-image packet data buffers */
+#define MIPI_CSIS_PKTDATA_ODD		0x2000
+#define MIPI_CSIS_PKTDATA_EVEN		0x3000
+#define MIPI_CSIS_PKTDATA_SIZE		SZ_4K
+
+#define DEFAULT_SCLK_CSIS_FREQ	166000000UL
+
+enum {
+	ST_POWERED	= 1,
+	ST_STREAMING	= 2,
+	ST_SUSPENDED	= 4,
+};
+
+struct mipi_csis_event {
+	u32 mask;
+	const char * const name;
+	unsigned int counter;
+};
+
+static const struct mipi_csis_event mipi_csis_events[] = {
+	/* Errors */
+	{ MIPI_CSIS_INTSRC_ERR_SOT_HS,	"SOT Error" },
+	{ MIPI_CSIS_INTSRC_ERR_LOST_FS,	"Lost Frame Start Error" },
+	{ MIPI_CSIS_INTSRC_ERR_LOST_FE,	"Lost Frame End Error" },
+	{ MIPI_CSIS_INTSRC_ERR_OVER,	"FIFO Overflow Error" },
+	{ MIPI_CSIS_INTSRC_ERR_ECC,	"ECC Error" },
+	{ MIPI_CSIS_INTSRC_ERR_CRC,	"CRC Error" },
+	{ MIPI_CSIS_INTSRC_ERR_UNKNOWN,	"Unknown Error" },
+	/* Non-image data receive events */
+	{ MIPI_CSIS_INTSRC_EVEN_BEFORE,	"Non-image data before even frame" },
+	{ MIPI_CSIS_INTSRC_EVEN_AFTER,	"Non-image data after even frame" },
+	{ MIPI_CSIS_INTSRC_ODD_BEFORE,	"Non-image data before odd frame" },
+	{ MIPI_CSIS_INTSRC_ODD_AFTER,	"Non-image data after odd frame" },
+	/* Frame start/end */
+	{ MIPI_CSIS_INTSRC_FRAME_START,	"Frame Start" },
+	{ MIPI_CSIS_INTSRC_FRAME_END,	"Frame End" },
+};
+#define MIPI_CSIS_NUM_EVENTS ARRAY_SIZE(mipi_csis_events)
+
+struct csis_pktbuf {
+	u32 *data;
+	unsigned int len;
+};
+
+struct csis_hw_reset {
+	struct regmap *src;
+	u8 req_src;
+	u8 rst_bit;
+};
+
+/**
+ * struct csi_state - the driver's internal state data structure
+ * @lock: mutex serializing the subdev and power management operations,
+ *        protecting @format and @flags members
+ * @sd: v4l2_subdev associated with CSIS device instance
+ * @index: the hardware instance index
+ * @pdev: CSIS platform device
+ * @phy: pointer to the CSIS generic PHY
+ * @regs: mmaped I/O registers memory
+ * @supplies: CSIS regulator supplies
+ * @clock: CSIS clocks
+ * @irq: requested s5p-mipi-csis irq number
+ * @flags: the state variable for power and streaming control
+ * @clock_frequency: device bus clock frequency
+ * @hs_settle: HS-RX settle time
+ * @clk_settle: Clk settle time
+ * @num_lanes: number of MIPI-CSI data lanes used
+ * @max_num_lanes: maximum number of MIPI-CSI data lanes supported
+ * @wclk_ext: CSI wrapper clock: 0 - bus clock, 1 - external SCLK_CAM
+ * @csis_fmt: current CSIS pixel format
+ * @format: common media bus format for the source and sink pad
+ * @slock: spinlock protecting structure members below
+ * @pkt_buf: the frame embedded (non-image) data buffer
+ * @events: MIPI-CSIS event (error) counters
+ */
+struct csi_state {
+	struct mutex lock;
+	struct device		*dev;
+	struct v4l2_subdev mipi_sd;
+	struct v4l2_subdev *sensor_sd;
+	struct v4l2_device	v4l2_dev;
+
+	u8 index;
+	struct platform_device *pdev;
+	struct phy *phy;
+	void __iomem *regs;
+	struct clk *mipi_clk;
+	struct clk *phy_clk;
+	struct clk *disp_axi;
+	struct clk *disp_apb;
+	int irq;
+	u32 flags;
+
+	u32 clk_frequency;
+	u32 hs_settle;
+	u32 clk_settle;
+	u32 num_lanes;
+	u32 max_num_lanes;
+	u8 wclk_ext;
+
+	const struct csis_pix_format *csis_fmt;
+	struct v4l2_mbus_framefmt format;
+
+	spinlock_t slock;
+	struct csis_pktbuf pkt_buf;
+	struct mipi_csis_event events[MIPI_CSIS_NUM_EVENTS];
+
+	struct v4l2_async_subdev    asd;
+	struct v4l2_async_notifier  subdev_notifier;
+	struct v4l2_async_subdev    *async_subdevs[2];
+
+	struct csis_hw_reset hw_reset;
+	struct regulator     *mipi_phy_regulator;
+};
+
+/**
+ * struct csis_pix_format - CSIS pixel format description
+ * @pix_width_alignment: horizontal pixel alignment, width will be
+ *                       multiple of 2^pix_width_alignment
+ * @code: corresponding media bus code
+ * @fmt_reg: MIPI_CSIS_CONFIG register value
+ * @data_alignment: MIPI-CSI data alignment in bits
+ */
+struct csis_pix_format {
+	unsigned int pix_width_alignment;
+	u32 code;
+	u32 fmt_reg;
+	u8 data_alignment;
+};
+
+static const struct csis_pix_format mipi_csis_formats[] = {
+	{
+		.code = MEDIA_BUS_FMT_YUYV8_2X8,
+		.fmt_reg = MIPI_CSIS_ISPCFG_FMT_YCBCR422_8BIT,
+		.data_alignment = 16,
+	}, {
+		.code = MEDIA_BUS_FMT_VYUY8_2X8,
+		.fmt_reg = MIPI_CSIS_ISPCFG_FMT_YCBCR422_8BIT,
+		.data_alignment = 16,
+	}, {
+		.code = MEDIA_BUS_FMT_SBGGR8_1X8,
+		.fmt_reg = MIPI_CSIS_ISPCFG_FMT_RAW8,
+		.data_alignment = 8,
+	}
+};
+
+typedef int (*mipi_csis_phy_reset_t)(struct csi_state *state);
+
+#define mipi_csis_write(__csis, __r, __v) writel(__v, __csis->regs + __r)
+#define mipi_csis_read(__csis, __r) readl(__csis->regs + __r)
+
+static struct csi_state *mipi_sd_to_csi_state(struct v4l2_subdev *sdev)
+{
+	return container_of(sdev, struct csi_state, mipi_sd);
+}
+
+static inline struct csi_state
+				*notifier_to_mipi_dev(struct v4l2_async_notifier *n)
+{
+	return container_of(n, struct csi_state, subdev_notifier);
+}
+
+static const struct csis_pix_format *find_csis_format(u32 code)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(mipi_csis_formats); i++)
+		if (code == mipi_csis_formats[i].code)
+			return &mipi_csis_formats[i];
+	return NULL;
+}
+
+static void mipi_csis_enable_interrupts(struct csi_state *state, bool on)
+{
+	u32 val = mipi_csis_read(state, MIPI_CSIS_INTMSK);
+	if (on)
+		val |= 0xf00fffff;
+	else
+		val &= ~0xf00fffff;
+	mipi_csis_write(state, MIPI_CSIS_INTMSK, val);
+}
+
+static void mipi_csis_sw_reset(struct csi_state *state)
+{
+	u32 val = mipi_csis_read(state, MIPI_CSIS_CMN_CTRL);
+
+	mipi_csis_write(state, MIPI_CSIS_CMN_CTRL, val | MIPI_CSIS_CMN_CTRL_RESET);
+	udelay(10);
+}
+
+static int mipi_csis_phy_init(struct csi_state *state)
+{
+	int ret;
+
+	state->mipi_phy_regulator = devm_regulator_get(state->dev,
+				"mipi-phy");
+
+	ret = regulator_set_voltage(state->mipi_phy_regulator,
+			1000000, 1000000);
+
+	return ret;
+}
+
+static int mipi_csis_phy_reset_mx8mm(struct csi_state *state)
+{
+	struct reset_control *phy_reset;
+
+	phy_reset = devm_reset_control_get_exclusive(state->dev, "csi,mipi_rst");
+	if (IS_ERR(phy_reset))
+		return PTR_ERR(phy_reset);
+
+	reset_control_assert(phy_reset);
+	usleep_range(10, 20);
+	reset_control_deassert(phy_reset);
+	usleep_range(10, 20);
+
+	return 0;
+
+}
+
+static int mipi_csis_phy_reset(struct csi_state *state)
+{
+	struct device_node *np = state->dev->of_node;
+	struct device_node *node;
+	phandle phandle;
+	u32 out_val[3];
+	int ret;
+
+	ret = of_property_read_u32_array(np, "csis-phy-reset", out_val, 3);
+	if (ret) {
+		dev_dbg(state->dev, "no csis-hw-reset property found\n");
+	} else {
+		phandle = *out_val;
+
+		node = of_find_node_by_phandle(phandle);
+		if (!node) {
+			dev_dbg(state->dev, "not find src node by phandle\n");
+			ret = PTR_ERR(node);
+		}
+		state->hw_reset.src = syscon_node_to_regmap(node);
+		if (IS_ERR(state->hw_reset.src)) {
+			dev_err(state->dev, "failed to get src regmap\n");
+			ret = PTR_ERR(state->hw_reset.src);
+		}
+		of_node_put(node);
+		if (ret < 0)
+			return ret;
+
+		state->hw_reset.req_src = out_val[1];
+		state->hw_reset.rst_bit = out_val[2];
+
+		/* reset mipi phy */
+		regmap_update_bits(state->hw_reset.src, state->hw_reset.req_src,
+			1 << state->hw_reset.rst_bit, 1 << state->hw_reset.rst_bit);
+		msleep(20);
+		regmap_update_bits(state->hw_reset.src, state->hw_reset.req_src,
+			1 << state->hw_reset.rst_bit, 0);
+
+	}
+	return ret;
+}
+
+static void mipi_csis_system_enable(struct csi_state *state, int on)
+{
+	u32 val, mask;
+
+	val = mipi_csis_read(state, MIPI_CSIS_CMN_CTRL);
+	if (on)
+		val |= MIPI_CSIS_CMN_CTRL_ENABLE;
+	else
+		val &= ~MIPI_CSIS_CMN_CTRL_ENABLE;
+	mipi_csis_write(state, MIPI_CSIS_CMN_CTRL, val);
+
+	val = mipi_csis_read(state, MIPI_CSIS_DPHYCTRL);
+	val &= ~MIPI_CSIS_DPHYCTRL_ENABLE;
+	if (on) {
+		mask = (1 << (state->num_lanes + 1)) - 1;
+		val |= (mask & MIPI_CSIS_DPHYCTRL_ENABLE);
+	}
+	mipi_csis_write(state, MIPI_CSIS_DPHYCTRL, val);
+}
+
+/* Called with the state.lock mutex held */
+static void __mipi_csis_set_format(struct csi_state *state)
+{
+	struct v4l2_mbus_framefmt *mf = &state->format;
+	u32 val;
+
+	v4l2_dbg(1, debug, &state->mipi_sd, "fmt: %#x, %d x %d\n",
+		 mf->code, mf->width, mf->height);
+
+	/* Color format */
+	val = mipi_csis_read(state, MIPI_CSIS_ISPCONFIG_CH0);
+	val = (val & ~MIPI_CSIS_ISPCFG_FMT_MASK) | state->csis_fmt->fmt_reg;
+	mipi_csis_write(state, MIPI_CSIS_ISPCONFIG_CH0, val);
+
+	/* Pixel resolution */
+	val = mf->width | (mf->height << 16);
+	mipi_csis_write(state, MIPI_CSIS_ISPRESOL_CH0, val);
+}
+
+static void mipi_csis_set_hsync_settle(struct csi_state *state,
+								int hs_settle, int clk_settle)
+{
+	u32 val = mipi_csis_read(state, MIPI_CSIS_DPHYCTRL);
+
+	val = (val & ~MIPI_CSIS_DPHYCTRL_HSS_MASK) |
+				(hs_settle << 24) | (clk_settle << 22);
+
+	mipi_csis_write(state, MIPI_CSIS_DPHYCTRL, val);
+}
+
+static void mipi_csis_set_params(struct csi_state *state)
+{
+	u32 val;
+
+	val = mipi_csis_read(state, MIPI_CSIS_CMN_CTRL);
+	val &= ~MIPI_CSIS_CMN_CTRL_LANE_NR_MASK;
+	val |= (state->num_lanes - 1) << MIPI_CSIS_CMN_CTRL_LANE_NR_OFFSET;
+	mipi_csis_write(state, MIPI_CSIS_CMN_CTRL, val);
+
+	__mipi_csis_set_format(state);
+
+	mipi_csis_set_hsync_settle(state, state->hs_settle, state->clk_settle);
+
+	val = mipi_csis_read(state, MIPI_CSIS_ISPCONFIG_CH0);
+	if (state->csis_fmt->data_alignment == 32)
+		val |= MIPI_CSIS_ISPCFG_ALIGN_32BIT;
+	else /* Normal output */
+		val &= ~MIPI_CSIS_ISPCFG_ALIGN_32BIT;
+	mipi_csis_write(state, MIPI_CSIS_ISPCONFIG_CH0, val);
+
+	val = (0 << MIPI_CSIS_ISPSYNC_HSYNC_LINTV_OFFSET) |
+		(0 << MIPI_CSIS_ISPSYNC_VSYNC_SINTV_OFFSET) |
+		(0 << MIPI_CSIS_ISPSYNC_VSYNC_EINTV_OFFSET);
+	mipi_csis_write(state, MIPI_CSIS_ISPSYNC_CH0, val);
+
+	val = mipi_csis_read(state, MIPI_CSIS_CLK_CTRL);
+	val &= ~MIPI_CSIS_CLK_CTRL_WCLK_SRC;
+	if (state->wclk_ext)
+		val |= MIPI_CSIS_CLK_CTRL_WCLK_SRC;
+	val |= MIPI_CSIS_CLK_CTRL_CLKGATE_TRAIL_CH0(15);
+	val &= ~MIPI_CSIS_CLK_CTRL_CLKGATE_EN_MSK;
+	mipi_csis_write(state, MIPI_CSIS_CLK_CTRL, val);
+
+	mipi_csis_write(state, MIPI_CSIS_DPHYBCTRL_L, 0x1f4);
+	mipi_csis_write(state, MIPI_CSIS_DPHYBCTRL_H, 0);
+
+	/* Update the shadow register. */
+	val = mipi_csis_read(state, MIPI_CSIS_CMN_CTRL);
+	mipi_csis_write(state, MIPI_CSIS_CMN_CTRL, val | MIPI_CSIS_CMN_CTRL_UPDATE_SHADOW |
+					MIPI_CSIS_CMN_CTRL_UPDATE_SHADOW_CTRL);
+}
+
+static void mipi_csis_clk_enable(struct csi_state *state)
+{
+	clk_prepare_enable(state->mipi_clk);
+	clk_prepare_enable(state->phy_clk);
+	if (state->disp_axi)
+		clk_prepare_enable(state->disp_axi);
+	if (state->disp_apb)
+		clk_prepare_enable(state->disp_apb);
+}
+
+static void mipi_csis_clk_disable(struct csi_state *state)
+{
+	clk_disable_unprepare(state->mipi_clk);
+	clk_disable_unprepare(state->phy_clk);
+	if (state->disp_axi)
+		clk_disable_unprepare(state->disp_axi);
+	if (state->disp_apb)
+		clk_disable_unprepare(state->disp_apb);
+}
+
+static int mipi_csis_clk_get(struct csi_state *state)
+{
+	struct device *dev = &state->pdev->dev;
+	int ret = true;
+
+	state->mipi_clk = devm_clk_get(dev, "mipi_clk");
+	if (IS_ERR(state->mipi_clk)) {
+		dev_err(dev, "Could not get mipi csi clock\n");
+		return -ENODEV;
+	}
+
+	state->phy_clk = devm_clk_get(dev, "phy_clk");
+	if (IS_ERR(state->phy_clk)) {
+		dev_err(dev, "Could not get mipi phy clock\n");
+		return -ENODEV;
+	}
+
+	state->disp_axi = devm_clk_get(dev, "disp_axi");
+	if (IS_ERR(state->disp_axi)) {
+		dev_warn(dev, "Could not get disp_axi clock\n");
+		state->disp_axi = NULL;
+	}
+
+	state->disp_apb = devm_clk_get(dev, "disp_apb");
+	if (IS_ERR(state->disp_apb)) {
+		dev_warn(dev, "Could not get disp apb clock\n");
+		state->disp_apb = NULL;
+	}
+
+	/* Set clock rate */
+	if (state->clk_frequency)
+		ret = clk_set_rate(state->mipi_clk,
+				   state->clk_frequency);
+	else
+		dev_WARN(dev, "No clock frequency specified!\n");
+	if (ret < 0) {
+		dev_err(dev, "set rate filed, rate=%d\n", state->clk_frequency);
+		return -EINVAL;
+	}
+
+	return ret;
+}
+
+static void dump_regs(struct csi_state *state, const char *label)
+{
+	struct {
+		u32 offset;
+		const char * const name;
+	} registers[] = {
+		{ 0x00, "CTRL" },
+		{ 0x04, "DPHYCTRL" },
+		{ 0x08, "CONFIG" },
+		{ 0x0c, "DPHYSTS" },
+		{ 0x10, "INTMSK" },
+		{ 0x2c, "RESOL" },
+		{ 0x38, "SDW_CONFIG" },
+	};
+	u32 i;
+
+	v4l2_info(&state->mipi_sd, "--- %s ---\n", label);
+
+	for (i = 0; i < ARRAY_SIZE(registers); i++) {
+		u32 cfg = mipi_csis_read(state, registers[i].offset);
+		v4l2_info(&state->mipi_sd, "%10s: 0x%08x\n", registers[i].name, cfg);
+	}
+}
+
+static void mipi_csis_start_stream(struct csi_state *state)
+{
+	mipi_csis_sw_reset(state);
+	mipi_csis_set_params(state);
+	mipi_csis_system_enable(state, true);
+	mipi_csis_enable_interrupts(state, true);
+}
+
+static void mipi_csis_stop_stream(struct csi_state *state)
+{
+	mipi_csis_enable_interrupts(state, false);
+	mipi_csis_system_enable(state, false);
+}
+
+static void mipi_csis_clear_counters(struct csi_state *state)
+{
+	unsigned long flags;
+	int i;
+
+	spin_lock_irqsave(&state->slock, flags);
+	for (i = 0; i < MIPI_CSIS_NUM_EVENTS; i++)
+		state->events[i].counter = 0;
+	spin_unlock_irqrestore(&state->slock, flags);
+}
+
+static void mipi_csis_log_counters(struct csi_state *state, bool non_errors)
+{
+	int i = non_errors ? MIPI_CSIS_NUM_EVENTS : MIPI_CSIS_NUM_EVENTS - 4;
+	unsigned long flags;
+
+	spin_lock_irqsave(&state->slock, flags);
+
+	for (i--; i >= 0; i--) {
+		if (state->events[i].counter > 0 || debug)
+			v4l2_info(&state->mipi_sd, "%s events: %d\n",
+				  state->events[i].name,
+				  state->events[i].counter);
+	}
+	spin_unlock_irqrestore(&state->slock, flags);
+}
+
+/*
+ * V4L2 subdev operations
+ */
+static int mipi_csis_s_power(struct v4l2_subdev *mipi_sd, int on)
+{
+	struct csi_state *state = mipi_sd_to_csi_state(mipi_sd);
+	struct device *dev = &state->pdev->dev;
+
+	v4l2_subdev_call(state->sensor_sd, core, s_power, on);
+
+	if (on)
+		return pm_runtime_get_sync(dev);
+
+	return pm_runtime_put_sync(dev);
+}
+
+static int mipi_csis_s_stream(struct v4l2_subdev *mipi_sd, int enable)
+{
+	struct csi_state *state = mipi_sd_to_csi_state(mipi_sd);
+	int ret = 0;
+
+	v4l2_dbg(1, debug, mipi_sd, "%s: %d, state: 0x%x\n",
+		 __func__, enable, state->flags);
+
+	if (enable) {
+		mipi_csis_clear_counters(state);
+		ret = pm_runtime_get_sync(&state->pdev->dev);
+		if (ret && ret != 1)
+			return ret;
+	}
+
+	mutex_lock(&state->lock);
+	if (enable) {
+		if (state->flags & ST_SUSPENDED) {
+			ret = -EBUSY;
+			goto unlock;
+		}
+		mipi_csis_start_stream(state);
+		v4l2_subdev_call(state->sensor_sd, video, s_stream, true);
+		state->flags |= ST_STREAMING;
+	} else {
+		v4l2_subdev_call(state->sensor_sd, video, s_stream, false);
+		mipi_csis_stop_stream(state);
+		state->flags &= ~ST_STREAMING;
+		if (debug > 0)
+			mipi_csis_log_counters(state, true);
+	}
+unlock:
+	mutex_unlock(&state->lock);
+	if (!enable)
+		pm_runtime_put(&state->pdev->dev);
+
+	return ret == 1 ? 0 : ret;
+}
+
+static int mipi_csis_enum_mbus_code(struct v4l2_subdev *mipi_sd,
+				    struct v4l2_subdev_pad_config *cfg,
+				    struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct csi_state *state = mipi_sd_to_csi_state(mipi_sd);
+	struct v4l2_subdev *sensor_sd = state->sensor_sd;
+	struct csis_pix_format const *csis_fmt;
+	int ret;
+
+	ret = v4l2_subdev_call(sensor_sd, pad, enum_mbus_code, NULL, code);
+	if (ret < 0)
+		return -EINVAL;
+
+	csis_fmt = find_csis_format(code->code);
+	if (csis_fmt == NULL) {
+		dev_err(state->dev, "format not match\n");
+		return -EINVAL;
+	}
+
+	return ret;
+}
+
+static int mipi_csis_set_fmt(struct v4l2_subdev *mipi_sd,
+			     struct v4l2_subdev_pad_config *cfg,
+			     struct v4l2_subdev_format *format)
+{
+	struct csi_state *state = mipi_sd_to_csi_state(mipi_sd);
+	struct v4l2_subdev *sensor_sd = state->sensor_sd;
+	struct csis_pix_format const *csis_fmt;
+	struct v4l2_mbus_framefmt *mf  = &format->format;
+
+	if (format->pad)
+		return -EINVAL;
+
+	csis_fmt = find_csis_format(mf->code);
+	if (csis_fmt == NULL)
+		csis_fmt = &mipi_csis_formats[0];
+
+	v4l2_subdev_call(sensor_sd, pad, set_fmt, NULL, format);
+
+	mf->code = csis_fmt->code;
+	v4l_bound_align_image(&mf->width, 1, CSIS_MAX_PIX_WIDTH,
+			      csis_fmt->pix_width_alignment,
+			      &mf->height, 1, CSIS_MAX_PIX_HEIGHT, 1,
+			      0);
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY)
+		return 0;
+
+	state->format.code = mf->code;
+	state->format.width = mf->width;
+	state->format.height = mf->height;
+
+	mutex_lock(&state->lock);
+	state->csis_fmt = csis_fmt;
+	mutex_unlock(&state->lock);
+
+	return 0;
+}
+
+static int mipi_csis_get_fmt(struct v4l2_subdev *mipi_sd,
+			     struct v4l2_subdev_pad_config *cfg,
+			     struct v4l2_subdev_format *format)
+{
+	struct csi_state *state = mipi_sd_to_csi_state(mipi_sd);
+	struct v4l2_subdev *sensor_sd = state->sensor_sd;
+
+	if (format->pad)
+		return -EINVAL;
+
+	return v4l2_subdev_call(sensor_sd, pad, get_fmt, NULL, format);
+}
+
+static int mipi_csis_s_rx_buffer(struct v4l2_subdev *mipi_sd, void *buf,
+			       unsigned int *size)
+{
+	struct csi_state *state = mipi_sd_to_csi_state(mipi_sd);
+	unsigned long flags;
+
+	*size = min_t(unsigned int, *size, MIPI_CSIS_PKTDATA_SIZE);
+
+	spin_lock_irqsave(&state->slock, flags);
+	state->pkt_buf.data = buf;
+	state->pkt_buf.len = *size;
+	spin_unlock_irqrestore(&state->slock, flags);
+
+	return 0;
+}
+
+static int mipi_csis_s_parm(struct v4l2_subdev *mipi_sd, struct v4l2_streamparm *a)
+{
+	struct csi_state *state = mipi_sd_to_csi_state(mipi_sd);
+	struct v4l2_subdev *sensor_sd = state->sensor_sd;
+
+	return v4l2_subdev_call(sensor_sd, video, s_parm, a);
+}
+
+static int mipi_csis_g_parm(struct v4l2_subdev *mipi_sd, struct v4l2_streamparm *a)
+{
+	struct csi_state *state = mipi_sd_to_csi_state(mipi_sd);
+	struct v4l2_subdev *sensor_sd = state->sensor_sd;
+
+	return v4l2_subdev_call(sensor_sd, video, g_parm, a);
+}
+
+static int mipi_csis_enum_framesizes(struct v4l2_subdev *mipi_sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_frame_size_enum *fse)
+{
+	struct csi_state *state = mipi_sd_to_csi_state(mipi_sd);
+	struct v4l2_subdev *sensor_sd = state->sensor_sd;
+
+	return v4l2_subdev_call(sensor_sd, pad, enum_frame_size, NULL, fse);
+}
+
+static int mipi_csis_enum_frameintervals(struct v4l2_subdev *mipi_sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_frame_interval_enum *fie)
+{
+	struct csi_state *state = mipi_sd_to_csi_state(mipi_sd);
+	struct v4l2_subdev *sensor_sd = state->sensor_sd;
+
+	return v4l2_subdev_call(sensor_sd, pad, enum_frame_interval, NULL, fie);
+}
+
+static int mipi_csis_log_status(struct v4l2_subdev *mipi_sd)
+{
+	struct csi_state *state = mipi_sd_to_csi_state(mipi_sd);
+
+	mutex_lock(&state->lock);
+	mipi_csis_log_counters(state, true);
+	if (debug && (state->flags & ST_POWERED))
+		dump_regs(state, __func__);
+	mutex_unlock(&state->lock);
+	return 0;
+}
+
+static struct v4l2_subdev_core_ops mipi_csis_core_ops = {
+	.s_power = mipi_csis_s_power,
+	.log_status = mipi_csis_log_status,
+};
+
+static struct v4l2_subdev_video_ops mipi_csis_video_ops = {
+	.s_rx_buffer = mipi_csis_s_rx_buffer,
+	.s_stream = mipi_csis_s_stream,
+
+	.s_parm = mipi_csis_s_parm,
+	.g_parm = mipi_csis_g_parm,
+};
+
+static const struct v4l2_subdev_pad_ops mipi_csis_pad_ops = {
+	.enum_frame_size       = mipi_csis_enum_framesizes,
+	.enum_frame_interval   = mipi_csis_enum_frameintervals,
+	.enum_mbus_code        = mipi_csis_enum_mbus_code,
+	.get_fmt               = mipi_csis_get_fmt,
+	.set_fmt               = mipi_csis_set_fmt,
+};
+
+static struct v4l2_subdev_ops mipi_csis_subdev_ops = {
+	.core = &mipi_csis_core_ops,
+	.video = &mipi_csis_video_ops,
+	.pad = &mipi_csis_pad_ops,
+};
+
+static irqreturn_t mipi_csis_irq_handler(int irq, void *dev_id)
+{
+	struct csi_state *state = dev_id;
+	struct csis_pktbuf *pktbuf = &state->pkt_buf;
+	unsigned long flags;
+	u32 status;
+
+	status = mipi_csis_read(state, MIPI_CSIS_INTSRC);
+
+	spin_lock_irqsave(&state->slock, flags);
+
+	if ((status & MIPI_CSIS_INTSRC_NON_IMAGE_DATA) && pktbuf->data) {
+		u32 offset;
+
+		if (status & MIPI_CSIS_INTSRC_EVEN)
+			offset = MIPI_CSIS_PKTDATA_EVEN;
+		else
+			offset = MIPI_CSIS_PKTDATA_ODD;
+
+		memcpy(pktbuf->data, state->regs + offset, pktbuf->len);
+		pktbuf->data = NULL;
+		rmb();
+	}
+
+	/* Update the event/error counters */
+	if ((status & MIPI_CSIS_INTSRC_ERRORS) || debug) {
+		int i;
+		for (i = 0; i < MIPI_CSIS_NUM_EVENTS; i++) {
+			if (!(status & state->events[i].mask))
+				continue;
+			state->events[i].counter++;
+			v4l2_dbg(2, debug, &state->mipi_sd, "%s: %d\n",
+				 state->events[i].name,
+				 state->events[i].counter);
+		}
+		v4l2_dbg(2, debug, &state->mipi_sd, "status: %08x\n", status);
+	}
+	spin_unlock_irqrestore(&state->slock, flags);
+
+	mipi_csis_write(state, MIPI_CSIS_INTSRC, status);
+	return IRQ_HANDLED;
+}
+
+static int subdev_notifier_bound(struct v4l2_async_notifier *notifier,
+			    struct v4l2_subdev *subdev,
+			    struct v4l2_async_subdev *asd)
+{
+	struct csi_state *state = notifier_to_mipi_dev(notifier);
+
+	/* Find platform data for this sensor subdev */
+	if (state->asd.match.fwnode == dev_fwnode(subdev->dev))
+		state->sensor_sd = subdev;
+
+	if (subdev == NULL)
+		return -EINVAL;
+
+	v4l2_info(&state->v4l2_dev, "Registered sensor subdevice: %s\n",
+		  subdev->name);
+
+	return 0;
+}
+
+static int mipi_csis_parse_dt(struct platform_device *pdev,
+			    struct csi_state *state)
+{
+	struct device_node *node = pdev->dev.of_node;
+
+	if (of_property_read_u32(node, "clock-frequency",
+				 &state->clk_frequency))
+		state->clk_frequency = DEFAULT_SCLK_CSIS_FREQ;
+	if (of_property_read_u32(node, "bus-width",
+				 &state->max_num_lanes))
+		return -EINVAL;
+
+	node = of_graph_get_next_endpoint(node, NULL);
+	if (!node) {
+		dev_err(&pdev->dev, "No port node at %s\n",
+				pdev->dev.of_node->full_name);
+		return -EINVAL;
+	}
+
+	/* Get MIPI CSI-2 bus configration from the endpoint node. */
+	of_property_read_u32(node, "csis-hs-settle",
+					&state->hs_settle);
+
+	of_property_read_u32(node, "csis-clk-settle",
+					&state->clk_settle);
+	state->wclk_ext = of_property_read_bool(node,
+					"csis-wclk");
+
+	of_property_read_u32(node, "data-lanes",
+					&state->num_lanes);
+	of_node_put(node);
+
+	return 0;
+}
+
+static int mipi_csis_pm_resume(struct device *dev, bool runtime);
+static const struct of_device_id mipi_csis_of_match[];
+
+static const struct v4l2_async_notifier_operations mxc_mipi_csi_subdev_ops = {
+	.bound = subdev_notifier_bound,
+};
+
+/* register parent dev */
+static int mipi_csis_subdev_host(struct csi_state *state)
+{
+	struct device_node *parent = state->dev->of_node;
+	struct device_node *node, *port, *rem;
+	int ret;
+
+	v4l2_async_notifier_init(&state->subdev_notifier);
+
+	/* Attach sensors linked to csi receivers */
+	for_each_available_child_of_node(parent, node) {
+		if (of_node_cmp(node->name, "port"))
+			continue;
+
+		/* The csi node can have only port subnode. */
+		port = of_get_next_child(node, NULL);
+		if (!port)
+			continue;
+		rem = of_graph_get_remote_port_parent(port);
+		of_node_put(port);
+		if (rem == NULL) {
+			v4l2_info(&state->v4l2_dev,
+						"Remote device at %s not found\n",
+						port->full_name);
+			return -1;
+		}
+
+		INIT_LIST_HEAD(&state->asd.list);
+		state->asd.match_type = V4L2_ASYNC_MATCH_FWNODE;
+		state->asd.match.fwnode = of_fwnode_handle(rem);
+		state->async_subdevs[0] = &state->asd;
+
+		of_node_put(rem);
+		break;
+	}
+
+	ret = v4l2_async_notifier_add_subdev(&state->subdev_notifier,
+					&state->asd);
+	if (ret) {
+		dev_err(state->dev, "failed to add subdev to a notifier\n");
+		return ret;
+	}
+	state->subdev_notifier.v4l2_dev = &state->v4l2_dev;
+	state->subdev_notifier.ops = &mxc_mipi_csi_subdev_ops;
+
+	ret = v4l2_async_notifier_register(&state->v4l2_dev,
+					&state->subdev_notifier);
+	if (ret)
+		dev_err(state->dev,
+					"Error register async notifier regoster\n");
+
+	return ret;
+}
+
+/* init subdev */
+static int mipi_csis_subdev_init(struct v4l2_subdev *mipi_sd,
+		struct platform_device *pdev,
+		const struct v4l2_subdev_ops *ops)
+{
+	struct csi_state *state = platform_get_drvdata(pdev);
+	int ret = 0;
+
+	v4l2_subdev_init(mipi_sd, ops);
+	mipi_sd->owner = THIS_MODULE;
+	snprintf(mipi_sd->name, sizeof(mipi_sd->name), "%s.%d",
+		 CSIS_SUBDEV_NAME, state->index);
+	mipi_sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	mipi_sd->dev = &pdev->dev;
+
+	state->csis_fmt = &mipi_csis_formats[0];
+	state->format.code = mipi_csis_formats[0].code;
+	state->format.width = MIPI_CSIS_DEF_PIX_WIDTH;
+	state->format.height = MIPI_CSIS_DEF_PIX_HEIGHT;
+
+	/* This allows to retrieve the platform device id by the host driver */
+	v4l2_set_subdevdata(mipi_sd, pdev);
+
+	ret = v4l2_async_register_subdev(mipi_sd);
+	if (ret < 0)
+		dev_err(&pdev->dev, "%s--Async register faialed, ret=%d\n", __func__, ret);
+
+	return ret;
+}
+
+static int mipi_csis_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct v4l2_subdev *mipi_sd;
+	struct resource *mem_res;
+	struct csi_state *state;
+	const struct of_device_id *of_id;
+	mipi_csis_phy_reset_t phy_reset_fn;
+	int ret = -ENOMEM;
+
+	state = devm_kzalloc(dev, sizeof(*state), GFP_KERNEL);
+	if (!state)
+		return -ENOMEM;
+
+	mutex_init(&state->lock);
+	spin_lock_init(&state->slock);
+
+	state->pdev = pdev;
+	mipi_sd = &state->mipi_sd;
+	state->dev = dev;
+
+	ret = mipi_csis_parse_dt(pdev, state);
+	if (ret < 0)
+		return ret;
+
+	if (state->num_lanes == 0 || state->num_lanes > state->max_num_lanes) {
+		dev_err(dev, "Unsupported number of data lanes: %d (max. %d)\n",
+			state->num_lanes, state->max_num_lanes);
+		return -EINVAL;
+	}
+
+	mipi_csis_phy_init(state);
+	of_id = of_match_node(mipi_csis_of_match, dev->of_node);
+	if (!of_id || !of_id->data)
+		return -EINVAL;
+
+	phy_reset_fn = of_id->data;
+
+	mem_res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	state->regs = devm_ioremap_resource(dev, mem_res);
+	if (IS_ERR(state->regs))
+		return PTR_ERR(state->regs);
+
+	state->irq = platform_get_irq(pdev, 0);
+	if (state->irq < 0) {
+		dev_err(dev, "Failed to get irq\n");
+		return state->irq;
+	}
+
+	ret = mipi_csis_clk_get(state);
+	if (ret < 0)
+		return ret;
+
+	mipi_csis_clk_enable(state);
+
+	phy_reset_fn(state);
+
+	ret = devm_request_irq(dev, state->irq, mipi_csis_irq_handler,
+			       0, dev_name(dev), state);
+	if (ret) {
+		dev_err(dev, "Interrupt request failed\n");
+		goto e_clkdis;
+	}
+
+	/* First register a v4l2 device */
+	ret = v4l2_device_register(dev, &state->v4l2_dev);
+	if (ret) {
+		v4l2_err(dev->driver,
+			"Unable to register v4l2 device.\n");
+		goto e_clkdis;
+	}
+	v4l2_info(&state->v4l2_dev, "mipi csi v4l2 device registered\n");
+
+	/* .. and a pointer to the subdev. */
+	platform_set_drvdata(pdev, state);
+
+	ret = mipi_csis_subdev_init(&state->mipi_sd, pdev, &mipi_csis_subdev_ops);
+	if (ret < 0)
+		goto e_sd_mipi;
+
+	memcpy(state->events, mipi_csis_events, sizeof(state->events));
+
+	/* subdev host register */
+	ret = mipi_csis_subdev_host(state);
+	if (ret < 0)
+		goto e_sd_host;
+
+	pm_runtime_enable(dev);
+	if (!pm_runtime_enabled(dev)) {
+		ret = mipi_csis_pm_resume(dev, true);
+		if (ret < 0)
+			goto e_sd_host;
+	}
+
+	mipi_csis_clk_disable(state);
+	dev_info(&pdev->dev,
+			"lanes: %d, hs_settle: %d, clk_settle: %d, wclk: %d, freq: %u\n",
+		 state->num_lanes, state->hs_settle, state->clk_settle,
+		 state->wclk_ext, state->clk_frequency);
+	return 0;
+
+e_sd_host:
+	v4l2_async_notifier_unregister(&state->subdev_notifier);
+	v4l2_device_unregister(&state->v4l2_dev);
+e_sd_mipi:
+	v4l2_async_unregister_subdev(&state->mipi_sd);
+e_clkdis:
+	mipi_csis_clk_disable(state);
+	return ret;
+}
+
+static int mipi_csis_pm_suspend(struct device *dev, bool runtime)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct csi_state *state = platform_get_drvdata(pdev);
+	struct v4l2_subdev *mipi_sd = &state->mipi_sd;
+	int ret = 0;
+
+	v4l2_dbg(1, debug, mipi_sd, "%s: flags: 0x%x\n",
+		 __func__, state->flags);
+
+	mutex_lock(&state->lock);
+	if (state->flags & ST_POWERED) {
+		mipi_csis_stop_stream(state);
+		ret = regulator_disable(state->mipi_phy_regulator);
+		if (ret)
+			goto unlock;
+		mipi_csis_clk_disable(state);
+		state->flags &= ~ST_POWERED;
+		if (!runtime)
+			state->flags |= ST_SUSPENDED;
+	}
+ unlock:
+	mutex_unlock(&state->lock);
+	return ret ? -EAGAIN : 0;
+}
+
+static int mipi_csis_pm_resume(struct device *dev, bool runtime)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct csi_state *state = platform_get_drvdata(pdev);
+	struct v4l2_subdev *mipi_sd = &state->mipi_sd;
+	int ret = 0;
+
+	v4l2_dbg(1, debug, mipi_sd, "%s: flags: 0x%x\n",
+		 __func__, state->flags);
+
+	mutex_lock(&state->lock);
+	if (!runtime && !(state->flags & ST_SUSPENDED))
+		goto unlock;
+
+	if (!(state->flags & ST_POWERED)) {
+		ret = regulator_enable(state->mipi_phy_regulator);
+		if (!ret) {
+			state->flags |= ST_POWERED;
+		} else {
+			goto unlock;
+		}
+		mipi_csis_clk_enable(state);
+	}
+	if (state->flags & ST_STREAMING)
+		mipi_csis_start_stream(state);
+
+	state->flags &= ~ST_SUSPENDED;
+ unlock:
+	mutex_unlock(&state->lock);
+	return ret ? -EAGAIN : 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int mipi_csis_suspend(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct csi_state *state = platform_get_drvdata(pdev);
+
+	if (state->flags & ST_STREAMING) {
+		dev_warn(dev, "running, prevent entering suspend.\n");
+		return -EAGAIN;
+	}
+
+	return mipi_csis_pm_suspend(dev, false);
+}
+
+static int mipi_csis_resume(struct device *dev)
+{
+	return mipi_csis_pm_resume(dev, false);
+}
+#endif
+
+static int mipi_csis_runtime_suspend(struct device *dev)
+{
+	return mipi_csis_pm_suspend(dev, true);
+}
+
+static int mipi_csis_runtime_resume(struct device *dev)
+{
+	return mipi_csis_pm_resume(dev, true);
+}
+
+static int mipi_csis_remove(struct platform_device *pdev)
+{
+	struct csi_state *state = platform_get_drvdata(pdev);
+
+	v4l2_async_unregister_subdev(&state->mipi_sd);
+	v4l2_async_notifier_unregister(&state->subdev_notifier);
+	v4l2_device_unregister(&state->v4l2_dev);
+
+	pm_runtime_disable(&pdev->dev);
+	mipi_csis_pm_suspend(&pdev->dev, true);
+	pm_runtime_set_suspended(&pdev->dev);
+
+	return 0;
+}
+
+static const struct dev_pm_ops mipi_csis_pm_ops = {
+	SET_RUNTIME_PM_OPS(mipi_csis_runtime_suspend, mipi_csis_runtime_resume,
+			   NULL)
+	SET_SYSTEM_SLEEP_PM_OPS(mipi_csis_suspend, mipi_csis_resume)
+};
+
+static const struct of_device_id mipi_csis_of_match[] = {
+	{	.compatible = "fsl,imx7d-mipi-csi",
+		.data = (void *)&mipi_csis_phy_reset,
+	},
+	{	.compatible = "fsl,imx8mm-mipi-csi",
+		.data = (void *)&mipi_csis_phy_reset_mx8mm,
+	},
+	{ /* sentinel */ },
+};
+MODULE_DEVICE_TABLE(of, mipi_csis_of_match);
+
+static struct platform_driver mipi_csis_driver = {
+	.probe		= mipi_csis_probe,
+	.remove		= mipi_csis_remove,
+	.driver		= {
+		.of_match_table = mipi_csis_of_match,
+		.name		= CSIS_DRIVER_NAME,
+		.owner		= THIS_MODULE,
+		.pm		= &mipi_csis_pm_ops,
+	},
+};
+
+module_platform_driver(mipi_csis_driver);
+
+MODULE_DESCRIPTION("Freescale MIPI-CSI2 receiver driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/platform/mxc/capture/ov5640_mipi_v2.c b/drivers/media/platform/mxc/capture/ov5640_mipi_v2.c
new file mode 100644
index 000000000..2d347ea12
--- /dev/null
+++ b/drivers/media/platform/mxc/capture/ov5640_mipi_v2.c
@@ -0,0 +1,1881 @@
+/*
+ * Copyright (C) 2011-2016 Freescale Semiconductor, Inc. All Rights Reserved.
+ *
+ * Copyright 2018-2019 NXP
+ *
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/ctype.h>
+#include <linux/types.h>
+#include <linux/delay.h>
+#include <linux/clk.h>
+#include <linux/of_device.h>
+#include <linux/i2c.h>
+#include <linux/of_gpio.h>
+#include <linux/pinctrl/consumer.h>
+#include <linux/regulator/consumer.h>
+#include <linux/v4l2-mediabus.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
+
+#define OV5640_VOLTAGE_ANALOG               2800000
+#define OV5640_VOLTAGE_DIGITAL_CORE         1500000
+#define OV5640_VOLTAGE_DIGITAL_IO           1800000
+
+#define MIN_FPS 15
+#define MAX_FPS 30
+#define DEFAULT_FPS 30
+
+#define OV5640_XCLK_MIN 6000000
+#define OV5640_XCLK_MAX 24000000
+#define OV5640_XCLK_20MHZ 20000000
+
+#define OV5640_CHIP_ID_HIGH_BYTE	0x300A
+#define OV5640_CHIP_ID_LOW_BYTE		0x300B
+
+#define DEFAULT_SCCB_ID 0x78
+
+enum ov5640_mode {
+	ov5640_mode_MIN = 0,
+	ov5640_mode_VGA_640_480 = 0,
+	ov5640_mode_QVGA_320_240 = 1,
+	ov5640_mode_NTSC_720_480 = 2,
+	ov5640_mode_720P_1280_720 = 3,
+	ov5640_mode_1080P_1920_1080 = 4,
+	ov5640_mode_QSXGA_2592_1944 = 5,
+	ov5640_mode_MAX = 5,
+	ov5640_mode_INIT = 0xff, /*only for sensor init*/
+};
+
+enum ov5640_frame_rate {
+	ov5640_15_fps,
+	ov5640_30_fps
+};
+
+static int ov5640_framerates[] = {
+	[ov5640_15_fps] = 15,
+	[ov5640_30_fps] = 30,
+};
+
+struct ov5640_datafmt {
+	u32	code;
+	enum v4l2_colorspace		colorspace;
+};
+
+/* image size under 1280 * 960 are SUBSAMPLING
+ * image size upper 1280 * 960 are SCALING
+ */
+enum ov5640_downsize_mode {
+	SUBSAMPLING,
+	SCALING,
+};
+
+struct reg_value {
+	u16 u16RegAddr;
+	u8 u8Val;
+	u8 u8Mask;
+	u32 u32Delay_ms;
+};
+
+struct ov5640_mode_info {
+	enum ov5640_mode mode;
+	enum ov5640_downsize_mode dn_mode;
+	u32 width;
+	u32 height;
+	struct reg_value *init_data_ptr;
+	u32 init_data_size;
+};
+
+struct ov5640 {
+	struct v4l2_subdev		subdev;
+	struct i2c_client *i2c_client;
+	struct v4l2_pix_format pix;
+	const struct ov5640_datafmt	*fmt;
+	struct v4l2_captureparm streamcap;
+	bool on;
+
+	/* control settings */
+	int brightness;
+	int hue;
+	int contrast;
+	int saturation;
+	int red;
+	int green;
+	int blue;
+	int ae_mode;
+
+	u32 mclk;
+	u8 mclk_source;
+	struct clk *sensor_clk;
+	int csi;
+
+	void (*io_init)(struct ov5640 *);
+	int pwn_gpio, rst_gpio, gate_gpio;
+};
+
+struct ov5640_res {
+	int width;
+	int height;
+};
+
+/*!
+ * Maintains the information on the current state of the sesor.
+ */
+
+
+struct ov5640_res ov5640_valid_res[] = {
+	[0] = {640, 480},
+	[1] = {320, 240},
+	[2] = {720, 480},
+	[3] = {1280, 720},
+	[4] = {1920, 1080},
+	[5] = {2592, 1944},
+};
+
+static struct reg_value ov5640_init_setting_30fps_VGA[] = {
+
+	{0x3103, 0x11, 0, 0}, {0x3008, 0x82, 0, 5}, {0x3008, 0x42, 0, 0},
+	{0x3103, 0x03, 0, 0}, {0x3017, 0x00, 0, 0}, {0x3018, 0x00, 0, 0},
+	{0x3034, 0x18, 0, 0}, {0x3035, 0x14, 0, 0}, {0x3036, 0x38, 0, 0},
+	{0x3037, 0x13, 0, 0}, {0x3108, 0x01, 0, 0}, {0x3630, 0x36, 0, 0},
+	{0x3631, 0x0e, 0, 0}, {0x3632, 0xe2, 0, 0}, {0x3633, 0x12, 0, 0},
+	{0x3621, 0xe0, 0, 0}, {0x3704, 0xa0, 0, 0}, {0x3703, 0x5a, 0, 0},
+	{0x3715, 0x78, 0, 0}, {0x3717, 0x01, 0, 0}, {0x370b, 0x60, 0, 0},
+	{0x3705, 0x1a, 0, 0}, {0x3905, 0x02, 0, 0}, {0x3906, 0x10, 0, 0},
+	{0x3901, 0x0a, 0, 0}, {0x3731, 0x12, 0, 0}, {0x3600, 0x08, 0, 0},
+	{0x3601, 0x33, 0, 0}, {0x302d, 0x60, 0, 0}, {0x3620, 0x52, 0, 0},
+	{0x371b, 0x20, 0, 0}, {0x471c, 0x50, 0, 0}, {0x3a13, 0x43, 0, 0},
+	{0x3a18, 0x00, 0, 0}, {0x3a19, 0xf8, 0, 0}, {0x3635, 0x13, 0, 0},
+	{0x3636, 0x03, 0, 0}, {0x3634, 0x40, 0, 0}, {0x3622, 0x01, 0, 0},
+	{0x3c01, 0xa4, 0, 0}, {0x3c04, 0x28, 0, 0}, {0x3c05, 0x98, 0, 0},
+	{0x3c06, 0x00, 0, 0}, {0x3c07, 0x08, 0, 0}, {0x3c08, 0x00, 0, 0},
+	{0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
+	{0x3820, 0x41, 0, 0}, {0x3821, 0x07, 0, 0}, {0x3814, 0x31, 0, 0},
+	{0x3815, 0x31, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
+	{0x3802, 0x00, 0, 0}, {0x3803, 0x04, 0, 0}, {0x3804, 0x0a, 0, 0},
+	{0x3805, 0x3f, 0, 0}, {0x3806, 0x07, 0, 0}, {0x3807, 0x9b, 0, 0},
+	{0x3808, 0x02, 0, 0}, {0x3809, 0x80, 0, 0}, {0x380a, 0x01, 0, 0},
+	{0x380b, 0xe0, 0, 0}, {0x380c, 0x07, 0, 0}, {0x380d, 0x68, 0, 0},
+	{0x380e, 0x03, 0, 0}, {0x380f, 0xd8, 0, 0}, {0x3810, 0x00, 0, 0},
+	{0x3811, 0x10, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x06, 0, 0},
+	{0x3618, 0x00, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x64, 0, 0},
+	{0x3709, 0x52, 0, 0}, {0x370c, 0x03, 0, 0}, {0x3a02, 0x03, 0, 0},
+	{0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x27, 0, 0},
+	{0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
+	{0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
+	{0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0}, {0x3000, 0x00, 0, 0},
+	{0x3002, 0x1c, 0, 0}, {0x3004, 0xff, 0, 0}, {0x3006, 0xc3, 0, 0},
+	{0x300e, 0x45, 0, 0}, {0x302e, 0x08, 0, 0}, {0x4300, 0x30, 0, 0},
+	{0x501f, 0x00, 0, 0}, {0x4713, 0x03, 0, 0}, {0x4407, 0x04, 0, 0},
+	{0x440e, 0x00, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
+	{0x4837, 0x0a, 0, 0}, {0x4800, 0x04, 0, 0}, {0x3824, 0x02, 0, 0},
+	{0x5000, 0xa7, 0, 0}, {0x5001, 0xa3, 0, 0}, {0x5180, 0xff, 0, 0},
+	{0x5181, 0xf2, 0, 0}, {0x5182, 0x00, 0, 0}, {0x5183, 0x14, 0, 0},
+	{0x5184, 0x25, 0, 0}, {0x5185, 0x24, 0, 0}, {0x5186, 0x09, 0, 0},
+	{0x5187, 0x09, 0, 0}, {0x5188, 0x09, 0, 0}, {0x5189, 0x88, 0, 0},
+	{0x518a, 0x54, 0, 0}, {0x518b, 0xee, 0, 0}, {0x518c, 0xb2, 0, 0},
+	{0x518d, 0x50, 0, 0}, {0x518e, 0x34, 0, 0}, {0x518f, 0x6b, 0, 0},
+	{0x5190, 0x46, 0, 0}, {0x5191, 0xf8, 0, 0}, {0x5192, 0x04, 0, 0},
+	{0x5193, 0x70, 0, 0}, {0x5194, 0xf0, 0, 0}, {0x5195, 0xf0, 0, 0},
+	{0x5196, 0x03, 0, 0}, {0x5197, 0x01, 0, 0}, {0x5198, 0x04, 0, 0},
+	{0x5199, 0x6c, 0, 0}, {0x519a, 0x04, 0, 0}, {0x519b, 0x00, 0, 0},
+	{0x519c, 0x09, 0, 0}, {0x519d, 0x2b, 0, 0}, {0x519e, 0x38, 0, 0},
+	{0x5381, 0x1e, 0, 0}, {0x5382, 0x5b, 0, 0}, {0x5383, 0x08, 0, 0},
+	{0x5384, 0x0a, 0, 0}, {0x5385, 0x7e, 0, 0}, {0x5386, 0x88, 0, 0},
+	{0x5387, 0x7c, 0, 0}, {0x5388, 0x6c, 0, 0}, {0x5389, 0x10, 0, 0},
+	{0x538a, 0x01, 0, 0}, {0x538b, 0x98, 0, 0}, {0x5300, 0x08, 0, 0},
+	{0x5301, 0x30, 0, 0}, {0x5302, 0x10, 0, 0}, {0x5303, 0x00, 0, 0},
+	{0x5304, 0x08, 0, 0}, {0x5305, 0x30, 0, 0}, {0x5306, 0x08, 0, 0},
+	{0x5307, 0x16, 0, 0}, {0x5309, 0x08, 0, 0}, {0x530a, 0x30, 0, 0},
+	{0x530b, 0x04, 0, 0}, {0x530c, 0x06, 0, 0}, {0x5480, 0x01, 0, 0},
+	{0x5481, 0x08, 0, 0}, {0x5482, 0x14, 0, 0}, {0x5483, 0x28, 0, 0},
+	{0x5484, 0x51, 0, 0}, {0x5485, 0x65, 0, 0}, {0x5486, 0x71, 0, 0},
+	{0x5487, 0x7d, 0, 0}, {0x5488, 0x87, 0, 0}, {0x5489, 0x91, 0, 0},
+	{0x548a, 0x9a, 0, 0}, {0x548b, 0xaa, 0, 0}, {0x548c, 0xb8, 0, 0},
+	{0x548d, 0xcd, 0, 0}, {0x548e, 0xdd, 0, 0}, {0x548f, 0xea, 0, 0},
+	{0x5490, 0x1d, 0, 0}, {0x5580, 0x02, 0, 0}, {0x5583, 0x40, 0, 0},
+	{0x5584, 0x10, 0, 0}, {0x5589, 0x10, 0, 0}, {0x558a, 0x00, 0, 0},
+	{0x558b, 0xf8, 0, 0}, {0x5800, 0x23, 0, 0}, {0x5801, 0x14, 0, 0},
+	{0x5802, 0x0f, 0, 0}, {0x5803, 0x0f, 0, 0}, {0x5804, 0x12, 0, 0},
+	{0x5805, 0x26, 0, 0}, {0x5806, 0x0c, 0, 0}, {0x5807, 0x08, 0, 0},
+	{0x5808, 0x05, 0, 0}, {0x5809, 0x05, 0, 0}, {0x580a, 0x08, 0, 0},
+	{0x580b, 0x0d, 0, 0}, {0x580c, 0x08, 0, 0}, {0x580d, 0x03, 0, 0},
+	{0x580e, 0x00, 0, 0}, {0x580f, 0x00, 0, 0}, {0x5810, 0x03, 0, 0},
+	{0x5811, 0x09, 0, 0}, {0x5812, 0x07, 0, 0}, {0x5813, 0x03, 0, 0},
+	{0x5814, 0x00, 0, 0}, {0x5815, 0x01, 0, 0}, {0x5816, 0x03, 0, 0},
+	{0x5817, 0x08, 0, 0}, {0x5818, 0x0d, 0, 0}, {0x5819, 0x08, 0, 0},
+	{0x581a, 0x05, 0, 0}, {0x581b, 0x06, 0, 0}, {0x581c, 0x08, 0, 0},
+	{0x581d, 0x0e, 0, 0}, {0x581e, 0x29, 0, 0}, {0x581f, 0x17, 0, 0},
+	{0x5820, 0x11, 0, 0}, {0x5821, 0x11, 0, 0}, {0x5822, 0x15, 0, 0},
+	{0x5823, 0x28, 0, 0}, {0x5824, 0x46, 0, 0}, {0x5825, 0x26, 0, 0},
+	{0x5826, 0x08, 0, 0}, {0x5827, 0x26, 0, 0}, {0x5828, 0x64, 0, 0},
+	{0x5829, 0x26, 0, 0}, {0x582a, 0x24, 0, 0}, {0x582b, 0x22, 0, 0},
+	{0x582c, 0x24, 0, 0}, {0x582d, 0x24, 0, 0}, {0x582e, 0x06, 0, 0},
+	{0x582f, 0x22, 0, 0}, {0x5830, 0x40, 0, 0}, {0x5831, 0x42, 0, 0},
+	{0x5832, 0x24, 0, 0}, {0x5833, 0x26, 0, 0}, {0x5834, 0x24, 0, 0},
+	{0x5835, 0x22, 0, 0}, {0x5836, 0x22, 0, 0}, {0x5837, 0x26, 0, 0},
+	{0x5838, 0x44, 0, 0}, {0x5839, 0x24, 0, 0}, {0x583a, 0x26, 0, 0},
+	{0x583b, 0x28, 0, 0}, {0x583c, 0x42, 0, 0}, {0x583d, 0xce, 0, 0},
+	{0x5025, 0x00, 0, 0}, {0x3a0f, 0x30, 0, 0}, {0x3a10, 0x28, 0, 0},
+	{0x3a1b, 0x30, 0, 0}, {0x3a1e, 0x26, 0, 0}, {0x3a11, 0x60, 0, 0},
+	{0x3a1f, 0x14, 0, 0}, {0x3008, 0x42, 0, 0}, {0x3c00, 0x04, 0, 300},
+};
+
+static struct reg_value ov5640_setting_30fps_QVGA_320_240[] = {
+	{0x3008, 0x42, 0, 0},
+	{0x3035, 0x12, 0, 0}, {0x3036, 0x38, 0, 0}, {0x3c07, 0x08, 0, 0},
+	{0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
+	{0x3820, 0x41, 0, 0}, {0x3821, 0x07, 0, 0}, {0x3814, 0x31, 0, 0},
+	{0x3815, 0x31, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
+	{0x3802, 0x00, 0, 0}, {0x3803, 0x04, 0, 0}, {0x3804, 0x0a, 0, 0},
+	{0x3805, 0x3f, 0, 0}, {0x3806, 0x07, 0, 0}, {0x3807, 0x9b, 0, 0},
+	{0x3808, 0x01, 0, 0}, {0x3809, 0x40, 0, 0}, {0x380a, 0x00, 0, 0},
+	{0x380b, 0xF0, 0, 0}, {0x380c, 0x07, 0, 0}, {0x380d, 0x68, 0, 0},
+	{0x380e, 0x04, 0, 0}, {0x380f, 0x38, 0, 0}, {0x3810, 0x00, 0, 0},
+	{0x3811, 0x10, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x06, 0, 0},
+	{0x3618, 0x00, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x64, 0, 0},
+	{0x3709, 0x52, 0, 0}, {0x370c, 0x03, 0, 0}, {0x3a02, 0x03, 0, 0},
+	{0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x0e, 0, 0},
+	{0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
+	{0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
+	{0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0}, {0x4713, 0x03, 0, 0},
+	{0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
+	{0x3824, 0x02, 0, 0}, {0x5001, 0xa3, 0, 0},
+	{0x4005, 0x1a, 0, 0}, {0x3008, 0x02, 0, 0}, {0x3503, 0x00, 0, 0},
+};
+
+static struct reg_value ov5640_setting_30fps_VGA_640_480[] = {
+	{0x3008, 0x42, 0, 0},
+	{0x3035, 0x12, 0, 0}, {0x3036, 0x38, 0, 0}, {0x3c07, 0x08, 0, 0},
+	{0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
+	{0x3820, 0x41, 0, 0}, {0x3821, 0x07, 0, 0}, {0x3814, 0x31, 0, 0},
+	{0x3815, 0x31, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
+	{0x3802, 0x00, 0, 0}, {0x3803, 0x04, 0, 0}, {0x3804, 0x0a, 0, 0},
+	{0x3805, 0x3f, 0, 0}, {0x3806, 0x07, 0, 0}, {0x3807, 0x9b, 0, 0},
+	{0x3808, 0x02, 0, 0}, {0x3809, 0x80, 0, 0}, {0x380a, 0x01, 0, 0},
+	{0x380b, 0xe0, 0, 0}, {0x380c, 0x07, 0, 0}, {0x380d, 0x68, 0, 0},
+	{0x380e, 0x04, 0, 0}, {0x380f, 0x38, 0, 0}, {0x3810, 0x00, 0, 0},
+	{0x3811, 0x10, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x06, 0, 0},
+	{0x3618, 0x00, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x64, 0, 0},
+	{0x3709, 0x52, 0, 0}, {0x370c, 0x03, 0, 0}, {0x3a02, 0x03, 0, 0},
+	{0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x0e, 0, 0},
+	{0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
+	{0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
+	{0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0}, {0x4713, 0x03, 0, 0},
+	{0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
+	{0x3824, 0x02, 0, 0}, {0x5001, 0xa3, 0, 0},
+	{0x4005, 0x1a, 0, 0}, {0x3008, 0x02, 0, 0}, {0x3503, 0x00, 0, 0},
+};
+
+static struct reg_value ov5640_setting_30fps_NTSC_720_480[] = {
+	{0x3008, 0x42, 0, 0},
+	{0x3035, 0x12, 0, 0}, {0x3036, 0x38, 0, 0}, {0x3c07, 0x08, 0, 0},
+	{0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
+	{0x3820, 0x41, 0, 0}, {0x3821, 0x07, 0, 0}, {0x3814, 0x31, 0, 0},
+	{0x3815, 0x31, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
+	{0x3802, 0x00, 0, 0}, {0x3803, 0x04, 0, 0}, {0x3804, 0x0a, 0, 0},
+	{0x3805, 0x3f, 0, 0}, {0x3806, 0x07, 0, 0}, {0x3807, 0x9b, 0, 0},
+	{0x3808, 0x02, 0, 0}, {0x3809, 0xd0, 0, 0}, {0x380a, 0x01, 0, 0},
+	{0x380b, 0xe0, 0, 0}, {0x380c, 0x07, 0, 0}, {0x380d, 0x68, 0, 0},
+	{0x380e, 0x03, 0, 0}, {0x380f, 0xd8, 0, 0}, {0x3810, 0x00, 0, 0},
+	{0x3811, 0x10, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x3c, 0, 0},
+	{0x3618, 0x00, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x64, 0, 0},
+	{0x3709, 0x52, 0, 0}, {0x370c, 0x03, 0, 0}, {0x3a02, 0x03, 0, 0},
+	{0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x27, 0, 0},
+	{0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
+	{0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
+	{0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0}, {0x4713, 0x03, 0, 0},
+	{0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
+	{0x3824, 0x02, 0, 0}, {0x5001, 0xa3, 0, 0},
+	{0x4005, 0x1a, 0, 0}, {0x3008, 0x02, 0, 0}, {0x3503, 0, 0, 0},
+};
+
+static struct reg_value ov5640_setting_30fps_720P_1280_720[] = {
+	{0x3008, 0x42, 0, 0},
+	{0x3035, 0x21, 0, 0}, {0x3036, 0x54, 0, 0}, {0x3c07, 0x07, 0, 0},
+	{0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
+	{0x3820, 0x41, 0, 0}, {0x3821, 0x07, 0, 0}, {0x3814, 0x31, 0, 0},
+	{0x3815, 0x31, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
+	{0x3802, 0x00, 0, 0}, {0x3803, 0xfa, 0, 0}, {0x3804, 0x0a, 0, 0},
+	{0x3805, 0x3f, 0, 0}, {0x3806, 0x06, 0, 0}, {0x3807, 0xa9, 0, 0},
+	{0x3808, 0x05, 0, 0}, {0x3809, 0x00, 0, 0}, {0x380a, 0x02, 0, 0},
+	{0x380b, 0xd0, 0, 0}, {0x380c, 0x07, 0, 0}, {0x380d, 0x64, 0, 0},
+	{0x380e, 0x02, 0, 0}, {0x380f, 0xe4, 0, 0}, {0x3810, 0x00, 0, 0},
+	{0x3811, 0x10, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x04, 0, 0},
+	{0x3618, 0x00, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x64, 0, 0},
+	{0x3709, 0x52, 0, 0}, {0x370c, 0x03, 0, 0}, {0x3a02, 0x02, 0, 0},
+	{0x3a03, 0xe4, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0xbc, 0, 0},
+	{0x3a0a, 0x01, 0, 0}, {0x3a0b, 0x72, 0, 0}, {0x3a0e, 0x01, 0, 0},
+	{0x3a0d, 0x02, 0, 0}, {0x3a14, 0x02, 0, 0}, {0x3a15, 0xe4, 0, 0},
+	{0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0}, {0x4713, 0x02, 0, 0},
+	{0x4407, 0x04, 0, 0}, {0x460b, 0x37, 0, 0}, {0x460c, 0x20, 0, 0},
+	{0x3824, 0x04, 0, 0}, {0x5001, 0x83, 0, 0}, {0x4005, 0x1a, 0, 0},
+	{0x3008, 0x02, 0, 0}, {0x3503, 0,    0, 0},
+};
+
+static struct reg_value ov5640_setting_30fps_1080P_1920_1080[] = {
+	{0x3008, 0x42, 0, 0},
+	{0x3035, 0x21, 0, 0}, {0x3036, 0x54, 0, 0}, {0x3c07, 0x08, 0, 0},
+	{0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
+	{0x3820, 0x40, 0, 0}, {0x3821, 0x06, 0, 0}, {0x3814, 0x11, 0, 0},
+	{0x3815, 0x11, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
+	{0x3802, 0x00, 0, 0}, {0x3803, 0x00, 0, 0}, {0x3804, 0x0a, 0, 0},
+	{0x3805, 0x3f, 0, 0}, {0x3806, 0x07, 0, 0}, {0x3807, 0x9f, 0, 0},
+	{0x3808, 0x0a, 0, 0}, {0x3809, 0x20, 0, 0}, {0x380a, 0x07, 0, 0},
+	{0x380b, 0x98, 0, 0}, {0x380c, 0x0b, 0, 0}, {0x380d, 0x1c, 0, 0},
+	{0x380e, 0x07, 0, 0}, {0x380f, 0xb0, 0, 0}, {0x3810, 0x00, 0, 0},
+	{0x3811, 0x10, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x04, 0, 0},
+	{0x3618, 0x04, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x21, 0, 0},
+	{0x3709, 0x12, 0, 0}, {0x370c, 0x00, 0, 0}, {0x3a02, 0x03, 0, 0},
+	{0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x27, 0, 0},
+	{0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
+	{0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
+	{0x4001, 0x02, 0, 0}, {0x4004, 0x06, 0, 0}, {0x4713, 0x03, 0, 0},
+	{0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
+	{0x3824, 0x02, 0, 0}, {0x5001, 0x83, 0, 0}, {0x3035, 0x11, 0, 0},
+	{0x3036, 0x54, 0, 0}, {0x3c07, 0x07, 0, 0}, {0x3c08, 0x00, 0, 0},
+	{0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
+	{0x3800, 0x01, 0, 0}, {0x3801, 0x50, 0, 0}, {0x3802, 0x01, 0, 0},
+	{0x3803, 0xb2, 0, 0}, {0x3804, 0x08, 0, 0}, {0x3805, 0xef, 0, 0},
+	{0x3806, 0x05, 0, 0}, {0x3807, 0xf1, 0, 0}, {0x3808, 0x07, 0, 0},
+	{0x3809, 0x80, 0, 0}, {0x380a, 0x04, 0, 0}, {0x380b, 0x38, 0, 0},
+	{0x380c, 0x09, 0, 0}, {0x380d, 0xc4, 0, 0}, {0x380e, 0x04, 0, 0},
+	{0x380f, 0x60, 0, 0}, {0x3612, 0x2b, 0, 0}, {0x3708, 0x64, 0, 0},
+	{0x3a02, 0x04, 0, 0}, {0x3a03, 0x60, 0, 0}, {0x3a08, 0x01, 0, 0},
+	{0x3a09, 0x50, 0, 0}, {0x3a0a, 0x01, 0, 0}, {0x3a0b, 0x18, 0, 0},
+	{0x3a0e, 0x03, 0, 0}, {0x3a0d, 0x04, 0, 0}, {0x3a14, 0x04, 0, 0},
+	{0x3a15, 0x60, 0, 0}, {0x4713, 0x02, 0, 0}, {0x4407, 0x04, 0, 0},
+	{0x460b, 0x37, 0, 0}, {0x460c, 0x20, 0, 0}, {0x3824, 0x04, 0, 0},
+	{0x4005, 0x1a, 0, 0}, {0x3008, 0x02, 0, 0},
+	{0x3503, 0, 0, 0},
+};
+
+static struct reg_value ov5640_setting_15fps_QSXGA_2592_1944[] = {
+	{0x3008, 0x42, 0, 0},
+	{0x3820, 0x40, 0, 0}, {0x3821, 0x06, 0, 0}, /*disable flip*/
+	{0x3035, 0x11, 0, 0}, {0x3036, 0x54, 0, 0}, {0x3c07, 0x08, 0, 0},
+	{0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
+	{0x3820, 0x40, 0, 0}, {0x3821, 0x06, 0, 0}, {0x3814, 0x11, 0, 0},
+	{0x3815, 0x11, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
+	{0x3802, 0x00, 0, 0}, {0x3803, 0x00, 0, 0}, {0x3804, 0x0a, 0, 0},
+	{0x3805, 0x3f, 0, 0}, {0x3806, 0x07, 0, 0}, {0x3807, 0x9f, 0, 0},
+	{0x3808, 0x0a, 0, 0}, {0x3809, 0x20, 0, 0}, {0x380a, 0x07, 0, 0},
+	{0x380b, 0x98, 0, 0}, {0x380c, 0x0b, 0, 0}, {0x380d, 0x1c, 0, 0},
+	{0x380e, 0x07, 0, 0}, {0x380f, 0xb0, 0, 0}, {0x3810, 0x00, 0, 0},
+	{0x3811, 0x10, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x04, 0, 0},
+	{0x3618, 0x04, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x21, 0, 0},
+	{0x3709, 0x12, 0, 0}, {0x370c, 0x00, 0, 0}, {0x3a02, 0x03, 0, 0},
+	{0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x27, 0, 0},
+	{0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
+	{0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
+	{0x4001, 0x02, 0, 0}, {0x4004, 0x06, 0, 0}, {0x4713, 0x03, 0, 0},
+	{0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
+	{0x3824, 0x02, 0, 0}, {0x5001, 0x83, 0, 70}, {0x3008, 0x02, 0, 0},
+};
+
+static struct ov5640_mode_info ov5640_mode_info_data[2][ov5640_mode_MAX + 1] = {
+	{
+		{ov5640_mode_VGA_640_480, -1, 0, 0, NULL, 0},
+		{ov5640_mode_QVGA_320_240, -1, 0, 0, NULL, 0},
+		{ov5640_mode_NTSC_720_480, -1, 0, 0, NULL, 0},
+		{ov5640_mode_720P_1280_720, -1, 0, 0, NULL, 0},
+		{ov5640_mode_1080P_1920_1080, -1, 0, 0, NULL, 0},
+		{ov5640_mode_QSXGA_2592_1944, SCALING, 2592, 1944,
+		ov5640_setting_15fps_QSXGA_2592_1944,
+		ARRAY_SIZE(ov5640_setting_15fps_QSXGA_2592_1944)},
+	},
+	{
+		{ov5640_mode_VGA_640_480, SUBSAMPLING, 640,  480,
+		ov5640_setting_30fps_VGA_640_480,
+		ARRAY_SIZE(ov5640_setting_30fps_VGA_640_480)},
+		{ov5640_mode_QVGA_320_240, SUBSAMPLING, 320,  240,
+		ov5640_setting_30fps_QVGA_320_240,
+		ARRAY_SIZE(ov5640_setting_30fps_QVGA_320_240)},
+		{ov5640_mode_NTSC_720_480, SUBSAMPLING, 720, 480,
+		ov5640_setting_30fps_NTSC_720_480,
+		ARRAY_SIZE(ov5640_setting_30fps_NTSC_720_480)},
+		{ov5640_mode_720P_1280_720, SUBSAMPLING, 1280, 720,
+		ov5640_setting_30fps_720P_1280_720,
+		ARRAY_SIZE(ov5640_setting_30fps_720P_1280_720)},
+		{ov5640_mode_1080P_1920_1080, SCALING, 1920, 1080,
+		ov5640_setting_30fps_1080P_1920_1080,
+		ARRAY_SIZE(ov5640_setting_30fps_1080P_1920_1080)},
+		{ov5640_mode_QSXGA_2592_1944, -1, 0, 0, NULL, 0},
+	},
+};
+
+static struct regulator *io_regulator;
+static struct regulator *core_regulator;
+static struct regulator *analog_regulator;
+static struct regulator *gpo_regulator;
+static DEFINE_MUTEX(ov5640_mutex);
+
+static int ov5640_probe(struct i2c_client *adapter,
+				const struct i2c_device_id *device_id);
+static int ov5640_remove(struct i2c_client *client);
+
+static s32 ov5640_read_reg(struct ov5640 *sensor, u16 reg, u8 *val);
+static s32 ov5640_write_reg(struct ov5640 *sensor, u16 reg, u8 val);
+#ifdef CONFIG_OF
+static const struct of_device_id ov5640_mipi_v2_of_match[] = {
+	{ .compatible = "ovti,ov5640_mipi",
+	},
+	{ /* sentinel */ }
+};
+
+MODULE_DEVICE_TABLE(of, ov5640_mipi_v2_of_match);
+#endif
+
+static const struct i2c_device_id ov5640_id[] = {
+	{"ov5640_mipi", 0},
+	{},
+};
+
+MODULE_DEVICE_TABLE(i2c, ov5640_id);
+
+static struct i2c_driver ov5640_i2c_driver = {
+	.driver = {
+		  .owner = THIS_MODULE,
+		  .name  = "ov5640_mipi",
+#ifdef CONFIG_OF
+		  .of_match_table = of_match_ptr(ov5640_mipi_v2_of_match),
+#endif
+		  },
+	.probe  = ov5640_probe,
+	.remove = ov5640_remove,
+	.id_table = ov5640_id,
+};
+
+static const struct ov5640_datafmt ov5640_colour_fmts[] = {
+	{MEDIA_BUS_FMT_YUYV8_2X8, V4L2_COLORSPACE_JPEG},
+};
+
+static int get_capturemode(int width, int height)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(ov5640_valid_res); i++) {
+		if ((ov5640_valid_res[i].width == width) &&
+		     (ov5640_valid_res[i].height == height))
+			return i;
+	}
+
+	return -1;
+}
+
+static struct ov5640 *to_ov5640(const struct i2c_client *client)
+{
+	return container_of(i2c_get_clientdata(client), struct ov5640, subdev);
+}
+
+/* Find a data format by a pixel code in an array */
+static const struct ov5640_datafmt
+			*ov5640_find_datafmt(u32 code)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(ov5640_colour_fmts); i++)
+		if (ov5640_colour_fmts[i].code == code)
+			return ov5640_colour_fmts + i;
+
+	return NULL;
+}
+
+static inline void ov5640_power_down(struct ov5640 *sensor, int enable)
+{
+	if (sensor->pwn_gpio < 0)
+		return;
+
+	if (!enable)
+		gpio_set_value_cansleep(sensor->pwn_gpio, 0);
+	else
+		gpio_set_value_cansleep(sensor->pwn_gpio, 1);
+
+	msleep(2);
+}
+
+static int ov5640_update_slave_id(struct ov5640 *sensor)
+{
+	struct device *dev = &sensor->i2c_client->dev;
+	u8 slave_id;
+	struct i2c_client *tmp_client;
+	s32 retval;
+
+	if (sensor->i2c_client->addr << 1 == DEFAULT_SCCB_ID)
+		return 0; /* nothing to do */
+
+	/* Change camera i2c slave address */
+	slave_id = (u8)(sensor->i2c_client->addr << 1); /* new slave id*/
+	tmp_client = sensor->i2c_client;
+
+	sensor->i2c_client =
+		i2c_new_dummy_device(tmp_client->adapter, DEFAULT_SCCB_ID >> 1);
+	if (!sensor->i2c_client) {
+		dev_err(dev, "Failed to communicate on 0x%x\n",
+			DEFAULT_SCCB_ID);
+		return -1;
+	}
+
+	retval = ov5640_write_reg(sensor, 0x3100, slave_id);
+	i2c_unregister_device(sensor->i2c_client);
+	sensor->i2c_client = tmp_client;
+	if (retval != 0) {
+		dev_err(dev, "Failed to update SCCB_ID to 0x%x\n", slave_id);
+		return -1;
+	}
+
+	ov5640_read_reg(sensor, 0x3100, &slave_id);
+	dev_dbg(dev, "Updated SCCB_ID 0x%x\n", slave_id);
+
+	return 0;
+}
+
+static void ov5640_reset(struct ov5640 *sensor)
+{
+	if (sensor->rst_gpio < 0 || sensor->pwn_gpio < 0)
+		return;
+
+	/* camera reset */
+	gpio_set_value(sensor->rst_gpio, 1);
+
+	/* camera power dowmn */
+	gpio_set_value(sensor->pwn_gpio, 1);
+	msleep(5);
+
+	gpio_set_value(sensor->rst_gpio, 0);
+	msleep(1);
+
+	gpio_set_value(sensor->pwn_gpio, 0);
+	msleep(5);
+
+	gpio_set_value(sensor->rst_gpio, 1);
+	msleep(5);
+}
+
+static int ov5640_regulator_enable(struct device *dev)
+{
+	int ret = 0;
+
+	io_regulator = devm_regulator_get(dev, "DOVDD");
+	if (!IS_ERR(io_regulator)) {
+		regulator_set_voltage(io_regulator,
+				      OV5640_VOLTAGE_DIGITAL_IO,
+				      OV5640_VOLTAGE_DIGITAL_IO);
+		ret = regulator_enable(io_regulator);
+		if (ret) {
+			pr_err("%s:io set voltage error\n", __func__);
+			return ret;
+		} else {
+			dev_dbg(dev,
+				"%s:io set voltage ok\n", __func__);
+		}
+	} else {
+		pr_err("%s: cannot get io voltage error\n", __func__);
+		io_regulator = NULL;
+	}
+
+	core_regulator = devm_regulator_get(dev, "DVDD");
+	if (!IS_ERR(core_regulator)) {
+		regulator_set_voltage(core_regulator,
+				      OV5640_VOLTAGE_DIGITAL_CORE,
+				      OV5640_VOLTAGE_DIGITAL_CORE);
+		ret = regulator_enable(core_regulator);
+		if (ret) {
+			pr_err("%s:core set voltage error\n", __func__);
+			return ret;
+		} else {
+			dev_dbg(dev,
+				"%s:core set voltage ok\n", __func__);
+		}
+	} else {
+		core_regulator = NULL;
+		pr_err("%s: cannot get core voltage error\n", __func__);
+	}
+
+	analog_regulator = devm_regulator_get(dev, "AVDD");
+	if (!IS_ERR(analog_regulator)) {
+		regulator_set_voltage(analog_regulator,
+				      OV5640_VOLTAGE_ANALOG,
+				      OV5640_VOLTAGE_ANALOG);
+		ret = regulator_enable(analog_regulator);
+		if (ret) {
+			pr_err("%s:analog set voltage error\n",
+				__func__);
+			return ret;
+		} else {
+			dev_dbg(dev,
+				"%s:analog set voltage ok\n", __func__);
+		}
+	} else {
+		analog_regulator = NULL;
+		pr_err("%s: cannot get analog voltage error\n", __func__);
+	}
+
+	return ret;
+}
+
+static void ov5640_regualtor_disable(void)
+{
+	if (analog_regulator)
+		regulator_disable(analog_regulator);
+
+	if (core_regulator)
+		regulator_disable(core_regulator);
+
+	if (io_regulator)
+		regulator_disable(io_regulator);
+
+	if (gpo_regulator)
+		regulator_disable(gpo_regulator);
+}
+
+static s32 ov5640_write_reg(struct ov5640 *sensor, u16 reg, u8 val)
+{
+	struct device *dev = &sensor->i2c_client->dev;
+	u8 au8Buf[3] = {0};
+
+	au8Buf[0] = reg >> 8;
+	au8Buf[1] = reg & 0xff;
+	au8Buf[2] = val;
+
+	if (i2c_master_send(sensor->i2c_client, au8Buf, 3) < 0) {
+		dev_err(dev, "Write reg error: reg=%x, val=%x\n", reg, val);
+		return -1;
+	}
+
+	return 0;
+}
+
+static s32 ov5640_read_reg(struct ov5640 *sensor, u16 reg, u8 *val)
+{
+	struct device *dev = &sensor->i2c_client->dev;
+	u8 au8RegBuf[2] = {0};
+	u8 u8RdVal = 0;
+
+	au8RegBuf[0] = reg >> 8;
+	au8RegBuf[1] = reg & 0xff;
+
+	if (i2c_master_send(sensor->i2c_client, au8RegBuf, 2) != 2) {
+		dev_err(dev, "Read reg error: reg=%x\n", reg);
+		return -1;
+	}
+
+	if (i2c_master_recv(sensor->i2c_client, &u8RdVal, 1) != 1) {
+		dev_err(dev, "Read reg error: reg=%x, val=%x\n", reg, u8RdVal);
+		return -1;
+	}
+
+	*val = u8RdVal;
+
+	return u8RdVal;
+}
+
+static int prev_sysclk, prev_HTS;
+static int AE_low, AE_high, AE_Target = 52;
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int ov5640_get_register(struct v4l2_subdev *sd,
+			       struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ov5640 *sensor = to_ov5640(client);
+	int ret;
+	u8 val;
+
+	if (reg->reg & ~0xffff)
+		return -EINVAL;
+
+	reg->size = 1;
+
+	ret = ov5640_read_reg(sensor, reg->reg, &val);
+	if (!ret)
+		reg->val = (__u64)val;
+
+	return ret;
+}
+
+static int ov5640_set_register(struct v4l2_subdev *sd,
+			       const struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ov5640 *sensor = to_ov5640(client);
+
+	if (reg->reg & ~0xffff || reg->val & ~0xff)
+		return -EINVAL;
+
+	return ov5640_write_reg(sensor, reg->reg, reg->val);
+}
+#endif
+
+static void OV5640_stream_on(struct ov5640 *sensor)
+{
+	ov5640_write_reg(sensor, 0x4202, 0x00);
+}
+
+static void OV5640_stream_off(struct ov5640 *sensor)
+{
+	ov5640_write_reg(sensor, 0x4202, 0x0f);
+	ov5640_write_reg(sensor, 0x3008, 0x42);
+}
+
+static int OV5640_get_sysclk(struct ov5640 *sensor)
+{
+	 /* calculate sysclk */
+	int xvclk = sensor->mclk / 10000;
+	int temp1, temp2;
+	int Multiplier, PreDiv, VCO, SysDiv, Pll_rdiv;
+	int Bit_div2x = 1, sclk_rdiv, sysclk;
+	u8 temp;
+
+	int sclk_rdiv_map[] = {1, 2, 4, 8};
+
+	temp1 = ov5640_read_reg(sensor, 0x3034, &temp);
+	temp2 = temp1 & 0x0f;
+	if (temp2 == 8 || temp2 == 10)
+		Bit_div2x = temp2 / 2;
+
+	temp1 = ov5640_read_reg(sensor, 0x3035, &temp);
+	SysDiv = temp1>>4;
+	if (SysDiv == 0)
+		SysDiv = 16;
+
+	temp1 = ov5640_read_reg(sensor, 0x3036, &temp);
+	Multiplier = temp1;
+
+	temp1 = ov5640_read_reg(sensor, 0x3037, &temp);
+	PreDiv = temp1 & 0x0f;
+	Pll_rdiv = ((temp1 >> 4) & 0x01) + 1;
+
+	temp1 = ov5640_read_reg(sensor, 0x3108, &temp);
+	temp2 = temp1 & 0x03;
+	sclk_rdiv = sclk_rdiv_map[temp2];
+
+	VCO = xvclk * Multiplier / PreDiv;
+
+	sysclk = VCO / SysDiv / Pll_rdiv * 2 / Bit_div2x / sclk_rdiv;
+
+	return sysclk;
+}
+
+static void OV5640_set_night_mode(struct ov5640 *sensor)
+{
+	 /* read HTS from register settings */
+	u8 mode;
+
+	ov5640_read_reg(sensor, 0x3a00, &mode);
+	mode &= 0xfb;
+	ov5640_write_reg(sensor, 0x3a00, mode);
+}
+
+static int OV5640_get_HTS(struct ov5640 *sensor)
+{
+	 /* read HTS from register settings */
+	int HTS;
+	u8 temp;
+
+	HTS = ov5640_read_reg(sensor, 0x380c, &temp);
+	HTS = (HTS<<8) + ov5640_read_reg(sensor, 0x380d, &temp);
+
+	return HTS;
+}
+
+static int OV5640_get_VTS(struct ov5640 *sensor)
+{
+	 /* read VTS from register settings */
+	int VTS;
+	u8 temp;
+
+	/* total vertical size[15:8] high byte */
+	VTS = ov5640_read_reg(sensor, 0x380e, &temp);
+
+	VTS = (VTS<<8) + ov5640_read_reg(sensor, 0x380f, &temp);
+
+	return VTS;
+}
+
+static int OV5640_set_VTS(struct ov5640 *sensor, int VTS)
+{
+	 /* write VTS to registers */
+	 int temp;
+
+	 temp = VTS & 0xff;
+	 ov5640_write_reg(sensor, 0x380f, temp);
+
+	 temp = VTS>>8;
+	 ov5640_write_reg(sensor, 0x380e, temp);
+
+	 return 0;
+}
+
+static int OV5640_get_shutter(struct ov5640 *sensor)
+{
+	 /* read shutter, in number of line period */
+	int shutter;
+	u8 temp;
+
+	shutter = (ov5640_read_reg(sensor, 0x03500, &temp) & 0x0f);
+	shutter = (shutter<<8) + ov5640_read_reg(sensor, 0x3501, &temp);
+	shutter = (shutter<<4) + (ov5640_read_reg(sensor, 0x3502, &temp)>>4);
+
+	 return shutter;
+}
+
+static int OV5640_set_shutter(struct ov5640 *sensor, int shutter)
+{
+	 /* write shutter, in number of line period */
+	 int temp;
+
+	 shutter = shutter & 0xffff;
+
+	 temp = shutter & 0x0f;
+	 temp = temp<<4;
+	 ov5640_write_reg(sensor, 0x3502, temp);
+
+	 temp = shutter & 0xfff;
+	 temp = temp>>4;
+	 ov5640_write_reg(sensor, 0x3501, temp);
+
+	 temp = shutter>>12;
+	 ov5640_write_reg(sensor, 0x3500, temp);
+
+	 return 0;
+}
+
+
+static int OV5640_get_gain16(struct ov5640 *sensor)
+{
+	 /* read gain, 16 = 1x */
+	int gain16;
+	u8 temp;
+
+	gain16 = ov5640_read_reg(sensor, 0x350a, &temp) & 0x03;
+	gain16 = (gain16<<8) + ov5640_read_reg(sensor, 0x350b, &temp);
+
+	return gain16;
+}
+
+static int OV5640_set_gain16(struct ov5640 *sensor, int gain16)
+{
+	/* write gain, 16 = 1x */
+	u8 temp;
+	gain16 = gain16 & 0x3ff;
+
+	temp = gain16 & 0xff;
+	ov5640_write_reg(sensor, 0x350b, temp);
+
+	temp = gain16>>8;
+	ov5640_write_reg(sensor, 0x350a, temp);
+
+	return 0;
+}
+
+static int OV5640_get_light_freq(struct ov5640 *sensor)
+{
+	/* get banding filter value */
+	int temp, temp1, light_freq = 0;
+	u8 tmp;
+
+	temp = ov5640_read_reg(sensor, 0x3c01, &tmp);
+
+	if (temp & 0x80) {
+		/* manual */
+		temp1 = ov5640_read_reg(sensor, 0x3c00, &tmp);
+		if (temp1 & 0x04) {
+			/* 50Hz */
+			light_freq = 50;
+		} else {
+			/* 60Hz */
+			light_freq = 60;
+		}
+	} else {
+		/* auto */
+		temp1 = ov5640_read_reg(sensor, 0x3c0c, &tmp);
+		if (temp1 & 0x01) {
+			/* 50Hz */
+			light_freq = 50;
+		} else {
+			/* 60Hz */
+		}
+	}
+	return light_freq;
+}
+
+static void OV5640_set_bandingfilter(struct ov5640 *sensor)
+{
+	int prev_VTS;
+	int band_step60, max_band60, band_step50, max_band50;
+
+	/* read preview PCLK */
+	prev_sysclk = OV5640_get_sysclk(sensor);
+	/* read preview HTS */
+	prev_HTS = OV5640_get_HTS(sensor);
+
+	/* read preview VTS */
+	prev_VTS = OV5640_get_VTS(sensor);
+
+	/* calculate banding filter */
+	/* 60Hz */
+	band_step60 = prev_sysclk * 100/prev_HTS * 100/120;
+	ov5640_write_reg(sensor, 0x3a0a, (band_step60 >> 8));
+	ov5640_write_reg(sensor, 0x3a0b, (band_step60 & 0xff));
+
+	max_band60 = (int)((prev_VTS-4)/band_step60);
+	ov5640_write_reg(sensor, 0x3a0d, max_band60);
+
+	/* 50Hz */
+	band_step50 = prev_sysclk * 100/prev_HTS;
+	ov5640_write_reg(sensor, 0x3a08, (band_step50 >> 8));
+	ov5640_write_reg(sensor, 0x3a09, (band_step50 & 0xff));
+
+	max_band50 = (int)((prev_VTS-4)/band_step50);
+	ov5640_write_reg(sensor, 0x3a0e, max_band50);
+}
+
+static int OV5640_set_AE_target(struct ov5640 *sensor, int target)
+{
+	/* stable in high */
+	int fast_high, fast_low;
+	AE_low = target * 23 / 25;	/* 0.92 */
+	AE_high = target * 27 / 25;	/* 1.08 */
+
+	fast_high = AE_high<<1;
+	if (fast_high > 255)
+		fast_high = 255;
+
+	fast_low = AE_low >> 1;
+
+	ov5640_write_reg(sensor, 0x3a0f, AE_high);
+	ov5640_write_reg(sensor, 0x3a10, AE_low);
+	ov5640_write_reg(sensor, 0x3a1b, AE_high);
+	ov5640_write_reg(sensor, 0x3a1e, AE_low);
+	ov5640_write_reg(sensor, 0x3a11, fast_high);
+	ov5640_write_reg(sensor, 0x3a1f, fast_low);
+
+	return 0;
+}
+
+static void OV5640_turn_on_AE_AG(struct ov5640 *sensor, int enable)
+{
+	u8 ae_ag_ctrl;
+
+	ov5640_read_reg(sensor, 0x3503, &ae_ag_ctrl);
+	if (enable) {
+		/* turn on auto AE/AG */
+		ae_ag_ctrl = ae_ag_ctrl & ~(0x03);
+	} else {
+		/* turn off AE/AG */
+		ae_ag_ctrl = ae_ag_ctrl | 0x03;
+	}
+	ov5640_write_reg(sensor, 0x3503, ae_ag_ctrl);
+}
+
+static bool binning_on(struct ov5640 *sensor)
+{
+	u8 temp;
+	ov5640_read_reg(sensor, 0x3821, &temp);
+	temp &= 0xfe;
+	if (temp)
+		return true;
+	else
+		return false;
+}
+
+static void ov5640_set_virtual_channel(struct ov5640 *sensor, int channel)
+{
+	u8 channel_id;
+
+	ov5640_read_reg(sensor, 0x4814, &channel_id);
+	channel_id &= ~(3 << 6);
+	ov5640_write_reg(sensor, 0x4814, channel_id | (channel << 6));
+}
+
+/* download ov5640 settings to sensor through i2c */
+static int ov5640_download_firmware(struct ov5640 *sensor,
+				    struct reg_value *pModeSetting,
+				    s32 ArySize)
+{
+	register u32 Delay_ms = 0;
+	register u16 RegAddr = 0;
+	register u8 Mask = 0;
+	register u8 Val = 0;
+	u8 RegVal = 0;
+	int i, retval = 0;
+
+	for (i = 0; i < ArySize; ++i, ++pModeSetting) {
+		Delay_ms = pModeSetting->u32Delay_ms;
+		RegAddr = pModeSetting->u16RegAddr;
+		Val = pModeSetting->u8Val;
+		Mask = pModeSetting->u8Mask;
+
+		if (Mask) {
+			retval = ov5640_read_reg(sensor, RegAddr, &RegVal);
+			if (retval < 0)
+				goto err;
+
+			RegVal &= ~(u8)Mask;
+			Val &= Mask;
+			Val |= RegVal;
+		}
+
+		retval = ov5640_write_reg(sensor, RegAddr, Val);
+		if (retval < 0)
+			goto err;
+
+		if (Delay_ms)
+			msleep(Delay_ms);
+	}
+err:
+	return retval;
+}
+
+/* sensor changes between scaling and subsampling
+ * go through exposure calcualtion
+ */
+static int ov5640_change_mode_exposure_calc(struct ov5640 *sensor,
+				enum ov5640_frame_rate frame_rate,
+				enum ov5640_mode mode)
+{
+	struct reg_value *pModeSetting = NULL;
+	s32 ArySize = 0;
+	u8 average;
+	int prev_shutter, prev_gain16;
+	int cap_shutter, cap_gain16;
+	int cap_sysclk, cap_HTS, cap_VTS;
+	int light_freq, cap_bandfilt, cap_maxband;
+	long cap_gain16_shutter;
+	int retval = 0;
+
+	/* check if the input mode and frame rate is valid */
+	pModeSetting =
+		ov5640_mode_info_data[frame_rate][mode].init_data_ptr;
+	ArySize =
+		ov5640_mode_info_data[frame_rate][mode].init_data_size;
+
+	sensor->pix.width =
+		ov5640_mode_info_data[frame_rate][mode].width;
+	sensor->pix.height =
+		ov5640_mode_info_data[frame_rate][mode].height;
+
+	if (sensor->pix.width == 0 || sensor->pix.height == 0 ||
+		pModeSetting == NULL || ArySize == 0)
+		return -EINVAL;
+
+	/* auto focus */
+	/* OV5640_auto_focus();//if no af function, just skip it */
+
+	/* turn off AE/AG */
+	OV5640_turn_on_AE_AG(sensor, 0);
+
+	/* read preview shutter */
+	prev_shutter = OV5640_get_shutter(sensor);
+	if ((binning_on(sensor)) && (mode != ov5640_mode_720P_1280_720)
+			&& (mode != ov5640_mode_1080P_1920_1080))
+		prev_shutter *= 2;
+
+	/* read preview gain */
+	prev_gain16 = OV5640_get_gain16(sensor);
+
+	/* get average */
+	ov5640_read_reg(sensor, 0x56a1, &average);
+
+	/* turn off night mode for capture */
+	OV5640_set_night_mode(sensor);
+
+	/* turn off overlay */
+	/* ov5640_write_reg(0x3022, 0x06);//if no af function, just skip it */
+
+	OV5640_stream_off(sensor);
+
+	/* Write capture setting */
+	retval = ov5640_download_firmware(sensor, pModeSetting, ArySize);
+	if (retval < 0)
+		goto err;
+
+	/* read capture VTS */
+	cap_VTS = OV5640_get_VTS(sensor);
+	cap_HTS = OV5640_get_HTS(sensor);
+	cap_sysclk = OV5640_get_sysclk(sensor);
+
+	/* calculate capture banding filter */
+	light_freq = OV5640_get_light_freq(sensor);
+	if (light_freq == 60) {
+		/* 60Hz */
+		cap_bandfilt = cap_sysclk * 100 / cap_HTS * 100 / 120;
+	} else {
+		/* 50Hz */
+		cap_bandfilt = cap_sysclk * 100 / cap_HTS;
+	}
+	cap_maxband = (int)((cap_VTS - 4)/cap_bandfilt);
+
+	/* calculate capture shutter/gain16 */
+	if (average > AE_low && average < AE_high) {
+		/* in stable range */
+		cap_gain16_shutter =
+		  prev_gain16 * prev_shutter * cap_sysclk/prev_sysclk
+		  * prev_HTS/cap_HTS * AE_Target / average;
+	} else {
+		cap_gain16_shutter =
+		  prev_gain16 * prev_shutter * cap_sysclk/prev_sysclk
+		  * prev_HTS/cap_HTS;
+	}
+
+	/* gain to shutter */
+	if (cap_gain16_shutter < (cap_bandfilt * 16)) {
+		/* shutter < 1/100 */
+		cap_shutter = cap_gain16_shutter/16;
+		if (cap_shutter < 1)
+			cap_shutter = 1;
+
+		cap_gain16 = cap_gain16_shutter/cap_shutter;
+		if (cap_gain16 < 16)
+			cap_gain16 = 16;
+	} else {
+		if (cap_gain16_shutter >
+				(cap_bandfilt * cap_maxband * 16)) {
+			/* exposure reach max */
+			cap_shutter = cap_bandfilt * cap_maxband;
+			cap_gain16 = cap_gain16_shutter / cap_shutter;
+		} else {
+			/* 1/100 < (cap_shutter = n/100) =< max */
+			cap_shutter =
+			  ((int) (cap_gain16_shutter/16 / cap_bandfilt))
+			  *cap_bandfilt;
+			cap_gain16 = cap_gain16_shutter / cap_shutter;
+		}
+	}
+
+	/* write capture gain */
+	OV5640_set_gain16(sensor, cap_gain16);
+
+	/* write capture shutter */
+	if (cap_shutter > (cap_VTS - 4)) {
+		cap_VTS = cap_shutter + 4;
+		OV5640_set_VTS(sensor, cap_VTS);
+	}
+	OV5640_set_shutter(sensor, cap_shutter);
+
+err:
+	return retval;
+}
+
+/* if sensor changes inside scaling or subsampling
+ * change mode directly
+ * */
+static int ov5640_change_mode_direct(struct ov5640 *sensor,
+				enum ov5640_frame_rate frame_rate,
+				enum ov5640_mode mode)
+{
+	struct reg_value *pModeSetting = NULL;
+	s32 ArySize = 0;
+	int retval = 0;
+
+	/* check if the input mode and frame rate is valid */
+	pModeSetting =
+		ov5640_mode_info_data[frame_rate][mode].init_data_ptr;
+	ArySize =
+		ov5640_mode_info_data[frame_rate][mode].init_data_size;
+
+	sensor->pix.width =
+		ov5640_mode_info_data[frame_rate][mode].width;
+	sensor->pix.height =
+		ov5640_mode_info_data[frame_rate][mode].height;
+
+	if (sensor->pix.width == 0 || sensor->pix.height == 0 ||
+		pModeSetting == NULL || ArySize == 0)
+		return -EINVAL;
+
+	/* turn off AE/AG */
+	OV5640_turn_on_AE_AG(sensor, 0);
+
+	OV5640_stream_off(sensor);
+
+	/* Write capture setting */
+	retval = ov5640_download_firmware(sensor, pModeSetting, ArySize);
+	if (retval < 0)
+		goto err;
+
+	OV5640_turn_on_AE_AG(sensor, 1);
+
+err:
+	return retval;
+}
+
+static int ov5640_init_mode(struct ov5640 *sensor,
+			    enum ov5640_frame_rate frame_rate,
+			    enum ov5640_mode mode, enum ov5640_mode orig_mode)
+{
+	struct device *dev = &sensor->i2c_client->dev;
+	struct reg_value *pModeSetting = NULL;
+	s32 ArySize = 0;
+	int retval = 0;
+	u32 msec_wait4stable = 0;
+	enum ov5640_downsize_mode dn_mode, orig_dn_mode;
+
+	if ((mode > ov5640_mode_MAX || mode < ov5640_mode_MIN)
+		&& (mode != ov5640_mode_INIT)) {
+		dev_err(dev, "Wrong ov5640 mode detected!\n");
+		return -1;
+	}
+
+	dn_mode = ov5640_mode_info_data[frame_rate][mode].dn_mode;
+	orig_dn_mode = ov5640_mode_info_data[frame_rate][orig_mode].dn_mode;
+	if (mode == ov5640_mode_INIT) {
+		pModeSetting = ov5640_init_setting_30fps_VGA;
+		ArySize = ARRAY_SIZE(ov5640_init_setting_30fps_VGA);
+
+		sensor->pix.width = 640;
+		sensor->pix.height = 480;
+		retval = ov5640_download_firmware(sensor, pModeSetting,
+						  ArySize);
+		if (retval < 0)
+			goto err;
+
+		pModeSetting = ov5640_setting_30fps_VGA_640_480;
+		ArySize = ARRAY_SIZE(ov5640_setting_30fps_VGA_640_480);
+		retval = ov5640_download_firmware(sensor, pModeSetting,
+						  ArySize);
+	} else if ((dn_mode == SUBSAMPLING && orig_dn_mode == SCALING) ||
+			(dn_mode == SCALING && orig_dn_mode == SUBSAMPLING)) {
+		/* change between subsampling and scaling
+		 * go through exposure calucation */
+		retval = ov5640_change_mode_exposure_calc(sensor,
+				frame_rate, mode);
+	} else {
+		/* change inside subsampling or scaling
+		 * download firmware directly */
+		retval = ov5640_change_mode_direct(sensor, frame_rate, mode);
+	}
+
+	if (retval < 0)
+		goto err;
+
+	OV5640_set_AE_target(sensor, AE_Target);
+	OV5640_get_light_freq(sensor);
+	OV5640_set_bandingfilter(sensor);
+	ov5640_set_virtual_channel(sensor, sensor->csi);
+
+	/* add delay to wait for sensor stable */
+	if (mode == ov5640_mode_QSXGA_2592_1944) {
+		/* dump the first two frames: 1/7.5*2
+		 * the frame rate of QSXGA is 7.5fps */
+		msec_wait4stable = 267;
+	} else {
+		/* dump the first eighteen frames: 1/30*18 */
+		msec_wait4stable = 600;
+	}
+	msleep(msec_wait4stable);
+
+err:
+	return retval;
+}
+
+/*!
+ * ov5640_s_power - V4L2 sensor interface handler for VIDIOC_S_POWER ioctl
+ * @s: pointer to standard V4L2 device structure
+ * @on: indicates power mode (on or off)
+ *
+ * Turns the power on or off, depending on the value of on and returns the
+ * appropriate error code.
+ */
+static int ov5640_s_power(struct v4l2_subdev *sd, int on)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ov5640 *sensor = to_ov5640(client);
+
+	if (on && !sensor->on) {
+		if (io_regulator)
+			if (regulator_enable(io_regulator) != 0)
+				return -EIO;
+		if (core_regulator)
+			if (regulator_enable(core_regulator) != 0)
+				return -EIO;
+		if (gpo_regulator)
+			if (regulator_enable(gpo_regulator) != 0)
+				return -EIO;
+		if (analog_regulator)
+			if (regulator_enable(analog_regulator) != 0)
+				return -EIO;
+	} else if (!on && sensor->on) {
+		ov5640_regualtor_disable();
+	}
+
+	sensor->on = on;
+
+	return 0;
+}
+
+/*!
+ * ov5640_g_parm - V4L2 sensor interface handler for VIDIOC_G_PARM ioctl
+ * @s: pointer to standard V4L2 sub device structure
+ * @a: pointer to standard V4L2 VIDIOC_G_PARM ioctl structure
+ *
+ * Returns the sensor's video CAPTURE parameters.
+ */
+static int ov5640_g_parm(struct v4l2_subdev *sd, struct v4l2_streamparm *a)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ov5640 *sensor = to_ov5640(client);
+	struct device *dev = &sensor->i2c_client->dev;
+	struct v4l2_captureparm *cparm = &a->parm.capture;
+	int ret = 0;
+
+	switch (a->type) {
+	/* This is the only case currently handled. */
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+		memset(a, 0, sizeof(*a));
+		a->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+		cparm->capability = sensor->streamcap.capability;
+		cparm->timeperframe = sensor->streamcap.timeperframe;
+		cparm->capturemode = sensor->streamcap.capturemode;
+		ret = 0;
+		break;
+
+	/* These are all the possible cases. */
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
+	case V4L2_BUF_TYPE_VIDEO_OVERLAY:
+	case V4L2_BUF_TYPE_VBI_CAPTURE:
+	case V4L2_BUF_TYPE_VBI_OUTPUT:
+	case V4L2_BUF_TYPE_SLICED_VBI_CAPTURE:
+	case V4L2_BUF_TYPE_SLICED_VBI_OUTPUT:
+		ret = -EINVAL;
+		break;
+
+	default:
+		dev_warn(dev, "Type is unknown - %d\n", a->type);
+		ret = -EINVAL;
+		break;
+	}
+
+	return ret;
+}
+
+/*!
+ * ov5460_s_parm - V4L2 sensor interface handler for VIDIOC_S_PARM ioctl
+ * @s: pointer to standard V4L2 sub device structure
+ * @a: pointer to standard V4L2 VIDIOC_S_PARM ioctl structure
+ *
+ * Configures the sensor to use the input parameters, if possible.  If
+ * not possible, reverts to the old parameters and returns the
+ * appropriate error code.
+ */
+static int ov5640_s_parm(struct v4l2_subdev *sd, struct v4l2_streamparm *a)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ov5640 *sensor = to_ov5640(client);
+	struct device *dev = &sensor->i2c_client->dev;
+	struct v4l2_fract *timeperframe = &a->parm.capture.timeperframe;
+	u32 tgt_fps;	/* target frames per secound */
+	enum ov5640_frame_rate frame_rate;
+	enum ov5640_mode orig_mode;
+	int ret = 0;
+
+	switch (a->type) {
+	/* This is the only case currently handled. */
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+		/* Check that the new frame rate is allowed. */
+		if ((timeperframe->numerator == 0) ||
+		    (timeperframe->denominator == 0)) {
+			timeperframe->denominator = DEFAULT_FPS;
+			timeperframe->numerator = 1;
+		}
+
+		tgt_fps = timeperframe->denominator /
+			  timeperframe->numerator;
+
+		if (tgt_fps > MAX_FPS) {
+			timeperframe->denominator = MAX_FPS;
+			timeperframe->numerator = 1;
+		} else if (tgt_fps < MIN_FPS) {
+			timeperframe->denominator = MIN_FPS;
+			timeperframe->numerator = 1;
+		}
+
+		/* Actual frame rate we use */
+		tgt_fps = timeperframe->denominator /
+			  timeperframe->numerator;
+
+		if (tgt_fps == 15)
+			frame_rate = ov5640_15_fps;
+		else if (tgt_fps == 30)
+			frame_rate = ov5640_30_fps;
+		else {
+			dev_warn(dev,
+				"The camera frame rate is not supported!\n");
+			return -EINVAL;
+		}
+
+		orig_mode = sensor->streamcap.capturemode;
+		ret = ov5640_init_mode(sensor, frame_rate,
+				(u32)a->parm.capture.capturemode, orig_mode);
+		if (ret < 0)
+			return ret;
+
+		sensor->streamcap.timeperframe = *timeperframe;
+		sensor->streamcap.capturemode =
+				(u32)a->parm.capture.capturemode;
+
+		break;
+
+	/* These are all the possible cases. */
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
+	case V4L2_BUF_TYPE_VIDEO_OVERLAY:
+	case V4L2_BUF_TYPE_VBI_CAPTURE:
+	case V4L2_BUF_TYPE_VBI_OUTPUT:
+	case V4L2_BUF_TYPE_SLICED_VBI_CAPTURE:
+	case V4L2_BUF_TYPE_SLICED_VBI_OUTPUT:
+		dev_warn(dev, "Type is not V4L2_BUF_TYPE_VIDEO_CAPTURE but %d\n",
+			a->type);
+		ret = -EINVAL;
+		break;
+
+	default:
+		dev_warn(dev, "Type is unknown - %d\n", a->type);
+		ret = -EINVAL;
+		break;
+	}
+
+	return ret;
+}
+
+static int ov5640_set_fmt(struct v4l2_subdev *sd,
+			struct v4l2_subdev_pad_config *cfg,
+			struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *mf = &format->format;
+	const struct ov5640_datafmt *fmt = ov5640_find_datafmt(mf->code);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ov5640 *sensor = to_ov5640(client);
+	int capturemode;
+
+	if (!fmt) {
+		mf->code	= ov5640_colour_fmts[0].code;
+		mf->colorspace	= ov5640_colour_fmts[0].colorspace;
+		fmt		= &ov5640_colour_fmts[0];
+	}
+
+	mf->field	= V4L2_FIELD_NONE;
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY)
+		return 0;
+
+	sensor->fmt = fmt;
+
+	capturemode = get_capturemode(mf->width, mf->height);
+	if (capturemode >= 0) {
+		sensor->streamcap.capturemode = capturemode;
+		sensor->pix.width = mf->width;
+		sensor->pix.height = mf->height;
+		return 0;
+	}
+
+	dev_err(&client->dev, "Set format failed %d, %d\n",
+		fmt->code, fmt->colorspace);
+	return -EINVAL;
+}
+
+
+static int ov5640_get_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_pad_config *cfg,
+			  struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *mf = &format->format;
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ov5640 *sensor = to_ov5640(client);
+	const struct ov5640_datafmt *fmt = sensor->fmt;
+
+	if (format->pad)
+		return -EINVAL;
+
+	mf->code	= fmt->code;
+	mf->colorspace	= fmt->colorspace;
+	mf->field	= V4L2_FIELD_NONE;
+
+	mf->width	= sensor->pix.width;
+	mf->height	= sensor->pix.height;
+
+	return 0;
+}
+
+static int ov5640_enum_mbus_code(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_pad_config *cfg,
+				 struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->pad || code->index >= ARRAY_SIZE(ov5640_colour_fmts))
+		return -EINVAL;
+
+	code->code = ov5640_colour_fmts[code->index].code;
+	return 0;
+}
+
+/*!
+ * ov5640_enum_framesizes - V4L2 sensor interface handler for
+ *			   VIDIOC_ENUM_FRAMESIZES ioctl
+ * @s: pointer to standard V4L2 device structure
+ * @fsize: standard V4L2 VIDIOC_ENUM_FRAMESIZES ioctl structure
+ *
+ * Return 0 if successful, otherwise -EINVAL.
+ */
+static int ov5640_enum_framesizes(struct v4l2_subdev *sd,
+			       struct v4l2_subdev_pad_config *cfg,
+			       struct v4l2_subdev_frame_size_enum *fse)
+{
+	if (fse->index > ov5640_mode_MAX)
+		return -EINVAL;
+
+	fse->max_width =
+			max(ov5640_mode_info_data[0][fse->index].width,
+			    ov5640_mode_info_data[1][fse->index].width);
+	fse->min_width = fse->max_width;
+	fse->max_height =
+			max(ov5640_mode_info_data[0][fse->index].height,
+			    ov5640_mode_info_data[1][fse->index].height);
+	fse->min_height = fse->max_height;
+	return 0;
+}
+
+/*!
+ * ov5640_enum_frameintervals - V4L2 sensor interface handler for
+ *			       VIDIOC_ENUM_FRAMEINTERVALS ioctl
+ * @s: pointer to standard V4L2 device structure
+ * @fival: standard V4L2 VIDIOC_ENUM_FRAMEINTERVALS ioctl structure
+ *
+ * Return 0 if successful, otherwise -EINVAL.
+ */
+static int ov5640_enum_frameintervals(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_frame_interval_enum *fie)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct device *dev = &client->dev;
+	int i, j, count = 0;
+
+	if (fie->index > ov5640_mode_MAX)
+		return -EINVAL;
+
+	if (fie->width == 0 || fie->height == 0 ||
+	    fie->code == 0) {
+		dev_warn(dev, "Please assign pixel format, width and height\n");
+		return -EINVAL;
+	}
+
+	fie->interval.numerator = 1;
+
+	count = 0;
+	for (i = 0; i < ARRAY_SIZE(ov5640_mode_info_data); i++) {
+		for (j = 0; j < (ov5640_mode_MAX + 1); j++) {
+			if (fie->width == ov5640_mode_info_data[i][j].width
+			 && fie->height == ov5640_mode_info_data[i][j].height
+			 && ov5640_mode_info_data[i][j].init_data_ptr != NULL) {
+				count++;
+			}
+			if (fie->index == (count - 1)) {
+				fie->interval.denominator =
+						ov5640_framerates[i];
+				return 0;
+			}
+		}
+	}
+
+	return -EINVAL;
+}
+
+/*!
+ * dev_init - V4L2 sensor init
+ * @s: pointer to standard V4L2 device structure
+ *
+ */
+static int init_device(struct ov5640 *sensor)
+{
+	struct device *dev = &sensor->i2c_client->dev;
+	u32 tgt_xclk;	/* target xclk */
+	u32 tgt_fps;	/* target frames per secound */
+	enum ov5640_frame_rate frame_rate;
+	int ret;
+
+	sensor->on = true;
+
+	/* mclk */
+	tgt_xclk = sensor->mclk;
+	tgt_xclk = min(tgt_xclk, (u32)OV5640_XCLK_MAX);
+	tgt_xclk = max(tgt_xclk, (u32)OV5640_XCLK_MIN);
+	sensor->mclk = tgt_xclk;
+
+	dev_dbg(dev, "Setting mclk to %d MHz\n", tgt_xclk / 1000000);
+
+	/* Default camera frame rate is set in probe */
+	tgt_fps = sensor->streamcap.timeperframe.denominator /
+		  sensor->streamcap.timeperframe.numerator;
+
+	if (tgt_fps == 15)
+		frame_rate = ov5640_15_fps;
+	else if (tgt_fps == 30)
+		frame_rate = ov5640_30_fps;
+	else
+		return -EINVAL; /* Only support 15fps or 30fps now. */
+
+	ret = ov5640_init_mode(sensor, frame_rate, ov5640_mode_INIT,
+			       ov5640_mode_INIT);
+
+	return ret;
+}
+
+static int ov5640_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ov5640 *sensor = to_ov5640(client);
+	struct device *dev = &sensor->i2c_client->dev;
+
+	dev_info(dev, "s_stream: %d\n", enable);
+	if (enable)
+		OV5640_stream_on(sensor);
+	else
+		OV5640_stream_off(sensor);
+	return 0;
+}
+
+static struct v4l2_subdev_video_ops ov5640_subdev_video_ops = {
+	.g_parm = ov5640_g_parm,
+	.s_parm = ov5640_s_parm,
+	.s_stream = ov5640_s_stream,
+};
+
+static const struct v4l2_subdev_pad_ops ov5640_subdev_pad_ops = {
+	.enum_frame_size       = ov5640_enum_framesizes,
+	.enum_frame_interval   = ov5640_enum_frameintervals,
+	.enum_mbus_code        = ov5640_enum_mbus_code,
+	.set_fmt               = ov5640_set_fmt,
+	.get_fmt               = ov5640_get_fmt,
+};
+
+static struct v4l2_subdev_core_ops ov5640_subdev_core_ops = {
+	.s_power	= ov5640_s_power,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register	= ov5640_get_register,
+	.s_register	= ov5640_set_register,
+#endif
+};
+
+static struct v4l2_subdev_ops ov5640_subdev_ops = {
+	.core	= &ov5640_subdev_core_ops,
+	.video	= &ov5640_subdev_video_ops,
+	.pad	= &ov5640_subdev_pad_ops,
+};
+
+static void ov5640_adjust_setting_20mhz(void)
+{
+	struct reg_value *regsetting;
+	int i, array_size;
+
+	/* adjust for INIT mode */
+	regsetting = ov5640_init_setting_30fps_VGA;
+	array_size = ARRAY_SIZE(ov5640_init_setting_30fps_VGA);
+
+	for (i = 0; i < array_size; i++, regsetting++)
+		if (regsetting->u16RegAddr == 0x3037)
+			regsetting->u8Val = 0x17;
+}
+
+/*!
+ * ov5640 I2C probe function
+ *
+ * @param adapter            struct i2c_adapter *
+ * @return  Error code indicating success or failure
+ */
+static int ov5640_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	struct pinctrl *pinctrl;
+	struct device *dev = &client->dev;
+	int retval;
+	u8 chip_id_high, chip_id_low;
+	struct ov5640 *sensor;
+
+	sensor = devm_kzalloc(dev, sizeof(*sensor), GFP_KERNEL);
+	if (!sensor)
+		return -ENOMEM;
+
+	/* ov5640 pinctrl */
+	pinctrl = devm_pinctrl_get_select_default(dev);
+	if (IS_ERR(pinctrl))
+		dev_warn(dev, "No pin available\n");
+
+	sensor->gate_gpio= of_get_named_gpio(dev->of_node, "gate-gpios", 0);
+	if (!gpio_is_valid(sensor->gate_gpio))
+		dev_warn(dev, "no sensor gatepin available");
+	else {
+		retval = devm_gpio_request_one(dev, sensor->gate_gpio, GPIOF_OUT_INIT_HIGH,
+						"ov5640_mipi_gate");
+		if (retval < 0) {
+			dev_warn(dev, "Failed to set gate pin\n");
+			dev_warn(dev, "retval=%d\n", retval);
+			return retval;
+		}
+	}
+
+	/* request power down pin */
+	sensor->pwn_gpio = of_get_named_gpio(dev->of_node, "pwn-gpios", 0);
+	if (!gpio_is_valid(sensor->pwn_gpio))
+		dev_warn(dev, "No sensor pwdn pin available");
+	else {
+		retval = devm_gpio_request_one(dev, sensor->pwn_gpio,
+				GPIOF_OUT_INIT_HIGH, "ov5640_mipi_pwdn");
+		if (retval < 0) {
+			dev_warn(dev, "Failed to set power pin\n");
+			dev_warn(dev, "retval=%d\n", retval);
+			return retval;
+		}
+	}
+
+	/* request reset pin */
+	sensor->rst_gpio = of_get_named_gpio(dev->of_node, "rst-gpios", 0);
+	if (!gpio_is_valid(sensor->rst_gpio))
+		dev_warn(dev, "No sensor reset pin available");
+	else {
+		retval = devm_gpio_request_one(dev, sensor->rst_gpio,
+				GPIOF_OUT_INIT_HIGH, "ov5640_mipi_reset");
+		if (retval < 0) {
+			dev_warn(dev, "Failed to set reset pin\n");
+			return retval;
+		}
+	}
+
+
+	sensor->sensor_clk = devm_clk_get(dev, "csi_mclk");
+	if (IS_ERR(sensor->sensor_clk)) {
+		/* assuming clock enabled by default */
+		sensor->sensor_clk = NULL;
+		dev_err(dev, "clock-frequency missing or invalid\n");
+		return PTR_ERR(sensor->sensor_clk);
+	}
+
+	retval = of_property_read_u32(dev->of_node, "mclk",
+					&(sensor->mclk));
+	if (retval) {
+		dev_err(dev, "mclk missing or invalid\n");
+		return retval;
+	}
+
+	if (sensor->mclk == OV5640_XCLK_20MHZ)
+		ov5640_adjust_setting_20mhz();
+
+	retval = of_property_read_u32(dev->of_node, "mclk_source",
+					(u32 *) &(sensor->mclk_source));
+	if (retval) {
+		dev_err(dev, "mclk_source missing or invalid\n");
+		return retval;
+	}
+
+	retval = of_property_read_u32(dev->of_node, "csi_id",
+					&(sensor->csi));
+	if (retval) {
+		dev_err(dev, "csi id missing or invalid\n");
+		return retval;
+	}
+
+	clk_prepare_enable(sensor->sensor_clk);
+
+	sensor->io_init = ov5640_reset;
+	sensor->i2c_client = client;
+	sensor->pix.pixelformat = V4L2_PIX_FMT_YUYV;
+	sensor->pix.width = 640;
+	sensor->pix.height = 480;
+	sensor->streamcap.capability = V4L2_MODE_HIGHQUALITY |
+					   V4L2_CAP_TIMEPERFRAME;
+	sensor->streamcap.capturemode = 0;
+	sensor->streamcap.timeperframe.denominator = DEFAULT_FPS;
+	sensor->streamcap.timeperframe.numerator = 1;
+
+	ov5640_regulator_enable(&client->dev);
+
+	mutex_lock(&ov5640_mutex);
+	{
+		ov5640_reset(sensor);
+		ov5640_power_down(sensor, 0);
+		retval = ov5640_update_slave_id(sensor);
+	}
+	mutex_unlock(&ov5640_mutex);
+	if (retval < 0) {
+		clk_disable_unprepare(sensor->sensor_clk);
+		return -ENODEV;
+	}
+
+	retval = ov5640_read_reg(sensor, OV5640_CHIP_ID_HIGH_BYTE,
+				 &chip_id_high);
+	if (retval < 0 || chip_id_high != 0x56) {
+		dev_warn(dev, "Camera is not found\n");
+		ov5640_regualtor_disable();
+		clk_disable_unprepare(sensor->sensor_clk);
+		return -ENODEV;
+	}
+	retval = ov5640_read_reg(sensor, OV5640_CHIP_ID_LOW_BYTE, &chip_id_low);
+	if (retval < 0 || chip_id_low != 0x40) {
+		dev_warn(dev, "Camera is not found\n");
+		ov5640_regualtor_disable();
+		clk_disable_unprepare(sensor->sensor_clk);
+		return -ENODEV;
+	}
+
+
+	retval = init_device(sensor);
+	if (retval < 0) {
+		ov5640_regualtor_disable();
+		clk_disable_unprepare(sensor->sensor_clk);
+		dev_warn(dev, "Camera init failed\n");
+		ov5640_power_down(sensor, 1);
+		return retval;
+	}
+
+	v4l2_i2c_subdev_init(&sensor->subdev, client, &ov5640_subdev_ops);
+
+	sensor->subdev.grp_id = 678;
+	retval = v4l2_async_register_subdev(&sensor->subdev);
+	if (retval < 0)
+		dev_err(&client->dev, "Async register failed, ret=%d\n",
+			retval);
+
+	OV5640_stream_off(sensor);
+	dev_info(dev, "Camera is found\n");
+	return retval;
+}
+
+/*!
+ * ov5640 I2C detach function
+ *
+ * @param client            struct i2c_client *
+ * @return  Error code indicating success or failure
+ */
+static int ov5640_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ov5640 *sensor = to_ov5640(client);
+
+	v4l2_async_unregister_subdev(sd);
+
+	clk_disable_unprepare(sensor->sensor_clk);
+
+	ov5640_power_down(sensor, 1);
+
+	ov5640_regualtor_disable();
+
+	return 0;
+}
+
+module_i2c_driver(ov5640_i2c_driver);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("OV5640 MIPI Camera Driver");
+MODULE_LICENSE("GPL");
+MODULE_VERSION("1.0");
+MODULE_ALIAS("CSI");
diff --git a/drivers/media/v4l2-core/v4l2-dev.c b/drivers/media/v4l2-core/v4l2-dev.c
index a593ea059..9ffc00a42 100644
--- a/drivers/media/v4l2-core/v4l2-dev.c
+++ b/drivers/media/v4l2-core/v4l2-dev.c
@@ -592,6 +592,7 @@ static void determine_valid_ioctls(struct video_device *vdev)
 	set_bit(_IOC_NR(VIDIOC_DBG_G_REGISTER), valid_ioctls);
 	set_bit(_IOC_NR(VIDIOC_DBG_S_REGISTER), valid_ioctls);
 #endif
+	SET_VALID_IOCTL(ops, VIDIOC_DBG_G_CHIP_IDENT, vidioc_g_chip_ident);
 	/* yes, really vidioc_subscribe_event */
 	SET_VALID_IOCTL(ops, VIDIOC_DQEVENT, vidioc_subscribe_event);
 	SET_VALID_IOCTL(ops, VIDIOC_SUBSCRIBE_EVENT, vidioc_subscribe_event);
diff --git a/drivers/media/v4l2-core/v4l2-ioctl.c b/drivers/media/v4l2-core/v4l2-ioctl.c
index 9eda8b91d..9121f044e 100644
--- a/drivers/media/v4l2-core/v4l2-ioctl.c
+++ b/drivers/media/v4l2-core/v4l2-ioctl.c
@@ -26,6 +26,8 @@
 #include <media/videobuf2-v4l2.h>
 #include <media/v4l2-mc.h>
 #include <media/v4l2-mem2mem.h>
+#include <media/v4l2-chip-ident.h>
+#include <media/videobuf2-core.h>
 
 #include <trace/events/v4l2.h>
 
@@ -681,6 +683,20 @@ static void v4l_print_decoder_cmd(const void *arg, bool write_only)
 		pr_info("pts=%llu\n", p->stop.pts);
 }
 
+static void v4l_print_dbg_chip_ident(const void *arg, bool write_only)
+{
+	const struct v4l2_dbg_chip_ident *p = arg;
+
+	pr_cont("type=%u, ", p->match.type);
+	if (p->match.type == V4L2_CHIP_MATCH_I2C_DRIVER)
+		pr_cont("name=%.*s, ",
+				(int)sizeof(p->match.name), p->match.name);
+	else
+		pr_cont("addr=%u, ", p->match.addr);
+	pr_cont("chip_ident=%u, revision=0x%x\n",
+			p->ident, p->revision);
+}
+
 static void v4l_print_dbg_chip_info(const void *arg, bool write_only)
 {
 	const struct v4l2_dbg_chip_info *p = arg;
@@ -1303,6 +1319,7 @@ static void v4l_fill_fmtdesc(struct v4l2_fmtdesc *fmt)
 	case V4L2_PIX_FMT_YUV444:	descr = "16-bit A/XYUV 4-4-4-4"; break;
 	case V4L2_PIX_FMT_YUV555:	descr = "16-bit A/XYUV 1-5-5-5"; break;
 	case V4L2_PIX_FMT_YUV565:	descr = "16-bit YUV 5-6-5"; break;
+	case V4L2_PIX_FMT_YUV24:	descr = "24-bit YUV 4:4:4 8-8-8"; break;
 	case V4L2_PIX_FMT_YUV32:	descr = "32-bit A/XYUV 8-8-8-8"; break;
 	case V4L2_PIX_FMT_AYUV32:	descr = "32-bit AYUV 8-8-8-8"; break;
 	case V4L2_PIX_FMT_XYUV32:	descr = "32-bit XYUV 8-8-8-8"; break;
@@ -2133,22 +2150,7 @@ static int v4l_s_parm(const struct v4l2_ioctl_ops *ops,
 	struct v4l2_streamparm *p = arg;
 	int ret = check_fmt(file, p->type);
 
-	if (ret)
-		return ret;
-
-	/* Note: extendedmode is never used in drivers */
-	if (V4L2_TYPE_IS_OUTPUT(p->type)) {
-		memset(p->parm.output.reserved, 0,
-		       sizeof(p->parm.output.reserved));
-		p->parm.output.extendedmode = 0;
-		p->parm.output.outputmode &= V4L2_MODE_HIGHQUALITY;
-	} else {
-		memset(p->parm.capture.reserved, 0,
-		       sizeof(p->parm.capture.reserved));
-		p->parm.capture.extendedmode = 0;
-		p->parm.capture.capturemode &= V4L2_MODE_HIGHQUALITY;
-	}
-	return ops->vidioc_s_parm(file, fh, p);
+	return ret ? ret : ops->vidioc_s_parm(file, fh, p);
 }
 
 static int v4l_queryctrl(const struct v4l2_ioctl_ops *ops,
@@ -2562,6 +2564,18 @@ static int v4l_dbg_s_register(const struct v4l2_ioctl_ops *ops,
 #endif
 }
 
+static int v4l_dbg_g_chip_ident(const struct v4l2_ioctl_ops *ops,
+				struct file *file, void *fh, void *arg)
+{
+	struct v4l2_dbg_chip_ident *p = arg;
+
+	p->ident = V4L2_IDENT_NONE;
+	p->revision = 0;
+	if (p->match.type == V4L2_CHIP_MATCH_SUBDEV)
+		return -EINVAL;
+	return ops->vidioc_g_chip_ident(file, fh, p);
+}
+
 static int v4l_dbg_g_chip_info(const struct v4l2_ioctl_ops *ops,
 				struct file *file, void *fh, void *arg)
 {
@@ -2837,6 +2851,7 @@ static const struct v4l2_ioctl_info v4l2_ioctls[] = {
 	IOCTL_INFO(VIDIOC_TRY_DECODER_CMD, v4l_stub_try_decoder_cmd, v4l_print_decoder_cmd, 0),
 	IOCTL_INFO(VIDIOC_DBG_S_REGISTER, v4l_dbg_s_register, v4l_print_dbg_register, 0),
 	IOCTL_INFO(VIDIOC_DBG_G_REGISTER, v4l_dbg_g_register, v4l_print_dbg_register, 0),
+	IOCTL_INFO(VIDIOC_DBG_G_CHIP_IDENT, v4l_dbg_g_chip_ident, v4l_print_dbg_chip_ident, 0),
 	IOCTL_INFO(VIDIOC_S_HW_FREQ_SEEK, v4l_s_hw_freq_seek, v4l_print_hw_freq_seek, INFO_FL_PRIO),
 	IOCTL_INFO(VIDIOC_S_DV_TIMINGS, v4l_stub_s_dv_timings, v4l_print_dv_timings, INFO_FL_PRIO | INFO_FL_CLEAR(v4l2_dv_timings, bt.flags)),
 	IOCTL_INFO(VIDIOC_G_DV_TIMINGS, v4l_stub_g_dv_timings, v4l_print_dv_timings, 0),
@@ -3331,6 +3346,7 @@ video_usercopy(struct file *file, unsigned int orig_cmd, unsigned long arg,
 	kvfree(mbuf);
 	return err;
 }
+EXPORT_SYMBOL(video_usercopy);
 
 long video_ioctl2(struct file *file,
 	       unsigned int cmd, unsigned long arg)
