Merge remote-tracking branch 'fsl-linux-sdk/imx_2.6.38' into imx_2.6.38_android

Conflicts:
	arch/arm/mach-mx6/board-mx6q_arm2.c
	arch/arm/mach-mx6/board-mx6q_sabrelite.c

49 files changed, 947 insertions, 636 deletions

diff --git a/Documentation/arm/imx/udc.txt b/Documentation/arm/imx/udc.txt
new file mode 100644
index 000000000000..fdbaa848983b
--- /dev/null
+++ b/Documentation/arm/imx/udc.txt
@@ -0,0 +1,16 @@
+How to test remote wakeup (Make sure connecting to usb host and gadget is loaded)
+1. Prepare a usb host which featured with remote wakeup
+alternatively, You can use another i.mx board (like mx50 rdp, or mx6q)
+2. Boot up both boards, and connect two boards with usb cable
+3. At device side, do below commands:
+- modprobe g_file_storage file=/dev/mmcblk0p1 (other gadgets are also ok)
+- echo 1 > /sys/devices/platform/fsl-usb2-udc/gadget/remote_wakeup
+4. At host side, do below command:
+- echo enabled > /sys/devices/platform/fsl-ehci.1/power/wakeup
+- echo standby > /sys/power/state
+5. Send Remote wakeup to wake up host
+At device side, do below command:
+echo 1 > /sys/devices/platform/fsl-usb2-udc/gadget/start_remote_wakeup
+6. The expected behaviour is: host is waked up, and there is
+not re-enumeration happens.
+
diff --git a/arch/arm/configs/imx6_defconfig b/arch/arm/configs/imx6_defconfig
index 0f2c0e802856..5ac6c160b0a8 100644
--- a/arch/arm/configs/imx6_defconfig
+++ b/arch/arm/configs/imx6_defconfig
@@ -299,7 +299,7 @@ CONFIG_IMX_HAVE_PLATFORM_IMX_MIPI_CSI2=y
 # CONFIG_ARCH_MXC91231 is not set
 # CONFIG_ARCH_MX5 is not set
 CONFIG_ARCH_MX6=y
-CONFIG_FORCE_MAX_ZONEORDER=13
+CONFIG_FORCE_MAX_ZONEORDER=14
 CONFIG_ARCH_MX6Q=y
 CONFIG_SOC_IMX6Q=y
 CONFIG_MACH_MX6Q_ARM2=y
@@ -1857,7 +1857,7 @@ CONFIG_DMA_ENGINE=y
 # CONFIG_DMATEST is not set
 # CONFIG_AUXDISPLAY is not set
 # CONFIG_UIO is not set
-CONFIG_STAGING=y
+# CONFIG_STAGING is not set
 # CONFIG_STAGING_EXCLUDE_BUILD is not set
 # CONFIG_USB_DABUSB is not set
 # CONFIG_USB_SE401 is not set
@@ -1885,8 +1885,8 @@ CONFIG_STAGING=y
 # CONFIG_SMB_FS is not set
 # CONFIG_EASYCAP is not set
 CONFIG_MACH_NO_WESTBRIDGE=y
-CONFIG_ATH6K_LEGACY=m
-CONFIG_AR600x_SD31_XXX=y
+# CONFIG_ATH6K_LEGACY is not set
+# CONFIG_AR600x_SD31_XXX is not set
 # CONFIG_AR600x_WB31_XXX is not set
 # CONFIG_AR600x_SD32_XXX is not set
 # CONFIG_AR600x_CUSTOM_XXX is not set
diff --git a/arch/arm/mach-mx5/Makefile b/arch/arm/mach-mx5/Makefile
index 0128d640793b..8dbcf863f42e 100644
--- a/arch/arm/mach-mx5/Makefile
+++ b/arch/arm/mach-mx5/Makefile
@@ -4,7 +4,7 @@
 
 # Object file lists.
 obj-y   := cpu.o mm.o devices.o ehci.o bus_freq.o sdram_autogating.o  \
-pm.o system.o suspend.o usb_dr.o usb_h1.o usb_h2.o
+pm.o system.o suspend.o usb_dr.o usb_h1.o usb_h2.o cpu_regulator-mx5.o
 
 obj-$(CONFIG_SOC_IMX50) += clock_mx50.o mm-mx50.o mx50_wfi.o mx50_suspend.o mx50_freq.o mx50_ddr_freq.o
 obj-$(CONFIG_SOC_IMX51) += clock.o
diff --git a/arch/arm/mach-mx5/board-mx50_rdp.c b/arch/arm/mach-mx5/board-mx50_rdp.c
index a960dd224cfc..a686c71c1c46 100644
--- a/arch/arm/mach-mx5/board-mx50_rdp.c
+++ b/arch/arm/mach-mx5/board-mx50_rdp.c
@@ -94,15 +94,10 @@
 #define MX50_RDP_SD3_WP		IMX_GPIO_NR(5, 28) 	/*GPIO_5_28 */
 #define MX50_RDP_USB_OTG_PWR	IMX_GPIO_NR(6, 25)	/*GPIO_6_25*/
 
-extern int (*set_cpu_voltage)(u32 volt);
-extern int mx5_set_cpu_voltage(struct regulator *gp_reg, u32 cpu_volt);
-
 extern int mx50_rdp_init_mc13892(void);
-
+extern void mx5_cpu_regulator_init(void);
 extern char *lp_reg_id;
