Support BT656 and BT1120 output for iMX6 ipuv3.

	modified:   arch/arm/configs/imx6_defconfig
	modified:   arch/arm/mach-mx6/board-mx6q_sabresd.c
	modified:   arch/arm/mach-mx6/devices-imx6q.h
	modified:   arch/arm/plat-mxc/include/mach/ipu-v3.h
	modified:   drivers/media/video/mxc/output/mxc_vout.c
	modified:   drivers/mxc/ipu3/ipu_common.c
	modified:   drivers/mxc/ipu3/ipu_device.c
	modified:   drivers/mxc/ipu3/ipu_disp.c
	modified:   drivers/mxc/ipu3/ipu_prv.h
	modified:   drivers/mxc/ipu3/ipu_regs.h
	modified:   drivers/video/mxc/Kconfig
	modified:   drivers/video/mxc/Makefile
	new file:   drivers/video/mxc/mxc_bt656if.c
	modified:   drivers/video/mxc/mxc_ipuv3_fb.c
	modified:   include/linux/ipu.h
	modified:   include/linux/mxcfb.h

16 files changed, 1439 insertions, 283 deletions

diff --git a/arch/arm/configs/imx6_defconfig b/arch/arm/configs/imx6_defconfig
index 6f30e36a0f5a..49965dca7883 100644
--- a/arch/arm/configs/imx6_defconfig
+++ b/arch/arm/configs/imx6_defconfig
@@ -1769,6 +1769,7 @@ CONFIG_FB_MXC_EDID=y
 CONFIG_FB_MXC_SYNC_PANEL=y
 # CONFIG_FB_MXC_EPSON_VGA_SYNC_PANEL is not set
 CONFIG_FB_MXC_LDB=y
+CONFIG_FB_MXC_BT656=y
 CONFIG_FB_MXC_MIPI_DSI=y
 CONFIG_FB_MXC_TRULY_WVGA_SYNC_PANEL=y
 # CONFIG_FB_MXC_CLAA_WVGA_SYNC_PANEL is not set
diff --git a/arch/arm/mach-mx6/board-mx6q_sabresd.c b/arch/arm/mach-mx6/board-mx6q_sabresd.c
index 69afc3576522..f29a83646737 100644
--- a/arch/arm/mach-mx6/board-mx6q_sabresd.c
+++ b/arch/arm/mach-mx6/board-mx6q_sabresd.c
@@ -1339,6 +1339,12 @@ static struct fsl_mxc_ldb_platform_data ldb_data = {
 	.sec_disp_id = 1,
 };
 
