Linux 2.6.31 Release for Digi ConnectCore Wi-i.MX boards

Digi 01262011 Release

9 files changed, 591 insertions, 219 deletions

diff --git a/drivers/video/modedb.c b/drivers/video/modedb.c
index 34e4e7995169..9c4739c8b19c 100644
--- a/drivers/video/modedb.c
+++ b/drivers/video/modedb.c
@@ -35,6 +35,11 @@ EXPORT_SYMBOL_GPL(fb_mode_option);
      */
 
 static const struct fb_videomode modedb[] = {
+	    {
+		/* 800x480 @ 60 Hz, 31.5 kHz hsync */
+	    "LQ070Y3DG3B", 60, 800, 480, 44000, 0, 50, 25, 10, 128, 10,
+	    FB_SYNC_EXT,FB_VMODE_NONINTERLACED
+	    },
     {
 	/* 640x400 @ 70 Hz, 31.5 kHz hsync */
 	NULL, 70, 640, 400, 39721, 40, 24, 39, 9, 96, 2,
diff --git a/drivers/video/mxc/Kconfig b/drivers/video/mxc/Kconfig
index 268879626fc2..42f990ac3499 100644
--- a/drivers/video/mxc/Kconfig
+++ b/drivers/video/mxc/Kconfig
@@ -26,12 +26,21 @@ config FB_MXC_EPSON_VGA_SYNC_PANEL
 
 config FB_MXC_TVOUT_TVE
 	tristate "MXC TVE TV Out Encoder"
-	depends on FB_MXC_SYNC_PANEL
-	depends on MXC_IPU_V3
+        depends on FB_MXC_SYNC_PANEL
+        depends on MXC_IPU_V3
+
+config FB_MXC_LDB
+        tristate "MXC LDB"
+        depends on FB_MXC_SYNC_PANEL
+        depends on MXC_IPU_V3
 
 config FB_MXC_CLAA_WVGA_SYNC_PANEL
+        depends on FB_MXC_SYNC_PANEL
+        tristate "CLAA WVGA Panel"
+
+config FB_MXC_SII9022
 	depends on FB_MXC_SYNC_PANEL
-	tristate "CLAA WVGA Panel"
+	tristate "Si Image SII9022 DVI/HDMI Interface Chip"
 
 config FB_MXC_CH7026
 	depends on FB_MXC_SYNC_PANEL
@@ -43,12 +52,21 @@ config FB_MXC_TVOUT_CH7024
 
 config FB_MXC_LOW_PWR_DISPLAY
 	bool "Low Power Display Refresh Mode"
-	depends on FB_MXC_SYNC_PANEL && MXC_FB_IRAM
-	default y
+        depends on FB_MXC_SYNC_PANEL && MXC_FB_IRAM
+        default y
+
+config VIDEO_AD9389
+        tristate "Analog Devices AD9389/AD9889 digital video encoders"
+        depends on I2C && FB_MXC_SYNC_PANEL
+        ---help---
+          Support for the AD9389/AD9889 HDMI/DVI Video transmiter.
+
+          To compile this driver as a module, choose M here: the
+          module will be called ad9389.
 
 config FB_MXC_INTERNAL_MEM
-	bool "Framebuffer in Internal RAM"
-	depends on FB_MXC_SYNC_PANEL && MXC_FB_IRAM
+        bool "Framebuffer in Internal RAM"
+        depends on FB_MXC_SYNC_PANEL && MXC_FB_IRAM
 	default y
 
 config FB_MXC_ASYNC_PANEL
@@ -66,9 +84,24 @@ config FB_MXC_EPSON_PANEL
 
 endmenu
 
+config FB_MXC_EINK_PANEL
+        depends on FB_MXC
+        depends on DMA_ENGINE
+        select FB_DEFERRED_IO
+        tristate "E-Ink Panel Framebuffer"
+
+config FB_MXC_EINK_AUTO_UPDATE_MODE
+    bool "E-Ink Auto-update Mode Support"
+    default n
+    depends on FB_MXC_EINK_PANEL
+
+config FB_MXC_ELCDIF_FB
+        depends on FB && ARCH_MXC
+        tristate "Support MXC ELCDIF framebuffer"
+
 choice
-	prompt "Async Panel Interface Type"
-	depends on FB_MXC_ASYNC_PANEL && FB_MXC
+        prompt "Async Panel Interface Type"
+        depends on FB_MXC_ASYNC_PANEL && FB_MXC
 	default FB_MXC_ASYNC_PANEL_IFC_16_BIT
 
 config FB_MXC_ASYNC_PANEL_IFC_8_BIT
diff --git a/drivers/video/mxc/Makefile b/drivers/video/mxc/Makefile
index d2454aac2604..d823ab29f030 100644
--- a/drivers/video/mxc/Makefile
+++ b/drivers/video/mxc/Makefile
@@ -18,5 +18,7 @@ obj-$(CONFIG_FB_MXC_EPSON_VGA_SYNC_PANEL)   += mxcfb_epson_vga.o
 obj-$(CONFIG_FB_MXC_CLAA_WVGA_SYNC_PANEL)   += mxcfb_claa_wvga.o
 obj-$(CONFIG_FB_MXC_TVOUT_CH7024)           += ch7024.o
 obj-$(CONFIG_FB_MXC_TVOUT_TVE)              += tve.o
-obj-$(CONFIG_FB_MXC_CH7026)		    		+= mxcfb_ch7026.o
-#obj-$(CONFIG_FB_MODE_HELPERS)				+= mxc_edid.o
+obj-$(CONFIG_FB_MXC_CH7026)                             += mxcfb_ch7026.o
+#obj-$(CONFIG_FB_MODE_HELPERS)                           += mxc_edid.o
+obj-$(CONFIG_VIDEO_AD9389)                      += ad9389.o
+obj-$(CONFIG_FB_MXC_SII9022)                            += mxcfb_sii9022.o
diff --git a/drivers/video/mxc/ccwmx51_display.c b/drivers/video/mxc/ccwmx51_display.c
index 70a5b25f3fe9..212f59e919e4 100755
--- a/drivers/video/mxc/ccwmx51_display.c
+++ b/drivers/video/mxc/ccwmx51_display.c
@@ -22,23 +22,37 @@
 #include <mach/hardware.h>
 #include <mach/mxc.h>
 
+#define MAX_DISPLAYS	2
+#define DISP0_ID	"DISP3 BG"
+#define DISP1_ID	"DISP3 BG - DI1"
+
 static void lcd_poweron(struct ccwmx51_lcd_pdata *plat);
 static void lcd_poweroff(struct ccwmx51_lcd_pdata *plat);
 
-static struct platform_device *plcd_dev;
+static struct platform_device *plcd_dev[MAX_DISPLAYS] = {NULL, NULL};
+
+static int lcd_get_index(struct fb_info *info)
+{
+	if (!strcmp(info->fix.id, DISP0_ID))
+		return 0;
+	else if (!strcmp(info->fix.id, DISP1_ID))
+		return 1;
+	return -1;
+}
 
 static void lcd_init_fb(struct fb_info *info)
 {
-	struct ccwmx51_lcd_pdata *plat = plcd_dev->dev.platform_data;
 	struct fb_var_screeninfo var;
+	struct ccwmx51_lcd_pdata *plat;
+	int i = lcd_get_index(info);
 
-	memset(&var, 0, sizeof(var));
+	if (i < 0)
+		return;
 
+	plat = plcd_dev[i]->dev.platform_data;
+	memset(&var, 0, sizeof(var));
 	fb_videomode_to_var(&var, plat->fb_pdata.mode);
-
 	var.activate = FB_ACTIVATE_ALL;
-	var.yres_virtual = var.yres;
-
 	acquire_console_sem();
 	info->flags |= FBINFO_MISC_USEREVENT;
 	fb_set_var(info, &var);
@@ -74,18 +88,37 @@ static struct notifier_block nb = {
 static int __devinit lcd_sync_probe(struct platform_device *pdev)
 {
 	struct ccwmx51_lcd_pdata *plat = pdev->dev.platform_data;
+	int i;
+
+	if (!plat)
+		return -ENODEV;
 
- 	if (!plat)
- 		return -ENODEV;
+	if (plat->vif < 0 || plat->vif > (MAX_DISPLAYS - 1))
+		return -EINVAL;
 
-	if (plat->reset)
-		plat->reset();
+	if (plat->init)
+		plat->init(plat->vif);
 
-	plcd_dev = pdev;
-	lcd_init_fb(registered_fb[plat->vif]);
-	fb_show_logo(registered_fb[plat->vif], 0);
-	fb_register_client(&nb);
+	plcd_dev[plat->vif] = pdev;
 
+	for (i = 0; i < num_registered_fb; i++) {
+		if ((!strcmp(registered_fb[i]->fix.id, DISP0_ID) && plat->vif == 0) ||
+		    (!strcmp(registered_fb[i]->fix.id, DISP1_ID) && plat->vif == 1)) {
+			lcd_init_fb(registered_fb[i]);
+			/* Clear the screen */
+			memset((char *)registered_fb[i]->screen_base, 0,
+			       registered_fb[i]->fix.smem_len);
+			fb_show_logo(registered_fb[i], 0);
+		}
+	}
+
+	/**
+	 * Register the block notifier only once. The device being notified can be
+	 * retrieved from the received event. There are some issues when the same
+	 * notifier is registered multiple times.
+	 */
+	if (plcd_dev[0] == NULL && plcd_dev[1] == NULL)
+		fb_register_client(&nb);
 	lcd_poweron(plat);
 
 	return 0;
@@ -95,9 +128,14 @@ static int __devexit lcd_sync_remove(struct platform_device *pdev)
 {
 	struct ccwmx51_lcd_pdata *plat = pdev->dev.platform_data;
 
-	fb_unregister_client(&nb);
 	lcd_poweroff(plat);
-	plcd_dev = NULL;
+	if (plat->deinit)
+		plat->deinit(plat->vif);
+
+	plcd_dev[plat->vif] = NULL;
+
+	if (plcd_dev[0] == NULL && plcd_dev[1] == NULL)
+		fb_unregister_client(&nb);
 
 	return 0;
 }
@@ -129,14 +167,14 @@ static struct platform_driver lcd_driver = {
 
 static void lcd_poweron(struct ccwmx51_lcd_pdata *plat)
 {
-	if (plat && plat->bl_enable)
-		plat->bl_enable(0);
+        if (plat && plat->bl_enable)
+                plat->bl_enable(1, plat->vif);
 }
 
 static void lcd_poweroff(struct ccwmx51_lcd_pdata *plat)
 {
-	if (plat && plat->bl_enable)
-		plat->bl_enable(1);
+        if (plat && plat->bl_enable)
+                plat->bl_enable(0, plat->vif);
 }
 
 static int __init lcd_sync_init(void)
diff --git a/drivers/video/mxc/mxc_ipuv3_fb.c b/drivers/video/mxc/mxc_ipuv3_fb.c
index 76c6d523c291..cc48e62637ca 100644
--- a/drivers/video/mxc/mxc_ipuv3_fb.c
+++ b/drivers/video/mxc/mxc_ipuv3_fb.c
@@ -22,7 +22,6 @@
  *
  * @ingroup Framebuffer
  */
-
 /*!
  * Include files
  */
@@ -44,9 +43,13 @@
 #include <linux/io.h>
 #include <linux/ipu.h>
 #include <linux/mxcfb.h>
+#include <linux/earlysuspend.h>
 #include <asm/mach-types.h>
 #include <asm/uaccess.h>
 #include <mach/hardware.h>
+#include <linux/suspend.h>
+
+static int vt_switch;
 
 /*
  * Driver name
@@ -56,11 +59,14 @@
  * Structure containing the MXC specific framebuffer information.
  */
 struct mxcfb_info {
+	char *fb_mode_str;
+	int default_bpp;
 	int cur_blank;
 	int next_blank;
 	ipu_channel_t ipu_ch;
 	int ipu_di;
 	u32 ipu_di_pix_fmt;
+	bool ipu_ext_clk;
 	bool overlay;
 	bool alpha_chan_en;
 	dma_addr_t alpha_phy_addr0;
@@ -74,6 +80,8 @@ struct mxcfb_info {
 
 	u32 pseudo_palette[16];
 
+	bool wait4vsync;
+	uint32_t waitcnt;
 	struct semaphore flip_sem;
 	struct semaphore alpha_flip_sem;
 	struct completion vsync_complete;
@@ -93,12 +101,10 @@ enum {
 	BOTH_OFF
 };
 
-static char *fb_mode;
-static unsigned long default_bpp = 16;
+#define FB_DEVICE_NUM 3
 static bool g_dp_in_use;
 LIST_HEAD(fb_alloc_list);
-static struct fb_info *mxcfb_info[3];
-static int ext_clk_used;
+static struct fb_info *mxcfb_info[FB_DEVICE_NUM];
 
 static uint32_t bpp_to_pixfmt(struct fb_info *fbi)
 {
@@ -125,6 +131,7 @@ static irqreturn_t mxcfb_irq_handler(int irq, void *dev_id);
 static int mxcfb_blank(int blank, struct fb_info *info);
 static int mxcfb_map_video_memory(struct fb_info *fbi);
 static int mxcfb_unmap_video_memory(struct fb_info *fbi);
+static int mxcfb_option_setup(struct fb_info *info, char *options);
 
 /*
  * Set fixed framebuffer parameters based on variable settings.
@@ -165,30 +172,42 @@ static int _setup_disp_channel1(struct fb_info *fbi)
 
 		for (i = 0; i < num_registered_fb; i++) {
 			mxc_fbi_tmp = (struct mxcfb_info *)
-				(registered_fb[i]->par);
+			    (registered_fb[i]->par);
 			if (mxc_fbi_tmp->ipu_ch == MEM_BG_SYNC) {
-				fbi->var.vmode =
-				registered_fb[i]->var.vmode;
+				fbi->var.vmode = registered_fb[i]->var.vmode;
 				mxc_fbi->ipu_di_pix_fmt =
-				mxc_fbi_tmp->ipu_di_pix_fmt;
+				    mxc_fbi_tmp->ipu_di_pix_fmt;
 				break;
 			}
 		}
 	}
-	if (fbi->var.vmode & FB_VMODE_INTERLACED) {
-		params.mem_dp_bg_sync.interlaced = true;
-		params.mem_dp_bg_sync.out_pixel_fmt =
-			IPU_PIX_FMT_YUV444;
+	if (mxc_fbi->ipu_ch == MEM_DC_SYNC) {
+		if (fbi->var.vmode & FB_VMODE_INTERLACED) {
+			params.mem_dc_sync.interlaced = true;
+			params.mem_dc_sync.out_pixel_fmt =
+				IPU_PIX_FMT_YUV444;
+		} else {
+			if (mxc_fbi->ipu_di_pix_fmt)
+				params.mem_dc_sync.out_pixel_fmt = mxc_fbi->ipu_di_pix_fmt;
+			else
+				params.mem_dc_sync.out_pixel_fmt = IPU_PIX_FMT_RGB666;
+		}
+		params.mem_dc_sync.in_pixel_fmt = bpp_to_pixfmt(fbi);
 	} else {
-		if (mxc_fbi->ipu_di_pix_fmt)
-			params.mem_dp_bg_sync.out_pixel_fmt = mxc_fbi->ipu_di_pix_fmt;
-		else
-			params.mem_dp_bg_sync.out_pixel_fmt = IPU_PIX_FMT_RGB666;
+		if (fbi->var.vmode & FB_VMODE_INTERLACED) {
+			params.mem_dp_bg_sync.interlaced = true;
+			params.mem_dp_bg_sync.out_pixel_fmt =
+				IPU_PIX_FMT_YUV444;
+		} else {
+			if (mxc_fbi->ipu_di_pix_fmt)
+				params.mem_dp_bg_sync.out_pixel_fmt = mxc_fbi->ipu_di_pix_fmt;
+			else
+				params.mem_dp_bg_sync.out_pixel_fmt = IPU_PIX_FMT_RGB666;
+		}
+		params.mem_dp_bg_sync.in_pixel_fmt = bpp_to_pixfmt(fbi);
+		if (mxc_fbi->alpha_chan_en)
+			params.mem_dp_bg_sync.alpha_chan_en = true;
 	}
-	params.mem_dp_bg_sync.in_pixel_fmt = bpp_to_pixfmt(fbi);
-	if (mxc_fbi->alpha_chan_en)
-		params.mem_dp_bg_sync.alpha_chan_en = true;
-
 	ipu_init_channel(mxc_fbi->ipu_ch, &params);
 
 	return 0;
@@ -201,10 +220,12 @@ static int _setup_disp_channel2(struct fb_info *fbi)
 	int fb_stride;
 
 	switch (bpp_to_pixfmt(fbi)) {
-	case V4L2_PIX_FMT_YUV420:
-	case V4L2_PIX_FMT_YVU420:
-	case V4L2_PIX_FMT_NV12:
-	case V4L2_PIX_FMT_YUV422P:
+	case IPU_PIX_FMT_YUV420P2:
+	case IPU_PIX_FMT_YVU420P:
+	case IPU_PIX_FMT_NV12:
+	case IPU_PIX_FMT_YUV422P:
+	case IPU_PIX_FMT_YVU422P:
+	case IPU_PIX_FMT_YUV420P:
 		fb_stride = fbi->var.xres_virtual;
 		break;
 	default:
@@ -212,7 +233,7 @@ static int _setup_disp_channel2(struct fb_info *fbi)
 	}
 
 	mxc_fbi->cur_ipu_buf = 1;
-	sema_init(&mxc_fbi->flip_sem, 1);
+	sema_init(&mxc_fbi->flip_sem, 0);
 	if (mxc_fbi->alpha_chan_en) {
 		mxc_fbi->cur_ipu_alpha_buf = 1;
 		sema_init(&mxc_fbi->alpha_flip_sem, 1);
@@ -226,8 +247,7 @@ static int _setup_disp_channel2(struct fb_info *fbi)
 					 IPU_ROTATE_NONE,
 					 fbi->fix.smem_start +
 					 (fbi->fix.line_length * fbi->var.yres),
-					 fbi->fix.smem_start,
-					 0, 0);
+					 fbi->fix.smem_start, 0, 0);
 	if (retval) {
 		dev_err(fbi->device,
 			"ipu_init_channel_buffer error %d\n", retval);
@@ -297,38 +317,47 @@ static int mxcfb_set_par(struct fb_info *fbi)
 						  mxc_fbi->alpha_phy_addr1);
 
 			mxc_fbi->alpha_virt_addr0 =
-					dma_alloc_coherent(fbi->device,
-						  alpha_mem_len,
-						  &mxc_fbi->alpha_phy_addr0,
-						  GFP_DMA | GFP_KERNEL);
+			    dma_alloc_coherent(fbi->device,
+					       alpha_mem_len,
+					       &mxc_fbi->alpha_phy_addr0,
+					       GFP_DMA | GFP_KERNEL);
 
 			mxc_fbi->alpha_virt_addr1 =
-					dma_alloc_coherent(fbi->device,
-						  alpha_mem_len,
-						  &mxc_fbi->alpha_phy_addr1,
-						  GFP_DMA | GFP_KERNEL);
+			    dma_alloc_coherent(fbi->device,
+					       alpha_mem_len,
+					       &mxc_fbi->alpha_phy_addr1,
+					       GFP_DMA | GFP_KERNEL);
 			if (mxc_fbi->alpha_virt_addr0 == NULL ||
 			    mxc_fbi->alpha_virt_addr1 == NULL) {
 				dev_err(fbi->device, "mxcfb: dma alloc for"
 					" alpha buffer failed.\n");
 				if (mxc_fbi->alpha_virt_addr0)
 					dma_free_coherent(fbi->device,
-						  mxc_fbi->alpha_mem_len,
-						  mxc_fbi->alpha_virt_addr0,
-						  mxc_fbi->alpha_phy_addr0);
+							  mxc_fbi->
+							  alpha_mem_len,
+							  mxc_fbi->
+							  alpha_virt_addr0,
+							  mxc_fbi->
+							  alpha_phy_addr0);
 				if (mxc_fbi->alpha_virt_addr1)
 					dma_free_coherent(fbi->device,
-						  mxc_fbi->alpha_mem_len,
-						  mxc_fbi->alpha_virt_addr1,
-						  mxc_fbi->alpha_phy_addr1);
+							  mxc_fbi->
+							  alpha_mem_len,
+							  mxc_fbi->
+							  alpha_virt_addr1,
+							  mxc_fbi->
+							  alpha_phy_addr1);
 				return -ENOMEM;
 			}
 			mxc_fbi->alpha_mem_len = alpha_mem_len;
 		}
 	}
 
+#if !(defined(CONFIG_CCWMX51_DISP0) && defined(CONFIG_CCWMX51_DISP1))
+	/* FIXME this lines of code doesnt allow to run the dual head... */
 	if (mxc_fbi->next_blank != FB_BLANK_UNBLANK)
 		return retval;
+#endif
 
 	_setup_disp_channel1(fbi);
 
@@ -345,9 +374,9 @@ static int mxcfb_set_par(struct fb_info *fbi)
 			else
 				out_pixel_fmt = IPU_PIX_FMT_RGB666;
 		}
-		if (fbi->var.vmode & FB_VMODE_ODD_FLD_FIRST) /* PAL */
+		if (fbi->var.vmode & FB_VMODE_ODD_FLD_FIRST)	/* PAL */
 			sig_cfg.odd_field_first = true;
-		if ((fbi->var.sync & FB_SYNC_EXT) || ext_clk_used)
+		if ((fbi->var.sync & FB_SYNC_EXT) || mxc_fbi->ipu_ext_clk)
 			sig_cfg.ext_clk = true;
 		if (fbi->var.sync & FB_SYNC_HOR_HIGH_ACT)
 			sig_cfg.Hsync_pol = true;
@@ -453,8 +482,7 @@ static int swap_channels(struct fb_info *fbi)
 		ch_to = MEM_BG_SYNC;
 
 	for (i = 0; i < num_registered_fb; i++) {
-		mxc_fbi_to =
-			(struct mxcfb_info *)mxcfb_info[i]->par;
+		mxc_fbi_to = (struct mxcfb_info *)mxcfb_info[i]->par;
 		if (mxc_fbi_to->ipu_ch == ch_to) {
 			fbi_to = mxcfb_info[i];
 			break;
@@ -519,14 +547,14 @@ static int swap_channels(struct fb_info *fbi)
 	}
 
 	if (ipu_request_irq(mxc_fbi_from->ipu_ch_irq, mxcfb_irq_handler, 0,
-		MXCFB_NAME, fbi) != 0) {
+			    MXCFB_NAME, fbi) != 0) {
 		dev_err(fbi->device, "Error registering irq %d\n",
 			mxc_fbi_from->ipu_ch_irq);
 		return -EBUSY;
 	}
 	ipu_disable_irq(mxc_fbi_from->ipu_ch_irq);
 	if (ipu_request_irq(mxc_fbi_to->ipu_ch_irq, mxcfb_irq_handler, 0,
-		MXCFB_NAME, fbi_to) != 0) {
+			    MXCFB_NAME, fbi_to) != 0) {
 		dev_err(fbi_to->device, "Error registering irq %d\n",
 			mxc_fbi_to->ipu_ch_irq);
 		return -EBUSY;
@@ -545,17 +573,48 @@ static int swap_channels(struct fb_info *fbi)
  */
 static int mxcfb_check_var(struct fb_var_screeninfo *var, struct fb_info *info)
 {
+	struct mxcfb_info *mxc_fbi = (struct mxcfb_info *)info->par;
 	u32 vtotal;
 	u32 htotal;
 
+	/* fg should not bigger than bg */
+	if (mxc_fbi->ipu_ch == MEM_FG_SYNC) {
+		struct fb_info *fbi_tmp;
+		struct mxcfb_info *mxc_fbi_tmp;
+		int i, bg_xres, bg_yres;
+		int16_t pos_x, pos_y;
+
+		bg_xres = var->xres;
+		bg_yres = var->yres;
+
+		for (i = 0; i < num_registered_fb; i++) {
+			fbi_tmp = registered_fb[i];
+			mxc_fbi_tmp = (struct mxcfb_info *)
+				(fbi_tmp->par);
+			if (mxc_fbi_tmp->ipu_ch == MEM_BG_SYNC) {
+				bg_xres = fbi_tmp->var.xres;
+				bg_yres = fbi_tmp->var.yres;
+				break;
+			}
+		}
+
+		ipu_disp_get_window_pos(mxc_fbi->ipu_ch, &pos_x, &pos_y);
+
+		if ((var->xres + pos_x) > bg_xres)
+			var->xres = bg_xres - pos_x;
+		if ((var->yres + pos_y) > bg_yres)
+			var->yres = bg_yres - pos_y;
+	}
+
 	if (var->xres_virtual < var->xres)
 		var->xres_virtual = var->xres;
 	if (var->yres_virtual < var->yres)
 		var->yres_virtual = var->yres;
 
 	if ((var->bits_per_pixel != 32) && (var->bits_per_pixel != 24) &&
-	    (var->bits_per_pixel != 16) && (var->bits_per_pixel != 8))
-		var->bits_per_pixel = default_bpp;
+	    (var->bits_per_pixel != 16) && (var->bits_per_pixel != 12) &&
+	    (var->bits_per_pixel != 8))
+		var->bits_per_pixel = mxc_fbi->default_bpp;
 
 	switch (var->bits_per_pixel) {
 	case 8:
@@ -723,7 +782,7 @@ static int mxcfb_ioctl(struct fb_info *fbi, unsigned int cmd, unsigned long arg)
 			}
 
 			if (ipu_disp_set_global_alpha(mxc_fbi->ipu_ch,
-						      (bool)ga.enable,
+						      (bool) ga.enable,
 						      ga.alpha)) {
 				retval = -EINVAL;
 				break;
@@ -750,7 +809,7 @@ static int mxcfb_ioctl(struct fb_info *fbi, unsigned int cmd, unsigned long arg)
 			}
 
 			if (ipu_disp_set_global_alpha(mxc_fbi->ipu_ch,
-						      !(bool)la.enable, 0)) {
+						      !(bool) la.enable, 0)) {
 				retval = -EINVAL;
 				break;
 			}
@@ -765,8 +824,11 @@ static int mxcfb_ioctl(struct fb_info *fbi, unsigned int cmd, unsigned long arg)
 
 				for (i = 0; i < num_registered_fb; i++) {
 					char *idstr = registered_fb[i]->fix.id;
-					if (strcmp(idstr, video_plane_idstr) == 0) {
-						((struct mxcfb_info *)(registered_fb[i]->par))->alpha_chan_en = false;
+					if (strcmp(idstr, video_plane_idstr) ==
+					    0) {
+						((struct mxcfb_info
+						  *)(registered_fb[i]->par))->
+				  alpha_chan_en = false;
 						break;
 					}
 				}
@@ -794,8 +856,8 @@ static int mxcfb_ioctl(struct fb_info *fbi, unsigned int cmd, unsigned long arg)
 			uint32_t ipu_alp_ch_irq;
 
 			if (!(((mxc_fbi->ipu_ch == MEM_FG_SYNC) ||
-			     (mxc_fbi->ipu_ch == MEM_BG_SYNC)) &&
-			     (mxc_fbi->alpha_chan_en))) {
+			       (mxc_fbi->ipu_ch == MEM_BG_SYNC)) &&
+			      (mxc_fbi->alpha_chan_en))) {
 				dev_err(fbi->device,
 					"Should use background or overlay "
 					"framebuffer to set the alpha buffer "
@@ -822,11 +884,10 @@ static int mxcfb_ioctl(struct fb_info *fbi, unsigned int cmd, unsigned long arg)
 			down(&mxc_fbi->alpha_flip_sem);
 
 			mxc_fbi->cur_ipu_alpha_buf =
-						!mxc_fbi->cur_ipu_alpha_buf;
+			    !mxc_fbi->cur_ipu_alpha_buf;
 			if (ipu_update_channel_buffer(mxc_fbi->ipu_ch,
 						      IPU_ALPHA_IN_BUFFER,
-						      mxc_fbi->
-							cur_ipu_alpha_buf,
+						      mxc_fbi->cur_ipu_alpha_buf,
 						      base) == 0) {
 				ipu_select_buffer(mxc_fbi->ipu_ch,
 						  IPU_ALPHA_IN_BUFFER,
@@ -864,9 +925,9 @@ static int mxcfb_ioctl(struct fb_info *fbi, unsigned int cmd, unsigned long arg)
 				break;
 			}
 			retval = ipu_disp_set_gamma_correction(mxc_fbi->ipu_ch,
-							gamma.enable,
-							gamma.constk,
-							gamma.slopek);
+							       gamma.enable,
+							       gamma.constk,
+							       gamma.slopek);
 			break;
 		}
 	case MXCFB_WAIT_FOR_VSYNC:
@@ -876,7 +937,8 @@ static int mxcfb_ioctl(struct fb_info *fbi, unsigned int cmd, unsigned long arg)
 				int i;
 				for (i = 0; i < num_registered_fb; i++) {
 					bg_mxcfbi =
-					((struct mxcfb_info *)(registered_fb[i]->par));
+					    ((struct mxcfb_info
+					      *)(registered_fb[i]->par));
 
 					if (bg_mxcfbi->ipu_ch == MEM_BG_SYNC)
 						break;
@@ -891,21 +953,25 @@ static int mxcfb_ioctl(struct fb_info *fbi, unsigned int cmd, unsigned long arg)
 				break;
 			}
 
-			down(&mxc_fbi->flip_sem);
 			init_completion(&mxc_fbi->vsync_complete);
 
 			ipu_clear_irq(mxc_fbi->ipu_ch_irq);
+			mxc_fbi->wait4vsync = 1;
 			ipu_enable_irq(mxc_fbi->ipu_ch_irq);
-			retval = wait_for_completion_interruptible_timeout(
-				&mxc_fbi->vsync_complete, 1 * HZ);
+			retval =
+			    wait_for_completion_interruptible_timeout(&mxc_fbi->
+								      vsync_complete,
+								      1 * HZ);
 			if (retval == 0) {
 				dev_err(fbi->device,
 					"MXCFB_WAIT_FOR_VSYNC: timeout %d\n",
 					retval);
+				mxc_fbi->wait4vsync = 0;
 				retval = -ETIME;
 			} else if (retval > 0) {
 				retval = 0;
 			}
+
 			break;
 		}
 	case FBIO_ALLOC:
@@ -986,7 +1052,8 @@ static int mxcfb_ioctl(struct fb_info *fbi, unsigned int cmd, unsigned long arg)
 
 			for (i = 0; i < num_registered_fb; i++) {
 				bg_mxcfbi =
-				((struct mxcfb_info *)(registered_fb[i]->par));
+				    ((struct mxcfb_info *)(registered_fb[i]->
+							   par));
 
 				if (bg_mxcfbi->ipu_ch == MEM_BG_SYNC) {
 					bg_fbi = registered_fb[i];
@@ -1005,13 +1072,15 @@ static int mxcfb_ioctl(struct fb_info *fbi, unsigned int cmd, unsigned long arg)
 				if (bg_fbi->var.xres < fbi->var.xres)
 					pos.x = 0;
 				else
-					pos.x = bg_fbi->var.xres - fbi->var.xres;
+					pos.x =
+					    bg_fbi->var.xres - fbi->var.xres;
 			}
 			if (fbi->var.yres + pos.y > bg_fbi->var.yres) {
 				if (bg_fbi->var.yres < fbi->var.yres)
 					pos.y = 0;
 				else
-					pos.y = bg_fbi->var.yres - fbi->var.yres;
+					pos.y =
+					    bg_fbi->var.yres - fbi->var.yres;
 			}
 
 			retval = ipu_disp_set_window_pos(mxc_fbi->ipu_ch,
@@ -1026,12 +1095,39 @@ static int mxcfb_ioctl(struct fb_info *fbi, unsigned int cmd, unsigned long arg)
 	case MXCFB_GET_FB_IPU_CHAN:
 		{
 			struct mxcfb_info *mxc_fbi =
-				(struct mxcfb_info *)fbi->par;
+			    (struct mxcfb_info *)fbi->par;
 
 			if (put_user(mxc_fbi->ipu_ch, argp))
 				return -EFAULT;
 			break;
 		}
+	case MXCFB_GET_DIFMT:
+		{
+			struct mxcfb_info *mxc_fbi =
+			    (struct mxcfb_info *)fbi->par;
+
+			if (put_user(mxc_fbi->ipu_di_pix_fmt, argp))
+				return -EFAULT;
+			break;
+		}
+	case MXCFB_GET_FB_IPU_DI:
+		{
+			struct mxcfb_info *mxc_fbi =
+			    (struct mxcfb_info *)fbi->par;
+
+			if (put_user(mxc_fbi->ipu_di, argp))
+				return -EFAULT;
+			break;
+		}
+	case MXCFB_GET_FB_BLANK:
+		{
+			struct mxcfb_info *mxc_fbi =
+				(struct mxcfb_info *)fbi->par;
+
+			if (put_user(mxc_fbi->cur_blank, argp))
+				return -EFAULT;
+			break;
+		}
 	default:
 		retval = -EINVAL;
 	}
@@ -1081,7 +1177,7 @@ static int
 mxcfb_pan_display(struct fb_var_screeninfo *var, struct fb_info *info)
 {
 	struct mxcfb_info *mxc_fbi = (struct mxcfb_info *)info->par,
-			  *mxc_graphic_fbi = NULL;
+	    *mxc_graphic_fbi = NULL;
 	u_int y_bottom;
 	unsigned long base, active_alpha_phy_addr = 0;
 	bool loc_alpha_en = false;
@@ -1102,7 +1198,7 @@ mxcfb_pan_display(struct fb_var_screeninfo *var, struct fb_info *info)
 		int j;
 		for (j = 0; j < num_registered_fb; j++) {
 			bg_mxcfbi =
-				((struct mxcfb_info *)(registered_fb[j]->par));
+			    ((struct mxcfb_info *)(registered_fb[j]->par));
 
 			if (bg_mxcfbi->ipu_ch == MEM_BG_SYNC)
 				break;
@@ -1122,12 +1218,9 @@ mxcfb_pan_display(struct fb_var_screeninfo *var, struct fb_info *info)
 		return -EINVAL;
 
 	base = (var->yoffset * var->xres_virtual + var->xoffset);
-	base *= (var->bits_per_pixel) / 8;
+	base = (var->bits_per_pixel) * base / 8;
 	base += info->fix.smem_start;
 
-	dev_dbg(info->device, "Updating SDC %s buf %d address=0x%08lX\n",
-		info->fix.id, mxc_fbi->cur_ipu_buf, base);
-
 	/* Check if DP local alpha is enabled and find the graphic fb */
 	if (mxc_fbi->ipu_ch == MEM_BG_SYNC || mxc_fbi->ipu_ch == MEM_FG_SYNC) {
 		for (i = 0; i < num_registered_fb; i++) {
@@ -1135,13 +1228,13 @@ mxcfb_pan_display(struct fb_var_screeninfo *var, struct fb_info *info)
 			if ((strcmp(idstr, "DISP3 BG") == 0 ||
 			     strcmp(idstr, "DISP3 FG") == 0) &&
 			    ((struct mxcfb_info *)
-			      (registered_fb[i]->par))->alpha_chan_en) {
+			     (registered_fb[i]->par))->alpha_chan_en) {
 				loc_alpha_en = true;
 				mxc_graphic_fbi = (struct mxcfb_info *)
-						(registered_fb[i]->par);
+				    (registered_fb[i]->par);
 				active_alpha_phy_addr = mxc_fbi->cur_ipu_buf ?
-					mxc_graphic_fbi->alpha_phy_addr1 :
-					mxc_graphic_fbi->alpha_phy_addr0;
+				    mxc_graphic_fbi->alpha_phy_addr1 :
+				    mxc_graphic_fbi->alpha_phy_addr0;
 				dev_dbg(info->device, "Updating SDC graphic "
 					"buf %d address=0x%08lX\n",
 					mxc_fbi->cur_ipu_buf,
@@ -1151,10 +1244,13 @@ mxcfb_pan_display(struct fb_var_screeninfo *var, struct fb_info *info)
 		}
 	}
 
-	down(&mxc_fbi->flip_sem);
 	init_completion(&mxc_fbi->vsync_complete);
 
 	mxc_fbi->cur_ipu_buf = !mxc_fbi->cur_ipu_buf;
+
+	dev_dbg(info->device, "Updating SDC %s buf %d address=0x%08lX\n",
+		info->fix.id, mxc_fbi->cur_ipu_buf, base);
+
 	if (ipu_update_channel_buffer(mxc_fbi->ipu_ch, IPU_INPUT_BUFFER,
 				      mxc_fbi->cur_ipu_buf, base) == 0) {
 		/* Update the DP local alpha buffer only for graphic plane */
@@ -1176,8 +1272,13 @@ mxcfb_pan_display(struct fb_var_screeninfo *var, struct fb_info *info)
 		dev_err(info->device,
 			"Error updating SDC buf %d to address=0x%08lX\n",
 			mxc_fbi->cur_ipu_buf, base);
+		mxc_fbi->cur_ipu_buf = !mxc_fbi->cur_ipu_buf;
+		ipu_clear_irq(mxc_fbi->ipu_ch_irq);
+		ipu_enable_irq(mxc_fbi->ipu_ch_irq);
+		return -EBUSY;
 	}
 
+	down(&mxc_fbi->flip_sem);
 	dev_dbg(info->device, "Update complete\n");
 
 	info->var.xoffset = var->xoffset;
@@ -1211,9 +1312,9 @@ static int mxcfb_mmap(struct fb_info *fbi, struct vm_area_struct *vma)
 		len = fbi->fix.smem_len - offset;
 		vma->vm_pgoff = (fbi->fix.smem_start + offset) >> PAGE_SHIFT;
 	} else if ((vma->vm_pgoff ==
-			(mxc_fbi->alpha_phy_addr0 >> PAGE_SHIFT)) ||
+		    (mxc_fbi->alpha_phy_addr0 >> PAGE_SHIFT)) ||
 		   (vma->vm_pgoff ==
-			(mxc_fbi->alpha_phy_addr1 >> PAGE_SHIFT))) {
+		    (mxc_fbi->alpha_phy_addr1 >> PAGE_SHIFT))) {
 		len = mxc_fbi->alpha_mem_len;
 	} else {
 		list_for_each_entry(mem, &fb_alloc_list, list) {
@@ -1269,9 +1370,14 @@ static irqreturn_t mxcfb_irq_handler(int irq, void *dev_id)
 	struct fb_info *fbi = dev_id;
 	struct mxcfb_info *mxc_fbi = fbi->par;
 
-	complete(&mxc_fbi->vsync_complete);
-	up(&mxc_fbi->flip_sem);
-	ipu_disable_irq(irq);
+	if (mxc_fbi->wait4vsync) {
+		complete(&mxc_fbi->vsync_complete);
+		ipu_disable_irq(irq);
+		mxc_fbi->wait4vsync = 0;
+	} else {
+		up(&mxc_fbi->flip_sem);
+		ipu_disable_irq(irq);
+	}
 	return IRQ_HANDLED;
 }
 
@@ -1288,14 +1394,10 @@ static irqreturn_t mxcfb_alpha_irq_handler(int irq, void *dev_id)
 /*
  * Suspends the framebuffer and blanks the screen. Power management support
  */
-static int mxcfb_suspend(struct platform_device *pdev, pm_message_t state)
+static void mxcfb_suspend_one(struct fb_info *fbi)
 {
-	struct fb_info *fbi = platform_get_drvdata(pdev);
 	struct mxcfb_info *mxc_fbi = (struct mxcfb_info *)fbi->par;
 	int saved_blank;
-#ifdef CONFIG_FB_MXC_LOW_PWR_DISPLAY
-	void *fbmem;
-#endif
 
 	acquire_console_sem();
 	fb_set_suspend(fbi, 1);
@@ -1303,25 +1405,43 @@ static int mxcfb_suspend(struct platform_device *pdev, pm_message_t state)
 	mxcfb_blank(FB_BLANK_POWERDOWN, fbi);
 	mxc_fbi->next_blank = saved_blank;
 	release_console_sem();
-
-	return 0;
 }
 
 /*
  * Resumes the framebuffer and unblanks the screen. Power management support
  */
-static int mxcfb_resume(struct platform_device *pdev)
+static void mxcfb_resume_one(struct fb_info *fbi)
 {
-	struct fb_info *fbi = platform_get_drvdata(pdev);
 	struct mxcfb_info *mxc_fbi = (struct mxcfb_info *)fbi->par;
 
 	acquire_console_sem();
 	mxcfb_blank(mxc_fbi->next_blank, fbi);
 	fb_set_suspend(fbi, 0);
 	release_console_sem();
+}
+
+#ifndef CONFIG_HAS_EARLYSUSPEND
+
+static int mxcfb_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	struct fb_info *fbi = platform_get_drvdata(pdev);
+
+	if (fbi && (strcmp(fbi->fix.id, "DISP3 FG") == 0))
+		mxcfb_suspend_one(fbi);
+
+	return 0;
+}
+
+static int mxcfb_resume(struct platform_device *pdev)
+{
+	struct fb_info *fbi = platform_get_drvdata(pdev);
+
+	if (fbi && (strcmp(fbi->fix.id, "DISP3 FG") == 0))
+		mxcfb_resume_one(fbi);
 
 	return 0;
 }
+#endif
 
 /*
  * Main framebuffer functions
@@ -1341,12 +1461,12 @@ static int mxcfb_map_video_memory(struct fb_info *fbi)
 {
 	if (fbi->fix.smem_len < fbi->var.yres_virtual * fbi->fix.line_length)
 		fbi->fix.smem_len = fbi->var.yres_virtual *
-				    fbi->fix.line_length;
+		    fbi->fix.line_length;
 
 	fbi->screen_base = dma_alloc_writecombine(fbi->device,
-				fbi->fix.smem_len,
-				(dma_addr_t *)&fbi->fix.smem_start,
-				GFP_DMA);
+						  fbi->fix.smem_len,
+						  (dma_addr_t *) & fbi->fix.
+						  smem_start, GFP_DMA);
 	if (fbi->screen_base == 0) {
 		dev_err(fbi->device, "Unable to allocate framebuffer memory\n");
 		fbi->fix.smem_len = 0;
@@ -1443,6 +1563,7 @@ static ssize_t swap_disp_chan(struct device *dev,
 	struct mxcfb_info *mxcfbi = (struct mxcfb_info *)info->par;
 	struct mxcfb_info *fg_mxcfbi = NULL;
 
+	acquire_console_sem();
 	/* swap only happen between DP-BG and DC, while DP-FG disable */
 	if (((mxcfbi->ipu_ch == MEM_BG_SYNC) &&
 	     (strstr(buf, "1-layer-fb") != NULL)) ||
@@ -1451,17 +1572,16 @@ static ssize_t swap_disp_chan(struct device *dev,
 		int i;
 
 		for (i = 0; i < num_registered_fb; i++) {
-			fg_mxcfbi =
-				(struct mxcfb_info *)mxcfb_info[i]->par;
+			fg_mxcfbi = (struct mxcfb_info *)mxcfb_info[i]->par;
 			if (fg_mxcfbi->ipu_ch == MEM_FG_SYNC)
 				break;
 			else
 				fg_mxcfbi = NULL;
 		}
-		if (!fg_mxcfbi ||
-			fg_mxcfbi->cur_blank == FB_BLANK_UNBLANK) {
+		if (!fg_mxcfbi || fg_mxcfbi->cur_blank == FB_BLANK_UNBLANK) {
 			dev_err(dev,
 				"Can not switch while fb2(fb-fg) is on.\n");
+			release_console_sem();
 			return count;
 		}
 
@@ -1469,8 +1589,10 @@ static ssize_t swap_disp_chan(struct device *dev,
 			dev_err(dev, "Swap display channel failed.\n");
 	}
 
+	release_console_sem();
 	return count;
 }
+
 DEVICE_ATTR(fsl_disp_property, 644, show_disp_chan, swap_disp_chan);
 
 /*!
@@ -1487,6 +1609,8 @@ static int mxcfb_probe(struct platform_device *pdev)
 	struct mxcfb_info *mxcfbi;
 	struct mxc_fb_platform_data *plat_data = pdev->dev.platform_data;
 	struct resource *res;
+	char *options, *mstr;
+	char name[] = "mxcdi0fb";
 	int ret = 0;
 
 	/*
@@ -1499,6 +1623,13 @@ static int mxcfb_probe(struct platform_device *pdev)
 	}
 	mxcfbi = (struct mxcfb_info *)fbi->par;
 
+	name[5] += pdev->id;
+	if (fb_get_options(name, &options))
+		return -ENODEV;
+
+	if (options)
+		mxcfb_option_setup(fbi, options);
+
 	if (!g_dp_in_use) {
 		mxcfbi->ipu_ch_irq = IPU_IRQ_BG_SYNC_EOF;
 		mxcfbi->ipu_ch = MEM_BG_SYNC;
@@ -1521,7 +1652,7 @@ static int mxcfb_probe(struct platform_device *pdev)
 					    mxcfb_alpha_irq_handler, 0,
 					    MXCFB_NAME, fbi) != 0) {
 				dev_err(&pdev->dev, "Error registering BG "
-						    "alpha irq handler.\n");
+					"alpha irq handler.\n");
 				ret = -EBUSY;
 				goto err1;
 			}
@@ -1534,7 +1665,7 @@ static int mxcfb_probe(struct platform_device *pdev)
 					    mxcfb_alpha_irq_handler, 0,
 					    MXCFB_NAME, fbi) != 0) {
 				dev_err(&pdev->dev, "Error registering BG "
-						    "alpha irq handler.\n");
+					"alpha irq handler.\n");
 				ret = -EBUSY;
 				goto err1;
 			}
@@ -1552,7 +1683,7 @@ static int mxcfb_probe(struct platform_device *pdev)
 				    mxcfb_alpha_irq_handler, 0,
 				    MXCFB_NAME, fbi) != 0) {
 			dev_err(&pdev->dev, "Error registering FG alpha irq "
-					    "handler.\n");
+				"handler.\n");
 			ret = -EBUSY;
 			goto err1;
 		}
@@ -1572,31 +1703,63 @@ static int mxcfb_probe(struct platform_device *pdev)
 	if (res && res->end) {
 		fbi->fix.smem_len = res->end - res->start + 1;
 		fbi->fix.smem_start = res->start;
-		fbi->screen_base = ioremap(fbi->fix.smem_start, fbi->fix.smem_len);
+		fbi->screen_base =
+		    ioremap(fbi->fix.smem_start, fbi->fix.smem_len);
 	}
 
 	/* Need dummy values until real panel is configured */
 	fbi->var.xres = 240;
 	fbi->var.yres = 320;
 
-	if (!fb_mode && plat_data && plat_data->mode_str)
-		fb_find_mode(&fbi->var, fbi, plat_data->mode_str, NULL, 0, NULL,
-			     default_bpp);
-
-	if (fb_mode)
-		fb_find_mode(&fbi->var, fbi, fb_mode, NULL, 0, NULL,
-			     default_bpp);
+	if (!mxcfbi->default_bpp)
+#ifdef CONFIG_CCWMX51_DEFAULT_VIDEO_BPP
+		mxcfbi->default_bpp = CONFIG_CCWMX51_DEFAULT_VIDEO_BPP;
+#else
+		mxcfbi->default_bpp = 16;
+#endif
 
-	if (plat_data) {
+	if (plat_data && !mxcfbi->ipu_di_pix_fmt)
 		mxcfbi->ipu_di_pix_fmt = plat_data->interface_pix_fmt;
-		if (!fb_mode && plat_data->mode)
-			fb_videomode_to_var(&fbi->var, plat_data->mode);
+
+	if (plat_data && plat_data->mode && plat_data->num_modes)
+		fb_videomode_to_modelist(plat_data->mode, plat_data->num_modes,
+				&fbi->modelist);
+
+	if (!mxcfbi->fb_mode_str && plat_data && plat_data->mode_str)
+		mxcfbi->fb_mode_str = plat_data->mode_str;
+
+	if (mxcfbi->fb_mode_str) {
+
+#ifdef CONFIG_MODULE_CCXMX51
+		if ((mstr = strstr(mxcfbi->fb_mode_str, "VGA@")) != NULL)
+			mxcfbi->fb_mode_str = mstr + 4;
+#endif
+
+		ret =
+		    fb_find_mode(&fbi->var, fbi, mxcfbi->fb_mode_str, NULL, 0,
+				 NULL, mxcfbi->default_bpp);
+		if ((!ret || (ret > 2)) && plat_data && plat_data->mode
+		    && plat_data->num_modes)
+			fb_find_mode(&fbi->var, fbi, mxcfbi->fb_mode_str,
+				     plat_data->mode, plat_data->num_modes,
+				     NULL, mxcfbi->default_bpp);
+
+#ifdef CONFIG_MODULE_CCXMX51
+		/* This improves the VGA modes on the CCWi-i.MX51 */
+		if (mstr != NULL) {
+			mxcfbi->ipu_ext_clk = true;
+			fbi->var.sync |= FB_SYNC_CLK_LAT_FALL;
+		}
+#endif
 	}
 
 	mxcfb_check_var(&fbi->var, fbi);
 
+	pm_set_vt_switch(vt_switch);
+
 	/* Default Y virtual size is 2x panel size */
-	fbi->var.yres_virtual = fbi->var.yres * 3;
+	fbi->var.yres_virtual = ((fbi->var.yres + 127) & ~127) * 2;
+	fbi->var.xres_virtual = (fbi->var.xres + 31) & ~31;
 
 	mxcfb_set_fix(fbi);
 
@@ -1617,12 +1780,12 @@ static int mxcfb_probe(struct platform_device *pdev)
 
 	return 0;
 
-err2:
+      err2:
 	ipu_free_irq(mxcfbi->ipu_ch_irq, fbi);
-err1:
+      err1:
 	fb_dealloc_cmap(&fbi->cmap);
 	framebuffer_release(fbi);
-err0:
+      err0:
 	return ret;
 }
 
@@ -1655,38 +1818,108 @@ static struct platform_driver mxcfb_driver = {
 		   },
 	.probe = mxcfb_probe,
 	.remove = mxcfb_remove,
+#ifndef CONFIG_HAS_EARLYSUSPEND
 	.suspend = mxcfb_suspend,
 	.resume = mxcfb_resume,
+#endif
 };
 
 /*
  * Parse user specified options (`video=trident:')
  * example:
- * 	video=trident:800x600,bpp=16,noaccel
+ * 	video=mxcdi0fb:RGB24, 1024x768M-16@60,bpp=16,noaccel
  */
-int mxcfb_setup(char *options)
+static int mxcfb_option_setup(struct fb_info *info, char *options)
 {
+	struct mxcfb_info *mxcfbi = info->par;
 	char *opt;
+
 	if (!options || !*options)
 		return 0;
+
 	while ((opt = strsep(&options, ",")) != NULL) {
 		if (!*opt)
 			continue;
+
+		if (!strncmp(opt, "RGB24", 5)) {
+			mxcfbi->ipu_di_pix_fmt = IPU_PIX_FMT_RGB24;
+			continue;
+		}
+		if (!strncmp(opt, "BGR24", 5)) {
+			mxcfbi->ipu_di_pix_fmt = IPU_PIX_FMT_BGR24;
+			continue;
+		}
+		if (!strncmp(opt, "RGB565", 6)) {
+			mxcfbi->ipu_di_pix_fmt = IPU_PIX_FMT_RGB565;
+			continue;
+		}
+		if (!strncmp(opt, "RGB666", 6)) {
+			mxcfbi->ipu_di_pix_fmt = IPU_PIX_FMT_RGB666;
+			continue;
+		}
+		if (!strncmp(opt, "YUV444", 6)) {
+			mxcfbi->ipu_di_pix_fmt = IPU_PIX_FMT_YUV444;
+			continue;
+		}
+		if (!strncmp(opt, "LVDS666", 7)) {
+			mxcfbi->ipu_di_pix_fmt = IPU_PIX_FMT_LVDS666;
+			continue;
+		}
+		if (!strncmp(opt, "YUYV16", 6)) {
+			mxcfbi->ipu_di_pix_fmt = IPU_PIX_FMT_YUYV;
+			continue;
+		}
+		if (!strncmp(opt, "UYVY16", 6)) {
+			mxcfbi->ipu_di_pix_fmt = IPU_PIX_FMT_UYVY;
+			continue;
+		}
+		if (!strncmp(opt, "YVYU16", 6)) {
+			mxcfbi->ipu_di_pix_fmt = IPU_PIX_FMT_YVYU;
+			continue;
+		}
+		if (!strncmp(opt, "VYUY16", 6)) {
+			mxcfbi->ipu_di_pix_fmt = IPU_PIX_FMT_VYUY;
+			continue;
+		}
 		if (!strncmp(opt, "ext_clk", 7)) {
-			ext_clk_used = true;
+			mxcfbi->ipu_ext_clk = true;
 			continue;
-		} else
-			ext_clk_used = false;
-
+		}
 		if (!strncmp(opt, "bpp=", 4))
-			default_bpp = simple_strtoul(opt + 4, NULL, 0);
+			mxcfbi->default_bpp = simple_strtoul(opt + 4, NULL, 0);
 		else
-			fb_mode = opt;
-
+			mxcfbi->fb_mode_str = opt;
 	}
+
 	return 0;
 }
 
+#ifdef CONFIG_HAS_EARLYSUSPEND
+static void mxcfb_early_suspend(struct early_suspend *h)
+{
+	int i;
+	for (i = FB_DEVICE_NUM - 1; i >= 0; i--) {
+		if (mxcfb_info[i])
+			mxcfb_suspend_one(mxcfb_info[i]);
+	}
+}
+
+static void mxcfb_later_resume(struct early_suspend *h)
+{
+	int i;
+	for (i = 0; i < FB_DEVICE_NUM; i++) {
+		if (mxcfb_info[i])
+			mxcfb_resume_one(mxcfb_info[i]);
+	}
+}
+
+struct early_suspend fbdrv_earlysuspend = {
+	.level = EARLY_SUSPEND_LEVEL_DISABLE_FB,
+	.suspend = mxcfb_early_suspend,
+	.resume = mxcfb_later_resume,
+};
+#endif
+
 /*!
  * Main entry function for the framebuffer. The function registers the power
  * management callback functions with the kernel and also registers the MXCFB
@@ -1696,26 +1929,23 @@ int mxcfb_setup(char *options)
  */
 int __init mxcfb_init(void)
 {
-	int ret = 0;
-#ifndef MODULE
-	char *option = NULL;
-#endif
-
-#ifndef MODULE
-	if (fb_get_options("mxcfb", &option))
-		return -ENODEV;
-	mxcfb_setup(option);
-#endif
+	int ret;
 
 	ret = platform_driver_register(&mxcfb_driver);
+	if (!ret)
+		register_early_suspend(&fbdrv_earlysuspend);
 	return ret;
 }
 
 void mxcfb_exit(void)
 {
+	unregister_early_suspend(&fbdrv_earlysuspend);
 	platform_driver_unregister(&mxcfb_driver);
 }
 
+module_param(vt_switch, int, 0);
+MODULE_PARM_DESC(vt_switch, "enable VT switch during suspend/resume");
+
 module_init(mxcfb_init);
 module_exit(mxcfb_exit);
 
diff --git a/drivers/video/mxc/mxcfb_claa_wvga.c b/drivers/video/mxc/mxcfb_claa_wvga.c
index bd5ff7b83f8c..8f696c19e7d9 100644
--- a/drivers/video/mxc/mxcfb_claa_wvga.c
+++ b/drivers/video/mxc/mxcfb_claa_wvga.c
@@ -48,8 +48,8 @@ static int lcd_on;
 
 static struct fb_videomode video_modes[] = {
 	{
-	 /* 800x480 @ 55 Hz , pixel clk @ 25MHz */
-	 "CLAA-WVGA", 55, 800, 480, 40000, 40, 40, 5, 5, 20, 10,
+	 /* 800x480 @ 57 Hz , pixel clk @ 27MHz */
+	 "CLAA-WVGA", 57, 800, 480, 37037, 40, 60, 10, 10, 20, 10,
 	 FB_SYNC_CLK_LAT_FALL,
 	 FB_VMODE_NONINTERLACED,
 	 0,},
@@ -77,13 +77,14 @@ static int lcd_fb_event(struct notifier_block *nb, unsigned long val, void *v)
 {
 	struct fb_event *event = v;
 
-	if (strcmp(event->info->fix.id, "DISP3 BG")) {
+	if (strcmp(event->info->fix.id, "DISP3 BG") &&
+	    strcmp(event->info->fix.id, "mxc_elcdif_fb"))
 		return 0;
-	}
 
 	switch (val) {
 	case FB_EVENT_FB_REGISTERED:
 		lcd_init_fb(event->info);
+		fb_show_logo(event->info, 0);
 		lcd_poweron();
 		break;
 	case FB_EVENT_BLANK:
@@ -133,7 +134,8 @@ static int __devinit lcd_probe(struct platform_device *pdev)
 	}
 
 	for (i = 0; i < num_registered_fb; i++) {
-		if (strcmp(registered_fb[i]->fix.id, "DISP3 BG") == 0) {
+		if (strcmp(registered_fb[i]->fix.id, "DISP3 BG") == 0 ||
+		    strcmp(registered_fb[i]->fix.id, "mxc_elcdif_fb") == 0) {
 			lcd_init_fb(registered_fb[i]);
 			fb_show_logo(registered_fb[i], 0);
 			lcd_poweron();
diff --git a/drivers/video/mxc/tve.c b/drivers/video/mxc/tve.c
index 2d2929c0cbd9..793b470cdce4 100644
--- a/drivers/video/mxc/tve.c
+++ b/drivers/video/mxc/tve.c
@@ -68,6 +68,8 @@ static int enabled;		/* enable power on or not */
 DEFINE_SPINLOCK(tve_lock);
 
 static struct fb_info *tve_fbi;
+static struct fb_modelist tve_modelist;
+static bool g_enable_tve;
 
 struct tve_data {
 	struct platform_device *pdev;
@@ -77,6 +79,7 @@ struct tve_data {
 	int detect;
 	void *base;
 	int irq;
+	int blank;
 	struct clk *clk;
 	struct regulator *dac_reg;
 	struct regulator *dig_reg;
@@ -221,40 +224,50 @@ static int _is_tvout_mode_hd_compatible(void)
 static int tve_setup(int mode)
 {
 	u32 reg;
-	struct clk *pll3_clk;
-	unsigned long pll3_clock_rate = 216000000, di1_clock_rate = 27000000;
+	struct clk *tve_parent_clk;
+	unsigned long parent_clock_rate = 216000000, di1_clock_rate = 27000000;
+	unsigned long tve_clock_rate = 216000000;
 	struct clk *ipu_di1_clk;
 	unsigned long lock_flags;
 
-	if (tve.cur_mode == mode)
-		return 0;
-
 	spin_lock_irqsave(&tve_lock, lock_flags);
 
-	tve.cur_mode = mode;
-
 	switch (mode) {
 	case TVOUT_FMT_PAL:
 	case TVOUT_FMT_NTSC:
-		pll3_clock_rate = 216000000;
+		parent_clock_rate = 216000000;
 		di1_clock_rate = 27000000;
 		break;
 	case TVOUT_FMT_720P60:
-		pll3_clock_rate = 297000000;
+		parent_clock_rate = 297000000;
+		if (cpu_is_mx53())
+			tve_clock_rate = 297000000;
 		di1_clock_rate = 74250000;
 		break;
 	}
 	if (enabled)
 		clk_disable(tve.clk);
 
-	pll3_clk = clk_get(NULL, "pll3");
+	tve_parent_clk = clk_get_parent(tve.clk);
 	ipu_di1_clk = clk_get(NULL, "ipu_di1_clk");
 
-	clk_disable(pll3_clk);
-	clk_set_rate(pll3_clk, pll3_clock_rate);
-	clk_set_rate(ipu_di1_clk, di1_clock_rate);
+	clk_disable(tve_parent_clk);
+	clk_set_rate(tve_parent_clk, parent_clock_rate);
+
+	if (cpu_is_mx53())
+		clk_set_rate(tve.clk, tve_clock_rate);
 
 	clk_enable(tve.clk);
+	clk_set_rate(ipu_di1_clk, di1_clock_rate);
+
+	if (tve.cur_mode == mode) {
+		if (!enabled)
+			clk_disable(tve.clk);
+		spin_unlock_irqrestore(&tve_lock, lock_flags);
+		return 0;
+	}
+
+	tve.cur_mode = mode;
 
 	/* select output video format */
 	if (mode == TVOUT_FMT_PAL) {
@@ -497,6 +510,14 @@ static irqreturn_t tve_detect_handler(int irq, void *data)
 	return IRQ_HANDLED;
 }
 
+/* Re-construct clk for tve display */
+static inline void tve_recfg_fb(struct fb_info *fbi)
+{
+	fbi->flags &= ~FBINFO_MISC_USEREVENT;
+	fbi->var.activate |= FB_ACTIVATE_FORCE;
+	fb_set_var(fbi, &fbi->var);
+}
+
 int tve_fb_event(struct notifier_block *nb, unsigned long val, void *v)
 {
 	struct fb_event *event = v;
@@ -509,9 +530,9 @@ int tve_fb_event(struct notifier_block *nb, unsigned long val, void *v)
 			break;
 
 		tve_fbi = fbi;
-		fb_add_videomode(&video_modes[0], &tve_fbi->modelist);
-		fb_add_videomode(&video_modes[1], &tve_fbi->modelist);
-		fb_add_videomode(&video_modes[2], &tve_fbi->modelist);
+		fb_add_videomode(&video_modes[0], &tve_modelist.list);
+		fb_add_videomode(&video_modes[1], &tve_modelist.list);
+		fb_add_videomode(&video_modes[2], &tve_modelist.list);
 		break;
 	case FB_EVENT_MODE_CHANGE:
 	{
@@ -525,7 +546,7 @@ int tve_fb_event(struct notifier_block *nb, unsigned long val, void *v)
 
 		fb_var_to_videomode(&cur_mode, &fbi->var);
 
-		list_for_each(pos, &tve_fbi->modelist) {
+		list_for_each(pos, &tve_modelist.list) {
 			modelist = list_entry(pos, struct fb_modelist, list);
 			mode = &modelist->mode;
 			if (fb_mode_is_equal(&cur_mode, mode)) {
@@ -564,31 +585,33 @@ int tve_fb_event(struct notifier_block *nb, unsigned long val, void *v)
 			return 0;
 
 		if (*((int *)event->data) == FB_BLANK_UNBLANK) {
-			if (fb_mode_is_equal(fbi->mode, &video_modes[0])) {
-				if (tve.cur_mode != TVOUT_FMT_NTSC) {
+			if (tve.blank != FB_BLANK_UNBLANK) {
+				if (fb_mode_is_equal(fbi->mode, &video_modes[0])) {
 					tve_disable();
 					tve_setup(TVOUT_FMT_NTSC);
-				}
-				tve_enable();
-			} else if (fb_mode_is_equal(fbi->mode,
-					&video_modes[1])) {
-				if (tve.cur_mode != TVOUT_FMT_PAL) {
+					tve_enable();
+					tve_recfg_fb(fbi);
+				} else if (fb_mode_is_equal(fbi->mode,
+							&video_modes[1])) {
 					tve_disable();
 					tve_setup(TVOUT_FMT_PAL);
-				}
-				tve_enable();
-			} else if (fb_mode_is_equal(fbi->mode,
-					&video_modes[2])) {
-				if (tve.cur_mode != TVOUT_FMT_720P60) {
+					tve_enable();
+					tve_recfg_fb(fbi);
+				} else if (fb_mode_is_equal(fbi->mode,
+							&video_modes[2])) {
 					tve_disable();
 					tve_setup(TVOUT_FMT_720P60);
+					tve_enable();
+					tve_recfg_fb(fbi);
+				} else {
+					tve_setup(TVOUT_FMT_OFF);
 				}
-				tve_enable();
-			} else {
-				tve_setup(TVOUT_FMT_OFF);
+				tve.blank = FB_BLANK_UNBLANK;
 			}
-		} else
+		} else {
 			tve_disable();
+			tve.blank = FB_BLANK_POWERDOWN;
+		}
 		break;
 	}
 	return 0;
@@ -652,6 +675,11 @@ static int tve_probe(struct platform_device *pdev)
 	struct tve_platform_data *plat_data = pdev->dev.platform_data;
 	u32 conf_reg;
 
+	if (g_enable_tve == false)
+		return -ENODEV;
+
+	INIT_LIST_HEAD(&tve_modelist.list);
+
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (res == NULL)
 		return -ENOMEM;
@@ -699,9 +727,9 @@ static int tve_probe(struct platform_device *pdev)
 	}
 
 	if (tve_fbi != NULL) {
-		fb_add_videomode(&video_modes[0], &tve_fbi->modelist);
-		fb_add_videomode(&video_modes[1], &tve_fbi->modelist);
-		fb_add_videomode(&video_modes[2], &tve_fbi->modelist);
+		fb_add_videomode(&video_modes[0], &tve_modelist.list);
+		fb_add_videomode(&video_modes[1], &tve_modelist.list);
+		fb_add_videomode(&video_modes[2], &tve_modelist.list);
 	}
 
 	tve.dac_reg = regulator_get(&pdev->dev, plat_data->dac_reg);
@@ -748,22 +776,40 @@ static int tve_probe(struct platform_device *pdev)
 
 	clk_disable(tve.clk);
 
+	ret = fb_register_client(&nb);
+	if (ret < 0)
+		goto err2;
+
+	tve.blank = -1;
+
 	/* is primary display? */
 	if (primary) {
-		struct fb_event event;
+		struct fb_var_screeninfo var;
+		const struct fb_videomode *mode;
+
+		memset(&var, 0, sizeof(var));
+		mode = fb_match_mode(&tve_fbi->var, &tve_modelist.list);
+		if (mode) {
+			pr_debug("TVE: fb mode found\n");
+			fb_videomode_to_var(&var, mode);
+		} else {
+			pr_warning("TVE: can not find video mode\n");
+			goto done;
+		}
+		acquire_console_sem();
+		tve_fbi->flags |= FBINFO_MISC_USEREVENT;
+		fb_set_var(tve_fbi, &var);
+		tve_fbi->flags &= ~FBINFO_MISC_USEREVENT;
+		release_console_sem();
 
-		event.info = tve_fbi;
-		tve_fb_event(NULL, FB_EVENT_MODE_CHANGE, &event);
 		acquire_console_sem();
 		fb_blank(tve_fbi, FB_BLANK_UNBLANK);
 		release_console_sem();
+
 		fb_show_logo(tve_fbi, 0);
 	}
 
-	ret = fb_register_client(&nb);
-	if (ret < 0)
-		goto err2;
-
+done:
 	return 0;
 err2:
 	device_remove_file(&pdev->dev, &dev_attr_headphone);
@@ -842,6 +888,14 @@ static struct platform_driver tve_driver = {
 	.resume = tve_resume,
 };
 
+static int __init enable_tve_setup(char *options)
+{
+	g_enable_tve = true;
+
+	return 1;
+}
+__setup("tve", enable_tve_setup);
+
 static int __init tve_init(void)
 {
 	return platform_driver_register(&tve_driver);
diff --git a/drivers/video/mxs/Kconfig b/drivers/video/mxs/Kconfig
index aef4aa59dcad..35b896e95d4f 100644
--- a/drivers/video/mxs/Kconfig
+++ b/drivers/video/mxs/Kconfig
@@ -20,3 +20,9 @@ config FB_MXS_LCD_LMS430
 	default y if ARCH_MX23
 	---help---
 	  Use LMS430 dotclock LCD panel for MXS
+
+config FB_MXS_TVENC
+	depends on ARCH_MXS
+	bool "TVENC"
+	---help---
+	  Use TVOUT encoder for MXS
diff --git a/drivers/video/mxs/Makefile b/drivers/video/mxs/Makefile
index a9580add3757..fbab953718c7 100644
--- a/drivers/video/mxs/Makefile
+++ b/drivers/video/mxs/Makefile
@@ -2,3 +2,5 @@ obj-$(CONFIG_ARCH_MXS)				+= lcdif.o
 obj-$(CONFIG_FB_MXS)				+= mxsfb.o
 obj-$(CONFIG_FB_MXS_LCD_43WVF1G)		+= lcd_43wvf1g.o
 obj-$(CONFIG_FB_MXS_LCD_LMS430)			+= lcd_lms430.o
+# TVOUT support
+obj-$(CONFIG_FB_MXS_TVENC)			+= tvenc.o