-
-static char *gp_reg_id;
-static struct regulator *cpu_regulator;
+extern char *gp_reg_id;
 static int max17135_regulator_init(struct max17135 *max17135);
 
 static iomux_v3_cfg_t mx50_rdp_pads[] __initdata = {
@@ -796,18 +791,6 @@ static struct mxc_regulator_platform_data rdp_regulator_data = {
 	.vcc_reg_id = "lp_vcc",
 };
 
-static int mx50_rdp_set_cpu_voltage(u32 cpu_volt)
-{
-	int ret = -EINVAL;
-
-	if (cpu_regulator == NULL)
-		cpu_regulator = regulator_get(NULL, gp_reg_id);
-	if (!IS_ERR(cpu_regulator))
-		ret = mx5_set_cpu_voltage(cpu_regulator, cpu_volt);
-
-	return ret;
-}
-
 static const struct esdhc_platform_data mx50_rdp_sd1_data __initconst = {
 	.cd_gpio = MX50_RDP_SD1_CD,
 	.wp_gpio = MX50_RDP_SD1_WP,
@@ -826,7 +809,6 @@ static const struct esdhc_platform_data mx50_rdp_sd3_data __initconst = {
 static void __init fixup_mxc_board(struct machine_desc *desc, struct tag *tags,
 				   char **cmdline, struct meminfo *mi)
 {
-	set_cpu_voltage = mx50_rdp_set_cpu_voltage;
 }
 
 static void mx50_rdp_usbotg_vbus(bool on)
@@ -901,6 +883,8 @@ static void __init mx50_rdp_board_init(void)
 	imx50_add_busfreq();
 
 	mx50_rdp_init_usb();
+
+	mx5_cpu_regulator_init();
 }
 
 static void __init mx50_rdp_timer_init(void)
diff --git a/arch/arm/mach-mx5/board-mx51_babbage.c b/arch/arm/mach-mx5/board-mx51_babbage.c
index 17a02201e241..b808dfdaf846 100644
--- a/arch/arm/mach-mx5/board-mx51_babbage.c
+++ b/arch/arm/mach-mx5/board-mx51_babbage.c
@@ -84,11 +84,8 @@
 #define	MX51_USB_PLL_DIV_24_MHZ	0x02
 
 extern char *lp_reg_id;
-extern int (*set_cpu_voltage)(u32 volt);
-extern int mx5_set_cpu_voltage(struct regulator *gp_reg, u32 cpu_volt);
-
-static struct regulator *cpu_regulator;
-static char *gp_reg_id;
+extern char *gp_reg_id;
+extern void mx5_cpu_regulator_init(void);
 
 static struct gpio_keys_button babbage_buttons[] = {
 	{
@@ -562,19 +559,6 @@ static struct i2c_board_info mxc_i2c_hs_board_info[] __initdata = {
 
 static struct mxc_gpu_platform_data gpu_data __initdata;
 
-static int mx51_bbg_set_cpu_voltage(u32 cpu_volt)
-{
-	int ret = -EINVAL;
-
-	if (cpu_regulator == NULL)
-		cpu_regulator = regulator_get(NULL, gp_reg_id);
-	if (!IS_ERR(cpu_regulator))
-		ret = mx5_set_cpu_voltage(cpu_regulator, cpu_volt);
-
-	return ret;
-
-}
-
 static void __init fixup_mxc_board(struct machine_desc *desc, struct tag *tags,
 				   char **cmdline, struct meminfo *mi)
 {
@@ -586,8 +570,6 @@ static void __init fixup_mxc_board(struct machine_desc *desc, struct tag *tags,
 	int fb_mem = 0;
 	char *str;
 
-	set_cpu_voltage = mx51_bbg_set_cpu_voltage;
-
 	for_each_tag(mem_tag, tags) {
 		if (mem_tag->hdr.tag == ATAG_MEM) {
 			total_mem = mem_tag->u.mem.size;
@@ -826,6 +808,7 @@ static void __init mx51_babbage_init(void)
 
 	imx51_add_dvfs_core(&bbg_dvfscore_data);
 	imx51_add_busfreq();
+	mx5_cpu_regulator_init();
 }
 
 static void __init mx51_babbage_timer_init(void)
diff --git a/arch/arm/mach-mx5/board-mx53_ard.c b/arch/arm/mach-mx5/board-mx53_ard.c
index f07ed8e48790..9541210b77b1 100644
--- a/arch/arm/mach-mx5/board-mx53_ard.c
+++ b/arch/arm/mach-mx5/board-mx53_ard.c
@@ -61,12 +61,9 @@
 #define ARD_SSI_STEERING		(MAX7310_BASE_ADDR + 6)
 #define ARD_GPS_RST_B			(MAX7310_BASE_ADDR + 7)
 
-static struct regulator *cpu_regulator;
-static char *gp_reg_id;
-
+extern char *gp_reg_id;
 extern char *lp_reg_id;
-extern void (*set_cpu_voltage)(u32 volt);
-extern int mx5_set_cpu_voltage(struct regulator *gp_reg, u32 cpu_volt);
+extern void mx5_cpu_regulator_init(void);
 
 static iomux_v3_cfg_t mx53_ard_pads[] = {
 	/* UART */
@@ -354,22 +351,9 @@ static struct mxc_regulator_platform_data ard_regulator_data = {
 	.cpu_reg_id = "SW1",
 };
 
-static int mx53_ard_set_cpu_voltage(u32 cpu_volt)
-{
-	int ret = -EINVAL;
-
-	if (cpu_regulator == NULL)
-		cpu_regulator = regulator_get(NULL, gp_reg_id);
-	if (!IS_ERR(cpu_regulator))
-		ret = mx5_set_cpu_voltage(cpu_regulator, cpu_volt);
-
-	return ret;
-}
-
 static void __init fixup_mxc_board(struct machine_desc *desc, struct tag *tags,
 				   char **cmdline, struct meminfo *mi)
 {
-	set_cpu_voltage = mx53_ard_set_cpu_voltage;
 }
 
 static inline void mx53_ard_init_uart(void)
@@ -501,6 +485,8 @@ static void __init mx53_ard_board_init(void)
 
 	imx53_init_audio();
 
+	mx5_cpu_regulator_init();
+
 	/* this call required to release SCC RAM partition held by ROM
 	  * during boot, even if SCC2 driver is not part of the image
 	  */
diff --git a/arch/arm/mach-mx5/board-mx53_evk.c b/arch/arm/mach-mx5/board-mx53_evk.c
index 63d555893dc7..be70320403fc 100644
--- a/arch/arm/mach-mx5/board-mx53_evk.c
+++ b/arch/arm/mach-mx5/board-mx53_evk.c
@@ -80,11 +80,8 @@
 #define ARM2_LCD_CONTRAST		IMX_GPIO_NR(4, 20)	/* GPIO_4_20 */
 
 extern char *lp_reg_id;
-extern int (*set_cpu_voltage)(u32 volt);
-extern int mx5_set_cpu_voltage(struct regulator *gp_reg, u32 cpu_volt);
-
-static struct regulator *cpu_regulator;
-static char *gp_reg_id;
+extern char *gp_reg_id;
+extern void mx5_cpu_regulator_init(void);
 
 static iomux_v3_cfg_t mx53common_pads[] = {
 	MX53_PAD_EIM_WAIT__GPIO5_0,
@@ -728,22 +725,9 @@ static struct mxc_regulator_platform_data evk_regulator_data = {
 	.vcc_reg_id = "SW2",
 };
 
-static int mx53_evk_set_cpu_voltage(u32 cpu_volt)
-{
-	int ret = -EINVAL;
-
-	if (cpu_regulator == NULL)
-		cpu_regulator = regulator_get(NULL, gp_reg_id);
-	if (!IS_ERR(cpu_regulator))
-		ret = mx5_set_cpu_voltage(cpu_regulator, cpu_volt);
-
-	return ret;
-}
-
 static void __init fixup_mxc_board(struct machine_desc *desc, struct tag *tags,
 				   char **cmdline, struct meminfo *mi)
 {
-	set_cpu_voltage = mx53_evk_set_cpu_voltage;
 }
 
 static void __init mx53_evk_board_init(void)
@@ -797,6 +781,8 @@ static void __init mx53_evk_board_init(void)
 	imx53_add_spdif_dai();
 	imx53_add_spdif_audio_device();
 
+	mx5_cpu_regulator_init();
+
 	/* this call required to release SCC RAM partition held by ROM
 	  * during boot, even if SCC2 driver is not part of the image
 	  */
diff --git a/arch/arm/mach-mx5/board-mx53_loco.c b/arch/arm/mach-mx5/board-mx53_loco.c
index 1430fb2588aa..0fdce6db953b 100644
--- a/arch/arm/mach-mx5/board-mx53_loco.c
+++ b/arch/arm/mach-mx5/board-mx53_loco.c
@@ -73,15 +73,11 @@ extern void __iomem *gpc_base;
 extern void __iomem *ccm_base;
 extern void __iomem *imx_otg_base;
 extern char *lp_reg_id;
-extern int (*set_cpu_voltage)(u32 volt);
-extern int mx5_set_cpu_voltage(struct regulator *gp_reg, u32 cpu_volt);
-
+extern char *gp_reg_id;
+extern void mx5_cpu_regulator_init(void);
 extern int __init mx53_loco_init_da9052(void);
 extern int __init mx53_loco_init_mc34708(u32 int_gpio);
 
-static struct regulator *cpu_regulator;
-static char *gp_reg_id;
-
 static iomux_v3_cfg_t mx53_loco_pads[] = {
 	/* FEC */
 	MX53_PAD_FEC_MDC__FEC_MDC,
@@ -599,17 +595,6 @@ static struct mxc_regulator_platform_data loco_regulator_data = {
 	.cpu_reg_id = "cpu_vddgp",
 };
 
-static int mx53_loco_set_cpu_voltage(u32 cpu_volt)
-{
-	int ret = -EINVAL;
-
-	if (cpu_regulator == NULL)
-		cpu_regulator = regulator_get(NULL, gp_reg_id);
-	if (!IS_ERR(cpu_regulator))
-		ret = mx5_set_cpu_voltage(cpu_regulator, cpu_volt);
-	return ret;
-}
-
 static void __init fixup_mxc_board(struct machine_desc *desc, struct tag *tags,
 				   char **cmdline, struct meminfo *mi)
 {
@@ -621,8 +606,6 @@ static void __init fixup_mxc_board(struct machine_desc *desc, struct tag *tags,
 	int fb_mem = SZ_32M;
 	char *str;
 
-	set_cpu_voltage = mx53_loco_set_cpu_voltage;
-
 	for_each_tag(mem_tag, tags) {
 		if (mem_tag->hdr.tag == ATAG_MEM) {
 			total_mem = mem_tag->u.mem.size;
@@ -782,6 +765,7 @@ static void __init mx53_loco_board_init(void)
 	imx53_add_mxc_scc2();
 	imx53_add_dvfs_core(&loco_dvfs_core_data);
 	imx53_add_busfreq();
+	mx5_cpu_regulator_init();
 }
 
 static void __init mx53_loco_timer_init(void)
diff --git a/arch/arm/mach-mx5/board-mx53_smd.c b/arch/arm/mach-mx5/board-mx53_smd.c
index 51ef71d9f267..3f7091c4cdda 100644
--- a/arch/arm/mach-mx5/board-mx53_smd.c
+++ b/arch/arm/mach-mx5/board-mx53_smd.c
@@ -84,13 +84,10 @@
 static struct clk *sata_clk, *sata_ref_clk;
 
 extern char *lp_reg_id;
-extern int (*set_cpu_voltage)(u32 volt);
-extern int mx5_set_cpu_voltage(struct regulator *gp_reg, u32 cpu_volt);
-
+extern char *gp_reg_id;
+extern void mx5_cpu_regulator_init(void);
 extern int mx53_smd_init_da9052(void);
-
-static struct regulator *cpu_regulator;
-static char *gp_reg_id;
+extern void mx5_cpu_regulator_init(void);
 
 static iomux_v3_cfg_t mx53_smd_pads[] = {
 	MX53_PAD_CSI0_DAT10__UART1_TXD_MUX,
@@ -745,22 +742,9 @@ static struct mxc_regulator_platform_data smd_regulator_data = {
 	.cpu_reg_id = "DA9052_BUCK_CORE",
 };
 
-static int mx53_smd_set_cpu_voltage(u32 cpu_volt)
-{
-	int ret = -EINVAL;
-
-	if (cpu_regulator == NULL)
-		cpu_regulator = regulator_get(NULL, gp_reg_id);
-	if (!IS_ERR(cpu_regulator))
-		ret = mx5_set_cpu_voltage(cpu_regulator, cpu_volt);
-
-	return ret;
-}
-
 static void __init fixup_mxc_board(struct machine_desc *desc, struct tag *tags,
 				   char **cmdline, struct meminfo *mi)
 {
-	set_cpu_voltage = mx53_smd_set_cpu_voltage;
 }
 
 static void __init mx53_smd_board_init(void)
@@ -848,6 +832,8 @@ static void __init mx53_smd_board_init(void)
 	  * during boot, even if SCC2 driver is not part of the image
 	  */
 	imx53_add_mxc_scc2();
+
+	mx5_cpu_regulator_init();
 }
 
 static void __init mx53_smd_timer_init(void)
diff --git a/arch/arm/mach-mx5/cpu.c b/arch/arm/mach-mx5/cpu.c
index 4d4cd3d9794c..04625af15039 100644
--- a/arch/arm/mach-mx5/cpu.c
+++ b/arch/arm/mach-mx5/cpu.c
@@ -172,18 +172,6 @@ static int get_mx50_srev(void)
 	return 0;
 }
 
-int mx5_set_cpu_voltage(struct regulator *gp_reg, u32 cpu_volt)
-{
-	u32 ret = 0;
-
-	if (!IS_ERR(gp_reg)) {
-		ret = regulator_set_voltage(gp_reg, cpu_volt, cpu_volt);
-		if (ret < 0)
-			printk(KERN_DEBUG "COULD NOT SET GP VOLTAGE!!!!\n");
-	}
-	return ret;
-}
-
 /*
  * Returns:
  *	the silicon revision of the cpu
diff --git a/arch/arm/mach-mx5/cpu_regulator-mx5.c b/arch/arm/mach-mx5/cpu_regulator-mx5.c
new file mode 100644
index 000000000000..b7d01e91ad39
--- /dev/null
+++ b/arch/arm/mach-mx5/cpu_regulator-mx5.c
@@ -0,0 +1,30 @@
+/*
+ * Copyright (C) 2011 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/types.h>
+#include <linux/kernel.h>
+#include <mach/hardware.h>
+#include <linux/err.h>
+#include <linux/regulator/consumer.h>
+
+struct regulator *cpu_regulator;
+char *gp_reg_id;
+
+
+void mx5_cpu_regulator_init(void)
+{
+	cpu_regulator = regulator_get(NULL, gp_reg_id);
+	if (IS_ERR(cpu_regulator))
+		printk(KERN_ERR "%s: failed to get cpu regulator\n", __func__);
+}
+
diff --git a/arch/arm/mach-mx6/Makefile b/arch/arm/mach-mx6/Makefile
index bd55b5ba9c3f..1bb1cd43e7be 100644
--- a/arch/arm/mach-mx6/Makefile
+++ b/arch/arm/mach-mx6/Makefile
@@ -3,7 +3,7 @@
 #
 
 # Object file lists.
-obj-y   := cpu.o mm.o system.o devices.o dummy_gpio.o irq.o bus_freq.o usb_dr.o usb_h1.o usb_h2.o usb_h3.o pm.o cpu_op-mx6.o mx6_wfi.o mx6_fec.o mx6_anatop_regulator.o
+obj-y   := cpu.o mm.o system.o devices.o dummy_gpio.o irq.o bus_freq.o usb_dr.o usb_h1.o usb_h2.o usb_h3.o pm.o cpu_op-mx6.o mx6_wfi.o mx6_fec.o mx6_anatop_regulator.o cpu_regulator-mx6.o
 
 obj-$(CONFIG_ARCH_MX6) += clock.o mx6q_suspend.o
 obj-$(CONFIG_MACH_MX6Q_ARM2) += board-mx6q_arm2.o
diff --git a/arch/arm/mach-mx6/board-mx6q_arm2.c b/arch/arm/mach-mx6/board-mx6q_arm2.c
index 218a202bd75f..237e727df870 100644
--- a/arch/arm/mach-mx6/board-mx6q_arm2.c
+++ b/arch/arm/mach-mx6/board-mx6q_arm2.c
@@ -37,7 +37,6 @@
 #include <linux/mtd/mtd.h>
 #include <linux/mtd/map.h>
 #include <linux/mtd/partitions.h>
-#include <linux/regulator/consumer.h>
 #include <linux/pmic_external.h>
 #include <linux/pmic_status.h>
 #include <linux/ipu.h>
@@ -116,10 +115,8 @@ static int flexcan_en;
 
 extern struct regulator *(*get_cpu_regulator)(void);
 extern void (*put_cpu_regulator)(void);
-extern int (*set_cpu_voltage)(u32 volt);
-extern int mx6_set_cpu_voltage(u32 cpu_volt);
-static struct regulator *cpu_regulator;
-static char *gp_reg_id;
+extern char *gp_reg_id;
+extern void mx6_cpu_regulator_init(void);
 
 static iomux_v3_cfg_t mx6q_arm2_pads[] = {
 
@@ -733,14 +730,18 @@ static struct i2c_board_info mxc_i2c0_board_info[] __initdata = {
 	},
 };
 
-static struct imxi2c_platform_data mx6q_arm2_i2c_data = {
-	.bitrate = 400000,
+static struct imxi2c_platform_data mx6q_arm2_i2c0_data = {
+	.bitrate = 100000,
 };
 
-static struct imxi2c_platform_data mx6q_arm2_i2c0_data = {
+static struct imxi2c_platform_data mx6q_arm2_i2c1_data = {
 	.bitrate = 100000,
 };
 
+static struct imxi2c_platform_data mx6q_arm2_i2c2_data = {
+	.bitrate = 400000,
+};
+
 static struct i2c_board_info mxc_i2c2_board_info[] __initdata = {
 	{
 		I2C_BOARD_INFO("max7310", 0x1F),
@@ -934,8 +935,8 @@ static void mx6q_sabreauto_reset_mipi_dsi(void)
 }
 
 static struct mipi_dsi_platform_data mipi_dsi_pdata = {
-	.ipu_id	 = 0,
-	.disp_id = 0,
+	.ipu_id	 = 1,
+	.disp_id = 1,
 	.lcd_panel = "TRULY-WVGA",
 	.reset   = mx6q_sabreauto_reset_mipi_dsi,
 };
@@ -948,10 +949,10 @@ static struct ipuv3_fb_platform_data sabr_fb_data[] = {
 	.default_bpp = 32,
 	.int_clk = false,
 	}, {
-	.disp_dev = "lcd",
-	.interface_pix_fmt = IPU_PIX_FMT_RGB565,
-	.mode_str = "CLAA-WVGA",
-	.default_bpp = 32,
+	.disp_dev = "mipi_dsi",
+	.interface_pix_fmt = IPU_PIX_FMT_RGB24,
+	.mode_str = "TRULY-WVGA",
+	.default_bpp = 24,
 	.int_clk = false,
 	}, {
 	.disp_dev = "ldb",
@@ -959,7 +960,13 @@ static struct ipuv3_fb_platform_data sabr_fb_data[] = {
 	.mode_str = "LDB-XGA",
 	.default_bpp = 32,
 	.int_clk = false,
-	},
+	}, {
+	.disp_dev = "lcd",
+	.interface_pix_fmt = IPU_PIX_FMT_RGB565,
+	.mode_str = "CLAA-WVGA",
+	.default_bpp = 16,
+	.int_clk = false,
+	}
 };
 
 static void hdmi_init(int ipu_id, int disp_id)
@@ -1016,7 +1023,7 @@ static struct fsl_mxc_ldb_platform_data ldb_data = {
 	.ext_ref = 1,
 	.mode = LDB_SEP0,
 	.sec_ipu_id = 1,
-	.sec_disp_id = 1,
+	.sec_disp_id = 0,
 };
 
 static struct imx_ipuv3_platform_data ipu_data[] = {
@@ -1381,19 +1388,6 @@ static struct mxc_dvfs_platform_data arm2_dvfscore_data = {
 	.delay_time = 80,
 };
 
-static int mx6_arm2_set_cpu_voltage(u32 cpu_volt)
-{
-	int ret = -EINVAL;
-
-	if (cpu_regulator == NULL)
-		cpu_regulator = regulator_get(NULL, gp_reg_id);
-
-	if (!IS_ERR(cpu_regulator))
-		ret = regulator_set_voltage(cpu_regulator,
-						    cpu_volt, cpu_volt);
-	return ret;
-}
-
 static void __init fixup_mxc_board(struct machine_desc *desc, struct tag *tags,
 				   char **cmdline, struct meminfo *mi)
 {
@@ -1416,8 +1410,6 @@ static void __init fixup_mxc_board(struct machine_desc *desc, struct tag *tags,
 			break;
 		}
 	}
-
-	set_cpu_voltage = mx6_arm2_set_cpu_voltage;
 }
 
 static int __init early_enable_spdif(char *p)
@@ -1516,13 +1508,13 @@ static void __init mx6_board_init(void)
 	imx6q_add_imx_snvs_rtc();
 
 	imx6q_add_imx_i2c(0, &mx6q_arm2_i2c0_data);
-	imx6q_add_imx_i2c(1, &mx6q_arm2_i2c_data);
+	imx6q_add_imx_i2c(1, &mx6q_arm2_i2c1_data);
 	i2c_register_board_info(0, mxc_i2c0_board_info,
 			ARRAY_SIZE(mxc_i2c0_board_info));
 	i2c_register_board_info(1, mxc_i2c1_board_info,
 			ARRAY_SIZE(mxc_i2c1_board_info));
 	if (!spdif_en) {
-		imx6q_add_imx_i2c(2, &mx6q_arm2_i2c_data);
+		imx6q_add_imx_i2c(2, &mx6q_arm2_i2c2_data);
 		i2c_register_board_info(2, mxc_i2c2_board_info,
 				ARRAY_SIZE(mxc_i2c2_board_info));
 	}
@@ -1547,6 +1539,8 @@ static void __init mx6_board_init(void)
 	imx6q_add_vpu();
 	imx6q_init_audio();
 	platform_device_register(&arm2_vmmc_reg_devices);
+	mx6_cpu_regulator_init();
+
 	imx_asrc_data.asrc_core_clk = clk_get(NULL, "asrc_clk");
 	imx_asrc_data.asrc_audio_clk = clk_get(NULL, "asrc_serial_clk");
 	imx6q_add_asrc(&imx_asrc_data);
diff --git a/arch/arm/mach-mx6/board-mx6q_sabreauto.c b/arch/arm/mach-mx6/board-mx6q_sabreauto.c
index 063b8276605f..63195322c55f 100644
--- a/arch/arm/mach-mx6/board-mx6q_sabreauto.c
+++ b/arch/arm/mach-mx6/board-mx6q_sabreauto.c
@@ -37,7 +37,6 @@
 #include <linux/mtd/mtd.h>
 #include <linux/mtd/map.h>
 #include <linux/mtd/partitions.h>
-#include <linux/regulator/consumer.h>
 #include <linux/pmic_external.h>
 #include <linux/pmic_status.h>
 #include <linux/ipu.h>
@@ -112,10 +111,8 @@ static int mipi_sensor;
 
 extern struct regulator *(*get_cpu_regulator)(void);
 extern void (*put_cpu_regulator)(void);
-extern int (*set_cpu_voltage)(u32 volt);
-extern int mx6_set_cpu_voltage(u32 cpu_volt);
-static struct regulator *cpu_regulator;
-static char *gp_reg_id;
+extern char *gp_reg_id;
+extern void mx6_cpu_regulator_init(void);
 
 static iomux_v3_cfg_t mx6q_sabreauto_pads[] = {
 
@@ -942,7 +939,7 @@ static struct imx_ipuv3_platform_data ipu_data[] = {
 	},
 };
 
-static struct platform_pwm_backlight_data mx6_arm2_pwm_backlight_data = {
+static struct platform_pwm_backlight_data mx6_sabreauto_pwm_backlight_data = {
 	.pwm_id = 0,
 	.max_brightness = 255,
 	.dft_brightness = 128,
@@ -995,19 +992,19 @@ static struct mipi_csi2_platform_data mipi_csi2_pdata = {
 	.pixel_clk = "emi_clk",
 };
 
-static void arm2_suspend_enter(void)
+static void sabreauto_suspend_enter(void)
 {
 	/* suspend preparation */
 }
 
-static void arm2_suspend_exit(void)
+static void sabreauto_suspend_exit(void)
 {
 	/* resmue resore */
 }
 static const struct pm_platform_data mx6q_sabreauto_pm_data __initconst = {
 	.name = "imx_pm",
-	.suspend_enter = arm2_suspend_enter,
-	.suspend_exit = arm2_suspend_exit,
+	.suspend_enter = sabreauto_suspend_enter,
+	.suspend_exit = sabreauto_suspend_exit,
 };
 
 static struct mxc_audio_platform_data sab_audio_data = {
@@ -1022,131 +1019,131 @@ static struct imx_esai_platform_data sab_esai_pdata = {
 	.flags = IMX_ESAI_NET,
 };
 
-static struct regulator_consumer_supply arm2_vmmc_consumers[] = {
+static struct regulator_consumer_supply sabreauto_vmmc_consumers[] = {
 	REGULATOR_SUPPLY("vmmc", "sdhci-esdhc-imx.1"),
 	REGULATOR_SUPPLY("vmmc", "sdhci-esdhc-imx.2"),
 	REGULATOR_SUPPLY("vmmc", "sdhci-esdhc-imx.3"),
 };
 
-static struct regulator_init_data arm2_vmmc_init = {
-	.num_consumer_supplies = ARRAY_SIZE(arm2_vmmc_consumers),
-	.consumer_supplies = arm2_vmmc_consumers,
+static struct regulator_init_data sabreauto_vmmc_init = {
+	.num_consumer_supplies = ARRAY_SIZE(sabreauto_vmmc_consumers),
+	.consumer_supplies = sabreauto_vmmc_consumers,
 };
 
-static struct fixed_voltage_config arm2_vmmc_reg_config = {
+static struct fixed_voltage_config sabreauto_vmmc_reg_config = {
 	.supply_name		= "vmmc",
 	.microvolts		= 3300000,
 	.gpio			= -1,
-	.init_data		= &arm2_vmmc_init,
+	.init_data		= &sabreauto_vmmc_init,
 };
 
-static struct platform_device arm2_vmmc_reg_devices = {
+static struct platform_device sabreauto_vmmc_reg_devices = {
 	.name	= "reg-fixed-voltage",
 	.id	= 0,
 	.dev	= {
-		.platform_data = &arm2_vmmc_reg_config,
+		.platform_data = &sabreauto_vmmc_reg_config,
 	},
 };
 
 #ifdef CONFIG_SND_SOC_CS42888
 
-static struct regulator_consumer_supply cs42888_arm2_consumer_va = {
+static struct regulator_consumer_supply cs42888_sabreauto_consumer_va = {
 	.supply = "VA",
 	.dev_name = "0-0048",
 };
 
-static struct regulator_consumer_supply cs42888_arm2_consumer_vd = {
+static struct regulator_consumer_supply cs42888_sabreauto_consumer_vd = {
 	.supply = "VD",
 	.dev_name = "0-0048",
 };
 
-static struct regulator_consumer_supply cs42888_arm2_consumer_vls = {
+static struct regulator_consumer_supply cs42888_sabreauto_consumer_vls = {
 	.supply = "VLS",
 	.dev_name = "0-0048",
 };
 
-static struct regulator_consumer_supply cs42888_arm2_consumer_vlc = {
+static struct regulator_consumer_supply cs42888_sabreauto_consumer_vlc = {
 	.supply = "VLC",
 	.dev_name = "0-0048",
 };
 
-static struct regulator_init_data cs42888_arm2_va_reg_initdata = {
+static struct regulator_init_data cs42888_sabreauto_va_reg_initdata = {
 	.num_consumer_supplies = 1,
-	.consumer_supplies = &cs42888_arm2_consumer_va,
+	.consumer_supplies = &cs42888_sabreauto_consumer_va,
 };
 
-static struct regulator_init_data cs42888_arm2_vd_reg_initdata = {
+static struct regulator_init_data cs42888_sabreauto_vd_reg_initdata = {
 	.num_consumer_supplies = 1,
-	.consumer_supplies = &cs42888_arm2_consumer_vd,
+	.consumer_supplies = &cs42888_sabreauto_consumer_vd,
 };
 
-static struct regulator_init_data cs42888_arm2_vls_reg_initdata = {
+static struct regulator_init_data cs42888_sabreauto_vls_reg_initdata = {
 	.num_consumer_supplies = 1,
-	.consumer_supplies = &cs42888_arm2_consumer_vls,
+	.consumer_supplies = &cs42888_sabreauto_consumer_vls,
 };
 
-static struct regulator_init_data cs42888_arm2_vlc_reg_initdata = {
+static struct regulator_init_data cs42888_sabreauto_vlc_reg_initdata = {
 	.num_consumer_supplies = 1,
-	.consumer_supplies = &cs42888_arm2_consumer_vlc,
+	.consumer_supplies = &cs42888_sabreauto_consumer_vlc,
 };
 
-static struct fixed_voltage_config cs42888_arm2_va_reg_config = {
+static struct fixed_voltage_config cs42888_sabreauto_va_reg_config = {
 	.supply_name		= "VA",
 	.microvolts		= 2800000,
 	.gpio			= -1,
-	.init_data		= &cs42888_arm2_va_reg_initdata,
+	.init_data		= &cs42888_sabreauto_va_reg_initdata,
 };
 
-static struct fixed_voltage_config cs42888_arm2_vd_reg_config = {
+static struct fixed_voltage_config cs42888_sabreauto_vd_reg_config = {
 	.supply_name		= "VD",
 	.microvolts		= 2800000,
 	.gpio			= -1,
-	.init_data		= &cs42888_arm2_vd_reg_initdata,
+	.init_data		= &cs42888_sabreauto_vd_reg_initdata,
 };
 
-static struct fixed_voltage_config cs42888_arm2_vls_reg_config = {
+static struct fixed_voltage_config cs42888_sabreauto_vls_reg_config = {
 	.supply_name		= "VLS",
 	.microvolts		= 2800000,
 	.gpio			= -1,
-	.init_data		= &cs42888_arm2_vls_reg_initdata,
+	.init_data		= &cs42888_sabreauto_vls_reg_initdata,
 };
 
-static struct fixed_voltage_config cs42888_arm2_vlc_reg_config = {
+static struct fixed_voltage_config cs42888_sabreauto_vlc_reg_config = {
 	.supply_name		= "VLC",
 	.microvolts		= 2800000,
 	.gpio			= -1,
-	.init_data		= &cs42888_arm2_vlc_reg_initdata,
+	.init_data		= &cs42888_sabreauto_vlc_reg_initdata,
 };
 
-static struct platform_device cs42888_arm2_va_reg_devices = {
+static struct platform_device cs42888_sabreauto_va_reg_devices = {
 	.name	= "reg-fixed-voltage",
 	.id	= 3,
 	.dev	= {
-		.platform_data = &cs42888_arm2_va_reg_config,
+		.platform_data = &cs42888_sabreauto_va_reg_config,
 	},
 };
 
-static struct platform_device cs42888_arm2_vd_reg_devices = {
+static struct platform_device cs42888_sabreauto_vd_reg_devices = {
 	.name	= "reg-fixed-voltage",
 	.id	= 4,
 	.dev	= {
-		.platform_data = &cs42888_arm2_vd_reg_config,
+		.platform_data = &cs42888_sabreauto_vd_reg_config,
 	},
 };
 
-static struct platform_device cs42888_arm2_vls_reg_devices = {
+static struct platform_device cs42888_sabreauto_vls_reg_devices = {
 	.name	= "reg-fixed-voltage",
 	.id	= 5,
 	.dev	= {
-		.platform_data = &cs42888_arm2_vls_reg_config,
+		.platform_data = &cs42888_sabreauto_vls_reg_config,
 	},
 };
 
-static struct platform_device cs42888_arm2_vlc_reg_devices = {
+static struct platform_device cs42888_sabreauto_vlc_reg_devices = {
 	.name	= "reg-fixed-voltage",
 	.id	= 6,
 	.dev	= {
-		.platform_data = &cs42888_arm2_vlc_reg_config,
+		.platform_data = &cs42888_sabreauto_vlc_reg_config,
 	},
 };
 
@@ -1154,78 +1151,78 @@ static struct platform_device cs42888_arm2_vlc_reg_devices = {
 
 #ifdef CONFIG_SND_SOC_SGTL5000
 
-static struct regulator_consumer_supply sgtl5000_arm2_consumer_vdda = {
+static struct regulator_consumer_supply sgtl5000_sabreauto_consumer_vdda = {
 	.supply = "VDDA",
 	.dev_name = "0-000a",
 };
 
-static struct regulator_consumer_supply sgtl5000_arm2_consumer_vddio = {
+static struct regulator_consumer_supply sgtl5000_sabreauto_consumer_vddio = {
 	.supply = "VDDIO",
 	.dev_name = "0-000a",
 };
 
-static struct regulator_consumer_supply sgtl5000_arm2_consumer_vddd = {
+static struct regulator_consumer_supply sgtl5000_sabreauto_consumer_vddd = {
 	.supply = "VDDD",
 	.dev_name = "0-000a",
 };
 
-static struct regulator_init_data sgtl5000_arm2_vdda_reg_initdata = {
+static struct regulator_init_data sgtl5000_sabreauto_vdda_reg_initdata = {
 	.num_consumer_supplies = 1,
-	.consumer_supplies = &sgtl5000_arm2_consumer_vdda,
+	.consumer_supplies = &sgtl5000_sabreauto_consumer_vdda,
 };
 
-static struct regulator_init_data sgtl5000_arm2_vddio_reg_initdata = {
+static struct regulator_init_data sgtl5000_sabreauto_vddio_reg_initdata = {
 	.num_consumer_supplies = 1,
-	.consumer_supplies = &sgtl5000_arm2_consumer_vddio,
+	.consumer_supplies = &sgtl5000_sabreauto_consumer_vddio,
 };
 
-static struct regulator_init_data sgtl5000_arm2_vddd_reg_initdata = {
+static struct regulator_init_data sgtl5000_sabreauto_vddd_reg_initdata = {
 	.num_consumer_supplies = 1,
-	.consumer_supplies = &sgtl5000_arm2_consumer_vddd,
+	.consumer_supplies = &sgtl5000_sabreauto_consumer_vddd,
 };
 
-static struct fixed_voltage_config sgtl5000_arm2_vdda_reg_config = {
+static struct fixed_voltage_config sgtl5000_sabreauto_vdda_reg_config = {
 	.supply_name		= "VDDA",
 	.microvolts		= 1800000,
 	.gpio			= -1,
-	.init_data		= &sgtl5000_arm2_vdda_reg_initdata,
+	.init_data		= &sgtl5000_sabreauto_vdda_reg_initdata,
 };
 
-static struct fixed_voltage_config sgtl5000_arm2_vddio_reg_config = {
+static struct fixed_voltage_config sgtl5000_sabreauto_vddio_reg_config = {
 	.supply_name		= "VDDIO",
 	.microvolts		= 3300000,
 	.gpio			= -1,
-	.init_data		= &sgtl5000_arm2_vddio_reg_initdata,
+	.init_data		= &sgtl5000_sabreauto_vddio_reg_initdata,
 };
 
-static struct fixed_voltage_config sgtl5000_arm2_vddd_reg_config = {
+static struct fixed_voltage_config sgtl5000_sabreauto_vddd_reg_config = {
 	.supply_name		= "VDDD",
 	.microvolts		= 0,
 	.gpio			= -1,
-	.init_data		= &sgtl5000_arm2_vddd_reg_initdata,
+	.init_data		= &sgtl5000_sabreauto_vddd_reg_initdata,
 };
 
-static struct platform_device sgtl5000_arm2_vdda_reg_devices = {
+static struct platform_device sgtl5000_sabreauto_vdda_reg_devices = {
 	.name	= "reg-fixed-voltage",
 	.id	= 7,
 	.dev	= {
-		.platform_data = &sgtl5000_arm2_vdda_reg_config,
+		.platform_data = &sgtl5000_sabreauto_vdda_reg_config,
 	},
 };
 
-static struct platform_device sgtl5000_arm2_vddio_reg_devices = {
+static struct platform_device sgtl5000_sabreauto_vddio_reg_devices = {
 	.name	= "reg-fixed-voltage",
 	.id	= 8,
 	.dev	= {
-		.platform_data = &sgtl5000_arm2_vddio_reg_config,
+		.platform_data = &sgtl5000_sabreauto_vddio_reg_config,
 	},
 };
 
-static struct platform_device sgtl5000_arm2_vddd_reg_devices = {
+static struct platform_device sgtl5000_sabreauto_vddd_reg_devices = {
 	.name	= "reg-fixed-voltage",
 	.id	= 9,
 	.dev	= {
-		.platform_data = &sgtl5000_arm2_vddd_reg_config,
+		.platform_data = &sgtl5000_sabreauto_vddd_reg_config,
 	},
 };
 
@@ -1249,16 +1246,16 @@ static int __init imx6q_init_audio(void)
 	clk_set_rate(esai_clk, 101647058);
 
 #ifdef CONFIG_SND_SOC_SGTL5000
-	platform_device_register(&sgtl5000_arm2_vdda_reg_devices);
-	platform_device_register(&sgtl5000_arm2_vddio_reg_devices);
-	platform_device_register(&sgtl5000_arm2_vddd_reg_devices);
+	platform_device_register(&sgtl5000_sabreauto_vdda_reg_devices);
+	platform_device_register(&sgtl5000_sabreauto_vddio_reg_devices);
+	platform_device_register(&sgtl5000_sabreauto_vddd_reg_devices);
 #endif
 
 #ifdef CONFIG_SND_SOC_CS42888
-	platform_device_register(&cs42888_arm2_va_reg_devices);
-	platform_device_register(&cs42888_arm2_vd_reg_devices);
-	platform_device_register(&cs42888_arm2_vls_reg_devices);
-	platform_device_register(&cs42888_arm2_vlc_reg_devices);
+	platform_device_register(&cs42888_sabreauto_va_reg_devices);
+	platform_device_register(&cs42888_sabreauto_vd_reg_devices);
+	platform_device_register(&cs42888_sabreauto_vls_reg_devices);
+	platform_device_register(&cs42888_sabreauto_vlc_reg_devices);
 #endif
 	return 0;
 }
@@ -1271,7 +1268,7 @@ static int __init early_use_esai_record(char *p)
 
 early_param("esai_record", early_use_esai_record);
 
-static struct mxc_dvfs_platform_data arm2_dvfscore_data = {
+static struct mxc_dvfs_platform_data sabreauto_dvfscore_data = {
 	.reg_id = "cpu_vddgp",
 	.clk1_id = "cpu_clk",
 	.clk2_id = "gpc_dvfs_clk",
@@ -1294,23 +1291,9 @@ static struct mxc_dvfs_platform_data arm2_dvfscore_data = {
 	.delay_time = 80,
 };
 
-static int mx6_arm2_set_cpu_voltage(u32 cpu_volt)
-{
-	int ret = -EINVAL;
-
-	if (cpu_regulator == NULL)
-		cpu_regulator = regulator_get(NULL, gp_reg_id);
-
-	if (!IS_ERR(cpu_regulator))
-		ret = regulator_set_voltage(cpu_regulator,
-						    cpu_volt, cpu_volt);
-	return ret;
-}
-
 static void __init fixup_mxc_board(struct machine_desc *desc, struct tag *tags,
 				   char **cmdline, struct meminfo *mi)
 {
-	set_cpu_voltage = mx6_arm2_set_cpu_voltage;
 }
 
 static int __init early_enable_mipi_sensor(char *p)
@@ -1406,7 +1389,7 @@ static void __init mx6_board_init(void)
 			mx6q_sabreauto_mipi_sensor_pads,
 			ARRAY_SIZE(mx6q_sabreauto_mipi_sensor_pads));
 
-	gp_reg_id = arm2_dvfscore_data.reg_id;
+	gp_reg_id = sabreauto_dvfscore_data.reg_id;
 	mx6q_sabreauto_init_uart();
 	imx6q_add_mipi_csi2(&mipi_csi2_pdata);
 	imx6q_add_mxc_hdmi_core(&hdmi_core_data);
@@ -1456,7 +1439,8 @@ static void __init mx6_board_init(void)
 	imx6q_add_ahci(0, &mx6q_sabreauto_sata_data);
 	imx6q_add_vpu();
 	imx6q_init_audio();
-	platform_device_register(&arm2_vmmc_reg_devices);
+	platform_device_register(&sabreauto_vmmc_reg_devices);
+	mx6_cpu_regulator_init();
 	imx_asrc_data.asrc_core_clk = clk_get(NULL, "asrc_clk");
 	imx_asrc_data.asrc_audio_clk = clk_get(NULL, "asrc_serial_clk");
 	imx6q_add_asrc(&imx_asrc_data);
@@ -1487,10 +1471,10 @@ static void __init mx6_board_init(void)
 	imx6q_add_dma();
 	imx6q_add_gpmi(&mx6q_gpmi_nfc_platform_data);
 
-	imx6q_add_dvfs_core(&arm2_dvfscore_data);
+	imx6q_add_dvfs_core(&sabreauto_dvfscore_data);
 
 	imx6q_add_mxc_pwm(0);
-	imx6q_add_mxc_pwm_backlight(0, &mx6_arm2_pwm_backlight_data);
+	imx6q_add_mxc_pwm_backlight(0, &mx6_sabreauto_pwm_backlight_data);
 
 	if (spdif_en) {
 		mxc_spdif_data.spdif_core_clk = clk_get_sys("mxc_spdif.0",
diff --git a/arch/arm/mach-mx6/board-mx6q_sabrelite.c b/arch/arm/mach-mx6/board-mx6q_sabrelite.c
index cf08ac324b5a..ef1320b30ea6 100755
--- a/arch/arm/mach-mx6/board-mx6q_sabrelite.c
+++ b/arch/arm/mach-mx6/board-mx6q_sabrelite.c
@@ -59,6 +59,7 @@
 #include <mach/iomux-mx6q.h>
 #include <mach/imx-uart.h>
 #include <mach/viv_gpu.h>
+#include <mach/ahci_sata.h>
 #include <mach/ipu-v3.h>
 #include <mach/mxc_hdmi.h>
 #include <mach/mxc_asrc.h>
@@ -74,6 +75,7 @@
 #include "devices-imx6q.h"
 #include "crm_regs.h"
 #include "cpu_op-mx6.h"
+
 #define MX6Q_SABRELITE_SD3_CD		IMX_GPIO_NR(7, 0)
 #define MX6Q_SABRELITE_SD3_WP		IMX_GPIO_NR(7, 1)
 #define MX6Q_SABRELITE_SD4_CD		IMX_GPIO_NR(2, 6)
@@ -96,13 +98,13 @@
 		PAD_CTL_DSE_40ohm | PAD_CTL_HYS)
 
 void __init early_console_setup(unsigned long base, struct clk *clk);
+static struct clk *sata_clk;
+
+extern char *gp_reg_id;
 
 extern struct regulator *(*get_cpu_regulator)(void);
 extern void (*put_cpu_regulator)(void);
-extern int (*set_cpu_voltage)(u32 volt);
-extern int mx6_set_cpu_voltage(u32 cpu_volt);
-static struct regulator *cpu_regulator;
-static char *gp_reg_id;
+extern void mx6_cpu_regulator_init(void);
 
 static iomux_v3_cfg_t mx6q_sabrelite_pads[] = {
 	/* AUDMUX */
@@ -591,6 +593,92 @@ static void __init imx6q_sabrelite_init_usb(void)
 	mx6_usb_h1_init();
 }
 
+/* HW Initialization, if return 0, initialization is successful. */
+static int mx6q_sabrelite_sata_init(struct device *dev, void __iomem *addr)
+{
+	u32 tmpdata;
+	int ret = 0, iterations = 20;
+	struct clk *clk;
+
+	sata_clk = clk_get(dev, "imx_sata_clk");
+	if (IS_ERR(sata_clk)) {
+		dev_err(dev, "no sata clock.\n");
+		return PTR_ERR(sata_clk);
+	}
+	ret = clk_enable(sata_clk);
+	if (ret) {
+		dev_err(dev, "can't enable sata clock.\n");
+		goto put_sata_clk;
+	}
+
+	/* Set PHY Paremeters, two steps to configure the GPR13,
+	 * one write for rest of parameters, mask of first write is 0x07FFFFFD,
+	 * and the other one write for setting the mpll_clk_off_b
+	 *.rx_eq_val_0(iomuxc_gpr13[26:24]),
+	 *.los_lvl(iomuxc_gpr13[23:19]),
+	 *.rx_dpll_mode_0(iomuxc_gpr13[18:16]),
+	 *.sata_speed(iomuxc_gpr13[15]),
+	 *.mpll_ss_en(iomuxc_gpr13[14]),
+	 *.tx_atten_0(iomuxc_gpr13[13:11]),
+	 *.tx_boost_0(iomuxc_gpr13[10:7]),
+	 *.tx_lvl(iomuxc_gpr13[6:2]),
+	 *.mpll_ck_off(iomuxc_gpr13[1]),
+	 *.tx_edgerate_0(iomuxc_gpr13[0]),
+	 */
+	tmpdata = readl(IOMUXC_GPR13);
+	writel(((tmpdata & ~0x07FFFFFD) | 0x0593A044), IOMUXC_GPR13);
+
+	/* enable SATA_PHY PLL */
+	tmpdata = readl(IOMUXC_GPR13);
+	writel(((tmpdata & ~0x2) | 0x2), IOMUXC_GPR13);
+
+	/* Get the AHB clock rate, and configure the TIMER1MS reg later */
+	clk = clk_get(NULL, "ahb");
+	if (IS_ERR(clk)) {
+		dev_err(dev, "no ahb clock.\n");
+		ret = PTR_ERR(clk);
+		goto release_sata_clk;
+	}
+	tmpdata = clk_get_rate(clk) / 1000;
+	clk_put(clk);
+
+	sata_init(addr, tmpdata);
+
+	/* Release resources when there is no device on the port */
+	do {
+		if ((readl(addr + PORT_SATA_SR) & 0xF) == 0)
+			msleep(25);
+		else
+			break;
+
+		if (iterations == 0) {
+			dev_info(dev, "NO sata disk.\n");
+			ret = -ENODEV;
+			goto release_sata_clk;
+		}
+	} while (iterations-- > 0);
+
+	return ret;
+
+release_sata_clk:
+	clk_disable(sata_clk);
+put_sata_clk:
+	clk_put(sata_clk);
+
+	return ret;
+}
+
+static void mx6q_sabrelite_sata_exit(struct device *dev)
+{
+	clk_disable(sata_clk);
+	clk_put(sata_clk);
+}
+
+static struct ahci_platform_data mx6q_sabrelite_sata_data = {
+	.init = mx6q_sabrelite_sata_init,
+	.exit = mx6q_sabrelite_sata_exit,
+};
+
 static struct gpio mx6q_sabrelite_flexcan_gpios[] = {
 	{ MX6Q_SABRELITE_CAN1_EN, GPIOF_OUT_INIT_LOW, "flexcan1-en" },
 	{ MX6Q_SABRELITE_CAN1_STBY, GPIOF_OUT_INIT_LOW, "flexcan1-stby" },
@@ -917,19 +1005,6 @@ static struct mxc_dvfs_platform_data sabrelite_dvfscore_data = {
 	.delay_time = 80,
 };
 
-static int mx6_sabre_set_cpu_voltage(u32 cpu_volt)
-{
-	int ret = -EINVAL;
-
-	if (cpu_regulator == NULL)
-		cpu_regulator = regulator_get(NULL, gp_reg_id);
-
-	if (!IS_ERR(cpu_regulator))
-		ret = regulator_set_voltage(cpu_regulator,
-						    cpu_volt, cpu_volt);
-	return ret;
-}
-
 static void __init fixup_mxc_board(struct machine_desc *desc, struct tag *tags,
 				   char **cmdline, struct meminfo *mi)
 {
@@ -952,8 +1027,6 @@ static void __init fixup_mxc_board(struct machine_desc *desc, struct tag *tags,
 			break;
 		}
 	}
-
-	set_cpu_voltage = mx6_sabre_set_cpu_voltage;
 }
 
 /*!
@@ -1006,6 +1079,7 @@ static void __init mx6_sabrelite_board_init(void)
 	imx6q_add_sdhci_usdhc_imx(2, &mx6q_sabrelite_sd3_data);
 	imx_add_viv_gpu(&imx6_gpu_data, &imx6q_gpu_pdata);
 	imx6q_sabrelite_init_usb();
+	imx6q_add_ahci(0, &mx6q_sabrelite_sata_data);
 	imx6q_add_vpu();
 	imx6q_init_audio();
 	platform_device_register(&sabrelite_vmmc_reg_devices);
@@ -1028,6 +1102,7 @@ static void __init mx6_sabrelite_board_init(void)
 	imx6q_add_dma();
 
 	imx6q_add_dvfs_core(&sabrelite_dvfscore_data);
+	mx6_cpu_regulator_init();
 
 	mxc_register_device(&mxc_android_pmem_device, &android_pmem_data);
 	mxc_register_device(&mxc_android_pmem_gpu_device,
diff --git a/arch/arm/mach-mx6/clock.c b/arch/arm/mach-mx6/clock.c
index 12a8d3583921..cefdd850ee5f 100644
--- a/arch/arm/mach-mx6/clock.c
+++ b/arch/arm/mach-mx6/clock.c
@@ -23,6 +23,7 @@
 #include <linux/clk.h>
 #include <linux/io.h>
 #include <linux/clkdev.h>
+#include <linux/regulator/consumer.h>
 #include <asm/div64.h>
 #include <mach/hardware.h>
 #include <mach/common.h>
@@ -39,9 +40,8 @@
 
 extern u32 arm_max_freq;
 extern int mxc_jtag_enabled;
+extern struct regulator *cpu_regulator;
 extern struct cpu_op *(*get_cpu_op)(int *op);
-extern int mx6_set_cpu_voltage(u32 cpu_volt);
-
 extern int lp_high_freq;
 extern int lp_med_freq;
 
@@ -4794,7 +4794,6 @@ static struct clk_lookup lookups[] = {
 	_REGISTER_CLOCK("mxs-perfmon.2", "perfmon", perfmon2_clk),
 };
 
-
 static void clk_tree_init(void)
 
 {
@@ -4886,9 +4885,14 @@ int __init mx6_clocks_init(unsigned long ckil, unsigned long osc,
 	clk_set_parent(&asrc_clk[1], &pll3_sw_clk);
 	clk_set_rate(&asrc_clk[1], 7500000);
 
-	/* set the NAND to 11MHz. Too fast will cause dma timeout. */
+	/* set the GPMI clock to : 11MHz */
 	clk_set_rate(&enfc_clk, enfc_clk.round_rate(&enfc_clk, 11000000));
 
+#ifdef CONFIG_MTD_NAND_GPMI_NFC
+	/* set the DMA clock */
+	clk_set_rate(&usdhc3_clk, usdhc3_clk.round_rate(&usdhc3_clk, 11000000));
+#endif
+
 	mx6_cpu_op_init();
 	cpu_op_tbl = get_cpu_op(&cpu_op_nr);
 
@@ -4931,10 +4935,6 @@ int __init mx6_clocks_init(unsigned long ckil, unsigned long osc,
 	base = ioremap(GPT_BASE_ADDR, SZ_4K);
 	mxc_timer_init(&gpt_clk[0], base, MXC_INT_GPT);
 
-	/* Set the core to max frequency requested. */
-	mx6_set_cpu_voltage(cpu_op_tbl[0].cpu_voltage);
-	clk_set_rate(&cpu_clk, cpu_op_tbl[0].pll_rate);
-
 	lp_high_freq = 0;
 	lp_med_freq = 0;
 
diff --git a/arch/arm/mach-mx6/cpu.c b/arch/arm/mach-mx6/cpu.c
index e392a3cbef10..c9690ed7814a 100644
--- a/arch/arm/mach-mx6/cpu.c
+++ b/arch/arm/mach-mx6/cpu.c
@@ -44,21 +44,6 @@ u32 arm_max_freq = CPU_AT_1GHz;
 void __iomem *gpc_base;
 void __iomem *ccm_base;
 
-int mx6_set_cpu_voltage(u32 cpu_volt)
-{
-	u32 reg, val;
-
-	val = (cpu_volt - 725000) / 25000;
-
-	reg = __raw_readl(ANADIG_REG_CORE);
-	reg &= ~(ANADIG_REG_TARGET_MASK << ANADIG_REG0_CORE_TARGET_OFFSET);
-	reg |= ((val + 1) << ANADIG_REG0_CORE_TARGET_OFFSET);
-
-	__raw_writel(reg, ANADIG_REG_CORE);
-
-	return 0;
-}
-
 static int __init post_cpu_init(void)
 {
 	unsigned int reg;
diff --git a/arch/arm/mach-mx6/cpu_regulator-mx6.c b/arch/arm/mach-mx6/cpu_regulator-mx6.c
new file mode 100644
index 000000000000..35f86a9cd473
--- /dev/null
+++ b/arch/arm/mach-mx6/cpu_regulator-mx6.c
@@ -0,0 +1,53 @@
+/*
+ * Copyright (C) 2011 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/types.h>
+#include <linux/kernel.h>
+#include <linux/err.h>
+#include <linux/regulator/consumer.h>
+#include <linux/clk.h>
+
+#include <asm/cpu.h>
+
+#include <mach/clock.h>
+#include <mach/hardware.h>
+
+struct regulator *cpu_regulator;
+char *gp_reg_id;
+static struct clk *cpu_clk;
+static int cpu_op_nr;
+static struct cpu_op *cpu_op_tbl;
+extern struct cpu_op *(*get_cpu_op)(int *op);
+
+
+void mx6_cpu_regulator_init(void)
+{
+	cpu_regulator = regulator_get(NULL, gp_reg_id);
+	if (IS_ERR(cpu_regulator))
+		printk(KERN_ERR "%s: failed to get cpu regulator\n", __func__);
+	else {
+		cpu_clk = clk_get(NULL, "cpu_clk");
+		if (IS_ERR(cpu_clk)) {
+			printk(KERN_ERR "%s: failed to get cpu clock\n",
+			       __func__);
+		} else {
+			cpu_op_tbl = get_cpu_op(&cpu_op_nr);
+			/* Set the core to max frequency requested. */
+			regulator_set_voltage(cpu_regulator,
+					      cpu_op_tbl[0].cpu_voltage,
+					      cpu_op_tbl[0].cpu_voltage);
+			clk_set_rate(cpu_clk, cpu_op_tbl[0].pll_rate);
+		}
+	}
+}
+
diff --git a/arch/arm/mach-mx6/usb_dr.c b/arch/arm/mach-mx6/usb_dr.c
index 957b021e0a73..de84a4ded23b 100644
--- a/arch/arm/mach-mx6/usb_dr.c
+++ b/arch/arm/mach-mx6/usb_dr.c
@@ -365,19 +365,6 @@ static void host_wakeup_handler(struct fsl_usb2_platform_data *pdata)
 
 #ifdef CONFIG_USB_GADGET_ARC
 /* Beginning of device related operation for DR port */
-static void _gadget_discharge_dp(bool enable)
-{
-	void __iomem *phy_reg = MX6_IO_ADDRESS(USB_PHY0_BASE_ADDR);
-	if (enable) {
-		__raw_writel(BF_USBPHY_DEBUG_ENHSTPULLDOWN(0x2), phy_reg + HW_USBPHY_DEBUG_SET);
-		__raw_writel(BF_USBPHY_DEBUG_HSTPULLDOWN(0x2), phy_reg + HW_USBPHY_DEBUG_SET);
-	} else {
-		__raw_writel(BF_USBPHY_DEBUG_ENHSTPULLDOWN(0x2), phy_reg + HW_USBPHY_DEBUG_CLR);
-		__raw_writel(BF_USBPHY_DEBUG_HSTPULLDOWN(0x2), phy_reg + HW_USBPHY_DEBUG_CLR);
-	}
-
-}
-
 static void _device_phy_lowpower_suspend(struct fsl_usb2_platform_data *pdata, bool enable)
 {
 	__phy_lowpower_suspend(pdata, enable, ENABLED_BY_DEVICE);
@@ -463,7 +450,6 @@ void __init mx6_usb_dr_init(void)
 	dr_utmi_config.wake_up_enable = _device_wakeup_enable;
 	dr_utmi_config.phy_lowpower_suspend = _device_phy_lowpower_suspend;
 	dr_utmi_config.is_wakeup_event = _is_device_wakeup;
-	dr_utmi_config.gadget_discharge_dp = _gadget_discharge_dp;
 	dr_utmi_config.wakeup_pdata = &dr_wakeup_config;
 	dr_utmi_config.wakeup_handler = device_wakeup_handler;
 	pdev = imx6q_add_fsl_usb2_udc(&dr_utmi_config);
diff --git a/arch/arm/plat-mxc/cpu.c b/arch/arm/plat-mxc/cpu.c
index 3b35f55dea84..7566cee26d1d 100644
--- a/arch/arm/plat-mxc/cpu.c
+++ b/arch/arm/plat-mxc/cpu.c
@@ -22,7 +22,6 @@
 unsigned int __mxc_cpu_type;
 EXPORT_SYMBOL(__mxc_cpu_type);
 extern int mxc_early_serial_console_init(unsigned long base, struct clk *clk);
-int (*set_cpu_voltage)(u32 volt);
 void (*set_num_cpu_op)(int num);
 
 void mxc_set_cpu_type(unsigned int type)
diff --git a/arch/arm/plat-mxc/cpufreq.c b/arch/arm/plat-mxc/cpufreq.c
index f64b23129616..15060e737c5d 100644
--- a/arch/arm/plat-mxc/cpufreq.c
+++ b/arch/arm/plat-mxc/cpufreq.c
@@ -32,8 +32,6 @@
 #define CLK32_FREQ	32768
 #define NANOSECOND	(1000 * 1000 * 1000)
 
-int cpufreq_trig_needed;
-
 static int cpu_freq_khz_min;
 static int cpu_freq_khz_max;
 
@@ -44,9 +42,9 @@ static int cpu_op_nr;
 static struct cpu_op *cpu_op_tbl;
 static u32 pre_suspend_rate;
 
+extern struct regulator *cpu_regulator;
 extern int dvfs_core_is_active;
 extern struct cpu_op *(*get_cpu_op)(int *op);
-extern int (*set_cpu_voltage)(u32 cpu_volt);
 
 int set_cpu_freq(int freq)
 {
@@ -68,7 +66,8 @@ int set_cpu_freq(int freq)
 
 	/*Set the voltage for the GP domain. */
 	if (freq > org_cpu_rate) {
-		ret = set_cpu_voltage(gp_volt);
+		ret = regulator_set_voltage(cpu_regulator, gp_volt,
+					    gp_volt);
 		if (ret < 0) {
 			printk(KERN_DEBUG "COULD NOT SET GP VOLTAGE!!!!\n");
 			return ret;
@@ -83,7 +82,8 @@ int set_cpu_freq(int freq)
 	}
 
 	if (freq < org_cpu_rate) {
-		ret = set_cpu_voltage(gp_volt);
+		ret = regulator_set_voltage(cpu_regulator, gp_volt,
+					    gp_volt);
 		if (ret < 0) {
 			printk(KERN_DEBUG "COULD NOT SET GP VOLTAGE!!!!\n");
 			return ret;
@@ -122,12 +122,10 @@ static int mxc_set_target(struct cpufreq_policy *policy,
 	if (policy->cpu > num_cpus)
 		return 0;
 
-#ifdef CONFIG_ARCH_MX5
 	if (dvfs_core_is_active) {
 		printk(KERN_DEBUG"DVFS-CORE is active, cannot change frequency using CPUFREQ\n");
 		return ret;
 	}
-#endif
 
 	cpufreq_frequency_table_target(policy, imx_freq_table,
 			target_freq, relation, &index);
diff --git a/arch/arm/plat-mxc/devices/platform-imx-mipi_dsi.c b/arch/arm/plat-mxc/devices/platform-imx-mipi_dsi.c
index 1c583eec7e0f..136a8fcc73db 100644
--- a/arch/arm/plat-mxc/devices/platform-imx-mipi_dsi.c
+++ b/arch/arm/plat-mxc/devices/platform-imx-mipi_dsi.c
@@ -33,6 +33,7 @@
 	[_id] = imx_mipi_dsi_data_entry_single(soc, _id, _hwid, _size)
 
 #ifdef CONFIG_SOC_IMX6Q
+#define SOC_IOMUXC_BASE_ADDR MX6Q_IOMUXC_BASE_ADDR
 const struct imx_mipi_dsi_data imx6q_mipi_dsi_data __initconst =
 			imx_mipi_dsi_data_entry_single(MX6Q, 0, , SZ_4K);
 #endif
@@ -47,6 +48,10 @@ struct platform_device *__init imx_add_mipi_dsi(
 			.end = data->iobase + data->iosize - 1,
 			.flags = IORESOURCE_MEM,
 		}, {
+			.start = SOC_IOMUXC_BASE_ADDR,
+			.end = SOC_IOMUXC_BASE_ADDR + SZ_4K - 1,
+			.flags = IORESOURCE_MEM,
+		}, {
 			.start = data->irq,
 			.end = data->irq,
 			.flags = IORESOURCE_IRQ,
diff --git a/arch/arm/plat-mxc/devices/platform-imx_ipuv3.c b/arch/arm/plat-mxc/devices/platform-imx_ipuv3.c
index 4c3ebaf327dc..0750265fa3a7 100644
--- a/arch/arm/plat-mxc/devices/platform-imx_ipuv3.c
+++ b/arch/arm/plat-mxc/devices/platform-imx_ipuv3.c
@@ -210,11 +210,15 @@ struct platform_device *__init imx_add_ipuv3_fb(
 		const int id,
 		const struct ipuv3_fb_platform_data *pdata)
 {
-	if (pdata->res_size > 0) {
+	if (pdata->res_size[0] > 0) {
 		struct resource res[] = {
 			{
-				.start = pdata->res_base,
-				.end = pdata->res_base + pdata->res_size - 1,
+				.start = pdata->res_base[0],
+				.end = pdata->res_base[0] + pdata->res_size[0] - 1,
+				.flags = IORESOURCE_MEM,
+			}, {
+				.start = pdata->res_base[1],
+				.end = pdata->res_base[1] + pdata->res_size[1] - 1,
 				.flags = IORESOURCE_MEM,
 			},
 		};
diff --git a/arch/arm/plat-mxc/dvfs_core.c b/arch/arm/plat-mxc/dvfs_core.c
index 006530416edd..6616831aea50 100644
--- a/arch/arm/plat-mxc/dvfs_core.c
+++ b/arch/arm/plat-mxc/dvfs_core.c
@@ -84,6 +84,7 @@
 #define CCM_CDCR_ARM_FREQ_SHIFT_DIVIDER		0x4
 #define CCM_CDHIPR_ARM_PODF_BUSY		0x10000
 
+int cpufreq_trig_needed;
 int dvfs_core_is_active;
 static struct mxc_dvfs_platform_data *dvfs_data;
 static struct device *dvfs_dev;
@@ -98,8 +99,7 @@ static int maxf;
 static int minf;
 
 extern void setup_pll(void);
-extern int cpufreq_trig_needed;
-extern int (*set_cpu_voltage)(u32 cpu_volt);
+extern struct regulator *cpu_regulator;
 
 struct timeval core_prev_intr;
 
@@ -118,7 +118,6 @@ static struct clk *dvfs_clk;
 
 static int cpu_op_nr;
 extern struct cpu_op *(*get_cpu_op)(int *op);
-extern int (*set_cpu_voltage)(u32 cpu_volt);
 
 static inline unsigned long dvfs_cpu_jiffies(unsigned long old, u_int div, u_int mult)
 {
@@ -211,7 +210,8 @@ static int mx5_set_cpu_freq(int op)
 
 		/*Set the voltage for the GP domain. */
 		if (rate > org_cpu_rate) {
-			ret = set_cpu_voltage(gp_volt);
+			ret = regulator_set_voltage(cpu_regulator, gp_volt,
+						    gp_volt);
 			if (ret < 0) {
 				printk(KERN_DEBUG "COULD NOT SET GP VOLTAGE\n");
 				return ret;
@@ -258,7 +258,8 @@ static int mx5_set_cpu_freq(int op)
 		spin_unlock_irqrestore(&mxc_dvfs_core_lock, flags);
 
 		if (rate < org_cpu_rate) {
-			ret = set_cpu_voltage(gp_volt);
+			ret = regulator_set_voltage(cpu_regulator, gp_volt,
+						    gp_volt);
 			if (ret < 0) {
 				printk(KERN_DEBUG
 				       "COULD NOT SET GP VOLTAGE!!!!\n");
@@ -299,7 +300,8 @@ static int mx5_set_cpu_freq(int op)
 		}
 		/* Check if FSVAI indicate freq up */
 		if (podf < arm_podf) {
-			ret = set_cpu_voltage(gp_volt);
+			ret = regulator_set_voltage(cpu_regulator, gp_volt,
+						    gp_volt);
 			if (ret < 0) {
 				printk(KERN_DEBUG
 				       "COULD NOT SET GP VOLTAGE!!!!\n");
@@ -359,7 +361,8 @@ static int mx5_set_cpu_freq(int op)
 		spin_unlock_irqrestore(&mxc_dvfs_core_lock, flags);
 
 		if (vinc == 0) {
-			ret = set_cpu_voltage(gp_volt);
+			ret = regulator_set_voltage(cpu_regulator, gp_volt,
+						    gp_volt);
 			if (ret < 0) {
 				printk(KERN_DEBUG
 				       "COULD NOT SET GP VOLTAGE\n!!!");
@@ -393,7 +396,8 @@ static int mx6_set_cpu_freq(int op)
 
 	if (rate > org_cpu_rate) {
 		/* Increase voltage first. */
-		ret = set_cpu_voltage(gp_volt);
+		ret = regulator_set_voltage(cpu_regulator, gp_volt,
+					    gp_volt);
 		if (ret < 0) {
 			printk(KERN_DEBUG "COULD NOT INCREASE GP VOLTAGE!!!!\n");
 			return ret;
@@ -408,7 +412,8 @@ static int mx6_set_cpu_freq(int op)
 
 	if (rate < org_cpu_rate) {
 		/* Increase voltage first. */
-		ret = set_cpu_voltage(gp_volt);
+		ret = regulator_set_voltage(cpu_regulator, gp_volt,
+					    gp_volt);
 		if (ret < 0) {
 			printk(KERN_DEBUG "COULD NOT INCREASE GP VOLTAGE!!!!\n");
 			return ret;
@@ -485,12 +490,12 @@ static int start_dvfs(void)
 	reg = __raw_readl(dvfs_data->membase + MXC_DVFSCORE_CNTR);
 	/* FSVAIM=0 */
 	reg = (reg & ~MXC_DVFSCNTR_FSVAIM);
+
 	/* Set MAXF, MINF */
-	if (!cpu_is_mx6q()) {
-		reg = (reg & ~(MXC_DVFSCNTR_MAXF_MASK
-					| MXC_DVFSCNTR_MINF_MASK));
-		reg |= 1 << MXC_DVFSCNTR_MAXF_OFFSET;
-	}
+	reg = (reg & ~(MXC_DVFSCNTR_MAXF_MASK
+				| MXC_DVFSCNTR_MINF_MASK));
+	reg |= 1 << MXC_DVFSCNTR_MAXF_OFFSET;
+
 	/* Select ARM domain */
 	reg |= MXC_DVFSCNTR_DVFIS;
 	/* Enable DVFS frequency adjustment interrupt */
@@ -526,7 +531,6 @@ static int start_dvfs(void)
 	spin_unlock_irqrestore(&mxc_dvfs_core_lock, flags);
 
 	printk(KERN_DEBUG "DVFS is started\n");
-
 	return 0;
 }
 
@@ -667,6 +671,11 @@ END:
 			dvfs_cpu_jiffies(old_loops_per_jiffy,
 				curr_cpu/1000, clk_get_rate(cpu_clk) / 1000);
 #endif
+#if defined (CONFIG_CPU_FREQ)
+		/* Fix CPU frequency for CPUFREQ. */
+		for (cpu = 0; cpu < num_online_cpus(); cpu++)
+			cpufreq_get(cpu);
+#endif
 		cpufreq_trig_needed = 0;
 	}
 
@@ -734,6 +743,11 @@ void stop_dvfs(void)
 				curr_cpu/1000, clk_get_rate(cpu_clk) / 1000);
 #endif
 
+#if defined (CONFIG_CPU_FREQ)
+			/* Fix CPU frequency for CPUFREQ. */
+			for (cpu = 0; cpu < num_online_cpus(); cpu++)
+				cpufreq_get(cpu);
+#endif
 		}
 		spin_lock_irqsave(&mxc_dvfs_core_lock, flags);
 
@@ -1066,6 +1080,12 @@ static int __init dvfs_init(void)
 
 	dvfs_core_is_active = 0;
 	printk(KERN_INFO "DVFS driver module loaded\n");
+
+	/* Enable DVFS by default. */
+	if (start_dvfs() != 0)
+		printk(KERN_ERR "Failed to start DVFS\n");
+	printk(KERN_INFO "DVFS driver Enabled\n");
+
 	return 0;
 }
 
@@ -1090,7 +1110,7 @@ static void __exit dvfs_cleanup(void)
 
 }
 
-module_init(dvfs_init);
+late_initcall(dvfs_init);
 module_exit(dvfs_cleanup);
 
 MODULE_AUTHOR("Freescale Semiconductor, Inc.");
diff --git a/arch/arm/plat-mxc/include/mach/ipu-v3.h b/arch/arm/plat-mxc/include/mach/ipu-v3.h
index dd18f36c3ff2..02c699d9e067 100644
--- a/arch/arm/plat-mxc/include/mach/ipu-v3.h
+++ b/arch/arm/plat-mxc/include/mach/ipu-v3.h
@@ -342,6 +342,10 @@ enum ipu_irq_line {
 	IPU_IRQ_BG_ALPHA_SYNC_EOF = 51,
 	IPU_IRQ_BG_ALPHA_ASYNC_EOF = 52,
 
+	IPU_IRQ_BG_SYNC_NFACK = 64 + 23,
+	IPU_IRQ_FG_SYNC_NFACK = 64 + 27,
+	IPU_IRQ_DC_SYNC_NFACK = 64 + 28,
+
 	IPU_IRQ_DP_SF_START = 448 + 2,
 	IPU_IRQ_DP_SF_END = 448 + 3,
 	IPU_IRQ_BG_SF_END = IPU_IRQ_DP_SF_END,
@@ -712,8 +716,8 @@ struct ipuv3_fb_platform_data {
 	bool				int_clk;
 
 	/* reserved mem */
-	resource_size_t 		res_base;
-	resource_size_t 		res_size;
+	resource_size_t 		res_base[2];
+	resource_size_t 		res_size[2];
 };
 
 struct imx_ipuv3_platform_data {
diff --git a/drivers/media/video/mxc/capture/mxc_v4l2_capture.c b/drivers/media/video/mxc/capture/mxc_v4l2_capture.c
index 2d0e63c3af0e..bc119c108106 100644
--- a/drivers/media/video/mxc/capture/mxc_v4l2_capture.c
+++ b/drivers/media/video/mxc/capture/mxc_v4l2_capture.c
@@ -382,6 +382,12 @@ static int mxc_streamon(cam_data *cam)
 			"queued yet\n");
 		return -EINVAL;
 	}
+	if (cam->enc_update_eba &&
+		cam->ready_q.prev == cam->ready_q.next) {
+		pr_err("ERROR: v4l2 capture: mxc_streamon buffer need ping pong "
+			"at least two buffers\n");
+		return -EINVAL;
+	}
 
 	cam->capture_pid = current->pid;
 
diff --git a/drivers/media/video/mxc/capture/ov5642.c b/drivers/media/video/mxc/capture/ov5642.c
index f874635d201c..0a20b6f6d356 100644
--- a/drivers/media/video/mxc/capture/ov5642.c
+++ b/drivers/media/video/mxc/capture/ov5642.c
@@ -1798,9 +1798,6 @@ static struct i2c_driver ov5642_i2c_driver = {
 	.id_table = ov5642_id,
 };
 
-extern void gpio_sensor_active(unsigned int csi_index);
-extern void gpio_sensor_inactive(unsigned int csi);
-
 static s32 ov5642_write_reg(u16 reg, u8 val)
 {
 	u8 au8Buf[3] = {0};
@@ -2160,7 +2157,6 @@ static int ioctl_s_power(struct v4l2_int_device *s, int on)
 	struct sensor *sensor = s->priv;
 
 	if (on && !sensor->on) {
-		gpio_sensor_active(ov5642_data.csi);
 		if (io_regulator)
 			if (regulator_enable(io_regulator) != 0)
 				return -EIO;
@@ -2186,7 +2182,6 @@ static int ioctl_s_power(struct v4l2_int_device *s, int on)
 			regulator_disable(io_regulator);
 		if (gpo_regulator)
 			regulator_disable(gpo_regulator);
-		gpio_sensor_inactive(ov5642_data.csi);
 	}
 
 	sensor->on = on;
@@ -2570,7 +2565,6 @@ static int ioctl_dev_init(struct v4l2_int_device *s)
 	u32 tgt_fps;	/* target frames per secound */
 	enum ov5642_frame_rate frame_rate;
 
-	gpio_sensor_active(ov5642_data.csi);
 	ov5642_data.on = true;
 
 	/* mclk */
@@ -2630,8 +2624,6 @@ err:
  */
 static int ioctl_dev_exit(struct v4l2_int_device *s)
 {
-	gpio_sensor_inactive(ov5642_data.csi);
-
 	return 0;
 }
 
diff --git a/drivers/mtd/nand/Kconfig b/drivers/mtd/nand/Kconfig
index 00043d508a8e..5f78087b4730 100644
--- a/drivers/mtd/nand/Kconfig
+++ b/drivers/mtd/nand/Kconfig
@@ -435,6 +435,8 @@ config MTD_NAND_GPMI_NFC
         depends on MTD_NAND && (SOC_IMX23 || SOC_IMX28 || SOC_IMX50 || SOC_IMX6Q)
 	select MTD_PARTITIONS
 	select MTD_CMDLINE_PARTS
+	select MTC_CHAR
+	select MXS_DMA
         help
 	 Enables NAND Flash support for IMX23 or IMX28.
 	 The GPMI controller is very powerful, with the help of BCH
diff --git a/drivers/mtd/nand/gpmi-nfc/hal-mx50.c b/drivers/mtd/nand/gpmi-nfc/hal-mx50.c
index ee0d318c4893..6545c84007f9 100644
--- a/drivers/mtd/nand/gpmi-nfc/hal-mx50.c
+++ b/drivers/mtd/nand/gpmi-nfc/hal-mx50.c
@@ -339,7 +339,7 @@ static int init(struct gpmi_nfc_data *this)
 	clk_enable(resources->clock);
 
 	/* Reset the GPMI block. */
-	mxs_reset_block(resources->gpmi_regs + HW_GPMI_CTRL0, true);
+	mxs_reset_block(resources->gpmi_regs + HW_GPMI_CTRL0, false);
 
 	/* Choose NAND mode. */
 	__raw_writel(BM_GPMI_CTRL1_GPMI_MODE,
diff --git a/drivers/usb/gadget/arcotg_udc.c b/drivers/usb/gadget/arcotg_udc.c
index 391ba5820527..1d48dd57b80e 100644
--- a/drivers/usb/gadget/arcotg_udc.c
+++ b/drivers/usb/gadget/arcotg_udc.c
@@ -1562,6 +1562,9 @@ static void ch9getstatus(struct fsl_udc *udc, u8 request_type, u16 value,
 		ERR("Can't respond to getstatus request \n");
 		goto stall;
 	}
+	/* Status phase */
+	if (ep0_prime_status(udc, EP_DIR_OUT))
+		ep0stall(udc);
 	return;
 stall:
 	ep0stall(udc);
@@ -1633,6 +1636,12 @@ static void setup_received_irq(struct fsl_udc *udc,
 			 * This will be set when OTG support is added */
 			if (setup->wValue == USB_DEVICE_TEST_MODE)
 				ptc = setup->wIndex >> 8;
+			else if (setup->wValue == USB_DEVICE_REMOTE_WAKEUP) {
+				if (setup->bRequest == USB_REQ_SET_FEATURE)
+					udc->remote_wakeup = 1;
+				else
+					udc->remote_wakeup = 0;
+			}
 			else if (gadget_is_otg(&udc->gadget)) {
 				if (setup->bRequest ==
 				    USB_DEVICE_B_HNP_ENABLE)
@@ -2009,20 +2018,10 @@ static void port_change_irq(struct fsl_udc *udc)
 /* Process suspend interrupt */
 static void suspend_irq(struct fsl_udc *udc)
 {
-	u32 otgsc = 0;
-
 	pr_debug("%s begins\n", __func__);
 
 	udc->resume_state = udc->usb_state;
 	udc->usb_state = USB_STATE_SUSPENDED;
-
-	/* Set discharge vbus */
-	otgsc = fsl_readl(&dr_regs->otgsc);
-	otgsc &= ~(OTGSC_INTSTS_MASK);
-	otgsc |= OTGSC_CTRL_VBUS_DISCHARGE;
-	fsl_writel(otgsc, &dr_regs->otgsc);
-
-
 	/* report suspend to the driver, serial.c does not support this */
 	if (udc->driver->suspend)
 		udc->driver->suspend(&udc->gadget);
@@ -2107,7 +2106,7 @@ static void gadget_wait_line_to_se0(void)
 					" your hardware design!\n");
 			break;
 		}
-		msleep(10);
+		msleep(1);
 	}
 }
 
@@ -2144,10 +2143,6 @@ static void fsl_gadget_disconnect_event(struct work_struct *work)
 	u32 tmp;
 
 	pdata = udc->pdata;
-
-	/* enable pulldown dp */
-	if (pdata->gadget_discharge_dp)
-		pdata->gadget_discharge_dp(true);
 	/*
 	 * Some boards are very slow change line state from J to SE0 for DP,
 	 * So, we need to discharge DP, otherwise there is a wakeup interrupt
@@ -2155,10 +2150,6 @@ static void fsl_gadget_disconnect_event(struct work_struct *work)
 	 */
 	gadget_wait_line_to_se0();
 
-	/* Disable pulldown dp */
-	if (pdata->gadget_discharge_dp)
-		pdata->gadget_discharge_dp(false);
-
 	/*
 	 * Wait class drivers finish, an well-behaviour class driver should
 	 * call ep_disable when it is notified to be disconnected.
@@ -2216,6 +2207,7 @@ bool try_wake_up_udc(struct fsl_udc *udc)
 			fsl_writel(tmp | USB_CMD_RUN_STOP, &dr_regs->usbcmd);
 			printk(KERN_DEBUG "%s: udc out low power mode\n", __func__);
 		} else {
+			printk(KERN_INFO "USB device disconnected\n");
 			fsl_writel(tmp & ~USB_CMD_RUN_STOP, &dr_regs->usbcmd);
 			/* here we need disable B_SESSION_IRQ, after
 			 * schedule_work finished, it need to be enabled again.
@@ -2754,6 +2746,109 @@ static int fsl_proc_read(char *page, char **start, off_t off, int count,
 
 #endif				/* CONFIG_USB_GADGET_DEBUG_FILES */
 
+/* sys entries for device control */
+static ssize_t fsl_udc_remote_wakeup_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	return sprintf(buf, "%d\n", udc_controller->remote_wakeup);
+}
+
+static ssize_t fsl_udc_remote_wakeup_store(struct device *dev, struct device_attribute *attr,
+			 const char *buf, size_t count)
+{
+	int value;
+	unsigned long flags;
+	struct fsl_udc *udc = udc_controller;
+
+	spin_lock_irqsave(&udc->lock, flags);
+	if (udc_controller->stopped) {
+		pr_warning(KERN_ERR "Controller has already stopped, Quit.\n"
+				"Please make sure usb is online, and controller"
+				" is not stopped.\n");
+		spin_unlock_irqrestore(&udc->lock, flags);
+		return -ESHUTDOWN;
+	}
+
+	if (sscanf(buf, "%d", &value) != 1) {
+		printk(KERN_ERR "Only numeric 0/1 is valid\n");
+		spin_unlock_irqrestore(&udc->lock, flags);
+		return -EINVAL;
+	}
+	if (value == 0 || value == 1)
+		udc_controller->remote_wakeup = value;
+	else {
+		printk(KERN_ERR "Only numeric 0/1 is valid\n");
+		spin_unlock_irqrestore(&udc->lock, flags);
+		return -EINVAL;
+	}
+
+	spin_unlock_irqrestore(&udc->lock, flags);
+
+	return count;
+}
+
+static DEVICE_ATTR(remote_wakeup, S_IRUGO | S_IWUGO, fsl_udc_remote_wakeup_show,
+		fsl_udc_remote_wakeup_store);
+
+static ssize_t fsl_udc_start_remote_wakeup_store(struct device *dev, struct device_attribute *attr,
+			 const char *buf, size_t count)
+{
+	int value;
+	unsigned long flags;
+	struct fsl_udc *udc = udc_controller;
+
+	spin_lock_irqsave(&udc->lock, flags);
+	if (udc_controller->stopped) {
+		pr_warning(KERN_ERR "Controller has already stopped, Quit.\n"
+				"Please make sure usb is online, and controller"
+				" is not stopped.\n");
+		spin_unlock_irqrestore(&udc->lock, flags);
+		return -ESHUTDOWN;
+	}
+
+	if (sscanf(buf, "%d", &value) != 1) {
+		printk(KERN_ERR "Only numeric 1 is valid\n");
+		spin_unlock_irqrestore(&udc->lock, flags);
+		return -EINVAL;
+	}
+
+	if (value != 1) {
+		printk(KERN_ERR "Only numeric 1 is valid\n");
+		spin_unlock_irqrestore(&udc->lock, flags);
+		return -EINVAL;
+	}
+
+	if (udc_controller->remote_wakeup == 1) {
+		u32 temp;
+		printk(KERN_INFO "Send Resume signal\n");
+		temp = fsl_readl(&dr_regs->portsc1);
+		temp |= PORTSCX_PORT_FORCE_RESUME;
+		fsl_writel(temp, &dr_regs->portsc1);
+	} else
+		printk(KERN_WARNING "udc remote wakeup has not enabled\n");
+
+	spin_unlock_irqrestore(&udc->lock, flags);
+
+	return count;
+}
+
+static DEVICE_ATTR(start_remote_wakeup, S_IWUGO, NULL,
+		fsl_udc_start_remote_wakeup_store);
+
+static struct attribute *fsl_udc_attrs[] = {
+	&dev_attr_remote_wakeup.attr,
+	&dev_attr_start_remote_wakeup.attr,
+	NULL,
+};
+
+static const struct attribute_group fsl_udc_attr_group = {
+	.attrs = fsl_udc_attrs,
+};
+
+static const struct attribute_group *fsl_udc_attr_groups[] = {
+	&fsl_udc_attr_group,
+	NULL,
+};
 /*-------------------------------------------------------------------------*/
 
 /* Release udc structures */
@@ -2984,6 +3079,7 @@ static int __devinit fsl_udc_probe(struct platform_device *pdev)
 	dev_set_name(&udc_controller->gadget.dev, "gadget");
 	udc_controller->gadget.dev.release = fsl_udc_release;
 	udc_controller->gadget.dev.parent = &pdev->dev;
+	udc_controller->gadget.dev.groups = fsl_udc_attr_groups;
 	ret = device_register(&udc_controller->gadget.dev);
 	if (ret < 0)
 		goto err3;
@@ -3056,6 +3152,7 @@ static int __devinit fsl_udc_probe(struct platform_device *pdev)
 	dr_clk_gate(false);
 
 	create_proc_file();
+
 	return 0;
 
 err4:
diff --git a/drivers/video/fbmem.c b/drivers/video/fbmem.c
index c5edf8471c86..e2bf95370e40 100644
--- a/drivers/video/fbmem.c
+++ b/drivers/video/fbmem.c
@@ -949,17 +949,6 @@ fb_set_var(struct fb_info *info, struct fb_var_screeninfo *var)
 			old_var = info->var;
 			info->var = *var;
 
-			/* call pre-mode change */
-			if (flags & FBINFO_MISC_USEREVENT) {
-				struct fb_event event;
-				int evnt = FB_EVENT_PREMODE_CHANGE;
-
-				info->flags &= ~FBINFO_MISC_USEREVENT;
-				event.info = info;
-				event.data = &mode;
-				fb_notifier_call_chain(evnt, &event);
-			}
-
 			if (info->fbops->fb_set_par) {
 				ret = info->fbops->fb_set_par(info);
 
diff --git a/drivers/video/mxc/ldb.c b/drivers/video/mxc/ldb.c
index fe6efe9facb5..7912cdbb1e32 100644
--- a/drivers/video/mxc/ldb.c
+++ b/drivers/video/mxc/ldb.c
@@ -80,7 +80,7 @@
 
 struct ldb_data {
 	struct platform_device *pdev;
-	struct mxc_dispdrv_entry *disp_ldb;
+	struct mxc_dispdrv_handle *disp_ldb;
 	uint32_t *reg;
 	uint32_t *control_reg;
 	uint32_t *gpr3_reg;
@@ -209,6 +209,55 @@ static int find_ldb_setting(struct ldb_data *ldb, struct fb_info *fbi)
 	return -EINVAL;
 }
 
+static int ldb_disp_setup(struct mxc_dispdrv_handle *disp, struct fb_info *fbi)
+{
+	uint32_t reg;
+	uint32_t pixel_clk, rounded_pixel_clk;
+	struct clk *ldb_clk_parent;
+	struct ldb_data *ldb = mxc_dispdrv_getdata(disp);
+	int setting_idx, di;
+
+	setting_idx = find_ldb_setting(ldb, fbi);
+	if (setting_idx < 0)
+		return setting_idx;
+
+	di = ldb->setting[setting_idx].di;
+
+	/* vsync setup */
+	reg = readl(ldb->control_reg);
+	if (fbi->var.sync & FB_SYNC_VERT_HIGH_ACT) {
+		if (di == 0)
+			reg = (reg & ~LDB_DI0_VS_POL_MASK)
+				| LDB_DI0_VS_POL_ACT_HIGH;
+		else
+			reg = (reg & ~LDB_DI1_VS_POL_MASK)
+				| LDB_DI1_VS_POL_ACT_HIGH;
+	} else {
+		if (di == 0)
+			reg = (reg & ~LDB_DI0_VS_POL_MASK)
+				| LDB_DI0_VS_POL_ACT_LOW;
+		else
+			reg = (reg & ~LDB_DI1_VS_POL_MASK)
+				| LDB_DI1_VS_POL_ACT_LOW;
+	}
+	writel(reg, ldb->control_reg);
+
+	/* clk setup */
+	pixel_clk = (PICOS2KHZ(fbi->var.pixclock)) * 1000UL;
+	ldb_clk_parent = clk_get_parent(ldb->ldb_di_clk[di]);
+	if ((ldb->mode == LDB_SPL_DI0) || (ldb->mode == LDB_SPL_DI1))
+		clk_set_rate(ldb_clk_parent, pixel_clk * 7 / 2);
+	else
+		clk_set_rate(ldb_clk_parent, pixel_clk * 7);
+	rounded_pixel_clk = clk_round_rate(ldb->ldb_di_clk[di],
+			pixel_clk);
+	clk_set_rate(ldb->ldb_di_clk[di], rounded_pixel_clk);
+	clk_enable(ldb->ldb_di_clk[di]);
+	ldb->setting[setting_idx].clk_en = true;
+
+	return 0;
+}
+
 int ldb_fb_event(struct notifier_block *nb, unsigned long val, void *v)
 {
 	struct ldb_data *ldb = container_of(nb, struct ldb_data, nb);
@@ -237,45 +286,6 @@ int ldb_fb_event(struct notifier_block *nb, unsigned long val, void *v)
 	}
 
 	switch (val) {
-	case FB_EVENT_PREMODE_CHANGE:
-	{
-		uint32_t reg;
-		uint32_t pixel_clk, rounded_pixel_clk;
-		struct clk *ldb_clk_parent;
-
-		/* vsync setup */
-		reg = readl(ldb->control_reg);
-		if (fbi->var.sync & FB_SYNC_VERT_HIGH_ACT) {
-			if (di == 0)
-				reg = (reg & ~LDB_DI0_VS_POL_MASK)
-					| LDB_DI0_VS_POL_ACT_HIGH;
-			else
-				reg = (reg & ~LDB_DI1_VS_POL_MASK)
-					| LDB_DI1_VS_POL_ACT_HIGH;
-		} else {
-			if (di == 0)
-				reg = (reg & ~LDB_DI0_VS_POL_MASK)
-					| LDB_DI0_VS_POL_ACT_LOW;
-			else
-				reg = (reg & ~LDB_DI1_VS_POL_MASK)
-					| LDB_DI1_VS_POL_ACT_LOW;
-		}
-		writel(reg, ldb->control_reg);
-
-		/* clk setup */
-		pixel_clk = (PICOS2KHZ(fbi->var.pixclock)) * 1000UL;
-		ldb_clk_parent = clk_get_parent(ldb->ldb_di_clk[di]);
-		if ((ldb->mode == LDB_SPL_DI0) || (ldb->mode == LDB_SPL_DI1))
-			clk_set_rate(ldb_clk_parent, pixel_clk * 7 / 2);
-		else
-			clk_set_rate(ldb_clk_parent, pixel_clk * 7);
-		rounded_pixel_clk = clk_round_rate(ldb->ldb_di_clk[di],
-				pixel_clk);
-		clk_set_rate(ldb->ldb_di_clk[di], rounded_pixel_clk);
-		clk_enable(ldb->ldb_di_clk[di]);
-		ldb->setting[setting_idx].clk_en = true;
-		break;
-	}
 	case FB_EVENT_BLANK:
 	{
 		if (*((int *)event->data) == FB_BLANK_UNBLANK) {
@@ -406,11 +416,11 @@ static int ldb_ipu_ldb_route(int ipu, int di, struct ldb_data *ldb)
 	return 0;
 }
 
-static int ldb_disp_init(struct mxc_dispdrv_entry *disp)
+static int ldb_disp_init(struct mxc_dispdrv_handle *disp,
+	struct mxc_dispdrv_setting *setting)
 {
 	int ret = 0, i;
 	struct ldb_data *ldb = mxc_dispdrv_getdata(disp);
-	struct mxc_dispdrv_setting *setting = mxc_dispdrv_getsetting(disp);
 	struct fsl_mxc_ldb_platform_data *plat_data = ldb->pdev->dev.platform_data;
 	struct resource *res;
 	uint32_t base_addr;
@@ -687,7 +697,7 @@ static int ldb_disp_init(struct mxc_dispdrv_entry *disp)
 	return ret;
 }
 
-static void ldb_disp_deinit(struct mxc_dispdrv_entry *disp)
+static void ldb_disp_deinit(struct mxc_dispdrv_handle *disp)
 {
 	struct ldb_data *ldb = mxc_dispdrv_getdata(disp);
 	int i;
@@ -708,6 +718,7 @@ static struct mxc_dispdrv_driver ldb_drv = {
 	.name 	= DISPDRV_LDB,
 	.init 	= ldb_disp_init,
 	.deinit	= ldb_disp_deinit,
+	.setup = ldb_disp_setup,
 };
 
 /*!
@@ -744,6 +755,7 @@ static int ldb_remove(struct platform_device *pdev)
 {
 	struct ldb_data *ldb = dev_get_drvdata(&pdev->dev);
 
+	mxc_dispdrv_puthandle(ldb->disp_ldb);
 	mxc_dispdrv_unregister(ldb->disp_ldb);
 	kfree(ldb);
 	return 0;
diff --git a/drivers/video/mxc/mipi_dsi.c b/drivers/video/mxc/mipi_dsi.c
index 8483041faae2..c236ee7434ec 100644
--- a/drivers/video/mxc/mipi_dsi.c
+++ b/drivers/video/mxc/mipi_dsi.c
@@ -50,6 +50,8 @@
 #define PHY_HS2LP_MAXTIME		(0x40)
 #define	PHY_STOP_WAIT_TIME		(0x20)
 #define	DSI_CLKMGR_CFG_CLK_DIV		(0x107)
+#define	MIPI_MUX_CTRL(v)		(((v) & 0x3) << 4)
+#define	IOMUXC_GPR3_OFFSET		(0xc)
 #define	MIPI_LCD_SLEEP_MODE_DELAY	(120)
 
 static struct mipi_dsi_match_lcd mipi_dsi_lcd_db[] = {
@@ -95,7 +97,6 @@ static const struct _mipi_dsi_phy_pll_clk mipi_dsi_phy_pll_clk_table[] = {
 	{180,  0x24}, /*  160-180MHz	*/
 	{160,  0x04}, /*  150-160MHz	*/
 };
-static int dsi_power_on;
 
 static int valid_mode(int pixel_fmt)
 {
@@ -119,20 +120,10 @@ static inline void mipi_dsi_write_register(struct mipi_dsi_info *mipi_dsi,
 				u32 reg, u32 val)
 {
 	iowrite32(val, mipi_dsi->mmio_base + reg);
-	msleep(1);
 	dev_dbg(&mipi_dsi->pdev->dev, "\t\twrite_reg:0x%02x, val:0x%08x.\n",
 			reg, val);
 }
 
-static void mipi_dsi_dump_registers(struct mipi_dsi_info *mipi_dsi)
-{
-	int i;
-	u32 val;
-
-	for (i = MIPI_DSI_VERSION; i <= MIPI_DSI_PHY_TST_CTRL1; i += 4)
-		mipi_dsi_read_register(mipi_dsi, i, &val);
-}
-
 int mipi_dsi_pkt_write(struct mipi_dsi_info *mipi_dsi,
 				u8 data_type, const u32 *buf, int len)
 {
@@ -200,7 +191,7 @@ int mipi_dsi_pkt_read(struct mipi_dsi_info *mipi_dsi,
 	int		read_len = 0;
 
 	if (!len) {
-		mipi_dbg("%s,%d: error!\n", __func__, __LINE__);
+		mipi_dbg("%s, len = 0 invalid error!\n", __func__);
 		return -EINVAL;
 	}
 
@@ -387,11 +378,6 @@ static void mipi_dsi_enable_controller(struct mipi_dsi_info *mipi_dsi,
 
 		mipi_dsi_dphy_init(mipi_dsi, DSI_PHY_CLK_INIT_COMMAND,
 					mipi_dsi->dphy_pll_config);
-
-		val = DSI_VID_MODE_CFG_EN | DSI_VID_MODE_CFG_EN_BURSTMODE |
-				DSI_VID_MODE_CFG_EN_LP_MODE;
-		mipi_dsi_write_register(mipi_dsi, MIPI_DSI_VID_MODE_CFG, val);
-		dsi_power_on = 1;
 	} else {
 		mipi_dsi_dphy_init(mipi_dsi, DSI_PHY_CLK_INIT_COMMAND,
 					mipi_dsi->dphy_pll_config);
@@ -429,12 +415,44 @@ static irqreturn_t mipi_dsi_irq_handler(int irq, void *data)
 	return IRQ_HANDLED;
 }
 
-static void mipi_dsi_power_on(struct mipi_dsi_info *mipi_dsi)
+static inline void mipi_dsi_set_mode(struct mipi_dsi_info *mipi_dsi,
+	bool cmd_mode)
+{
+	u32	val;
+
+	if (cmd_mode) {
+		mipi_dsi_write_register(mipi_dsi, MIPI_DSI_PWR_UP,
+			DSI_PWRUP_RESET);
+		mipi_dsi_read_register(mipi_dsi, MIPI_DSI_CMD_MODE_CFG, &val);
+		val |= MIPI_DSI_CMD_MODE_CFG_EN_CMD_MODE;
+		mipi_dsi_write_register(mipi_dsi, MIPI_DSI_CMD_MODE_CFG, val);
+		mipi_dsi_write_register(mipi_dsi, MIPI_DSI_VID_MODE_CFG, 0);
+		mipi_dsi_write_register(mipi_dsi, MIPI_DSI_PWR_UP,
+			DSI_PWRUP_POWERUP);
+	} else {
+		mipi_dsi_write_register(mipi_dsi, MIPI_DSI_PWR_UP,
+			DSI_PWRUP_RESET);
+		 /* Disable Command mode when tranfering video data */
+		mipi_dsi_read_register(mipi_dsi, MIPI_DSI_CMD_MODE_CFG, &val);
+		val &= ~MIPI_DSI_CMD_MODE_CFG_EN_CMD_MODE;
+		mipi_dsi_write_register(mipi_dsi, MIPI_DSI_CMD_MODE_CFG, val);
+		val = DSI_VID_MODE_CFG_EN | DSI_VID_MODE_CFG_EN_BURSTMODE |
+				DSI_VID_MODE_CFG_EN_LP_MODE;
+		mipi_dsi_write_register(mipi_dsi, MIPI_DSI_VID_MODE_CFG, val);
+		mipi_dsi_write_register(mipi_dsi, MIPI_DSI_PWR_UP,
+			DSI_PWRUP_POWERUP);
+		mipi_dsi_write_register(mipi_dsi, MIPI_DSI_PHY_IF_CTRL,
+				DSI_PHY_IF_CTRL_TX_REQ_CLK_HS);
+	}
+}
+
+static int mipi_dsi_power_on(struct mxc_dispdrv_handle *disp)
 {
 	int err;
-	u32 val;
+	struct mipi_dsi_info *mipi_dsi = mxc_dispdrv_getdata(disp);
 
-	if (!dsi_power_on) {
+	if (!mipi_dsi->dsi_power_on) {
+		clk_enable(mipi_dsi->dphy_clk);
 		mipi_dsi_enable_controller(mipi_dsi, false);
 		err = mipi_dsi_dcs_cmd(mipi_dsi, MIPI_DCS_EXIT_SLEEP_MODE,
 			NULL, 0);
@@ -443,50 +461,43 @@ static void mipi_dsi_power_on(struct mipi_dsi_info *mipi_dsi)
 				"MIPI DSI DCS Command sleep-in error!\n");
 		}
 		msleep(MIPI_LCD_SLEEP_MODE_DELAY);
+		mipi_dsi_set_mode(mipi_dsi, false);
+		mipi_dsi->dsi_power_on = 1;
 	}
-	dsi_power_on = 1;
-	 /* Disable Command mode when tranfering video data */
-	mipi_dsi_read_register(mipi_dsi, MIPI_DSI_CMD_MODE_CFG, &val);
-	val &= ~MIPI_DSI_CMD_MODE_CFG_EN_CMD_MODE;
-	mipi_dsi_write_register(mipi_dsi, MIPI_DSI_CMD_MODE_CFG, val);
-
-	mipi_dsi_read_register(mipi_dsi, MIPI_DSI_VID_MODE_CFG, &val);
-	val |= DSI_VID_MODE_CFG_EN;
-	mipi_dsi_write_register(mipi_dsi, MIPI_DSI_VID_MODE_CFG, val);
 
-	mipi_dsi_write_register(mipi_dsi, MIPI_DSI_PHY_IF_CTRL,
-				DSI_PHY_IF_CTRL_TX_REQ_CLK_HS);
-
-	mipi_dsi_dump_registers(mipi_dsi);
+	return 0;
 }
 
-static void mipi_dsi_power_off(struct mipi_dsi_info *mipi_dsi)
+void mipi_dsi_power_off(struct mxc_dispdrv_handle *disp)
 {
 	int err;
-	u32	val;
-
-	if (dsi_power_on) {
-		mipi_dsi_read_register(mipi_dsi, MIPI_DSI_CMD_MODE_CFG, &val);
-		val |= MIPI_DSI_CMD_MODE_CFG_EN_CMD_MODE;
-		mipi_dsi_write_register(mipi_dsi, MIPI_DSI_CMD_MODE_CFG, val);
-		mipi_dsi_read_register(mipi_dsi, MIPI_DSI_VID_MODE_CFG, &val);
-		val &= ~DSI_VID_MODE_CFG_EN;
-		mipi_dsi_write_register(mipi_dsi, MIPI_DSI_VID_MODE_CFG, val);
+	struct mipi_dsi_info *mipi_dsi = mxc_dispdrv_getdata(disp);
 
+	if (mipi_dsi->dsi_power_on) {
+		mipi_dsi_set_mode(mipi_dsi, true);
 		err = mipi_dsi_dcs_cmd(mipi_dsi, MIPI_DCS_ENTER_SLEEP_MODE,
 			NULL, 0);
 		if (err) {
 			dev_err(&mipi_dsi->pdev->dev,
 				"MIPI DSI DCS Command display on error!\n");
 		}
+		/* To allow time for the supply voltages
+		 * and clock circuits to stabilize.
+		 */
+		msleep(5);
+		/* video stream timing on */
+		mipi_dsi_set_mode(mipi_dsi, false);
 		msleep(MIPI_LCD_SLEEP_MODE_DELAY);
 
+		mipi_dsi_set_mode(mipi_dsi, true);
 		mipi_dsi_disable_controller(mipi_dsi);
-		dsi_power_on = 0;
+		mipi_dsi->dsi_power_on = 0;
+		clk_disable(mipi_dsi->dphy_clk);
 	}
 }
 
-static int mipi_dsi_lcd_init(struct mipi_dsi_info *mipi_dsi)
+static int mipi_dsi_lcd_init(struct mipi_dsi_info *mipi_dsi,
+	struct mxc_dispdrv_setting *setting)
 {
 	int		err;
 	int		size;
@@ -494,9 +505,7 @@ static int mipi_dsi_lcd_init(struct mipi_dsi_info *mipi_dsi)
 	struct  fb_videomode *mipi_lcd_modedb;
 	struct  fb_videomode mode;
 	struct  device		 *dev = &mipi_dsi->pdev->dev;
-	struct  mxc_dispdrv_setting	  *setting;
 
-	setting = mxc_dispdrv_getsetting(mipi_dsi->disp_mipi);
 	for (i = 0; i < ARRAY_SIZE(mipi_dsi_lcd_db); i++) {
 		if (!strcmp(mipi_dsi->lcd_panel,
 			mipi_dsi_lcd_db[i].lcd_panel)) {
@@ -551,16 +560,6 @@ static int mipi_dsi_lcd_init(struct mipi_dsi_info *mipi_dsi)
 	dev_dbg(dev, "dphy_pll_config:0x%x.\n", mipi_dsi->dphy_pll_config);
 
 	mipi_dsi_enable_controller(mipi_dsi, true);
-
-	err = mipi_dsi->lcd_callback->mipi_lcd_setup(mipi_dsi);
-	if (err < 0) {
-		dev_err(dev, "failed to init mipi lcd.\n");
-		return err;
-	}
-
-	mipi_dsi_write_register(mipi_dsi, MIPI_DSI_PHY_IF_CTRL,
-				DSI_PHY_IF_CTRL_TX_REQ_CLK_HS);
-
 	return 0;
 }
 
@@ -570,16 +569,36 @@ static int mipi_dsi_fb_event(struct notifier_block *nb,
 	struct mipi_dsi_info *mipi_dsi =
 		container_of(nb, struct mipi_dsi_info, nb);
 	struct fb_event *fbevent = data;
+	struct fb_info *fbi = fbevent->info;
+	char *id_di[] = {
+		 "DISP4 BG",
+		 "DISP4 BG - DI1",
+		};
+	char *id;
+	int err;
 
-	switch (event) {
-	case FB_EVENT_FB_REGISTERED:
-		break;
+	id = id_di[mipi_dsi->disp_id];
+	*(id + 4) = mipi_dsi->ipu_id ? '4' : '3';
+	if (strcmp(id_di[mipi_dsi->disp_id], fbi->fix.id))
+		return 0;
 
+	switch (event) {
 	case FB_EVENT_BLANK:
-		if (*((int *)fbevent->data) == FB_BLANK_UNBLANK)
-			mipi_dsi_power_on(mipi_dsi);
-		else
-			mipi_dsi_power_off(mipi_dsi);
+		if (*((int *)fbevent->data) == FB_BLANK_UNBLANK) {
+			if (!mipi_dsi->lcd_inited) {
+				err = mipi_dsi->lcd_callback->mipi_lcd_setup(
+					mipi_dsi);
+				if (err < 0) {
+					dev_err(&mipi_dsi->pdev->dev,
+						"failed to init mipi lcd.");
+					return err;
+				}
+				mipi_dsi_set_mode(mipi_dsi, false);
+				mipi_dsi->dsi_power_on = 1;
+				mipi_dsi->lcd_inited = 1;
+			}
+			mipi_dsi_power_on(mipi_dsi->disp_mipi);
+		}
 		break;
 
 	default:
@@ -589,20 +608,21 @@ static int mipi_dsi_fb_event(struct notifier_block *nb,
 	return 0;
 }
 
-static int mipi_dsi_disp_init(struct mxc_dispdrv_entry *disp)
+static int mipi_dsi_disp_init(struct mxc_dispdrv_handle *disp,
+	struct mxc_dispdrv_setting *setting)
 {
 	int	   err;
+	void __iomem *reg_base;
+	u32    val;
 	char   dphy_clk[] = "mipi_pllref_clk";
 	struct resource *res;
 	struct resource *res_irq;
 	struct device	*dev;
 	struct mipi_dsi_info		  *mipi_dsi;
-	struct mxc_dispdrv_setting	  *setting;
 	struct mipi_dsi_platform_data *pdata;
 
 	mipi_dsi = mxc_dispdrv_getdata(disp);
-	setting = mxc_dispdrv_getsetting(disp);
-	if (IS_ERR(mipi_dsi) || IS_ERR(setting)) {
+	if (IS_ERR(mipi_dsi)) {
 		pr_err("failed to get dispdrv data\n");
 		return -EINVAL;
 	}
@@ -613,6 +633,9 @@ static int mipi_dsi_disp_init(struct mxc_dispdrv_entry *disp)
 		return -EINVAL;
 	}
 
+	if ((1 != pdata->ipu_id) || (1 != pdata->disp_id))
+		dev_warn(dev, "only work bidirection mode on IPU1-DI1\n");
+
 	mipi_dsi->lcd_panel = kstrdup(pdata->lcd_panel, GFP_KERNEL);
 	if (!mipi_dsi->lcd_panel)
 		return -ENOMEM;
@@ -625,14 +648,14 @@ static int mipi_dsi_disp_init(struct mxc_dispdrv_entry *disp)
 
 	res = platform_get_resource(mipi_dsi->pdev, IORESOURCE_MEM, 0);
 	if (!res) {
-		mipi_dbg("%s,%d: error!\n", __func__, __LINE__);
+		dev_err(dev, "get platform resource 0 error\n");
 		return -ENODEV;
 	}
 
 	res = request_mem_region(res->start, resource_size(res),
 				mipi_dsi->pdev->name);
 	if (!res) {
-		mipi_dbg("%s,%d: error!\n", __func__, __LINE__);
+		dev_err(dev, "request mem region error\n");
 		return -EBUSY;
 	}
 	mipi_dsi->mmio_base = ioremap(res->start, resource_size(res));
@@ -642,6 +665,24 @@ static int mipi_dsi_disp_init(struct mxc_dispdrv_entry *disp)
 		goto err_ioremap;
 	}
 
+	res = platform_get_resource(mipi_dsi->pdev, IORESOURCE_MEM, 1);
+	if (!res) {
+		dev_err(dev, "get platform resource 1 error\n");
+		err = -ENODEV;
+		goto err_get_res1;
+	}
+
+	reg_base = ioremap(res->start, resource_size(res));
+	if (!reg_base) {
+		dev_err(dev, "Cannot map iomuxc registers\n");
+		err = -EIO;
+		goto err_ioremap_iomuxc;
+	}
+	val = ioread32(reg_base + IOMUXC_GPR3_OFFSET);
+	val |= MIPI_MUX_CTRL((pdata->ipu_id << 1) | (pdata->disp_id));
+	iowrite32(val, reg_base + IOMUXC_GPR3_OFFSET);
+	iounmap(reg_base);
+
 	res_irq = platform_get_resource(mipi_dsi->pdev, IORESOURCE_IRQ, 0);
 	if (!res_irq) {
 		dev_err(dev, "failed to acquire irq resource\n");
@@ -658,7 +699,7 @@ static int mipi_dsi_disp_init(struct mxc_dispdrv_entry *disp)
 	}
 	err = clk_enable(mipi_dsi->dphy_clk);
 	if (err)
-		mipi_dbg("%s,%d: error!\n", __func__, __LINE__);
+		dev_err(dev, "clk_enable error:%d!\n", err);
 
 	dev_dbg(dev, "got resources: regs %p, irq:%d\n",
 				mipi_dsi->mmio_base, mipi_dsi->irq);
@@ -768,7 +809,7 @@ static int mipi_dsi_disp_init(struct mxc_dispdrv_entry *disp)
 		goto err_req_irq;
 	}
 
-	err = mipi_dsi_lcd_init(mipi_dsi);
+	err = mipi_dsi_lcd_init(mipi_dsi, setting);
 	if (err < 0) {
 		dev_err(dev, "failed to init mipi dsi lcd\n");
 		goto err_dsi_lcd;
@@ -801,6 +842,8 @@ err_ioreg:
 	clk_put(mipi_dsi->dphy_clk);
 err_clk:
 err_get_irq:
+err_get_res1:
+err_ioremap_iomuxc:
 	iounmap(mipi_dsi->mmio_base);
 err_ioremap:
 	release_mem_region(res->start, resource_size(res));
@@ -808,7 +851,7 @@ err_ioremap:
 	return err;
 }
 
-static void mipi_dsi_disp_deinit(struct mxc_dispdrv_entry *disp)
+static void mipi_dsi_disp_deinit(struct mxc_dispdrv_handle *disp)
 {
 	struct mipi_dsi_info    *mipi_dsi;
 	struct resource			*res;
@@ -818,7 +861,7 @@ static void mipi_dsi_disp_deinit(struct mxc_dispdrv_entry *disp)
 
 	disable_irq(mipi_dsi->irq);
 	free_irq(mipi_dsi->irq, mipi_dsi);
-	mipi_dsi_power_off(mipi_dsi);
+	mipi_dsi_power_off(mipi_dsi->disp_mipi);
 	fb_unregister_client(&mipi_dsi->nb);
 	if (mipi_dsi->bl)
 		backlight_device_unregister(mipi_dsi->bl);
@@ -828,7 +871,6 @@ static void mipi_dsi_disp_deinit(struct mxc_dispdrv_entry *disp)
 		regulator_put(mipi_dsi->core_regulator);
 	if (mipi_dsi->io_regulator)
 		regulator_put(mipi_dsi->io_regulator);
-	clk_disable(mipi_dsi->dphy_clk);
 	clk_put(mipi_dsi->dphy_clk);
 	iounmap(mipi_dsi->mmio_base);
 	release_mem_region(res->start, resource_size(res));
@@ -838,6 +880,8 @@ static struct mxc_dispdrv_driver mipi_dsi_drv = {
 	.name	= DISPDRV_MIPI,
 	.init	= mipi_dsi_disp_init,
 	.deinit	= mipi_dsi_disp_deinit,
+	.enable = mipi_dsi_power_on,
+	.disable = mipi_dsi_power_off,
 };
 
 /**
@@ -863,7 +907,8 @@ static int mipi_dsi_probe(struct platform_device *pdev)
 	mipi_dsi->pdev = pdev;
 	mipi_dsi->disp_mipi = mxc_dispdrv_register(&mipi_dsi_drv);
 	if (IS_ERR(mipi_dsi->disp_mipi)) {
-		mipi_dbg("%s,%d: error!\n", __func__, __LINE__);
+		dev_err(&pdev->dev, "mxc_dispdrv_register error:%ld!\n",
+			PTR_ERR(mipi_dsi->disp_mipi));
 		ret = -ENOMEM;
 		goto register_failed;
 	}
@@ -884,7 +929,7 @@ static void mipi_dsi_shutdown(struct platform_device *pdev)
 {
 	struct mipi_dsi_info *mipi_dsi = dev_get_drvdata(&pdev->dev);
 
-	mipi_dsi_power_off(mipi_dsi);
+	mipi_dsi_power_off(mipi_dsi->disp_mipi);
 	if (mipi_dsi->lcd_power)
 		mipi_dsi->lcd_power(false);
 	if (mipi_dsi->backlight_power)
@@ -895,6 +940,7 @@ static int __devexit mipi_dsi_remove(struct platform_device *pdev)
 {
 	struct mipi_dsi_info *mipi_dsi = dev_get_drvdata(&pdev->dev);
 
+	mxc_dispdrv_puthandle(mipi_dsi->disp_mipi);
 	mxc_dispdrv_unregister(mipi_dsi->disp_mipi);
 	kfree(mipi_dsi);
 	dev_set_drvdata(&pdev->dev, NULL);
diff --git a/drivers/video/mxc/mipi_dsi.h b/drivers/video/mxc/mipi_dsi.h
index a70a75559dac..3e9c47d4e9dd 100644
--- a/drivers/video/mxc/mipi_dsi.h
+++ b/drivers/video/mxc/mipi_dsi.h
@@ -61,18 +61,20 @@ struct mipi_dsi_match_lcd {
 
 /* driver private data */
 struct mipi_dsi_info {
+	struct platform_device		*pdev;
+	void __iomem			*mmio_base;
+	int				dsi_power_on;
+	int				lcd_inited;
+	u32				dphy_pll_config;
 	int				ipu_id;
 	int				disp_id;
 	char				*lcd_panel;
 	int				irq;
-	u32				dphy_pll_config;
 	struct clk			*dphy_clk;
-	void __iomem			*mmio_base;
-	struct platform_device		*pdev;
-	struct mxc_dispdrv_entry	*disp_mipi;
-	struct notifier_block		nb;
+	struct mxc_dispdrv_handle	*disp_mipi;
 	struct  fb_videomode		*mode;
 	struct  mipi_lcd_config		*lcd_config;
+	struct notifier_block		nb;
 	/* board related power control */
 	struct backlight_device		*bl;
 	struct regulator		*io_regulator;
diff --git a/drivers/video/mxc/mxc_dispdrv.c b/drivers/video/mxc/mxc_dispdrv.c
index 06b7af944f5b..3a8a8d216eb9 100644
--- a/drivers/video/mxc/mxc_dispdrv.c
+++ b/drivers/video/mxc/mxc_dispdrv.c
@@ -22,8 +22,9 @@
  * Move all dev_suspend() things into fb_notifier for SUSPEND, if there is;
  * Move all dev_resume() things into fb_notifier for RESUME, if there is;
  *
- * ipuv3 fb driver could call mxc_dispdrv_init(setting) before a fb need be added, with fbi param
- * passing by setting, after mxc_dispdrv_init() return, FB driver should get the basic setting
+ * ipuv3 fb driver could call mxc_dispdrv_gethandle(name, setting) before a fb
+ * need be added, with fbi param passing by setting, after
+ * mxc_dispdrv_gethandle() return, FB driver should get the basic setting
  * about fbi info and ipuv3-hw (ipu_id and disp_id).
  *
  * @ingroup Framebuffer
@@ -42,16 +43,14 @@ static LIST_HEAD(dispdrv_list);
 static DEFINE_MUTEX(dispdrv_lock);
 
 struct mxc_dispdrv_entry {
-	const char *name;
-	struct list_head list;
-	int (*init) (struct mxc_dispdrv_entry *);
-	void (*deinit) (struct mxc_dispdrv_entry *);
+	/* Note: drv always the first element */
+	struct mxc_dispdrv_driver *drv;
 	bool active;
-	struct mxc_dispdrv_setting setting;
 	void *priv;
+	struct list_head list;
 };
 
-struct mxc_dispdrv_entry *mxc_dispdrv_register(struct mxc_dispdrv_driver *drv)
+struct mxc_dispdrv_handle *mxc_dispdrv_register(struct mxc_dispdrv_driver *drv)
 {
 	struct mxc_dispdrv_entry *new;
 
@@ -63,23 +62,21 @@ struct mxc_dispdrv_entry *mxc_dispdrv_register(struct mxc_dispdrv_driver *drv)
 		return ERR_PTR(-ENOMEM);
 	}
 
-	new->name = drv->name;
-	new->init = drv->init;
-	new->deinit = drv->deinit;
-
+	new->drv = drv;
 	list_add_tail(&new->list, &dispdrv_list);
+
 	mutex_unlock(&dispdrv_lock);
 
-	return new;
+	return (struct mxc_dispdrv_handle *)new;
 }
 EXPORT_SYMBOL_GPL(mxc_dispdrv_register);
 
-int mxc_dispdrv_unregister(struct mxc_dispdrv_entry *entry)
+int mxc_dispdrv_unregister(struct mxc_dispdrv_handle *handle)
 {
+	struct mxc_dispdrv_entry *entry = (struct mxc_dispdrv_entry *)handle;
+
 	if (entry) {
 		mutex_lock(&dispdrv_lock);
-		if (entry->active && entry->deinit)
-			entry->deinit(entry);
 		list_del(&entry->list);
 		mutex_unlock(&dispdrv_lock);
 		kfree(entry);
@@ -89,41 +86,48 @@ int mxc_dispdrv_unregister(struct mxc_dispdrv_entry *entry)
 }
 EXPORT_SYMBOL_GPL(mxc_dispdrv_unregister);
 
-int mxc_dispdrv_init(char *name, struct mxc_dispdrv_setting *setting)
+struct mxc_dispdrv_handle *mxc_dispdrv_gethandle(char *name,
+	struct mxc_dispdrv_setting *setting)
 {
-	int ret = 0, found = 0;
-	struct mxc_dispdrv_entry *disp;
+	int ret, found = 0;
+	struct mxc_dispdrv_entry *entry;
 
 	mutex_lock(&dispdrv_lock);
-	list_for_each_entry(disp, &dispdrv_list, list) {
-		if (!strcmp(disp->name, name)) {
-			if (disp->init) {
-				memcpy(&disp->setting, setting,
-						sizeof(struct mxc_dispdrv_setting));
-				ret = disp->init(disp);
-				if (ret >= 0) {
-					disp->active = true;
-					/* setting may need fix-up */
-					memcpy(setting, &disp->setting,
-							sizeof(struct mxc_dispdrv_setting));
-					found = 1;
-					break;
-				}
+	list_for_each_entry(entry, &dispdrv_list, list) {
+		if (!strcmp(entry->drv->name, name) && (entry->drv->init)) {
+			ret = entry->drv->init((struct mxc_dispdrv_handle *)
+				entry, setting);
+			if (ret >= 0) {
+				entry->active = true;
+				found = 1;
+				break;
 			}
 		}
 	}
+	mutex_unlock(&dispdrv_lock);
 
-	if (!found)
-		ret = -EINVAL;
+	return found ? (struct mxc_dispdrv_handle *)entry:ERR_PTR(-ENODEV);
+}
+EXPORT_SYMBOL_GPL(mxc_dispdrv_gethandle);
 
+void mxc_dispdrv_puthandle(struct mxc_dispdrv_handle *handle)
+{
+	struct mxc_dispdrv_entry *entry = (struct mxc_dispdrv_entry *)handle;
+
+	mutex_lock(&dispdrv_lock);
+	if (entry && entry->active && entry->drv->deinit) {
+		entry->drv->deinit(handle);
+		entry->active = false;
+	}
 	mutex_unlock(&dispdrv_lock);
 
-	return ret;
 }
-EXPORT_SYMBOL_GPL(mxc_dispdrv_init);
+EXPORT_SYMBOL_GPL(mxc_dispdrv_puthandle);
 
-int mxc_dispdrv_setdata(struct mxc_dispdrv_entry *entry, void *data)
+int mxc_dispdrv_setdata(struct mxc_dispdrv_handle *handle, void *data)
 {
+	struct mxc_dispdrv_entry *entry = (struct mxc_dispdrv_entry *)handle;
+
 	if (entry) {
 		entry->priv = data;
 		return 0;
@@ -132,21 +136,13 @@ int mxc_dispdrv_setdata(struct mxc_dispdrv_entry *entry, void *data)
 }
 EXPORT_SYMBOL_GPL(mxc_dispdrv_setdata);
 
-void *mxc_dispdrv_getdata(struct mxc_dispdrv_entry *entry)
+void *mxc_dispdrv_getdata(struct mxc_dispdrv_handle *handle)
 {
+	struct mxc_dispdrv_entry *entry = (struct mxc_dispdrv_entry *)handle;
+
 	if (entry) {
 		return entry->priv;
 	} else
 		return ERR_PTR(-EINVAL);
 }
 EXPORT_SYMBOL_GPL(mxc_dispdrv_getdata);
-
-struct mxc_dispdrv_setting
-	*mxc_dispdrv_getsetting(struct mxc_dispdrv_entry *entry)
-{
-	if (entry) {
-		return &entry->setting;
-	} else
-		return ERR_PTR(-EINVAL);
-}
-EXPORT_SYMBOL_GPL(mxc_dispdrv_getsetting);
diff --git a/drivers/video/mxc/mxc_dispdrv.h b/drivers/video/mxc/mxc_dispdrv.h
index f5f62a2c3cb3..c6d05fa37799 100644
--- a/drivers/video/mxc/mxc_dispdrv.h
+++ b/drivers/video/mxc/mxc_dispdrv.h
@@ -12,13 +12,10 @@
  */
 #ifndef __MXC_DISPDRV_H__
 #define __MXC_DISPDRV_H__
+#include <linux/fb.h>
 
-struct mxc_dispdrv_entry;
-
-struct mxc_dispdrv_driver {
-	const char *name;
-	int (*init) (struct mxc_dispdrv_entry *);
-	void (*deinit) (struct mxc_dispdrv_entry *);
+struct mxc_dispdrv_handle {
+	struct mxc_dispdrv_driver *drv;
 };
 
 struct mxc_dispdrv_setting {
@@ -33,11 +30,23 @@ struct mxc_dispdrv_setting {
 	int disp_id;
 };
 
-struct mxc_dispdrv_entry *mxc_dispdrv_register(struct mxc_dispdrv_driver *drv);
-int mxc_dispdrv_unregister(struct mxc_dispdrv_entry *entry);
-int mxc_dispdrv_init(char *name, struct mxc_dispdrv_setting *setting);
-int mxc_dispdrv_setdata(struct mxc_dispdrv_entry *entry, void *data);
-void *mxc_dispdrv_getdata(struct mxc_dispdrv_entry *entry);
-struct mxc_dispdrv_setting
-	*mxc_dispdrv_getsetting(struct mxc_dispdrv_entry *entry);
+struct mxc_dispdrv_driver {
+	const char *name;
+	int (*init) (struct mxc_dispdrv_handle *, struct mxc_dispdrv_setting *);
+	void (*deinit) (struct mxc_dispdrv_handle *);
+	/* display driver enable function for extension */
+	int (*enable) (struct mxc_dispdrv_handle *);
+	/* display driver disable function, called at early part of fb_blank */
+	void (*disable) (struct mxc_dispdrv_handle *);
+	/* display driver setup function, called at early part of fb_set_par */
+	int (*setup) (struct mxc_dispdrv_handle *, struct fb_info *fbi);
+};
+
+struct mxc_dispdrv_handle *mxc_dispdrv_register(struct mxc_dispdrv_driver *drv);
+int mxc_dispdrv_unregister(struct mxc_dispdrv_handle *handle);
+struct mxc_dispdrv_handle *mxc_dispdrv_gethandle(char *name,
+	struct mxc_dispdrv_setting *setting);
+void mxc_dispdrv_puthandle(struct mxc_dispdrv_handle *handle);
+int mxc_dispdrv_setdata(struct mxc_dispdrv_handle *handle, void *data);
+void *mxc_dispdrv_getdata(struct mxc_dispdrv_handle *handle);
 #endif
diff --git a/drivers/video/mxc/mxc_dvi.c b/drivers/video/mxc/mxc_dvi.c
index a392cb9708e1..923db4385184 100644
--- a/drivers/video/mxc/mxc_dvi.c
+++ b/drivers/video/mxc/mxc_dvi.c
@@ -53,7 +53,7 @@
 struct mxc_dvi_data {
 	struct i2c_client *client;
 	struct platform_device *pdev;
-	struct mxc_dispdrv_entry *disp_dvi;
+	struct mxc_dispdrv_handle *disp_dvi;
 	struct delayed_work det_work;
 	struct fb_info *fbi;
 	struct mxc_edid_cfg edid_cfg;
@@ -179,11 +179,11 @@ static irqreturn_t mxc_dvi_detect_handler(int irq, void *data)
 	return IRQ_HANDLED;
 }
 
-static int dvi_init(struct mxc_dispdrv_entry *disp)
+static int dvi_init(struct mxc_dispdrv_handle *disp,
+	struct mxc_dispdrv_setting *setting)
 {
 	int ret = 0;
 	struct mxc_dvi_data *dvi = mxc_dispdrv_getdata(disp);
-	struct mxc_dispdrv_setting *setting = mxc_dispdrv_getsetting(disp);
 	struct fsl_mxc_dvi_platform_data *plat = dvi->client->dev.platform_data;
 
 	setting->dev_id = dvi->ipu = plat->ipu_id;
@@ -282,7 +282,7 @@ err:
 	return ret;
 }
 
-static void dvi_deinit(struct mxc_dispdrv_entry *disp)
+static void dvi_deinit(struct mxc_dispdrv_handle *disp)
 {
 	struct mxc_dvi_data *dvi = mxc_dispdrv_getdata(disp);
 
@@ -340,6 +340,7 @@ static int __devexit mxc_dvi_remove(struct i2c_client *client)
 {
 	struct mxc_dvi_data *dvi = i2c_get_clientdata(client);
 
+	mxc_dispdrv_puthandle(dvi->disp_dvi);
 	mxc_dispdrv_unregister(dvi->disp_dvi);
 	platform_device_unregister(dvi->pdev);
 	kfree(dvi);
diff --git a/drivers/video/mxc/mxc_edid.c b/drivers/video/mxc/mxc_edid.c
index 48502d05d58b..386df9259971 100644
--- a/drivers/video/mxc/mxc_edid.c
+++ b/drivers/video/mxc/mxc_edid.c
@@ -32,7 +32,6 @@
 #include "../edid.h"
 
 #undef DEBUG  /* define this for verbose EDID parsing output */
-
 #ifdef DEBUG
 #define DPRINTK(fmt, args...) printk(fmt, ## args)
 #else
@@ -185,6 +184,19 @@ static void get_detailed_timing(unsigned char *block,
 	}
 	mode->flag = FB_MODE_IS_DETAILED;
 
+	if ((H_SIZE / 16) == (V_SIZE / 9))
+		mode->vmode |= FB_VMODE_ASPECT_16_9;
+	else if ((H_SIZE / 4) == (V_SIZE / 3))
+		mode->vmode |= FB_VMODE_ASPECT_4_3;
+	else if ((mode->xres / 16) == (mode->yres / 9))
+		mode->vmode |= FB_VMODE_ASPECT_16_9;
+	else if ((mode->xres / 4) == (mode->yres / 3))
+		mode->vmode |= FB_VMODE_ASPECT_4_3;
+
+	if (mode->vmode & FB_VMODE_ASPECT_16_9)
+		DPRINTK("Aspect ratio: 16:9\n");
+	if (mode->vmode & FB_VMODE_ASPECT_4_3)
+		DPRINTK("Aspect ratio: 4:3\n");
 	DPRINTK("      %d MHz ",  PIXEL_CLOCK/1000000);
 	DPRINTK("%d %d %d %d ", H_ACTIVE, H_ACTIVE + H_SYNC_OFFSET,
 	       H_ACTIVE + H_SYNC_OFFSET + H_SYNC_WIDTH, H_ACTIVE + H_BLANKING);
diff --git a/drivers/video/mxc/mxc_ipuv3_fb.c b/drivers/video/mxc/mxc_ipuv3_fb.c
index 0b5c2f06722b..6c82bf04c7a5 100644
--- a/drivers/video/mxc/mxc_ipuv3_fb.c
+++ b/drivers/video/mxc/mxc_ipuv3_fb.c
@@ -78,6 +78,7 @@ struct mxcfb_info {
 	void *alpha_virt_addr1;
 	uint32_t alpha_mem_len;
 	uint32_t ipu_ch_irq;
+	uint32_t ipu_ch_nf_irq;
 	uint32_t ipu_alp_ch_irq;
 	uint32_t cur_ipu_buf;
 	uint32_t cur_ipu_alpha_buf;
@@ -85,13 +86,14 @@ struct mxcfb_info {
 	u32 pseudo_palette[16];
 
 	bool mode_found;
-	volatile bool wait4vsync;
 	struct semaphore flip_sem;
 	struct semaphore alpha_flip_sem;
 	struct completion vsync_complete;
 
 	void *ipu;
 	struct fb_info *ovfbi;
+
+	struct mxc_dispdrv_handle *dispdrv;
 };
 
 struct mxcfb_alloc_list {
@@ -152,6 +154,7 @@ static struct fb_info *found_registered_fb(ipu_channel_t ipu_ch, int ipu_id)
 }
 
 static irqreturn_t mxcfb_irq_handler(int irq, void *dev_id);
+static irqreturn_t mxcfb_nf_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);
@@ -288,8 +291,19 @@ static int mxcfb_set_par(struct fb_info *fbi)
 
 	dev_dbg(fbi->device, "Reconfiguring framebuffer\n");
 
+	if (mxc_fbi->dispdrv && mxc_fbi->dispdrv->drv->setup) {
+		retval = mxc_fbi->dispdrv->drv->setup(mxc_fbi->dispdrv, fbi);
+		if (retval < 0) {
+			dev_err(fbi->device, "setup error, dispdrv:%s.\n",
+					mxc_fbi->dispdrv->drv->name);
+			return -EINVAL;
+		}
+	}
+
 	ipu_clear_irq(mxc_fbi->ipu, mxc_fbi->ipu_ch_irq);
 	ipu_disable_irq(mxc_fbi->ipu, mxc_fbi->ipu_ch_irq);
+	ipu_clear_irq(mxc_fbi->ipu, mxc_fbi->ipu_ch_nf_irq);
+	ipu_disable_irq(mxc_fbi->ipu, mxc_fbi->ipu_ch_nf_irq);
 	ipu_disable_channel(mxc_fbi->ipu, mxc_fbi->ipu_ch, true);
 	ipu_uninit_channel(mxc_fbi->ipu, mxc_fbi->ipu_ch);
 	mxcfb_set_fix(fbi);
@@ -434,6 +448,9 @@ static int _swap_channels(struct fb_info *fbi_from,
 	tmp = mxc_fbi_from->ipu_ch_irq;
 	mxc_fbi_from->ipu_ch_irq = mxc_fbi_to->ipu_ch_irq;
 	mxc_fbi_to->ipu_ch_irq = tmp;
+	tmp = mxc_fbi_from->ipu_ch_nf_irq;
+	mxc_fbi_from->ipu_ch_nf_irq = mxc_fbi_to->ipu_ch_nf_irq;
+	mxc_fbi_to->ipu_ch_nf_irq = tmp;
 	ovfbi = mxc_fbi_from->ovfbi;
 	mxc_fbi_from->ovfbi = mxc_fbi_to->ovfbi;
 	mxc_fbi_to->ovfbi = ovfbi;
@@ -482,6 +499,10 @@ static int swap_channels(struct fb_info *fbi_from)
 	ipu_clear_irq(mxc_fbi_to->ipu, mxc_fbi_to->ipu_ch_irq);
 	ipu_free_irq(mxc_fbi_from->ipu, mxc_fbi_from->ipu_ch_irq, fbi_from);
 	ipu_free_irq(mxc_fbi_to->ipu, mxc_fbi_to->ipu_ch_irq, fbi_to);
+	ipu_clear_irq(mxc_fbi_from->ipu, mxc_fbi_from->ipu_ch_nf_irq);
+	ipu_clear_irq(mxc_fbi_to->ipu, mxc_fbi_to->ipu_ch_nf_irq);
+	ipu_free_irq(mxc_fbi_from->ipu, mxc_fbi_from->ipu_ch_nf_irq, fbi_from);
+	ipu_free_irq(mxc_fbi_to->ipu, mxc_fbi_to->ipu_ch_nf_irq, fbi_to);
 
 	if (mxc_fbi_from->cur_blank == FB_BLANK_UNBLANK) {
 		if (mxc_fbi_to->cur_blank == FB_BLANK_UNBLANK)
@@ -515,6 +536,9 @@ static int swap_channels(struct fb_info *fbi_from)
 		i = mxc_fbi_from->ipu_ch_irq;
 		mxc_fbi_from->ipu_ch_irq = mxc_fbi_to->ipu_ch_irq;
 		mxc_fbi_to->ipu_ch_irq = i;
+		i = mxc_fbi_from->ipu_ch_nf_irq;
+		mxc_fbi_from->ipu_ch_nf_irq = mxc_fbi_to->ipu_ch_nf_irq;
+		mxc_fbi_to->ipu_ch_nf_irq = i;
 		break;
 	default:
 		break;
@@ -534,6 +558,20 @@ static int swap_channels(struct fb_info *fbi_from)
 		return -EBUSY;
 	}
 	ipu_disable_irq(mxc_fbi_to->ipu, mxc_fbi_to->ipu_ch_irq);
+	if (ipu_request_irq(mxc_fbi_from->ipu, mxc_fbi_from->ipu_ch_nf_irq, mxcfb_nf_irq_handler, 0,
+		MXCFB_NAME, fbi_from) != 0) {
+		dev_err(fbi_from->device, "Error registering irq %d\n",
+			mxc_fbi_from->ipu_ch_nf_irq);
+		return -EBUSY;
+	}
+	ipu_disable_irq(mxc_fbi_from->ipu, mxc_fbi_from->ipu_ch_nf_irq);
+	if (ipu_request_irq(mxc_fbi_to->ipu, mxc_fbi_to->ipu_ch_nf_irq, mxcfb_irq_handler, 0,
+		MXCFB_NAME, fbi_to) != 0) {
+		dev_err(fbi_to->device, "Error registering irq %d\n",
+			mxc_fbi_to->ipu_ch_nf_irq);
+		return -EBUSY;
+	}
+	ipu_disable_irq(mxc_fbi_to->ipu, mxc_fbi_to->ipu_ch_nf_irq);
 
 	return 0;
 }
@@ -929,17 +967,14 @@ static int mxcfb_ioctl(struct fb_info *fbi, unsigned int cmd, unsigned long arg)
 			}
 
 			init_completion(&mxc_fbi->vsync_complete);
-
-			ipu_clear_irq(mxc_fbi->ipu, mxc_fbi->ipu_ch_irq);
-			mxc_fbi->wait4vsync = true;
-			ipu_enable_irq(mxc_fbi->ipu, mxc_fbi->ipu_ch_irq);
+			ipu_clear_irq(mxc_fbi->ipu, mxc_fbi->ipu_ch_nf_irq);
+			ipu_enable_irq(mxc_fbi->ipu, mxc_fbi->ipu_ch_nf_irq);
 			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 = false;
 				retval = -ETIME;
 			} else if (retval > 0) {
 				retval = 0;
@@ -1129,6 +1164,8 @@ static int mxcfb_blank(int blank, struct fb_info *info)
 	case FB_BLANK_VSYNC_SUSPEND:
 	case FB_BLANK_HSYNC_SUSPEND:
 	case FB_BLANK_NORMAL:
+		if (mxc_fbi->dispdrv && mxc_fbi->dispdrv->drv->disable)
+			mxc_fbi->dispdrv->drv->disable(mxc_fbi->dispdrv);
 		ipu_disable_channel(mxc_fbi->ipu, mxc_fbi->ipu_ch, true);
 		if (mxc_fbi->ipu_di >= 0)
 			ipu_uninit_sync_panel(mxc_fbi->ipu, mxc_fbi->ipu_di);
@@ -1374,14 +1411,18 @@ static irqreturn_t mxcfb_irq_handler(int irq, void *dev_id)
 	struct fb_info *fbi = dev_id;
 	struct mxcfb_info *mxc_fbi = fbi->par;
 
-	if (mxc_fbi->wait4vsync) {
-		complete(&mxc_fbi->vsync_complete);
-		ipu_disable_irq(mxc_fbi->ipu, irq);
-		mxc_fbi->wait4vsync = false;
-	} else {
-		up(&mxc_fbi->flip_sem);
-		ipu_disable_irq(mxc_fbi->ipu, irq);
-	}
+	up(&mxc_fbi->flip_sem);
+	ipu_disable_irq(mxc_fbi->ipu, irq);
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t mxcfb_nf_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);
+	ipu_disable_irq(mxc_fbi->ipu, irq);
 	return IRQ_HANDLED;
 }
 
@@ -1629,10 +1670,11 @@ static int mxcfb_dispdrv_init(struct platform_device *pdev,
 
 	dev_info(&pdev->dev, "register mxc display driver %s\n", disp_dev);
 
-	ret = mxc_dispdrv_init(disp_dev, &setting);
-	if (ret < 0) {
-		dev_err(&pdev->dev,
-			"register mxc display driver failed with %d\n", ret);
+	mxcfbi->dispdrv = mxc_dispdrv_gethandle(disp_dev, &setting);
+	if (IS_ERR(mxcfbi->dispdrv)) {
+		ret = PTR_ERR(mxcfbi->dispdrv);
+		dev_err(&pdev->dev, "NO mxc display driver found!\n");
+		return ret;
 	} else {
 		/* fix-up  */
 		mxcfbi->ipu_di_pix_fmt = setting.if_fmt;
@@ -1756,11 +1798,18 @@ static int mxcfb_register(struct fb_info *fbi)
 
 	if (ipu_request_irq(mxcfbi->ipu, mxcfbi->ipu_ch_irq, mxcfb_irq_handler, 0,
 				MXCFB_NAME, fbi) != 0) {
-		dev_err(fbi->device, "Error registering BG irq handler.\n");
+		dev_err(fbi->device, "Error registering EOF irq handler.\n");
 		ret = -EBUSY;
 		goto err0;
 	}
 	ipu_disable_irq(mxcfbi->ipu, mxcfbi->ipu_ch_irq);
+	if (ipu_request_irq(mxcfbi->ipu, mxcfbi->ipu_ch_nf_irq, mxcfb_nf_irq_handler, 0,
+				MXCFB_NAME, fbi) != 0) {
+		dev_err(fbi->device, "Error registering NFACK irq handler.\n");
+		ret = -EBUSY;
+		goto err1;
+	}
+	ipu_disable_irq(mxcfbi->ipu, mxcfbi->ipu_ch_nf_irq);
 
 	if (mxcfbi->ipu_alp_ch_irq != -1)
 		if (ipu_request_irq(mxcfbi->ipu, mxcfbi->ipu_alp_ch_irq,
@@ -1769,7 +1818,7 @@ static int mxcfb_register(struct fb_info *fbi)
 			dev_err(fbi->device, "Error registering alpha irq "
 					"handler.\n");
 			ret = -EBUSY;
-			goto err1;
+			goto err2;
 		}
 
 	mxcfb_check_var(&fbi->var, fbi);
@@ -1797,12 +1846,14 @@ static int mxcfb_register(struct fb_info *fbi)
 
 	ret = register_framebuffer(fbi);
 	if (ret < 0)
-		goto err2;
+		goto err3;
 
 	return ret;
-err2:
+err3:
 	if (mxcfbi->ipu_alp_ch_irq != -1)
 		ipu_free_irq(mxcfbi->ipu, mxcfbi->ipu_alp_ch_irq, fbi);
+err2:
+	ipu_free_irq(mxcfbi->ipu, mxcfbi->ipu_ch_nf_irq, fbi);
 err1:
 	ipu_free_irq(mxcfbi->ipu, mxcfbi->ipu_ch_irq, fbi);
 err0:
@@ -1817,12 +1868,14 @@ static void mxcfb_unregister(struct fb_info *fbi)
 		ipu_free_irq(mxcfbi->ipu, mxcfbi->ipu_alp_ch_irq, fbi);
 	if (mxcfbi->ipu_ch_irq)
 		ipu_free_irq(mxcfbi->ipu, mxcfbi->ipu_ch_irq, fbi);
+	if (mxcfbi->ipu_ch_nf_irq)
+		ipu_free_irq(mxcfbi->ipu, mxcfbi->ipu_ch_nf_irq, fbi);
 
 	unregister_framebuffer(fbi);
 }
 
 static int mxcfb_setup_overlay(struct platform_device *pdev,
-		struct fb_info *fbi_bg)
+		struct fb_info *fbi_bg, struct resource *res)
 {
 	struct fb_info *ovfbi;
 	struct mxcfb_info *mxcfbi_bg = (struct mxcfb_info *)fbi_bg->par;
@@ -1843,6 +1896,7 @@ static int mxcfb_setup_overlay(struct platform_device *pdev,
 	}
 	mxcfbi_fg->ipu_id = mxcfbi_bg->ipu_id;
 	mxcfbi_fg->ipu_ch_irq = IPU_IRQ_FG_SYNC_EOF;
+	mxcfbi_fg->ipu_ch_nf_irq = IPU_IRQ_FG_SYNC_NFACK;
 	mxcfbi_fg->ipu_alp_ch_irq = IPU_IRQ_FG_ALPHA_SYNC_EOF;
 	mxcfbi_fg->ipu_ch = MEM_FG_SYNC;
 	mxcfbi_fg->ipu_di = -1;
@@ -1854,6 +1908,14 @@ static int mxcfb_setup_overlay(struct platform_device *pdev,
 	ovfbi->var.xres = 240;
 	ovfbi->var.yres = 320;
 
+	if (res && res->end) {
+		ovfbi->fix.smem_len = res->end - res->start + 1;
+		ovfbi->fix.smem_start = res->start;
+		ovfbi->screen_base = ioremap(
+					ovfbi->fix.smem_start,
+					ovfbi->fix.smem_len);
+	}
+
 	ret = mxcfb_register(ovfbi);
 	if (ret < 0)
 		goto register_ov_failed;
@@ -1942,6 +2004,7 @@ static int mxcfb_probe(struct platform_device *pdev)
 		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);
+		memset(fbi->screen_base, 0, fbi->fix.smem_len);
 	}
 
 	mxcfbi->ipu = ipu_get_soc(mxcfbi->ipu_id);
@@ -1953,6 +2016,7 @@ static int mxcfb_probe(struct platform_device *pdev)
 	/* first user uses DP with alpha feature */
 	if (!g_dp_in_use[mxcfbi->ipu_id]) {
 		mxcfbi->ipu_ch_irq = IPU_IRQ_BG_SYNC_EOF;
+		mxcfbi->ipu_ch_nf_irq = IPU_IRQ_BG_SYNC_NFACK;
 		mxcfbi->ipu_alp_ch_irq = IPU_IRQ_BG_ALPHA_SYNC_EOF;
 		mxcfbi->ipu_ch = MEM_BG_SYNC;
 		mxcfbi->cur_blank = mxcfbi->next_blank = FB_BLANK_UNBLANK;
@@ -1964,7 +2028,9 @@ static int mxcfb_probe(struct platform_device *pdev)
 		if (ret < 0)
 			goto mxcfb_register_failed;
 
-		ret = mxcfb_setup_overlay(pdev, fbi);
+		res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+		ret = mxcfb_setup_overlay(pdev, fbi, res);
+
 		if (ret < 0) {
 			mxcfb_unregister(fbi);
 			goto mxcfb_setupoverlay_failed;
@@ -1973,6 +2039,7 @@ static int mxcfb_probe(struct platform_device *pdev)
 		g_dp_in_use[mxcfbi->ipu_id] = true;
 	} else {
 		mxcfbi->ipu_ch_irq = IPU_IRQ_DC_SYNC_EOF;
+		mxcfbi->ipu_ch_nf_irq = IPU_IRQ_DC_SYNC_NFACK;
 		mxcfbi->ipu_alp_ch_irq = -1;
 		mxcfbi->ipu_ch = MEM_DC_SYNC;
 		mxcfbi->cur_blank = mxcfbi->next_blank = FB_BLANK_POWERDOWN;
diff --git a/drivers/video/mxc/mxc_lcdif.c b/drivers/video/mxc/mxc_lcdif.c
index d9d7fa306a83..5cbdc73c4e75 100644
--- a/drivers/video/mxc/mxc_lcdif.c
+++ b/drivers/video/mxc/mxc_lcdif.c
@@ -21,7 +21,7 @@
 
 struct mxc_lcdif_data {
 	struct platform_device *pdev;
-	struct mxc_dispdrv_entry *disp_lcdif;
+	struct mxc_dispdrv_handle *disp_lcdif;
 };
 
 #define DISPDRV_LCD	"lcd"
@@ -42,11 +42,11 @@ static struct fb_videomode lcdif_modedb[] = {
 };
 static int lcdif_modedb_sz = ARRAY_SIZE(lcdif_modedb);
 
-static int lcdif_init(struct mxc_dispdrv_entry *disp)
+static int lcdif_init(struct mxc_dispdrv_handle *disp,
+	struct mxc_dispdrv_setting *setting)
 {
 	int ret, i;
 	struct mxc_lcdif_data *lcdif = mxc_dispdrv_getdata(disp);
-	struct mxc_dispdrv_setting *setting = mxc_dispdrv_getsetting(disp);
 	struct fsl_mxc_lcd_platform_data *plat_data
 			= lcdif->pdev->dev.platform_data;
 	struct fb_videomode *modedb = lcdif_modedb;
@@ -77,7 +77,7 @@ static int lcdif_init(struct mxc_dispdrv_entry *disp)
 	return ret;
 }
 
-void lcdif_deinit(struct mxc_dispdrv_entry *disp)
+void lcdif_deinit(struct mxc_dispdrv_handle *disp)
 {
 	/*TODO*/
 }
@@ -113,6 +113,7 @@ static int mxc_lcdif_remove(struct platform_device *pdev)
 {
 	struct mxc_lcdif_data *lcdif = dev_get_drvdata(&pdev->dev);
 
+	mxc_dispdrv_puthandle(lcdif->disp_lcdif);
 	mxc_dispdrv_unregister(lcdif->disp_lcdif);
 	kfree(lcdif);
 	return 0;
diff --git a/drivers/video/mxc/mxcfb_hx8369_wvga.c b/drivers/video/mxc/mxcfb_hx8369_wvga.c
index f9e5de4a4097..2d89aa0b1a5b 100644
--- a/drivers/video/mxc/mxcfb_hx8369_wvga.c
+++ b/drivers/video/mxc/mxcfb_hx8369_wvga.c
@@ -352,9 +352,15 @@ static int mipid_bl_get_brightness(struct backlight_device *bl)
 	return hx8369bl_brightness;
 }
 
+static int mipi_bl_check_fb(struct backlight_device *bl, struct fb_info *fbi)
+{
+	return 0;
+}
+
 static const struct backlight_ops mipid_lcd_bl_ops = {
 	.update_status = mipid_bl_update_status,
 	.get_brightness = mipid_bl_get_brightness,
+	.check_fb = mipi_bl_check_fb,
 };
 
 static int mipid_init_backlight(struct mipi_dsi_info *mipi_dsi)
diff --git a/drivers/video/mxc/mxcfb_sii902x.c b/drivers/video/mxc/mxcfb_sii902x.c
index f626f649f9cc..f917674423c3 100644
--- a/drivers/video/mxc/mxcfb_sii902x.c
+++ b/drivers/video/mxc/mxcfb_sii902x.c
@@ -188,7 +188,7 @@
 struct sii902x_data {
 	struct platform_device *pdev;
 	struct i2c_client *client;
-	struct mxc_dispdrv_entry *disp_hdmi;
+	struct mxc_dispdrv_handle *disp_hdmi;
 	struct regulator *io_reg;
 	struct regulator *analog_reg;
 	struct delayed_work det_work;
@@ -1064,11 +1064,11 @@ static int sii902x_TPI_init(struct i2c_client *client)
 	return 0;
 }
 
-static int sii902x_disp_init(struct mxc_dispdrv_entry *disp)
+static int sii902x_disp_init(struct mxc_dispdrv_handle *disp,
+	struct mxc_dispdrv_setting *setting)
 {
 	int ret = 0;
 	struct sii902x_data *sii902x = mxc_dispdrv_getdata(disp);
-	struct mxc_dispdrv_setting *setting = mxc_dispdrv_getsetting(disp);
 	struct fsl_mxc_lcd_platform_data *plat = sii902x->client->dev.platform_data;
 	bool found = false;
 	static bool inited;
@@ -1215,7 +1215,7 @@ register_pltdev_failed:
 	return ret;
 }
 
-static void sii902x_disp_deinit(struct mxc_dispdrv_entry *disp)
+static void sii902x_disp_deinit(struct mxc_dispdrv_handle *disp)
 {
 	struct sii902x_data *sii902x = mxc_dispdrv_getdata(disp);
 	struct fsl_mxc_lcd_platform_data *plat = sii902x->client->dev.platform_data;
@@ -1273,6 +1273,7 @@ static int __devexit sii902x_remove(struct i2c_client *client)
 {
 	struct sii902x_data *sii902x = i2c_get_clientdata(client);
 
+	mxc_dispdrv_puthandle(sii902x->disp_hdmi);
 	mxc_dispdrv_unregister(sii902x->disp_hdmi);
 	kfree(sii902x);
 	return 0;
diff --git a/drivers/video/mxc/tve.c b/drivers/video/mxc/tve.c
index 98b506d37309..7a5c3b5d0edf 100644
--- a/drivers/video/mxc/tve.c
+++ b/drivers/video/mxc/tve.c
@@ -120,8 +120,8 @@ struct tve_data {
 	struct delayed_work cd_work;
 	struct tve_reg_mapping *regs;
 	struct tve_reg_fields_mapping *reg_fields;
-	struct mxc_dispdrv_entry *disp_tve;
-	struct mxc_dispdrv_entry *disp_vga;
+	struct mxc_dispdrv_handle *disp_tve;
+	struct mxc_dispdrv_handle *disp_vga;
 	struct notifier_block nb;
 };
 
@@ -934,6 +934,15 @@ int tve_fb_setup(struct tve_data *tve, struct fb_info *fbi)
 	return 0;
 }
 
+static inline int tve_disp_setup(struct mxc_dispdrv_handle *disp,
+	struct fb_info *fbi)
+{
+	struct tve_data *tve = mxc_dispdrv_getdata(disp);
+
+	tve_fb_setup(tve, fbi);
+	return 0;
+}
+
 int tve_fb_event(struct notifier_block *nb, unsigned long val, void *v)
 {
 	struct tve_data *tve = container_of(nb, struct tve_data, nb);
@@ -945,11 +954,6 @@ int tve_fb_event(struct notifier_block *nb, unsigned long val, void *v)
 		return 0;
 
 	switch (val) {
-	case FB_EVENT_PREMODE_CHANGE:
-	{
-		tve_fb_setup(tve, fbi);
-		break;
-	}
 	case FB_EVENT_BLANK:
 		if (fbi->mode == NULL)
 			return 0;
@@ -1028,11 +1032,11 @@ static int _tve_get_revision(struct tve_data *tve)
 	return rev;
 }
 
-static int tve_drv_init(struct mxc_dispdrv_entry *disp, bool vga)
+static int tve_drv_init(struct mxc_dispdrv_handle *disp, bool vga,
+	struct mxc_dispdrv_setting *setting)
 {
 	int ret;
 	struct tve_data *tve = mxc_dispdrv_getdata(disp);
-	struct mxc_dispdrv_setting *setting = mxc_dispdrv_getsetting(disp);
 	struct fsl_mxc_tve_platform_data *plat_data
 			= tve->pdev->dev.platform_data;
 	struct resource *res;
@@ -1190,17 +1194,19 @@ get_res_failed:
 
 }
 
-static int tvout_init(struct mxc_dispdrv_entry *disp)
+static int tvout_init(struct mxc_dispdrv_handle *disp,
+	struct mxc_dispdrv_setting *setting)
 {
-	return tve_drv_init(disp, 0);
+	return tve_drv_init(disp, 0, setting);
 }
 
-static int vga_init(struct mxc_dispdrv_entry *disp)
+static int vga_init(struct mxc_dispdrv_handle *disp,
+	struct mxc_dispdrv_setting *setting)
 {
-	return tve_drv_init(disp, 1);
+	return tve_drv_init(disp, 1, setting);
 }
 
-void tvout_deinit(struct mxc_dispdrv_entry *disp)
+void tvout_deinit(struct mxc_dispdrv_handle *disp)
 {
 	struct tve_data *tve = mxc_dispdrv_getdata(disp);
 
@@ -1217,6 +1223,7 @@ static struct mxc_dispdrv_driver tve_drv = {
 	.name 	= DISPDRV_TVE,
 	.init  	= tvout_init,
 	.deinit = tvout_deinit,
+	.setup = tve_disp_setup,
 };
 
 static struct mxc_dispdrv_driver vga_drv = {
@@ -1236,6 +1243,8 @@ static int tve_dispdrv_init(struct tve_data *tve)
 
 static void tve_dispdrv_deinit(struct tve_data *tve)
 {
+	mxc_dispdrv_puthandle(tve->disp_tve);
+	mxc_dispdrv_puthandle(tve->disp_vga);
 	mxc_dispdrv_unregister(tve->disp_tve);
 	mxc_dispdrv_unregister(tve->disp_vga);
 }
diff --git a/drivers/video/mxc_hdmi.c b/drivers/video/mxc_hdmi.c
index 09930158fb0e..0d1641f7858f 100644
--- a/drivers/video/mxc_hdmi.c
+++ b/drivers/video/mxc_hdmi.c
@@ -117,7 +117,7 @@ struct hdmi_data_info {
 struct mxc_hdmi {
 	struct platform_device *pdev;
 	struct platform_device *core_pdev;
-	struct mxc_dispdrv_entry *disp_mxc_hdmi;
+	struct mxc_dispdrv_handle *disp_mxc_hdmi;
 	struct fb_info *fbi;
 	struct clk *hdmi_isfr_clk;
 	struct clk *hdmi_iahb_clk;
@@ -1616,11 +1616,11 @@ static int mxc_hdmi_fb_event(struct notifier_block *nb,
 	return 0;
 }
 
-static int mxc_hdmi_disp_init(struct mxc_dispdrv_entry *disp)
+static int mxc_hdmi_disp_init(struct mxc_dispdrv_handle *disp,
+	struct mxc_dispdrv_setting *setting)
 {
 	int ret = 0;
 	struct mxc_hdmi *hdmi = mxc_dispdrv_getdata(disp);
-	struct mxc_dispdrv_setting *setting = mxc_dispdrv_getsetting(disp);
 	struct fsl_mxc_hdmi_platform_data *plat = hdmi->pdev->dev.platform_data;
 	int irq = platform_get_irq(hdmi->pdev, 0);
 	bool found = false;
@@ -1810,7 +1810,7 @@ egetpins:
 	return ret;
 }
 
-static void mxc_hdmi_disp_deinit(struct mxc_dispdrv_entry *disp)
+static void mxc_hdmi_disp_deinit(struct mxc_dispdrv_handle *disp)
 {
 	struct mxc_hdmi *hdmi = mxc_dispdrv_getdata(disp);
 	struct fsl_mxc_hdmi_platform_data *plat = hdmi->pdev->dev.platform_data;
@@ -1892,6 +1892,8 @@ static int mxc_hdmi_remove(struct platform_device *pdev)
 
 	fb_unregister_client(&hdmi->nb);
 
+	mxc_dispdrv_puthandle(hdmi->disp_mxc_hdmi);
+	mxc_dispdrv_unregister(hdmi->disp_mxc_hdmi);
 	/* No new work will be scheduled, wait for running ISR */
 	free_irq(irq, hdmi);
 	kfree(hdmi);
diff --git a/include/linux/fb.h b/include/linux/fb.h
index 030312c6adf0..68ba85a00c06 100644
--- a/include/linux/fb.h
+++ b/include/linux/fb.h
@@ -547,8 +547,6 @@ struct fb_cursor_user {
 #define FB_EVENT_FB_UNBIND              0x0E
 /*      CONSOLE-SPECIFIC: remap all consoles to new fb - for vga switcheroo */
 #define FB_EVENT_REMAP_ALL_CONSOLE      0x0F
-/*	PRE MODE CHANGE added by fsl */
-#define FB_EVENT_PREMODE_CHANGE		0x10
 
 struct fb_event {
 	struct fb_info *info;
diff --git a/include/linux/fsl_devices.h b/include/linux/fsl_devices.h
index cc1d8574dc7f..60812da073b8 100644
--- a/include/linux/fsl_devices.h
+++ b/include/linux/fsl_devices.h
@@ -119,7 +119,6 @@ struct fsl_usb2_platform_data {
 	enum usb_wakeup_event (*is_wakeup_event)(struct fsl_usb2_platform_data *);
 	void (*wakeup_handler)(struct fsl_usb2_platform_data *);
 	void (*hsic_post_ops)(void);
-	void (*gadget_discharge_dp) (bool);
 
 	struct fsl_usb2_wakeup_platform_data *wakeup_pdata;
 	struct platform_device *pdev;
diff --git a/sound/soc/codecs/cs42888.h b/sound/soc/codecs/cs42888.h
index a650c9da5338..bea8beb8b70d 100644
--- a/sound/soc/codecs/cs42888.h
+++ b/sound/soc/codecs/cs42888.h
@@ -19,6 +19,7 @@
  * the .codec_dai field of your machine driver's snd_soc_dai_link structure.
  */
 extern struct snd_soc_dai_driver cs42888_dai;
+#define CS42888_RST 235
 
 /*
  * The ASoC codec device structure for the CS42888.  Assign this structure
diff --git a/sound/soc/imx/imx-cs42888.c b/sound/soc/imx/imx-cs42888.c
index d34be0a0e8f9..4c9ca9857464 100644
--- a/sound/soc/imx/imx-cs42888.c
+++ b/sound/soc/imx/imx-cs42888.c
@@ -20,6 +20,7 @@
 #include <linux/delay.h>
 #include <linux/regulator/consumer.h>
 #include <linux/fsl_devices.h>
+#include <linux/gpio.h>
 #include <sound/core.h>
 #include <sound/pcm.h>
 #include <sound/soc.h>
@@ -33,7 +34,6 @@
 #include "imx-esai.h"
 #include "../codecs/cs42888.h"
 
-
 struct imx_priv_state {
 	int hw;
 };
@@ -45,8 +45,13 @@ static int imx_3stack_startup(struct snd_pcm_substream *substream)
 {
 	struct snd_soc_pcm_runtime *rtd = substream->private_data;
 	struct snd_soc_dai *cpu_dai = rtd->cpu_dai;
-	if (!cpu_dai->active)
+
+	if (!cpu_dai->active) {
 		hw_state.hw = 0;
+		gpio_direction_output(CS42888_RST, 0);
+		msleep(100);
+		gpio_direction_output(CS42888_RST, 1);
+	}
 
 	return 0;
 }
diff --git a/sound/soc/imx/imx-ssi.c b/sound/soc/imx/imx-ssi.c
index 8cf85b06b780..559dbfb7319d 100644
--- a/sound/soc/imx/imx-ssi.c
+++ b/sound/soc/imx/imx-ssi.c
@@ -99,10 +99,6 @@ static int imx_ssi_set_dai_fmt(struct snd_soc_dai *cpu_dai, unsigned int fmt)
 	case SND_SOC_DAIFMT_I2S:
 		/* data on rising edge of bclk, frame low 1clk before data */
 		strcr |= SSI_STCR_TFSI | SSI_STCR_TEFS | SSI_STCR_TXBIT0;
-		if (ssi->flags & IMX_SSI_USE_I2S_SLAVE) {
-			scr &= ~SSI_I2S_MODE_MASK;
-			scr |= SSI_SCR_I2S_MODE_SLAVE;
-		}
 		break;
 	case SND_SOC_DAIFMT_LEFT_J:
 		/* data on rising edge of bclk, frame high with data */
@@ -138,10 +134,21 @@ static int imx_ssi_set_dai_fmt(struct snd_soc_dai *cpu_dai, unsigned int fmt)
 
 	/* DAI clock master masks */
 	switch (fmt & SND_SOC_DAIFMT_MASTER_MASK) {
+	case SND_SOC_DAIFMT_CBS_CFS:
+		strcr |= SSI_STCR_TFDIR | SSI_STCR_TXDIR;
+		if ((fmt & SND_SOC_DAIFMT_FORMAT_MASK) == SND_SOC_DAIFMT_I2S) {
+			scr &= ~SSI_I2S_MODE_MASK;
+			scr |= SSI_SCR_I2S_MODE_MSTR;
+		}
+		break;
 	case SND_SOC_DAIFMT_CBM_CFM:
+		strcr &= ~(SSI_STCR_TFDIR | SSI_STCR_TXDIR);
+		if ((fmt & SND_SOC_DAIFMT_FORMAT_MASK) == SND_SOC_DAIFMT_I2S) {
+			scr &= ~SSI_I2S_MODE_MASK;
+			scr |= SSI_SCR_I2S_MODE_SLAVE;
+		}
 		break;
 	default:
-		/* Master mode not implemented, needs handling of clocks. */
 		return -EINVAL;
 	}
 
@@ -279,9 +286,10 @@ static int imx_ssi_hw_params(struct snd_pcm_substream *substream,
 
 	scr = readl(ssi->base + SSI_SCR);
 
-	if (channels == 1)
+	if (channels == 1) {
 		scr &= ~SSI_SCR_NET;
-	else
+		scr &= ~SSI_I2S_MODE_MASK;
+	} else
 		scr |= SSI_SCR_NET;
 
 	writel(scr, ssi->base + SSI_SCR);