Linux 2.6.31 Release for Digi ConnectCore Wi-i.MX boards2.6.31-digi-201102101717

23 files changed, 2041 insertions, 1259 deletions

diff --git a/drivers/media/video/Kconfig b/drivers/media/video/Kconfig
index c64c8d201262..e7185d1cf281 100644
--- a/drivers/media/video/Kconfig
+++ b/drivers/media/video/Kconfig
@@ -592,6 +592,17 @@ config VIDEO_MXS_PXP
 	  To compile this driver as a module, choose M here: the
 	  module will be called pxp.
 
+config VIDEO_MXC_PXP_V4L2
+	tristate "MXC PxP V4L2 driver"
+	depends on VIDEO_DEV && VIDEO_V4L2 && ARCH_MX5
+	select VIDEOBUF_DMA_CONTIG
+	---help---
+	  This is a video4linux driver for the Freescale PxP
+	  (Pixel Pipeline). This module supports output overlay of
+	  the MXC framebuffer on a video stream.
+
+	  To compile this driver as a module, choose M here.
+
 config VIDEO_MXC_OPL
 	tristate
 	depends on VIDEO_DEV && ARCH_MXC
diff --git a/drivers/media/video/Makefile b/drivers/media/video/Makefile
index 49cb18802024..a4de2a151253 100644
--- a/drivers/media/video/Makefile
+++ b/drivers/media/video/Makefile
@@ -94,6 +94,7 @@ obj-$(CONFIG_VIDEO_MXC_CSI_CAMERA) += mxc/capture/
 obj-$(CONFIG_VIDEO_MXC_IPU_OUTPUT) += mxc/output/
 obj-$(CONFIG_VIDEO_MXC_IPUV1_WVGA_OUTPUT) += mxc/output/
 obj-$(CONFIG_VIDEO_MXC_EMMA_OUTPUT) += mxc/output/
+obj-$(CONFIG_VIDEO_MXC_PXP_V4L2) += mxc/output/
 obj-$(CONFIG_VIDEO_MXC_OPL) += mxc/opl/
 obj-$(CONFIG_VIDEO_PXP)	+= pxp.o
 obj-$(CONFIG_VIDEO_MXS_PXP)	+= mxs_pxp.o
diff --git a/drivers/media/video/mxc/capture/Kconfig b/drivers/media/video/mxc/capture/Kconfig
index adab0a886f36..276dfa424feb 100644
--- a/drivers/media/video/mxc/capture/Kconfig
+++ b/drivers/media/video/mxc/capture/Kconfig
@@ -58,6 +58,22 @@ config MXC_CAMERA_MICRON111
 	---help---
 	  If you plan to use the mt9v111 Camera with your MXC system, say Y here.
 
+config MXC_CAMERA_MICRON111_1
+    tristate "Micron mt9v111 camera 1 support"
+    select I2C_MXC
+    depends on ! VIDEO_MXC_EMMA_CAMERA
+    depends on MXC_CAMERA_MICRON111
+    ---help---
+      If you plan to use the mt9v111 Camera 1 with your MXC system, say Y here.
+
+config MXC_CAMERA_MICRON111_2
+    tristate "Micron mt9v111 camera 2 support"
+    select I2C_MXC
+    depends on ! VIDEO_MXC_EMMA_CAMERA
+    depends on MXC_CAMERA_MICRON111
+    ---help---
+      If you plan to use the mt9v111 Camera 2 with your MXC system, say Y here.
+
 config MXC_CAMERA_OV2640
 	tristate "OmniVision ov2640 camera support"
 	depends on !VIDEO_MXC_EMMA_CAMERA
@@ -72,7 +88,7 @@ config MXC_CAMERA_OV3640
 
 config MXC_TVIN_ADV7180
 	tristate "Analog Device adv7180 TV Decoder Input support"
-	depends on MACH_MX35_3DS
+	depends on (MACH_MX35_3DS || MACH_MX51_3DS)
 	---help---
 	  If you plan to use the adv7180 video decoder with your MXC system, say Y here.
 
diff --git a/drivers/media/video/mxc/capture/Makefile b/drivers/media/video/mxc/capture/Makefile
index 112923c8fc8f..03ff094171bf 100644
--- a/drivers/media/video/mxc/capture/Makefile
+++ b/drivers/media/video/mxc/capture/Makefile
@@ -35,5 +35,5 @@ obj-$(CONFIG_MXC_CAMERA_OV2640) += ov2640_camera.o
 ov3640_camera-objs := ov3640.o sensor_clock.o
 obj-$(CONFIG_MXC_CAMERA_OV3640) += ov3640_camera.o
 
-adv7180_tvin-objs := adv7180.o sensor_clock.o
+adv7180_tvin-objs := adv7180.o
 obj-$(CONFIG_MXC_TVIN_ADV7180) += adv7180_tvin.o
diff --git a/drivers/media/video/mxc/capture/adv7180.c b/drivers/media/video/mxc/capture/adv7180.c
index 1edee763bebc..527a0d1ad9fa 100644
--- a/drivers/media/video/mxc/capture/adv7180.c
+++ b/drivers/media/video/mxc/capture/adv7180.c
@@ -1,5 +1,5 @@
 /*
- * Copyright 2005-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2005-2010 Freescale Semiconductor, Inc. All Rights Reserved.
  */
 
 /*
@@ -39,6 +39,7 @@ static struct regulator *dvddio_regulator;
 static struct regulator *dvdd_regulator;
 static struct regulator *avdd_regulator;
 static struct regulator *pvdd_regulator;
+static struct mxc_tvin_platform_data *tvin_plat;
 
 extern void gpio_sensor_active(void);
 extern void gpio_sensor_inactive(void);
@@ -118,26 +119,26 @@ static video_fmt_t video_fmts[] = {
 	{			/*! NTSC */
 	 .v4l2_id = V4L2_STD_NTSC,
 	 .name = "NTSC",
-	 .raw_width = 720 - 1,	/* SENS_FRM_WIDTH */
-	 .raw_height = 288 - 1,	/* SENS_FRM_HEIGHT */
+	 .raw_width = 720,	/* SENS_FRM_WIDTH */
+	 .raw_height = 525,	/* SENS_FRM_HEIGHT */
 	 .active_width = 720,	/* ACT_FRM_WIDTH plus 1 */
-	 .active_height = (480 / 2),	/* ACT_FRM_WIDTH plus 1 */
+	 .active_height = 480,	/* ACT_FRM_WIDTH plus 1 */
 	 },
 	{			/*! (B, G, H, I, N) PAL */
 	 .v4l2_id = V4L2_STD_PAL,
 	 .name = "PAL",
-	 .raw_width = 720 - 1,
-	 .raw_height = (576 / 2) + 24 * 2 - 1,
+	 .raw_width = 720,
+	 .raw_height = 625,
 	 .active_width = 720,
-	 .active_height = (576 / 2),
+	 .active_height = 576,
 	 },
 	{			/*! Unlocked standard */
 	 .v4l2_id = V4L2_STD_ALL,
 	 .name = "Autodetect",
-	 .raw_width = 720 - 1,
-	 .raw_height = (576 / 2) + 24 * 2 - 1,
+	 .raw_width = 720,
+	 .raw_height = 625,
 	 .active_width = 720,
-	 .active_height = (576 / 2),
+	 .active_height = 576,
 	 },
 };
 
@@ -246,6 +247,10 @@ static void adv7180_get_std(v4l2_std_id *std)
 
 	dev_dbg(&adv7180_data.i2c_client->dev, "In adv7180_get_std\n");
 
+	/* Make sure power on */
+	if (tvin_plat->pwdn)
+		tvin_plat->pwdn(0);
+
 	/* Read the AD_RESULT to get the detect output video standard */
 	tmp = adv7180_read(ADV7180_STATUS_1) & 0x70;
 