+static struct fsl_mxc_lcd_platform_data bt656_data = {
+	.ipu_id = 0, 
+	.disp_id = 0, 
+	.default_ifmt = IPU_PIX_FMT_BT656,
+};
+
 static struct max8903_pdata charger1_data = {
 	.dok = SABRESD_CHARGE_DOK_B,
 	.uok = SABRESD_CHARGE_UOK_B,
@@ -1722,6 +1728,7 @@ static void __init mx6_sabresd_board_init(void)
 	imx6q_add_mipi_dsi(&mipi_dsi_pdata);
 	imx6q_add_lcdif(&lcdif_data);
 	imx6q_add_ldb(&ldb_data);
+	imx6q_add_bt656(&bt656_data);
 	imx6q_add_v4l2_output(0);
 	imx6q_add_v4l2_capture(0, &capture_data[0]);
 	imx6q_add_v4l2_capture(1, &capture_data[1]);
diff --git a/arch/arm/mach-mx6/devices-imx6q.h b/arch/arm/mach-mx6/devices-imx6q.h
index ff49ce1b1419..c70cb0862bc1 100644
--- a/arch/arm/mach-mx6/devices-imx6q.h
+++ b/arch/arm/mach-mx6/devices-imx6q.h
@@ -123,6 +123,10 @@ extern const struct imx_ldb_data imx6q_ldb_data __initconst;
 #define imx6q_add_ldb(pdata) \
 	imx_add_ldb(&imx6q_ldb_data, pdata);
 
+#define imx6q_add_bt656(pdata)	\
+	platform_device_register_resndata(NULL, "mxc_bt656if",\
+			0, NULL, 0, pdata, sizeof(*pdata));
+
 #define imx6q_add_v4l2_output(id)	\
 	platform_device_register_resndata(NULL, "mxc_v4l2_output",\
 			id, NULL, 0, NULL, 0);
diff --git a/arch/arm/plat-mxc/include/mach/ipu-v3.h b/arch/arm/plat-mxc/include/mach/ipu-v3.h
index 519e628d73ee..62d2433de75e 100644
--- a/arch/arm/plat-mxc/include/mach/ipu-v3.h
+++ b/arch/arm/plat-mxc/include/mach/ipu-v3.h
@@ -730,6 +730,7 @@ uint32_t bytes_per_pixel(uint32_t fmt);
 struct ipuv3_fb_platform_data {
 	char				disp_dev[32];
 	u32				interface_pix_fmt;
+	u32				fb_pix_fmt;
 	char				*mode_str;
 	int				default_bpp;
 	bool				int_clk;
diff --git a/drivers/media/video/mxc/output/mxc_vout.c b/drivers/media/video/mxc/output/mxc_vout.c
index ec789ec2e826..593994ec353b 100644
--- a/drivers/media/video/mxc/output/mxc_vout.c
+++ b/drivers/media/video/mxc/output/mxc_vout.c
@@ -64,6 +64,7 @@ struct mxc_vout_fb {
 	int ipu_id;
 	struct v4l2_rect crop_bounds;
 	unsigned int disp_fmt;
+	unsigned int if_fmt;
 	bool disp_support_csc;
 	bool disp_support_windows;
 };
@@ -278,12 +279,28 @@ static ipu_channel_t get_ipu_channel(struct fb_info *fbi)
 static unsigned int get_ipu_fmt(struct fb_info *fbi)
 {
 	mm_segment_t old_fs;
-	unsigned int fb_fmt;
+	unsigned int di_fmt;
 
 	if (fbi->fbops->fb_ioctl) {
 		old_fs = get_fs();
 		set_fs(KERNEL_DS);
 		fbi->fbops->fb_ioctl(fbi, MXCFB_GET_DIFMT,
+				(unsigned long)&di_fmt);
+		set_fs(old_fs);
+	}
+
+	return di_fmt;
+}
+
+static unsigned int get_fb_fmt(struct fb_info *fbi)
+{
+	mm_segment_t old_fs;
+	unsigned int fb_fmt;
+
+	if (fbi->fbops->fb_ioctl) {
+		old_fs = get_fs();
+		set_fs(KERNEL_DS);
+		fbi->fbops->fb_ioctl(fbi, MXCFB_GET_FBFMT,
 				(unsigned long)&fb_fmt);
 		set_fs(old_fs);
 	}
@@ -312,14 +329,21 @@ static void update_display_setting(void)
 		g_fb_setting[i].crop_bounds.top = 0;
 		g_fb_setting[i].crop_bounds.width = fbi->var.xres;
 		g_fb_setting[i].crop_bounds.height = fbi->var.yres;
-		g_fb_setting[i].disp_fmt = get_ipu_fmt(fbi);
+		g_fb_setting[i].disp_fmt = get_fb_fmt(fbi);
+		g_fb_setting[i].if_fmt = get_ipu_fmt(fbi);
 
 		if (get_ipu_channel(fbi) == MEM_BG_SYNC) {
 			bg_crop_bounds[g_fb_setting[i].ipu_id] =
 				g_fb_setting[i].crop_bounds;
-			g_fb_setting[i].disp_support_csc = true;
+			if(colorspaceofpixel(g_fb_setting[i].disp_fmt) != colorspaceofpixel(g_fb_setting[i].if_fmt))
+				g_fb_setting[i].disp_support_csc = false;  // DP CSC need be used for fb to di output.
+			else
+				g_fb_setting[i].disp_support_csc = true;
 		} else if (get_ipu_channel(fbi) == MEM_FG_SYNC) {
-			g_fb_setting[i].disp_support_csc = true;
+			if(colorspaceofpixel(g_fb_setting[i].disp_fmt) != colorspaceofpixel(g_fb_setting[i].if_fmt))
+				g_fb_setting[i].disp_support_csc = false;  // DP CSC need be used for fb to di output.
+			else
+				g_fb_setting[i].disp_support_csc = true;
 			g_fb_setting[i].disp_support_windows = true;
 		}
 	}
@@ -375,10 +399,7 @@ static int update_setting_from_fbi(struct mxc_vout_output *vout,
 	vout->task.output.crop.pos.y = 0;
 	vout->task.output.crop.w = vout->crop_bounds.width;
 	vout->task.output.crop.h = vout->crop_bounds.height;
-	if (colorspaceofpixel(vout->disp_fmt) == YUV_CS)
-		vout->task.output.format = IPU_PIX_FMT_UYVY;
-	else
-		vout->task.output.format = IPU_PIX_FMT_RGB565;
+	vout->task.output.format = vout->disp_fmt;
 
 	return 0;
 }
@@ -1077,18 +1098,7 @@ static int mxc_vout_try_task(struct mxc_vout_output *vout)
 		v4l2_info(vout->vfd->v4l2_dev, "Bypass IC.\n");
 		output->format = input->format;
 	} else {
-		/* if need CSC, choose IPU-DP or IPU_IC do it */
-		if (vout->disp_support_csc) {
-			if (colorspaceofpixel(input->format) == YUV_CS)
-				output->format = IPU_PIX_FMT_UYVY;
-			else
-				output->format = IPU_PIX_FMT_RGB565;
-		} else {
-			if (colorspaceofpixel(vout->disp_fmt) == YUV_CS)
-				output->format = IPU_PIX_FMT_UYVY;
-			else
-				output->format = IPU_PIX_FMT_RGB565;
-		}
+		output->format = vout->disp_fmt;
 
 		vout->tiled_bypass_pp = false;
 		if ((IPU_PIX_FMT_TILED_NV12 == input->format) ||
diff --git a/drivers/mxc/ipu3/ipu_common.c b/drivers/mxc/ipu3/ipu_common.c
index 3a760a1d9f5f..24caedbd96fe 100644
--- a/drivers/mxc/ipu3/ipu_common.c
+++ b/drivers/mxc/ipu3/ipu_common.c
@@ -315,6 +315,9 @@ static int __devinit ipu_probe(struct platform_device *pdev)
 		/* Set MCU_T to divide MCU access window into 2 */
 		ipu_cm_write(ipu, 0x00400000L | (IPU_MCU_T_DEFAULT << 18),
 			     IPU_DISP_GEN);
+	} else {
+		ipu->fg_csc_type = ipu->bg_csc_type = CSC_NONE;
+		ipu->color_key_4rgb = true;
 	}
 
 	/* Set sync refresh channels and CSI->mem channel as high priority */
diff --git a/drivers/mxc/ipu3/ipu_device.c b/drivers/mxc/ipu3/ipu_device.c
index be1725129ce2..7e0115d1f792 100644
--- a/drivers/mxc/ipu3/ipu_device.c
+++ b/drivers/mxc/ipu3/ipu_device.c
@@ -458,6 +458,8 @@ cs_t colorspaceofpixel(int fmt)
 	case IPU_PIX_FMT_RGBA32:
 	case IPU_PIX_FMT_RGB32:
 	case IPU_PIX_FMT_ABGR32:
+	case IPU_PIX_FMT_LVDS666:
+	case IPU_PIX_FMT_LVDS888:
 		return RGB_CS;
 		break;
 	case IPU_PIX_FMT_UYVY:
@@ -472,6 +474,8 @@ cs_t colorspaceofpixel(int fmt)
 	case IPU_PIX_FMT_NV12:
 	case IPU_PIX_FMT_TILED_NV12:
 	case IPU_PIX_FMT_TILED_NV12F:
+	case IPU_PIX_FMT_BT656:
+	case IPU_PIX_FMT_BT1120:
 		return YUV_CS;
 		break;
 	default:
diff --git a/drivers/mxc/ipu3/ipu_disp.c b/drivers/mxc/ipu3/ipu_disp.c
index 35b78199b0df..13ba1a4f15fa 100644
--- a/drivers/mxc/ipu3/ipu_disp.c
+++ b/drivers/mxc/ipu3/ipu_disp.c
@@ -47,6 +47,16 @@ struct dp_csc_param_t {
 #define DC_DISP_ID_SERIAL	2
 #define DC_DISP_ID_ASYNC	3
 
+/* DC microcode address */
+#define DC_MCODE_BT656_START			101
+#define DC_MCODE_BT656_VSYNC_FF		141
+#define DC_MCODE_BT656_EOFIELD		167
+#define DC_MCODE_BT656_DATA_W		173
+#define DC_MCODE_BT656_NL				179
+
+#define BT656_IF_DI_MSB			23  /* For 8 bits BT656: 23 for DISP_DAT23 ~ DISP_DAT16; 7 for DISP_DAT7 ~ DISP_DAT0 */
+								      /* For 16 bits BT1120: 23 for DISP_DAT23 ~ DISP_DAT8; 15 for DISP_DAT15 ~ DISP_DAT0 */
+
 static inline struct ipu_soc *pixelclk2ipu(struct clk *clk)
 {
 	struct ipu_soc *ipu;
@@ -516,33 +526,76 @@ static void _ipu_dc_map_clear(struct ipu_soc *ipu, int map)
 
 static void _ipu_dc_write_tmpl(struct ipu_soc *ipu,
 			int word, u32 opcode, u32 operand, int map,
-			int wave, int glue, int sync, int stop)
+			int wave, int glue, int sync, int stop, int lf, int af)
 {
-	u32 reg;
-
-	if (opcode == WRG) {
-		reg = sync;
-		reg |= (glue << 4);
-		reg |= (++wave << 11);
-		reg |= ((operand & 0x1FFFF) << 15);
-		ipu_dc_tmpl_write(ipu, reg, word * 2);
-
-		reg = (operand >> 17);
-		reg |= opcode << 7;
-		reg |= (stop << 9);
-		ipu_dc_tmpl_write(ipu, reg, word * 2 + 1);
-	} else {
-		reg = sync;
-		reg |= (glue << 4);
-		reg |= (++wave << 11);
-		reg |= (++map << 15);
-		reg |= (operand << 20) & 0xFFF00000;
-		ipu_dc_tmpl_write(ipu, reg, word * 2);
-
-		reg = (operand >> 12);
-		reg |= opcode << 4;
-		reg |= (stop << 9);
-		ipu_dc_tmpl_write(ipu, reg, word * 2 + 1);
+	u32 reg, opcmd;
+
+	switch(opcode) {
+		case WROD:
+			reg = sync;
+			reg |= (glue << 4);
+			reg |= (++wave << 11);
+			reg |= (++map << 15);
+			reg |= (operand << 20) & 0xFFF00000;
+			ipu_dc_tmpl_write(ipu, reg, word * 2);
+			
+			opcmd = 0x18 | (lf << 1);
+			reg = (operand >> 12);
+			reg |= opcmd << 4;
+			reg |= (stop << 9);
+			ipu_dc_tmpl_write(ipu, reg, word * 2 + 1);
+			break;
+
+		case WRG:
+			reg = sync;
+			reg |= (glue << 4);
+			reg |= (++wave << 11);
+			reg |= ((operand & 0x1FFFF) << 15);
+			ipu_dc_tmpl_write(ipu, reg, word * 2);
+			
+			opcmd = 0x01;
+			reg = (operand >> 17);
+			reg |= opcmd << 7;
+			reg |= (stop << 9);
+			ipu_dc_tmpl_write(ipu, reg, word * 2 + 1);
+			break;
+
+		case HMA:
+			reg = (operand << 5);
+			ipu_dc_tmpl_write(ipu, reg, word * 2);
+
+			opcmd = 0x02;
+			reg = (opcmd << 5);
+			reg |= (stop << 9);
+			ipu_dc_tmpl_write(ipu, reg, word * 2 + 1);
+			break;
+
+		case HMA1:
+			reg = (operand << 5);
+			ipu_dc_tmpl_write(ipu, reg, word * 2);
+
+			opcmd = 0x01;
+			reg = (opcmd << 5);
+			reg |= (stop << 9);
+			ipu_dc_tmpl_write(ipu, reg, word * 2 + 1);
+			break;
+
+		case BMA:
+			reg = sync;
+			reg |= (operand << 5);
+			ipu_dc_tmpl_write(ipu, reg, word * 2);
+
+			opcmd = 0x03;
+			reg = (af << 3);
+			reg |= (lf << 4);
+			reg |= (opcmd << 5);
+			reg |= (stop << 9);
+			ipu_dc_tmpl_write(ipu, reg, word * 2 + 1);
+			break;
+
+		default:
+			dev_err(ipu->dev, "WARNING: unsupported opcode.\n");
+			break;
 	}
 }
 
@@ -642,6 +695,7 @@ void __ipu_dp_csc_setup(struct ipu_soc *ipu,
 		reg = ipu_dp_read(ipu, DP_COM_CONF(dp));
 		reg &= ~DP_COM_CONF_CSC_DEF_MASK;
 		reg |= dp_csc_param.mode;
+//		reg |= (1 << 11);  /* Y range 16-235, U/V range 16-240. */
 		ipu_dp_write(ipu, reg, DP_COM_CONF(dp));
 	}
 
@@ -787,9 +841,17 @@ void _ipu_dc_init(struct ipu_soc *ipu, int dc_chan, int di, bool interlaced, uin
 
 	if ((dc_chan == 1) || (dc_chan == 5)) {
 		if (interlaced) {
-			_ipu_dc_link_event(ipu, dc_chan, DC_EVT_NL, 0, 3);
-			_ipu_dc_link_event(ipu, dc_chan, DC_EVT_EOL, 0, 2);
-			_ipu_dc_link_event(ipu, dc_chan, DC_EVT_NEW_DATA, 0, 1);
+			if((pixel_fmt == IPU_PIX_FMT_BT656) || (pixel_fmt == IPU_PIX_FMT_BT1120)) {
+				_ipu_dc_link_event(ipu, dc_chan, DC_EVT_NL, DC_MCODE_BT656_NL, 2);
+				_ipu_dc_link_event(ipu, dc_chan, DC_EVT_NF, DC_MCODE_BT656_VSYNC_FF, 5);
+				_ipu_dc_link_event(ipu, dc_chan, DC_EVT_EOF, DC_MCODE_BT656_START, 4);
+				_ipu_dc_link_event(ipu, dc_chan, DC_EVT_EOFIELD, DC_MCODE_BT656_EOFIELD, 3);
+				_ipu_dc_link_event(ipu, dc_chan, DC_EVT_NEW_DATA, DC_MCODE_BT656_DATA_W, 0);
+			} else {
+				_ipu_dc_link_event(ipu, dc_chan, DC_EVT_NL, 0, 3);
+				_ipu_dc_link_event(ipu, dc_chan, DC_EVT_EOL, 0, 2);
+				_ipu_dc_link_event(ipu, dc_chan, DC_EVT_NEW_DATA, 0, 1);
+			}
 		} else {
 			if (di) {
 				_ipu_dc_link_event(ipu, dc_chan, DC_EVT_NL, 2, 3);
@@ -815,12 +877,37 @@ void _ipu_dc_init(struct ipu_soc *ipu, int dc_chan, int di, bool interlaced, uin
 				}
 			}
 		}
-		_ipu_dc_link_event(ipu, dc_chan, DC_EVT_NF, 0, 0);
-		_ipu_dc_link_event(ipu, dc_chan, DC_EVT_NFIELD, 0, 0);
-		_ipu_dc_link_event(ipu, dc_chan, DC_EVT_EOF, 0, 0);
-		_ipu_dc_link_event(ipu, dc_chan, DC_EVT_EOFIELD, 0, 0);
-		_ipu_dc_link_event(ipu, dc_chan, DC_EVT_NEW_CHAN, 0, 0);
-		_ipu_dc_link_event(ipu, dc_chan, DC_EVT_NEW_ADDR, 0, 0);
+
+		if((pixel_fmt == IPU_PIX_FMT_BT656) || (pixel_fmt == IPU_PIX_FMT_BT1120)) {
+			_ipu_dc_link_event(ipu, dc_chan, DC_EVT_EOL, 0, 0);
+			_ipu_dc_link_event(ipu, dc_chan, DC_EVT_NFIELD, 0, 0);
+			_ipu_dc_link_event(ipu, dc_chan, DC_EVT_NEW_CHAN, 0, 0);
+			_ipu_dc_link_event(ipu, dc_chan, DC_EVT_NEW_ADDR, 0, 0);
+
+			// config DC_UGDEx_0 for DC event 0: new line, disable autorepeat, enable odd mode, set event 0 priority to 1
+			reg = (0x1 << 25) | (0x1 << 3);
+			reg |= (DC_MCODE_BT656_DATA_W << 16);
+			if(pixel_fmt == IPU_PIX_FMT_BT656)
+				reg |= ((DC_MCODE_BT656_DATA_W + 3) << 8);
+			else if(pixel_fmt == IPU_PIX_FMT_BT1120)
+				reg |= ((DC_MCODE_BT656_DATA_W + 1) << 8);
+
+			if(dc_chan == 1)
+				reg |= (0x1 << 0);
+			else if(dc_chan == 5)
+				reg |= (0x3 << 0);
+
+			ipu_dc_write(ipu, reg, DC_UGDE_0(DC_DISP_ID_SYNC(di)));
+			ipu_dc_write(ipu, 0, DC_UGDE_1(DC_DISP_ID_SYNC(di)));
+			ipu_dc_write(ipu, 0, DC_UGDE_2(DC_DISP_ID_SYNC(di)));
+		} else {
+			_ipu_dc_link_event(ipu, dc_chan, DC_EVT_NF, 0, 0);
+			_ipu_dc_link_event(ipu, dc_chan, DC_EVT_NFIELD, 0, 0);
+			_ipu_dc_link_event(ipu, dc_chan, DC_EVT_EOF, 0, 0);
+			_ipu_dc_link_event(ipu, dc_chan, DC_EVT_EOFIELD, 0, 0);
+			_ipu_dc_link_event(ipu, dc_chan, DC_EVT_NEW_CHAN, 0, 0);
+			_ipu_dc_link_event(ipu, dc_chan, DC_EVT_NEW_ADDR, 0, 0);
+		}
 
 		reg = 0x2;
 		reg |= DC_DISP_ID_SYNC(di) << DC_WR_CH_CONF_PROG_DISP_ID_OFFSET;
@@ -1060,6 +1147,31 @@ void _ipu_init_dc_mappings(struct ipu_soc *ipu)
 	_ipu_dc_map_config(ipu, 4, 1, 13, 0xFC);
 	_ipu_dc_map_config(ipu, 4, 2, 21, 0xFC);
 
+#ifdef BT656_IF_DI_MSB
+	/* IPU_PIX_FMT_VYUY 16bit width */
+	_ipu_dc_map_clear(ipu, 5);
+	_ipu_dc_map_config(ipu, 5, 0, BT656_IF_DI_MSB - 8, 0xFF);
+	_ipu_dc_map_config(ipu, 5, 1, 0, 0x0);
+	_ipu_dc_map_config(ipu, 5, 2, BT656_IF_DI_MSB, 0xFF);
+	_ipu_dc_map_clear(ipu, 6);
+	_ipu_dc_map_config(ipu, 6, 0, 0, 0x0);
+	_ipu_dc_map_config(ipu, 6, 1, BT656_IF_DI_MSB - 8, 0xFF);
+	_ipu_dc_map_config(ipu, 6, 2, BT656_IF_DI_MSB, 0xFF);
+
+	// IPU_PIX_FMT_UYVY 16bit width for BT1120
+	_ipu_dc_map_clear(ipu, 7);  //UY
+	_ipu_dc_map_link(ipu, 7, 6, 0, 6, 1, 6, 2);
+	_ipu_dc_map_clear(ipu, 8);  //VY
+	_ipu_dc_map_link(ipu, 8, 5, 0, 5, 1, 5, 2);
+
+	// IPU_PIX_FMT_UYVY 8bit width for BT656
+	_ipu_dc_map_clear(ipu, 9);  //U
+	_ipu_dc_map_link(ipu, 9, 6, 0, 6, 2, 6, 0);
+	_ipu_dc_map_clear(ipu, 10);  //Y
+	_ipu_dc_map_link(ipu, 10, 6, 0, 6, 0, 6, 2);
+	_ipu_dc_map_clear(ipu, 11);  //V
+	_ipu_dc_map_link(ipu, 11, 6, 2, 6, 0, 6, 0);
+#else
 	/* IPU_PIX_FMT_VYUY 16bit width */
 	_ipu_dc_map_clear(ipu, 5);
 	_ipu_dc_map_config(ipu, 5, 0, 7, 0xFF);
@@ -1087,6 +1199,7 @@ void _ipu_init_dc_mappings(struct ipu_soc *ipu)
 	_ipu_dc_map_link(ipu, 11, 5, 1, 5, 2, 5, 0);
 	_ipu_dc_map_clear(ipu, 12);
 	_ipu_dc_map_link(ipu, 12, 5, 2, 5, 1, 5, 0);
+#endif
 
 	/* IPU_PIX_FMT_GBR24 */
 	/* IPU_PIX_FMT_VYU444 */
@@ -1114,12 +1227,20 @@ int _ipu_pixfmt_to_map(uint32_t fmt)
 		return 4;
 	case IPU_PIX_FMT_VYUY:
 		return 6;
+#ifdef BT656_IF_DI_MSB
+	case IPU_PIX_FMT_UYVY:
+	case IPU_PIX_FMT_BT1120:
+		return 8;
+	case IPU_PIX_FMT_BT656:
+		return 11;
+#else
 	case IPU_PIX_FMT_UYVY:
 		return 8;
 	case IPU_PIX_FMT_YUYV:
 		return 10;
 	case IPU_PIX_FMT_YVYU:
 		return 12;
+#endif
 	case IPU_PIX_FMT_GBR24:
 	case IPU_PIX_FMT_VYU444:
 		return 13;
@@ -1161,6 +1282,580 @@ void _ipu_dp_set_csc_coefficients(struct ipu_soc *ipu, ipu_channel_t channel, in
 	__ipu_dp_csc_setup(ipu, dp, dp_csc_param, true);
 }
 
+static void _ipu_dc_setup_bt656_interlaced(struct ipu_soc *ipu, 
+			    int u_map, int y_map, int v_map, 
+			    bool is_bt1120, int di_msb, 
+			    uint32_t bt656_h_start_width,
+			    uint32_t bt656_v_start_width_field0, 
+			    uint32_t bt656_v_end_width_field0, 
+			    uint32_t bt656_v_start_width_field1, 
+			    uint32_t bt656_v_end_width_field1)
+{
+	uint32_t microcode_addr_FirstPart, microcode_addr_SecondPart;
+	uint32_t microcode_addr_EOFIELD, microcode_addr_DataW, microcode_addr_NL, microcode_addr_FirstPartSF;
+	uint32_t microcode_addr_SAV0, microcode_addr_SAV1, microcode_addr_SAVff;
+	uint32_t microcode_addr_BlankDone, microcode_addr_VsyncSyncFF, microcode_addr_VsyncSyncCont;
+	uint32_t loop_N_times, loop_BL0_times, loop_BL1_times, loop_BL2_times, loop_BL3_times;
+	uint32_t microcode_start_addr, microcode_addr;
+	uint32_t second_offset = 0x50, second_field;
+	uint32_t vsync_sync_cont = 0x30;
+
+	second_field = (vsync_sync_cont - 1 + second_offset);
+	microcode_addr = microcode_start_addr = DC_MCODE_BT656_START;
+
+	microcode_addr_FirstPart = microcode_start_addr + 0x1;
+	microcode_addr_SAV0 = microcode_start_addr + 0xB;
+	microcode_addr_SecondPart = microcode_start_addr + 0x13;
+	microcode_addr_SAV1 = microcode_start_addr + 0x20;
+	microcode_addr_VsyncSyncFF = microcode_start_addr + 0x28;  //DC_MCODE_BT656_VSYNC_FF
+	microcode_addr_VsyncSyncCont = microcode_start_addr + vsync_sync_cont;
+	microcode_addr_SAVff = microcode_start_addr + 0x3A;
+	microcode_addr_EOFIELD = microcode_start_addr + 0x48 - 6;  //DATA_W - 6, DC_MCODE_BT656_EOFIELD
+	microcode_addr_DataW = microcode_start_addr + 0x48;  //DC_MCODE_BT656_DATA_W
+	microcode_addr_NL = microcode_start_addr + 0x48 + 6;  //DATA_W + 6, DC_MCODE_BT656_NL
+	microcode_addr_FirstPartSF = microcode_start_addr + second_offset;
+	microcode_addr_BlankDone = microcode_start_addr + second_field - 0x2;
+	
+	if(is_bt1120)
+		loop_N_times = (bt656_h_start_width - 8) - 1;  //horizontal blanking
+	else
+		loop_N_times = (bt656_h_start_width - 8) / 2 - 1;  //horizontal blanking
+
+	loop_BL0_times = bt656_v_end_width_field1 - 1;  //vertical blanking type 0
+	loop_BL1_times = bt656_v_start_width_field0 - 1;  //vertical blanking type 1
+	loop_BL2_times = bt656_v_end_width_field0 - 1;  //vertical blanking type 2
+	loop_BL3_times = bt656_v_start_width_field1 - 1;  //vertical blanking type 3
+
+	if(is_bt1120)
+		_ipu_dc_write_tmpl(ipu, microcode_addr, WROD, 0, v_map, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+	else
+		_ipu_dc_write_tmpl(ipu, microcode_addr, WROD, 0, y_map, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+	microcode_addr ++;
+
+	//First part
+	_ipu_dc_write_tmpl(ipu, microcode_addr, HMA1, microcode_addr_SAV0, 0, 0, 0, 0, 0, 0, 0);
+	microcode_addr ++;
+
+	//wait he, send eav_0[0]
+	_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, (0xFF << (di_msb - 7)), 0, 0, 0, DI_BT656_SYNC_HSYNC, 0, 0, 0);
+	microcode_addr ++;
+
+	//wait he, send eav_0[1]
+	_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, 0x00, 0, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+	microcode_addr ++;
+
+	//wait ipp clk, send eav_0[2]
+	_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, 0x00, 0, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+	microcode_addr ++;
+
+	//wait ipp clk, send eav_0[3]  //Blanking aferr FIELD1 F=1 V=1 H=1 P3=0 P2=0 P1=0 P0=1 =>11110001 => 0xF1
+	_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, (0xF1 << (di_msb - 7)), 0, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+	microcode_addr ++;
+
+	//Store jump address 0 HEAD_LOOP_0
+	_ipu_dc_write_tmpl(ipu, microcode_addr, HMA, microcode_addr + 1, 0, 0, 0, 0, 0, 0, 0);
+	microcode_addr ++;
+
+	//HEAD_LOOP_0:
+	if(is_bt1120) {
+		_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, (0x1080 << (di_msb - 15)), 0, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+	} else {
+		//wait ipp_clk, send 0x80
+		//First 8 bit word in blanking
+		_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, (0x80 << (di_msb - 7)), 0, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+		microcode_addr ++;
+
+		//wait ipp_clk, send 0x10
+		_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, (0x10 << (di_msb - 7)), 0, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+	}
+	microcode_addr ++;
+
+	//LOOP:
+	//Jump Store jump address 0 N_TIMES //else jump Store jump address 1
+	_ipu_dc_write_tmpl(ipu, microcode_addr, BMA, loop_N_times, 0, 0, 0, 0, 0, 1, 1);
+
+	microcode_addr = microcode_addr_SAV0;
+
+	//Sav0:
+	//wait ipp clk, send sav_0[0]
+	_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, (0xFF << (di_msb - 7)), 0, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+	microcode_addr ++;
+
+	//wait ipp clk, send sav_0[1]
+	_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, 0x00, 0, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+	microcode_addr ++;
+
+	//Store jump address 1: First part
+	_ipu_dc_write_tmpl(ipu, microcode_addr, HMA1, microcode_addr_FirstPart, 0, 0, 0, 0, 0, 0, 0);
+	microcode_addr ++;
+
+	//wait ipp clk, send sav_0[2]
+	_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, 0x00, 0, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+	microcode_addr ++;
+
+	//wait ipp clk, send sav_0[3]  //Blanking aferr FIELD1 F=1 V=1 H=0 P3=1 P2=1 P1=0 P0=0 =>11101100 => 0xEC
+	_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, (0xEC << (di_msb - 7)), 0, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+	microcode_addr ++;
+
+	//Store jump address 0 sencond part
+	_ipu_dc_write_tmpl(ipu, microcode_addr, HMA, microcode_addr_SecondPart, 0, 0, 0, 0, 0, 0, 0);
+	microcode_addr ++;
+
+	//Jump Store jump address 1 BL0_TIMES //else jump Store jump address 0
+	_ipu_dc_write_tmpl(ipu, microcode_addr, BMA, loop_BL0_times, 0, 0, 0, 0, 0, 0, 0);
+
+	microcode_addr = microcode_addr_SecondPart;
+
+	//Second part
+	//wait he, send eav_1[0]
+	_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, (0xFF << (di_msb - 7)), 0, 0, 0, DI_BT656_SYNC_HSYNC, 0, 0, 0);
+	microcode_addr ++;
+
+	//wait ipp clk, send eav_1[1]
+	_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, 0x00, 0, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+	microcode_addr ++;
+
+	//store jump address0 Sav_1
+	_ipu_dc_write_tmpl(ipu, microcode_addr, HMA, microcode_addr_SAV1, 0, 0, 0, 0, 0, 0, 0);
+	microcode_addr ++;
+
+	//wait ipp clk, send eav_1[2]
+	_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, 0x00, 0, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+	microcode_addr ++;
+
+	//wait ipp clk, send eav_1[3]
+	_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, (0xB6 << (di_msb - 7)), 0, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+	microcode_addr ++;
+
+	//Store jump address 1 HEAD_LOOP_1
+	_ipu_dc_write_tmpl(ipu, microcode_addr, HMA1, microcode_addr + 1, 0, 0, 0, 0, 0, 0, 0);
+	microcode_addr ++;
+
+	//HEAD_LOOP_1:
+	if(is_bt1120) {
+		_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, (0x1080 << (di_msb - 15)), 0, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+	} else {
+		//wait ipp_clk, send 0x80
+		//First 8 bit word in blanking
+		_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, (0x80 << (di_msb - 7)), 0, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+		microcode_addr ++;
+
+		//wait ipp_clk, send 0x10
+		_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, (0x10 << (di_msb - 7)), 0, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+	}
+	microcode_addr ++;
+
+	//LOOP:
+	//Jump Store jump address 1 N_TIMES //else jump Store jump address 1
+	_ipu_dc_write_tmpl(ipu, microcode_addr, BMA, loop_N_times, 0, 0, 0, 0, 0, 0, 0);
+
+	microcode_addr = microcode_addr_SAV1;
+
+	//Sav1:
+	//wait ipp clk, send sav_1[0]
+	_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, (0xFF << (di_msb - 7)), 0, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+	microcode_addr ++;
+
+	//wait ipp clk, send sav_1[1]
+	_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, 0x00, 0, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+	microcode_addr ++;
+
+	//Store jump address 0: Second part
+	_ipu_dc_write_tmpl(ipu, microcode_addr, HMA, microcode_addr_SecondPart, 0, 0, 0, 0, 0, 0, 0);
+	microcode_addr ++;
+
+	//wait ipp clk, send sav_1[2]
+	_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, 0x00, 0, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+	microcode_addr ++;
+
+	//wait ipp clk, send sav_1[3]
+	_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, (0xAB << (di_msb - 7)), 0, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+	microcode_addr ++;
+
+	//Store jump address 1 blank done
+	_ipu_dc_write_tmpl(ipu, microcode_addr, HMA1, microcode_addr_BlankDone, 0, 0, 0, 0, 0, 0, 0);
+	microcode_addr ++;
+
+	//Jump Store jump address 0 BL1_TIMES //else jump Store jump address 1
+	_ipu_dc_write_tmpl(ipu, microcode_addr, BMA, loop_BL1_times, 0, 0, 0, 0, 0, 1, 1);
+
+	//New vsync
+	microcode_addr = microcode_addr_VsyncSyncFF;
+
+	//if nf
+	//Start here, no blank
+	
+	//Store jump address 0 VSYNC_SYNC_CONT
+	_ipu_dc_write_tmpl(ipu, microcode_addr, HMA, microcode_addr_VsyncSyncCont, 0, 0, 0, 0, 0, 0, 0);
+	microcode_addr ++;
+
+	//wait for synchronization
+	//wait ve, send eav_FF[0]
+	_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, (0xFF << (di_msb - 7)), 0, 0, 0, DI_BT656_SYNC_AFIELD, 0, 0, 0);
+	microcode_addr ++;
+
+	//Jump Store jump address 0
+	// go to vsync_sync_cont
+	_ipu_dc_write_tmpl(ipu, microcode_addr, BMA, 0, 0, 0, 0, 0, 0, 0, 0);
+
+	microcode_addr = microcode_addr_VsyncSyncCont - 1;
+
+	//First field
+	//wait he, send eav_FF[0]
+	_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, (0xFF << (di_msb - 7)), 0, 0, 0, DI_BT656_SYNC_HSYNC, 0, 0, 0);
+	microcode_addr ++;
+
+	//vync_sync_cont:
+	//wait ipp clk, send eav_FF[1]
+	_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, 0x00, 0, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+	microcode_addr ++;
+
+	//Store jump address 0: sav_ff
+	_ipu_dc_write_tmpl(ipu, microcode_addr, HMA, microcode_addr_SAVff, 0, 0, 0, 0, 0, 0, 0);
+	microcode_addr ++;
+
+	//wait ipp clk, send eav_FF[2]
+	_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, 0x00, 0, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+	microcode_addr ++;
+
+	//wait ipp clk, send eav_FF[3]
+	_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, (0x9D << (di_msb - 7)), 0, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+	microcode_addr ++;
+
+	//Store jump address 1: Header_loop_ff
+	_ipu_dc_write_tmpl(ipu, microcode_addr, HMA1, microcode_addr + 1, 0, 0, 0, 0, 0, 0, 0);
+	microcode_addr ++;
+
+	//HEAD_LOOP_FF:
+	if(is_bt1120) {
+		_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, (0x1080 << (di_msb - 15)), 0, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+	} else {
+		//wait ipp_clk, send 0x80
+		//First 8 bit word in blanking
+		_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, (0x80 << (di_msb - 7)), 0, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+		microcode_addr ++;
+
+		//wait ipp_clk, send 0x10
+		_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, (0x10 << (di_msb - 7)), 0, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+	}
+	microcode_addr ++;
+
+	//LOOP:
+	//Jump Store jump address 1 N_TIMES //else jump Store jump address 0
+	_ipu_dc_write_tmpl(ipu, microcode_addr, BMA, loop_N_times, 0, 0, 0, 0, 0, 0, 0);
+
+	microcode_addr = microcode_addr_SAVff;
+
+	//Savff:
+	//wait ipp clk, send sav_ff[0]
+	_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, (0xFF << (di_msb - 7)), 0, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+	microcode_addr ++;
+
+	//wait ipp clk, send sav_ff[1]
+	_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, 0x00, 0, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+	microcode_addr ++;
+
+	//Store jump address 1: data_w
+	_ipu_dc_write_tmpl(ipu, microcode_addr, HMA1, microcode_addr_DataW, 0, 0, 0, 0, 0, 0, 0);
+	microcode_addr ++;
+
+	//wait ipp clk, send sav_ff[2]
+	_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, 0x00, 0, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+	microcode_addr ++;
+
+	//wait ipp clk, send sav_ff[3]
+	_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, (0x80 << (di_msb - 7)), 0, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+	microcode_addr ++;
+
+	//Store jump address 1 blank done
+	_ipu_dc_write_tmpl(ipu, microcode_addr, HMA, microcode_addr_VsyncSyncCont - 1, 0, 0, 0, 0, 0, 0, 0);
+	microcode_addr ++;
+
+	//Jump Store jump address 0 data_w
+	_ipu_dc_write_tmpl(ipu, microcode_addr, BMA, 0, 0, 0, 0, 0, 0, 1, 1);
+
+	//share routine
+	//EOField: last data of first field if not NF
+	microcode_addr = microcode_addr_EOFIELD;
+
+	if(is_bt1120)
+		_ipu_dc_write_tmpl(ipu, microcode_addr, WROD, 0, v_map, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+	else
+		_ipu_dc_write_tmpl(ipu, microcode_addr, WROD, 0, y_map, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+	microcode_addr ++;
+
+	//Store jump address 1 FIRST_PART_SF
+	_ipu_dc_write_tmpl(ipu, microcode_addr, HMA1, microcode_addr_FirstPartSF, 0, 0, 0, 0, 0, 0, 0);
+	microcode_addr ++;
+
+	//Jump Store jump address 0 FIRST_PART_SF
+	_ipu_dc_write_tmpl(ipu, microcode_addr, BMA, 0, 0, 0, 0, 0, 0, 1, 1);
+
+	//regular data sending
+	microcode_addr = microcode_addr_DataW;
+
+	if(is_bt1120) {
+		_ipu_dc_write_tmpl(ipu, microcode_addr, WROD, 0, u_map, 0, 0, DI_BT656_SYNC_BASECLK, 1, 0, 0);
+		microcode_addr ++;
+
+		_ipu_dc_write_tmpl(ipu, microcode_addr, WROD, 0, v_map, 0, 0, DI_BT656_SYNC_BASECLK, 1, 0, 0);
+	} else {
+		//wait ipp clk, send data[0]
+		_ipu_dc_write_tmpl(ipu, microcode_addr, WROD, 0, u_map, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+		microcode_addr ++;
+
+		//wait ipp clk, send data[1]
+		_ipu_dc_write_tmpl(ipu, microcode_addr, WROD, 0, y_map, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+		microcode_addr ++;
+
+		//wait ipp clk, send data[2]
+		_ipu_dc_write_tmpl(ipu, microcode_addr, WROD, 0, v_map, 0, 0, DI_BT656_SYNC_BASECLK, 1, 0, 0);
+		microcode_addr ++;
+
+		//wait ipp clk, send data[3]
+		_ipu_dc_write_tmpl(ipu, microcode_addr, WROD, 0, y_map, 0, 0, DI_BT656_SYNC_BASECLK, 1, 0, 0);
+	}
+
+	microcode_addr = microcode_addr_NL;
+	//N_L: new line with data if not NF
+
+	//Jump Store jump address 0 //go to FIRST_FIELD/SECOND_FIELD
+	_ipu_dc_write_tmpl(ipu, microcode_addr, BMA, 0, 0, 0, 0, 0, 0, 0, 0);
+
+	///////////////////////////////////////
+	//				   Second field
+	///////////////////////////////////////
+	microcode_addr = microcode_start_addr + second_offset; // microcode_addr_FirstPartSF
+
+	//First part
+	_ipu_dc_write_tmpl(ipu, microcode_addr, HMA1, microcode_addr_SAV0 + second_offset, 0, 0, 0, 0, 0, 0, 0);
+	microcode_addr ++;
+
+	//wait he, send eav_0[0]
+	_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, (0xFF << (di_msb - 7)), 0, 0, 0, DI_BT656_SYNC_HSYNC, 0, 0, 0);
+	microcode_addr ++;
+
+	//wait ipp clk, send eav_0[1]
+	_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, 0x00, 0, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+	microcode_addr ++;
+
+	//wait ipp clk, send eav_0[2]
+	_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, 0x00, 0, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+	microcode_addr ++;
+
+	//wait ipp clk, send eav_0[3]  //Blanking aferr FIELD0 F=0 V=1 H=1 P3=0 P2=1 P1=1 P0=0 =>10110110 => 0xB6
+	_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, (0xB6 << (di_msb - 7)), 0, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+	microcode_addr ++;
+
+	//Store jump address 0 HEAD_LOOP_0
+	_ipu_dc_write_tmpl(ipu, microcode_addr, HMA, microcode_addr + 1, 0, 0, 0, 0, 0, 0, 0);
+	microcode_addr ++;
+
+	//HEAD_LOOP_0:
+	if(is_bt1120) {
+		_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, (0x1080 << (di_msb - 15)), 0, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+	} else {
+		//wait ipp_clk, send 0x80
+		//First 8 bit word in blanking
+		_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, (0x80 << (di_msb - 7)), 0, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+		microcode_addr ++;
+
+		//wait ipp_clk, send 0x10
+		_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, (0x10 << (di_msb - 7)), 0, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+	}
+	microcode_addr ++;
+
+	_ipu_dc_write_tmpl(ipu, microcode_addr, BMA, loop_N_times, 0, 0, 0, 0, 0, 1, 1);
+
+	microcode_addr = microcode_addr_SAV0 + second_offset;
+
+	//Sav0:
+	//wait ipp clk, send sav_0[0]
+	_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, (0xFF << (di_msb - 7)), 0, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+	microcode_addr ++;
+
+	//wait ipp clk, send sav_0[1]
+	_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, 0x00, 0, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+	microcode_addr ++;
+
+	//Store jump address 1: First part
+	_ipu_dc_write_tmpl(ipu, microcode_addr, HMA1, microcode_addr_FirstPartSF, 0, 0, 0, 0, 0, 0, 0);
+	microcode_addr ++;
+
+	//wait ipp clk, send sav_0[2]
+	_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, 0x00, 0, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+	microcode_addr ++;
+
+	//wait ipp clk, send sav_0[3]  //Blanking aferr FIELD0 F=0 V=1 H=0 P3=1 P2=0 P1=1 P0=1 =>10101011 => 0xAB
+	_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, (0xAB << (di_msb - 7)), 0, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+	microcode_addr ++;
+
+	//Store jump address 0 sencond part
+	_ipu_dc_write_tmpl(ipu, microcode_addr, HMA, microcode_addr_SecondPart + second_offset, 0, 0, 0, 0, 0, 0, 0);
+	microcode_addr ++;
+
+	//Jump Store jump address 1 BL0_TIMES //else jump Store jump address 0
+	_ipu_dc_write_tmpl(ipu, microcode_addr, BMA, loop_BL2_times, 0, 0, 0, 0, 0, 0, 0);
+
+	microcode_addr = microcode_addr_SecondPart + second_offset;
+
+	//Second part
+	//wait he, send eav_1[0]
+	_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, (0xFF << (di_msb - 7)), 0, 0, 0, DI_BT656_SYNC_HSYNC, 0, 0, 0);
+	microcode_addr ++;
+
+	//wait ipp clk, send eav_1[1]
+	_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, 0x00, 0, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+	microcode_addr ++;
+
+	//store jump address0 Sav_1
+	_ipu_dc_write_tmpl(ipu, microcode_addr, HMA, microcode_addr_SAV1 + second_offset, 0, 0, 0, 0, 0, 0, 0);
+	microcode_addr ++;
+
+	//wait ipp clk, send eav_1[2]
+	_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, 0x00, 0, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+	microcode_addr ++;
+
+	//wait ipp clk, send eav_1[3]  //Blanking before field1 F=1 V=1 H=1 P3=0 P2=0 P1=0 P0=1 =>11110001=>0XF1
+	_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, (0xF1 << (di_msb - 7)), 0, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+	microcode_addr ++;
+
+	//Store jump address 1 HEAD_LOOP_1
+	_ipu_dc_write_tmpl(ipu, microcode_addr, HMA1, microcode_addr + 1, 0, 0, 0, 0, 0, 0, 0);
+	microcode_addr ++;
+
+	//HEAD_LOOP_1:
+	if(is_bt1120) {
+		_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, (0x1080 << (di_msb - 15)), 0, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+	} else {
+		//wait ipp_clk, send 0x80
+		//First 8 bit word in blanking
+		_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, (0x80 << (di_msb - 7)), 0, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+		microcode_addr ++;
+
+		//wait ipp_clk, send 0x10
+		_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, (0x10 << (di_msb - 7)), 0, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+	}
+	microcode_addr ++;
+
+	//LOOP:
+	//Jump Store jump address 1 N_TIMES //else jump Store jump address 1
+	_ipu_dc_write_tmpl(ipu, microcode_addr, BMA, loop_N_times, 0, 0, 0, 0, 0, 0, 0);
+
+	microcode_addr = microcode_addr_SAV1 + second_offset;
+
+	//Sav1:
+	//wait ipp clk, send sav_1[0]
+	_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, (0xFF << (di_msb - 7)), 0, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+	microcode_addr ++;
+
+	//wait ipp clk, send sav_1[1]
+	_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, 0x00, 0, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+	microcode_addr ++;
+
+	//Store jump address 0: Second part
+	_ipu_dc_write_tmpl(ipu, microcode_addr, HMA, microcode_addr_SecondPart + second_offset, 0, 0, 0, 0, 0, 0, 0);
+	microcode_addr ++;
+
+	//wait ipp clk, send sav_1[2]
+	_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, 0x00, 0, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+	microcode_addr ++;
+
+	//wait ipp clk, send sav_1[3]  //Blanking before field1 F=1 V=1 H=0 P3=1 P2=1 P1=0 P0=0 =>11101100=>0XEC
+	_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, (0xEC << (di_msb - 7)), 0, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+	microcode_addr ++;
+
+	//Store jump address 1 blank done
+	_ipu_dc_write_tmpl(ipu, microcode_addr, HMA1, microcode_addr_BlankDone, 0, 0, 0, 0, 0, 0, 0);
+	microcode_addr ++;
+
+	//Jump Store jump address 0 BL1_TIMES //else jump Store jump address 1
+	_ipu_dc_write_tmpl(ipu, microcode_addr, BMA, loop_BL3_times, 0, 0, 0, 0, 0, 1, 1);
+
+	microcode_addr = microcode_addr_BlankDone;
+
+	//Store jump address 0 second field
+	_ipu_dc_write_tmpl(ipu, microcode_addr, HMA, microcode_addr_BlankDone + 2, 0, 0, 0, 0, 0, 0, 0);
+	microcode_addr ++;
+
+	// Stop
+	_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, 0x00, 0, 0, 0, DI_BT656_SYNC_BASECLK, 1, 0, 0);
+	microcode_addr ++;
+
+	//SECOND FIELD microcode_addr_BlankDone+2
+	//wait he, send eav_FF[0]
+	_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, (0xFF << (di_msb - 7)), 0, 0, 0, DI_BT656_SYNC_HSYNC, 0, 0, 0);
+	microcode_addr ++;
+
+	//wait ipp clk, send eav_FF[1]
+	_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, 0x00, 0, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+	microcode_addr ++;
+
+	//Store jump address 0: sav_ff
+	_ipu_dc_write_tmpl(ipu, microcode_addr, HMA, microcode_addr_SAVff + second_offset, 0, 0, 0, 0, 0, 0, 0);
+	microcode_addr ++;
+
+	//wait ipp clk, send eav_FF[2]
+	_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, 0x00, 0, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+	microcode_addr ++;
+
+	//wait ipp clk, send eav_FF[3]	//H Blanking in field1 F=1, V=0, H=1, P3=1,P2=0, P1=0, P0=1=>11011010=>0xda
+	_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, (0xDA << (di_msb - 7)), 0, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+	microcode_addr ++;
+
+	//Store jump address 1: Header_loop_ff
+	_ipu_dc_write_tmpl(ipu, microcode_addr, HMA1, microcode_addr + 1, 0, 0, 0, 0, 0, 0, 0);
+	microcode_addr ++;
+
+	//HEAD_LOOP_FF:
+	if(is_bt1120) {
+		_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, (0x1080 << (di_msb - 15)), 0, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+	} else {
+		//wait ipp_clk, send 0x80
+		//First 8 bit word in blanking
+		_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, (0x80 << (di_msb - 7)), 0, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+		microcode_addr ++;
+
+		//wait ipp_clk, send 0x10
+		_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, (0x10 << (di_msb - 7)), 0, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+	}
+	microcode_addr ++;
+
+	//LOOP:
+	//Jump Store jump address 1 N_TIMES //else jump Store jump address 0
+	_ipu_dc_write_tmpl(ipu, microcode_addr, BMA, loop_N_times, 0, 0, 0, 0, 0, 0, 0);
+
+	microcode_addr = microcode_addr_SAVff + second_offset;
+
+	//Savff:
+	//wait ipp clk, send sav_ff[0]
+	_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, (0xFF << (di_msb - 7)), 0, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+	microcode_addr ++;
+
+	//wait ipp clk, send sav_ff[1]
+	_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, 0x00, 0, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+	microcode_addr ++;
+
+	//Store jump address 1: data_w
+	_ipu_dc_write_tmpl(ipu, microcode_addr, HMA1, microcode_addr_DataW, 0, 0, 0, 0, 0, 0, 0);
+	microcode_addr ++;
+
+	//wait ipp clk, send sav_ff[2]
+	_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, 0x00, 0, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+	microcode_addr ++;
+
+	//wait ipp clk, send sav_ff[3]
+	_ipu_dc_write_tmpl(ipu, microcode_addr, WRG, (0xC7 << (di_msb - 7)), 0, 0, 0, DI_BT656_SYNC_BASECLK, 0, 0, 0);
+	microcode_addr ++;
+
+	//Store jump address 0 second field
+	_ipu_dc_write_tmpl(ipu, microcode_addr, HMA, microcode_addr_BlankDone + 2, 0, 0, 0, 0, 0, 0, 0);
+	microcode_addr ++;
+
+	//Jump Store jump address 1
+	_ipu_dc_write_tmpl(ipu, microcode_addr, BMA, 0, 0, 0, 0, 0, 0, 1, 1);
+}
+
 void ipu_set_csc_coefficients(struct ipu_soc *ipu, ipu_channel_t channel, int32_t param[][3])
 {
 	_ipu_dp_set_csc_coefficients(ipu, channel, param);
@@ -1242,15 +1937,36 @@ int32_t ipu_init_sync_panel(struct ipu_soc *ipu, int disp, uint32_t pixel_clk,
 	uint32_t h_total, v_total;
 	int map;
 	struct clk *di_parent;
+	uint32_t bt656_h_start_width;
+	uint32_t bt656_v_start_width_field0 = 0, bt656_v_end_width_field0 = 0;
+	uint32_t bt656_v_start_width_field1 = 0, bt656_v_end_width_field1 = 0;
+	int u_map = 0, y_map = 0, v_map = 0;
 
 	dev_dbg(ipu->dev, "panel size = %d x %d\n", width, height);
 
 	if ((v_sync_width == 0) || (h_sync_width == 0))
 		return EINVAL;
 
-	adapt_panel_to_ipu_restricitions(ipu, &v_start_width, &v_sync_width, &v_end_width);
-	h_total = width + h_sync_width + h_start_width + h_end_width;
-	v_total = height + v_sync_width + v_start_width + v_end_width;
+	if((pixel_fmt == IPU_PIX_FMT_BT656) || (pixel_fmt == IPU_PIX_FMT_BT1120)) {
+		bt656_h_start_width = h_sync_width;
+		bt656_v_start_width_field0 = h_start_width;
+		bt656_v_end_width_field0 = h_end_width;
+		bt656_v_start_width_field1 =v_start_width ;
+		bt656_v_end_width_field1 = v_end_width;
+
+		v_total = height + bt656_v_start_width_field0 + bt656_v_end_width_field0 + bt656_v_start_width_field1 + bt656_v_end_width_field1;
+		if(pixel_fmt == IPU_PIX_FMT_BT656) {
+			/* BT656 */
+			h_total = bt656_h_start_width + width * 2;
+		} else {
+			/* BT1120 */
+			h_total = bt656_h_start_width + width;
+		}
+	} else {
+		adapt_panel_to_ipu_restricitions(ipu, &v_start_width, &v_sync_width, &v_end_width);
+		h_total = width + h_sync_width + h_start_width + h_end_width;
+		v_total = height + v_sync_width + v_start_width + v_end_width;
+	}
 
 	/* Init clocking */
 	dev_dbg(ipu->dev, "pixel clk = %d\n", pixel_clk);
@@ -1271,9 +1987,10 @@ int32_t ipu_init_sync_panel(struct ipu_soc *ipu, int disp, uint32_t pixel_clk,
 		 * we will only use 1/2 fraction for ipu clk,
 		 * so if the clk rate is not fit, try ext clk.
 		 */
-		if (!sig.int_clk &&
+		if ((!sig.int_clk &&
 			((rounded_pixel_clk >= pixel_clk + pixel_clk/200) ||
-			(rounded_pixel_clk <= pixel_clk - pixel_clk/200))) {
+			(rounded_pixel_clk <= pixel_clk - pixel_clk/200))) || 
+			(pixel_fmt == IPU_PIX_FMT_BT656) || (pixel_fmt == IPU_PIX_FMT_BT1120)) {
 			dev_dbg(ipu->dev, "try ipu ext di clk\n");
 			rounded_pixel_clk = pixel_clk * 2;
 			rounded_parent_clk = clk_round_rate(di_parent,
@@ -1309,6 +2026,17 @@ int32_t ipu_init_sync_panel(struct ipu_soc *ipu, int disp, uint32_t pixel_clk,
 		mutex_unlock(&ipu->mutex_lock);
 		return -EINVAL;
 	}
+	
+	if(pixel_fmt == IPU_PIX_FMT_BT656) {
+		u_map = map - 2;
+		y_map = map - 1;
+		v_map = map;
+		h_total = bt656_h_start_width + width * 2;
+	} else if(pixel_fmt == IPU_PIX_FMT_BT1120) {
+		u_map = map - 1;
+		v_map = map;
+		h_total = bt656_h_start_width + width;
+	}
 
 	/*clear DI*/
 	di_gen = ipu_di_read(ipu, disp, DI_GENERAL);
@@ -1317,172 +2045,289 @@ int32_t ipu_init_sync_panel(struct ipu_soc *ipu, int disp, uint32_t pixel_clk,
 
 	if (sig.interlaced) {
 		if (g_ipu_hw_rev >= 2) {
-			/* Setup internal HSYNC waveform */
-			_ipu_di_sync_config(ipu,
-					disp, 		/* display */
-					1, 		/* counter */
-					h_total/2 - 1, 	/* run count */
-					DI_SYNC_CLK,	/* run_resolution */
-					0, 		/* offset */
-					DI_SYNC_NONE, 	/* offset resolution */
-					0, 		/* repeat count */
-					DI_SYNC_NONE, 	/* CNT_CLR_SEL */
-					0, 		/* CNT_POLARITY_GEN_EN */
-					DI_SYNC_NONE, 	/* CNT_POLARITY_CLR_SEL */
-					DI_SYNC_NONE, 	/* CNT_POLARITY_TRIGGER_SEL */
-					0, 		/* COUNT UP */
-					0		/* COUNT DOWN */
-					);
-
-			/* Field 1 VSYNC waveform */
-			_ipu_di_sync_config(ipu,
-					disp, 		/* display */
-					2, 		/* counter */
-					h_total - 1, 	/* run count */
-					DI_SYNC_CLK,	/* run_resolution */
-					0, 		/* offset */
-					DI_SYNC_NONE, 	/* offset resolution */
-					0, 		/* repeat count */
-					DI_SYNC_NONE, 	/* CNT_CLR_SEL */
-					0, 		/* CNT_POLARITY_GEN_EN */
-					DI_SYNC_NONE, 	/* CNT_POLARITY_CLR_SEL */
-					DI_SYNC_NONE, 	/* CNT_POLARITY_TRIGGER_SEL */
-					0, 		/* COUNT UP */
-					2*div		/* COUNT DOWN */
-					);
-
-			/* Setup internal HSYNC waveform */
-			_ipu_di_sync_config(ipu,
-					disp, 		/* display */
-					3, 		/* counter */
-					v_total*2 - 1, 	/* run count */
-					DI_SYNC_INT_HSYNC,	/* run_resolution */
-					1, 			/* offset */
-					DI_SYNC_INT_HSYNC, 	/* offset resolution */
-					0, 		/* repeat count */
-					DI_SYNC_NONE, 	/* CNT_CLR_SEL */
-					0, 		/* CNT_POLARITY_GEN_EN */
-					DI_SYNC_NONE, 	/* CNT_POLARITY_CLR_SEL */
-					DI_SYNC_NONE, 	/* CNT_POLARITY_TRIGGER_SEL */
-					0, 		/* COUNT UP */
-					2*div		/* COUNT DOWN */
-					);
-
-			/* Active Field ? */
-			_ipu_di_sync_config(ipu,
-					disp, 		/* display */
-					4, 		/* counter */
-					v_total/2 - 1, 	/* run count */
-					DI_SYNC_HSYNC,	/* run_resolution */
-					v_start_width, 	/*  offset */
-					DI_SYNC_HSYNC, 	/* offset resolution */
-					2, 		/* repeat count */
-					DI_SYNC_VSYNC, 	/* CNT_CLR_SEL */
-					0, 		/* CNT_POLARITY_GEN_EN */
-					DI_SYNC_NONE, 	/* CNT_POLARITY_CLR_SEL */
-					DI_SYNC_NONE, 	/* CNT_POLARITY_TRIGGER_SEL */
-					0, 		/* COUNT UP */
-					0		/* COUNT DOWN */
-					);
-
-			/* Active Line */
-			_ipu_di_sync_config(ipu,
-					disp, 		/* display */
-					5, 		/* counter */
-					0, 		/* run count */
-					DI_SYNC_HSYNC,	/* run_resolution */
-					0, 		/*  offset */
-					DI_SYNC_NONE, 	/* offset resolution */
-					height/2, 	/* repeat count */
-					4, 		/* CNT_CLR_SEL */
-					0, 		/* CNT_POLARITY_GEN_EN */
-					DI_SYNC_NONE, 	/* CNT_POLARITY_CLR_SEL */
-					DI_SYNC_NONE, 	/* CNT_POLARITY_TRIGGER_SEL */
-					0, 		/* COUNT UP */
-					0		/* COUNT DOWN */
-					);
-
-			/* Field 0 VSYNC waveform */
-			_ipu_di_sync_config(ipu,
-					disp, 		/* display */
-					6, 		/* counter */
-					v_total - 1, 	/* run count */
-					DI_SYNC_HSYNC,	/* run_resolution */
-					0, 		/* offset */
-					DI_SYNC_NONE, 	/* offset resolution */
-					0, 		/* repeat count */
-					DI_SYNC_NONE, 	/* CNT_CLR_SEL  */
-					0, 		/* CNT_POLARITY_GEN_EN */
-					DI_SYNC_NONE, 	/* CNT_POLARITY_CLR_SEL */
-					DI_SYNC_NONE, 	/* CNT_POLARITY_TRIGGER_SEL */
-					0, 		/* COUNT UP */
-					0		/* COUNT DOWN */
-					);
-
-			/* DC VSYNC waveform */
-			vsync_cnt = 7;
-			_ipu_di_sync_config(ipu,
-					disp, 		/* display */
-					7, 		/* counter */
-					v_total/2 - 1, 	/* run count */
-					DI_SYNC_HSYNC,	/* run_resolution  */
-					9, 		/* offset  */
-					DI_SYNC_HSYNC, 	/* offset resolution */
-					2, 		/* repeat count */
-					DI_SYNC_VSYNC, 	/* CNT_CLR_SEL */
-					0, 		/* CNT_POLARITY_GEN_EN */
-					DI_SYNC_NONE, 	/* CNT_POLARITY_CLR_SEL */
-					DI_SYNC_NONE, 	/* CNT_POLARITY_TRIGGER_SEL */
-					0, 		/* COUNT UP */
-					0		/* COUNT DOWN */
-					);
-
-			/* active pixel waveform */
-			_ipu_di_sync_config(ipu,
-					disp, 		/* display */
-					8, 		/* counter */
-					0, 		/* run count  */
-					DI_SYNC_CLK,	/* run_resolution */
-					h_start_width, 	/* offset  */
-					DI_SYNC_CLK, 	/* offset resolution */
-					width, 		/* repeat count  */
-					5, 		/* CNT_CLR_SEL  */
-					0, 		/* CNT_POLARITY_GEN_EN  */
-					DI_SYNC_NONE, 	/* CNT_POLARITY_CLR_SEL */
-					DI_SYNC_NONE, 	/* CNT_POLARITY_TRIGGER_SEL  */
-					0, 		/* COUNT UP  */
-					0		/* COUNT DOWN */
-					);
-
-			/* Second VSYNC */
-			_ipu_di_sync_config(ipu,
-					disp, 		/* display */
-					9, 		/* counter */
-					v_total - 1, 	/* run count */
-					DI_SYNC_INT_HSYNC,	/* run_resolution */
-					v_total/2, 		/* offset  */
-					DI_SYNC_INT_HSYNC, 	/* offset resolution  */
-					0, 		/* repeat count */
-					DI_SYNC_HSYNC, 	/* CNT_CLR_SEL */
-					0, 		/* CNT_POLARITY_GEN_EN  */
-					DI_SYNC_NONE, 	/* CNT_POLARITY_CLR_SEL  */
-					DI_SYNC_NONE, 	/* CNT_POLARITY_TRIGGER_SEL */
-					0, 		/* COUNT UP */
-					2*div		/* COUNT DOWN */
-					);
-
-			/* set gentime select and tag sel */
-			reg = ipu_di_read(ipu, disp, DI_SW_GEN1(9));
-			reg &= 0x1FFFFFFF;
-			reg |= (3-1)<<29 | 0x00008000;
-			ipu_di_write(ipu, disp, reg, DI_SW_GEN1(9));
-
-			ipu_di_write(ipu, disp, v_total / 2 - 1, DI_SCR_CONF);
-
-			/* set y_sel = 1 */
-			di_gen |= 0x10000000;
-			di_gen |= DI_GEN_POLARITY_5;
-			di_gen |= DI_GEN_POLARITY_8;
+			if((pixel_fmt == IPU_PIX_FMT_BT656) || (pixel_fmt == IPU_PIX_FMT_BT1120)) {
+				/* COUNTER_1: basic clock */
+				_ipu_di_sync_config(ipu,
+						disp, 		/* display */
+						DI_BT656_SYNC_BASECLK, 		/* counter */
+						0, 	/* run count */
+						DI_SYNC_CLK,	/* run_resolution */
+						0, 		/* offset */
+						DI_SYNC_NONE, 	/* offset resolution */
+						0, 		/* repeat count */
+						DI_SYNC_NONE, 	/* CNT_CLR_SEL */
+						0, 		/* CNT_POLARITY_GEN_EN */
+						DI_SYNC_NONE, 	/* CNT_POLARITY_CLR_SEL */
+						DI_SYNC_NONE, 	/* CNT_POLARITY_TRIGGER_SEL */
+						0, 		/* COUNT UP */
+						0		/* COUNT DOWN */
+						);
+
+				/* COUNTER_2: HSYNC for each line */
+				_ipu_di_sync_config(ipu,
+						disp, 		/* display */
+						DI_BT656_SYNC_HSYNC, 		/* counter */
+						h_total - 1, 	/* run count */
+						DI_SYNC_CLK,	/* run_resolution */
+						0, 		/* offset */
+						DI_SYNC_NONE, 	/* offset resolution */
+						0, 		/* repeat count */
+						DI_SYNC_NONE, 	/* CNT_CLR_SEL */
+						0, 		/* CNT_POLARITY_GEN_EN */
+						DI_SYNC_NONE, 	/* CNT_POLARITY_CLR_SEL */
+						DI_SYNC_NONE, 	/* CNT_POLARITY_TRIGGER_SEL */
+						0, 		/* COUNT UP */
+						2*div		/* COUNT DOWN */
+						);
+
+				/* COUNTER_3: internal VSYNC for each frame */
+				_ipu_di_sync_config(ipu,
+						disp, 		/* display */
+						DI_BT656_SYNC_IVSYNC, 		/* counter */
+						v_total - 1, 	/* run count */
+						DI_BT656_SYNC_HSYNC,	/* run_resolution */
+						0, 			/* offset */
+						DI_SYNC_NONE, 	/* offset resolution */
+						0, 		/* repeat count */
+						DI_SYNC_NONE, 	/* CNT_CLR_SEL */
+						0, 		/* CNT_POLARITY_GEN_EN */
+						DI_SYNC_NONE, 	/* CNT_POLARITY_CLR_SEL */
+						DI_SYNC_NONE, 	/* CNT_POLARITY_TRIGGER_SEL */
+						0, 		/* COUNT UP */
+						0		/* COUNT DOWN */
+						);
+
+				vsync_cnt = DI_BT656_SYNC_VSYNC;
+
+				/* COUNTER_4: VSYNC for field1 only */
+				_ipu_di_sync_config(ipu,
+						disp, 		/* display */
+						DI_BT656_SYNC_VSYNC, 		/* counter */
+						0, 	/* run count */
+						DI_BT656_SYNC_HSYNC,	/* run_resolution */
+						bt656_v_start_width_field0 + height / 2 + bt656_v_end_width_field0, 	/*  offset */
+						DI_BT656_SYNC_HSYNC, 	/* offset resolution */
+						1, 		/* repeat count */
+						DI_BT656_SYNC_IVSYNC, 	/* CNT_CLR_SEL */
+						0, 		/* CNT_POLARITY_GEN_EN */
+						DI_SYNC_NONE, 	/* CNT_POLARITY_CLR_SEL */
+						DI_SYNC_NONE, 	/* CNT_POLARITY_TRIGGER_SEL */
+						0, 		/* COUNT UP */
+						2*div		/* COUNT DOWN */
+						);
+
+				/* COUNTER_5: first active line for field0 */
+				_ipu_di_sync_config(ipu,
+						disp, 		/* display */
+						DI_BT656_SYNC_AFIELD, 		/* counter */
+						0, 		/* run count */
+						DI_BT656_SYNC_HSYNC,	/* run_resolution */
+						bt656_v_start_width_field0 + height / 2 + bt656_v_end_width_field0 + 2, 		/*  offset */
+						DI_BT656_SYNC_HSYNC, 	/* offset resolution */
+						1, 	/* repeat count */
+						DI_BT656_SYNC_IVSYNC, 		/* CNT_CLR_SEL */
+						0, 		/* CNT_POLARITY_GEN_EN */
+						DI_SYNC_NONE, 	/* CNT_POLARITY_CLR_SEL */
+						DI_SYNC_NONE, 	/* CNT_POLARITY_TRIGGER_SEL */
+						0, 		/* COUNT UP */
+						0		/* COUNT DOWN */
+						);
+
+				/* COUNTER_9: VSYNC for field0 only */
+				_ipu_di_sync_config(ipu,
+						disp, 		/* display */
+						DI_BT656_SYNC_NVSYNC, 		/* counter */
+						0, 	/* run count */
+						DI_BT656_SYNC_HSYNC - 1,	/* run_resolution, the counter#9 setting is different with others , no necessary to +1! */
+						0, 		/* offset  */
+						DI_SYNC_NONE, 	/* offset resolution  */
+						1, 		/* repeat count */
+						DI_BT656_SYNC_IVSYNC - 1, 	/* CNT_CLR_SEL, the counter#9 setting is different with others , no necessary to +1! */
+						0, 		/* CNT_POLARITY_GEN_EN  */
+						DI_SYNC_NONE, 	/* CNT_POLARITY_CLR_SEL  */
+						DI_SYNC_NONE, 	/* CNT_POLARITY_TRIGGER_SEL */
+						0, 		/* COUNT UP */
+						2*div		/* COUNT DOWN */
+						);
+
+				/* set gentime select and tag sel */
+				reg = ipu_di_read(ipu, disp, DI_SW_GEN1(9));
+				reg &= 0x1FFFFFFF;
+				reg |= (DI_BT656_SYNC_VSYNC - 1) << 29;
+				ipu_di_write(ipu, disp, reg, DI_SW_GEN1(9));
+
+				ipu_di_write(ipu, disp, height / 2 + bt656_v_start_width_field0 + bt656_v_end_width_field0 - 1, DI_SCR_CONF);
+
+				/* set y_sel = DI_BT656_SYNC_HSYNC - 1 */
+				di_gen |= ((DI_BT656_SYNC_HSYNC - 1) << 28);
+			} else {
+				/* Setup internal HSYNC waveform */
+				_ipu_di_sync_config(ipu,
+						disp, 		/* display */
+						1, 		/* counter */
+						h_total/2 - 1, 	/* run count */
+						DI_SYNC_CLK,	/* run_resolution */
+						0, 		/* offset */
+						DI_SYNC_NONE, 	/* offset resolution */
+						0, 		/* repeat count */
+						DI_SYNC_NONE, 	/* CNT_CLR_SEL */
+						0, 		/* CNT_POLARITY_GEN_EN */
+						DI_SYNC_NONE, 	/* CNT_POLARITY_CLR_SEL */
+						DI_SYNC_NONE, 	/* CNT_POLARITY_TRIGGER_SEL */
+						0, 		/* COUNT UP */
+						0		/* COUNT DOWN */
+						);
+
+				/* Field 1 VSYNC waveform */
+				_ipu_di_sync_config(ipu,
+						disp, 		/* display */
+						2, 		/* counter */
+						h_total - 1, 	/* run count */
+						DI_SYNC_CLK,	/* run_resolution */
+						0, 		/* offset */
+						DI_SYNC_NONE, 	/* offset resolution */
+						0, 		/* repeat count */
+						DI_SYNC_NONE, 	/* CNT_CLR_SEL */
+						0, 		/* CNT_POLARITY_GEN_EN */
+						DI_SYNC_NONE, 	/* CNT_POLARITY_CLR_SEL */
+						DI_SYNC_NONE, 	/* CNT_POLARITY_TRIGGER_SEL */
+						0, 		/* COUNT UP */
+						2*div		/* COUNT DOWN */
+						);
+
+				/* Setup internal HSYNC waveform */
+				_ipu_di_sync_config(ipu,
+						disp, 		/* display */
+						3, 		/* counter */
+						v_total*2 - 1, 	/* run count */
+						DI_SYNC_INT_HSYNC,	/* run_resolution */
+						1, 			/* offset */
+						DI_SYNC_INT_HSYNC, 	/* offset resolution */
+						0, 		/* repeat count */
+						DI_SYNC_NONE, 	/* CNT_CLR_SEL */
+						0, 		/* CNT_POLARITY_GEN_EN */
+						DI_SYNC_NONE, 	/* CNT_POLARITY_CLR_SEL */
+						DI_SYNC_NONE, 	/* CNT_POLARITY_TRIGGER_SEL */
+						0, 		/* COUNT UP */
+						2*div		/* COUNT DOWN */
+						);
+
+				/* Active Field ? */
+				_ipu_di_sync_config(ipu,
+						disp, 		/* display */
+						4, 		/* counter */
+						v_total/2 - 1, 	/* run count */
+						DI_SYNC_HSYNC,	/* run_resolution */
+						v_start_width, 	/*  offset */
+						DI_SYNC_HSYNC, 	/* offset resolution */
+						2, 		/* repeat count */
+						DI_SYNC_VSYNC, 	/* CNT_CLR_SEL */
+						0, 		/* CNT_POLARITY_GEN_EN */
+						DI_SYNC_NONE, 	/* CNT_POLARITY_CLR_SEL */
+						DI_SYNC_NONE, 	/* CNT_POLARITY_TRIGGER_SEL */
+						0, 		/* COUNT UP */
+						0		/* COUNT DOWN */
+						);
+
+				/* Active Line */
+				_ipu_di_sync_config(ipu,
+						disp, 		/* display */
+						5, 		/* counter */
+						0, 		/* run count */
+						DI_SYNC_HSYNC,	/* run_resolution */
+						0, 		/*  offset */
+						DI_SYNC_NONE, 	/* offset resolution */
+						height/2, 	/* repeat count */
+						4, 		/* CNT_CLR_SEL */
+						0, 		/* CNT_POLARITY_GEN_EN */
+						DI_SYNC_NONE, 	/* CNT_POLARITY_CLR_SEL */
+						DI_SYNC_NONE, 	/* CNT_POLARITY_TRIGGER_SEL */
+						0, 		/* COUNT UP */
+						0		/* COUNT DOWN */
+						);
+
+				/* Field 0 VSYNC waveform */
+				_ipu_di_sync_config(ipu,
+						disp, 		/* display */
+						6, 		/* counter */
+						v_total - 1, 	/* run count */
+						DI_SYNC_HSYNC,	/* run_resolution */
+						0, 		/* offset */
+						DI_SYNC_NONE, 	/* offset resolution */
+						0, 		/* repeat count */
+						DI_SYNC_NONE, 	/* CNT_CLR_SEL  */
+						0, 		/* CNT_POLARITY_GEN_EN */
+						DI_SYNC_NONE, 	/* CNT_POLARITY_CLR_SEL */
+						DI_SYNC_NONE, 	/* CNT_POLARITY_TRIGGER_SEL */
+						0, 		/* COUNT UP */
+						0		/* COUNT DOWN */
+						);
+
+				/* DC VSYNC waveform */
+				vsync_cnt = 7;
+				_ipu_di_sync_config(ipu,
+						disp, 		/* display */
+						7, 		/* counter */
+						v_total/2 - 1, 	/* run count */
+						DI_SYNC_HSYNC,	/* run_resolution  */
+						9, 		/* offset  */
+						DI_SYNC_HSYNC, 	/* offset resolution */
+						2, 		/* repeat count */
+						DI_SYNC_VSYNC, 	/* CNT_CLR_SEL */
+						0, 		/* CNT_POLARITY_GEN_EN */
+						DI_SYNC_NONE, 	/* CNT_POLARITY_CLR_SEL */
+						DI_SYNC_NONE, 	/* CNT_POLARITY_TRIGGER_SEL */
+						0, 		/* COUNT UP */
+						0		/* COUNT DOWN */
+						);
+
+				/* active pixel waveform */
+				_ipu_di_sync_config(ipu,
+						disp, 		/* display */
+						8, 		/* counter */
+						0, 		/* run count  */
+						DI_SYNC_CLK,	/* run_resolution */
+						h_start_width, 	/* offset  */
+						DI_SYNC_CLK, 	/* offset resolution */
+						width, 		/* repeat count  */
+						5, 		/* CNT_CLR_SEL  */
+						0, 		/* CNT_POLARITY_GEN_EN  */
+						DI_SYNC_NONE, 	/* CNT_POLARITY_CLR_SEL */
+						DI_SYNC_NONE, 	/* CNT_POLARITY_TRIGGER_SEL  */
+						0, 		/* COUNT UP  */
+						0		/* COUNT DOWN */
+						);
+
+				/* Second VSYNC */
+				_ipu_di_sync_config(ipu,
+						disp, 		/* display */
+						9, 		/* counter */
+						v_total - 1, 	/* run count */
+						DI_SYNC_INT_HSYNC,	/* run_resolution */
+						v_total/2, 		/* offset  */
+						DI_SYNC_INT_HSYNC, 	/* offset resolution  */
+						0, 		/* repeat count */
+						DI_SYNC_HSYNC, 	/* CNT_CLR_SEL */
+						0, 		/* CNT_POLARITY_GEN_EN  */
+						DI_SYNC_NONE, 	/* CNT_POLARITY_CLR_SEL  */
+						DI_SYNC_NONE, 	/* CNT_POLARITY_TRIGGER_SEL */
+						0, 		/* COUNT UP */
+						2*div		/* COUNT DOWN */
+						);
+
+				/* set gentime select and tag sel */
+				reg = ipu_di_read(ipu, disp, DI_SW_GEN1(9));
+				reg &= 0x1FFFFFFF;
+				reg |= (3-1)<<29 | 0x00008000;
+				ipu_di_write(ipu, disp, reg, DI_SW_GEN1(9));
+
+				ipu_di_write(ipu, disp, v_total / 2 - 1, DI_SCR_CONF);
+
+				/* set y_sel = 1 */
+				di_gen |= 0x10000000;
+				di_gen |= DI_GEN_POLARITY_5;
+				di_gen |= DI_GEN_POLARITY_8;
+			}
 		} else {
 			/* Setup internal HSYNC waveform */
 			_ipu_di_sync_config(ipu, disp, 1, h_total - 1, DI_SYNC_CLK,
@@ -1555,13 +2400,35 @@ int32_t ipu_init_sync_panel(struct ipu_soc *ipu, int disp, uint32_t pixel_clk,
 					v_end_width + height / 2 - 1, DI_SCR_CONF);
 		}
 
-		/* Init template microcode */
-		_ipu_dc_write_tmpl(ipu, 0, WROD(0), 0, map, SYNC_WAVE, 0, 8, 1);
+		if((pixel_fmt == IPU_PIX_FMT_BT656) || (pixel_fmt == IPU_PIX_FMT_BT1120)) {
+			/* Init template microcode */
+			if(pixel_fmt == IPU_PIX_FMT_BT656) {
+				_ipu_dc_setup_bt656_interlaced(ipu, u_map, y_map, v_map, 0, BT656_IF_DI_MSB, 
+						bt656_h_start_width, 
+						bt656_v_start_width_field0, bt656_v_end_width_field0, 
+						bt656_v_start_width_field1, bt656_v_end_width_field1);
+			} else {
+				_ipu_dc_setup_bt656_interlaced(ipu, u_map, y_map, v_map, 1, BT656_IF_DI_MSB, 
+						bt656_h_start_width, 
+						bt656_v_start_width_field0, bt656_v_end_width_field0, 
+						bt656_v_start_width_field1, bt656_v_end_width_field1);
+			}
 
-		if (sig.Hsync_pol)
-			di_gen |= DI_GEN_POLARITY_3;
-		if (sig.Vsync_pol)
-			di_gen |= DI_GEN_POLARITY_2;
+			ipu_dc_write(ipu, (width - 1), DC_UGDE_3(disp));
+
+			if (sig.Hsync_pol)
+				di_gen |= DI_GEN_POLARITY_2;
+			if (sig.Vsync_pol)
+				di_gen |= DI_GEN_POLARITY_4;
+		} else {
+			/* Init template microcode */
+			_ipu_dc_write_tmpl(ipu, 0, WROD, 0, map, SYNC_WAVE, 0, 8, 1, 0, 0);
+
+			if (sig.Hsync_pol)
+				di_gen |= DI_GEN_POLARITY_2;
+			if (sig.Vsync_pol)
+				di_gen |= DI_GEN_POLARITY_3;
+		}
 	} else {
 		/* Setup internal HSYNC waveform */
 		_ipu_di_sync_config(ipu, disp, 1, h_total - 1, DI_SYNC_CLK,
@@ -1635,30 +2502,30 @@ int32_t ipu_init_sync_panel(struct ipu_soc *ipu, int disp, uint32_t pixel_clk,
 				(pixel_fmt == IPU_PIX_FMT_UYVY) ||
 				(pixel_fmt == IPU_PIX_FMT_YVYU) ||
 				(pixel_fmt == IPU_PIX_FMT_VYUY)) {
-				_ipu_dc_write_tmpl(ipu, 8, WROD(0), 0, (map - 1), SYNC_WAVE, 0, 5, 1);
-				_ipu_dc_write_tmpl(ipu, 9, WROD(0), 0, map, SYNC_WAVE, 0, 5, 1);
+				_ipu_dc_write_tmpl(ipu, 8, WROD, 0, (map - 1), SYNC_WAVE, 0, 5, 1, 0, 0);
+				_ipu_dc_write_tmpl(ipu, 9, WROD, 0, map, SYNC_WAVE, 0, 5, 1, 0, 0);
 				/* configure user events according to DISP NUM */
 				ipu_dc_write(ipu, (width - 1), DC_UGDE_3(disp));
 			}
-			_ipu_dc_write_tmpl(ipu, 2, WROD(0), 0, map, SYNC_WAVE, 8, 5, 1);
-			_ipu_dc_write_tmpl(ipu, 3, WROD(0), 0, map, SYNC_WAVE, 4, 5, 0);
-			_ipu_dc_write_tmpl(ipu, 4, WRG, 0, map, NULL_WAVE, 0, 0, 1);
-			_ipu_dc_write_tmpl(ipu, 1, WROD(0), 0, map, SYNC_WAVE, 0, 5, 1);
+			_ipu_dc_write_tmpl(ipu, 2, WROD, 0, map, SYNC_WAVE, 8, 5, 1, 0, 0);
+			_ipu_dc_write_tmpl(ipu, 3, WROD, 0, map, SYNC_WAVE, 4, 5, 0, 0, 0);
+			_ipu_dc_write_tmpl(ipu, 4, WRG, 0, map, NULL_WAVE, 0, 0, 1, 0, 0);
+			_ipu_dc_write_tmpl(ipu, 1, WROD, 0, map, SYNC_WAVE, 0, 5, 1, 0, 0);
 
 		} else {
 			if ((pixel_fmt == IPU_PIX_FMT_YUYV) ||
 				(pixel_fmt == IPU_PIX_FMT_UYVY) ||
 				(pixel_fmt == IPU_PIX_FMT_YVYU) ||
 				(pixel_fmt == IPU_PIX_FMT_VYUY)) {
-				_ipu_dc_write_tmpl(ipu, 10, WROD(0), 0, (map - 1), SYNC_WAVE, 0, 5, 1);
-				_ipu_dc_write_tmpl(ipu, 11, WROD(0), 0, map, SYNC_WAVE, 0, 5, 1);
+				_ipu_dc_write_tmpl(ipu, 10, WROD, 0, (map - 1), SYNC_WAVE, 0, 5, 1, 0, 0);
+				_ipu_dc_write_tmpl(ipu, 11, WROD, 0, map, SYNC_WAVE, 0, 5, 1, 0, 0);
 				/* configure user events according to DISP NUM */
 				ipu_dc_write(ipu, width - 1, DC_UGDE_3(disp));
 			}
-		   _ipu_dc_write_tmpl(ipu, 5, WROD(0), 0, map, SYNC_WAVE, 8, 5, 1);
-		   _ipu_dc_write_tmpl(ipu, 6, WROD(0), 0, map, SYNC_WAVE, 4, 5, 0);
-		   _ipu_dc_write_tmpl(ipu, 7, WRG, 0, map, NULL_WAVE, 0, 0, 1);
-		   _ipu_dc_write_tmpl(ipu, 12, WROD(0), 0, map, SYNC_WAVE, 0, 5, 1);
+		   _ipu_dc_write_tmpl(ipu, 5, WROD, 0, map, SYNC_WAVE, 8, 5, 1, 0, 0);
+		   _ipu_dc_write_tmpl(ipu, 6, WROD, 0, map, SYNC_WAVE, 4, 5, 0, 0, 0);
+		   _ipu_dc_write_tmpl(ipu, 7, WRG, 0, map, NULL_WAVE, 0, 0, 1, 0, 0);
+		   _ipu_dc_write_tmpl(ipu, 12, WROD, 0, map, SYNC_WAVE, 0, 5, 1, 0, 0);
 		}
 
 		if (sig.Hsync_pol) {
@@ -1682,10 +2549,19 @@ int32_t ipu_init_sync_panel(struct ipu_soc *ipu, int disp, uint32_t pixel_clk,
 	if (!sig.clk_pol)
 		di_gen |= DI_GEN_POLARITY_DISP_CLK;
 
+	if((pixel_fmt == IPU_PIX_FMT_BT656) || (pixel_fmt == IPU_PIX_FMT_BT1120)) {
+		/* select external VSYNC for DI error recovery */
+		di_gen |= (1 << 10);
+	}
+
 	ipu_di_write(ipu, disp, di_gen, DI_GENERAL);
 
-	ipu_di_write(ipu, disp, (--vsync_cnt << DI_VSYNC_SEL_OFFSET) |
-			0x00000002, DI_SYNC_AS_GEN);
+	if((pixel_fmt == IPU_PIX_FMT_BT656) || (pixel_fmt == IPU_PIX_FMT_BT1120))
+		ipu_di_write(ipu, disp, (--vsync_cnt << DI_VSYNC_SEL_OFFSET), DI_SYNC_AS_GEN);
+	else
+		ipu_di_write(ipu, disp, (--vsync_cnt << DI_VSYNC_SEL_OFFSET) |
+				0x00000002, DI_SYNC_AS_GEN);
+
 	reg = ipu_di_read(ipu, disp, DI_POL);
 	reg &= ~(DI_POL_DRDY_DATA_POLARITY | DI_POL_DRDY_POLARITY_15);
 	if (sig.enable_pol)
@@ -1753,7 +2629,7 @@ int ipu_init_async_panel(struct ipu_soc *ipu, int disp, int type, uint32_t cycle
 		_ipu_di_data_pin_config(ipu, disp, ASYNC_SER_WAVE, DI_PIN_SER_RS,
 					2, 0, 0);
 
-		_ipu_dc_write_tmpl(ipu, 0x64, WROD(0), 0, map, ASYNC_SER_WAVE, 0, 0, 1);
+		_ipu_dc_write_tmpl(ipu, 0x64, WROD, 0, map, ASYNC_SER_WAVE, 0, 0, 1, 0, 0);
 
 		/* Configure DC for serial panel */
 		ipu_dc_write(ipu, 0x14, DC_DISP_CONF1(DC_DISP_ID_SERIAL));
diff --git a/drivers/mxc/ipu3/ipu_prv.h b/drivers/mxc/ipu3/ipu_prv.h
index a0d71dd5dad3..cb1b54b37155 100644
--- a/drivers/mxc/ipu3/ipu_prv.h
+++ b/drivers/mxc/ipu3/ipu_prv.h
@@ -124,7 +124,6 @@ struct ipu_soc {
 
 	int	vdoa_en;
 	struct task_struct *thread[2];
-
 };
 
 struct ipu_channel {
diff --git a/drivers/mxc/ipu3/ipu_regs.h b/drivers/mxc/ipu3/ipu_regs.h
index 458b0e09115d..55acb435672d 100644
--- a/drivers/mxc/ipu3/ipu_regs.h
+++ b/drivers/mxc/ipu3/ipu_regs.h
@@ -706,8 +706,22 @@ enum di_sync_wave {
 	DI_SYNC_DE = 5,
 };
 
+enum di_bt656_sync_wave {
+	DI_BT656_SYNC_BASECLK = 1,
+	DI_BT656_SYNC_HSYNC = 2,
+	DI_BT656_SYNC_IVSYNC = 3,
+	DI_BT656_SYNC_VSYNC = 4,
+	DI_BT656_SYNC_AFIELD = 5,
+	DI_BT656_SYNC_NVSYNC = 9,
+};
+
 /* DC template opcodes */
-#define WROD(lf)		(0x18 | (lf << 1))
-#define WRG	        	(0x01)
+enum dc_template_cmd {
+	WROD = 0,	/* real opcode is 0x18 */
+	WRG = 1,	/* real opcode is 0x01 */
+	HMA = 2,		/* real opcode is 0x02 */ 
+	HMA1 = 3,	/* real opcode is 0x01 */
+	BMA = 4,		/* real opcode is 0x03 */
+};
 
 #endif
diff --git a/drivers/video/mxc/Kconfig b/drivers/video/mxc/Kconfig
index 83b094267760..0729771220fe 100644
--- a/drivers/video/mxc/Kconfig
+++ b/drivers/video/mxc/Kconfig
@@ -39,6 +39,11 @@ config FB_MXC_LDB
 	depends on FB_MXC_SYNC_PANEL
 	depends on MXC_IPU_V3
 
+config FB_MXC_BT656
+	tristate "MXC BT656 and BT1120 output"
+	depends on FB_MXC_SYNC_PANEL
+	depends on MXC_IPU_V3
+
 config FB_MXC_MIPI_DSI
 	tristate "MXC MIPI_DSI"
 	depends on FB_MXC_SYNC_PANEL
diff --git a/drivers/video/mxc/Makefile b/drivers/video/mxc/Makefile
index 092d0aacba27..064d1ce56a80 100644
--- a/drivers/video/mxc/Makefile
+++ b/drivers/video/mxc/Makefile
@@ -2,6 +2,7 @@ obj-$(CONFIG_FB_MXC_TVOUT_TVE)             	+= tve.o
 obj-$(CONFIG_FB_MXC_SII902X)                    += mxcfb_sii902x.o
 obj-$(CONFIG_FB_MXC_SII902X_ELCDIF)           += mxcfb_sii902x_elcdif.o
 obj-$(CONFIG_FB_MXC_LDB)			+= ldb.o
+obj-$(CONFIG_FB_MXC_BT656)			+= mxc_bt656if.o
 obj-$(CONFIG_FB_MXC_MIPI_DSI)			+= mipi_dsi.o
 obj-$(CONFIG_FB_MXC_TRULY_WVGA_SYNC_PANEL) 	+= mxcfb_hx8369_wvga.o
 obj-$(CONFIG_FB_MXC_EDID)			+= mxc_edid.o
diff --git a/drivers/video/mxc/mxc_bt656if.c b/drivers/video/mxc/mxc_bt656if.c
new file mode 100644
index 000000000000..dc174252f433
--- /dev/null
+++ b/drivers/video/mxc/mxc_bt656if.c
@@ -0,0 +1,181 @@
+/*
+ * Copyright (C) 2013 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/mxcfb.h>
+#include <linux/fsl_devices.h>
+#include "mxc_dispdrv.h"
+
+struct mxc_bt656if_data {
+	struct platform_device *pdev;
+	struct mxc_dispdrv_handle *disp_bt656if;
+};
+
+#define DISPDRV_BT656	"bt656"
+
+/*
+ * left_margin: used for field0 vStart width in lines
+ *
+ * right_margin: used for field0 vEnd width in lines
+ *
+ * up_margin: used for field1 vStart width in lines
+ *
+ * down_margin: used for field1 vEnd width in lines
+ *
+ * hsync_len: EAV Code + Blanking Video + SAV Code (in pixel clock count)
+ *         For BT656 NTSC, it is 4 + 67*4 + 4 = 276.
+ *         For BT1120 NTSC, it is 4 + 67*2 + 4 = 142.
+ *         For BT656 PAL, it is 4 + 70*4 + 4 = 288.
+ *         For BT1120 PAL, it is 4 + 70*2 + 4 = 148.
+ *
+ * vsync_len: not used, set to 1
+ */
+static struct fb_videomode bt656if_modedb[] = {
+	{
+	 /* NTSC Interlaced output */
+	 "BT656-NTSC", 60, 720, 480, 37037,
+	 19, 3,
+	 20, 3,
+	 276, 1,
+	 FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	 FB_VMODE_INTERLACED,
+	 FB_MODE_IS_DETAILED,},
+	{
+	 /* PAL Interlaced output */
+	 "BT656-PAL", 50, 720, 576, 37037,
+	 22, 2,
+	 23, 2,
+	 288, 1,
+	 FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	 FB_VMODE_INTERLACED,
+	 FB_MODE_IS_DETAILED,},
+	{
+	 /* NTSC Interlaced output */
+	 "BT1120-NTSC", 30, 720, 480, 74074,
+	 19, 3,
+	 20, 3,
+	 142, 1,
+	 FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	 FB_VMODE_INTERLACED,
+	 FB_MODE_IS_DETAILED,},
+	{
+	 /* PAL Interlaced output */
+	 "BT1120-PAL", 25, 720, 576, 74074,
+	 22, 2,
+	 23, 2,
+	 148, 1,
+	 FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	 FB_VMODE_INTERLACED,
+	 FB_MODE_IS_DETAILED,},
+};
+static int bt656if_modedb_sz = ARRAY_SIZE(bt656if_modedb);
+
+static int bt656if_init(struct mxc_dispdrv_handle *disp,
+	struct mxc_dispdrv_setting *setting)
+{
+	int ret, i;
+	struct mxc_bt656if_data *bt656if = mxc_dispdrv_getdata(disp);
+	struct fsl_mxc_lcd_platform_data *plat_data
+			= bt656if->pdev->dev.platform_data;
+	struct fb_videomode *modedb = bt656if_modedb;
+	int modedb_sz = bt656if_modedb_sz;
+
+	/* use platform defined ipu/di */
+	setting->dev_id = plat_data->ipu_id;
+	setting->disp_id = plat_data->disp_id;
+
+	ret = fb_find_mode(&setting->fbi->var, setting->fbi, setting->dft_mode_str,
+				modedb, modedb_sz, NULL, setting->default_bpp);
+	if (!ret) {
+		fb_videomode_to_var(&setting->fbi->var, &modedb[0]);
+		setting->if_fmt = plat_data->default_ifmt;
+	}
+
+	INIT_LIST_HEAD(&setting->fbi->modelist);
+	for (i = 0; i < modedb_sz; i++) {
+		fb_add_videomode(&modedb[i],
+				&setting->fbi->modelist);
+	}
+
+	return ret;
+}
+
+static void bt656if_deinit(struct mxc_dispdrv_handle *disp)
+{
+	/*TODO*/
+}
+
+static struct mxc_dispdrv_driver bt656if_drv = {
+	.name = DISPDRV_BT656,
+	.init = bt656if_init,
+	.deinit = bt656if_deinit,
+};
+
+static int mxc_bt656if_probe(struct platform_device *pdev)
+{
+	int ret = 0;
+	struct mxc_bt656if_data *bt656if;
+
+	bt656if = kzalloc(sizeof(struct mxc_bt656if_data), GFP_KERNEL);
+	if (!bt656if) {
+		ret = -ENOMEM;
+		goto alloc_failed;
+	}
+
+	bt656if->pdev = pdev;
+	bt656if->disp_bt656if = mxc_dispdrv_register(&bt656if_drv);
+	mxc_dispdrv_setdata(bt656if->disp_bt656if, bt656if);
+
+	dev_set_drvdata(&pdev->dev, bt656if);
+
+alloc_failed:
+	return ret;
+}
+
+static int mxc_bt656if_remove(struct platform_device *pdev)
+{
+	struct mxc_bt656if_data *bt656if = dev_get_drvdata(&pdev->dev);
+
+	mxc_dispdrv_puthandle(bt656if->disp_bt656if);
+	mxc_dispdrv_unregister(bt656if->disp_bt656if);
+	kfree(bt656if);
+	return 0;
+}
+
+static struct platform_driver mxc_bt656if_driver = {
+	.driver = {
+		   .name = "mxc_bt656if",
+		   },
+	.probe = mxc_bt656if_probe,
+	.remove = mxc_bt656if_remove,
+};
+
+static int __init mxc_bt656if_init(void)
+{
+	return platform_driver_register(&mxc_bt656if_driver);
+}
+
+static void __exit mxc_bt656if_exit(void)
+{
+	platform_driver_unregister(&mxc_bt656if_driver);
+}
+
+module_init(mxc_bt656if_init);
+module_exit(mxc_bt656if_exit);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("i.MX ipuv3 BT656 and BT1120 output driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/mxc/mxc_ipuv3_fb.c b/drivers/video/mxc/mxc_ipuv3_fb.c
index 66ea47daf122..8412c250edbe 100644
--- a/drivers/video/mxc/mxc_ipuv3_fb.c
+++ b/drivers/video/mxc/mxc_ipuv3_fb.c
@@ -67,6 +67,7 @@ struct mxcfb_info {
 	ipu_channel_t ipu_ch;
 	int ipu_id;
 	int ipu_di;
+	u32 fb_pix_fmt;
 	u32 ipu_di_pix_fmt;
 	bool ipu_int_clk;
 	bool overlay;
@@ -135,6 +136,26 @@ enum {
 static bool g_dp_in_use[2];
 LIST_HEAD(fb_alloc_list);
 
+static bool is_rgb_pixfmt_fb(uint32_t pixfmt)
+{
+	bool ret = false;
+
+	switch (pixfmt) {
+	case IPU_PIX_FMT_RGB565:
+	case IPU_PIX_FMT_RGB24:
+	case IPU_PIX_FMT_BGR24:
+	case IPU_PIX_FMT_RGB32:
+	case IPU_PIX_FMT_BGR32:
+	case IPU_PIX_FMT_ABGR32:
+		ret = true;
+		break;
+
+	default:
+		break;
+	}
+	return ret;
+}
+
 /* Return default standard(RGB) pixel format */
 static uint32_t bpp_to_pixfmt(int bpp)
 {
@@ -206,30 +227,6 @@ static int check_var_pixfmt(struct fb_var_screeninfo *var)
 	return ret;
 }
 
-static uint32_t fbi_to_pixfmt(struct fb_info *fbi)
-{
-	int i;
-	uint32_t pixfmt = 0;
-
-	if (fbi->var.nonstd)
-		return fbi->var.nonstd;
-
-	for (i = 0; i < ARRAY_SIZE(mxcfb_pfmts); i++) {
-		if (bitfield_is_equal(fbi->var.red, mxcfb_pfmts[i].red) &&
-		    bitfield_is_equal(fbi->var.green, mxcfb_pfmts[i].green) &&
-		    bitfield_is_equal(fbi->var.blue, mxcfb_pfmts[i].blue) &&
-		    bitfield_is_equal(fbi->var.transp, mxcfb_pfmts[i].transp)) {
-			pixfmt = mxcfb_pfmts[i].fb_pix_fmt;
-			break;
-		}
-	}
-
-	if (pixfmt == 0)
-		dev_err(fbi->device, "cannot get pixel format\n");
-
-	return pixfmt;
-}
-
 static struct fb_info *found_registered_fb(ipu_channel_t ipu_ch, int ipu_id)
 {
 	int i;
@@ -289,13 +286,13 @@ static int _setup_disp_channel1(struct fb_info *fbi)
 		if (fbi->var.vmode & FB_VMODE_INTERLACED)
 			params.mem_dc_sync.interlaced = true;
 		params.mem_dc_sync.out_pixel_fmt = mxc_fbi->ipu_di_pix_fmt;
-		params.mem_dc_sync.in_pixel_fmt = fbi_to_pixfmt(fbi);
+		params.mem_dc_sync.in_pixel_fmt = mxc_fbi->fb_pix_fmt;
 	} else {
 		params.mem_dp_bg_sync.di = mxc_fbi->ipu_di;
 		if (fbi->var.vmode & FB_VMODE_INTERLACED)
 			params.mem_dp_bg_sync.interlaced = true;
 		params.mem_dp_bg_sync.out_pixel_fmt = mxc_fbi->ipu_di_pix_fmt;
-		params.mem_dp_bg_sync.in_pixel_fmt = fbi_to_pixfmt(fbi);
+		params.mem_dp_bg_sync.in_pixel_fmt = mxc_fbi->fb_pix_fmt;
 		if (mxc_fbi->alpha_chan_en)
 			params.mem_dp_bg_sync.alpha_chan_en = true;
 	}
@@ -312,7 +309,7 @@ static int _setup_disp_channel2(struct fb_info *fbi)
 	unsigned long base;
 	unsigned int fr_xoff, fr_yoff, fr_w, fr_h;
 
-	switch (fbi_to_pixfmt(fbi)) {
+	switch (mxc_fbi->fb_pix_fmt) {
 	case IPU_PIX_FMT_YUV420P2:
 	case IPU_PIX_FMT_YVU420P:
 	case IPU_PIX_FMT_NV12:
@@ -360,7 +357,7 @@ static int _setup_disp_channel2(struct fb_info *fbi)
 
 	retval = ipu_init_channel_buffer(mxc_fbi->ipu,
 					 mxc_fbi->ipu_ch, IPU_INPUT_BUFFER,
-					 fbi_to_pixfmt(fbi),
+					 mxc_fbi->fb_pix_fmt,
 					 fbi->var.xres, fbi->var.yres,
 					 fb_stride,
 					 fbi->var.rotate,
@@ -378,7 +375,7 @@ static int _setup_disp_channel2(struct fb_info *fbi)
 	/* update u/v offset */
 	ipu_update_channel_offset(mxc_fbi->ipu, mxc_fbi->ipu_ch,
 			IPU_INPUT_BUFFER,
-			fbi_to_pixfmt(fbi),
+			mxc_fbi->fb_pix_fmt,
 			fr_w,
 			fr_h,
 			fr_w,
@@ -1314,6 +1311,25 @@ static int mxcfb_ioctl(struct fb_info *fbi, unsigned int cmd, unsigned long arg)
 
 			break;
 		}
+	case MXCFB_GET_FBFMT:
+		{
+			struct mxcfb_info *mxc_fbi =
+				(struct mxcfb_info *)fbi->par;
+
+			if (put_user(mxc_fbi->fb_pix_fmt, argp))
+				return -EFAULT;
+			break;
+		}
+	case MXCFB_SET_FBFMT:
+		{
+			struct mxcfb_info *mxc_fbi =
+				(struct mxcfb_info *)fbi->par;
+
+			if (get_user(mxc_fbi->fb_pix_fmt, argp))
+				return -EFAULT;
+
+			break;
+		}
 	default:
 		retval = -EINVAL;
 	}
@@ -1401,7 +1417,7 @@ mxcfb_pan_display(struct fb_var_screeninfo *var, struct fb_info *info)
 	if (y_bottom > info->var.yres_virtual)
 		return -EINVAL;
 
-	switch (fbi_to_pixfmt(info)) {
+	switch (mxc_fbi->fb_pix_fmt) {
 	case IPU_PIX_FMT_YUV420P2:
 	case IPU_PIX_FMT_YVU420P:
 	case IPU_PIX_FMT_NV12:
@@ -1488,7 +1504,7 @@ mxcfb_pan_display(struct fb_var_screeninfo *var, struct fb_info *info)
 		/* update u/v offset */
 		ipu_update_channel_offset(mxc_fbi->ipu, mxc_fbi->ipu_ch,
 				IPU_INPUT_BUFFER,
-				fbi_to_pixfmt(info),
+				mxc_fbi->fb_pix_fmt,
 				fr_w,
 				fr_h,
 				fr_w,
@@ -1887,6 +1903,10 @@ static int mxcfb_dispdrv_init(struct platform_device *pdev,
 		mxcfbi->ipu_di_pix_fmt = setting.if_fmt;
 		mxcfbi->default_bpp = setting.default_bpp;
 
+		/* if "fbpix" was not set from boot command, use default value */
+		if(mxcfbi->fb_pix_fmt == 0)
+			mxcfbi->fb_pix_fmt = bpp_to_pixfmt(setting.default_bpp);
+
 		/* setting */
 		mxcfbi->ipu_id = setting.dev_id;
 		mxcfbi->ipu_di = setting.disp_id;
@@ -1947,6 +1967,10 @@ static int mxcfb_option_setup(struct platform_device *pdev, struct fb_info *fbi)
 				pdata->interface_pix_fmt = IPU_PIX_FMT_YVYU;
 			else if (!strncmp(opt+3, "VYUY16", 6))
 				pdata->interface_pix_fmt = IPU_PIX_FMT_VYUY;
+			else if (!strncmp(opt+3, "BT656", 5))
+				pdata->interface_pix_fmt = IPU_PIX_FMT_BT656;
+			else if (!strncmp(opt+3, "BT1120", 6))
+				pdata->interface_pix_fmt = IPU_PIX_FMT_BT1120;
 		} else if (!strncmp(opt, "fbpix=", 6)) {
 			if (!strncmp(opt+6, "RGB24", 5))
 				fb_pix_fmt = IPU_PIX_FMT_RGB24;
@@ -1960,11 +1984,25 @@ static int mxcfb_option_setup(struct platform_device *pdev, struct fb_info *fbi)
 				fb_pix_fmt = IPU_PIX_FMT_ABGR32;
 			else if (!strncmp(opt+6, "RGB565", 6))
 				fb_pix_fmt = IPU_PIX_FMT_RGB565;
-
-			if (fb_pix_fmt) {
+			else if (!strncmp(opt+6, "YUYV16", 6))
+				fb_pix_fmt = IPU_PIX_FMT_YUYV;
+			else if (!strncmp(opt+6, "UYVY16", 6))
+				fb_pix_fmt = IPU_PIX_FMT_UYVY;
+			else if (!strncmp(opt+6, "YVYU16", 6))
+				fb_pix_fmt = IPU_PIX_FMT_YVYU;
+			else if (!strncmp(opt+6, "VYUY16", 6))
+				fb_pix_fmt = IPU_PIX_FMT_VYUY;
+			else if (!strncmp(opt+6, "YUV444", 6))
+				fb_pix_fmt = IPU_PIX_FMT_YUV444;
+			else if (!strncmp(opt+6, "VYU444", 6))
+				fb_pix_fmt = IPU_PIX_FMT_VYU444;
+
+			if (fb_pix_fmt && is_rgb_pixfmt_fb(fb_pix_fmt)) {
 				pixfmt_to_var(fb_pix_fmt, &fbi->var);
 				pdata->default_bpp =
 					fbi->var.bits_per_pixel;
+			} else {
+				pdata->default_bpp = fmt_to_bpp(fb_pix_fmt);
 			}
 		} else if (!strncmp(opt, "int_clk", 7)) {
 			pdata->int_clk = true;
@@ -1987,6 +2025,10 @@ static int mxcfb_option_setup(struct platform_device *pdev, struct fb_info *fbi)
 	if (fb_mode_str)
 		pdata->mode_str = fb_mode_str;
 
+	if(fb_pix_fmt == 0)
+		fb_pix_fmt = bpp_to_pixfmt(pdata->default_bpp);
+	pdata->fb_pix_fmt = fb_pix_fmt;
+
 	return 0;
 }
 
@@ -2148,12 +2190,15 @@ static int mxcfb_setup_overlay(struct platform_device *pdev,
 	mxcfbi_fg->ipu_ch = MEM_FG_SYNC;
 	mxcfbi_fg->ipu_di = -1;
 	mxcfbi_fg->ipu_di_pix_fmt = mxcfbi_bg->ipu_di_pix_fmt;
+	mxcfbi_fg->fb_pix_fmt = mxcfbi_bg->fb_pix_fmt;
+	mxcfbi_fg->default_bpp = mxcfbi_bg->default_bpp;
 	mxcfbi_fg->overlay = true;
 	mxcfbi_fg->cur_blank = mxcfbi_fg->next_blank = FB_BLANK_POWERDOWN;
 
 	/* Need dummy values until real panel is configured */
 	ovfbi->var.xres = 240;
 	ovfbi->var.yres = 320;
+	ovfbi->var.bits_per_pixel = mxcfbi_fg->default_bpp;
 
 	if (res && res->start && res->end) {
 		ovfbi->fix.smem_len = res->end - res->start + 1;
@@ -2236,6 +2281,7 @@ static int mxcfb_probe(struct platform_device *pdev)
 	mxcfbi = (struct mxcfb_info *)fbi->par;
 	mxcfbi->ipu_int_clk = plat_data->int_clk;
 	mxcfbi->late_init = plat_data->late_init;
+	mxcfbi->fb_pix_fmt = plat_data->fb_pix_fmt;
 	mxcfbi->first_set_par = true;
 	ret = mxcfb_dispdrv_init(pdev, fbi);
 	if (ret < 0)
diff --git a/include/linux/ipu.h b/include/linux/ipu.h
index f8178f1b0473..5fabcce5b711 100644
--- a/include/linux/ipu.h
+++ b/include/linux/ipu.h
@@ -96,6 +96,8 @@ typedef enum {
 #define IPU_PIX_FMT_GENERIC_32 fourcc('I', 'P', 'U', '1')	/*!< IPU Generic Data */
 #define IPU_PIX_FMT_LVDS666 fourcc('L', 'V', 'D', '6')	/*!< IPU Generic Data */
 #define IPU_PIX_FMT_LVDS888 fourcc('L', 'V', 'D', '8')	/*!< IPU Generic Data */
+#define IPU_PIX_FMT_BT656    fourcc('B', 'T', '6', '5')	/*!< BT656, 16 UYVY */
+#define IPU_PIX_FMT_BT1120    fourcc('B', 'T', '1', '1')	/*!< BT1120, 16 UYVY */
 /*! @} */
 /*! @name RGB Formats */
 /*! @{ */
diff --git a/include/linux/mxcfb.h b/include/linux/mxcfb.h
index be391e0b7e05..84c15e6112cc 100644
--- a/include/linux/mxcfb.h
+++ b/include/linux/mxcfb.h
@@ -144,6 +144,8 @@ struct mxcfb_waveform_modes {
 #define MXCFB_GET_DIFMT	       _IOR('F', 0x2A, u_int32_t)
 #define MXCFB_GET_FB_BLANK     _IOR('F', 0x2B, u_int32_t)
 #define MXCFB_SET_DIFMT		_IOW('F', 0x2C, u_int32_t)
+#define MXCFB_GET_FBFMT	       _IOR('F', 0x35, u_int32_t)
+#define MXCFB_SET_FBFMT	       _IOR('F', 0x36, u_int32_t)
 
 /* IOCTLs for E-ink panel updates */
 #define MXCFB_SET_WAVEFORM_MODES	_IOW('F', 0x2B, struct mxcfb_waveform_modes)