@@ -335,6 +340,11 @@ static int ioctl_s_power(struct v4l2_int_device *s, int on)
 
 	if (on && !sensor->on) {
 		gpio_sensor_active();
+
+		/* Make sure pwoer on */
+		if (tvin_plat->pwdn)
+			tvin_plat->pwdn(0);
+
 		if (adv7180_write_reg(ADV7180_PWR_MNG, 0) != 0)
 			return -EIO;
 	} else if (!on && sensor->on) {
@@ -500,6 +510,10 @@ static int ioctl_g_ctrl(struct v4l2_int_device *s, struct v4l2_control *vc)
 
 	dev_dbg(&adv7180_data.i2c_client->dev, "In adv7180:ioctl_g_ctrl\n");
 
+	/* Make sure power on */
+	if (tvin_plat->pwdn)
+		tvin_plat->pwdn(0);
+
 	switch (vc->id) {
 	case V4L2_CID_BRIGHTNESS:
 		dev_dbg(&adv7180_data.i2c_client->dev,
@@ -593,6 +607,10 @@ static int ioctl_s_ctrl(struct v4l2_int_device *s, struct v4l2_control *vc)
 
 	dev_dbg(&adv7180_data.i2c_client->dev, "In adv7180:ioctl_s_ctrl\n");
 
+	/* Make sure power on */
+	if (tvin_plat->pwdn)
+		tvin_plat->pwdn(0);
+
 	switch (vc->id) {
 	case V4L2_CID_BRIGHTNESS:
 		dev_dbg(&adv7180_data.i2c_client->dev,
@@ -803,13 +821,13 @@ static int adv7180_probe(struct i2c_client *client,
 {
 	int rev_id;
 	int ret = 0;
-	struct mxc_tvin_platform_data *plat_data = client->dev.platform_data;
+	tvin_plat = client->dev.platform_data;
 
 	dev_dbg(&adv7180_data.i2c_client->dev, "In adv7180_probe\n");
 
-	if (plat_data->dvddio_reg) {
+	if (tvin_plat->dvddio_reg) {
 		dvddio_regulator =
-		    regulator_get(&client->dev, plat_data->dvddio_reg);
+		    regulator_get(&client->dev, tvin_plat->dvddio_reg);
 		if (!IS_ERR_VALUE((unsigned long)dvddio_regulator)) {
 			regulator_set_voltage(dvddio_regulator, 3300000, 3300000);
 			if (regulator_enable(dvddio_regulator) != 0)
@@ -817,9 +835,9 @@ static int adv7180_probe(struct i2c_client *client,
 		}
 	}
 
-	if (plat_data->dvdd_reg) {
+	if (tvin_plat->dvdd_reg) {
 		dvdd_regulator =
-		    regulator_get(&client->dev, plat_data->dvdd_reg);
+		    regulator_get(&client->dev, tvin_plat->dvdd_reg);
 		if (!IS_ERR_VALUE((unsigned long)dvdd_regulator)) {
 			regulator_set_voltage(dvdd_regulator, 1800000, 1800000);
 			if (regulator_enable(dvdd_regulator) != 0)
@@ -827,9 +845,9 @@ static int adv7180_probe(struct i2c_client *client,
 		}
 	}
 
-	if (plat_data->avdd_reg) {
+	if (tvin_plat->avdd_reg) {
 		avdd_regulator =
-		    regulator_get(&client->dev, plat_data->avdd_reg);
+		    regulator_get(&client->dev, tvin_plat->avdd_reg);
 		if (!IS_ERR_VALUE((unsigned long)avdd_regulator)) {
 			regulator_set_voltage(avdd_regulator, 1800000, 1800000);
 			if (regulator_enable(avdd_regulator) != 0)
@@ -837,9 +855,9 @@ static int adv7180_probe(struct i2c_client *client,
 		}
 	}
 
-	if (plat_data->pvdd_reg) {
+	if (tvin_plat->pvdd_reg) {
 		pvdd_regulator =
-		    regulator_get(&client->dev, plat_data->pvdd_reg);
+		    regulator_get(&client->dev, tvin_plat->pvdd_reg);
 		if (!IS_ERR_VALUE((unsigned long)pvdd_regulator)) {
 			regulator_set_voltage(pvdd_regulator, 1800000, 1800000);
 			if (regulator_enable(pvdd_regulator) != 0)
@@ -847,11 +865,12 @@ static int adv7180_probe(struct i2c_client *client,
 		}
 	}
 
-	if (plat_data->reset)
-		plat_data->reset();
 
-	if (plat_data->pwdn)
-		plat_data->pwdn(1);
+	if (tvin_plat->reset)
+		tvin_plat->reset();
+
+	if (tvin_plat->pwdn)
+		tvin_plat->pwdn(0);
 
 	msleep(1);
 
@@ -913,7 +932,7 @@ static int adv7180_detach(struct i2c_client *client)
 		__func__, IF_NAME, client->addr << 1, client->adapter->name);
 
 	if (plat_data->pwdn)
-		plat_data->pwdn(0);
+		plat_data->pwdn(1);
 
 	if (dvddio_regulator) {
 		regulator_disable(dvddio_regulator);
diff --git a/drivers/media/video/mxc/capture/csi_v4l2_capture.c b/drivers/media/video/mxc/capture/csi_v4l2_capture.c
index 9bddc3692996..cf224e0673f0 100644
--- a/drivers/media/video/mxc/capture/csi_v4l2_capture.c
+++ b/drivers/media/video/mxc/capture/csi_v4l2_capture.c
@@ -1,5 +1,5 @@
 /*
- * Copyright 2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2009-2010 Freescale Semiconductor, Inc. All Rights Reserved.
  */
 
 /*
@@ -789,15 +789,15 @@ static ssize_t csi_v4l_read(struct file *file, char *buf, size_t count,
 							  (cam->v2f.fmt.
 							   pix.sizeimage),
 							  &cam->
-							  still_buf,
+							  still_buf[0],
 							  GFP_DMA | GFP_KERNEL);
 		if (cam->still_buf_vaddr == NULL) {
 			pr_err("alloc dma memory failed\n");
 			return -ENOMEM;
 		}
 		cam->still_counter = 0;
-		__raw_writel(cam->still_buf, CSI_CSIDMASA_FB2);
-		__raw_writel(cam->still_buf, CSI_CSIDMASA_FB1);
+		__raw_writel(cam->still_buf[0], CSI_CSIDMASA_FB2);
+		__raw_writel(cam->still_buf[0], CSI_CSIDMASA_FB1);
 		__raw_writel(__raw_readl(CSI_CSICR3) | BIT_DMA_REFLASH_RFF,
 			     CSI_CSICR3);
 		__raw_writel(__raw_readl(CSI_CSISR), CSI_CSISR);
@@ -813,8 +813,8 @@ static ssize_t csi_v4l_read(struct file *file, char *buf, size_t count,
 
 	if (cam->still_buf_vaddr != NULL) {
 		dma_free_coherent(0, PAGE_ALIGN(cam->v2f.fmt.pix.sizeimage),
-				  cam->still_buf_vaddr, cam->still_buf);
-		cam->still_buf = 0;
+				  cam->still_buf_vaddr, cam->still_buf[0]);
+		cam->still_buf[0] = 0;
 		cam->still_buf_vaddr = NULL;
 	}
 
diff --git a/drivers/media/video/mxc/capture/emma_v4l2_capture.c b/drivers/media/video/mxc/capture/emma_v4l2_capture.c
index 9cb08b26f1cd..170807716ec6 100644
--- a/drivers/media/video/mxc/capture/emma_v4l2_capture.c
+++ b/drivers/media/video/mxc/capture/emma_v4l2_capture.c
@@ -1,5 +1,5 @@
 /*
- * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2004-2010 Freescale Semiconductor, Inc. All Rights Reserved.
  */
 
 /*
@@ -858,7 +858,7 @@ static void mxc_csi_dma_chaining(void *data)
 
 		/* Config DMA */
 		memset(&dma_request, 0, sizeof(mxc_dma_requestbuf_t));
-		dma_request.dst_addr = cam->still_buf
+		dma_request.dst_addr = cam->still_buf[0]
 		    + (chained % max_dma) * CSI_DMA_LENGTH;
 		dma_request.src_addr = (dma_addr_t) CSI_CSIRXFIFO_PHYADDR;
 		dma_request.num_of_bytes = count;
@@ -1040,7 +1040,7 @@ mxc_v4l_read(struct file *file, char *buf, size_t count, loff_t *ppos)
 
 	cam->still_buf_vaddr = dma_alloc_coherent(0,
 						  PAGE_ALIGN(CSI_MEM_SIZE),
-						  &cam->still_buf,
+						  &cam->still_buf[0],
 						  GFP_DMA | GFP_KERNEL);
 
 	if (!cam->still_buf_vaddr) {
@@ -1120,8 +1120,8 @@ mxc_v4l_read(struct file *file, char *buf, size_t count, loff_t *ppos)
 
       exit1:
 	dma_free_coherent(0, PAGE_ALIGN(CSI_MEM_SIZE),
-			  cam->still_buf_vaddr, cam->still_buf);
-	cam->still_buf = 0;
+			  cam->still_buf_vaddr, cam->still_buf[0]);
+	cam->still_buf[0] = 0;
 
       exit0:
 	up(&cam->busy_lock);
@@ -1160,7 +1160,8 @@ mxc_v4l_read(struct file *file, char *buf, size_t count, loff_t *ppos)
 
 	v_address = dma_alloc_coherent(0,
 				       PAGE_ALIGN(cam->v2f.fmt.pix.sizeimage),
-				       &cam->still_buf, GFP_DMA | GFP_KERNEL);
+				       &cam->still_buf[0],
+				       GFP_DMA | GFP_KERNEL);
 
 	if (!v_address) {
 		pr_info("mxc_v4l_read failed at allocate still_buf\n");
@@ -1194,8 +1195,8 @@ mxc_v4l_read(struct file *file, char *buf, size_t count, loff_t *ppos)
 
       exit1:
 	dma_free_coherent(0, cam->v2f.fmt.pix.sizeimage, v_address,
-			  cam->still_buf);
-	cam->still_buf = 0;
+			  cam->still_buf[0]);
+	cam->still_buf[0] = 0;
 
       exit0:
 	up(&cam->busy_lock);
diff --git a/drivers/media/video/mxc/capture/ipu_csi_enc.c b/drivers/media/video/mxc/capture/ipu_csi_enc.c
index fd3f0c132c14..0b87282551ff 100644
--- a/drivers/media/video/mxc/capture/ipu_csi_enc.c
+++ b/drivers/media/video/mxc/capture/ipu_csi_enc.c
@@ -25,6 +25,7 @@
 #include "ipu_prp_sw.h"
 
 #ifdef CAMERA_DBG
+	extern void ipu_dump_registers(void);
 	#define CAMERA_TRACE(x) (printk)x
 #else
 	#define CAMERA_TRACE(x)
@@ -66,6 +67,7 @@ static int csi_enc_setup(cam_data *cam)
 	u32 pixel_fmt;
 	int err = 0;
 	dma_addr_t dummy = cam->dummy_frame.buffer.m.offset;
+	ipu_channel_t channel;
 
 	CAMERA_TRACE("In csi_enc_setup\n");
 	if (!cam) {
@@ -101,13 +103,18 @@ static int csi_enc_setup(cam_data *cam)
 
 	ipu_csi_enable_mclk_if(CSI_MCLK_ENC, cam->csi, true, true);
 
-	err = ipu_init_channel(CSI_MEM, &params);
+	if (cam->csi == 0)
+		channel = CSI_MEM0;
+	else
+		channel = CSI_MEM1;
+
+	err = ipu_init_channel(channel, &params);
 	if (err != 0) {
 		printk(KERN_ERR "ipu_init_channel %d\n", err);
 		return err;
 	}
 
-	err = ipu_init_channel_buffer(CSI_MEM, IPU_OUTPUT_BUFFER,
+	err = ipu_init_channel_buffer(channel, IPU_OUTPUT_BUFFER,
 				      pixel_fmt, cam->v2f.fmt.pix.width,
 				      cam->v2f.fmt.pix.height,
 				      cam->v2f.fmt.pix.width, IPU_ROTATE_NONE,
@@ -115,12 +122,12 @@ static int csi_enc_setup(cam_data *cam)
 				      cam->offset.u_offset,
 				      cam->offset.v_offset);
 	if (err != 0) {
-		printk(KERN_ERR "CSI_MEM output buffer\n");
+		printk(KERN_ERR "CSI_MEM%d output buffer\n",cam->csi);
 		return err;
 	}
-	err = ipu_enable_channel(CSI_MEM);
+	err = ipu_enable_channel(channel);
 	if (err < 0) {
-		printk(KERN_ERR "ipu_enable_channel CSI_MEM\n");
+		printk(KERN_ERR "ipu_enable_channel CSI_MEM%d\n",cam->csi);
 		return err;
 	}
 
@@ -135,24 +142,34 @@ static int csi_enc_setup(cam_data *cam)
  *
  * @return  status
  */
-static int csi_enc_eba_update(dma_addr_t eba, int *buffer_num)
+static int csi_enc_eba_update(int csi, dma_addr_t eba, int *buffer_num)
 {
 	int err = 0;
+	ipu_channel_t channel;
+
+	if (csi == 0)
+		channel = CSI_MEM0;
+	else
+		channel = CSI_MEM1;
 
-	pr_debug("eba %x\n", eba);
-	err = ipu_update_channel_buffer(CSI_MEM, IPU_OUTPUT_BUFFER,
+	err = ipu_update_channel_buffer(channel, IPU_OUTPUT_BUFFER,
 					*buffer_num, eba);
+
 	if (err != 0) {
-		ipu_clear_buffer_ready(CSI_MEM, IPU_OUTPUT_BUFFER,
+		ipu_clear_buffer_ready(channel, IPU_OUTPUT_BUFFER,
 				       *buffer_num);
 		printk(KERN_ERR "err %d buffer_num %d\n", err, *buffer_num);
 		return err;
 	}
 
-	ipu_select_buffer(CSI_MEM, IPU_OUTPUT_BUFFER, *buffer_num);
+	ipu_select_buffer(channel, IPU_OUTPUT_BUFFER, *buffer_num);
 
 	*buffer_num = (*buffer_num == 0) ? 1 : 0;
 
+#ifdef CAMERA_DBG
+	ipu_dump_registers ();
+#endif
+
 	return 0;
 }
 
@@ -166,6 +183,7 @@ static int csi_enc_enabling_tasks(void *private)
 {
 	cam_data *cam = (cam_data *) private;
 	int err = 0;
+	int ipu_irq_csi_out_eof;
 	CAMERA_TRACE("IPU:In csi_enc_enabling_tasks\n");
 
 	cam->dummy_frame.vaddress = dma_alloc_coherent(0,
@@ -182,11 +200,16 @@ static int csi_enc_enabling_tasks(void *private)
 	    PAGE_ALIGN(cam->v2f.fmt.pix.sizeimage);
 	cam->dummy_frame.buffer.m.offset = cam->dummy_frame.paddress;
 
-	ipu_clear_irq(IPU_IRQ_CSI0_OUT_EOF);
-	err = ipu_request_irq(IPU_IRQ_CSI0_OUT_EOF,
-			      csi_enc_callback, 0, "Mxc Camera", cam);
+	if (cam->csi == 0)
+		ipu_irq_csi_out_eof = IPU_IRQ_CSI0_OUT_EOF;
+	else
+		ipu_irq_csi_out_eof = IPU_IRQ_CSI1_OUT_EOF;
+	ipu_clear_irq(ipu_irq_csi_out_eof);
+	err = ipu_request_irq(ipu_irq_csi_out_eof,
+				csi_enc_callback, 0, "Mxc Camera", cam);
+
 	if (err != 0) {
-		printk(KERN_ERR "Error registering rot irq\n");
+		printk(KERN_ERR "Error registering eot irq for csi %d\n",cam->csi);
 		return err;
 	}
 
@@ -209,12 +232,24 @@ static int csi_enc_disabling_tasks(void *private)
 {
 	cam_data *cam = (cam_data *) private;
 	int err = 0;
+	ipu_channel_t channel;
+	int ipu_irq_csi_out_eof;
 
-	ipu_free_irq(IPU_IRQ_CSI0_OUT_EOF, cam);
+	if (cam->csi == 0)
+	{
+		channel = CSI_MEM0;
+		ipu_irq_csi_out_eof = IPU_IRQ_CSI0_OUT_EOF;
+	}
+	else
+	{
+		channel = CSI_MEM1;
+		ipu_irq_csi_out_eof = IPU_IRQ_CSI1_OUT_EOF;
+	}
 
-	err = ipu_disable_channel(CSI_MEM, true);
+	ipu_free_irq(ipu_irq_csi_out_eof, cam);
+	err = ipu_disable_channel(channel, true);
 
-	ipu_uninit_channel(CSI_MEM);
+	ipu_uninit_channel(channel);
 
 	if (cam->dummy_frame.vaddress != 0) {
 		dma_free_coherent(0, cam->dummy_frame.buffer.length,
diff --git a/drivers/media/video/mxc/capture/ipu_prp_enc.c b/drivers/media/video/mxc/capture/ipu_prp_enc.c
index 4b5426cb887d..0df8050ad7de 100644
--- a/drivers/media/video/mxc/capture/ipu_prp_enc.c
+++ b/drivers/media/video/mxc/capture/ipu_prp_enc.c
@@ -19,12 +19,14 @@
  * @ingroup IPU
  */
 
+#include <linux/types.h>
 #include <linux/dma-mapping.h>
 #include <linux/ipu.h>
 #include "mxc_v4l2_capture.h"
 #include "ipu_prp_sw.h"
 
 #ifdef CAMERA_DBG
+	extern void ipu_dump_registers(void);
 	#define CAMERA_TRACE(x) (printk)x
 #else
 	#define CAMERA_TRACE(x)
@@ -266,11 +268,10 @@ static int prp_enc_setup(cam_data * cam)
  *
  * @return  status
  */
-static int prp_enc_eba_update(dma_addr_t eba, int *buffer_num)
+static int prp_enc_eba_update(int csi, dma_addr_t eba, int *buffer_num)
 {
 	int err = 0;
 
-	pr_debug("eba %x\n", eba);
 	if (grotation >= IPU_ROTATE_90_RIGHT) {
 		err = ipu_update_channel_buffer(MEM_ROT_ENC_MEM,
 						IPU_OUTPUT_BUFFER, *buffer_num,
@@ -294,6 +295,11 @@ static int prp_enc_eba_update(dma_addr_t eba, int *buffer_num)
 	}
 
 	*buffer_num = (*buffer_num == 0) ? 1 : 0;
+
+#ifdef CAMERA_DBG
+	ipu_dump_registers ();
+#endif
+
 	return 0;
 }
 
@@ -350,7 +356,6 @@ static int prp_enc_disabling_tasks(void *private)
 	if (cam->rotation >= IPU_ROTATE_90_RIGHT) {
 		ipu_unlink_channels(CSI_PRP_ENC_MEM, MEM_ROT_ENC_MEM);
 	}
-
 	err = ipu_disable_channel(CSI_PRP_ENC_MEM, true);
 	if (cam->rotation >= IPU_ROTATE_90_RIGHT) {
 		err |= ipu_disable_channel(MEM_ROT_ENC_MEM, true);
diff --git a/drivers/media/video/mxc/capture/ipu_prp_vf_sdc_bg.c b/drivers/media/video/mxc/capture/ipu_prp_vf_sdc_bg.c
index 7f0984c42950..9f8078d558b0 100644
--- a/drivers/media/video/mxc/capture/ipu_prp_vf_sdc_bg.c
+++ b/drivers/media/video/mxc/capture/ipu_prp_vf_sdc_bg.c
@@ -182,6 +182,7 @@ static int prpvf_start(void *private)
 		printk(KERN_ERR "Error initializing CSI_PRP_VF_MEM\n");
 		goto out_3;
 	}
+
 	err = ipu_init_channel(MEM_ROT_VF_MEM, NULL);
 	if (err != 0) {
 		printk(KERN_ERR "Error MEM_ROT_VF_MEM channel\n");
@@ -200,6 +201,7 @@ static int prpvf_start(void *private)
 	}
 
 	if (cam->vf_rotation >= IPU_ROTATE_90_RIGHT) {
+
 		err = ipu_init_channel_buffer(MEM_ROT_VF_MEM, IPU_OUTPUT_BUFFER,
 					      format,
 					      vf.csi_prp_vf_mem.out_height,
diff --git a/drivers/media/video/mxc/capture/ipu_still.c b/drivers/media/video/mxc/capture/ipu_still.c
index 348bf2b9b564..22cf3f51e1cb 100644
--- a/drivers/media/video/mxc/capture/ipu_still.c
+++ b/drivers/media/video/mxc/capture/ipu_still.c
@@ -26,6 +26,9 @@
 #include "ipu_prp_sw.h"
 
 static int callback_eof_flag;
+#ifndef CONFIG_MXC_IPU_V1
+static int buffer_num;
+#endif
 
 #ifdef CONFIG_MXC_IPU_V1
 static int callback_flag;
@@ -42,10 +45,10 @@ static int callback_flag;
  */
 static irqreturn_t prp_csi_eof_callback(int irq, void *dev_id)
 {
-	if (callback_flag == 2) {
-		ipu_select_buffer(CSI_MEM, IPU_OUTPUT_BUFFER, 0);
+	ipu_select_buffer(CSI_MEM, IPU_OUTPUT_BUFFER,
+			  callback_flag%2 ? 1 : 0);
+	if (callback_flag == 0)
 		ipu_enable_channel(CSI_MEM);
-	}
 
 	callback_flag++;
 	return IRQ_HANDLED;
@@ -65,9 +68,12 @@ static irqreturn_t prp_still_callback(int irq, void *dev_id)
 	cam_data *cam = (cam_data *) dev_id;
 
 	callback_eof_flag++;
-	if (callback_eof_flag < 5)
-		ipu_select_buffer(CSI_MEM, IPU_OUTPUT_BUFFER, 0);
-	else {
+	if (callback_eof_flag < 5) {
+#ifndef CONFIG_MXC_IPU_V1
+		buffer_num = (buffer_num == 0) ? 1 : 0;
+		ipu_select_buffer(CSI_MEM, IPU_OUTPUT_BUFFER, buffer_num);
+#endif
+	} else {
 		cam->still_counter++;
 		wake_up_interruptible(&cam->still_queue);
 	}
@@ -87,6 +93,8 @@ static int prp_still_start(void *private)
 	u32 pixel_fmt;
 	int err;
 	ipu_channel_params_t params;
+	ipu_channel_t channel;
+	int ipu_irq_csi_out_eof;
 
 	if (cam->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_YUV420)
 		pixel_fmt = IPU_PIX_FMT_YUV420P;
@@ -113,20 +121,32 @@ static int prp_still_start(void *private)
 
 	ipu_csi_enable_mclk_if(CSI_MCLK_RAW, cam->csi, true, true);
 
+	if (cam->csi == 0) {
+		channel = CSI_MEM0;
+		ipu_irq_csi_out_eof = IPU_IRQ_CSI0_OUT_EOF;
+	}
+	else {
+		channel = CSI_MEM1;
+		ipu_irq_csi_out_eof = IPU_IRQ_CSI1_OUT_EOF;
+	}
+
 	memset(&params, 0, sizeof(params));
-	err = ipu_init_channel(CSI_MEM, &params);
+	params.csi_mem.csi = cam->csi;
+	err = ipu_init_channel(channel, &params);
 	if (err != 0)
 		return err;
 
-	err = ipu_init_channel_buffer(CSI_MEM, IPU_OUTPUT_BUFFER,
+	err = ipu_init_channel_buffer(channel, IPU_OUTPUT_BUFFER,
 				      pixel_fmt, cam->v2f.fmt.pix.width,
 				      cam->v2f.fmt.pix.height,
 				      cam->v2f.fmt.pix.width, IPU_ROTATE_NONE,
-				      cam->still_buf, 0, 0, 0);
+				      cam->still_buf[0], cam->still_buf[1],
+				      0, 0);
 	if (err != 0)
 		return err;
 
 #ifdef CONFIG_MXC_IPU_V1
+	ipu_clear_irq(IPU_IRQ_SENSOR_OUT_EOF);
 	err = ipu_request_irq(IPU_IRQ_SENSOR_OUT_EOF, prp_still_callback,
 			      0, "Mxc Camera", cam);
 	if (err != 0) {
@@ -135,6 +155,7 @@ static int prp_still_start(void *private)
 	}
 	callback_flag = 0;
 	callback_eof_flag = 0;
+	ipu_clear_irq(IPU_IRQ_SENSOR_EOF);
 	err = ipu_request_irq(IPU_IRQ_SENSOR_EOF, prp_csi_eof_callback,
 			      0, "Mxc Camera", NULL);
 	if (err != 0) {
@@ -142,8 +163,9 @@ static int prp_still_start(void *private)
 		return err;
 	}
 #else
-	ipu_clear_irq(IPU_IRQ_CSI0_OUT_EOF);
-	err = ipu_request_irq(IPU_IRQ_CSI0_OUT_EOF, prp_still_callback,
+
+	ipu_clear_irq(ipu_irq_csi_out_eof);
+	err = ipu_request_irq(ipu_irq_csi_out_eof, prp_still_callback,
 			      0, "Mxc Camera", cam);
 	if (err != 0) {
 		printk(KERN_ERR "Error registering irq.\n");
@@ -151,9 +173,9 @@ static int prp_still_start(void *private)
 	}
 
 	callback_eof_flag = 0;
+	ipu_select_buffer(channel, IPU_OUTPUT_BUFFER, 0);
+	ipu_enable_channel(channel);
 
-	ipu_select_buffer(CSI_MEM, IPU_OUTPUT_BUFFER, 0);
-	ipu_enable_channel(CSI_MEM);
 	ipu_enable_csi(cam->csi);
 #endif
 
@@ -170,17 +192,28 @@ static int prp_still_stop(void *private)
 {
 	cam_data *cam = (cam_data *) private;
 	int err = 0;
+	ipu_channel_t channel;
+	int ipu_irq_csi_out_eof;
+
+	if (cam->csi == 0) {
+		channel = CSI_MEM0;
+		ipu_irq_csi_out_eof = IPU_IRQ_CSI0_OUT_EOF;
+	}
+	else {
+		channel = CSI_MEM1;
+		ipu_irq_csi_out_eof = IPU_IRQ_CSI1_OUT_EOF;
+	}
 
 #ifdef CONFIG_MXC_IPU_V1
 	ipu_free_irq(IPU_IRQ_SENSOR_EOF, NULL);
 	ipu_free_irq(IPU_IRQ_SENSOR_OUT_EOF, cam);
 #else
-	ipu_free_irq(IPU_IRQ_CSI0_OUT_EOF, cam);
+	ipu_free_irq(ipu_irq_csi_out_eof, cam);
 #endif
 
 	ipu_disable_csi(cam->csi);
-	ipu_disable_channel(CSI_MEM, true);
-	ipu_uninit_channel(CSI_MEM);
+	ipu_disable_channel(channel, true);
+	ipu_uninit_channel(channel);
 	ipu_csi_enable_mclk_if(CSI_MCLK_RAW, cam->csi, false, false);
 
 	return err;
diff --git a/drivers/media/video/mxc/capture/mt9v111.c b/drivers/media/video/mxc/capture/mt9v111.c
index c95a20683924..dfdd455db3dd 100644
--- a/drivers/media/video/mxc/capture/mt9v111.c
+++ b/drivers/media/video/mxc/capture/mt9v111.c
@@ -18,6 +18,9 @@
  *
  * @ingroup Camera
  */
+
+//#define MT9V111_DEBUG
+
 #include <linux/module.h>
 #include <linux/init.h>
 #include <linux/slab.h>
@@ -40,7 +43,6 @@ static mt9v111_conf mt9v111_device;
 /*!
  * Holds the current frame rate.
  */
-static int reset_frame_rate = MT9V111_FRAME_RATE;
 
 struct sensor {
 	const struct mt9v111_platform_data *platform_data;
@@ -49,33 +51,45 @@ struct sensor {
 	struct v4l2_pix_format pix;
 	struct v4l2_captureparm streamcap;
 	bool on;
+	bool used;
 
 	/* control settings */
 	int brightness;
-	int hue;
-	int contrast;
 	int saturation;
-	int red;
-	int green;
-	int blue;
+	int sharpness;
+	int gain;
 	int ae_mode;
 
-} mt9v111_data;
-
-extern void gpio_sensor_active(void);
-extern void gpio_sensor_inactive(void);
+};
 
 static int mt9v111_probe(struct i2c_client *client,
 			 const struct i2c_device_id *id);
 static int mt9v111_remove(struct i2c_client *client);
 
 static const struct i2c_device_id mt9v111_id[] = {
-	{"mt9v111", 0},
+	{"mt9v111_1", 2},
+	{"mt9v111_2", 3},
 	{},
 };
 
+struct sensor mt9v111_data[ARRAY_SIZE(mt9v111_id)-1];
+
 MODULE_DEVICE_TABLE(i2c, mt9v111_id);
 
+static int mt9v111_suspend(struct i2c_client *client, pm_message_t mesg)
+{
+	pr_debug("In mt9v111_suspend\n");
+
+	return 0;
+}
+
+static int mt9v111_resume(struct i2c_client *client)
+{
+	pr_debug("In mt9v111_resume\n");
+
+	return 0;
+}
+
 static struct i2c_driver mt9v111_i2c_driver = {
 	.driver = {
 		   .owner = THIS_MODULE,
@@ -84,37 +98,52 @@ static struct i2c_driver mt9v111_i2c_driver = {
 	.probe = mt9v111_probe,
 	.remove = mt9v111_remove,
 	.id_table = mt9v111_id,
-/* To add power management add .suspend and .resume functions */
+	.suspend = mt9v111_suspend,
+	.resume = mt9v111_resume,
 };
 
 /*
  * Function definitions
  */
 
-#ifdef MT9V111_DEBUG
-static inline int mt9v111_read_reg(u8 reg)
+static int mt9v111_id_from_name ( const char * name )
+{
+	int id = -1;
+
+	if( name == NULL || ( strlen(name) < strlen("mt9v111_n") ) )
+		return -1;
+
+	id = (int)simple_strtol(name+strlen("mt9v111_"),NULL,0) - 1;
+	if( id >= ARRAY_SIZE(mt9v111_id) ) {
+		printk("Invalid sensor index %d for %s\n",id,name);
+		return -1;
+	}
+
+	return id;
+}
+
+static inline int mt9v111_read_reg(int sensorid , u8 reg)
 {
-	int val = i2c_smbus_read_word_data(mt9v111_data.i2c_client, reg);
+	int val = i2c_smbus_read_word_data(mt9v111_data[sensorid].i2c_client, reg);
 	if (val != -1)
 		val = cpu_to_be16(val);
 	return val;
 }
-#endif
 
 /*!
  * Writes to the register via I2C.
  */
-static inline int mt9v111_write_reg(u8 reg, u16 val)
+static inline int mt9v111_write_reg(int sensorid , u8 reg, u16 val)
 {
-	pr_debug("In mt9v111_write_reg (0x%x, 0x%x)\n", reg, val);
+	pr_debug("[%d] In mt9v111_write_reg (0x%x, 0x%x)\n", sensorid , reg, val);
 	pr_debug("   write reg %x val %x.\n", reg, val);
 
-	return i2c_smbus_write_word_data(mt9v111_data.i2c_client,
+	return i2c_smbus_write_word_data(mt9v111_data[sensorid].i2c_client,
 					 reg, cpu_to_be16(val));
 }
 
 /*!
- * Initialize mt9v111_sensor_lib
+ * Initialize mt9v111_sensor_lib_datasheet
  * Libarary for Sensor configuration through I2C
  *
  * @param       coreReg       Core Registers
@@ -122,7 +151,7 @@ static inline int mt9v111_write_reg(u8 reg, u16 val)
  *
  * @return status
  */
-static u8 mt9v111_sensor_lib(mt9v111_coreReg * coreReg, mt9v111_IFPReg * ifpReg)
+static u8 mt9v111_sensor_lib_datasheet(int sensorid , mt9v111_coreReg * coreReg, mt9v111_IFPReg * ifpReg)
 {
 	u8 reg;
 	u16 data;
@@ -130,200 +159,103 @@ static u8 mt9v111_sensor_lib(mt9v111_coreReg * coreReg, mt9v111_IFPReg * ifpReg)
 
 	pr_debug("In mt9v111_sensor_lib\n");
 
+	/* IFP R51(0x33)=5137,R57(0x39)=290,R59(0x3B)=1068,R62(0x3E)=4095,R89(0x59)=504,R90(0x5A)=605,R92(0x5C)=8222,R93(0x5D)=10021,R100(0x64)=4477 */
+
 	/*
 	 * setup to IFP registers
 	 */
 	reg = MT9V111I_ADDR_SPACE_SEL;
 	data = ifpReg->addrSpaceSel;
-	mt9v111_write_reg(reg, data);
-
-	/* Operation Mode Control */
-	reg = MT9V111I_MODE_CONTROL;
-	data = ifpReg->modeControl;
-	mt9v111_write_reg(reg, data);
-
-	/* Output format */
-	reg = MT9V111I_FORMAT_CONTROL;
-	data = ifpReg->formatControl;	/* Set bit 12 */
-	mt9v111_write_reg(reg, data);
-
-	/* AE limit 4 */
-	reg = MT9V111I_SHUTTER_WIDTH_LIMIT_AE;
-	data = ifpReg->gainLimitAE;
-	mt9v111_write_reg(reg, data);
-
-	reg = MT9V111I_OUTPUT_FORMAT_CTRL2;
-	data = ifpReg->outputFormatCtrl2;
-	mt9v111_write_reg(reg, data);
-
-	reg = MT9V111I_AE_SPEED;
-	data = ifpReg->AESpeed;
-	mt9v111_write_reg(reg, data);
-
-	/* output image size */
-	reg = MT9V111i_H_PAN;
-	data = 0x8000 | ifpReg->HPan;
-	mt9v111_write_reg(reg, data);
-
-	reg = MT9V111i_H_ZOOM;
-	data = 0x8000 | ifpReg->HZoom;
-	mt9v111_write_reg(reg, data);
-
-	reg = MT9V111i_H_SIZE;
-	data = 0x8000 | ifpReg->HSize;
-	mt9v111_write_reg(reg, data);
-
-	reg = MT9V111i_V_PAN;
-	data = 0x8000 | ifpReg->VPan;
-	mt9v111_write_reg(reg, data);
+	mt9v111_write_reg(sensorid,reg, data);
 
-	reg = MT9V111i_V_ZOOM;
-	data = 0x8000 | ifpReg->VZoom;
-	mt9v111_write_reg(reg, data);
+	reg = MT9V111I_LIMIT_SHARP_SATU_CTRL;
+	data = ifpReg->limitSharpSatuCtrl;
+	mt9v111_write_reg(sensorid,reg, data);
 
-	reg = MT9V111i_V_SIZE;
-	data = 0x8000 | ifpReg->VSize;
-	mt9v111_write_reg(reg, data);
-
-	reg = MT9V111i_H_PAN;
-	data = ~0x8000 & ifpReg->HPan;
-	mt9v111_write_reg(reg, data);
-#if 0
 	reg = MT9V111I_UPPER_SHUTTER_DELAY_LIM;
 	data = ifpReg->upperShutterDelayLi;
-	mt9v111_write_reg(reg, data);
+	mt9v111_write_reg(sensorid,reg, data);
+
+	reg = MT9V111I_IPF_BLACK_LEVEL_SUB;
+	data = ifpReg->ipfBlackLevelSub;
+	mt9v111_write_reg(sensorid,reg, data);
+
+	reg = MT9V111I_GAIN_THRE_CCAM_ADJ;
+	data = ifpReg->agimnThreCamAdj;
+	mt9v111_write_reg(sensorid,reg, data);
 
 	reg = MT9V111I_SHUTTER_60;
 	data = ifpReg->shutter_width_60;
-	mt9v111_write_reg(reg, data);
+	mt9v111_write_reg(sensorid,reg, data);
+
+	reg = MT9V111I_AUTO_EXPOSURE_17;
+	data = ifpReg->auto_exposure_17;
+	mt9v111_write_reg(sensorid,reg, data);
 
 	reg = MT9V111I_SEARCH_FLICK_60;
 	data = ifpReg->search_flicker_60;
-	mt9v111_write_reg(reg, data);
-#endif
+	mt9v111_write_reg(sensorid,reg, data);
+
+	reg = MT9V111I_RESERVED93;
+	data = ifpReg->reserved93;
+	mt9v111_write_reg(sensorid,reg, data);
+
+	reg = MT9V111I_RESERVED100;
+	data = ifpReg->reserved100;
+	mt9v111_write_reg(sensorid,reg, data);
 
 	/*
 	 * setup to sensor core registers
 	 */
 	reg = MT9V111I_ADDR_SPACE_SEL;
 	data = coreReg->addressSelect;
-	mt9v111_write_reg(reg, data);
-
-	/* enable changes and put the Sync bit on */
-	reg = MT9V111S_OUTPUT_CTRL;
-	data = MT9V111S_OUTCTRL_SYNC | MT9V111S_OUTCTRL_CHIP_ENABLE | 0x3000;
-	mt9v111_write_reg(reg, data);
+	mt9v111_write_reg(sensorid,reg, data);
 
-	/* min PIXCLK - Default */
-	reg = MT9V111S_PIXEL_CLOCK_SPEED;
-	data = coreReg->pixelClockSpeed;
-	mt9v111_write_reg(reg, data);
+	/* Core R5=46, R7[4]=0 (DEFAULT) ,R33=58369*/
 
-	/* Setup image flipping / Dark rows / row/column skip */
-	reg = MT9V111S_READ_MODE;
-	data = coreReg->readMode;
-	mt9v111_write_reg(reg, data);
-
-	/* zoom 0 */
-	reg = MT9V111S_DIGITAL_ZOOM;
-	data = coreReg->digitalZoom;
-	mt9v111_write_reg(reg, data);
-
-	/* min H-blank */
 	reg = MT9V111S_HOR_BLANKING;
 	data = coreReg->horizontalBlanking;
-	mt9v111_write_reg(reg, data);
-
-	/* min V-blank */
-	reg = MT9V111S_VER_BLANKING;
-	data = coreReg->verticalBlanking;
-	mt9v111_write_reg(reg, data);
-
-	reg = MT9V111S_SHUTTER_WIDTH;
-	data = coreReg->shutterWidth;
-	mt9v111_write_reg(reg, data);
+	mt9v111_write_reg(sensorid,reg, data);
 
-	reg = MT9V111S_SHUTTER_DELAY;
-	data = ifpReg->upperShutterDelayLi;
-	mt9v111_write_reg(reg, data);
-
-	/* changes become effective */
-	reg = MT9V111S_OUTPUT_CTRL;
-	data = MT9V111S_OUTCTRL_CHIP_ENABLE | 0x3000;
-	mt9v111_write_reg(reg, data);
+	reg = MT9V111S_RESERVED33;
+	data = coreReg->reserved33;
+	mt9v111_write_reg(sensorid,reg, data);
 
 	return error;
 }
 
-/*!
- * MT9V111 frame rate calculate
- *
- * @param frame_rate       int *
- * @param mclk             int
- * @return  None
- */
-static void mt9v111_rate_cal(int *frame_rate, int mclk)
+void mt9v111_config_datasheet(void)
 {
-	int num_clock_per_row;
-	int max_rate = 0;
+	pr_debug("In mt9v111_config_datasheet\n");
 
-	pr_debug("In mt9v111_rate_cal\n");
+	mt9v111_device.coreReg->addressSelect = MT9V111I_SEL_SCA;
 
-	num_clock_per_row = (MT9V111_MAX_WIDTH + 114 + MT9V111_HORZBLANK_MIN)
-			* 2;
-	max_rate = mclk / (num_clock_per_row *
-			   (MT9V111_MAX_HEIGHT + MT9V111_VERTBLANK_DEFAULT));
+	/* MT9V111I_ADDR_SPACE_SEL */
+	mt9v111_device.ifpReg->addrSpaceSel = MT9V111I_SEL_IFP;
 
-	if ((*frame_rate > max_rate) || (*frame_rate == 0)) {
-		*frame_rate = max_rate;
-	}
+	/* Recommended values for 30fps @ 27MHz from datasheet*/
 
-	mt9v111_device.coreReg->verticalBlanking
-	    = mclk / (*frame_rate * num_clock_per_row) - MT9V111_MAX_HEIGHT;
+	/* Core R5=132, R6=10 , R7[4]=0 (DEFAULT) ,R33=58369*/
 
-	reset_frame_rate = *frame_rate;
-}
+	mt9v111_device.coreReg->horizontalBlanking = 132;
+	mt9v111_device.coreReg->verticalBlanking = 10;
+	mt9v111_device.coreReg->reserved33 = 58369;
 
-/*!
- * MT9V111 sensor configuration
- */
-void mt9v111_config(void)
-{
-	pr_debug("In mt9v111_config\n");
+	/* IFP R51(0x33)=5137,R57(0x39)=290,R59(0x3B)=1068,R62(0x3E)=4095,R89(0x59)=504,R90(0x5A)=605,R92(0x5C)=8222,R93(0x5D)=10021,R100(0x64)=4477 */
 
-	mt9v111_device.coreReg->addressSelect = MT9V111I_SEL_SCA;
-	mt9v111_device.ifpReg->addrSpaceSel = MT9V111I_SEL_IFP;
+	mt9v111_device.ifpReg->limitSharpSatuCtrl = 5137;
+	mt9v111_device.ifpReg->upperShutterDelayLi = 290;
+	mt9v111_device.ifpReg->ipfBlackLevelSub = 1068;
+	mt9v111_device.ifpReg->agimnThreCamAdj = 4095;
 
-	mt9v111_device.coreReg->windowHeight = MT9V111_WINHEIGHT;
-	mt9v111_device.coreReg->windowWidth = MT9V111_WINWIDTH;
-	mt9v111_device.coreReg->zoomColStart = 0;
-	mt9v111_device.coreReg->zomRowStart = 0;
-	mt9v111_device.coreReg->digitalZoom = 0x0;
-
-	mt9v111_device.coreReg->verticalBlanking = MT9V111_VERTBLANK_DEFAULT;
-	mt9v111_device.coreReg->horizontalBlanking = MT9V111_HORZBLANK_MIN;
-	mt9v111_device.coreReg->pixelClockSpeed = 0;
-	mt9v111_device.coreReg->readMode = 0xd0a1;
-
-	mt9v111_device.ifpReg->outputFormatCtrl2 = 0;
-	mt9v111_device.ifpReg->gainLimitAE = 0x300;
-	mt9v111_device.ifpReg->AESpeed = 0x80;
-
-	/* here is the default value */
-	mt9v111_device.ifpReg->formatControl = 0xc800;
-	mt9v111_device.ifpReg->modeControl = 0x708e;
-	mt9v111_device.ifpReg->awbSpeed = 0x4514;
-	mt9v111_device.coreReg->shutterWidth = 0xf8;
-
-	/* output size */
-	mt9v111_device.ifpReg->HPan = 0;
-	mt9v111_device.ifpReg->HZoom = MT9V111_MAX_WIDTH;
-	mt9v111_device.ifpReg->HSize = MT9V111_MAX_WIDTH;
-	mt9v111_device.ifpReg->VPan = 0;
-	mt9v111_device.ifpReg->VZoom = MT9V111_MAX_HEIGHT;
-	mt9v111_device.ifpReg->VSize = MT9V111_MAX_HEIGHT;
+	mt9v111_device.ifpReg->shutter_width_60 = 504;
+	mt9v111_device.ifpReg->auto_exposure_17 = 605;
+	mt9v111_device.ifpReg->search_flicker_60 = 8222;
+	mt9v111_device.ifpReg->reserved93 = 10021;
+	mt9v111_device.ifpReg->reserved100 = 4477;
 }
 
+
 /*!
  * mt9v111 sensor set saturtionn
  *
@@ -331,7 +263,7 @@ void mt9v111_config(void)
 
  * @return  Error code of 0.
  */
-static int mt9v111_set_saturation(int saturation)
+static int mt9v111_set_saturation(int sensorid , int saturation)
 {
 	u8 reg;
 	u16 data;
@@ -357,6 +289,9 @@ static int mt9v111_set_saturation(int saturation)
 	case 25:
 		mt9v111_device.ifpReg->awbSpeed = 0x6514;
 		break;
+	case 0:
+		mt9v111_device.ifpReg->awbSpeed = 0x7514;
+		break;
 	default:
 		mt9v111_device.ifpReg->awbSpeed = 0x4514;
 		break;
@@ -364,12 +299,348 @@ static int mt9v111_set_saturation(int saturation)
 
 	reg = MT9V111I_ADDR_SPACE_SEL;
 	data = mt9v111_device.ifpReg->addrSpaceSel;
-	mt9v111_write_reg(reg, data);
+	mt9v111_write_reg(sensorid,reg, data);
 
 	/* Operation Mode Control */
 	reg = MT9V111I_AWB_SPEED;
 	data = mt9v111_device.ifpReg->awbSpeed;
-	mt9v111_write_reg(reg, data);
+	mt9v111_write_reg(sensorid,reg, data);
+
+	return 0;
+}
+
+#if 0
+/*!
+ * mt9v111 sensor set digital zoom
+ *
+ * @param on/off   int
+
+ * @return  0 on success, -1 on error.
+ */
+static int mt9v111_set_digitalzoom(int sensorid , unsigned int on)
+{
+	u8 reg;
+	u16 data;
+	pr_debug("In mt9v111_set_digitalzoom(%d)\n",on);
+
+	if( on > 1 )
+	    return -1;
+
+	mt9v111_device.coreReg->digitalZoom = on;
+
+	reg = MT9V111I_ADDR_SPACE_SEL;
+	data = mt9v111_device.coreReg->addressSelect;
+	mt9v111_write_reg(sensorid,reg, data);
+
+	/* Operation Mode Control */
+	reg = MT9V111S_DIGITAL_ZOOM;
+	data = mt9v111_device.coreReg->digitalZoom;
+	mt9v111_write_reg(sensorid,reg, data);
+
+	return 0;
+}
+
+/*!
+ * mt9v111 sensor set digital pan
+ *
+ * @param pan_level   int
+
+ * @return  0 on success, -1 on error.
+ */
+static int mt9v111_set_digitalpan (int sensorid , int pan_level)
+{
+	u8 reg;
+	u16 data;
+	pr_debug("In mt9v111_set_digitalpan(%d)\n",
+		pan_level);
+
+	mt9v111_device.ifpReg->HPan = 8;
+	if (pan_level & 0xFFFF0000) {
+		pan_level = (0xFFFFFFFF - pan_level);
+		pan_level = pan_level / 0x14;
+		mt9v111_device.ifpReg->HPan =
+			mt9v111_device.ifpReg->HPan - (pan_level & 0x3FF);
+	} else {
+		pan_level = pan_level / 0x14;
+		mt9v111_device.ifpReg->HPan =
+			mt9v111_device.ifpReg->HPan + (pan_level - 1);
+	}
+
+	reg = MT9V111I_ADDR_SPACE_SEL;
+	data = mt9v111_device.ifpReg->addrSpaceSel;
+	mt9v111_write_reg(sensorid,reg, data);
+
+	/* Operation Mode Control */
+	reg = MT9V111i_H_PAN;
+	data = mt9v111_device.ifpReg->HPan;
+	mt9v111_write_reg(sensorid,reg, data);
+
+	return 0;
+}
+
+/*!
+ * mt9v111 sensor set digital tilt
+ *
+ * @param tilt_level   int
+
+ * @return  0 on success, -1 on error.
+ */
+static int mt9v111_set_digitaltilt (int sensorid , int tilt_level)
+{
+	u8 reg;
+	u16 data;
+	pr_debug("In mt9v111_set_digitaltilt(%d)\n",
+		tilt_level);
+
+	mt9v111_device.ifpReg->VPan = 8;
+        if( tilt_level & 0xFFFF0000 ) {
+				tilt_level = (0xFFFFFFFF - tilt_level);
+				tilt_level = tilt_level / 0x14;
+                mt9v111_device.ifpReg->VPan = mt9v111_device.ifpReg->VPan - (tilt_level & 0x3FF);
+        }
+        else {
+				tilt_level = tilt_level / 0x14;
+                mt9v111_device.ifpReg->VPan = mt9v111_device.ifpReg->VPan + (tilt_level - 1);
+        }
+
+	reg = MT9V111I_ADDR_SPACE_SEL;
+	data = mt9v111_device.ifpReg->addrSpaceSel;
+	mt9v111_write_reg(sensorid,reg, data);
+
+	/* Operation Mode Control */
+	reg = MT9V111i_V_PAN;
+	data = mt9v111_device.ifpReg->VPan;
+	mt9v111_write_reg(sensorid,reg, data);
+
+	return 0;
+}
+
+/*!
+ * mt9v111 sensor set output resolution
+ *
+ * @param resolution   res
+
+ * @return  0 on success, -1 on error.
+ */
+static int mt9v111_set_outputresolution(int sensorid , MT9V111_OutputResolution res)
+{
+	u8 reg;
+	u16 data;
+	int zoom = 0;
+
+	pr_debug("In mt9v111_set_outputresolution(%d)\n",res);
+
+	switch (res) {
+	    case MT9V111_OutputResolution_VGA:
+			/* 640x480 */
+			mt9v111_device.ifpReg->HSize = 0x0280;
+			mt9v111_device.ifpReg->VSize = 0x01E0;
+			break;
+
+	    case MT9V111_OutputResolution_QVGA:
+			/* 320x240 */
+			mt9v111_device.ifpReg->HSize = 0x0140;
+			mt9v111_device.ifpReg->VSize = 0x00F0;
+			break;
+
+	    case MT9V111_OutputResolution_CIF:
+			/* 352x288 */
+			mt9v111_device.ifpReg->HSize = 0x0160;
+			mt9v111_device.ifpReg->VSize = 0x0120;
+			mt9v111_device.ifpReg->HZoom = 0x0160;
+			mt9v111_device.ifpReg->VZoom = 0x0120;
+			zoom = 1;
+			break;
+
+	    case MT9V111_OutputResolution_QCIF:
+			/* 176X220 */
+			mt9v111_device.ifpReg->HSize = 0x00B0;
+			mt9v111_device.ifpReg->VSize = 0x0090;
+			mt9v111_device.ifpReg->HZoom = 0x00B0;
+			mt9v111_device.ifpReg->VZoom = 0x0090;
+			zoom = 1;
+			break;
+
+	    case MT9V111_OutputResolution_QQVGA:
+			/* 2048*1536 */
+			mt9v111_device.ifpReg->HSize = 0x00A0;
+			mt9v111_device.ifpReg->VSize = 0x0078;
+			mt9v111_device.ifpReg->HZoom = 0x00A0;
+			mt9v111_device.ifpReg->VZoom = 0x0078;
+			zoom = 1;
+			break;
+
+	    case MT9V111_OutputResolution_SXGA:
+		/* 1280x1024 */
+			break;
+
+		default:
+			break;
+	}
+
+	reg = MT9V111I_ADDR_SPACE_SEL;
+	data = mt9v111_device.ifpReg->addrSpaceSel;
+	mt9v111_write_reg(sensorid,reg, data);
+
+	reg = MT9V111i_V_SIZE;
+	data = mt9v111_device.ifpReg->VSize;
+	mt9v111_write_reg(sensorid,reg, data);
+
+	reg = MT9V111i_H_SIZE;
+	data = mt9v111_device.ifpReg->HSize;
+	mt9v111_write_reg(sensorid,reg, data);
+
+	if ( zoom ) {
+	    reg = MT9V111i_V_ZOOM;
+	    data = mt9v111_device.ifpReg->VZoom;
+	    mt9v111_write_reg(sensorid,reg, data);
+
+	    reg = MT9V111i_H_ZOOM;
+	    data = mt9v111_device.ifpReg->HZoom;
+	    mt9v111_write_reg(sensorid,reg, data);
+	}
+
+	return 0;
+}
+
+/*!
+ * mt9v111 sensor set digital flash
+ *
+ * @param flash_level   int
+
+ * @return  0 on success, -1 on error.
+ */
+static int mt9v111_set_digitalflash (int sensorid , int flash_level)
+{
+	u8 reg;
+	u16 data = mt9v111_read_reg(sensorid,MT9V111i_FLASH_CTRL);
+	pr_debug("In mt9v111_set_digitalflash(%d)\n",
+			flash_level);
+
+	if(flash_level) {
+	    data &= (0xFF00);
+	    data |= ((flash_level & 0x00FF) | (1<<13));
+	}
+	else {
+	    data &= ~(1<<13);
+	}
+
+	reg = MT9V111I_ADDR_SPACE_SEL;
+	data = mt9v111_device.ifpReg->addrSpaceSel;
+	mt9v111_write_reg(sensorid,reg, data);
+
+	/* Operation Mode Control */
+	reg = MT9V111i_FLASH_CTRL;
+	mt9v111_device.ifpReg->flashCtrl = data;
+	mt9v111_write_reg(sensorid,reg, data);
+
+	return 0;
+}
+
+/*!
+ * mt9v111 sensor set digital monochrome
+ *
+ * @param on   int
+
+ * @return  0 on success, -1 on error.
+ */
+static int mt9v111_set_digitalmonochrome (int sensorid , int on)
+{
+	u8 reg;
+	u16 data = mt9v111_read_reg(sensorid,MT9V111I_FORMAT_CONTROL);
+	pr_debug("In mt9v111_set_digitalmonochrome(%d)\n",
+			on);
+
+        /* clear the monochrome bit field */
+        data &= ~(1<<5);
+
+         /* enable or disable monochrome mode */
+         if( on )
+                 data |= (0<<5);
+         else
+                 data |= (1<<5);
+
+	reg = MT9V111I_ADDR_SPACE_SEL;
+	data = mt9v111_device.ifpReg->addrSpaceSel;
+	mt9v111_write_reg(sensorid,reg, data);
+
+	/* Operation Mode Control */
+	reg = MT9V111I_FORMAT_CONTROL;
+	mt9v111_device.ifpReg->formatControl = data;
+	mt9v111_write_reg(sensorid,reg, data);
+
+	return 0;
+}
+#endif
+
+/*!
+ * mt9v111 sensor set digital sharpness
+ *
+ * @param value   int
+
+ * @return  0 on success, -1 on error.
+ */
+static int mt9v111_set_digitalsharpness (int sensorid , int value)
+{
+	u8 reg;
+	u16 data ;
+
+	pr_debug("In mt9v111_set_digitalsharpness(%d)\n",value);
+
+	reg = MT9V111I_ADDR_SPACE_SEL;
+	data = mt9v111_device.ifpReg->addrSpaceSel;
+	mt9v111_write_reg(sensorid,reg, data);
+
+	data = mt9v111_read_reg(sensorid,MT9V111I_APERTURE_GAIN);
+
+	/* erase current and remove auto reduce sharpness in low light */
+	 data &= ~(0x000F);
+	 data |= (value & (0x000F));
+	 if( data > (0x000F) )
+			 return -1;
+
+	/* Operation Mode Control */
+	reg = MT9V111I_APERTURE_GAIN;
+	mt9v111_device.ifpReg->apertureGain = data;
+	mt9v111_write_reg(sensorid,reg, data);
+
+	return 0;
+}
+
+/*!
+ * mt9v111 sensor set digital brightness
+ *
+ * @param value   int
+
+ * @return  0 on success, -1 on error.
+ */
+static int mt9v111_set_digitalbrightness (int sensorid , int value)
+{
+	u8 reg;
+	u16 data;
+	u32 max_brightness, min_brightness;
+
+	data = mt9v111_read_reg(sensorid,MT9V111I_CLIP_LIMIT_OUTPUT_LUMI);
+	max_brightness = data >> 8;
+	min_brightness = (u8)data;
+
+	if( value > max_brightness )
+		value = max_brightness;
+	else if( value < min_brightness )
+		value = min_brightness;
+
+	reg = MT9V111I_ADDR_SPACE_SEL;
+	data = mt9v111_device.ifpReg->addrSpaceSel;
+	mt9v111_write_reg(sensorid,reg, data);
+
+	data = mt9v111_read_reg(sensorid,MT9V111I_AE_PRECISION_TARGET);
+	data &= 0xFF00; /* Clear target luminance */
+	data |= ((u8)value );
+
+	/* Operation Mode Control */
+	reg = MT9V111I_AE_PRECISION_TARGET;
+	mt9v111_device.ifpReg->AEPrecisionTarget = data;
+	mt9v111_write_reg(sensorid,reg, data);
 
 	return 0;
 }
@@ -380,7 +651,7 @@ static int mt9v111_set_saturation(int saturation)
  * @param ae_mode      int
  * @return  Error code of 0 (no Error)
  */
-static int mt9v111_set_ae_mode(int ae_mode)
+static int mt9v111_set_ae_mode(int sensorid , int ae_mode)
 {
 	u8 reg;
 	u16 data;
@@ -408,19 +679,20 @@ static int mt9v111_set_ae_mode(int ae_mode)
 	/* V4L2_EXPOSURE_MANUAL = 1 needs register setting of 0x308E */
 	mt9v111_device.ifpReg->modeControl &= 0x3fff;
 	mt9v111_device.ifpReg->modeControl |= (ae_mode & 0x03) << 14;
-	mt9v111_data.ae_mode = ae_mode;
+	mt9v111_data[sensorid].ae_mode = ae_mode;
 
 	reg = MT9V111I_ADDR_SPACE_SEL;
 	data = mt9v111_device.ifpReg->addrSpaceSel;
-	mt9v111_write_reg(reg, data);
+	mt9v111_write_reg(sensorid,reg, data);
 
 	reg = MT9V111I_MODE_CONTROL;
 	data = mt9v111_device.ifpReg->modeControl;
-	mt9v111_write_reg(reg, data);
+	mt9v111_write_reg(sensorid,reg, data);
 
 	return 0;
 }
 
+#if 0
 /*!
  * mt9v111 sensor get AE measurement window mode configuration
  *
@@ -435,6 +707,7 @@ static void mt9v111_get_ae_mode(int *ae_mode)
 		*ae_mode = (mt9v111_device.ifpReg->modeControl & 0xc) >> 2;
 	}
 }
+#endif
 
 #ifdef MT9V111_DEBUG
 /*!
@@ -442,33 +715,33 @@ static void mt9v111_get_ae_mode(int *ae_mode)
  *
  * @return none
  */
-static void mt9v111_test_pattern(bool flag)
+static void mt9v111_test_pattern(int sensorid , bool flag)
 {
 	u16 data;
 
 	/* switch to sensor registers */
-	mt9v111_write_reg(MT9V111I_ADDR_SPACE_SEL, MT9V111I_SEL_SCA);
+	mt9v111_write_reg(sensorid,MT9V111I_ADDR_SPACE_SEL, MT9V111I_SEL_SCA);
 
 	if (flag == true) {
 		testpattern = MT9V111S_OUTCTRL_TEST_MODE;
 
-		data = mt9v111_read_reg(MT9V111S_ROW_NOISE_CTRL) & 0xBF;
-		mt9v111_write_reg(MT9V111S_ROW_NOISE_CTRL, data);
+		data = mt9v111_read_reg(sensorid,MT9V111S_ROW_NOISE_CTRL) & 0xBF;
+		mt9v111_write_reg(sensorid,MT9V111S_ROW_NOISE_CTRL, data);
 
-		mt9v111_write_reg(MT9V111S_TEST_DATA, 0);
+		mt9v111_write_reg(sensorid,MT9V111S_TEST_DATA, 0);
 
 		/* changes take effect */
 		data = MT9V111S_OUTCTRL_CHIP_ENABLE | testpattern | 0x3000;
-		mt9v111_write_reg(MT9V111S_OUTPUT_CTRL, data);
+		mt9v111_write_reg(sensorid,MT9V111S_OUTPUT_CTRL, data);
 	} else {
 		testpattern = 0;
 
-		data = mt9v111_read_reg(MT9V111S_ROW_NOISE_CTRL) | 0x40;
+		data = mt9v111_read_reg(sensorid,MT9V111S_ROW_NOISE_CTRL) | 0x40;
 		mt9v111_write_reg(MT9V111S_ROW_NOISE_CTRL, data);
 
 		/* changes take effect */
 		data = MT9V111S_OUTCTRL_CHIP_ENABLE | testpattern | 0x3000;
-		mt9v111_write_reg(MT9V111S_OUTPUT_CTRL, data);
+		mt9v111_write_reg(sensorid,MT9V111S_OUTPUT_CTRL, data);
 	}
 }
 #endif
@@ -507,6 +780,7 @@ static int ioctl_g_ifparm(struct v4l2_int_device *s, struct v4l2_ifparm *p)
 	p->u.bt656.clock_curr = MT9V111_MCLK;
 	p->if_type = V4L2_IF_TYPE_BT656;
 	p->u.bt656.mode = V4L2_IF_TYPE_BT656_MODE_NOBT_8BIT;
+	p->u.bt656.bt_sync_correct = 1;	// translates to CSI ext vsync
 	p->u.bt656.clock_min = MT9V111_CLK_MIN;
 	p->u.bt656.clock_max = MT9V111_CLK_MAX;
 
@@ -534,10 +808,12 @@ static int ioctl_s_power(struct v4l2_int_device *s, int on)
 
 	sensor->on = on;
 
-	if (on)
-		gpio_sensor_active();
-	else
-		gpio_sensor_inactive();
+	if(on) {
+		ipu_csi_enable_mclk_if(CSI_MCLK_I2C, 0 /* cam->csi */, true, true);
+	}
+	else {
+		ipu_csi_enable_mclk_if(CSI_MCLK_I2C, 0 /* cam->csi */, false, false);
+	}
 
 	return 0;
 }
@@ -554,19 +830,23 @@ static int ioctl_g_parm(struct v4l2_int_device *s, struct v4l2_streamparm *a)
 	int ret = 0;
 	struct v4l2_captureparm *cparm = &a->parm.capture;
 	/* s->priv points to mt9v111_data */
+	int sensorid = mt9v111_id_from_name(((struct sensor *)s->priv)->v4l2_int_device->name);
 
 	pr_debug("In mt9v111:ioctl_g_parm\n");
 
+	if( sensorid < 0 )
+		return ret;
+
 	switch (a->type) {
 	/* This is the only case currently handled. */
 	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
 		pr_debug("   type is V4L2_BUF_TYPE_VIDEO_CAPTURE\n");
 		memset(a, 0, sizeof(*a));
 		a->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-		cparm->capability = mt9v111_data.streamcap.capability;
+		cparm->capability = mt9v111_data[sensorid].streamcap.capability;
 		cparm->timeperframe =
-				mt9v111_data.streamcap.timeperframe;
-		cparm->capturemode = mt9v111_data.streamcap.capturemode;
+				mt9v111_data[sensorid].streamcap.timeperframe;
+		cparm->capturemode = mt9v111_data[sensorid].streamcap.capturemode;
 		ret = 0;
 		break;
 
@@ -605,9 +885,13 @@ static int ioctl_s_parm(struct v4l2_int_device *s, struct v4l2_streamparm *a)
 	int ret = 0;
 	struct v4l2_captureparm *cparm = &a->parm.capture;
 	/* s->priv points to mt9v111_data */
+	int sensorid = mt9v111_id_from_name(((struct sensor *)s->priv)->v4l2_int_device->name);
 
 	pr_debug("In mt9v111:ioctl_s_parm\n");
 
+	if( sensorid < 0 )
+		return ret;
+
 	switch (a->type) {
 	/* This is the only case currently handled. */
 	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
@@ -617,13 +901,13 @@ static int ioctl_s_parm(struct v4l2_int_device *s, struct v4l2_streamparm *a)
 		 * Changing the frame rate is not allowed on this
 		 *camera. */
 		if (cparm->timeperframe.denominator !=
-		    mt9v111_data.streamcap.timeperframe.denominator) {
+		    mt9v111_data[sensorid].streamcap.timeperframe.denominator) {
 			pr_err("ERROR: mt9v111: ioctl_s_parm: " \
 			       "This camera does not allow frame rate "
 			       "changes.\n");
 			ret = -EINVAL;
 		} else {
-			mt9v111_data.streamcap.timeperframe =
+			mt9v111_data[sensorid].streamcap.timeperframe =
 						cparm->timeperframe;
 		      /* Call any camera functions to match settings. */
 		}
@@ -635,7 +919,7 @@ static int ioctl_s_parm(struct v4l2_int_device *s, struct v4l2_streamparm *a)
 				"unsupported capture mode\n");
 			ret  = -EINVAL;
 		} else {
-			mt9v111_data.streamcap.capturemode =
+			mt9v111_data[sensorid].streamcap.capturemode =
 						cparm->capturemode;
 		      /* Call any camera functions to match settings. */
 		      /* Right now this camera only supports 1 mode. */
@@ -685,6 +969,7 @@ static int ioctl_g_fmt_cap(struct v4l2_int_device *s, struct v4l2_format *f)
 	return 0;
 }
 
+#if 0
 /*!
  * ioctl_queryctrl - V4L2 sensor interface handler for VIDIOC_QUERYCTRL ioctl
  * @s: pointer to standard V4L2 device structure
@@ -700,6 +985,7 @@ static int ioctl_queryctrl(struct v4l2_int_device *s, struct v4l2_queryctrl *qc)
 
 	return 0;
 }
+#endif
 
 /*!
  * ioctl_g_ctrl - V4L2 sensor interface handler for VIDIOC_G_CTRL ioctl
@@ -712,66 +998,29 @@ static int ioctl_queryctrl(struct v4l2_int_device *s, struct v4l2_queryctrl *qc)
  */
 static int ioctl_g_ctrl(struct v4l2_int_device *s, struct v4l2_control *vc)
 {
+	int sensorid = mt9v111_id_from_name(((struct sensor *)s->priv)->v4l2_int_device->name);
+
 	pr_debug("In mt9v111:ioctl_g_ctrl\n");
 
+	if( sensorid < 0 )
+		return 0;
+
 	switch (vc->id) {
 	case V4L2_CID_BRIGHTNESS:
 		pr_debug("   V4L2_CID_BRIGHTNESS\n");
-		vc->value = mt9v111_data.brightness;
-		break;
-	case V4L2_CID_CONTRAST:
-		pr_debug("   V4L2_CID_CONTRAST\n");
-		vc->value = mt9v111_data.contrast;
+		vc->value = mt9v111_data[sensorid].brightness;
 		break;
 	case V4L2_CID_SATURATION:
 		pr_debug("   V4L2_CID_SATURATION\n");
-		vc->value = mt9v111_data.saturation;
-		break;
-	case V4L2_CID_HUE:
-		pr_debug("   V4L2_CID_HUE\n");
-		vc->value = mt9v111_data.hue;
-		break;
-	case V4L2_CID_AUTO_WHITE_BALANCE:
-		pr_debug(
-			"   V4L2_CID_AUTO_WHITE_BALANCE\n");
-		vc->value = 0;
-		break;
-	case V4L2_CID_DO_WHITE_BALANCE:
-		pr_debug(
-			"   V4L2_CID_DO_WHITE_BALANCE\n");
-		vc->value = 0;
-		break;
-	case V4L2_CID_RED_BALANCE:
-		pr_debug("   V4L2_CID_RED_BALANCE\n");
-		vc->value = mt9v111_data.red;
-		break;
-	case V4L2_CID_BLUE_BALANCE:
-		pr_debug("   V4L2_CID_BLUE_BALANCE\n");
-		vc->value = mt9v111_data.blue;
-		break;
-	case V4L2_CID_GAMMA:
-		pr_debug("   V4L2_CID_GAMMA\n");
-		vc->value = 0;
+		vc->value = mt9v111_data[sensorid].saturation;
 		break;
 	case V4L2_CID_EXPOSURE:
 		pr_debug("   V4L2_CID_EXPOSURE\n");
-		vc->value = mt9v111_data.ae_mode;
-		break;
-	case V4L2_CID_AUTOGAIN:
-		pr_debug("   V4L2_CID_AUTOGAIN\n");
-		vc->value = 0;
+		vc->value = mt9v111_data[sensorid].ae_mode;
 		break;
 	case V4L2_CID_GAIN:
 		pr_debug("   V4L2_CID_GAIN\n");
-		vc->value = 0;
-		break;
-	case V4L2_CID_HFLIP:
-		pr_debug("   V4L2_CID_HFLIP\n");
-		vc->value = 0;
-		break;
-	case V4L2_CID_VFLIP:
-		pr_debug("   V4L2_CID_VFLIP\n");
-		vc->value = 0;
+		vc->value = mt9v111_data[sensorid].gain;
 		break;
 	default:
 		pr_debug("   Default case\n");
@@ -794,56 +1043,34 @@ static int ioctl_g_ctrl(struct v4l2_int_device *s, struct v4l2_control *vc)
 static int ioctl_s_ctrl(struct v4l2_int_device *s, struct v4l2_control *vc)
 {
 	int retval = 0;
+	int sensorid = mt9v111_id_from_name(((struct sensor *)s->priv)->v4l2_int_device->name);
 
 	pr_debug("In mt9v111:ioctl_s_ctrl %d\n",
 		vc->id);
 
+	if( sensorid < 0 )
+		return retval;
+
 	switch (vc->id) {
 	case V4L2_CID_BRIGHTNESS:
 		pr_debug("   V4L2_CID_BRIGHTNESS\n");
-		break;
-	case V4L2_CID_CONTRAST:
-		pr_debug("   V4L2_CID_CONTRAST\n");
+		mt9v111_set_digitalbrightness(sensorid,vc->value);
+		mt9v111_data[sensorid].brightness = vc->value;
 		break;
 	case V4L2_CID_SATURATION:
 		pr_debug("   V4L2_CID_SATURATION\n");
-		retval = mt9v111_set_saturation(vc->value);
-		break;
-	case V4L2_CID_HUE:
-		pr_debug("   V4L2_CID_HUE\n");
-		break;
-	case V4L2_CID_AUTO_WHITE_BALANCE:
-		pr_debug(
-			"   V4L2_CID_AUTO_WHITE_BALANCE\n");
-		break;
-	case V4L2_CID_DO_WHITE_BALANCE:
-		pr_debug(
-			"   V4L2_CID_DO_WHITE_BALANCE\n");
-		break;
-	case V4L2_CID_RED_BALANCE:
-		pr_debug("   V4L2_CID_RED_BALANCE\n");
-		break;
-	case V4L2_CID_BLUE_BALANCE:
-		pr_debug("   V4L2_CID_BLUE_BALANCE\n");
-		break;
-	case V4L2_CID_GAMMA:
-		pr_debug("   V4L2_CID_GAMMA\n");
+		retval = mt9v111_set_saturation(sensorid,vc->value);
+		mt9v111_data[sensorid].saturation = vc->value;
 		break;
 	case V4L2_CID_EXPOSURE:
 		pr_debug("   V4L2_CID_EXPOSURE\n");
-		retval = mt9v111_set_ae_mode(vc->value);
-		break;
-	case V4L2_CID_AUTOGAIN:
-		pr_debug("   V4L2_CID_AUTOGAIN\n");
+		retval = mt9v111_set_ae_mode(sensorid,vc->value);
+		mt9v111_data[sensorid].ae_mode = vc->value;
 		break;
 	case V4L2_CID_GAIN:
 		pr_debug("   V4L2_CID_GAIN\n");
-		break;
-	case V4L2_CID_HFLIP:
-		pr_debug("   V4L2_CID_HFLIP\n");
-		break;
-	case V4L2_CID_VFLIP:
-		pr_debug("   V4L2_CID_VFLIP\n");
+		mt9v111_set_digitalsharpness(sensorid,vc->value);
+		mt9v111_data[sensorid].gain = vc->value;
 		break;
 	default:
 		pr_debug("   Default case\n");
@@ -854,13 +1081,41 @@ static int ioctl_s_ctrl(struct v4l2_int_device *s, struct v4l2_control *vc)
 	return retval;
 }
 
+static void mt9v111_ifp_reset ( int sensorid )
+{
+	mt9v111_write_reg(sensorid,MT9V111S_ADDR_SPACE_SEL, 0x0001);
+	mt9v111_write_reg(sensorid,MT9V111I_SOFT_RESET, 0x0001);
+	msleep(100);
+	mt9v111_write_reg(sensorid,MT9V111I_SOFT_RESET, 0x0000);
+	msleep(100);
+}
+
+static void mt9v111_sensor_reset ( int sensorid )
+{
+	mt9v111_write_reg(sensorid,MT9V111S_ADDR_SPACE_SEL, 0x0004);
+	mt9v111_write_reg(sensorid,MT9V111S_RESET, 0x0001);
+	msleep(100);
+	mt9v111_write_reg(sensorid,MT9V111S_RESET, 0x0000);
+	msleep(100);
+}
+
 /*!
  * ioctl_init - V4L2 sensor interface handler for VIDIOC_INT_INIT
  * @s: pointer to standard V4L2 device structure
  */
 static int ioctl_init(struct v4l2_int_device *s)
 {
-	pr_debug("In mt9v111:ioctl_init\n");
+	int sensorid = 0;
+
+	sensorid = mt9v111_id_from_name(((struct sensor *)s->priv)->v4l2_int_device->name);
+	if( sensorid < 0 )
+		return 0;
+
+	pr_debug("In mt9v111:ioctl_init for sensor %d\n",sensorid);
+
+	mt9v111_sensor_reset(sensorid);
+	mt9v111_ifp_reset(sensorid);
+	mt9v111_sensor_lib_datasheet(sensorid,mt9v111_device.coreReg, mt9v111_device.ifpReg);
 
 	return 0;
 }
@@ -873,19 +1128,146 @@ static int ioctl_init(struct v4l2_int_device *s)
  */
 static int ioctl_dev_init(struct v4l2_int_device *s)
 {
+	int sensorid = 0;
 	uint32_t clock_rate = MT9V111_MCLK;
 
+	sensorid = mt9v111_id_from_name(((struct sensor *)s->priv)->v4l2_int_device->name);
+	if( sensorid < 0 )
+		return 0;
+
 	pr_debug("In mt9v111:ioctl_dev_init\n");
 
-	gpio_sensor_active();
+	set_mclk_rate(&clock_rate, 0);	// Both sensors use mclk0 on Digi ccwmx51
+
+	mt9v111_sensor_reset(sensorid);
+	mt9v111_ifp_reset(sensorid);
+	mt9v111_sensor_lib_datasheet(sensorid,mt9v111_device.coreReg, mt9v111_device.ifpReg);
 
-	set_mclk_rate(&clock_rate);
-	mt9v111_rate_cal(&reset_frame_rate, clock_rate);
-	mt9v111_sensor_lib(mt9v111_device.coreReg, mt9v111_device.ifpReg);
+	return 0;
+}
+
+/* list of image formats supported by sensor */
+static const struct v4l2_fmtdesc mt9v111_formats[] = {
+	{
+		.description = "RGB565",
+		.pixelformat = V4L2_PIX_FMT_RGB565,
+	},
+	{
+		.description = "YUV422 UYVY",
+		.pixelformat = V4L2_PIX_FMT_UYVY,
+	},
+};
+
+#define MT9V111_NUM_CAPTURE_FORMATS	ARRAY_SIZE(mt9v111_formats)
+
+static int ioctl_enum_fmt_cap(struct v4l2_int_device *s,
+				   struct v4l2_fmtdesc *fmt)
+{
+	int index = fmt->index;
+
+	switch (fmt->type) {
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+		if (index >= MT9V111_NUM_CAPTURE_FORMATS)
+			return -EINVAL;
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	fmt->flags = mt9v111_formats[index].flags;
+	strlcpy(fmt->description, mt9v111_formats[index].description,
+		sizeof(fmt->description));
+	fmt->pixelformat = mt9v111_formats[index].pixelformat;
 
 	return 0;
 }
 
+static int ioctl_s_fmt_cap(struct v4l2_int_device *s,
+			struct v4l2_format *f)
+{
+	unsigned short reg;
+	int sensorid = mt9v111_id_from_name(((struct sensor *)s->priv)->v4l2_int_device->name);
+	struct sensor *sensor = s->priv;
+	/* s->priv points to mt9v111_data */
+
+	if( sensorid < 0 )
+		return -ENODEV;
+
+	/* Select IFP registers */
+	mt9v111_write_reg (sensorid,MT9V111S_ADDR_SPACE_SEL, 0x0001);
+
+	switch (f->fmt.pix.pixelformat) {
+		case V4L2_PIX_FMT_RGB565:
+			/*MT9V111I_OUTPUT_FORMAT_CTRL2*/
+			reg = mt9v111_read_reg (sensorid,MT9V111I_OUTPUT_FORMAT_CTRL2);
+			reg &= ~(0x3 << 6);
+			mt9v111_write_reg (sensorid,MT9V111I_OUTPUT_FORMAT_CTRL2, reg);
+
+			/* MT9V111I_FORMAT_CONTROL */
+			reg = mt9v111_read_reg(sensorid,MT9V111I_FORMAT_CONTROL);
+			reg |= 1 << 12;
+			mt9v111_write_reg(sensorid,MT9V111I_FORMAT_CONTROL, reg);
+			break;
+
+		case V4L2_PIX_FMT_YUV444:
+		case V4L2_PIX_FMT_UYVY:
+		case V4L2_PIX_FMT_YVU420:
+		case V4L2_PIX_FMT_YUYV:
+			/* MT9V111I_FORMAT_CONTROL */
+			reg = mt9v111_read_reg(sensorid,MT9V111I_FORMAT_CONTROL);
+			reg &= ~(1 << 12);
+			mt9v111_write_reg(sensorid,MT9V111I_FORMAT_CONTROL, reg);
+			break;
+
+		default:
+			return -EINVAL;
+	}
+
+	sensor->pix.width = f->fmt.pix.width;
+	sensor->pix.height = f->fmt.pix.height;
+	sensor->pix.sizeimage = f->fmt.pix.sizeimage;
+	sensor->pix.pixelformat = f->fmt.pix.pixelformat;
+	return 0;
+}
+
+static int ioctl_try_fmt_cap(struct v4l2_int_device *s, struct v4l2_format *f)
+{
+	int i;
+
+	if (f->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	for( i=0 ; i < MT9V111_NUM_CAPTURE_FORMATS ; i++) {
+		if( f->fmt.pix.pixelformat == mt9v111_formats[i].pixelformat )
+			return 0;
+	}
+
+	return -EINVAL;
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int ioctl_get_register(struct v4l2_int_device *s,struct v4l2_dbg_register * dreg)
+{
+	int sensorid = mt9v111_id_from_name(((struct sensor *)s->priv)->v4l2_int_device->name);
+
+	ipu_csi_enable_mclk_if(CSI_MCLK_I2C, 0 /* cam->csi */ , true, true);
+	dreg->val = mt9v111_read_reg (sensorid,dreg->reg);
+	ipu_csi_enable_mclk_if(CSI_MCLK_I2C, 0 /* cam->csi */ , false, false);
+	return 0;
+}
+
+static int ioctl_set_register(struct v4l2_int_device *s,struct v4l2_dbg_register * dreg)
+{
+	int sensorid = mt9v111_id_from_name(((struct sensor *)s->priv)->v4l2_int_device->name);
+
+	ipu_csi_enable_mclk_if(CSI_MCLK_I2C, 0 /* cam->csi */ , true, true);
+	mt9v111_write_reg (sensorid,dreg->reg, dreg->val);
+	ipu_csi_enable_mclk_if(CSI_MCLK_I2C, 0 /* cam->csi */ , false, false);
+	return 0;
+}
+#endif
+
 /*!
  * This structure defines all the ioctls for this module and links them to the
  * enumeration.
@@ -910,16 +1292,23 @@ static struct v4l2_int_ioctl_desc mt9v111_ioctl_desc[] = {
 	/*!
 	 * VIDIOC_ENUM_FMT ioctl for the CAPTURE buffer type.
 	 */
-/*	{vidioc_int_enum_fmt_cap_num,
-				(v4l2_int_ioctl_func *) ioctl_enum_fmt_cap}, */
+	{vidioc_int_enum_fmt_cap_num,
+				(v4l2_int_ioctl_func *) ioctl_enum_fmt_cap},
+
 
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	{vidioc_int_g_register_num,
+				(v4l2_int_ioctl_func *) ioctl_get_register},
+	{vidioc_int_s_register_num,
+				(v4l2_int_ioctl_func *) ioctl_set_register},
+#endif
 	/*!
 	 * VIDIOC_TRY_FMT ioctl for the CAPTURE buffer type.
 	 * This ioctl is used to negotiate the image capture size and
 	 * pixel format without actually making it take effect.
 	 */
-/*	{vidioc_int_try_fmt_cap_num,
-				(v4l2_int_ioctl_func *) ioctl_try_fmt_cap}, */
+	{vidioc_int_try_fmt_cap_num,
+				(v4l2_int_ioctl_func *) ioctl_try_fmt_cap},
 
 	{vidioc_int_g_fmt_cap_num, (v4l2_int_ioctl_func *) ioctl_g_fmt_cap},
 
@@ -928,7 +1317,7 @@ static struct v4l2_int_ioctl_desc mt9v111_ioctl_desc[] = {
 	 * format, returns error code if format not supported or HW can't be
 	 * correctly configured.
 	 */
-/*	{vidioc_int_s_fmt_cap_num, (v4l2_int_ioctl_func *)ioctl_s_fmt_cap}, */
+	{vidioc_int_s_fmt_cap_num, (v4l2_int_ioctl_func *)ioctl_s_fmt_cap},
 
 	{vidioc_int_g_parm_num, (v4l2_int_ioctl_func *) ioctl_g_parm},
 	{vidioc_int_s_parm_num, (v4l2_int_ioctl_func *) ioctl_s_parm},
@@ -937,20 +1326,56 @@ static struct v4l2_int_ioctl_desc mt9v111_ioctl_desc[] = {
 	{vidioc_int_s_ctrl_num, (v4l2_int_ioctl_func *) ioctl_s_ctrl},
 };
 
-static struct v4l2_int_slave mt9v111_slave = {
-	.ioctls = mt9v111_ioctl_desc,
-	.num_ioctls = ARRAY_SIZE(mt9v111_ioctl_desc),
+static struct v4l2_int_slave mt9v111_slave[] = {
+	{
+		.ioctls = mt9v111_ioctl_desc,
+		.num_ioctls = ARRAY_SIZE(mt9v111_ioctl_desc),
+		.attach_to = "mxc_v4l2_cap_1",
+	},
+	{
+		.ioctls = mt9v111_ioctl_desc,
+		.num_ioctls = ARRAY_SIZE(mt9v111_ioctl_desc),
+		.attach_to = "mxc_v4l2_cap_2",
+	},
 };
 
-static struct v4l2_int_device mt9v111_int_device = {
-	.module = THIS_MODULE,
-	.name = "mt9v111",
-	.type = v4l2_int_type_slave,
-	.u = {
-		.slave = &mt9v111_slave,
+static struct v4l2_int_device mt9v111_int_device [] = {
+		{
+			.module = THIS_MODULE,
+			.type = v4l2_int_type_slave,
+			.u = {
+				.slave = &mt9v111_slave[0],
+				},
+		},
+		{
+			.module = THIS_MODULE,
+			.type = v4l2_int_type_slave,
+			.u = {
+				.slave = &mt9v111_slave[1],
+				},
 		},
 };
 
+static int mt9v111_read_id( int sensoridx )
+{
+	int sensorid = 0;
+	int ret = 0;
+
+	mt9v111_write_reg (sensoridx,MT9V111S_ADDR_SPACE_SEL, 0x0004);
+
+	sensorid = mt9v111_read_reg (sensoridx,MT9V111S_CHIP_VERSION);
+	if( sensorid == 0x823a )
+	{
+		printk(KERN_INFO" MT9V111 ID %x\n",sensorid);
+	}
+	else
+	{
+		printk(KERN_ERR" MT9V111 Could not detect sensor (read %x)\n",sensorid);
+		ret = -ENODEV;
+	}
+	return ret;
+}
+
 /*!
  * mt9v111 I2C probe function
  * Function set in i2c_driver struct.
@@ -962,32 +1387,56 @@ static int mt9v111_probe(struct i2c_client *client,
 			 const struct i2c_device_id *id)
 {
 	int retval;
+	int sensorid;
 
 	pr_debug("In mt9v111_probe  device id is %s\n", id->name);
 
+	sensorid = mt9v111_id_from_name(id->name);
+
+	if( sensorid < 0 )
+		return -ENODEV;
+
 	/* Set initial values for the sensor struct. */
-	memset(&mt9v111_data, 0, sizeof(mt9v111_data));
-	mt9v111_data.i2c_client = client;
+	memset(&mt9v111_data[sensorid], 0, sizeof(struct sensor));
+	mt9v111_data[sensorid].i2c_client = client;
 	pr_debug("   client name is %s\n", client->name);
-	mt9v111_data.pix.pixelformat = V4L2_PIX_FMT_UYVY;
-	mt9v111_data.pix.width = MT9V111_MAX_WIDTH;
-	mt9v111_data.pix.height = MT9V111_MAX_HEIGHT;
-	mt9v111_data.streamcap.capability = 0; /* No higher resolution or frame
-						* frame rate changes supported.
-						*/
-	mt9v111_data.streamcap.timeperframe.denominator = MT9V111_FRAME_RATE;
-	mt9v111_data.streamcap.timeperframe.numerator = 1;
+	mt9v111_data[sensorid].pix.pixelformat = V4L2_PIX_FMT_UYVY;
+	mt9v111_data[sensorid].pix.width = MT9V111_MAX_WIDTH;
+	mt9v111_data[sensorid].pix.height = MT9V111_MAX_HEIGHT;
+	mt9v111_data[sensorid].streamcap.capability = 0; /* No higher resolution or frame
+						* frame rate changes supported.*/
+	mt9v111_data[sensorid].streamcap.timeperframe.denominator = MT9V111_FRAME_RATE;
+	mt9v111_data[sensorid].streamcap.timeperframe.numerator = 1;
+
+	strcpy(mt9v111_int_device[sensorid].name,id->name);
+	pr_debug("   video device name is %s\n", mt9v111_data[sensorid].v4l2_int_device->name);
+	mt9v111_data[sensorid].v4l2_int_device = &mt9v111_int_device[sensorid];
+	mt9v111_int_device[sensorid].priv = &mt9v111_data[sensorid];
+
+	ipu_csi_enable_mclk_if(CSI_MCLK_I2C, sensorid , true, true);
+
+	if( mt9v111_read_id(sensorid) != 0) {
+		printk(KERN_ERR"mt9v111_probe: No sensor found\n");
+		ipu_csi_enable_mclk_if(CSI_MCLK_I2C, sensorid , false, false);
+		return -ENXIO;
+	}
 
-	mt9v111_int_device.priv = &mt9v111_data;
+#ifdef MT9V111_DEBUG
+	mt9v111_test_pattern(1);
+#endif
+
+	ipu_csi_enable_mclk_if(CSI_MCLK_I2C, sensorid , false, false);
 
 	pr_debug("   type is %d (expect %d)\n",
-		mt9v111_int_device.type, v4l2_int_type_slave);
+		mt9v111_int_device[sensorid].type, v4l2_int_type_slave);
 	pr_debug("   num ioctls is %d\n",
-		mt9v111_int_device.u.slave->num_ioctls);
+		mt9v111_int_device[sensorid].u.slave->num_ioctls);
 
 	/* This function attaches this structure to the /dev/video0 device.
 	 * The pointer in priv points to the mt9v111_data structure here.*/
-	retval = v4l2_int_device_register(&mt9v111_int_device);
+	retval = v4l2_int_device_register(&mt9v111_int_device[sensorid]);
+	if( retval == 0 )
+		mt9v111_data[sensorid].used = 1;
 
 	return retval;
 }
@@ -998,9 +1447,14 @@ static int mt9v111_probe(struct i2c_client *client,
  */
 static int mt9v111_remove(struct i2c_client *client)
 {
+	int i;
+
 	pr_debug("In mt9v111_remove\n");
 
-	v4l2_int_device_unregister(&mt9v111_int_device);
+	for ( i=0 ; i < ARRAY_SIZE(mt9v111_int_device) ; i++ ) {
+		if( mt9v111_data[i].used )
+			v4l2_int_device_unregister(&mt9v111_int_device[i]);
+	}
 	return 0;
 }
 
@@ -1033,13 +1487,13 @@ static __init int mt9v111_init(void)
 	memset(mt9v111_device.ifpReg, 0, sizeof(mt9v111_IFPReg));
 
 	/* Set contents of the just created structures. */
-	mt9v111_config();
+	mt9v111_config_datasheet();
 
 	/* Tells the i2c driver what functions to call for this driver. */
 	err = i2c_add_driver(&mt9v111_i2c_driver);
 	if (err != 0)
 		pr_err("%s:driver registration failed, error=%d \n",
-		       __func__, err);
+			   __func__, err);
 
 	return err;
 }
@@ -1055,7 +1509,6 @@ static void __exit mt9v111_clean(void)
 	pr_debug("In mt9v111_clean()\n");
 
 	i2c_del_driver(&mt9v111_i2c_driver);
-	gpio_sensor_inactive();
 
 	if (mt9v111_device.coreReg) {
 		kfree(mt9v111_device.coreReg);
diff --git a/drivers/media/video/mxc/capture/mt9v111.h b/drivers/media/video/mxc/capture/mt9v111.h
index cf38cec4757c..ba91a722a076 100644
--- a/drivers/media/video/mxc/capture/mt9v111.h
+++ b/drivers/media/video/mxc/capture/mt9v111.h
@@ -111,11 +111,14 @@
 #define MT9V111I_GAMMA_KNEE_Y90           0x57
 #define MT9V111I_GAMMA_VALUE_Y0           0x58
 #define MT9V111I_SHUTTER_60               0x59
+#define MT9V111I_AUTO_EXPOSURE_17         0x5A
 #define MT9V111I_SEARCH_FLICK_60          0x5c
+#define MT9V111I_RESERVED93	    	      0x5d
 #define MT9V111I_RATIO_IMAGE_GAIN_BASE    0x5e
 #define MT9V111I_RATIO_IMAGE_GAIN_DELTA   0x5f
 #define MT9V111I_SIGN_VALUE_REG5F         0x60
 #define MT9V111I_AE_GAIN                  0x62
+#define MT9V111I_RESERVED100              0x64
 #define MT9V111I_MAX_GAIN_AE              0x67
 #define MT9V111I_LENS_CORRECT_CTRL        0x80
 #define MT9V111I_SHADING_PARAMETER1       0x81
@@ -173,6 +176,7 @@
 #define MT9V111S_ROW_START_IN_ZOOM        0x13
 #define MT9V111S_DIGITAL_ZOOM             0x1e
 #define MT9V111S_READ_MODE                0x20
+#define MT9V111S_RESERVED33               0x21
 #define MT9V111S_DAC_CTRL                 0x27
 #define MT9V111S_GREEN1_GAIN              0x2b
 #define MT9V111S_BLUE_GAIN                0x2c
@@ -278,6 +282,7 @@ typedef struct {
 	u32 rowNoiseControl;
 	u32 darkTargetwNC;
 	u32 testData;		/*!< test mode */
+	u32 reserved33;
 	u32 globalGain;
 	u32 chipVersion;
 	u32 darkTargetwoNC;
@@ -375,11 +380,14 @@ typedef struct {
 	u32 gammaKneeY90;	/*!< Gamma knee points Y9 and Y10 */
 	u32 gammaKneeY0;	/*!< Gamma knee point Y0 */
 	u32 shutter_width_60;
+	u32 auto_exposure_17;
 	u32 search_flicker_60;
+	u32 reserved93;
 	u32 ratioImageGainBase;
 	u32 ratioImageGainDelta;
 	u32 signValueReg5F;
 	u32 aeGain;
+	u32 reserved100;
 	u32 maxGainAE;
 	u32 lensCorrectCtrl;
 	u32 shadingParameter1;	/*!< Shade Parameters */
diff --git a/drivers/media/video/mxc/capture/mx27_prpsw.c b/drivers/media/video/mxc/capture/mx27_prpsw.c
index ce7db16913ec..eca200a580f2 100644
--- a/drivers/media/video/mxc/capture/mx27_prpsw.c
+++ b/drivers/media/video/mxc/capture/mx27_prpsw.c
@@ -1,5 +1,5 @@
 /*
- * Copyright 2004-2007 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2004-2010 Freescale Semiconductor, Inc. All Rights Reserved.
  */
 
 /*
@@ -702,7 +702,7 @@ static int prp_still_start(void *private)
 	cam_data *cam = (cam_data *) private;
 
 	g_still_on = 1;
-	g_prp_cfg.ch2_ptr = (unsigned int)cam->still_buf;
+	g_prp_cfg.ch2_ptr = (unsigned int)cam->still_buf[0];
 	g_prp_cfg.ch2_ptr2 = 0;
 
 	if (prp_v4l2_cfg(&g_prp_cfg, cam))
diff --git a/drivers/media/video/mxc/capture/mxc_v4l2_capture.c b/drivers/media/video/mxc/capture/mxc_v4l2_capture.c
index 30ad533b0ebd..be9a988aff2f 100644
--- a/drivers/media/video/mxc/capture/mxc_v4l2_capture.c
+++ b/drivers/media/video/mxc/capture/mxc_v4l2_capture.c
@@ -18,6 +18,8 @@
  *
  * @ingroup MXC_V4L2_CAPTURE
  */
+
+
 #include <linux/version.h>
 #include <linux/module.h>
 #include <linux/init.h>
@@ -37,9 +39,9 @@
 #include <media/v4l2-int-device.h>
 #include "mxc_v4l2_capture.h"
 #include "ipu_prp_sw.h"
+#include "asm/delay.h"
 
 static int video_nr = -1;
-static cam_data *g_cam;
 
 /*! This data is used for the output to the display. */
 #define MXC_V4L2_CAPTURE_NUM_OUTPUTS	3
@@ -74,21 +76,23 @@ static struct v4l2_output mxc_capture_outputs[MXC_V4L2_CAPTURE_NUM_OUTPUTS] = {
 static struct v4l2_input mxc_capture_inputs[MXC_V4L2_CAPTURE_NUM_INPUTS] = {
 	{
 	 .index = 0,
-	 .name = "CSI IC MEM",
+	 .name = "CSI MEM",
 	 .type = V4L2_INPUT_TYPE_CAMERA,
 	 .audioset = 0,
 	 .tuner = 0,
 	 .std = V4L2_STD_UNKNOWN,
-	 .status = 0,
+	 .status = V4L2_IN_ST_NO_POWER,
 	 },
 	{
 	 .index = 1,
+// AG: CSI IC MEM works but has problems
+//	 .name = "CSI IC MEM",
 	 .name = "CSI MEM",
 	 .type = V4L2_INPUT_TYPE_CAMERA,
 	 .audioset = 0,
 	 .tuner = 0,
 	 .std = V4L2_STD_UNKNOWN,
-	 .status = V4L2_IN_ST_NO_POWER,
+	 .status = 0,
 	 },
 };
 
@@ -118,6 +122,7 @@ typedef struct {
 	u16 active_left;	/*!< Active left. */
 } video_fmt_t;
 
+#if 0
 /*!
  * Description of video formats supported.
  *
@@ -128,37 +133,39 @@ static video_fmt_t video_fmts[] = {
 	{			/*! NTSC */
 	 .v4l2_id = V4L2_STD_NTSC,
 	 .name = "NTSC",
-	 .raw_width = 720 - 1,		/* SENS_FRM_WIDTH */
-	 .raw_height = 288 - 1,		/* SENS_FRM_HEIGHT */
-	 .active_width = 720,		/* ACT_FRM_WIDTH plus 1 */
-	 .active_height = (480 / 2),	/* ACT_FRM_HEIGHT plus 1 */
-	 .active_top = 12,
+	 .raw_width = 720,		/* SENS_FRM_WIDTH */
+	 .raw_height = 525,		/* SENS_FRM_HEIGHT */
+	 .active_width = 720,		/* ACT_FRM_WIDTH */
+	 .active_height = 240,		/* ACT_FRM_HEIGHT */
+	 .active_top = 0,
 	 .active_left = 0,
 	 },
 	{			/*! (B, G, H, I, N) PAL */
 	 .v4l2_id = V4L2_STD_PAL,
 	 .name = "PAL",
-	 .raw_width = 720 - 1,
-	 .raw_height = (576 / 2) + 24 * 2 - 1,
+	 .raw_width = 720,
+	 .raw_height = 625,
 	 .active_width = 720,
-	 .active_height = (576 / 2),
+	 .active_height = 288,
 	 .active_top = 0,
 	 .active_left = 0,
 	 },
 	{			/*! Unlocked standard */
 	 .v4l2_id = V4L2_STD_ALL,
 	 .name = "Autodetect",
-	 .raw_width = 720 - 1,
-	 .raw_height = (576 / 2) + 24 * 2 - 1,
+	 .raw_width = 720,
+	 .raw_height = 625,
 	 .active_width = 720,
-	 .active_height = (576 / 2),
+	 .active_height = 288,
 	 .active_top = 0,
 	 .active_left = 0,
 	 },
 };
 
+
 /*!* Standard index of TV. */
 static video_fmt_idx video_index = TV_NOT_LOCKED;
+#endif
 
 static int mxc_v4l2_master_attach(struct v4l2_int_device *slave);
 static void mxc_v4l2_master_detach(struct v4l2_int_device *slave);
@@ -172,15 +179,27 @@ static struct v4l2_int_master mxc_v4l2_master = {
 	.detach = mxc_v4l2_master_detach,
 };
 
-static struct v4l2_int_device mxc_v4l2_int_device = {
+static struct v4l2_int_device mxc_v4l2_int_device [] = {
+	{
+	.module = THIS_MODULE,
+	.name = "mxc_v4l2_cap_1",
+	.type = v4l2_int_type_master,
+	.u = {
+		.master = &mxc_v4l2_master,
+		},
+	},
+	{
 	.module = THIS_MODULE,
-	.name = "mxc_v4l2_cap",
+	.name = "mxc_v4l2_cap_2",
 	.type = v4l2_int_type_master,
 	.u = {
 		.master = &mxc_v4l2_master,
 		},
+	},
 };
 
+static cam_data *g_cam[ARRAY_SIZE(mxc_v4l2_int_device)];
+
 /***************************************************************************
  * Functions for handling Frame buffers.
  **************************************************************************/
@@ -260,6 +279,7 @@ static int mxc_allocate_frame_buf(cam_data *cam, int count)
 static void mxc_free_frames(cam_data *cam)
 {
 	int i;
+	unsigned long lock_flags;
 
 	pr_debug("In MVC:mxc_free_frames\n");
 
@@ -269,9 +289,11 @@ static void mxc_free_frames(cam_data *cam)
 
 	cam->enc_counter = 0;
 	cam->skip_frame = 0;
+	spin_lock_irqsave(&cam->dqueue_int_lock, lock_flags);
 	INIT_LIST_HEAD(&cam->ready_q);
 	INIT_LIST_HEAD(&cam->working_q);
 	INIT_LIST_HEAD(&cam->done_q);
+	spin_unlock_irqrestore(&cam->dqueue_int_lock, lock_flags);
 }
 
 /*!
@@ -331,8 +353,7 @@ static int mxc_streamon(cam_data *cam)
 {
 	struct mxc_v4l_frame *frame;
 	int err = 0;
-
-	pr_debug("In MVC:mxc_streamon\n");
+	unsigned long lock_flags;
 
 	if (NULL == cam) {
 		pr_err("ERROR! cam parameter is NULL\n");
@@ -363,23 +384,27 @@ static int mxc_streamon(cam_data *cam)
 		}
 	}
 
+	spin_lock_irqsave(&cam->queue_int_lock, lock_flags);
 	cam->ping_pong_csi = 0;
 	if (cam->enc_update_eba) {
-		frame =
-		    list_entry(cam->ready_q.next, struct mxc_v4l_frame, queue);
-		list_del(cam->ready_q.next);
-		list_add_tail(&frame->queue, &cam->working_q);
-		err = cam->enc_update_eba(frame->buffer.m.offset,
-					  &cam->ping_pong_csi);
 
 		frame =
 		    list_entry(cam->ready_q.next, struct mxc_v4l_frame, queue);
 		list_del(cam->ready_q.next);
 		list_add_tail(&frame->queue, &cam->working_q);
-		err |= cam->enc_update_eba(frame->buffer.m.offset,
-					   &cam->ping_pong_csi);
+		err = cam->enc_update_eba(cam->csi,frame->buffer.m.offset,
+					  &cam->ping_pong_csi);
+		if (!list_empty(&cam->ready_q)) {
+			frame =
+				list_entry(cam->ready_q.next, struct mxc_v4l_frame, queue);
+			list_del(cam->ready_q.next);
+			list_add_tail(&frame->queue, &cam->working_q);
+			err |= cam->enc_update_eba(cam->csi,frame->buffer.m.offset,
+						   &cam->ping_pong_csi);
+		}
+		cam->capture_on = true;
 	} else {
-		return -EINVAL;
+		err = -EINVAL;
 	}
 
 	if (cam->overlay_on == true)
@@ -387,11 +412,9 @@ static int mxc_streamon(cam_data *cam)
 
 	if (cam->enc_enable_csi) {
 		err = cam->enc_enable_csi(cam);
-		if (err != 0)
-			return err;
 	}
 
-	cam->capture_on = true;
+	spin_unlock_irqrestore(&cam->queue_int_lock, lock_flags);
 
 	return err;
 }
@@ -543,8 +566,6 @@ static int start_preview(cam_data *cam)
 {
 	int err = 0;
 
-	pr_debug("MVC: start_preview\n");
-
 #if defined(CONFIG_MXC_IPU_PRP_VF_SDC) || defined(CONFIG_MXC_IPU_PRP_VF_SDC_MODULE)
 	pr_debug("   This is an SDC display\n");
 	if (cam->output == 0 || cam->output == 2) {
@@ -602,8 +623,6 @@ static int stop_preview(cam_data *cam)
 {
 	int err = 0;
 
-	pr_debug("MVC: stop preview\n");
-
 #if defined(CONFIG_MXC_IPU_PRP_VF_ADC) || defined(CONFIG_MXC_IPU_PRP_VF_ADC_MODULE)
 	if (cam->output == 1) {
 		err = prp_vf_adc_deselect(cam);
@@ -674,6 +693,29 @@ static int mxc_v4l2_g_fmt(cam_data *cam, struct v4l2_format *f)
 		 __func__,
 		 cam->crop_current.width, cam->crop_current.height);
 
+	retval = vidioc_int_g_fmt_cap(cam->sensor,f);
+	return retval;
+}
+
+/*!
+ * V4L2 - mxc_v4l2_enum_fmt function
+ *
+ * @param cam         structure cam_data *
+ *
+ * @param f           structure v4l2_fmtdesc *
+ *
+ * @return  status    0 success, EINVAL failed
+ */
+static int mxc_v4l2_enum_fmt(cam_data *cam, struct v4l2_fmtdesc *f)
+{
+	int retval = 0;
+
+	pr_debug("In MVC: mxc_v4l2_enum_fmt\n");
+
+	ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi, true, true);
+	retval = vidioc_int_enum_fmt_cap(cam->sensor,f);
+	ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi, false, false);
+
 	return retval;
 }
 
@@ -709,10 +751,14 @@ static int mxc_v4l2_s_fmt(cam_data *cam, struct v4l2_format *f)
 		 * for CSI MEM input mode.
 		 */
 		if (strcmp(mxc_capture_inputs[cam->current_input].name,
-			   "CSI MEM") == 0) {
+			   "CSI MEM") == 0 || strcmp(mxc_capture_inputs[cam->current_input].name,
+					   "CSI IC MEM") == 0) {
 			f->fmt.pix.width = cam->crop_current.width;
 			f->fmt.pix.height = cam->crop_current.height;
 		}
+		else {
+			printk("Error no match %s\n",mxc_capture_inputs[cam->current_input].name);
+		}
 
 		if (cam->rotation >= IPU_ROTATE_90_RIGHT) {
 			height = &f->fmt.pix.width;
@@ -811,6 +857,13 @@ static int mxc_v4l2_s_fmt(cam_data *cam, struct v4l2_format *f)
 				break;
 			}
 		}
+
+		ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi,
+				       true, true);
+		vidioc_int_s_fmt_cap(cam->sensor, f);
+		ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi,
+				       false, false);
+
 		break;
 	case V4L2_BUF_TYPE_VIDEO_OVERLAY:
 		pr_debug("   type=V4L2_BUF_TYPE_VIDEO_OVERLAY\n");
@@ -1057,63 +1110,10 @@ static int mxc_v4l2_s_ctrl(cam_data *cam, struct v4l2_control *c)
 	return ret;
 }
 
-/*!
- * V4L2 - mxc_v4l2_s_param function
- * Allows setting of capturemode and frame rate.
- *
- * @param cam         structure cam_data *
- * @param parm        structure v4l2_streamparm *
- *
- * @return  status    0 success, EINVAL failed
- */
-static int mxc_v4l2_s_param(cam_data *cam, struct v4l2_streamparm *parm)
-{
-	struct v4l2_ifparm ifparm;
+static int 	mxc_v4l2_init_csi( cam_data *cam ) {
 	struct v4l2_format cam_fmt;
-	struct v4l2_streamparm currentparm;
 	ipu_csi_signal_cfg_t csi_param;
-	int err = 0;
-
-	pr_debug("In mxc_v4l2_s_param\n");
-
-	if (parm->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) {
-		pr_err(KERN_ERR "mxc_v4l2_s_param invalid type\n");
-		return -EINVAL;
-	}
-
-	/* Stop the viewfinder */
-	if (cam->overlay_on == true) {
-		stop_preview(cam);
-	}
-
-	currentparm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-
-	/* First check that this device can support the changes requested. */
-	err = vidioc_int_g_parm(cam->sensor, &currentparm);
-	if (err) {
-		pr_err("%s: vidioc_int_g_parm returned an error %d\n",
-			__func__, err);
-		goto exit;
-	}
-
-	pr_debug("   Current capabilities are %x\n",
-			currentparm.parm.capture.capability);
-	pr_debug("   Current capturemode is %d  change to %d\n",
-			currentparm.parm.capture.capturemode,
-			parm->parm.capture.capturemode);
-	pr_debug("   Current framerate is %d  change to %d\n",
-			currentparm.parm.capture.timeperframe.denominator,
-			parm->parm.capture.timeperframe.denominator);
-
-	/* This will change any camera settings needed. */
-	ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi, true, true);
-	err = vidioc_int_s_parm(cam->sensor, parm);
-	ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi, false, false);
-	if (err) {
-		pr_err("%s: vidioc_int_s_parm returned an error %d\n",
-			__func__, err);
-		goto exit;
-	}
+	struct v4l2_ifparm ifparm;
 
 	/* If resolution changed, need to re-program the CSI */
 	/* Get new values. */
@@ -1131,13 +1131,13 @@ static int mxc_v4l2_s_param(cam_data *cam, struct v4l2_streamparm *parm)
 	csi_param.force_eof = 0;
 	csi_param.data_en_pol = 0;
 	csi_param.data_fmt = 0;
-	csi_param.csi = 0;
+	csi_param.csi = cam->csi;
 	csi_param.mclk = 0;
 
 	/* This may not work on other platforms. Check when adding a new one.*/
 	pr_debug("   clock_curr=mclk=%d\n", ifparm.u.bt656.clock_curr);
 	if (ifparm.u.bt656.clock_curr == 0) {
-		csi_param.clk_mode = IPU_CSI_CLK_MODE_CCIR656_PROGRESSIVE;
+		csi_param.clk_mode = IPU_CSI_CLK_MODE_CCIR656_INTERLACED;
 	} else {
 		csi_param.clk_mode = IPU_CSI_CLK_MODE_GATED_CLK;
 	}
@@ -1189,7 +1189,63 @@ static int mxc_v4l2_s_param(cam_data *cam, struct v4l2_streamparm *parm)
 	ipu_csi_init_interface(cam->crop_bounds.width,
 			       cam->crop_bounds.height,
 			       cam_fmt.fmt.pix.pixelformat, csi_param);
+	return 0;
+}
 
+/*!
+ * V4L2 - mxc_v4l2_s_param function
+ * Allows setting of capturemode and frame rate.
+ *
+ * @param cam         structure cam_data *
+ * @param parm        structure v4l2_streamparm *
+ *
+ * @return  status    0 success, EINVAL failed
+ */
+static int mxc_v4l2_s_param(cam_data *cam, struct v4l2_streamparm *parm)
+{
+	struct v4l2_streamparm currentparm;
+	int err = 0;
+
+	if (parm->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) {
+		pr_err(KERN_ERR "mxc_v4l2_s_param invalid type\n");
+		return -EINVAL;
+	}
+
+	/* Stop the viewfinder */
+	if (cam->overlay_on == true) {
+		stop_preview(cam);
+	}
+
+	currentparm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+	/* First check that this device can support the changes requested. */
+	err = vidioc_int_g_parm(cam->sensor, &currentparm);
+	if (err) {
+		pr_err("%s: vidioc_int_g_parm returned an error %d\n",
+			__func__, err);
+		goto exit;
+	}
+
+	pr_debug("   Current capabilities are %x\n",
+			currentparm.parm.capture.capability);
+	pr_debug("   Current capturemode is %d  change to %d\n",
+			currentparm.parm.capture.capturemode,
+			parm->parm.capture.capturemode);
+	pr_debug("   Current framerate is %d  change to %d\n",
+			currentparm.parm.capture.timeperframe.denominator,
+			parm->parm.capture.timeperframe.denominator);
+
+	/* This will change any camera settings needed. */
+	ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi, true, true);
+	err = vidioc_int_s_parm(cam->sensor, parm);
+	ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi, false, false);
+	if (err) {
+		pr_err("%s: vidioc_int_s_parm returned an error %d\n",
+			__func__, err);
+		goto exit;
+	}
+
+	err = mxc_v4l2_init_csi(cam);
 
 exit:
 	if (cam->overlay_on == true)
@@ -1198,6 +1254,7 @@ exit:
 	return err;
 }
 
+#if 0
 /*!
  * V4L2 - mxc_v4l2_s_std function
  *
@@ -1287,6 +1344,7 @@ static int mxc_v4l2_g_std(cam_data *cam, v4l2_std_id *e)
 
 	return 0;
 }
+#endif
 
 /*!
  * Dequeue one V4L capture buffer
@@ -1372,6 +1430,11 @@ static int mxc_v4l_open(struct file *file)
 		return -EBADF;
 	}
 
+	if(!cam->sensor) {
+		pr_err("ERROR: v4l2 capture: Unattached sensor!\n");
+		return -EBADF;
+	}
+
 	down(&cam->busy_lock);
 	err = 0;
 	if (signal_pending(current))
@@ -1411,12 +1474,9 @@ static int mxc_v4l_open(struct file *file)
 		csi_param.force_eof = 0;
 		csi_param.data_en_pol = 0;
 		csi_param.mclk = ifparm.u.bt656.clock_curr;
-
+		csi_param.ext_vsync = ifparm.u.bt656.bt_sync_correct;
 		csi_param.pixclk_pol = ifparm.u.bt656.latch_clk_inv;
 
-		/* Once we handle multiple inputs this will need to change. */
-		csi_param.csi = 0;
-
 		if (ifparm.u.bt656.mode
 				== V4L2_IF_TYPE_BT656_MODE_NOBT_8BIT)
 			csi_param.data_width = IPU_CSI_DATA_WIDTH_8;
@@ -1464,11 +1524,13 @@ static int mxc_v4l_open(struct file *file)
 			__func__,
 			cam->crop_current.width, cam->crop_current.height);
 
+		udelay(100);
+
 		csi_param.data_fmt = cam_fmt.fmt.pix.pixelformat;
 		pr_debug("On Open: Input to ipu size is %d x %d\n",
 				cam_fmt.fmt.pix.width, cam_fmt.fmt.pix.height);
 		ipu_csi_set_window_size(cam->crop_current.width,
-					cam->crop_current.width,
+					cam->crop_current.height,
 					cam->csi);
 		ipu_csi_set_window_pos(cam->crop_current.left,
 					cam->crop_current.top,
@@ -1478,12 +1540,16 @@ static int mxc_v4l_open(struct file *file)
 					cam_fmt.fmt.pix.pixelformat,
 					csi_param);
 
+		udelay(100);
+
 		ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi,
 				       true, true);
 		vidioc_int_init(cam->sensor);
 
 		ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi,
 				       false, false);
+
+		udelay(100);
 }
 
 	file->private_data = dev;
@@ -1519,7 +1585,7 @@ static int mxc_v4l_close(struct file *file)
 		err = stop_preview(cam);
 		cam->overlay_on = false;
 	}
-	if (cam->capture_pid == current->pid) {
+	if (cam->capture_pid == current->tgid) {
 		err |= mxc_streamoff(cam);
 		wake_up_interruptible(&cam->enc_queue);
 	}
@@ -1570,7 +1636,7 @@ static ssize_t mxc_v4l_read(struct file *file, char *buf, size_t count,
 			    loff_t *ppos)
 {
 	int err = 0;
-	u8 *v_address;
+	u8 *v_address[2];
 	struct video_device *dev = video_devdata(file);
 	cam_data *cam = video_get_drvdata(dev);
 
@@ -1581,11 +1647,17 @@ static ssize_t mxc_v4l_read(struct file *file, char *buf, size_t count,
 	if (cam->overlay_on == true)
 		stop_preview(cam);
 
-	v_address = dma_alloc_coherent(0,
+	v_address[0] = dma_alloc_coherent(0,
 				       PAGE_ALIGN(cam->v2f.fmt.pix.sizeimage),
-				       &cam->still_buf, GFP_DMA | GFP_KERNEL);
+				       &cam->still_buf[0],
+				       GFP_DMA | GFP_KERNEL);
+
+	v_address[1] = dma_alloc_coherent(0,
+				       PAGE_ALIGN(cam->v2f.fmt.pix.sizeimage),
+				       &cam->still_buf[1],
+				       GFP_DMA | GFP_KERNEL);
 
-	if (!v_address) {
+	if (!v_address[0] || !v_address[1]) {
 		err = -ENOBUFS;
 		goto exit0;
 	}
@@ -1593,14 +1665,14 @@ static ssize_t mxc_v4l_read(struct file *file, char *buf, size_t count,
 	err = prp_still_select(cam);
 	if (err != 0) {
 		err = -EIO;
-		goto exit1;
+		goto exit0;
 	}
 
 	cam->still_counter = 0;
 	err = cam->csi_start(cam);
 	if (err != 0) {
 		err = -EIO;
-		goto exit2;
+		goto exit1;
 	}
 
 	if (!wait_event_interruptible_timeout(cam->still_queue,
@@ -1609,19 +1681,23 @@ static ssize_t mxc_v4l_read(struct file *file, char *buf, size_t count,
 		pr_err("ERROR: v4l2 capture: mxc_v4l_read timeout counter %x\n",
 		       cam->still_counter);
 		err = -ETIME;
-		goto exit2;
+		goto exit1;
 	}
-	err = copy_to_user(buf, v_address, cam->v2f.fmt.pix.sizeimage);
-
-      exit2:
-	prp_still_deselect(cam);
+	err = copy_to_user(buf, v_address[1], cam->v2f.fmt.pix.sizeimage);
 
       exit1:
-	dma_free_coherent(0, cam->v2f.fmt.pix.sizeimage, v_address,
-			  cam->still_buf);
-	cam->still_buf = 0;
+	prp_still_deselect(cam);
 
       exit0:
+	if (v_address[0] != 0)
+		dma_free_coherent(0, cam->v2f.fmt.pix.sizeimage, v_address[0],
+				  cam->still_buf[0]);
+	if (v_address[1] != 0)
+		dma_free_coherent(0, cam->v2f.fmt.pix.sizeimage, v_address[1],
+				  cam->still_buf[1]);
+
+	cam->still_buf[0] = cam->still_buf[1] = 0;
+
 	if (cam->overlay_on == true) {
 		start_preview(cam);
 	}
@@ -1661,6 +1737,21 @@ static long mxc_v4l_do_ioctl(struct file *file,
 		return -EBUSY;
 
 	switch (ioctlnr) {
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	case VIDIOC_DBG_S_REGISTER: {
+		struct v4l2_dbg_register * dreg = arg;
+		vidioc_int_s_register(cam->sensor,dreg);
+		break;
+	}
+
+	case VIDIOC_DBG_G_REGISTER: {
+		struct v4l2_dbg_register * dreg = arg;
+		vidioc_int_g_register(cam->sensor,dreg);
+		break;
+	}
+#endif
+
 	/*!
 	 * V4l2 VIDIOC_QUERYCAP ioctl
 	 */
@@ -1695,6 +1786,14 @@ static long mxc_v4l_do_ioctl(struct file *file,
 		struct v4l2_format *sf = arg;
 		pr_debug("   case VIDIOC_S_FMT\n");
 		retval = mxc_v4l2_s_fmt(cam, sf);
+		mxc_v4l2_init_csi(cam);
+		break;
+	}
+
+	case VIDIOC_ENUM_FMT: {
+		struct v4l2_fmtdesc *fd = arg;
+		pr_debug("   case VIDIOC_ENUM_FMT\n");
+		retval = mxc_v4l2_enum_fmt(cam, fd);
 		break;
 	}
 
@@ -1775,9 +1874,8 @@ static long mxc_v4l_do_ioctl(struct file *file,
 				if (cam->skip_frame > 0) {
 					list_add_tail(&cam->frame[index].queue,
 						      &cam->working_q);
-
 					retval =
-					   cam->enc_update_eba(cam->
+					   cam->enc_update_eba(cam->csi,cam->
 							       frame[index].
 							       buffer.m.offset,
 							       &cam->
@@ -1999,28 +2097,6 @@ static long mxc_v4l_do_ioctl(struct file *file,
 		break;
 	}
 
-	/* linux v4l2 bug, kernel c0485619 user c0405619 */
-	case VIDIOC_ENUMSTD: {
-		struct v4l2_standard *e = arg;
-		pr_debug("   case VIDIOC_ENUMSTD\n");
-		*e = cam->standard;
-		break;
-	}
-
-	case VIDIOC_G_STD: {
-		v4l2_std_id *e = arg;
-		pr_debug("   case VIDIOC_G_STD\n");
-		retval = mxc_v4l2_g_std(cam, e);
-		break;
-	}
-
-	case VIDIOC_S_STD: {
-		v4l2_std_id *e = arg;
-		pr_debug("   case VIDIOC_S_STD\n");
-		retval = mxc_v4l2_s_std(cam, *e);
-
-		break;
-	}
 
 	case VIDIOC_ENUMOUTPUT: {
 		struct v4l2_output *output = arg;
@@ -2109,8 +2185,13 @@ static long mxc_v4l_do_ioctl(struct file *file,
 		break;
 	}
 
-	case VIDIOC_ENUM_FMT:
-	case VIDIOC_TRY_FMT:
+	case VIDIOC_TRY_FMT: {
+		struct v4l2_format * f = arg;
+		pr_debug("   case VIDIOC_TRY_FMT\n");
+		retval = vidioc_int_try_fmt_cap(cam->sensor,f);
+		break;
+	}
+
 	case VIDIOC_QUERYCTRL:
 	case VIDIOC_G_TUNER:
 	case VIDIOC_S_TUNER:
@@ -2163,7 +2244,7 @@ static int mxc_mmap(struct file *file, struct vm_area_struct *vma)
 		return -EINTR;
 
 	size = vma->vm_end - vma->vm_start;
-	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
+	vma->vm_page_prot = pgprot_writecombine(vma->vm_page_prot);
 
 	if (remap_pfn_range(vma, vma->vm_start,
 			    vma->vm_pgoff, size, vma->vm_page_prot)) {
@@ -2236,12 +2317,21 @@ static void camera_platform_release(struct device *device)
 }
 
 /*! Device Definition for Mt9v111 devices */
-static struct platform_device mxc_v4l2_devices = {
-	.name = "mxc_v4l2",
-	.dev = {
-		.release = camera_platform_release,
-		},
-	.id = 0,
+static struct platform_device mxc_v4l2_devices[] = {
+	{
+		.name = "mxc_v4l2_1",
+		.dev = {
+			.release = camera_platform_release,
+			},
+		.id = 0,
+	},
+	{
+		.name = "mxc_v4l2_2",
+		.dev = {
+			.release = camera_platform_release,
+			},
+		.id = 1,
+	}
 };
 
 /*!
@@ -2302,7 +2392,7 @@ static void camera_callback(u32 mask, void *dev)
 							 struct mxc_v4l_frame,
 							 queue);
 
-				if (cam->enc_update_eba(
+				if (cam->enc_update_eba(cam->csi,
 						ready_frame->buffer.m.offset,
 						&cam->ping_pong_csi) == 0) {
 					list_del(cam->ready_q.next);
@@ -2354,7 +2444,7 @@ static void camera_callback(u32 mask, void *dev)
 			ready_frame = list_entry(cam->ready_q.next,
 						 struct mxc_v4l_frame,
 						 queue);
-			if (cam->enc_update_eba(ready_frame->buffer.m.offset,
+			if (cam->enc_update_eba(cam->csi,ready_frame->buffer.m.offset,
 						&cam->ping_pong_csi) == 0) {
 				list_del(cam->ready_q.next);
 				list_add_tail(&ready_frame->queue,
@@ -2362,7 +2452,7 @@ static void camera_callback(u32 mask, void *dev)
 			} else
 				return;
 		} else {
-			if (cam->enc_update_eba(
+			if (cam->enc_update_eba(cam->csi,
 				cam->dummy_frame.buffer.m.offset,
 				&cam->ping_pong_csi) == -EACCES)
 				return;
@@ -2379,13 +2469,15 @@ static void camera_callback(u32 mask, void *dev)
  *
  * @return status  0 Success
  */
-static void init_camera_struct(cam_data *cam)
+static void init_camera_struct(cam_data *cam,unsigned int csi)
 {
-	pr_debug("In MVC: init_camera_struct\n");
+	pr_debug("In MVC: init_camera_struct for csi %d\n",csi);
 
 	/* Default everything to 0 */
 	memset(cam, 0, sizeof(cam_data));
 
+	cam->csi = csi;
+
 	init_MUTEX(&cam->param_lock);
 	init_MUTEX(&cam->busy_lock);
 
@@ -2396,7 +2488,7 @@ static void init_camera_struct(cam_data *cam)
 	*(cam->video_dev) = mxc_v4l_template;
 
 	video_set_drvdata(cam->video_dev, cam);
-	dev_set_drvdata(&mxc_v4l2_devices.dev, (void *)cam);
+	dev_set_drvdata(&mxc_v4l2_devices[csi].dev, (void *)cam);
 	cam->video_dev->minor = -1;
 
 	init_waitqueue_head(&cam->enc_queue);
@@ -2428,6 +2520,8 @@ static void init_camera_struct(cam_data *cam)
 	cam->skip_frame = 0;
 	cam->v4l2_fb.flags = V4L2_FBUF_FLAG_OVERLAY;
 
+	cam->current_input = cam->csi;
+
 	cam->v2f.fmt.pix.sizeimage = 352 * 288 * 3 / 2;
 	cam->v2f.fmt.pix.bytesperline = 288 * 3 / 2;
 	cam->v2f.fmt.pix.width = 288;
@@ -2438,9 +2532,6 @@ static void init_camera_struct(cam_data *cam)
 	cam->win.w.left = 0;
 	cam->win.w.top = 0;
 
-	cam->csi = 0;  /* Need to determine how to set this correctly with
-			* multiple video input devices. */
-
 	cam->enc_callback = camera_callback;
 	init_waitqueue_head(&cam->power_queue);
 	spin_lock_init(&cam->queue_int_lock);
@@ -2460,11 +2551,12 @@ static u8 camera_power(cam_data *cam, bool cameraOn)
 {
 	pr_debug("In MVC:camera_power on=%d\n", cameraOn);
 
+	if( !cam->open_count )
+		return 0;
+
 	if (cameraOn == true) {
-		ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi, true, true);
 		vidioc_int_s_power(cam->sensor, 1);
 	} else {
-		ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi, false, false);
 		vidioc_int_s_power(cam->sensor, 0);
 	}
 	return 0;
@@ -2491,15 +2583,15 @@ static int mxc_v4l2_suspend(struct platform_device *pdev, pm_message_t state)
 		return -1;
 	}
 
+	if (!cam->open_count) {
+		return 0;
+	}
+
 	cam->low_power = true;
 
 	if (cam->overlay_on == true)
 		stop_preview(cam);
-	if ((cam->capture_on == true) && cam->enc_disable) {
-		cam->enc_disable(cam);
-	}
 	camera_power(cam, false);
-
 	return 0;
 }
 
@@ -2522,14 +2614,19 @@ static int mxc_v4l2_resume(struct platform_device *pdev)
 		return -1;
 	}
 
+	if( !cam->open_count )
+		return 0;
+
 	cam->low_power = false;
 	wake_up_interruptible(&cam->power_queue);
+
 	camera_power(cam, true);
 
 	if (cam->overlay_on == true)
 		start_preview(cam);
+
 	if (cam->capture_on == true)
-		mxc_streamon(cam);
+	    mxc_streamon(cam);
 
 	return 0;
 }
@@ -2537,15 +2634,27 @@ static int mxc_v4l2_resume(struct platform_device *pdev)
 /*!
  * This structure contains pointers to the power management callback functions.
  */
-static struct platform_driver mxc_v4l2_driver = {
-	.driver = {
-		   .name = "mxc_v4l2",
-		   },
-	.probe = NULL,
-	.remove = NULL,
-	.suspend = mxc_v4l2_suspend,
-	.resume = mxc_v4l2_resume,
-	.shutdown = NULL,
+static struct platform_driver mxc_v4l2_driver[] = {
+	{
+		.driver = {
+			   .name = "mxc_v4l2_1",
+			   },
+		.probe = NULL,
+		.remove = NULL,
+		.suspend = mxc_v4l2_suspend,
+		.resume = mxc_v4l2_resume,
+		.shutdown = NULL,
+	},
+	{
+		.driver = {
+			   .name = "mxc_v4l2_2",
+			   },
+		.probe = NULL,
+		.remove = NULL,
+		.suspend = mxc_v4l2_suspend,
+		.resume = mxc_v4l2_resume,
+		.shutdown = NULL,
+	},
 };
 
 /*!
@@ -2567,6 +2676,7 @@ static int mxc_v4l2_master_attach(struct v4l2_int_device *slave)
 	}
 
 	ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi, true, true);
+	vidioc_int_s_power(cam->sensor, 1);
 	vidioc_int_dev_init(slave);
 	ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi, false, false);
 	cam_fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
@@ -2623,53 +2733,55 @@ static void mxc_v4l2_master_detach(struct v4l2_int_device *slave)
 static __init int camera_init(void)
 {
 	u8 err = 0;
+	int i;
 
 	pr_debug("In MVC:camera_init\n");
 
-	/* Register the device driver structure. */
-	err = platform_driver_register(&mxc_v4l2_driver);
-	if (err != 0) {
-		pr_err("ERROR: v4l2 capture:camera_init: "
-			"platform_driver_register failed.\n");
-		return err;
-	}
+	for (i = 0; i < ARRAY_SIZE(mxc_v4l2_int_device); i++) {
+		/* Register the device driver structure. */
+		err = platform_driver_register(&mxc_v4l2_driver[i]);
+		if (err != 0) {
+			pr_err("ERROR: v4l2 capture:camera_init: "
+				"platform_driver_register failed.\n");
+			return err;
+		}
 
-	/* Create g_cam and initialize it. */
-	if ((g_cam = kmalloc(sizeof(cam_data), GFP_KERNEL)) == NULL) {
-		pr_err("ERROR: v4l2 capture: failed to register camera\n");
-		platform_driver_unregister(&mxc_v4l2_driver);
-		return -1;
-	}
-	init_camera_struct(g_cam);
+		/* Create g_cam and initialize it. */
+		if ((g_cam [i] = kmalloc(sizeof(cam_data), GFP_KERNEL)) == NULL) {
+			pr_err("ERROR: v4l2 capture: failed to register camera\n");
+			platform_driver_unregister(&mxc_v4l2_driver[i]);
+			return -1;
+		}
+		init_camera_struct(g_cam [i], i);
 
-	/* Set up the v4l2 device and register it*/
-	mxc_v4l2_int_device.priv = g_cam;
-	/* This function contains a bug that won't let this be rmmod'd. */
-	v4l2_int_device_register(&mxc_v4l2_int_device);
+		/* Set up the v4l2 device and register it*/
+		mxc_v4l2_int_device[i].priv = g_cam [i];
+		/* This function contains a bug that won't let this be rmmod'd. */
+		v4l2_int_device_register(&mxc_v4l2_int_device[i]);
 
-	/* Register the I2C device */
-	err = platform_device_register(&mxc_v4l2_devices);
-	if (err != 0) {
-		pr_err("ERROR: v4l2 capture: camera_init: "
-		       "platform_device_register failed.\n");
-		platform_driver_unregister(&mxc_v4l2_driver);
-		kfree(g_cam);
-		g_cam = NULL;
-		return err;
-	}
+		/* Register the I2C device */
+		err = platform_device_register(&mxc_v4l2_devices[i]);
+		if (err != 0) {
+			pr_err("ERROR: v4l2 capture: camera_init: "
+				   "platform_device_register failed.\n");
+					platform_driver_unregister(&mxc_v4l2_driver[i]);
+					kfree(g_cam [i]);
+					g_cam [i] = NULL;
+			return err;
+		}
 
-	/* register v4l video device */
-	if (video_register_device(g_cam->video_dev, VFL_TYPE_GRABBER, video_nr)
-	    == -1) {
-		platform_device_unregister(&mxc_v4l2_devices);
-		platform_driver_unregister(&mxc_v4l2_driver);
-		kfree(g_cam);
-		g_cam = NULL;
-		pr_err("ERROR: v4l2 capture: video_register_device failed\n");
-		return -1;
+		/* register v4l video device */
+		if (video_register_device(g_cam[i]->video_dev, VFL_TYPE_GRABBER, video_nr)== -1) {
+			platform_device_unregister(&mxc_v4l2_devices[i]);
+			platform_driver_unregister(&mxc_v4l2_driver[i]);
+			kfree(g_cam[i]);
+			g_cam [i] = NULL;
+			pr_err("ERROR: v4l2 capture: video_register_device failed\n");
+			return -1;
+		}
+		pr_debug("   Video device registered: %s #%d\n",
+			g_cam[i]->video_dev->name, g_cam[i]->video_dev->minor);
 	}
-	pr_debug("   Video device registered: %s #%d\n",
-		 g_cam->video_dev->name, g_cam->video_dev->minor);
 
 	return err;
 }
@@ -2683,19 +2795,34 @@ static void __exit camera_exit(void)
 
 	pr_info("V4L2 unregistering video\n");
 
-	if (g_cam->open_count) {
+	if (g_cam[0]->open_count) {
+		pr_err("ERROR: v4l2 capture:camera open "
+			"-- setting ops to NULL\n");
+	} else {
+		pr_info("V4L2 freeing image input device\n");
+		v4l2_int_device_unregister(&mxc_v4l2_int_device[0]);
+		video_unregister_device(g_cam[0]->video_dev);
+		platform_driver_unregister(&mxc_v4l2_driver[0]);
+		platform_device_unregister(&mxc_v4l2_devices[0]);
+
+		mxc_free_frame_buf(g_cam[0]);
+		kfree(g_cam[0]);
+		g_cam[0] = NULL;
+	}
+
+	if (g_cam[1]->open_count) {
 		pr_err("ERROR: v4l2 capture:camera open "
 			"-- setting ops to NULL\n");
 	} else {
 		pr_info("V4L2 freeing image input device\n");
-		v4l2_int_device_unregister(&mxc_v4l2_int_device);
-		video_unregister_device(g_cam->video_dev);
-		platform_driver_unregister(&mxc_v4l2_driver);
-		platform_device_unregister(&mxc_v4l2_devices);
-
-		mxc_free_frame_buf(g_cam);
-		kfree(g_cam);
-		g_cam = NULL;
+		v4l2_int_device_unregister(&mxc_v4l2_int_device[1]);
+		video_unregister_device(g_cam[1]->video_dev);
+		platform_driver_unregister(&mxc_v4l2_driver[1]);
+		platform_device_unregister(&mxc_v4l2_devices[1]);
+
+		mxc_free_frame_buf(g_cam[1]);
+		kfree(g_cam[1]);
+		g_cam[1] = NULL;
 	}
 }
 
diff --git a/drivers/media/video/mxc/capture/mxc_v4l2_capture.h b/drivers/media/video/mxc/capture/mxc_v4l2_capture.h
index 45a211a80a38..abaaaea48447 100644
--- a/drivers/media/video/mxc/capture/mxc_v4l2_capture.h
+++ b/drivers/media/video/mxc/capture/mxc_v4l2_capture.h
@@ -36,6 +36,7 @@
 #include <media/v4l2-dev.h>
 
 #define FRAME_NUM 3
+//#define FRAME_NUM 4
 
 /*!
  * v4l2 frame structure.
@@ -123,7 +124,7 @@ typedef struct _cam_data {
 	/* still image capture */
 	wait_queue_head_t still_queue;
 	int still_counter;
-	dma_addr_t still_buf;
+	dma_addr_t still_buf[2];
 	void *still_buf_vaddr;
 
 	/* overlay */
@@ -166,7 +167,7 @@ typedef struct _cam_data {
 	struct v4l2_rect crop_defrect;
 	struct v4l2_rect crop_current;
 
-	int (*enc_update_eba) (dma_addr_t eba, int *bufferNum);
+	int (*enc_update_eba) (int csi,dma_addr_t eba, int *bufferNum);
 	int (*enc_enable) (void *private);
 	int (*enc_disable) (void *private);
 	int (*enc_enable_csi) (void *private);
@@ -194,6 +195,7 @@ typedef struct _cam_data {
 	/* camera sensor interface */
 	struct camera_sensor *cam_sensor; 	/* old version */
 	struct v4l2_int_device *sensor;
+	struct timeval tv_wakeup; // TODO - for testing. Remove later
 } cam_data;
 
 #if defined(CONFIG_MXC_IPU_V1) || defined(CONFIG_VIDEO_MXC_EMMA_CAMERA) \
diff --git a/drivers/media/video/mxc/capture/ov3640.c b/drivers/media/video/mxc/capture/ov3640.c
index 29e61234e7f8..899945b7d071 100644
--- a/drivers/media/video/mxc/capture/ov3640.c
+++ b/drivers/media/video/mxc/capture/ov3640.c
@@ -1,5 +1,5 @@
 /*
- * Copyright 2005-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2005-2010 Freescale Semiconductor, Inc. All Rights Reserved.
  */
 
 /*
@@ -26,7 +26,7 @@
 #define OV3640_VOLTAGE_ANALOG               2800000
 #define OV3640_VOLTAGE_DIGITAL_CORE         1500000
 #define OV3640_VOLTAGE_DIGITAL_IO           1800000
-
+#define OV3640_VOLTAGE_DIGITAL_GPO	    2800000
 
 /* Check these values! */
 #define MIN_FPS 15
@@ -40,8 +40,8 @@ enum ov3640_mode {
 	ov3640_mode_MIN = 0,
 	ov3640_mode_VGA_640_480 = 0,
 	ov3640_mode_QVGA_320_240 = 1,
-	ov3640_mode_QXGA_2048_1536 = 2,
-	ov3640_mode_XGA_1024_768 = 3,
+	ov3640_mode_XGA_1024_768 = 2,
+	ov3640_mode_QXGA_2048_1536 = 3,
 	ov3640_mode_NTSC_720_480 = 4,
 	ov3640_mode_PAL_720_576 = 5,
 	ov3640_mode_MAX = 5
@@ -93,6 +93,44 @@ struct sensor {
 } ov3640_data;
 
 static struct reg_value ov3640_setting_15fps_QXGA_2048_1536[] = {
+#if 0
+	/* The true 15fps QXGA setting. */
+	{0x3012, 0x80, 0, 0}, {0x304d, 0x41, 0, 0}, {0x3087, 0x16, 0, 0},
+	{0x30aa, 0x45, 0, 0}, {0x30b0, 0xff, 0, 0}, {0x30b1, 0xff, 0, 0},
+	{0x30b2, 0x13, 0, 0}, {0x30d7, 0x10, 0, 0}, {0x309e, 0x00, 0, 0},
+	{0x3602, 0x26, 0, 0}, {0x3603, 0x4D, 0, 0}, {0x364c, 0x04, 0, 0},
+	{0x360c, 0x12, 0, 0}, {0x361e, 0x00, 0, 0}, {0x361f, 0x11, 0, 0},
+	{0x3633, 0x03, 0, 0}, {0x3629, 0x3c, 0, 0}, {0x300e, 0x33, 0, 0},
+	{0x300f, 0x21, 0, 0}, {0x3010, 0x20, 0, 0}, {0x3011, 0x00, 0, 0},
+	{0x304c, 0x81, 0, 0}, {0x3029, 0x47, 0, 0}, {0x3070, 0x00, 0, 0},
+	{0x3071, 0xEC, 0, 0}, {0x301C, 0x06, 0, 0}, {0x3072, 0x00, 0, 0},
+	{0x3073, 0xC5, 0, 0}, {0x301D, 0x07, 0, 0}, {0x3018, 0x38, 0, 0},
+	{0x3019, 0x30, 0, 0}, {0x301a, 0x61, 0, 0}, {0x307d, 0x00, 0, 0},
+	{0x3087, 0x02, 0, 0}, {0x3082, 0x20, 0, 0}, {0x303c, 0x08, 0, 0},
+	{0x303d, 0x18, 0, 0}, {0x303e, 0x06, 0, 0}, {0x303F, 0x0c, 0, 0},
+	{0x3030, 0x62, 0, 0}, {0x3031, 0x26, 0, 0}, {0x3032, 0xe6, 0, 0},
+	{0x3033, 0x6e, 0, 0}, {0x3034, 0xea, 0, 0}, {0x3035, 0xae, 0, 0},
+	{0x3036, 0xa6, 0, 0}, {0x3037, 0x6a, 0, 0}, {0x3015, 0x12, 0, 0},
+	{0x3014, 0x04, 0, 0}, {0x3013, 0xf7, 0, 0}, {0x3104, 0x02, 0, 0},
+	{0x3105, 0xfd, 0, 0}, {0x3106, 0x00, 0, 0}, {0x3107, 0xff, 0, 0},
+	{0x3308, 0xa5, 0, 0}, {0x3316, 0xff, 0, 0}, {0x3317, 0x00, 0, 0},
+	{0x3087, 0x02, 0, 0}, {0x3082, 0x20, 0, 0}, {0x3300, 0x13, 0, 0},
+	{0x3301, 0xd6, 0, 0}, {0x3302, 0xef, 0, 0}, {0x30b8, 0x20, 0, 0},
+	{0x30b9, 0x17, 0, 0}, {0x30ba, 0x04, 0, 0}, {0x30bb, 0x08, 0, 0},
+	{0x3100, 0x02, 0, 0}, {0x3304, 0x00, 0, 0}, {0x3400, 0x00, 0, 0},
+	{0x3404, 0x02, 0, 0}, {0x3020, 0x01, 0, 0}, {0x3021, 0x1d, 0, 0},
+	{0x3022, 0x00, 0, 0}, {0x3023, 0x0a, 0, 0}, {0x3024, 0x08, 0, 0},
+	{0x3025, 0x18, 0, 0}, {0x3026, 0x06, 0, 0}, {0x3027, 0x0c, 0, 0},
+	{0x335f, 0x68, 0, 0}, {0x3360, 0x18, 0, 0}, {0x3361, 0x0c, 0, 0},
+	{0x3362, 0x68, 0, 0}, {0x3363, 0x08, 0, 0}, {0x3364, 0x04, 0, 0},
+	{0x3403, 0x42, 0, 0}, {0x3088, 0x08, 0, 0}, {0x3089, 0x00, 0, 0},
+	{0x308a, 0x06, 0, 0}, {0x308b, 0x00, 0, 0}, {0x3507, 0x06, 0, 0},
+	{0x350a, 0x4f, 0, 0}, {0x3600, 0xc4, 0, 0},
+#endif
+	/*
+	 * Only support 7.5fps for QXGA to workaround screen tearing issue
+	 * for 15fps when capturing still image.
+	 */
 	{0x3012, 0x80, 0, 0}, {0x304d, 0x45, 0, 0}, {0x30a7, 0x5e, 0, 0},
 	{0x3087, 0x16, 0, 0}, {0x309c, 0x1a, 0, 0}, {0x30a2, 0xe4, 0, 0},
 	{0x30aa, 0x42, 0, 0}, {0x30b0, 0xff, 0, 0}, {0x30b1, 0xff, 0, 0},
@@ -118,25 +156,9 @@ static struct reg_value ov3640_setting_15fps_QXGA_2048_1536[] = {
 	{0x30bb, 0x08, 0, 0}, {0x3507, 0x06, 0, 0}, {0x350a, 0x4f, 0, 0},
 	{0x3100, 0x02, 0, 0}, {0x3301, 0xde, 0, 0}, {0x3304, 0x00, 0, 0},
 	{0x3400, 0x00, 0, 0}, {0x3404, 0x02, 0, 0}, {0x3600, 0xc4, 0, 0},
-	{0x3302, 0xef, 0, 0}, {0x3020, 0x01, 0, 0}, {0x3021, 0x1d, 0, 0},
-	{0x3022, 0x00, 0, 0}, {0x3023, 0x0a, 0, 0}, {0x3024, 0x08, 0, 0},
-	{0x3025, 0x00, 0, 0}, {0x3026, 0x06, 0, 0}, {0x3027, 0x00, 0, 0},
-	{0x335f, 0x68, 0, 0}, {0x3360, 0x00, 0, 0}, {0x3361, 0x00, 0, 0},
-	{0x3362, 0x68, 0, 0}, {0x3363, 0x00, 0, 0}, {0x3364, 0x00, 0, 0},
-	{0x3403, 0x00, 0, 0}, {0x3088, 0x08, 0, 0}, {0x3089, 0x00, 0, 0},
-	{0x308a, 0x06, 0, 0}, {0x308b, 0x00, 0, 0}, {0x307c, 0x10, 0, 0},
-	{0x3090, 0xc0, 0, 0}, {0x304c, 0x84, 0, 0}, {0x308d, 0x04, 0, 0},
-	{0x3086, 0x03, 0, 0}, {0x3086, 0x00, 0, 0}, {0x3012, 0x00, 0, 0},
-	{0x3020, 0x01, 0, 0}, {0x3021, 0x1d, 0, 0}, {0x3022, 0x00, 0, 0},
-	{0x3023, 0x0a, 0, 0}, {0x3024, 0x08, 0, 0}, {0x3025, 0x18, 0, 0},
-	{0x3026, 0x06, 0, 0}, {0x3027, 0x0c, 0, 0}, {0x302a, 0x06, 0, 0},
-	{0x302b, 0x20, 0, 0}, {0x3075, 0x44, 0, 0}, {0x300d, 0x00, 0, 0},
-	{0x30d7, 0x00, 0, 0}, {0x3069, 0x40, 0, 0}, {0x303e, 0x01, 0, 0},
-	{0x303f, 0x80, 0, 0}, {0x3302, 0x20, 0, 0}, {0x335f, 0x68, 0, 0},
-	{0x3360, 0x18, 0, 0}, {0x3361, 0x0c, 0, 0}, {0x3362, 0x68, 0, 0},
-	{0x3363, 0x08, 0, 0}, {0x3364, 0x04, 0, 0}, {0x3403, 0x42, 0, 0},
 	{0x3088, 0x08, 0, 0}, {0x3089, 0x00, 0, 0}, {0x308a, 0x06, 0, 0},
-	{0x308b, 0x00, 0, 0},
+	{0x308b, 0x00, 0, 0}, {0x308d, 0x04, 0, 0}, {0x3086, 0x03, 0, 0},
+	{0x3086, 0x00, 0, 0}, {0x3011, 0x01, 0, 0},
 };
 
 static struct reg_value ov3640_setting_15fps_XGA_1024_768[] = {
@@ -674,6 +696,7 @@ static struct regulator *io_regulator;
 static struct regulator *core_regulator;
 static struct regulator *analog_regulator;
 static struct regulator *gpo_regulator;
+static struct mxc_camera_platform_data *camera_plat;
 
 static int ov3640_probe(struct i2c_client *adapter,
 				const struct i2c_device_id *device_id);
@@ -843,6 +866,10 @@ static int ioctl_s_power(struct v4l2_int_device *s, int on)
 		if (analog_regulator)
 			if (regulator_enable(analog_regulator) != 0)
 				return -EIO;
+		/* Make sure power on */
+		if (camera_plat->pwdn)
+			camera_plat->pwdn(0);
+
 	} else if (!on && sensor->on) {
 		if (analog_regulator)
 			regulator_disable(analog_regulator);
@@ -920,6 +947,10 @@ static int ioctl_s_parm(struct v4l2_int_device *s, struct v4l2_streamparm *a)
 	enum ov3640_frame_rate frame_rate;
 	int ret = 0;
 
+	/* Make sure power on */
+	if (camera_plat->pwdn)
+		camera_plat->pwdn(0);
+
 	switch (a->type) {
 	/* This is the only case currently handled. */
 	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
@@ -1286,17 +1317,25 @@ static int ov3640_probe(struct i2c_client *client,
 		gpo_regulator = regulator_get(&client->dev,
 					      plat_data->gpo_regulator);
 		if (!IS_ERR(gpo_regulator)) {
+			regulator_set_voltage(gpo_regulator,
+					      OV3640_VOLTAGE_DIGITAL_GPO,
+					      OV3640_VOLTAGE_DIGITAL_GPO);
 			if (regulator_enable(gpo_regulator) != 0) {
-				pr_err("%s:gpo3 enable error\n", __func__);
+				pr_err("%s:gpo enable error\n", __func__);
 				goto err4;
 			} else {
 				dev_dbg(&client->dev,
-					"%s:gpo3 enable ok\n", __func__);
+					"%s:gpo enable ok\n", __func__);
 			}
 		} else
 			gpo_regulator = NULL;
 	}
 
+	if (plat_data->pwdn)
+		plat_data->pwdn(0);
+
+	camera_plat = plat_data;
+
 	ov3640_int_device.priv = &ov3640_data;
 	retval = v4l2_int_device_register(&ov3640_int_device);
 
diff --git a/drivers/media/video/mxc/output/Makefile b/drivers/media/video/mxc/output/Makefile
index 1713fa3bf3ab..500442544902 100644
--- a/drivers/media/video/mxc/output/Makefile
+++ b/drivers/media/video/mxc/output/Makefile
@@ -6,6 +6,9 @@ endif
 ifeq ($(CONFIG_VIDEO_MXC_IPU_OUTPUT),y)
 	obj-$(CONFIG_VIDEO_MXC_OUTPUT)	+= mxc_v4l2_output.o
 endif
+ifeq ($(CONFIG_VIDEO_MXC_PXP_V4L2),y)
+	obj-$(CONFIG_VIDEO_MXC_PXP_V4L2)	+= mxc_pxp_v4l2.o
+endif
 ifeq ($(CONFIG_VIDEO_MXC_IPUV1_WVGA_OUTPUT),y)
 	obj-$(CONFIG_VIDEO_MXC_OUTPUT)	+= mx31_v4l2_wvga_output.o
 endif
diff --git a/drivers/media/video/mxc/output/mxc_v4l2_output.c b/drivers/media/video/mxc/output/mxc_v4l2_output.c
index 408f9b7871a8..b7bbfed73019 100644
--- a/drivers/media/video/mxc/output/mxc_v4l2_output.c
+++ b/drivers/media/video/mxc/output/mxc_v4l2_output.c
@@ -44,9 +44,7 @@ vout_data *g_vout;
 #define LOAD_3FIELDS(vout) ((INTERLACED_CONTENT(vout)) && \
 			    ((vout)->motion_sel != HIGH_MOTION))
 
-#define SDC_FG_FB_FORMAT        IPU_PIX_FMT_RGB565
-
-struct v4l2_output mxc_outputs[2] = {
+struct v4l2_output mxc_outputs[1] = {
 	{
 	 .index = MXC_V4L2_OUT_2_SDC,
 	 .name = "DISP3 Video Out",
@@ -54,23 +52,14 @@ struct v4l2_output mxc_outputs[2] = {
 						   but no other choice */
 	 .audioset = 0,
 	 .modulator = 0,
-	 .std = V4L2_STD_UNKNOWN},
-	{
-	 .index = MXC_V4L2_OUT_2_ADC,
-	 .name = "DISPx Video Out",
-	 .type = V4L2_OUTPUT_TYPE_ANALOG,	/* not really correct,
-						   but no other choice */
-	 .audioset = 0,
-	 .modulator = 0,
 	 .std = V4L2_STD_UNKNOWN}
 };
 
 static int video_nr = 16;
 static spinlock_t g_lock = SPIN_LOCK_UNLOCKED;
 static int last_index_n;
-static int last_index_c;
 static unsigned int ipu_ic_out_max_width_size;
-
+static unsigned int ipu_ic_out_max_height_size;
 /* debug counters */
 uint32_t g_irq_cnt;
 uint32_t g_buf_output_cnt;
@@ -256,10 +245,14 @@ static int select_display_buffer(vout_data *vout, int next_buf)
 {
 	int ret = 0;
 
+	vout->disp_buf_num = next_buf;
 	if (ipu_get_cur_buffer_idx(vout->display_ch, IPU_INPUT_BUFFER)
 			!= next_buf)
 		ret = ipu_select_buffer(vout->display_ch, IPU_INPUT_BUFFER,
 				next_buf);
+	else
+		dev_dbg(&vout->video_dev->dev,
+			"display buffer not ready for select\n");
 	return ret;
 }
 
@@ -290,110 +283,138 @@ static void setup_next_buf_timer(vout_data *vout, int index)
 			"timer handler next schedule: %lu\n", timeout);
 }
 
-static int wait_for_disp_vsync(vout_data *vout)
+static int finish_previous_frame(vout_data *vout)
 {
 	struct fb_info *fbi =
 		registered_fb[vout->output_fb_num[vout->cur_disp_output]];
 	mm_segment_t old_fs;
 	int ret = 0;
 
-	/* wait for display frame finish */
-	if (fbi->fbops->fb_ioctl) {
-		old_fs = get_fs();
-		set_fs(KERNEL_DS);
-		ret = fbi->fbops->fb_ioctl(fbi, MXCFB_WAIT_FOR_VSYNC,
-			(unsigned int)NULL);
-		set_fs(old_fs);
+	/* make sure buf[vout->disp_buf_num] in showing */
+	while (ipu_check_buffer_busy(vout->display_ch,
+			IPU_INPUT_BUFFER, vout->disp_buf_num)) {
+		if (fbi->fbops->fb_ioctl) {
+			old_fs = get_fs();
+			set_fs(KERNEL_DS);
+			ret = fbi->fbops->fb_ioctl(fbi, MXCFB_WAIT_FOR_VSYNC,
+					(unsigned int)NULL);
+			set_fs(old_fs);
+
+			if (ret < 0) {
+				/* ic_bypass need clear display buffer ready for next update*/
+				ipu_clear_buffer_ready(vout->display_ch, IPU_INPUT_BUFFER,
+						vout->disp_buf_num);
+			}
+		}
+	}
+
+	return ret;
+}
+
+static int show_current_frame(vout_data *vout)
+{
+	struct fb_info *fbi =
+		registered_fb[vout->output_fb_num[vout->cur_disp_output]];
+	mm_segment_t old_fs;
+	int ret = 0;
+
+	/* make sure buf[vout->disp_buf_num] begin to show */
+	if (ipu_get_cur_buffer_idx(vout->display_ch, IPU_INPUT_BUFFER)
+		!= vout->disp_buf_num) {
+		/* wait for display frame finish */
+		if (fbi->fbops->fb_ioctl) {
+			old_fs = get_fs();
+			set_fs(KERNEL_DS);
+			ret = fbi->fbops->fb_ioctl(fbi, MXCFB_WAIT_FOR_VSYNC,
+					(unsigned int)NULL);
+			set_fs(old_fs);
+		}
 	}
+
 	return ret;
 }
 
-static void timer_work_func(struct work_struct *work)
+static void icbypass_work_func(struct work_struct *work)
 {
 	vout_data *vout =
-		container_of(work, vout_data, timer_work);
+		container_of(work, vout_data, icbypass_work);
 	int index, ret;
 	int last_buf;
 	unsigned long lock_flags = 0;
 
-	/* wait 2 first frame finish for ic bypass mode*/
-	if ((g_buf_output_cnt == 0) && vout->ic_bypass) {
-		wait_for_disp_vsync(vout);
-		wait_for_disp_vsync(vout);
-		spin_lock_irqsave(&g_lock, lock_flags);
-		last_buf = vout->ipu_buf[0];
-		vout->v4l2_bufs[last_buf].flags = V4L2_BUF_FLAG_DONE;
-		queue_buf(&vout->done_q, last_buf);
-		vout->ipu_buf[0] = -1;
-		last_buf = vout->ipu_buf[1];
-		vout->v4l2_bufs[last_buf].flags = V4L2_BUF_FLAG_DONE;
-		queue_buf(&vout->done_q, last_buf);
-		vout->ipu_buf[1] = -1;
-		g_buf_output_cnt = 2;
-		wake_up_interruptible(&vout->v4l_bufq);
-		if (vout->state == STATE_STREAM_PAUSED) {
-			index = peek_next_buf(&vout->ready_q);
-			if (index != -1) {
-				/* Setup timer for next buffer, when stream has been paused */
-				pr_debug("next index %d\n", index);
-				setup_next_buf_timer(vout, index);
-				vout->state = STATE_STREAM_ON;
-			}
-		}
-		spin_unlock_irqrestore(&g_lock, lock_flags);
-		return;
-	}
-
-	if (wait_for_disp_vsync(vout) < 0) {
-		/* ic_bypass need clear display buffer ready for next update*/
-		ipu_clear_buffer_ready(vout->display_ch, IPU_INPUT_BUFFER,
-			!vout->next_done_ipu_buf);
-	}
+	finish_previous_frame(vout);
 
 	spin_lock_irqsave(&g_lock, lock_flags);
 
-	if (vout->ic_bypass) {
-		last_buf = vout->ipu_buf[vout->next_done_ipu_buf];
-		if (last_buf != -1) {
-			g_buf_output_cnt++;
-			vout->v4l2_bufs[last_buf].flags = V4L2_BUF_FLAG_DONE;
-			queue_buf(&vout->done_q, last_buf);
-			wake_up_interruptible(&vout->v4l_bufq);
-			vout->ipu_buf[vout->next_done_ipu_buf] = -1;
-			vout->next_done_ipu_buf = !vout->next_done_ipu_buf;
-		}
+	index = dequeue_buf(&vout->ready_q);
+	if (index == -1) {	/* no buffers ready, should never occur */
+		dev_err(&vout->video_dev->dev,
+				"mxc_v4l2out: timer - no queued buffers ready\n");
+		goto exit;
 	}
+	g_buf_dq_cnt++;
+	vout->frame_count++;
 
-	if (vout->ic_bypass)
-		ret = select_display_buffer(vout, vout->next_rdy_ipu_buf);
-	else if (LOAD_3FIELDS(vout))
-		ret = ipu_select_multi_vdi_buffer(vout->next_rdy_ipu_buf);
-	else
-		ret = ipu_select_buffer(vout->post_proc_ch, IPU_INPUT_BUFFER,
-				vout->next_rdy_ipu_buf);
+	vout->ipu_buf[vout->next_rdy_ipu_buf] = index;
+	ret = ipu_update_channel_buffer(vout->display_ch, IPU_INPUT_BUFFER,
+			vout->next_rdy_ipu_buf,
+			vout->v4l2_bufs[index].m.offset);
+	ret += select_display_buffer(vout, vout->next_rdy_ipu_buf);
 	if (ret < 0) {
 		dev_err(&vout->video_dev->dev,
-				"unable to set IPU buffer ready\n");
+				"unable to update buffer %d address rc=%d\n",
+				vout->next_rdy_ipu_buf, ret);
+		goto exit;
 	}
+	spin_unlock_irqrestore(&g_lock, lock_flags);
+	show_current_frame(vout);
+	spin_lock_irqsave(&g_lock, lock_flags);
+	vout->next_rdy_ipu_buf = !vout->next_rdy_ipu_buf;
 
-	/* Non IC split action */
-	if (!vout->pp_split)
-		vout->next_rdy_ipu_buf = !vout->next_rdy_ipu_buf;
+	last_buf = vout->ipu_buf[vout->next_done_ipu_buf];
+	if (last_buf != -1) {
+		g_buf_output_cnt++;
+		vout->v4l2_bufs[last_buf].flags = V4L2_BUF_FLAG_DONE;
+		queue_buf(&vout->done_q, last_buf);
+		wake_up_interruptible(&vout->v4l_bufq);
+		vout->ipu_buf[vout->next_done_ipu_buf] = -1;
+		vout->next_done_ipu_buf = !vout->next_done_ipu_buf;
+	}
 
-	/* Setup timer for next buffer */
-	index = peek_next_buf(&vout->ready_q);
-	if (index != -1)
-		setup_next_buf_timer(vout, index);
-	else
-		vout->state = STATE_STREAM_PAUSED;
+	if (g_buf_output_cnt > 0) {
+		/* Setup timer for next buffer */
+		index = peek_next_buf(&vout->ready_q);
+		if (index != -1)
+			setup_next_buf_timer(vout, index);
+		else
+			vout->state = STATE_STREAM_PAUSED;
 
+		if (vout->state == STATE_STREAM_STOPPING) {
+			if ((vout->ipu_buf[0] == -1) && (vout->ipu_buf[1] == -1)) {
+				vout->state = STATE_STREAM_OFF;
+			}
+		}
+	}
+exit:
 	spin_unlock_irqrestore(&g_lock, lock_flags);
+}
 
-	if (vout->state == STATE_STREAM_STOPPING) {
-		if ((vout->ipu_buf[0] == -1) && (vout->ipu_buf[1] == -1)) {
-			vout->state = STATE_STREAM_OFF;
-		}
+static int get_cur_fb_blank(vout_data *vout)
+{
+	struct fb_info *fbi =
+		registered_fb[vout->output_fb_num[vout->cur_disp_output]];
+	mm_segment_t old_fs;
+	int ret = 0;
+
+	if (fbi->fbops->fb_ioctl) {
+		old_fs = get_fs();
+		set_fs(KERNEL_DS);
+		ret = fbi->fbops->fb_ioctl(fbi, MXCFB_GET_FB_BLANK,
+				(unsigned int)(&vout->fb_blank));
+		set_fs(old_fs);
 	}
+
+	return ret;
 }
 
 static void mxc_v4l2out_timer_handler(unsigned long arg)
@@ -401,13 +422,13 @@ static void mxc_v4l2out_timer_handler(unsigned long arg)
 	int index, ret;
 	unsigned long lock_flags = 0;
 	vout_data *vout = (vout_data *) arg;
-	unsigned int aid_field_offset = 0, current_field_offset = 0;
 
 	spin_lock_irqsave(&g_lock, lock_flags);
 
 	if ((vout->state == STATE_STREAM_STOPPING)
 	    || (vout->state == STATE_STREAM_OFF))
 		goto exit0;
+
 	/*
 	 * If timer occurs before IPU h/w is ready, then set the state to
 	 * paused and the timer will be set again when next buffer is queued
@@ -419,98 +440,92 @@ static void mxc_v4l2out_timer_handler(unsigned long arg)
 		goto exit0;
 	}
 
-	/* Dequeue buffer and pass to IPU */
-	if (INTERLACED_CONTENT(vout)) {
-		if (((LOAD_3FIELDS(vout)) && (vout->next_rdy_ipu_buf)) ||
-		    ((!LOAD_3FIELDS(vout)) && !(vout->next_rdy_ipu_buf))) {
-			aid_field_offset = vout->bytesperline;
-			current_field_offset = 0;
-			index = last_index_n;
-		} else {
-			aid_field_offset = 0;
-			current_field_offset = vout->bytesperline;
-			index = dequeue_buf(&vout->ready_q);
-			if (index == -1) {	/* no buffers ready, should never occur */
-				dev_err(&vout->video_dev->dev,
-					"mxc_v4l2out: timer - no queued buffers ready\n");
-				goto exit0;
-			}
-			g_buf_dq_cnt++;
-			vout->frame_count++;
-			last_index_n = index;
-		}
-	} else {
-		current_field_offset = 0;
-		index = dequeue_buf(&vout->ready_q);
-		if (index == -1) {	/* no buffers ready, should never occur */
+	/* VDI need both buffer done before update buffer? */
+	if (INTERLACED_CONTENT(vout) &&
+		(vout->ipu_buf[!vout->next_rdy_ipu_buf] != -1)) {
+		dev_dbg(&vout->video_dev->dev, "IPU buffer busy\n");
+		vout->state = STATE_STREAM_PAUSED;
+		goto exit0;
+	}
+
+	/* Handle ic bypass mode in work queue */
+	if (vout->ic_bypass) {
+		if (queue_work(vout->v4l_wq, &vout->icbypass_work) == 0) {
 			dev_err(&vout->video_dev->dev,
-				"mxc_v4l2out: timer - no queued buffers ready\n");
-			goto exit0;
+				"ic bypass work was in queue already!\n ");
+			vout->state = STATE_STREAM_PAUSED;
 		}
-		g_buf_dq_cnt++;
-		vout->frame_count++;
+		goto exit0;
+	} else if (!vout->fb_blank &&
+		(ipu_get_cur_buffer_idx(vout->display_ch, IPU_INPUT_BUFFER)
+		== vout->next_disp_ipu_buf)) {
+		dev_dbg(&vout->video_dev->dev, "IPU disp busy\n");
+		get_cur_fb_blank(vout);
+		index = peek_next_buf(&vout->ready_q);
+		setup_next_buf_timer(vout, index);
+		goto exit0;
 	}
+	vout->fb_blank = 0;
+
+	/* Dequeue buffer and pass to IPU */
+	index = dequeue_buf(&vout->ready_q);
+	if (index == -1) {	/* no buffers ready, should never occur */
+		dev_err(&vout->video_dev->dev,
+			"mxc_v4l2out: timer - no queued buffers ready\n");
+		goto exit0;
+	}
+	g_buf_dq_cnt++;
+	vout->frame_count++;
 
 	/* update next buffer */
-	if (vout->ic_bypass) {
+	if (LOAD_3FIELDS(vout)) {
+		int index_n = index;
+		int index_p = last_index_n;
+		vout->ipu_buf_p[vout->next_rdy_ipu_buf] = last_index_n;
 		vout->ipu_buf[vout->next_rdy_ipu_buf] = index;
-		ret = ipu_update_channel_buffer(vout->display_ch, IPU_INPUT_BUFFER,
-				      vout->next_rdy_ipu_buf,
-				      vout->v4l2_bufs[index].m.offset);
+		vout->ipu_buf_n[vout->next_rdy_ipu_buf] = index;
+		ret = ipu_update_channel_buffer(vout->post_proc_ch,
+				IPU_INPUT_BUFFER,
+				vout->next_rdy_ipu_buf,
+				vout->v4l2_bufs[index].m.offset);
+		ret += ipu_update_channel_buffer(MEM_VDI_PRP_VF_MEM_P,
+				IPU_INPUT_BUFFER,
+				vout->next_rdy_ipu_buf,
+				vout->v4l2_bufs[index_p].m.offset + vout->bytesperline);
+		ret += ipu_update_channel_buffer(MEM_VDI_PRP_VF_MEM_N,
+				IPU_INPUT_BUFFER,
+				vout->next_rdy_ipu_buf,
+				vout->v4l2_bufs[index_n].m.offset) + vout->bytesperline;
+		last_index_n = index;
 	} else {
-		if (LOAD_3FIELDS(vout)) {
-			int index_n = index;
-			int index_p = last_index_c;
-			index = last_index_n;
-			vout->ipu_buf_p[vout->next_rdy_ipu_buf] = index_p;
-			vout->ipu_buf[vout->next_rdy_ipu_buf] = last_index_c = index;
-			vout->ipu_buf_n[vout->next_rdy_ipu_buf] = last_index_n = index_n;
-			last_index_n = vout->ipu_buf_n[vout->next_rdy_ipu_buf];
-			last_index_c = vout->ipu_buf[vout->next_rdy_ipu_buf];
+		vout->ipu_buf[vout->next_rdy_ipu_buf] = index;
+		if (vout->pp_split) {
+			vout->ipu_buf[!vout->next_rdy_ipu_buf] = index;
+			/* always left stripe */
 			ret = ipu_update_channel_buffer(vout->post_proc_ch,
-							IPU_INPUT_BUFFER,
-							vout->next_rdy_ipu_buf,
-							vout->v4l2_bufs[index].m.offset+current_field_offset);
-			ret += ipu_update_channel_buffer(MEM_VDI_PRP_VF_MEM_P,
-							 IPU_INPUT_BUFFER,
-							 vout->next_rdy_ipu_buf,
-							 vout->v4l2_bufs[index_p].m.offset+aid_field_offset);
-			ret += ipu_update_channel_buffer(MEM_VDI_PRP_VF_MEM_N,
-							 IPU_INPUT_BUFFER,
-							 vout->next_rdy_ipu_buf,
-							 vout->v4l2_bufs[index_n].m.offset+aid_field_offset);
-		} else {
-			vout->ipu_buf[vout->next_rdy_ipu_buf] = index;
-			if (vout->pp_split) {
-				vout->ipu_buf[!vout->next_rdy_ipu_buf] = index;
-				/* always left stripe */
-				ret = ipu_update_channel_buffer(vout->post_proc_ch,
-									IPU_INPUT_BUFFER,
-									0,/* vout->next_rdy_ipu_buf,*/
-									(vout->v4l2_bufs[index].m.offset) +
-									vout->pp_left_stripe.input_column +
-									current_field_offset);
-
-				/* the U/V offset has to be updated inside of IDMAC */
-				/* according to stripe offset */
-				ret += ipu_update_channel_offset(vout->post_proc_ch,
-									IPU_INPUT_BUFFER,
-									vout->v2f.fmt.pix.pixelformat,
-									vout->v2f.fmt.pix.width,
-									vout->v2f.fmt.pix.height,
-									vout->bytesperline,
-									vout->offset.u_offset,
-									vout->offset.v_offset,
-									0,
-									vout->pp_left_stripe.input_column + current_field_offset);
-
-			} else
-				ret = ipu_update_channel_buffer(vout->post_proc_ch,
-									IPU_INPUT_BUFFER,
-									vout->next_rdy_ipu_buf,
-									vout->v4l2_bufs[index].m.offset +
-									current_field_offset);
-		}
+					IPU_INPUT_BUFFER,
+					0,/* vout->next_rdy_ipu_buf,*/
+					(vout->v4l2_bufs[index].m.offset) +
+					vout->pp_left_stripe.input_column +
+					vout->pp_up_stripe.input_column * vout->bytesperline);
+
+			/* the U/V offset has to be updated inside of IDMAC */
+			/* according to stripe offset */
+			ret += ipu_update_channel_offset(vout->post_proc_ch,
+					IPU_INPUT_BUFFER,
+					vout->v2f.fmt.pix.pixelformat,
+					vout->v2f.fmt.pix.width,
+					vout->v2f.fmt.pix.height,
+					vout->bytesperline,
+					vout->offset.u_offset,
+					vout->offset.v_offset,
+					vout->pp_up_stripe.input_column,
+					vout->pp_left_stripe.input_column);
+		} else
+			ret = ipu_update_channel_buffer(vout->post_proc_ch,
+					IPU_INPUT_BUFFER,
+					vout->next_rdy_ipu_buf,
+					vout->v4l2_bufs[index].m.offset);
 	}
 
 	if (ret < 0) {
@@ -521,9 +536,35 @@ static void mxc_v4l2out_timer_handler(unsigned long arg)
 	}
 
 	/* set next buffer ready */
-	if (queue_work(vout->v4l_wq, &vout->timer_work) == 0) {
-		dev_err(&vout->video_dev->dev, "work was in queue already!\n ");
+	if (LOAD_3FIELDS(vout))
+		ret = ipu_select_multi_vdi_buffer(vout->next_rdy_ipu_buf);
+	else
+		ret = ipu_select_buffer(vout->post_proc_ch, IPU_INPUT_BUFFER,
+				vout->next_rdy_ipu_buf);
+	if (ret < 0) {
+		dev_err(&vout->video_dev->dev,
+				"unable to set IPU buffer ready\n");
+		goto exit0;
+	}
+
+	/* Split mode use buf 0 only, no need swith buf */
+	if (!vout->pp_split)
+		vout->next_rdy_ipu_buf = !vout->next_rdy_ipu_buf;
+
+	/* Always assume display in double buffers */
+	vout->next_disp_ipu_buf = !vout->next_disp_ipu_buf;
+
+	/* Setup timer for next buffer */
+	index = peek_next_buf(&vout->ready_q);
+	if (index != -1)
+		setup_next_buf_timer(vout, index);
+	else
 		vout->state = STATE_STREAM_PAUSED;
+
+	if (vout->state == STATE_STREAM_STOPPING) {
+		if ((vout->ipu_buf[0] == -1) && (vout->ipu_buf[1] == -1)) {
+			vout->state = STATE_STREAM_OFF;
+		}
 	}
 
 	spin_unlock_irqrestore(&g_lock, lock_flags);
@@ -540,12 +581,15 @@ static irqreturn_t mxc_v4l2out_work_irq_handler(int irq, void *dev_id)
 	int index;
 	unsigned long lock_flags = 0;
 	vout_data *vout = dev_id;
-	int pp_out_buf_num = 0;
+	int pp_out_buf_left_right = 0;
 	int disp_buf_num = 0;
 	int disp_buf_num_next = 1;
+	int local_buffer = 0;
 	int pp_out_buf_offset = 0;
+	int pp_out_buf_up_down = 0;
 	int release_buffer = 0;
-	u32 eba_offset;
+	u32 eba_offset = 0;
+	u32 vertical_offset = 0;
 	u16 x_pos;
 	u16 y_pos;
 	int ret = -1;
@@ -563,17 +607,37 @@ static irqreturn_t mxc_v4l2out_work_irq_handler(int irq, void *dev_id)
 	if (last_buf != -1) {
 		/* If IC split mode on, update output buffer number */
 		if (vout->pp_split) {
-			pp_out_buf_num = vout->pp_split_buf_num & 1;/* left/right stripe */
-			disp_buf_num = vout->pp_split_buf_num >> 1;
-			disp_buf_num_next = ((vout->pp_split_buf_num+2) & 3) >> 1;
-			if (!pp_out_buf_num) {/* next buffer is right stripe*/
-				eba_offset = vout->pp_right_stripe.input_column;/*always right stripe*/
+			pp_out_buf_up_down = vout->pp_split_buf_num & 1;/* left/right stripe */
+			pp_out_buf_left_right = (vout->pp_split_buf_num >> 1) & 1; /* up/down */
+			local_buffer = (vout->pp_split == 1) ? pp_out_buf_up_down :
+					pp_out_buf_left_right;
+			disp_buf_num = vout->pp_split_buf_num >> 2;
+			disp_buf_num_next =
+					((vout->pp_split_buf_num + (vout->pp_split << 0x1)) & 7) >> 2;
+			if ((!pp_out_buf_left_right) ||
+				((!pp_out_buf_up_down) && (vout->pp_split == 1))) {
+				if (vout->pp_split == 1) {
+						eba_offset = ((pp_out_buf_left_right + pp_out_buf_up_down) & 1) ?
+									vout->pp_right_stripe.input_column :
+									vout->pp_left_stripe.input_column;
+						vertical_offset = pp_out_buf_up_down ?
+									vout->pp_up_stripe.input_column :
+									vout->pp_down_stripe.input_column;
+
+				} else {
+						eba_offset = pp_out_buf_left_right ?
+									vout->pp_left_stripe.input_column :
+									vout->pp_right_stripe.input_column;
+						vertical_offset = pp_out_buf_left_right ?
+									vout->pp_up_stripe.input_column :
+									vout->pp_down_stripe.input_column;
+				}
+
 				ret = ipu_update_channel_buffer(vout->post_proc_ch,
 						IPU_INPUT_BUFFER,
-						1, /* right stripe */
+						(1 - local_buffer),
 						(vout->v4l2_bufs[vout->ipu_buf[disp_buf_num]].m.offset)
-						+ eba_offset);
-
+						+ eba_offset + vertical_offset * vout->bytesperline);
 				ret += ipu_update_channel_offset(vout->post_proc_ch,
 								IPU_INPUT_BUFFER,
 								vout->v2f.fmt.pix.pixelformat,
@@ -582,68 +646,78 @@ static irqreturn_t mxc_v4l2out_work_irq_handler(int irq, void *dev_id)
 								vout->bytesperline,
 								vout->offset.u_offset,
 								vout->offset.v_offset,
-								0,
-								vout->pp_right_stripe.input_column);
+								vertical_offset,
+								eba_offset);
 
 				/* select right stripe */
-				ret += ipu_select_buffer(vout->post_proc_ch,
-												IPU_INPUT_BUFFER, 1);
+				ret += ipu_select_buffer(vout->post_proc_ch, IPU_INPUT_BUFFER,
+							(1 - local_buffer));
 				if (ret < 0)
 					dev_err(&vout->video_dev->dev,
 					"unable to set IPU buffer ready\n");
+			}
 
-				vout->ipu_buf[vout->next_done_ipu_buf] = -1;
-				vout->next_done_ipu_buf = !vout->next_done_ipu_buf;
+			/* offset for next buffer's EBA */
+			eba_offset = 0;
+			if (vout->pp_split == 1) {
+				pp_out_buf_offset = ((vout->pp_split_buf_num >> 1) & 1) ?
+								vout->pp_left_stripe.output_column :
+								vout->pp_right_stripe.output_column;
 
-			} else /* right stripe is done, run display refresh */
-				select_display_buffer(vout, disp_buf_num);
+				eba_offset = ((vout->pp_split_buf_num & 1) ?
+								vout->pp_down_stripe.output_column :
+								vout->pp_up_stripe.output_column);
 
-			vout->next_rdy_ipu_buf = !vout->next_rdy_ipu_buf;
+			} else {
+				pp_out_buf_offset = ((vout->pp_split_buf_num >> 1) & 1) ?
+									vout->pp_right_stripe.output_column :
+									vout->pp_left_stripe.output_column;
+				eba_offset = ((vout->pp_split_buf_num >> 1) & 1) ?
+									vout->pp_down_stripe.output_column :
+									vout->pp_up_stripe.output_column;
+			}
 
-			/* offset for next buffer's EBA */
-			pp_out_buf_offset = pp_out_buf_num ? vout->pp_right_stripe.output_column :
-				vout->pp_left_stripe.output_column;
-			eba_offset = 0;
 			if (vout->cur_disp_output == 5) {
 				x_pos = (vout->crop_current.left / 8) * 8;
 				y_pos = vout->crop_current.top;
-				eba_offset = (vout->xres * y_pos + x_pos) * vout->bpp / 8;
+				eba_offset += (vout->xres * y_pos + x_pos) * vout->bpp / 8;
 			}
 
+
 			/* next buffer update */
 			eba_offset = vout->display_bufs[disp_buf_num_next] +
-								pp_out_buf_offset + eba_offset;
+						pp_out_buf_offset + eba_offset;
 
 			ipu_update_channel_buffer(vout->post_proc_ch, IPU_OUTPUT_BUFFER,
-												pp_out_buf_num, eba_offset);
+						local_buffer, eba_offset);
 
 			/* next buffer ready */
-			ret = ipu_select_buffer(vout->post_proc_ch, IPU_OUTPUT_BUFFER, pp_out_buf_num);
+			ret = ipu_select_buffer(vout->post_proc_ch, IPU_OUTPUT_BUFFER, local_buffer);
 
-			/* next stripe_buffer index 0..3 */
-			vout->pp_split_buf_num = (vout->pp_split_buf_num + 1) & 3;
+			/* next stripe_buffer index 0..7 */
+			vout->pp_split_buf_num = (vout->pp_split_buf_num + vout->pp_split) & 0x7;
 		} else {
-			/* show to display */
-			select_display_buffer(vout, vout->next_done_ipu_buf);
+			disp_buf_num = vout->next_done_ipu_buf;
 			ret += ipu_select_buffer(vout->display_input_ch, IPU_OUTPUT_BUFFER,
 					vout->next_done_ipu_buf);
 		}
 
 		/* release buffer. For split mode: if second stripe is done */
-		release_buffer = vout->pp_split ? pp_out_buf_num : 1;
+		release_buffer = vout->pp_split ? (!(vout->pp_split_buf_num & 0x3)) : 1;
 		if (release_buffer) {
-			if ((!INTERLACED_CONTENT(vout)) || (vout->next_done_ipu_buf)) {
-				g_buf_output_cnt++;
-				vout->v4l2_bufs[last_buf].flags = V4L2_BUF_FLAG_DONE;
-				queue_buf(&vout->done_q, last_buf);
-				wake_up_interruptible(&vout->v4l_bufq);
-			}
+			select_display_buffer(vout, disp_buf_num);
+			g_buf_output_cnt++;
+			vout->v4l2_bufs[last_buf].flags = V4L2_BUF_FLAG_DONE;
+			queue_buf(&vout->done_q, last_buf);
+			wake_up_interruptible(&vout->v4l_bufq);
 			vout->ipu_buf[vout->next_done_ipu_buf] = -1;
 			if (LOAD_3FIELDS(vout)) {
 				vout->ipu_buf_p[vout->next_done_ipu_buf] = -1;
 				vout->ipu_buf_n[vout->next_done_ipu_buf] = -1;
 			}
-			vout->next_done_ipu_buf = !vout->next_done_ipu_buf;
+			/* split mode use buf 0 only, no need switch buf */
+			if (!vout->pp_split)
+				vout->next_done_ipu_buf = !vout->next_done_ipu_buf;
 		}
 	} /* end of last_buf != -1 */
 
@@ -702,7 +776,6 @@ static int init_VDI_channel(vout_data *vout, ipu_channel_params_t params)
 static int init_VDI_in_channel_buffer(vout_data *vout, uint32_t in_pixel_fmt,
 				   uint16_t in_width, uint16_t in_height,
 				   uint32_t stride,
-				   dma_addr_t phyaddr_0, dma_addr_t phyaddr_1,
 				   uint32_t u_offset, uint32_t v_offset)
 {
 	struct device *dev = &vout->video_dev->dev;
@@ -710,7 +783,7 @@ static int init_VDI_in_channel_buffer(vout_data *vout, uint32_t in_pixel_fmt,
 	if (ipu_init_channel_buffer(MEM_VDI_PRP_VF_MEM, IPU_INPUT_BUFFER,
 				    in_pixel_fmt, in_width, in_height, stride,
 				    IPU_ROTATE_NONE,
-				    vout->v4l2_bufs[vout->ipu_buf[0]].m.offset+vout->bytesperline,
+				    vout->v4l2_bufs[vout->ipu_buf[0]].m.offset,
 				    vout->v4l2_bufs[vout->ipu_buf[0]].m.offset,
 				    u_offset, v_offset) != 0) {
 		dev_err(dev, "Error initializing VDI current input buffer\n");
@@ -721,7 +794,7 @@ static int init_VDI_in_channel_buffer(vout_data *vout, uint32_t in_pixel_fmt,
 					    IPU_INPUT_BUFFER,
 					    in_pixel_fmt, in_width, in_height,
 					    stride, IPU_ROTATE_NONE,
-					    vout->v4l2_bufs[vout->ipu_buf_p[0]].m.offset,
+					    vout->v4l2_bufs[vout->ipu_buf_p[0]].m.offset+vout->bytesperline,
 					    vout->v4l2_bufs[vout->ipu_buf_p[0]].m.offset+vout->bytesperline,
 					    u_offset, v_offset) != 0) {
 			dev_err(dev, "Error initializing VDI previous input buffer\n");
@@ -731,7 +804,7 @@ static int init_VDI_in_channel_buffer(vout_data *vout, uint32_t in_pixel_fmt,
 					    IPU_INPUT_BUFFER,
 					    in_pixel_fmt, in_width, in_height,
 					    stride, IPU_ROTATE_NONE,
-					    vout->v4l2_bufs[vout->ipu_buf_n[0]].m.offset,
+					    vout->v4l2_bufs[vout->ipu_buf_n[0]].m.offset+vout->bytesperline,
 					    vout->v4l2_bufs[vout->ipu_buf_n[0]].m.offset+vout->bytesperline,
 					    u_offset, v_offset) != 0) {
 			dev_err(dev, "Error initializing VDI next input buffer\n");
@@ -759,10 +832,7 @@ static int init_VDI(ipu_channel_params_t params, vout_data *vout,
 	params.mem_prp_vf_mem.in_pixel_fmt = vout->v2f.fmt.pix.pixelformat;
 	params.mem_prp_vf_mem.out_width = out_width;
 	params.mem_prp_vf_mem.out_height = out_height;
-	if (vout->display_ch == ADC_SYS2)
-		params.mem_prp_vf_mem.out_pixel_fmt = SDC_FG_FB_FORMAT;
-	else
-		params.mem_prp_vf_mem.out_pixel_fmt = bpp_to_fmt(fbi);
+	params.mem_prp_vf_mem.out_pixel_fmt = bpp_to_fmt(fbi);
 
 	if (init_VDI_channel(vout, params) != 0) {
 		dev_err(dev, "Error init_VDI_channel channel\n");
@@ -775,8 +845,6 @@ static int init_VDI(ipu_channel_params_t params, vout_data *vout,
 				       params.mem_prp_vf_mem.in_height,
 				       bytes_per_pixel(params.mem_prp_vf_mem.
 						       in_pixel_fmt),
-				       vout->v4l2_bufs[vout->ipu_buf[0]].m.offset,
-				       vout->v4l2_bufs[vout->ipu_buf[1]].m.offset,
 				       vout->offset.u_offset,
 				       vout->offset.v_offset) != 0) {
 		return -EINVAL;
@@ -805,7 +873,6 @@ static int init_VDI(ipu_channel_params_t params, vout_data *vout,
 			dev_err(dev, "Error initializing PRP output buffer\n");
 			return -EINVAL;
 		}
-
 		if (ipu_init_channel(MEM_ROT_VF_MEM, NULL) != 0) {
 			dev_err(dev, "Error initializing PP ROT channel\n");
 			return -EINVAL;
@@ -883,14 +950,14 @@ static int init_PP(ipu_channel_params_t *params, vout_data *vout,
 	u32 eba_offset;
 	u16 x_pos;
 	u16 y_pos;
+	dma_addr_t phy_addr0;
+	dma_addr_t phy_addr1;
+
 	eba_offset = 0;
 	x_pos = 0;
 	y_pos = 0;
 
-	if (vout->display_ch == ADC_SYS2)
-		params->mem_pp_mem.out_pixel_fmt = SDC_FG_FB_FORMAT;
-	else
-		params->mem_pp_mem.out_pixel_fmt = bpp_to_fmt(fbi);
+	params->mem_pp_mem.out_pixel_fmt = bpp_to_fmt(fbi);
 
 	if (vout->cur_disp_output == 5) {
 		x_pos = (vout->crop_current.left / 8) * 8;
@@ -907,37 +974,66 @@ static int init_PP(ipu_channel_params_t *params, vout_data *vout,
 	params->mem_pp_mem.in_pixel_fmt = vout->v2f.fmt.pix.pixelformat;
 	params->mem_pp_mem.out_width = out_width;
 	params->mem_pp_mem.out_height = out_height;
-	params->mem_pp_mem.out_resize_ratio = 0; /* 0 means unused */
-
+	params->mem_pp_mem.outh_resize_ratio = 0; /* 0 means unused */
+	params->mem_pp_mem.outv_resize_ratio = 0; /* 0 means unused */
 	/* split IC by two stripes, the by pass is impossible*/
 	if (vout->pp_split) {
-		ipu_calc_stripes_sizes(
-					params->mem_pp_mem.in_width, /* input frame width;>1 */
-					params->mem_pp_mem.out_width, /* output frame width; >1 */
-					ipu_ic_out_max_width_size,
-					(((unsigned long long)1) << 32), /* 32bit for fractional*/
-					1,	/* equal stripes */
-					params->mem_pp_mem.in_pixel_fmt,
-					params->mem_pp_mem.out_pixel_fmt,
-					&(vout->pp_left_stripe),
-					&(vout->pp_right_stripe));
-
-		vout->pp_left_stripe.input_column = vout->pp_left_stripe.input_column *
+		vout->pp_left_stripe.input_column = 0;
+		vout->pp_left_stripe.output_column = 0;
+		vout->pp_right_stripe.input_column = 0;
+		vout->pp_right_stripe.output_column = 0;
+		vout->pp_up_stripe.input_column = 0;
+		vout->pp_up_stripe.output_column = 0;
+		vout->pp_down_stripe.input_column = 0;
+		vout->pp_down_stripe.output_column = 0;
+		if (vout->pp_split != 3) {
+			ipu_calc_stripes_sizes(
+				params->mem_pp_mem.in_width, /* input frame width;>1 */
+				params->mem_pp_mem.out_width, /* output frame width; >1 */
+				ipu_ic_out_max_width_size,
+				(((unsigned long long)1) << 32), /* 32bit for fractional*/
+				1,	/* equal stripes */
+				params->mem_pp_mem.in_pixel_fmt,
+				params->mem_pp_mem.out_pixel_fmt,
+				&(vout->pp_left_stripe),
+				&(vout->pp_right_stripe));
+
+			vout->pp_left_stripe.input_column = vout->pp_left_stripe.input_column *
 								fmt_to_bpp(vout->v2f.fmt.pix.pixelformat) / 8;
-		vout->pp_left_stripe.output_column = vout->pp_left_stripe.output_column *
+			vout->pp_left_stripe.output_column = vout->pp_left_stripe.output_column *
 								fmt_to_bpp(params->mem_pp_mem.out_pixel_fmt) / 8;
-		vout->pp_right_stripe.input_column = vout->pp_right_stripe.input_column *
+			vout->pp_right_stripe.input_column = vout->pp_right_stripe.input_column *
 								fmt_to_bpp(vout->v2f.fmt.pix.pixelformat) / 8;
-		vout->pp_right_stripe.output_column = vout->pp_right_stripe.output_column *
+			vout->pp_right_stripe.output_column = vout->pp_right_stripe.output_column *
 								fmt_to_bpp(params->mem_pp_mem.out_pixel_fmt) / 8;
 
+
 		/* updare parameters */
 		params->mem_pp_mem.in_width = vout->pp_left_stripe.input_width;
 		params->mem_pp_mem.out_width = vout->pp_left_stripe.output_width;
 		out_width = vout->pp_left_stripe.output_width;
 		/* for using in ic_init*/
-		params->mem_pp_mem.out_resize_ratio = vout->pp_left_stripe.irr;
-
+		params->mem_pp_mem.outh_resize_ratio = vout->pp_left_stripe.irr;
+		}
+		if (vout->pp_split != 2) {
+			ipu_calc_stripes_sizes(
+				params->mem_pp_mem.in_height, /* input frame width;>1 */
+				params->mem_pp_mem.out_height, /* output frame width; >1 */
+				ipu_ic_out_max_height_size,
+				(((unsigned long long)1) << 32),/* 32bit for fractional */
+				1,	/* equal stripes */
+				params->mem_pp_mem.in_pixel_fmt,
+				params->mem_pp_mem.out_pixel_fmt,
+				&(vout->pp_up_stripe),
+				&(vout->pp_down_stripe));
+			vout->pp_down_stripe.output_column = vout->pp_down_stripe.output_column * out_stride;
+			vout->pp_up_stripe.output_column = vout->pp_up_stripe.output_column * out_stride;
+			params->mem_pp_mem.outv_resize_ratio = vout->pp_up_stripe.irr;
+			params->mem_pp_mem.in_height = vout->pp_up_stripe.input_width;/*height*/
+			out_height = vout->pp_up_stripe.output_width;/*height*/
+			if (vout->pp_split == 3)
+				vout->pp_split = 2;/*2 vertical stripe as two horizontal stripes */
+		}
 		vout->pp_split_buf_num = 0;
 	}
 
@@ -946,6 +1042,12 @@ static int init_PP(ipu_channel_params_t *params, vout_data *vout,
 		return -EINVAL;
 	}
 
+	/* always enable double buffer */
+	phy_addr0 = vout->v4l2_bufs[vout->ipu_buf[0]].m.offset;
+	if (vout->ipu_buf[1] == -1)
+		phy_addr1 = phy_addr0;
+	else
+		phy_addr1 = vout->v4l2_bufs[vout->ipu_buf[1]].m.offset;
 	if (ipu_init_channel_buffer(vout->post_proc_ch,
 				    IPU_INPUT_BUFFER,
 				    params->mem_pp_mem.in_pixel_fmt,
@@ -955,8 +1057,8 @@ static int init_PP(ipu_channel_params_t *params, vout_data *vout,
 				    bytes_per_pixel(params->mem_pp_mem.
 					    in_pixel_fmt),
 				    IPU_ROTATE_NONE,
-				    vout->v4l2_bufs[vout->ipu_buf[0]].m.offset,
-				    vout->v4l2_bufs[vout->ipu_buf[1]].m.offset,
+				    phy_addr0,
+				    phy_addr1,
 				    vout->offset.u_offset,
 				    vout->offset.v_offset) != 0) {
 		dev_err(dev, "Error initializing PP input buffer\n");
@@ -1048,20 +1150,27 @@ static int init_PP(ipu_channel_params_t *params, vout_data *vout,
 		ipu_update_channel_buffer(vout->post_proc_ch, IPU_INPUT_BUFFER,
 									0,
 									vout->v4l2_bufs[vout->ipu_buf[0]].m.offset +
-									vout->pp_left_stripe.input_column);
+									vout->pp_left_stripe.input_column +
+									vout->pp_up_stripe.input_column * vout->bytesperline);
+
+
 		ipu_update_channel_buffer(vout->post_proc_ch, IPU_INPUT_BUFFER,
 									1,
 									vout->v4l2_bufs[vout->ipu_buf[0]].m.offset +
-									vout->pp_right_stripe.input_column);
+									vout->pp_right_stripe.input_column +
+									vout->pp_up_stripe.input_column * vout->bytesperline);
+
 		ipu_update_channel_buffer(vout->post_proc_ch, IPU_OUTPUT_BUFFER,
 									0,
 									vout->display_bufs[0] + eba_offset +
-									vout->pp_left_stripe.output_column);
+									vout->pp_left_stripe.output_column +
+									vout->pp_up_stripe.output_column);
 
 		ipu_update_channel_buffer(vout->post_proc_ch, IPU_OUTPUT_BUFFER,
 									1,
 									vout->display_bufs[0] + eba_offset +
-									vout->pp_right_stripe.output_column);
+									vout->pp_right_stripe.output_column +
+									vout->pp_up_stripe.output_column);
 	}
 
 	return 0;
@@ -1084,8 +1193,8 @@ static int mxc_v4l2out_streamon(vout_data * vout)
 	    registered_fb[vout->output_fb_num[vout->cur_disp_output]];
 	u16 out_width;
 	u16 out_height;
-	bool use_direct_adc = false;
 	mm_segment_t old_fs;
+	unsigned int ipu_ch = CHAN_NONE;
 	int rc = 0;
 
 	dev_dbg(dev, "mxc_v4l2out_streamon: field format=%d\n",
@@ -1107,241 +1216,132 @@ static int mxc_v4l2out_streamon(vout_data * vout)
 		return -EINVAL;
 	}
 
+	/*
+	 * params init, check whether operation exceed the IC limitation:
+	 * whether split mode used ( ipu version >= ipuv3 only)
+	 */
 	g_irq_cnt = g_buf_output_cnt = g_buf_q_cnt = g_buf_dq_cnt = 0;
 	out_width = vout->crop_current.width;
 	out_height = vout->crop_current.height;
+	vout->disp_buf_num = 0;
 	vout->next_done_ipu_buf = 0;
-	vout->next_rdy_ipu_buf = 1;
+	vout->next_rdy_ipu_buf = vout->next_disp_ipu_buf = 1;
+	vout->fb_blank = 0;
 	vout->pp_split = 0;
-
-	if (!INTERLACED_CONTENT(vout)) {
-		vout->next_done_ipu_buf = vout->next_rdy_ipu_buf = 0;
-		vout->ipu_buf[0] = dequeue_buf(&vout->ready_q);
+	ipu_ic_out_max_height_size = 1024;
 #ifdef CONFIG_MXC_IPU_V1
+	if (cpu_is_mx35())
 		ipu_ic_out_max_width_size = 800;
+	else
+		ipu_ic_out_max_width_size = 720;
 #else
-		ipu_ic_out_max_width_size = 1024;
+	ipu_ic_out_max_width_size = 1024;
 #endif
+	if ((out_width > ipu_ic_out_max_width_size) ||
+		(out_height > ipu_ic_out_max_height_size))
+		vout->pp_split = 4;
+	if (!INTERLACED_CONTENT(vout)) {
+		vout->ipu_buf[0] = dequeue_buf(&vout->ready_q);
 		/* split IC by two stripes, the by pass is impossible*/
 		if ((out_width != vout->v2f.fmt.pix.width ||
 			out_height != vout->v2f.fmt.pix.height) &&
-			out_width > ipu_ic_out_max_width_size) {
-			vout->pp_split = 1;
+			vout->pp_split) {
+			vout->next_done_ipu_buf = vout->next_rdy_ipu_buf = 0;
 			vout->ipu_buf[1] = vout->ipu_buf[0];
 			vout->frame_count = 1;
+			if ((out_width > ipu_ic_out_max_width_size) &&
+				(out_height > ipu_ic_out_max_height_size))
+				vout->pp_split = 1; /*4 stripes*/
+			else if (!(out_height > ipu_ic_out_max_height_size))
+				vout->pp_split = 2; /*two horizontal stripes */
+			else
+				vout->pp_split = 3; /*2 vertical stripes*/
 		} else {
-			vout->ipu_buf[1] = dequeue_buf(&vout->ready_q);
-			vout->frame_count = 2;
+			vout->ipu_buf[1] = -1;
+			vout->frame_count = 1;
 		}
 	} else if (!LOAD_3FIELDS(vout)) {
 		vout->ipu_buf[0] = dequeue_buf(&vout->ready_q);
 		vout->ipu_buf[1] = -1;
 		vout->frame_count = 1;
-		last_index_n = vout->ipu_buf[0];
 	} else {
 		vout->ipu_buf_p[0] = dequeue_buf(&vout->ready_q);
-		vout->ipu_buf[0] = vout->ipu_buf_p[0];
-		vout->ipu_buf_n[0] = dequeue_buf(&vout->ready_q);
+		vout->ipu_buf[0] = dequeue_buf(&vout->ready_q);
+		vout->ipu_buf_n[0] = vout->ipu_buf[0];
 		vout->ipu_buf_p[1] = -1;
 		vout->ipu_buf[1] = -1;
 		vout->ipu_buf_n[1] = -1;
-		last_index_c = vout->ipu_buf[0];
 		last_index_n = vout->ipu_buf_n[0];
 		vout->frame_count = 2;
 	}
 
-	/* Init Display Channel */
-#ifdef CONFIG_FB_MXC_ASYNC_PANEL
-	if (vout->cur_disp_output < DISP3) {
-		vout->work_irq = IPU_IRQ_PP_IN_EOF;
-		ipu_clear_irq(vout->work_irq);
-		ipu_request_irq(vout->work_irq,
-				mxc_v4l2out_work_irq_handler,
-				0, vout->video_dev->name, vout);
-		mxcfb_set_refresh_mode(fbi, MXCFB_REFRESH_OFF, 0);
-		fbi = NULL;
-		if (ipu_can_rotate_in_place(vout->rotate)) {
-			dev_dbg(dev, "Using PP direct to ADC channel\n");
-			use_direct_adc = true;
-			vout->display_ch = MEM_PP_ADC;
-			vout->post_proc_ch = MEM_PP_ADC;
-
-			memset(&params, 0, sizeof(params));
-			params.mem_pp_adc.in_width = vout->v2f.fmt.pix.width;
-			params.mem_pp_adc.in_height = vout->v2f.fmt.pix.height;
-			params.mem_pp_adc.in_pixel_fmt =
-			    vout->v2f.fmt.pix.pixelformat;
-			params.mem_pp_adc.out_width = out_width;
-			params.mem_pp_adc.out_height = out_height;
-			params.mem_pp_adc.out_pixel_fmt = SDC_FG_FB_FORMAT;
-#ifdef CONFIG_FB_MXC_EPSON_PANEL
-			params.mem_pp_adc.out_left =
-			    2 + vout->crop_current.left;
-#else
-			params.mem_pp_adc.out_left =
-			    12 + vout->crop_current.left;
-#endif
-			params.mem_pp_adc.out_top = vout->crop_current.top;
-			if (ipu_init_channel(vout->post_proc_ch, &params) != 0) {
-				dev_err(dev, "Error initializing PP chan\n");
-				return -EINVAL;
-			}
-			if (ipu_init_channel_buffer(vout->post_proc_ch,
-						    IPU_INPUT_BUFFER,
-						    params.mem_pp_adc.
-						    in_pixel_fmt,
-						    params.mem_pp_adc.in_width,
-						    params.mem_pp_adc.in_height,
-						    vout->v2f.fmt.pix.
-						    bytesperline /
-						    bytes_per_pixel(params.
-								    mem_pp_adc.
-								    in_pixel_fmt),
-						    vout->rotate,
-						    vout->v4l2_bufs[vout->ipu_buf[0]].m.offset,
-						    vout->v4l2_bufs[vout->ipu_buf[1]].m.offset,
-						    vout->offset.u_offset,
-						    vout->offset.v_offset) !=
-			    0) {
-				dev_err(dev, "Error initializing PP in buf\n");
-				return -EINVAL;
-			}
-
-			if (ipu_init_channel_buffer(vout->post_proc_ch,
-						    IPU_OUTPUT_BUFFER,
-						    params.mem_pp_adc.
-						    out_pixel_fmt, out_width,
-						    out_height, out_width,
-						    vout->rotate, 0, 0, 0,
-						    0) != 0) {
-				dev_err(dev,
-					"Error initializing PP output buffer\n");
-				return -EINVAL;
-			}
-
-		} else {
-			dev_dbg(dev, "Using ADC SYS2 channel\n");
-			vout->display_ch = ADC_SYS2;
-			vout->post_proc_ch = MEM_PP_MEM;
-
-			if (vout->display_bufs[0]) {
-				mxc_free_buffers(vout->display_bufs,
-						 vout->display_bufs_vaddr,
-						 2, vout->display_buf_size);
-			}
-
-			vout->display_buf_size = vout->crop_current.width *
-			    vout->crop_current.height *
-			    fmt_to_bpp(SDC_FG_FB_FORMAT) / 8;
-			mxc_allocate_buffers(vout->display_bufs,
-					     vout->display_bufs_vaddr,
-					     2, vout->display_buf_size);
-
-			memset(&params, 0, sizeof(params));
-			params.adc_sys2.disp = vout->cur_disp_output;
-			params.adc_sys2.ch_mode = WriteTemplateNonSeq;
-#ifdef CONFIG_FB_MXC_EPSON_PANEL
-			params.adc_sys2.out_left = 2 + vout->crop_current.left;
-#else
-			params.adc_sys2.out_left = 12 + vout->crop_current.left;
-#endif
-			params.adc_sys2.out_top = vout->crop_current.top;
-			if (ipu_init_channel(ADC_SYS2, &params) < 0)
-				return -EINVAL;
-
-			if (ipu_init_channel_buffer(vout->display_ch,
-						    IPU_INPUT_BUFFER,
-						    SDC_FG_FB_FORMAT,
-						    out_width, out_height,
-						    out_width, IPU_ROTATE_NONE,
-						    vout->display_bufs[0],
-						    vout->display_bufs[1], 0,
-						    0) != 0) {
-				dev_err(dev,
-					"Error initializing SDC FG buffer\n");
-				return -EINVAL;
-			}
-		}
-	} else
-#endif
-	{			/* Use SDC */
-		unsigned int ipu_ch = CHAN_NONE;
-
-		dev_dbg(dev, "Using SDC channel\n");
-
-		if (INTERLACED_CONTENT(vout))
-			vout->work_irq = IPU_IRQ_PRP_VF_OUT_EOF;
-		else
-			vout->work_irq = IPU_IRQ_PP_IN_EOF;
-
-		/*
-		 * Bypass IC if resizing and rotation are not needed
-		 * Meanwhile, apply IC bypass to SDC only
-		 */
-		fbvar = fbi->var;
-		vout->xres = fbvar.xres;
-		vout->yres = fbvar.yres;
-
-		if (vout->cur_disp_output == 3 || vout->cur_disp_output == 5) {
-			fbvar.bits_per_pixel = 16;
+	/*
+	 * Bypass IC if resizing and rotation are not needed
+	 * Meanwhile, apply IC bypass to SDC only
+	 */
+	fbvar = fbi->var;
+	vout->xres = fbvar.xres;
+	vout->yres = fbvar.yres;
+
+	if (vout->cur_disp_output == 3 || vout->cur_disp_output == 5) {
+		fbvar.bits_per_pixel = 16;
+		if (vout->cur_disp_output == 3) {
+			/* Only set YUV for the first display. The second display can
+			 * only work in RGB */
 			if (format_is_yuv(vout->v2f.fmt.pix.pixelformat))
 				fbvar.nonstd = IPU_PIX_FMT_UYVY;
 			else
 				fbvar.nonstd = 0;
-			if (vout->cur_disp_output == 3) {
-				fbvar.xres = out_width;
-				fbvar.yres = out_height;
-				vout->xres = fbvar.xres;
-				vout->yres = fbvar.yres;
-			}
 
-			fbvar.xres_virtual = fbvar.xres;
-			fbvar.yres_virtual = fbvar.yres * 2;
+			fbvar.xres = out_width;
+			fbvar.yres = out_height;
+			vout->xres = fbvar.xres;
+			vout->yres = fbvar.yres;
 		}
 
-		if (out_width == vout->v2f.fmt.pix.width &&
-			out_height == vout->v2f.fmt.pix.height &&
-			vout->xres == out_width &&
-			vout->yres == out_height &&
-			ipu_can_rotate_in_place(vout->rotate)) {
-			vout->ic_bypass = 1;
-		} else {
-			vout->ic_bypass = 0;
-		}
+		fbvar.xres_virtual = fbvar.xres;
+		fbvar.yres_virtual = fbvar.yres * 2;
+	}
+
+	if (out_width == vout->v2f.fmt.pix.width &&
+	    out_height == vout->v2f.fmt.pix.height &&
+	    vout->xres == out_width &&
+	    vout->yres == out_height &&
+	    ipu_can_rotate_in_place(vout->rotate) &&
+	    (vout->bytesperline ==
+	     bytes_per_pixel(vout->v2f.fmt.pix.pixelformat) * out_width) &&
+	    !INTERLACED_CONTENT(vout)) {
+		vout->ic_bypass = 1;
+	} else {
+		vout->ic_bypass = 0;
+	}
 
 #ifdef CONFIG_MXC_IPU_V1
-		/* IPUv1 needs IC to do CSC */
-		if (format_is_yuv(vout->v2f.fmt.pix.pixelformat) !=
-		    format_is_yuv(bpp_to_fmt(fbi)))
-			vout->ic_bypass = 0;
+	/* IPUv1 needs IC to do CSC */
+	if (format_is_yuv(vout->v2f.fmt.pix.pixelformat) !=
+	    format_is_yuv(bpp_to_fmt(fbi)))
+		vout->ic_bypass = 0;
 #endif
 
-		/* We are using IC to do input cropping */
-		if (vout->queue_buf_paddr[vout->ipu_buf[0]] !=
-		    vout->v4l2_bufs[vout->ipu_buf[0]].m.offset ||
-		    vout->queue_buf_paddr[vout->ipu_buf[1]] !=
-		    vout->v4l2_bufs[vout->ipu_buf[1]].m.offset)
-			vout->ic_bypass = 0;
-
-		if (fbi->fbops->fb_ioctl) {
-			old_fs = get_fs();
-			set_fs(KERNEL_DS);
-			fbi->fbops->fb_ioctl(fbi, MXCFB_GET_FB_IPU_CHAN,
-					(unsigned long)&ipu_ch);
-			set_fs(old_fs);
-		}
+	if (fbi->fbops->fb_ioctl) {
+		old_fs = get_fs();
+		set_fs(KERNEL_DS);
+		fbi->fbops->fb_ioctl(fbi, MXCFB_GET_FB_IPU_CHAN,
+				(unsigned long)&ipu_ch);
+		set_fs(old_fs);
+	}
 
-		if (ipu_ch == CHAN_NONE) {
-			dev_err(dev, "Can not get display ipu channel\n");
-			return -EINVAL;
-		}
+	if (ipu_ch == CHAN_NONE) {
+		dev_err(dev, "Can not get display ipu channel\n");
+		return -EINVAL;
+	}
 
-		vout->display_ch = ipu_ch;
+	vout->display_ch = ipu_ch;
 
-		if (vout->ic_bypass) {
-			pr_debug("Bypassing IC\n");
-			vout->work_irq = -1;
-			switch (vout->v2f.fmt.pix.pixelformat) {
+	if (vout->ic_bypass) {
+		pr_debug("Bypassing IC\n");
+		vout->pp_split = 0;
+		switch (vout->v2f.fmt.pix.pixelformat) {
 			case V4L2_PIX_FMT_YUV420:
 			case V4L2_PIX_FMT_YVU420:
 			case V4L2_PIX_FMT_NV12:
@@ -1353,87 +1353,80 @@ static int mxc_v4l2out_streamon(vout_data * vout)
 			default:
 				fbvar.bits_per_pixel = 8*
 					bytes_per_pixel(vout->v2f.fmt.pix.pixelformat);
-			}
-			fbvar.nonstd = vout->v2f.fmt.pix.pixelformat;
 		}
+		fbvar.nonstd = vout->v2f.fmt.pix.pixelformat;
+	}
 
-		fbvar.activate |= FB_ACTIVATE_FORCE;
-		fb_set_var(fbi, &fbvar);
+	/* Init display channel through fb API */
+	fbvar.activate |= FB_ACTIVATE_FORCE;
+	fb_set_var(fbi, &fbvar);
 
-		if (fbi->fbops->fb_ioctl && vout->display_ch == MEM_FG_SYNC) {
-			fb_pos.x = vout->crop_current.left;
-			fb_pos.y = vout->crop_current.top;
-			old_fs = get_fs();
-			set_fs(KERNEL_DS);
-			fbi->fbops->fb_ioctl(fbi, MXCFB_SET_OVERLAY_POS,
-					     (unsigned long)&fb_pos);
-			set_fs(old_fs);
-		}
+	if (fbi->fbops->fb_ioctl && vout->display_ch == MEM_FG_SYNC) {
+		fb_pos.x = vout->crop_current.left;
+		fb_pos.y = vout->crop_current.top;
+		old_fs = get_fs();
+		set_fs(KERNEL_DS);
+		fbi->fbops->fb_ioctl(fbi, MXCFB_SET_OVERLAY_POS,
+				(unsigned long)&fb_pos);
+		set_fs(old_fs);
+	}
+
+	vout->display_bufs[1] = fbi->fix.smem_start;
+	vout->display_bufs[0] = fbi->fix.smem_start +
+		(fbi->fix.line_length * vout->yres);
+	vout->display_buf_size = vout->xres *
+		vout->yres * fbi->var.bits_per_pixel / 8;
+
+	/* fill black color for init fb, we assume fb has double buffer*/
+	if (format_is_yuv(vout->v2f.fmt.pix.pixelformat)) {
+		int i;
 
-		vout->display_bufs[1] = fbi->fix.smem_start;
-		vout->display_bufs[0] = fbi->fix.smem_start +
-			(fbi->fix.line_length * vout->yres);
-		vout->display_buf_size = vout->xres *
-			vout->yres * fbi->var.bits_per_pixel / 8;
-
-		/* fill black color for init fb, we assume fb has double buffer*/
-		if (format_is_yuv(vout->v2f.fmt.pix.pixelformat)) {
-			int i;
-
-			if ((vout->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_UYVY) ||
-				(vout->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_YUYV) ||
-				(!vout->ic_bypass)) {
-				short * tmp = (short *) fbi->screen_base;
-				short color;
-				if (vout->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_YUYV)
-					color = 0x8000;
-				else
-					color = 0x80;
-				for (i = 0; i < (fbi->fix.line_length * fbi->var.yres_virtual)/2;
+		if ((vout->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_UYVY) ||
+			(vout->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_YUYV) ||
+			(!vout->ic_bypass)) {
+			short * tmp = (short *) fbi->screen_base;
+			short color;
+			if (vout->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_YUYV)
+				color = 0x8000;
+			else
+				color = 0x80;
+			for (i = 0; i < (fbi->fix.line_length * fbi->var.yres_virtual)/2;
 					i++, tmp++)
-					*tmp = color;
-			} else if ((vout->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_YUV420) ||
+				*tmp = color;
+		} else if ((vout->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_YUV420) ||
 				(vout->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_YVU420) ||
 				(vout->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_NV12)) {
-				char * base = (char *)fbi->screen_base;
-				int j, screen_size = fbi->var.xres * fbi->var.yres;
-
-				for (j = 0; j < 2; j++) {
-					memset(base, 0, screen_size);
-					base += screen_size;
-					for (i = 0; i < screen_size/2; i++, base++)
-						*base = 0x80;
-				}
-			} else if (vout->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_YUV422P) {
-				char * base = (char *)fbi->screen_base;
-				int j, screen_size = fbi->var.xres * fbi->var.yres;
-
-				for (j = 0; j < 2; j++) {
-					memset(base, 0, screen_size);
-					base += screen_size;
-					for (i = 0; i < screen_size; i++, base++)
-						*base = 0x80;
-				}
+			char * base = (char *)fbi->screen_base;
+			int j, screen_size = fbi->var.xres * fbi->var.yres;
+
+			for (j = 0; j < 2; j++) {
+				memset(base, 0, screen_size);
+				base += screen_size;
+				for (i = 0; i < screen_size/2; i++, base++)
+					*base = 0x80;
+			}
+		} else if (vout->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_YUV422P) {
+			char * base = (char *)fbi->screen_base;
+			int j, screen_size = fbi->var.xres * fbi->var.yres;
+
+			for (j = 0; j < 2; j++) {
+				memset(base, 0, screen_size);
+				base += screen_size;
+				for (i = 0; i < screen_size; i++, base++)
+					*base = 0x80;
 			}
-		} else
-			memset(fbi->screen_base, 0x0,
-					fbi->fix.line_length * fbi->var.yres_virtual);
-
-		if (INTERLACED_CONTENT(vout))
-			vout->post_proc_ch = MEM_VDI_PRP_VF_MEM;
-		else
-			vout->post_proc_ch = MEM_PP_MEM;
-
-		if (!vout->ic_bypass) {
-			ipu_clear_irq(vout->work_irq);
-			ipu_request_irq(vout->work_irq,
-					mxc_v4l2out_work_irq_handler,
-					0, vout->video_dev->name, vout);
 		}
-	}
+	} else
+		memset(fbi->screen_base, 0x0,
+				fbi->fix.line_length * fbi->var.yres_virtual);
+
+	if (INTERLACED_CONTENT(vout))
+		vout->post_proc_ch = MEM_VDI_PRP_VF_MEM;
+	else if (!vout->ic_bypass)
+		vout->post_proc_ch = MEM_PP_MEM;
 
-	/* Init PP */
-	if (use_direct_adc == false && !vout->ic_bypass) {
+	/* Init IC channel */
+	if (!vout->ic_bypass) {
 		if (vout->rotate >= IPU_ROTATE_90_RIGHT) {
 			out_width = vout->crop_current.height;
 			out_height = vout->crop_current.width;
@@ -1441,7 +1434,11 @@ static int mxc_v4l2out_streamon(vout_data * vout)
 		vout->display_input_ch = vout->post_proc_ch;
 		memset(&params, 0, sizeof(params));
 		if (INTERLACED_CONTENT(vout)) {
-			rc = init_VDI(params, vout, dev, fbi, out_width, out_height);
+			if (vout->pp_split) {
+				dev_err(&vout->video_dev->dev, "VDI split has not supported yet.\n");
+				return -1;
+			} else
+				rc = init_VDI(params, vout, dev, fbi, out_width, out_height);
 		} else {
 			rc = init_PP(&params, vout, dev, fbi, out_width, out_height);
 		}
@@ -1449,52 +1446,79 @@ static int mxc_v4l2out_streamon(vout_data * vout)
 			return rc;
 	}
 
+	if (!vout->ic_bypass) {
+		switch (vout->display_input_ch) {
+		case MEM_PP_MEM:
+			vout->work_irq = IPU_IRQ_PP_OUT_EOF;
+			break;
+		case MEM_VDI_PRP_VF_MEM:
+			vout->work_irq = IPU_IRQ_PRP_VF_OUT_EOF;
+			break;
+		case MEM_ROT_VF_MEM:
+			vout->work_irq = IPU_IRQ_PRP_VF_ROT_OUT_EOF;
+			break;
+		case MEM_ROT_PP_MEM:
+			vout->work_irq = IPU_IRQ_PP_ROT_OUT_EOF;
+			break;
+		default:
+			dev_err(&vout->video_dev->dev,
+				"not support channel, should not be here\n");
+		}
+	} else
+		vout->work_irq = -1;
+
+	if (!vout->ic_bypass && (vout->work_irq > 0)) {
+		ipu_clear_irq(vout->work_irq);
+		ipu_request_irq(vout->work_irq,
+				mxc_v4l2out_work_irq_handler,
+				0, vout->video_dev->name, vout);
+	}
+
 	vout->state = STATE_STREAM_PAUSED;
 
-	if (use_direct_adc == false) {
-		if (fbi) {
-			acquire_console_sem();
-			fb_blank(fbi, FB_BLANK_UNBLANK);
-			release_console_sem();
-		} else {
-			ipu_enable_channel(vout->display_ch);
-		}
-		if (!vout->ic_bypass) {
+	/* Enable display and IC channels */
+	if (fbi) {
+		acquire_console_sem();
+		fb_blank(fbi, FB_BLANK_UNBLANK);
+		release_console_sem();
+	} else {
+		ipu_enable_channel(vout->display_ch);
+	}
+	if (!vout->ic_bypass) {
 #ifndef CONFIG_MXC_IPU_V1
-			ipu_enable_channel(vout->post_proc_ch);
+		ipu_enable_channel(vout->post_proc_ch);
 #endif
-			if (LOAD_3FIELDS(vout)) {
-				ipu_enable_channel(MEM_VDI_PRP_VF_MEM_P);
-				ipu_enable_channel(MEM_VDI_PRP_VF_MEM_N);
-				ipu_select_multi_vdi_buffer(0);
-			} else if (INTERLACED_CONTENT(vout)) {
-				ipu_select_buffer(vout->post_proc_ch, IPU_INPUT_BUFFER, 0);
-			} else {
-				ipu_select_buffer(vout->post_proc_ch, IPU_INPUT_BUFFER, 0);
-				if (!vout->pp_split)
-					ipu_select_buffer(vout->post_proc_ch, IPU_INPUT_BUFFER, 1);
-			}
-			ipu_select_buffer(vout->post_proc_ch, IPU_OUTPUT_BUFFER, 0);
-			ipu_select_buffer(vout->post_proc_ch, IPU_OUTPUT_BUFFER, 1);
+		if (LOAD_3FIELDS(vout)) {
+			ipu_enable_channel(MEM_VDI_PRP_VF_MEM_P);
+			ipu_enable_channel(MEM_VDI_PRP_VF_MEM_N);
+			ipu_select_multi_vdi_buffer(0);
+		} else
+			ipu_select_buffer(vout->post_proc_ch, IPU_INPUT_BUFFER, 0);
+		ipu_select_buffer(vout->post_proc_ch, IPU_OUTPUT_BUFFER, 0);
+		ipu_select_buffer(vout->post_proc_ch, IPU_OUTPUT_BUFFER, 1);
 #ifdef CONFIG_MXC_IPU_V1
-			ipu_enable_channel(vout->post_proc_ch);
+		ipu_enable_channel(vout->post_proc_ch);
 #endif
-		} else {
-			ipu_update_channel_buffer(vout->display_ch,
+	} else {
+		ipu_update_channel_buffer(vout->display_ch,
 				IPU_INPUT_BUFFER,
 				0, vout->v4l2_bufs[vout->ipu_buf[0]].m.offset);
-			ipu_update_channel_buffer(vout->display_ch,
-				IPU_INPUT_BUFFER,
-				1, vout->v4l2_bufs[vout->ipu_buf[1]].m.offset);
-			ipu_select_buffer(vout->display_ch, IPU_INPUT_BUFFER, 0);
-			ipu_select_buffer(vout->display_ch, IPU_INPUT_BUFFER, 1);
-			queue_work(vout->v4l_wq, &vout->timer_work);
-		}
-	} else {
-		ipu_select_buffer(vout->post_proc_ch, IPU_INPUT_BUFFER, 0);
-		ipu_select_buffer(vout->post_proc_ch, IPU_INPUT_BUFFER, 1);
-		ipu_enable_channel(vout->post_proc_ch);
+		if (vout->offset.u_offset || vout->offset.v_offset)
+			/* only update u/v offset */
+			ipu_update_channel_offset(vout->display_ch,
+					IPU_INPUT_BUFFER,
+					vout->v2f.fmt.pix.pixelformat,
+					vout->v2f.fmt.pix.width,
+					vout->v2f.fmt.pix.height,
+					vout->bytesperline,
+					vout->offset.u_offset,
+					vout->offset.v_offset,
+					0,
+					0);
+		ipu_select_buffer(vout->display_ch, IPU_INPUT_BUFFER, 0);
+		queue_work(vout->v4l_wq, &vout->icbypass_work);
 	}
+
 	vout->start_jiffies = jiffies;
 
 	msleep(1);
@@ -1529,7 +1553,8 @@ static int mxc_v4l2out_streamoff(vout_data * vout)
 	if (!vout->ic_bypass)
 		ipu_free_irq(vout->work_irq, vout);
 
-	cancel_work_sync(&vout->timer_work);
+	if (vout->ic_bypass)
+		cancel_work_sync(&vout->icbypass_work);
 
 	spin_lock_irqsave(&g_lock, lockflag);
 
@@ -1556,9 +1581,21 @@ static int mxc_v4l2out_streamoff(vout_data * vout)
 		}
 	}
 
-	if (vout->post_proc_ch == MEM_PP_MEM ||
+	if (vout->ic_bypass) {
+		fbi->var.activate |= FB_ACTIVATE_FORCE;
+		fb_set_var(fbi, &fbi->var);
+
+		if (vout->display_ch == MEM_FG_SYNC) {
+			acquire_console_sem();
+			fb_blank(fbi, FB_BLANK_POWERDOWN);
+			release_console_sem();
+		}
+
+		vout->display_bufs[0] = 0;
+		vout->display_bufs[1] = 0;
+	} else if (vout->post_proc_ch == MEM_PP_MEM ||
 	    vout->post_proc_ch == MEM_PRP_VF_MEM) {
-		/* SDC or ADC with Rotation */
+		/* SDC with Rotation */
 		if (!ipu_can_rotate_in_place(vout->rotate)) {
 			ipu_unlink_channels(MEM_PP_MEM, MEM_ROT_PP_MEM);
 			ipu_disable_channel(MEM_ROT_PP_MEM, true);
@@ -1571,28 +1608,23 @@ static int mxc_v4l2out_streamoff(vout_data * vout)
 		}
 		ipu_disable_channel(MEM_PP_MEM, true);
 
-		if (vout->display_ch == ADC_SYS2 ||
-			vout->display_ch == MEM_FG_SYNC) {
-			ipu_disable_channel(vout->display_ch, true);
-			ipu_uninit_channel(vout->display_ch);
-		} else {
-			fbi->var.activate |= FB_ACTIVATE_FORCE;
-			fb_set_var(fbi, &fbi->var);
+		fbi->var.activate |= FB_ACTIVATE_FORCE;
+		fb_set_var(fbi, &fbi->var);
 
-			if (vout->display_ch == MEM_FG_SYNC) {
-				acquire_console_sem();
-				fb_blank(fbi, FB_BLANK_POWERDOWN);
-				release_console_sem();
-			}
-
-			vout->display_bufs[0] = 0;
-			vout->display_bufs[1] = 0;
+		if (vout->display_ch == MEM_FG_SYNC) {
+			acquire_console_sem();
+			fb_blank(fbi, FB_BLANK_POWERDOWN);
+			release_console_sem();
 		}
 
+		vout->display_bufs[0] = 0;
+		vout->display_bufs[1] = 0;
+
 		ipu_uninit_channel(MEM_PP_MEM);
 		if (!ipu_can_rotate_in_place(vout->rotate))
 			ipu_uninit_channel(MEM_ROT_PP_MEM);
-	} else if (INTERLACED_CONTENT(vout) && (vout->post_proc_ch == MEM_VDI_PRP_VF_MEM)) {
+	} else if (INTERLACED_CONTENT(vout) &&
+		(vout->post_proc_ch == MEM_VDI_PRP_VF_MEM)) {
 		if (!ipu_can_rotate_in_place(vout->rotate)) {
 			ipu_unlink_channels(MEM_VDI_PRP_VF_MEM,
 					    MEM_ROT_VF_MEM);
@@ -1606,32 +1638,32 @@ static int mxc_v4l2out_streamoff(vout_data * vout)
 		}
 
 		ipu_disable_channel(MEM_VDI_PRP_VF_MEM, true);
+		if (LOAD_3FIELDS(vout)) {
+			ipu_disable_channel(MEM_VDI_PRP_VF_MEM_P, true);
+			ipu_disable_channel(MEM_VDI_PRP_VF_MEM_N, true);
+		}
 
-		if (vout->display_ch == ADC_SYS2 ||
-			vout->display_ch == MEM_FG_SYNC) {
-			ipu_disable_channel(vout->display_ch, true);
-			ipu_uninit_channel(vout->display_ch);
-		} else {
-			fbi->var.activate |= FB_ACTIVATE_FORCE;
-			fb_set_var(fbi, &fbi->var);
+		fbi->var.activate |= FB_ACTIVATE_FORCE;
+		fb_set_var(fbi, &fbi->var);
 
-			if (vout->display_ch == MEM_FG_SYNC) {
-				acquire_console_sem();
-				fb_blank(fbi, FB_BLANK_POWERDOWN);
-				release_console_sem();
-			}
-
-			vout->display_bufs[0] = 0;
-			vout->display_bufs[1] = 0;
+		if (vout->display_ch == MEM_FG_SYNC) {
+			acquire_console_sem();
+			fb_blank(fbi, FB_BLANK_POWERDOWN);
+			release_console_sem();
 		}
 
+		vout->display_bufs[0] = 0;
+		vout->display_bufs[1] = 0;
+
 		ipu_uninit_channel(MEM_VDI_PRP_VF_MEM);
+		if (LOAD_3FIELDS(vout)) {
+			ipu_uninit_channel(MEM_VDI_PRP_VF_MEM_P);
+			ipu_uninit_channel(MEM_VDI_PRP_VF_MEM_N);
+		}
 		if (!ipu_can_rotate_in_place(vout->rotate))
 			ipu_uninit_channel(MEM_ROT_VF_MEM);
-	} else {		/* ADC Direct */
-		ipu_disable_channel(MEM_PP_ADC, true);
-		ipu_uninit_channel(MEM_PP_ADC);
 	}
+
 	vout->ready_q.head = vout->ready_q.tail = 0;
 	vout->done_q.head = vout->done_q.tail = 0;
 	for (i = 0; i < vout->buffer_cnt; i++) {
@@ -1640,23 +1672,9 @@ static int mxc_v4l2out_streamoff(vout_data * vout)
 		vout->v4l2_bufs[i].timestamp.tv_usec = 0;
 	}
 
+	vout->post_proc_ch = CHAN_NONE;
 	vout->state = STATE_STREAM_OFF;
 
-#ifdef CONFIG_FB_MXC_ASYNC_PANEL
-	if (vout->cur_disp_output < DISP3) {
-		if (vout->display_bufs[0] != 0) {
-			mxc_free_buffers(vout->display_bufs,
-					 vout->display_bufs_vaddr, 2,
-					 vout->display_buf_size);
-		}
-
-		mxcfb_set_refresh_mode(registered_fb
-				       [vout->
-					output_fb_num[vout->cur_disp_output]],
-				       MXCFB_REFRESH_PARTIAL, 0);
-	}
-#endif
-
 	return retval;
 }
 
@@ -1751,7 +1769,6 @@ static int mxc_v4l2out_s_fmt(vout_data * vout, struct v4l2_format *f)
 		dev_err(&vout->video_dev->dev,
 			"De-interlacing not supported in this device!\n");
 		vout->field_fmt = V4L2_FIELD_NONE;
-		break;
 	case V4L2_FIELD_INTERLACED_BT:
 		dev_err(&vout->video_dev->dev,
 			"V4L2_FIELD_INTERLACED_BT field format not supported yet!\n");
@@ -1898,7 +1915,7 @@ static int mxc_v4l2out_open(struct file *file)
 			goto oops;
 		}
 
-		INIT_WORK(&vout->timer_work, timer_work_func);
+		INIT_WORK(&vout->icbypass_work, icbypass_work_func);
 	}
 
 	file->private_data = dev;
@@ -2284,12 +2301,8 @@ mxc_v4l2out_do_ioctl(struct file *file,
 				break;
 			}
 
-			if (output->index < 3) {
-				*output = mxc_outputs[MXC_V4L2_OUT_2_ADC];
-				output->name[4] = '0' + output->index;
-			} else {
+			if (output->index >= 3)
 				*output = mxc_outputs[MXC_V4L2_OUT_2_SDC];
-			}
 			break;
 		}
 	case VIDIOC_G_OUTPUT:
diff --git a/drivers/media/video/mxc/output/mxc_v4l2_output.h b/drivers/media/video/mxc/output/mxc_v4l2_output.h
index 0dd8eb0076a9..c26194f5ff90 100644
--- a/drivers/media/video/mxc/output/mxc_v4l2_output.h
+++ b/drivers/media/video/mxc/output/mxc_v4l2_output.h
@@ -39,7 +39,6 @@
 
 #define MXC_V4L2_OUT_NUM_OUTPUTS        6
 #define MXC_V4L2_OUT_2_SDC              0
-#define MXC_V4L2_OUT_2_ADC              1
 
 
 typedef struct {
@@ -80,7 +79,9 @@ typedef struct _vout_data {
 
 	struct timer_list output_timer;
 	struct workqueue_struct *v4l_wq;
-	struct work_struct timer_work;
+	struct work_struct icbypass_work;
+	int disp_buf_num;
+	int fb_blank;
 	unsigned long start_jiffies;
 	u32 frame_count;
 
@@ -89,6 +90,7 @@ typedef struct _vout_data {
 
 	s8 next_rdy_ipu_buf;
 	s8 next_done_ipu_buf;
+	s8 next_disp_ipu_buf;
 	s8 ipu_buf[2];
 	s8 ipu_buf_p[2];
 	s8 ipu_buf_n[2];
@@ -141,8 +143,10 @@ typedef struct _vout_data {
 	int pp_split; /* 0,1 */
 	struct stripe_param pp_left_stripe;
 	struct stripe_param pp_right_stripe; /* struct for split parameters */
-	/* IC ouput buffer number. Counting from 0 to 3 */
-	int pp_split_buf_num; /*  0..3 */
+	struct stripe_param pp_up_stripe;
+	struct stripe_param pp_down_stripe;
+	/* IC ouput buffer number. Counting from 0 to 7 */
+	int pp_split_buf_num; /*  0..7 */
 	u16 bpp ; /* bit per pixel */
 	u16 xres; /* width of physical frame (BGs) */
 	u16 yres; /* heigth of physical frame (BGs)*/
diff --git a/drivers/media/video/mxs_pxp.c b/drivers/media/video/mxs_pxp.c
index 83c9c52c3b0c..017d22458a22 100644
--- a/drivers/media/video/mxs_pxp.c
+++ b/drivers/media/video/mxs_pxp.c
@@ -31,6 +31,7 @@
 #include <linux/platform_device.h>
 #include <linux/vmalloc.h>
 #include <linux/videodev2.h>
+#include <linux/delay.h>
 
 #include <media/videobuf-dma-contig.h>
 #include <media/v4l2-common.h>
@@ -671,6 +672,7 @@ static int pxp_streamon(struct file *file, void *priv, enum v4l2_buf_type t)
 
 	pxp_set_outbuf(pxp);
 	ret = videobuf_streamon(&pxp->s0_vbq);
+	msleep(20);
 
 	if (!ret && (pxp->output == 0))
 		mxsfb_cfg_pxp(1, pxp->outb_phys);
@@ -686,7 +688,9 @@ static int pxp_streamoff(struct file *file, void *priv, enum v4l2_buf_type t)
 	if ((t != V4L2_BUF_TYPE_VIDEO_OUTPUT))
 		return -EINVAL;
 
+	cancel_work_sync(&pxp->work);
 	ret = videobuf_streamoff(&pxp->s0_vbq);
+	msleep(20);
 
 	if (!ret)
 		mxsfb_cfg_pxp(0, 0);
@@ -1101,8 +1105,10 @@ static int pxp_close(struct file *file)
 {
 	struct pxps *pxp = video_get_drvdata(video_devdata(file));
 
-	if (pxp->workqueue)
+	if (pxp->workqueue) {
+		flush_workqueue(pxp->workqueue);
 		destroy_workqueue(pxp->workqueue);
+	}
 
 	videobuf_stop(&pxp->s0_vbq);
 	videobuf_mmap_free(&pxp->s0_vbq);
diff --git a/drivers/media/video/uvc/uvc_video.c b/drivers/media/video/uvc/uvc_video.c
index 01b633c73480..be99c8797dcc 100644
--- a/drivers/media/video/uvc/uvc_video.c
+++ b/drivers/media/video/uvc/uvc_video.c
@@ -707,6 +707,10 @@ static void uvc_video_complete(struct urb *urb)
 	if ((ret = usb_submit_urb(urb, GFP_ATOMIC)) < 0) {
 		uvc_printk(KERN_ERR, "Failed to resubmit video URB (%d).\n",
 			ret);
+		if (ret == -ENODEV) {
+			uvc_queue_cancel(queue, 1);
+			return;
+		}
 	}
 }
 
diff --git a/drivers/media/video/videobuf-dma-contig.c b/drivers/media/video/videobuf-dma-contig.c
index 2ac8c2421ad2..dc4f32bbd83d 100644
--- a/drivers/media/video/videobuf-dma-contig.c
+++ b/drivers/media/video/videobuf-dma-contig.c
@@ -319,7 +319,7 @@ static int __videobuf_mmap_mapper(struct videobuf_queue *q,
 
 	mem->size = PAGE_ALIGN(q->bufs[first]->bsize);
 	mem->vaddr = dma_alloc_coherent(q->dev, mem->size,
-					&mem->dma_handle, GFP_KERNEL);
+					&mem->dma_handle, GFP_DMA);
 	if (!mem->vaddr) {
 		dev_err(q->dev, "dma_alloc_coherent size %ld failed\n",
 			mem->size);