ENGR00171216: Fix the DVFS and suspend/resume on mx53 SMD

This commit fixes the DVFS, suspend/resume, fuse check, clocks,
cpufreq for mx53 SMD.

Signed-off-by: Nitin Garg <nitin.garg@freescale.com>

19 files changed, 807 insertions, 219 deletions

diff --git a/arch/arm/configs/imx5_android_defconfig b/arch/arm/configs/imx5_android_defconfig
index 23a2538d3e99..5937c525a472 100644
--- a/arch/arm/configs/imx5_android_defconfig
+++ b/arch/arm/configs/imx5_android_defconfig
@@ -1,7 +1,7 @@
 #
 # Automatically generated make config: don't edit
 # Linux/arm 2.6.38 Kernel Configuration
-# Fri Feb  3 18:50:35 2012
+# Tue Feb 14 15:31:13 2012
 #
 CONFIG_ARM=y
 CONFIG_HAVE_PWM=y
@@ -1338,7 +1338,7 @@ CONFIG_HW_RANDOM=y
 # CONFIG_TCG_TPM is not set
 # CONFIG_DCC_TTY is not set
 # CONFIG_RAMOOPS is not set
-# CONFIG_MXC_IIM is not set
+CONFIG_MXC_IIM=y
 # CONFIG_MXS_VIIM is not set
 CONFIG_I2C=y
 CONFIG_I2C_BOARDINFO=y
@@ -2311,7 +2311,7 @@ CONFIG_RTC_INTF_ALARM_DEV=y
 # CONFIG_RTC_DRV_M48T59 is not set
 # CONFIG_RTC_DRV_MSM6242 is not set
 # CONFIG_RTC_MXC is not set
-CONFIG_RTC_DRV_MXC_V2=y
+# CONFIG_RTC_DRV_MXC_V2 is not set
 # CONFIG_RTC_DRV_SNVS is not set
 # CONFIG_RTC_DRV_BQ4802 is not set
 # CONFIG_RTC_DRV_RP5C01 is not set
@@ -2320,9 +2320,9 @@ CONFIG_RTC_DRV_MXC_V2=y
 #
 # on-CPU RTC drivers
 #
-# CONFIG_RTC_DRV_MC13XXX is not set
+CONFIG_RTC_DRV_MC13XXX=y
 CONFIG_RTC_DRV_MC34708=y
-# CONFIG_RTC_DRV_DA9052 is not set
+CONFIG_RTC_DRV_DA9052=y
 CONFIG_DMADEVICES=y
 # CONFIG_DMADEVICES_DEBUG is not set
 
diff --git a/arch/arm/mach-mx5/Makefile b/arch/arm/mach-mx5/Makefile
index 8dbcf863f42e..191cc0d7a3cc 100644
--- a/arch/arm/mach-mx5/Makefile
+++ b/arch/arm/mach-mx5/Makefile
@@ -4,13 +4,14 @@
 
 # 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 cpu_regulator-mx5.o
+pm.o system.o suspend.o usb_dr.o usb_h1.o usb_h2.o cpu_regulator-mx5.o \
+check_fuse.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
 obj-$(CONFIG_SOC_IMX53) += clock.o
 
-obj-$(CONFIG_CPU_FREQ_IMX)    += cpu_op-mx51.o cpu_op-mx53.o cpu_op-mx50.o
+obj-$(CONFIG_SOC_IMX53)    += cpu_op-mx51.o cpu_op-mx53.o cpu_op-mx50.o
 obj-$(CONFIG_MACH_MX51_BABBAGE) += board-mx51_babbage.o mx51_babbage_pmic_mc13892.o
 obj-$(CONFIG_MACH_MX51_3DS) += board-mx51_3ds.o
 obj-$(CONFIG_MACH_MX53_EVK) += board-mx53_evk.o
diff --git a/arch/arm/mach-mx5/board-mx53_smd.c b/arch/arm/mach-mx5/board-mx53_smd.c
index 1bf3ac6cdf4f..31488f4176c0 100644
--- a/arch/arm/mach-mx5/board-mx53_smd.c
+++ b/arch/arm/mach-mx5/board-mx53_smd.c
@@ -49,6 +49,8 @@
 #include <mach/ahci_sata.h>
 #include <mach/imx_rfkill.h>
 #include <mach/mxc_asrc.h>
+#include <mach/mxc_dvfs.h>
+#include <mach/check_fuse.h>
 
 #include <asm/mach-types.h>
 #include <asm/mach/arch.h>
@@ -135,12 +137,12 @@
 
 void __init early_console_setup(unsigned long base, struct clk *clk);
 static struct clk *sata_clk, *sata_ref_clk;
+static int fs_in_sdcard;
 
 extern char *lp_reg_id;
 extern char *gp_reg_id;
 extern void mx5_cpu_regulator_init(void);
 extern int mx53_smd_init_da9052(void);
-extern void mx5_cpu_regulator_init(void);
 
 static iomux_v3_cfg_t mx53_smd_pads[] = {
 	/* DI_VGA_HSYNC */
@@ -450,6 +452,8 @@ static void __init smd_add_device_buttons(void) {}
 
 static const struct imxuart_platform_data mx53_smd_uart_data __initconst = {
 	.flags = IMXUART_HAVE_RTSCTS,
+	.dma_req_rx = MX53_DMA_REQ_UART3_RX,
+	.dma_req_tx = MX53_DMA_REQ_UART3_TX,
 };
 
 static inline void mx53_smd_init_uart(void)
@@ -496,19 +500,22 @@ static struct mxc_pm_platform_data smd_pm_data = {
 	.suspend_exit = smd_suspend_exit,
 };
 
-
+/* SDIO Card Slot */
 static const struct esdhc_platform_data mx53_smd_sd1_data __initconst = {
 	.cd_gpio = MX53_SMD_SD1_CD,
 	.wp_gpio = MX53_SMD_SD1_WP,
 };
 
+/* SDIO Wifi */
 static const struct esdhc_platform_data mx53_smd_sd2_data __initconst = {
 	.always_present = 1,
 	.keep_power_at_suspend = 1,
 };
 
+/* SDIO Internal eMMC */
 static const struct esdhc_platform_data mx53_smd_sd3_data __initconst = {
 	.always_present = 1,
+	.support_8bit = 1,
 };
 
 static struct fsl_mxc_camera_platform_data camera_data = {
@@ -754,9 +761,9 @@ static int mx53_smd_sata_init(struct device *dev, void __iomem *addr)
 	tmpdata = clk_get_rate(clk) / 1000;
 	clk_put(clk);
 
-	sata_init(addr, tmpdata);
-
-	return ret;
+	ret = sata_init(addr, tmpdata);
+	if (ret == 0)
+		return ret;
 
 release_sata_ref_clk:
 	clk_disable(sata_ref_clk);
@@ -906,7 +913,7 @@ static struct ipuv3_fb_platform_data smd_fb_data[] = {
 	{
 	.disp_dev = "ldb",
 	.interface_pix_fmt = IPU_PIX_FMT_RGB666,
-	.mode_str = "XGA",
+	.mode_str = "LDB-XGA",
 	.default_bpp = 16,
 	.int_clk = false,
 	}, {
@@ -956,9 +963,32 @@ static struct mxc_spdif_platform_data mxc_spdif_data = {
 	.spdif_clk = NULL,	/* spdif bus clk */
 };
 
+static struct mxc_dvfs_platform_data smd_dvfs_core_data = {
+	.reg_id = "cpu_vddgp",
+	.clk1_id = "cpu_clk",
+	.clk2_id = "gpc_dvfs_clk",
+	.gpc_cntr_offset = MXC_GPC_CNTR_OFFSET,
+	.gpc_vcr_offset = MXC_GPC_VCR_OFFSET,
+	.ccm_cdcr_offset = MXC_CCM_CDCR_OFFSET,
+	.ccm_cacrr_offset = MXC_CCM_CACRR_OFFSET,
+	.ccm_cdhipr_offset = MXC_CCM_CDHIPR_OFFSET,
+	.prediv_mask = 0x1F800,
+	.prediv_offset = 11,
+	.prediv_val = 3,
+	.div3ck_mask = 0xE0000000,
+	.div3ck_offset = 29,
+	.div3ck_val = 2,
+	.emac_val = 0x08,
+	.upthr_val = 25,
+	.dnthr_val = 9,
+	.pncthr_val = 33,
+	.upcnt_val = 10,
+	.dncnt_val = 10,
+	.delay_time = 30,
+};
+
 static struct mxc_regulator_platform_data smd_regulator_data = {
-	.cpu_reg_id = "DA9052_BUCK_CORE",
-	.vcc_reg_id = "DA9052_BUCK_PRO",
+	.cpu_reg_id = "cpu_vddgp",
 };
 
 #if defined(CONFIG_BATTERY_MAX17085) || defined(CONFIG_BATTERY_MAX17085_MODULE)
@@ -1050,6 +1080,13 @@ static void __init fixup_mxc_board(struct machine_desc *desc, struct tag *tags,
 				imx53_gpu_data.gmem_reserved_size =
 						memparse(str, &str);
 			}
+
+			str = t->u.cmdline.cmdline;
+			str = strstr(str, "fs_sdcard=");
+			if (str != NULL) {
+				str += 10;
+				fs_in_sdcard = memparse(str, &str);
+			}
 			break;
 		}
 	}
@@ -1063,6 +1100,10 @@ static void mx53_smd_power_off(void)
 
 static int __init mx53_smd_power_init(void)
 {
+	/* cpu get regulator needs to be in lateinit so that
+	   regulator list gets updated for i2c da9052 regulators */
+	mx5_cpu_regulator_init();
+
 	if (machine_is_mx53_smd())
 		pm_power_off = mx53_smd_power_off;
 
@@ -1156,15 +1197,23 @@ static void __init mx53_smd_board_init(void)
 	for (i = 0; i < ARRAY_SIZE(smd_fb_data); i++)
 		imx53_add_ipuv3fb(i, &smd_fb_data[i]);
 	imx53_add_lcdif(&lcdif_data);
-	imx53_add_vpu();
+	if (!mxc_fuse_get_vpu_status())
+		imx53_add_vpu();
 	imx53_add_ldb(&ldb_data);
 	imx53_add_v4l2_output(0);
 	imx53_add_v4l2_capture(0);
 	imx53_add_mxc_pwm(1);
 	imx53_add_mxc_pwm_backlight(0, &mxc_pwm_backlight_data);
-	imx53_add_sdhci_esdhc_imx(0, &mx53_smd_sd1_data);
+
+	if (fs_in_sdcard == 1) {
+		imx53_add_sdhci_esdhc_imx(0, &mx53_smd_sd1_data);
+		imx53_add_sdhci_esdhc_imx(2, &mx53_smd_sd3_data);
+	} else {
+		imx53_add_sdhci_esdhc_imx(2, &mx53_smd_sd3_data);
+		imx53_add_sdhci_esdhc_imx(0, &mx53_smd_sd1_data);
+	}
+
 	imx53_add_sdhci_esdhc_imx(1, &mx53_smd_sd2_data);
-	imx53_add_sdhci_esdhc_imx(2, &mx53_smd_sd3_data);
 	imx53_add_ahci(0, &mx53_smd_sata_data);
 	mxc_register_device(&imx_ahci_device_hwmon, NULL);
 	mx53_smd_init_usb();
@@ -1208,15 +1257,17 @@ static void __init mx53_smd_board_init(void)
 	else
 		mx53_smd_gpu_pdata.z160_revision = 0;
 
-	imx53_add_mxc_gpu(&mx53_smd_gpu_pdata);
+	if (!mxc_fuse_get_gpu_status())
+		imx53_add_mxc_gpu(&mx53_smd_gpu_pdata);
 
 	/* this call required to release SCC RAM partition held by ROM
 	  * during boot, even if SCC2 driver is not part of the image
 	  */
 	imx53_add_mxc_scc2();
-
-	mx5_cpu_regulator_init();
 	smd_add_device_battery();
+
+	imx53_add_dvfs_core(&smd_dvfs_core_data);
+	imx53_add_busfreq();
 }
 
 static void __init mx53_smd_timer_init(void)
diff --git a/arch/arm/mach-mx5/check_fuse.c b/arch/arm/mach-mx5/check_fuse.c
new file mode 100644
index 000000000000..a4ae656e7462
--- /dev/null
+++ b/arch/arm/mach-mx5/check_fuse.c
@@ -0,0 +1,63 @@
+/*
+ * Copyright (C) 2010-2012 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+
+#include <linux/io.h>
+#include <linux/module.h>
+#include <mach/hardware.h>
+#include <mach/check_fuse.h>
+
+int mxc_fuse_get_gpu_status(void)
+{
+	void __iomem *reg_base = NULL;
+	u32 reg_val = 0;
+	int bit_status = 0;
+
+	if (cpu_is_mx53() || cpu_is_mx51()) {
+		reg_base = MX53_IO_ADDRESS(MX53_IIM_BASE_ADDR);
+		reg_val = readl(reg_base + MXC_IIM_MX5_DISABLERS_OFFSET);
+		bit_status = (reg_val & MXC_IIM_MX5_DISABLERS_GPU_MASK)
+				>> MXC_IIM_MX5_DISABLERS_GPU_SHIFT;
+	} else if (cpu_is_mx50()) {
+		reg_base = ioremap(MX50_OCOTP_CTRL_BASE_ADDR, SZ_8K);
+		reg_val = readl(reg_base + FSL_OCOTP_MX5_CFG2_OFFSET);
+		bit_status = (reg_val & FSL_OCOTP_MX5_DISABLERS_GPU_MASK)
+				>> FSL_OCOTP_MX5_DISABLERS_GPU_SHIFT;
+	}
+
+	return (1 == bit_status);
+}
+EXPORT_SYMBOL(mxc_fuse_get_gpu_status);
+
+int mxc_fuse_get_vpu_status(void)
+{
+	void __iomem *reg_base = NULL;
+	u32 reg_val = 0;
+	int bit_status = 0;
+
+	if (cpu_is_mx53()) {
+		reg_base = MX53_IO_ADDRESS(MX53_IIM_BASE_ADDR);
+		reg_val = readl(reg_base + MXC_IIM_MX5_DISABLERS_OFFSET);
+		bit_status = (reg_val & MXC_IIM_MX5_DISABLERS_VPU_MASK)
+				>> MXC_IIM_MX5_DISABLERS_VPU_SHIFT;
+	}
+
+	return (1 == bit_status);
+}
+EXPORT_SYMBOL(mxc_fuse_get_vpu_status);
+
diff --git a/arch/arm/mach-mx5/clock.c b/arch/arm/mach-mx5/clock.c
index 7ee4b00bc349..e881854bcd2e 100644
--- a/arch/arm/mach-mx5/clock.c
+++ b/arch/arm/mach-mx5/clock.c
@@ -54,7 +54,7 @@ static struct clk axi_a_clk;
 static struct clk axi_b_clk;
 static struct clk ddr_hf_clk;
 static struct clk mipi_hsp_clk;
-static struct clk gpu3d_clk;
+static struct clk gpu3d_clk[];
 static struct clk gpu2d_clk;
 static struct clk vpu_clk[];
 static int cpu_curr_op;
@@ -320,17 +320,6 @@ static struct clk fpm_clk = {
 	.disable = _fpm_disable,
 };
 
-static unsigned long _fpm_div2_get_rate(struct clk *clk)
-{
-	return  clk_get_rate(clk->parent) / 2;
-}
-
-static struct clk fpm_div2_clk = {
-	__INIT_CLK_DEBUG(fpm_div2_clk)
-	.parent = &fpm_clk,
-	.get_rate = _fpm_div2_get_rate,
-};
-
 static unsigned long _clk_pll_get_rate(struct clk *clk)
 {
 	long mfi, mfn, mfd, pdf, ref_clk, mfn_abs;
@@ -703,7 +692,8 @@ static unsigned long _clk_main_bus_get_rate(struct clk *clk)
 {
 	u32 div = 0;
 
-	if (dvfs_per_divider_active() || low_bus_freq_mode)
+	if (cpu_is_mx51() &&
+		(dvfs_per_divider_active() || low_bus_freq_mode))
 		div  = (__raw_readl(MXC_CCM_CDCR) & 0x3);
 	return clk_get_rate(clk->parent) / (div + 1);
 }
@@ -1368,14 +1358,21 @@ static struct clk ocram_clk = {
 };
 
 
-static struct clk aips_tz1_clk = {
-	__INIT_CLK_DEBUG(aips_tz1_clk)
+static struct clk aips_tz1_clk[] = {
+	{
+	__INIT_CLK_DEBUG(aips_tz1_clk_0)
 	.parent = &ahb_clk,
-	.secondary = &ahb_max_clk,
+	.secondary = &aips_tz1_clk[1],
 	.enable_reg = MXC_CCM_CCGR0,
 	.enable_shift = MXC_CCM_CCGRx_CG12_OFFSET,
 	.enable = _clk_enable,
 	.disable = _clk_disable_inwait,
+	},
+	{
+	__INIT_CLK_DEBUG(aips_tz1_clk_1)
+	.parent = &emi_fast_clk,
+	.secondary = &ahb_max_clk,
+	},
 };
 
 static struct clk aips_tz2_clk = {
@@ -1430,7 +1427,7 @@ static void _clk_sdma_disable(struct clk *clk)
 
 static struct clk sdma_clk[] = {
 	{
-	 __INIT_CLK_DEBUG(sdma_clk)
+	 __INIT_CLK_DEBUG(sdma_clk_0)
 	 .parent = &ahb_clk,
 	 .enable_reg = MXC_CCM_CCGR4,
 	 .enable_shift = MXC_CCM_CCGRx_CG15_OFFSET,
@@ -1438,11 +1435,14 @@ static struct clk sdma_clk[] = {
 	 .disable = _clk_sdma_disable,
 	 },
 	{
-	 .parent = &ipg_clk,
+	__INIT_CLK_DEBUG(sdma_clk_1)
+	.parent = &ipg_clk,
 #ifdef CONFIG_SDMA_IRAM
-	 .secondary = &emi_intr_clk[0],
+	.secondary = &emi_intr_clk[0],
+#else
+	.secondary = &emi_fast_clk,
 #endif
-	 },
+	},
 };
 
 static int _clk_ipu_enable(struct clk *clk)
@@ -2150,7 +2150,7 @@ static struct clk uart1_clk[] = {
 	 .id = 0,
 	 .parent = &ipg_clk,
 #if UART1_DMA_ENABLE
-	 .secondary = &aips_tz1_clk,
+	 .secondary = &aips_tz1_clk[0],
 #endif
 	 .enable_reg = MXC_CCM_CCGR1,
 	 .enable_shift = MXC_CCM_CCGRx_CG3_OFFSET,
@@ -2178,7 +2178,7 @@ static struct clk uart2_clk[] = {
 	 .id = 1,
 	 .parent = &ipg_clk,
 #if UART2_DMA_ENABLE
-	 .secondary = &aips_tz1_clk,
+	 .secondary = &aips_tz1_clk[0],
 #endif
 	 .enable_reg = MXC_CCM_CCGR1,
 	 .enable_shift = MXC_CCM_CCGRx_CG5_OFFSET,
@@ -2401,17 +2401,6 @@ static struct clk hsi2c_serial_clk = {
 	.disable = _clk_disable,
 };
 
-static struct clk hsi2c_clk = {
-	__INIT_CLK_DEBUG(hsi2c_clk)
-	.id = 0,
-	.parent = &ipg_clk,
-	.enable_reg = MXC_CCM_CCGR1,
-	.enable_shift = MXC_CCM_CCGRx_CG12_OFFSET,
-	.enable = _clk_enable,
-	.disable = _clk_disable,
-	.flags = AHB_HIGH_SET_POINT | CPU_FREQ_TRIG_UPDATE,
-};
-
 static unsigned long _clk_cspi_get_rate(struct clk *clk)
 {
 	u32 reg, prediv, podf;
@@ -2427,6 +2416,29 @@ static unsigned long _clk_cspi_get_rate(struct clk *clk)
 	return clk_get_rate(clk->parent) / (prediv * podf);
 }
 
+static int _clk_cspi_set_rate(struct clk *clk, unsigned long rate)
+{
+	u32 reg, div, pre, post;
+	u32 parent_rate = clk_get_rate(clk->parent);
+
+	div = parent_rate / rate;
+	if (div == 0)
+		div++;
+	if (((parent_rate / div) != rate))
+		return -EINVAL;
+
+	__calc_pre_post_dividers(div, &pre, &post);
+
+	reg = __raw_readl(MXC_CCM_CSCDR2) &
+		~(MXC_CCM_CSCDR2_ECSPI_CLK_PODF_MASK |
+		MXC_CCM_CSCDR2_ECSPI_CLK_PRED_MASK);
+	reg |= (post - 1) << MXC_CCM_CSCDR2_ECSPI_CLK_PODF_OFFSET;
+	reg |= (pre - 1) << MXC_CCM_CSCDR2_ECSPI_CLK_PRED_OFFSET;
+	__raw_writel(reg, MXC_CCM_CSCDR2);
+
+	return 0;
+}
+
 static int _clk_cspi_set_parent(struct clk *clk, struct clk *parent)
 {
 	u32 reg, mux;
@@ -2444,6 +2456,7 @@ static struct clk cspi_main_clk = {
 	__INIT_CLK_DEBUG(cspi_main_clk)
 	.parent = &pll3_sw_clk,
 	.get_rate = _clk_cspi_get_rate,
+	.set_rate = _clk_cspi_set_rate,
 	.set_parent = _clk_cspi_set_parent,
 };
 
@@ -2771,7 +2784,7 @@ static struct clk ssi3_clk[] = {
 	 .id = 2,
 	 .parent = &aips_tz2_clk,
 #ifdef CONFIG_SND_MXC_SOC_IRAM
-	 .secondary = &emi_intr_clk,
+	 .secondary = &emi_intr_clk[0],
 #else
 	 .secondary = &emi_fast_clk,
 #endif
@@ -3506,6 +3519,7 @@ static struct clk sata_clk = {
 	.enable_reg = MXC_CCM_CCGR4,
 	.enable_shift = MXC_CCM_CCGRx_CG1_OFFSET,
 	.disable = _clk_disable,
+	.secondary = &emi_fast_clk,
 };
 
 static struct clk ieee_1588_clk = {
@@ -4009,6 +4023,7 @@ static struct clk vpu_clk[] = {
 	{
 	__INIT_CLK_DEBUG(vpu_clk_2)
 	 .parent = &emi_fast_clk,
+	 .secondary = &emi_intr_clk[0],
 	 }
 };
 
@@ -4030,23 +4045,6 @@ static struct clk lpsr_clk = {
 	.set_parent = _clk_lpsr_set_parent,
 };
 
-static unsigned long _clk_pgc_get_rate(struct clk *clk)
-{
-	u32 reg, div;
-
-	reg = __raw_readl(MXC_CCM_CSCDR1);
-	div = (reg & MXC_CCM_CSCDR1_PGC_CLK_PODF_MASK) >>
-	    MXC_CCM_CSCDR1_PGC_CLK_PODF_OFFSET;
-	div = 1 >> div;
-	return clk_get_rate(clk->parent) / div;
-}
-
-static struct clk pgc_clk = {
-	__INIT_CLK_DEBUG(pgc_clk)
-	.parent = &ipg_clk,
-	.get_rate = _clk_pgc_get_rate,
-};
-
 static unsigned long _clk_usb_get_rate(struct clk *clk)
 {
 	return 60000000;
@@ -4076,15 +4074,21 @@ static struct clk rtc_clk = {
 	.disable = _clk_disable,
 };
 
-static struct clk ata_clk = {
-	__INIT_CLK_DEBUG(ata_clk)
-	.parent = &ipg_clk,
-	.secondary = &spba_clk,
+static struct clk ata_clk[] = {
+	{
+	__INIT_CLK_DEBUG(ata_clk_0)
+	.parent = &spba_clk,
+	.secondary = &ata_clk[1],
 	.enable = _clk_enable,
 	.enable_reg = MXC_CCM_CCGR4,
 	.enable_shift = MXC_CCM_CCGRx_CG0_OFFSET,
 	.disable = _clk_disable,
 	.flags = AHB_HIGH_SET_POINT | CPU_FREQ_TRIG_UPDATE,
+	},
+	{
+	__INIT_CLK_DEBUG(ata_clk_1)
+	.parent = &emi_fast_clk,
+	}
 };
 
 static struct clk owire_clk = {
@@ -4157,7 +4161,7 @@ static struct clk sahara_clk[] = {
 	.secondary = &sahara_clk[2],
 	},
 	{
-	.parent = &scc_clk,
+	.parent = &scc_clk[0],
 	.secondary = &emi_fast_clk,
 	}
 };
@@ -4186,8 +4190,9 @@ static struct clk garb_clk = {
 	.disable = _clk_disable,
 };
 
-static struct clk gpu3d_clk = {
-	__INIT_CLK_DEBUG(gpu3d_clk)
+static struct clk gpu3d_clk[] = {
+	{
+	__INIT_CLK_DEBUG(gpu3d_clk_0)
 	.parent = &axi_a_clk,
 	.set_parent = _clk_gpu3d_set_parent,
 	.enable = _clk_enable,
@@ -4195,7 +4200,13 @@ static struct clk gpu3d_clk = {
 	.enable_shift = MXC_CCM_CCGRx_CG1_OFFSET,
 	.disable = _clk_disable,
 	.flags = AHB_HIGH_SET_POINT | CPU_FREQ_TRIG_UPDATE,
+	.secondary = &gpu3d_clk[1],
+	},
+	{
+	__INIT_CLK_DEBUG(gpu3d_clk_1)
+	.parent = &emi_fast_clk,
 	.secondary = &garb_clk,
+	}
 };
 
 static int _clk_gpu2d_set_parent(struct clk *clk, struct clk *parent)
@@ -4215,6 +4226,7 @@ static struct clk gpu2d_clk = {
 	__INIT_CLK_DEBUG(gpu2d_clk)
 	.parent = &axi_a_clk,
 	.set_parent = _clk_gpu2d_set_parent,
+	.secondary = &emi_fast_clk,
 	.enable = _clk_enable,
 	.enable_reg = MXC_CCM_CCGR6,
 	.enable_shift = MXC_CCM_CCGRx_CG7_OFFSET,
@@ -4447,6 +4459,7 @@ static struct clk_lookup lookups[] = {
 	_REGISTER_CLOCK(NULL, "vpu_core_clk", vpu_clk[1]),
 	_REGISTER_CLOCK(NULL, "nfc_clk", emi_enfc_clk),
 	_REGISTER_CLOCK("imx-sdma", NULL, sdma_clk[0]),
+	_REGISTER_CLOCK("imx-sdma", NULL, sdma_clk[1]),
 	_REGISTER_CLOCK(NULL, "ipu1_clk", ipu_clk[0]),
 	_REGISTER_CLOCK(NULL, "ipu1_di0_clk", ipu_di_clk[0]),
 	_REGISTER_CLOCK(NULL, "ipu1_di1_clk", ipu_di_clk[1]),
@@ -4487,10 +4500,10 @@ static struct clk_lookup lookups[] = {
 	_REGISTER_CLOCK("mxc_vpu.0", NULL, vpu_clk[0]),
 	_REGISTER_CLOCK(NULL, "lpsr_clk", lpsr_clk),
 	_REGISTER_CLOCK("mxc_rtc.0", NULL, rtc_clk),
-	_REGISTER_CLOCK("pata_fsl", NULL, ata_clk),
+	_REGISTER_CLOCK("pata_fsl", NULL, ata_clk[0]),
 	_REGISTER_CLOCK("mxc_w1.0", NULL, owire_clk),
 	_REGISTER_CLOCK(NULL, "sahara_clk", sahara_clk[0]),
-	_REGISTER_CLOCK(NULL, "gpu3d_clk", gpu3d_clk),
+	_REGISTER_CLOCK(NULL, "gpu3d_clk", gpu3d_clk[0]),
 	_REGISTER_CLOCK(NULL, "garb_clk", garb_clk),
 	_REGISTER_CLOCK(NULL, "gpu2d_clk", gpu2d_clk),
 	_REGISTER_CLOCK("mxc_scc.0", NULL, scc_clk[0]),
@@ -4657,6 +4670,13 @@ int __init mx51_clocks_init(unsigned long ckil, unsigned long osc, unsigned long
 	esdhc2_clk[0].get_rate = _clk_esdhc2_get_rate;
 	esdhc2_clk[0].set_rate = _clk_esdhc2_set_rate;
 
+	esdhc1_clk[0].flags = AHB_MED_SET_POINT | CPU_FREQ_TRIG_UPDATE;
+	esdhc2_clk[0].flags = AHB_MED_SET_POINT | CPU_FREQ_TRIG_UPDATE;
+	esdhc3_clk[0].flags = AHB_MED_SET_POINT | CPU_FREQ_TRIG_UPDATE;
+	esdhc4_clk[0].flags = AHB_MED_SET_POINT | CPU_FREQ_TRIG_UPDATE;
+
+	ata_clk[1].secondary = &ahb_max_clk;
+
 	clk_tree_init();
 
 	for (i = 0; i < ARRAY_SIZE(lookups); i++) {
@@ -4731,7 +4751,7 @@ int __init mx51_clocks_init(unsigned long ckil, unsigned long osc, unsigned long
 	 */
 	clk_set_parent(&vpu_clk[0], &axi_a_clk);
 	clk_set_parent(&vpu_clk[1], &axi_a_clk);
-	clk_set_parent(&gpu3d_clk, &axi_a_clk);
+	clk_set_parent(&gpu3d_clk[0], &axi_a_clk);
 	clk_set_parent(&gpu2d_clk, &axi_a_clk);
 
 	/* move cspi to 24MHz */
@@ -4971,6 +4991,8 @@ int __init mx53_clocks_init(unsigned long ckil, unsigned long osc, unsigned long
 
 	vpu_clk[2].secondary = &emi_intr_clk[0];
 
+	ata_clk[1].secondary = &tmax3_clk;
+
 #if defined(CONFIG_USB_STATIC_IRAM) \
     || defined(CONFIG_USB_STATIC_IRAM_PPH)
 	usboh3_clk[1].secondary = &emi_intr_clk[1];
@@ -5132,7 +5154,7 @@ int __init mx53_clocks_init(unsigned long ckil, unsigned long osc, unsigned long
 
 	clk_set_parent(&arm_axi_clk, &axi_b_clk);
 	clk_set_parent(&ipu_clk[0], &axi_b_clk);
-	clk_set_parent(&gpu3d_clk, &axi_b_clk);
+	clk_set_parent(&gpu3d_clk[0], &axi_b_clk);
 	clk_set_parent(&gpu2d_clk, &axi_b_clk);
 
 	clk_set_parent(&emi_slow_clk, &ahb_clk);
diff --git a/arch/arm/mach-mx5/clock_mx50.c b/arch/arm/mach-mx5/clock_mx50.c
index 4d2c2b9b5e81..aa0e6b4ed2b5 100644
--- a/arch/arm/mach-mx5/clock_mx50.c
+++ b/arch/arm/mach-mx5/clock_mx50.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2010-2011 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright (C) 2010-2012 Freescale Semiconductor, Inc. All Rights Reserved.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -3407,9 +3407,7 @@ static void clk_tree_init(void)
 int __init mx50_clocks_init(unsigned long ckil, unsigned long osc, unsigned long ckih1)
 {
 	__iomem void *base;
-	int i = 0, j = 0, reg;
-	int wp_cnt = 0;
-	u32 pll1_rate;
+	int i = 0, reg;
 
 	pll1_base = MX50_DPLL1_BASE;
 	pll2_base = MX50_DPLL2_BASE;
diff --git a/arch/arm/mach-mx5/cpu.c b/arch/arm/mach-mx5/cpu.c
index 04625af15039..334423f0fe71 100644
--- a/arch/arm/mach-mx5/cpu.c
+++ b/arch/arm/mach-mx5/cpu.c
@@ -1,5 +1,5 @@
 /*
- * Copyright 2008-2011 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2008-2012 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
@@ -118,7 +118,7 @@ late_initcall(mx51_neon_fixup);
 
 static int get_mx53_srev(void)
 {
-	void __iomem *iim_base = MX51_IO_ADDRESS(MX53_IIM_BASE_ADDR);
+	void __iomem *iim_base = MX53_IO_ADDRESS(MX53_IIM_BASE_ADDR);
 	u32 rev = readl(iim_base + IIM_SREV) & 0xff;
 
 	switch (rev) {
@@ -260,6 +260,17 @@ static int __init post_cpu_init(void)
 	reg = 0x8;
 	__raw_writel(reg, arm_plat_base + CORTEXA8_PLAT_AMC);
 
+	if (cpu_is_mx53()) {
+		/*Allow for automatic gating of the EMI internal clock.
+		 * If this is done, emi_intr CCGR bits should be set to 11.
+		 */
+		base = ioremap(MX53_M4IF_BASE_ADDR, SZ_4K);
+		reg = __raw_readl(base + 0x8c);
+		reg &= ~0x1;
+		__raw_writel(reg, base + 0x8c);
+		iounmap(base);
+	}
+
 	if (cpu_is_mx50())
 		init_ddr_settings();
 
diff --git a/arch/arm/mach-mx5/cpu_op-mx53.c b/arch/arm/mach-mx5/cpu_op-mx53.c
index debe1bc411d4..2ec5433b7e26 100644
--- a/arch/arm/mach-mx5/cpu_op-mx53.c
+++ b/arch/arm/mach-mx5/cpu_op-mx53.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2011 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright (C) 2011-2012 Freescale Semiconductor, Inc. All Rights Reserved.
  */
 
 /*
@@ -49,7 +49,7 @@ static struct dvfs_op dvfs_core_setpoint_aec[] = {
 static struct dvfs_op dvfs_core_setpoint_ces_1_2G[] = {
 			{33, 25, 33, 10, 10, 0x08}, /*1_2GHz*/
 			{30, 18, 33, 20, 10, 0x08}, /* 800MHz */
-			{25, 8, 33, 20, 10, 0x08}, /* 400MHz */
+			{25, 0, 33, 20, 10, 0x08}, /* 400MHz */
 			{28, 8, 33, 20, 30, 0x08}, /* 400MHZ, 133MHz */
 			{29, 0, 33, 20, 10, 0x08},}; /* 400MHZ, 50MHz. */
 
@@ -57,7 +57,7 @@ static struct dvfs_op dvfs_core_setpoint_ces_1_2G[] = {
 static struct dvfs_op dvfs_core_setpoint_ces[] = {
 			{33, 25, 33, 10, 10, 0x08}, /*1GHz*/
 			{30, 18, 33, 20, 10, 0x08}, /* 800MHz */
-			{25, 8, 33, 20, 10, 0x08}, /* 400MHz */
+			{25, 0, 33, 20, 10, 0x08}, /* 400MHz */
 			{28, 8, 33, 20, 30, 0x08}, /* 400MHz, 133MHz */
 			{29, 0, 33, 20, 10, 0x08},}; /* 400MHz, 50MHz. */
 
@@ -71,7 +71,7 @@ static struct cpu_op cpu_op_aec[] = {
 	 .mfd = 2,
 	 .mfn = 1,
 	 .cpu_podf = 0,
-	 .cpu_voltage = 1050000,},
+	 .cpu_voltage = 1100000,},
 };
 
 /* working point for consumer 1G*/
@@ -84,7 +84,7 @@ static struct cpu_op cpu_op_ces[] = {
 	 .mfd = 11,
 	 .mfn = 5,
 	 .cpu_podf = 0,
-	 .cpu_voltage = 1200000,},
+	 .cpu_voltage = 1250000,},
 	{
 	 .pll_rate = 800000000,
 	 .cpu_rate = 800000000,
@@ -93,17 +93,16 @@ static struct cpu_op cpu_op_ces[] = {
 	 .mfd = 2,
 	 .mfn = 1,
 	 .cpu_podf = 0,
-	 .cpu_voltage = 1050000,},
+	 .cpu_voltage = 1100000,},
 	 {
 	  .pll_rate = 800000000,
 	  .cpu_rate = 400000000,
-	  .cpu_podf = 1,
-	  .cpu_voltage = 950000,},
-	{
-	 .pll_rate = 800000000,
-	 .cpu_rate = 160000000,
-	 .cpu_podf = 4,
-	 .cpu_voltage = 900000,},
+	 .pdf = 0,
+	 .mfi = 8,
+	 .mfd = 2,
+	 .mfn = 1,
+	 .cpu_podf = 1,
+	 .cpu_voltage = 950000,},
 };
 
 /* working point for consumer 1.2G*/
@@ -125,7 +124,7 @@ static struct cpu_op cpu_op_ces_1_2g[] = {
 	 .mfd = 11,
 	 .mfn = 5,
 	 .cpu_podf = 0,
-	 .cpu_voltage = 1200000,},
+	 .cpu_voltage = 1250000,},
 	{
 	 .pll_rate = 800000000,
 	 .cpu_rate = 800000000,
@@ -134,17 +133,12 @@ static struct cpu_op cpu_op_ces_1_2g[] = {
 	 .mfd = 2,
 	 .mfn = 1,
 	 .cpu_podf = 0,
-	 .cpu_voltage = 1050000,},
+	 .cpu_voltage = 1100000,},
 	 {
 	  .pll_rate = 800000000,
 	  .cpu_rate = 400000000,
 	  .cpu_podf = 1,
 	  .cpu_voltage = 950000,},
-	{
-	 .pll_rate = 800000000,
-	 .cpu_rate = 160000000,
-	 .cpu_podf = 4,
-	 .cpu_voltage = 900000,},
 };
 
 static struct dvfs_op *mx53_get_dvfs_core_table(int *wp)
diff --git a/arch/arm/mach-mx5/crm_regs.h b/arch/arm/mach-mx5/crm_regs.h
index 851434d75838..faa641460479 100644
--- a/arch/arm/mach-mx5/crm_regs.h
+++ b/arch/arm/mach-mx5/crm_regs.h
@@ -1,5 +1,5 @@
 /*
- * Copyright 2008-2011 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2008-2012 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
@@ -492,6 +492,10 @@
 /* MX53 */
 #define MXC_CCM_CSCDR2_ASRC_CLK_PODF_OFFSET	(9)
 #define MXC_CCM_CSCDR2_ASRC_CLK_PODF_MASK		(0x3F << 9)
+#define MXC_CCM_CSCDR2_ECSPI_CLK_PODF_OFFSET	(19)
+#define MXC_CCM_CSCDR2_ECSPI_CLK_PODF_MASK		(0x3F << 19)
+#define MXC_CCM_CSCDR2_ECSPI_CLK_PRED_OFFSET	(25)
+#define MXC_CCM_CSCDR2_ECSPI_CLK_PRED_MASK		(0x7 << 25)
 #define MXC_CCM_CSCDR2_IEEE_CLK_PRED_OFFSET		(6)
 #define MXC_CCM_CSCDR2_IEEE_CLK_PRED_MASK		(0x7 << 6)
 #define MXC_CCM_CSCDR2_IEEE_CLK_PODF_OFFSET	(0)
@@ -787,18 +791,16 @@
 #define MXC_SRPG_MEGAMIX_BASE	(MXC_GPC_BASE + 0x2E0)
 #define MXC_SRPG_EMI_BASE	(MXC_GPC_BASE + 0x300)
 
-/* CORTEXA8 platform */
-extern void __iomem *arm_plat_base;
-#define MXC_CORTEXA8_BASE		(arm_plat_base)
-#define MXC_CORTEXA8_PLAT_PVID		(arm_plat_base + 0x0)
-#define MXC_CORTEXA8_PLAT_GPC		(arm_plat_base + 0x4)
-#define MXC_CORTEXA8_PLAT_PIC		(arm_plat_base + 0x8)
-#define MXC_CORTEXA8_PLAT_LPC		(arm_plat_base + 0xC)
-#define MXC_CORTEXA8_PLAT_NEON_LPC	(arm_plat_base + 0x10)
-#define MXC_CORTEXA8_PLAT_ICGC		(arm_plat_base + 0x14)
-#define MXC_CORTEXA8_PLAT_AMC		(arm_plat_base + 0x18)
-#define MXC_CORTEXA8_PLAT_NMC		(arm_plat_base + 0x20)
-#define MXC_CORTEXA8_PLAT_NMS		(arm_plat_base + 0x24)
+/* CORTEXA8 platform offsets */
+#define MXC_CORTEXA8_PLAT_PVID		(0x0)
+#define MXC_CORTEXA8_PLAT_GPC		(0x4)
+#define MXC_CORTEXA8_PLAT_PIC		(0x8)
+#define MXC_CORTEXA8_PLAT_LPC		(0xC)
+#define MXC_CORTEXA8_PLAT_NEON_LPC	(0x10)
+#define MXC_CORTEXA8_PLAT_ICGC		(0x14)
+#define MXC_CORTEXA8_PLAT_AMC		(0x18)
+#define MXC_CORTEXA8_PLAT_NMC		(0x20)
+#define MXC_CORTEXA8_PLAT_NMS		(0x24)
 
 /* DVFS CORE */
 #define MXC_DVFSTHRS		(MXC_DVFS_CORE_BASE + 0x00)
diff --git a/arch/arm/mach-mx5/imx_bt_rfkill.c b/arch/arm/mach-mx5/imx_bt_rfkill.c
index 89ead0279fef..3c5a9d2caa6a 100644
--- a/arch/arm/mach-mx5/imx_bt_rfkill.c
+++ b/arch/arm/mach-mx5/imx_bt_rfkill.c
@@ -109,6 +109,8 @@ static int imx_bt_rfkill_probe(struct platform_device *dev)
 		goto error_rfk_alloc;
 	}
 
+	rfkill_set_led_trigger_name(rfk, "mxc_bt");
+
 	rc = rfkill_register(rfk);
 	if (rc)
 		goto error_rfkill;
diff --git a/arch/arm/mach-mx5/mx53_smd_pmic_da9053.c b/arch/arm/mach-mx5/mx53_smd_pmic_da9053.c
index df93412a8274..1149638f3dcc 100644
--- a/arch/arm/mach-mx5/mx53_smd_pmic_da9053.c
+++ b/arch/arm/mach-mx5/mx53_smd_pmic_da9053.c
@@ -39,7 +39,7 @@
 #include <mach/iomux-mx53.h>
 #include <mach/gpio.h>
 
-#define DA9052_LDO(max, min, rname, suspend_mv) \
+#define DA9052_LDO(max, min, rname, suspend_mv, num_consumers, consumers) \
 {\
 	.constraints = {\
 		.name		= (rname), \
@@ -55,42 +55,53 @@
 			.disabled = 0, \
 		}, \
 	},\
+	.num_consumer_supplies = (num_consumers), \
+	.consumer_supplies = (consumers), \
 }
 
-/* currently the suspend_mv field here takes no effects for DA9053
+/* CPU */
+static struct regulator_consumer_supply sw1_consumers[] = {
+	{
+		.supply = "cpu_vddgp",
+	}
+};
+
+/* currently the suspend_mv here takes no effects for DA9053
 preset-voltage have to be done in the latest stage during
 suspend*/
 static struct regulator_init_data da9052_regulators_init[] = {
 	DA9052_LDO(DA9052_LDO1_VOLT_UPPER,
-		DA9052_LDO1_VOLT_LOWER, "DA9052_LDO1", 1300),
+		DA9052_LDO1_VOLT_LOWER, "DA9052_LDO1", 1300, 0, NULL),
 	DA9052_LDO(DA9052_LDO2_VOLT_UPPER,
-		DA9052_LDO2_VOLT_LOWER, "DA9052_LDO2", 1300),
+		DA9052_LDO2_VOLT_LOWER, "DA9052_LDO2", 1300, 0, NULL),
 	DA9052_LDO(DA9052_LDO34_VOLT_UPPER,
-		DA9052_LDO34_VOLT_LOWER, "DA9052_LDO3", 3300),
+		DA9052_LDO34_VOLT_LOWER, "DA9052_LDO3", 3300, 0, NULL),
 	DA9052_LDO(DA9052_LDO34_VOLT_UPPER,
-		DA9052_LDO34_VOLT_LOWER, "DA9052_LDO4", 2775),
+		DA9052_LDO34_VOLT_LOWER, "DA9052_LDO4", 2775, 0, NULL),
 	DA9052_LDO(DA9052_LDO567810_VOLT_UPPER,
-		DA9052_LDO567810_VOLT_LOWER, "DA9052_LDO5", 1300),
+		DA9052_LDO567810_VOLT_LOWER, "DA9052_LDO5", 1300, 0, NULL),
 	DA9052_LDO(DA9052_LDO567810_VOLT_UPPER,
-		DA9052_LDO567810_VOLT_LOWER, "DA9052_LDO6", 1200),
+		DA9052_LDO567810_VOLT_LOWER, "DA9052_LDO6", 1200, 0, NULL),
 	DA9052_LDO(DA9052_LDO567810_VOLT_UPPER,
-		DA9052_LDO567810_VOLT_LOWER, "DA9052_LDO7", 2750),
+		DA9052_LDO567810_VOLT_LOWER, "DA9052_LDO7", 2750, 0, NULL),
 	DA9052_LDO(DA9052_LDO567810_VOLT_UPPER,
-		DA9052_LDO567810_VOLT_LOWER, "DA9052_LDO8", 1800),
+		DA9052_LDO567810_VOLT_LOWER, "DA9052_LDO8", 1800, 0, NULL),
 	DA9052_LDO(DA9052_LDO9_VOLT_UPPER,
-		DA9052_LDO9_VOLT_LOWER, "DA9052_LDO9", 2500),
+		DA9052_LDO9_VOLT_LOWER, "DA9052_LDO9", 2500, 0, NULL),
 	DA9052_LDO(DA9052_LDO567810_VOLT_UPPER,
-		DA9052_LDO567810_VOLT_LOWER, "DA9052_LDO10", 1200),
+		DA9052_LDO567810_VOLT_LOWER, "DA9052_LDO10", 1200, 0, NULL),
 
 	/* BUCKS */
 	DA9052_LDO(DA9052_BUCK_CORE_PRO_VOLT_UPPER,
-		DA9052_BUCK_CORE_PRO_VOLT_LOWER, "DA9052_BUCK_CORE", 850),
+		DA9052_BUCK_CORE_PRO_VOLT_LOWER, "DA9052_BUCK_CORE", 850,
+		ARRAY_SIZE(sw1_consumers), sw1_consumers),
 	DA9052_LDO(DA9052_BUCK_CORE_PRO_VOLT_UPPER,
-		DA9052_BUCK_CORE_PRO_VOLT_LOWER, "DA9052_BUCK_PRO", 950),
+		DA9052_BUCK_CORE_PRO_VOLT_LOWER, "DA9052_BUCK_PRO", 950,
+		0, NULL),
 	DA9052_LDO(DA9052_BUCK_MEM_VOLT_UPPER,
-		DA9052_BUCK_MEM_VOLT_LOWER, "DA9052_BUCK_MEM", 1500),
+		DA9052_BUCK_MEM_VOLT_LOWER, "DA9052_BUCK_MEM", 1500, 0, NULL),
 	DA9052_LDO(DA9052_BUCK_PERI_VOLT_UPPER,
-		DA9052_BUCK_PERI_VOLT_LOWER, "DA9052_BUCK_PERI", 2500)
+		DA9052_BUCK_PERI_VOLT_LOWER, "DA9052_BUCK_PERI", 2500, 0, NULL)
 };
 
 #define MX53_SMD_WiFi_BT_PWR_EN		IMX_GPIO_NR(3, 10)	/*GPIO_3_10 */
@@ -122,6 +133,7 @@ static struct platform_device wifi_bt_reg_device = {
 static struct regulator_consumer_supply sgtl5000_consumer[] = {
 	REGULATOR_SUPPLY("VDDA", NULL),
 	REGULATOR_SUPPLY("VDDIO", NULL),
+	REGULATOR_SUPPLY("VDDD", NULL),
 };
 
 static struct regulator_init_data sgtl5000_reg_initdata = {
diff --git a/arch/arm/mach-mx5/pm.c b/arch/arm/mach-mx5/pm.c
index 9240b7aec88d..6968cef0d7c7 100644
--- a/arch/arm/mach-mx5/pm.c
+++ b/arch/arm/mach-mx5/pm.c
@@ -1,5 +1,5 @@
 /*
- *  Copyright (C) 2008-2011 Freescale Semiconductor, Inc. All Rights Reserved.
+ *  Copyright (C) 2008-2012 Freescale Semiconductor, Inc. All Rights Reserved.
  */
 
 /*
@@ -46,9 +46,13 @@ static struct cpu_op *cpu_op_tbl;
 static int cpu_op_nr;
 static struct clk *cpu_clk;
 static struct mxc_pm_platform_data *pm_data;
+static int databahn_mode;
+
+static void __iomem *pll1_base;
 
 #if defined(CONFIG_CPU_FREQ)
 static int org_freq;
+extern int cpufreq_suspended;
 extern int set_cpu_freq(int wp);
 #endif
 
@@ -58,6 +62,7 @@ struct clk *gpc_dvfs_clk;
 extern void cpu_do_suspend_workaround(u32 sdclk_iomux_addr);
 extern void mx50_suspend(u32 databahn_addr);
 extern struct cpu_op *(*get_cpu_op)(int *wp);
+extern void __iomem *ccm_base;
 extern void __iomem *databahn_base;
 extern void da9053_suspend_cmd(void);
 extern void da9053_resume_dump(void);
@@ -65,7 +70,7 @@ extern void pm_da9053_i2c_init(u32 base_addr);
 
 extern int iram_ready;
 void *suspend_iram_base;
-void (*suspend_in_iram)(void *sdclk_iomux_addr) = NULL;
+void (*suspend_in_iram)(void *param1, void *param2, void* param3) = NULL;
 void __iomem *suspend_param1;
 
 #define TZIC_WAKEUP0_OFFSET            0x0E00
@@ -74,23 +79,6 @@ void __iomem *suspend_param1;
 #define TZIC_WAKEUP3_OFFSET            0x0E0C
 #define GPIO7_0_11_IRQ_BIT		(0x1<<11)
 
-static void mx53_smd_loco_irq_wake_fixup(void)
-{
-	void __iomem *tzic_base;
-	tzic_base = ioremap(MX53_TZIC_BASE_ADDR, SZ_4K);
-	if (NULL == tzic_base) {
-		pr_err("fail to map MX53_TZIC_BASE_ADDR\n");
-		return;
-	}
-	__raw_writel(0, tzic_base + TZIC_WAKEUP0_OFFSET);
-	__raw_writel(0, tzic_base + TZIC_WAKEUP1_OFFSET);
-	__raw_writel(0, tzic_base + TZIC_WAKEUP2_OFFSET);
-	/* only enable irq wakeup for da9053 */
-	__raw_writel(GPIO7_0_11_IRQ_BIT, tzic_base + TZIC_WAKEUP3_OFFSET);
-	iounmap(tzic_base);
-	pr_debug("only da9053 irq is wakeup-enabled\n");
-}
-
 static int mx5_suspend_enter(suspend_state_t state)
 {
 	if (gpc_dvfs_clk == NULL)
@@ -112,14 +100,38 @@ static int mx5_suspend_enter(suspend_state_t state)
 		return -EAGAIN;
 
 	if (state == PM_SUSPEND_MEM) {
-		local_flush_tlb_all();
-		flush_cache_all();
+		if (!cpu_is_mx53()) {
+			local_flush_tlb_all();
+			flush_cache_all();
+		}
 
 		if (pm_data && pm_data->suspend_enter)
 			pm_data->suspend_enter();
-
-		suspend_in_iram(suspend_param1);
-
+		if (cpu_is_mx51() || cpu_is_mx53()) {
+			/* Run the suspend code from iRAM. */
+			suspend_in_iram(suspend_param1, NULL, NULL);
+
+			if (!cpu_is_mx53()) {
+				/*clear the EMPGC0/1 bits */
+				__raw_writel(0, MXC_SRPG_EMPGC0_SRPGCR);
+				__raw_writel(0, MXC_SRPG_EMPGC1_SRPGCR);
+			}
+		} else {
+			if (cpu_is_mx50()) {
+				/* Store the LPM mode of databanhn */
+				databahn_mode = __raw_readl(
+					databahn_base + DATABAHN_CTL_REG20);
+
+				/* Suspend now. */
+				suspend_in_iram(databahn_base,
+						ccm_base, pll1_base);
+
+				/* Restore the LPM databahn_mode. */
+				__raw_writel(databahn_mode,
+					databahn_base + DATABAHN_CTL_REG20);
+
+			}
+		}
 		if (pm_data && pm_data->suspend_exit)
 			pm_data->suspend_exit();
 	} else {
@@ -137,7 +149,7 @@ static int mx5_suspend_enter(suspend_state_t state)
 static int mx5_suspend_prepare(void)
 {
 #if defined(CONFIG_CPU_FREQ)
-#define MX53_SUSPEND_CPU_WP 1000000000
+#define MX53_SUSPEND_CPU_WP 400000000
 	struct cpufreq_freqs freqs;
 	u32 suspend_wp = 0;
 	org_freq = clk_get_rate(cpu_clk);
@@ -155,6 +167,7 @@ static int mx5_suspend_prepare(void)
 	freqs.cpu = 0;
 	freqs.flags = 0;
 
+	cpufreq_suspended = 1;
 	if (clk_get_rate(cpu_clk) != cpu_op_tbl[suspend_wp].cpu_rate) {
 		set_cpu_freq(cpu_op_tbl[suspend_wp].cpu_rate);
 		cpufreq_notify_transition(&freqs, CPUFREQ_PRECHANGE);
@@ -177,6 +190,7 @@ static void mx5_suspend_finish(void)
 	freqs.cpu = 0;
 	freqs.flags = 0;
 
+	cpufreq_suspended = 0;
 
 	if (org_freq != clk_get_rate(cpu_clk)) {
 		set_cpu_freq(org_freq);
@@ -221,34 +235,49 @@ static struct platform_driver mx5_pm_driver = {
 	.probe = mx5_pm_probe,
 };
 
+#define SUSPEND_ID_MX51 1
+#define SUSPEND_ID_MX53 3
+#define SUSPEND_ID_NONE 4
 static int __init pm_init(void)
 {
-	unsigned long iram_paddr, cpaddr;
+	unsigned long iram_paddr;
+	void *cpaddr;
 
 	pr_info("Static Power Management for Freescale i.MX5\n");
 	if (platform_driver_register(&mx5_pm_driver) != 0) {
 		printk(KERN_ERR "mx5_pm_driver register failed\n");
 		return -ENODEV;
 	}
+	if (cpu_is_mx51())
+		pll1_base = MX51_IO_ADDRESS(MX51_PLL1_BASE_ADDR);
+	else if (cpu_is_mx53())
+		pll1_base = MX53_IO_ADDRESS(MX53_PLL1_BASE_ADDR);
+	else if (cpu_is_mx50())
+		pll1_base = MX50_IO_ADDRESS(MX50_PLL1_BASE_ADDR);
+
+	suspend_param1 = 0;
 	suspend_set_ops(&mx5_suspend_ops);
 	/* Move suspend routine into iRAM */
 	cpaddr = iram_alloc(SZ_4K, &iram_paddr);
 	/* Need to remap the area here since we want the memory region
 		 to be executable. */
 	suspend_iram_base = __arm_ioremap(iram_paddr, SZ_4K,
-					  MT_HIGH_VECTORS);
-	pr_info("cpaddr = %x suspend_iram_base=%x\n", cpaddr, suspend_iram_base);
+					  MT_MEMORY_NONCACHED);
+	pr_info("cpaddr = %x suspend_iram_base=%x\n", (unsigned int)cpaddr,
+					(unsigned int)suspend_iram_base);
 
 	if (cpu_is_mx51() || cpu_is_mx53()) {
-		suspend_param1 = MX51_IO_ADDRESS(MX51_IOMUXC_BASE_ADDR + 0x4b8);
-		memcpy(cpaddr, cpu_do_suspend_workaround,
+		suspend_param1 =
+			cpu_is_mx51() ? (void *)SUSPEND_ID_MX51: \
+					(void *)SUSPEND_ID_MX53;
+		memcpy(suspend_iram_base, cpu_do_suspend_workaround,
 			SZ_4K);
 	} else if (cpu_is_mx50()) {
 		/*
 		 * Need to run the suspend code from IRAM as the DDR needs
 		 * to be put into self refresh mode manually.
 		 */
-		memcpy(cpaddr, mx50_suspend, SZ_4K);
+		memcpy(suspend_iram_base, mx50_suspend, SZ_4K);
 
 		suspend_param1 = databahn_base;
 	}
diff --git a/arch/arm/mach-mx5/suspend.S b/arch/arm/mach-mx5/suspend.S
index c7937ec94d9f..a1059124664b 100644
--- a/arch/arm/mach-mx5/suspend.S
+++ b/arch/arm/mach-mx5/suspend.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2008-2011 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright (C) 2008-2012 Freescale Semiconductor, Inc. All Rights Reserved.
  */
 /*
  * The code contained herein is licensed under the GNU General Public
@@ -11,11 +11,72 @@
  */
 
 #include <linux/linkage.h>
+#include <mach/hardware.h>
+#include <mach/mx51.h>
+#include <mach/mx53.h>
 
 #define ARM_CTRL_DCACHE  1 << 2
 #define ARM_CTRL_ICACHE  1 << 12
 #define ARM_AUXCR_L2EN   1 << 1
 
+.macro PM_SET_BACKUP_REG, addr, num
+	ldr r2, =\addr
+	ldr r2, [r1, r2]
+	str r2, [r3, #(\num * 4)]
+.endm
+
+.macro PM_SET_HIGHZ_PAD, addr
+	ldr r2, =\addr
+	str r4, [r1, r2]
+.endm
+
+.macro PM_SET_RESTORE_REG, addr, num
+    ldr     r4, [r3, #(\num * 4)]
+    ldr     r2, =\addr
+    str     r4, [r1, r2]
+.endm
+
+.macro PM_SET_ADDR_REG, addr, reg
+	mov	\reg, #(\addr & 0x000000FF)
+	orr	\reg, \reg, #(\addr & 0x0000FF00)
+	orr	\reg, \reg, #(\addr & 0x00FF0000)
+	orr	\reg, \reg, #(\addr & 0xFF000000)
+.endm
+
+#define SUSPEND_ID_MX51 1
+#define SUSPEND_ID_MX53 3
+#define SUSPEND_ID_NONE 4
+
+#define MX51_DRAM_SDCLK_PAD_CTRL_ADDR MX51_IO_ADDRESS(0x73FA84B8)
+#define MX51_CCM_BASE MX51_IO_ADDRESS(0x73fd4000)
+#define MX51_PLL1_BASE MX51_IO_ADDRESS(0x83f80000)
+
+#define M4IF_MCR0_OFFSET	(0x008C)
+#define M4IF_MCR0_FDVFS		(0x1 << 11)
+#define M4IF_MCR0_FDVACK	(0x1 << 27)
+#define IOMUXC_BASE_ADDR_VIRT	MX53_IO_ADDRESS(MX53_IOMUXC_BASE_ADDR)
+#define M4IF_BASE_ADDR_VIRT	MX53_IO_ADDRESS(MX53_M4IF_BASE_ADDR)
+
+#define IOMUXC_SW_PAD_CTL_PAD_DRAM_DQM3		0x554
+#define IOMUXC_SW_PAD_CTL_PAD_DRAM_SDQS3	0x558
+#define IOMUXC_SW_PAD_CTL_PAD_DRAM_DQM2		0x560
+#define IOMUXC_SW_PAD_CTL_PAD_DRAM_SDODT1	0x564
+#define IOMUXC_SW_PAD_CTL_PAD_DRAM_SDQS2	0x568
+#define IOMUXC_SW_PAD_CTL_PAD_DRAM_SDCLK_1	0x570
+#define IOMUXC_SW_PAD_CTL_PAD_DRAM_CAS		0x574
+#define IOMUXC_SW_PAD_CTL_PAD_DRAM_SDCLK_0	0x578
+#define IOMUXC_SW_PAD_CTL_PAD_DRAM_SDQS0	0x57c
+#define IOMUXC_SW_PAD_CTL_PAD_DRAM_SDODT0	0x580
+#define IOMUXC_SW_PAD_CTL_PAD_DRAM_DQM0		0x584
+#define IOMUXC_SW_PAD_CTL_PAD_DRAM_RAS		0x588
+#define IOMUXC_SW_PAD_CTL_PAD_DRAM_SDQS1	0x590
+#define IOMUXC_SW_PAD_CTL_PAD_DRAM_DQM1		0x594
+#define IOMUXC_SW_PAD_CTL_GRP_ADDDS			0x6f0
+#define IOMUXC_SW_PAD_CTL_GRP_B0DS			0x718
+#define IOMUXC_SW_PAD_CTL_GRP_B1DS			0x71c
+#define IOMUXC_SW_PAD_CTL_GRP_CTLDS			0x720
+#define IOMUXC_SW_PAD_CTL_GRP_B2DS			0x728
+#define IOMUXC_SW_PAD_CTL_GRP_B3DS			0x72c
 
 /*
  *	cpu_do_suspend_workaround()
@@ -27,7 +88,9 @@
 ENTRY(cpu_do_suspend_workaround)
     stmfd   sp!, {r4,r5,r6,r7,r9,r10,r11}     @ Save registers
 
-   mov    r6, r0                       @save iomux address
+    mov    r6, r0                       @save iomux address
+    cmp     r6, #SUSPEND_ID_MX51
+    bne     mx53_start               @ don't disable cache on imx53
     /* Disable L1 caches */
     mrc     p15, 0, r0, c1, c0, 0    @ R0 = system control reg
     bic     r0, r0, #ARM_CTRL_ICACHE @ Disable ICache
@@ -78,26 +141,253 @@ FinishedClean:
     bic     r0, r0, #ARM_AUXCR_L2EN @ Disable L2 cache
     mcr     p15, 0, r0, c1, c0, 1   @ Update aux control reg
 
-#if 0
+mx53_start:
+    /* Do nothing for DDR */
+    cmp     r6, #SUSPEND_ID_NONE
+    beq     mx5x_wfi
 	/*Set the DDR drive strength to low */
-	ldr        r10, [r6]
-	and       r10, r10, #0xF1        @ clear bits 2-1
-	str        r10, [r6]
-#endif
+    cmp     r6, #SUSPEND_ID_MX51
+    bne     mx53_reduce_ddr_drive_strength
+	ldr r0, =MX51_DRAM_SDCLK_PAD_CTRL_ADDR
+	ldr	r1, [r0]
+	str r1, __mx5x_temp_stack
+	bic r1, r1, #0x6
+	str r1, [r0]
+mx53_reduce_ddr_drive_strength:
+    cmp     r6, #SUSPEND_ID_MX53
+    bne     mx5x_wfi
+
+mx53_force_ddr_selfrefresh:
+    /* Point R0 at M4IF register set */
+    ldr     r0, =M4IF_BASE_ADDR_VIRT
+
+    /* Point R1 at IOMUX register set */
+    ldr     r1, =IOMUXC_BASE_ADDR_VIRT
+
+    /* Point R3 at temporary IRAM storage for DDR pad config */
+    adr     r3, __mx5x_temp_stack
+
+	PM_SET_BACKUP_REG IOMUXC_SW_PAD_CTL_PAD_DRAM_DQM3,		0
+	PM_SET_BACKUP_REG IOMUXC_SW_PAD_CTL_PAD_DRAM_SDQS3, 	1
+	PM_SET_BACKUP_REG IOMUXC_SW_PAD_CTL_PAD_DRAM_DQM2,		2
+	PM_SET_BACKUP_REG IOMUXC_SW_PAD_CTL_PAD_DRAM_SDODT1,	3
+	PM_SET_BACKUP_REG IOMUXC_SW_PAD_CTL_PAD_DRAM_SDQS2, 	4
+	PM_SET_BACKUP_REG IOMUXC_SW_PAD_CTL_PAD_DRAM_SDCLK_1,	5
+	PM_SET_BACKUP_REG IOMUXC_SW_PAD_CTL_PAD_DRAM_CAS,		6
+	PM_SET_BACKUP_REG IOMUXC_SW_PAD_CTL_PAD_DRAM_SDCLK_0,	7
+	PM_SET_BACKUP_REG IOMUXC_SW_PAD_CTL_PAD_DRAM_SDQS0,		8
+	PM_SET_BACKUP_REG IOMUXC_SW_PAD_CTL_PAD_DRAM_SDODT0,	9
+	PM_SET_BACKUP_REG IOMUXC_SW_PAD_CTL_PAD_DRAM_DQM0,		10
+	PM_SET_BACKUP_REG IOMUXC_SW_PAD_CTL_PAD_DRAM_RAS,		11
+	PM_SET_BACKUP_REG IOMUXC_SW_PAD_CTL_PAD_DRAM_SDQS1, 	12
+	PM_SET_BACKUP_REG IOMUXC_SW_PAD_CTL_PAD_DRAM_DQM1,		13
+	PM_SET_BACKUP_REG IOMUXC_SW_PAD_CTL_GRP_ADDDS,			14
+	PM_SET_BACKUP_REG IOMUXC_SW_PAD_CTL_GRP_B0DS,			15
+	PM_SET_BACKUP_REG IOMUXC_SW_PAD_CTL_GRP_B1DS, 			16
+	PM_SET_BACKUP_REG IOMUXC_SW_PAD_CTL_GRP_CTLDS,			17
+	PM_SET_BACKUP_REG IOMUXC_SW_PAD_CTL_GRP_B2DS, 			18
+	PM_SET_BACKUP_REG IOMUXC_SW_PAD_CTL_GRP_B3DS, 			19
+
+    /* Set FDVFS bit of M4IF_MCR0 to request DDR to enter self-refresh */
+    ldr     r2,[r0, #M4IF_MCR0_OFFSET]
+    orr     r2, r2, #M4IF_MCR0_FDVFS
+    str     r2,[r0, #M4IF_MCR0_OFFSET]
+
+    /* Poll FDVACK bit of M4IF_MCR to wait for DDR to enter self-refresh */
+WAIT_SR_ACK:
+    ldr     r2,[r0, #M4IF_MCR0_OFFSET]
+    ands    r2, r2, #M4IF_MCR0_FDVACK
+    beq     WAIT_SR_ACK
+
+    /*
+     * Set DSE of all DDR I/O pads to 0 => HighZ
+     * except CKE which must drive during self-refresh
+     * according to JEDEC
+	 */
+
+    ldr     r4, =0
+	PM_SET_HIGHZ_PAD IOMUXC_SW_PAD_CTL_PAD_DRAM_DQM3
+    PM_SET_HIGHZ_PAD IOMUXC_SW_PAD_CTL_PAD_DRAM_SDQS3
+    PM_SET_HIGHZ_PAD IOMUXC_SW_PAD_CTL_PAD_DRAM_DQM2
+    PM_SET_HIGHZ_PAD IOMUXC_SW_PAD_CTL_PAD_DRAM_SDODT1
+    PM_SET_HIGHZ_PAD IOMUXC_SW_PAD_CTL_PAD_DRAM_SDQS2
+    PM_SET_HIGHZ_PAD IOMUXC_SW_PAD_CTL_PAD_DRAM_SDCLK_1
+    PM_SET_HIGHZ_PAD IOMUXC_SW_PAD_CTL_PAD_DRAM_CAS
+    PM_SET_HIGHZ_PAD IOMUXC_SW_PAD_CTL_PAD_DRAM_SDCLK_0
+    PM_SET_HIGHZ_PAD IOMUXC_SW_PAD_CTL_PAD_DRAM_SDQS0
+    PM_SET_HIGHZ_PAD IOMUXC_SW_PAD_CTL_PAD_DRAM_SDODT0
+    PM_SET_HIGHZ_PAD IOMUXC_SW_PAD_CTL_PAD_DRAM_DQM0
+    PM_SET_HIGHZ_PAD IOMUXC_SW_PAD_CTL_PAD_DRAM_RAS
+    PM_SET_HIGHZ_PAD IOMUXC_SW_PAD_CTL_PAD_DRAM_SDQS1
+    PM_SET_HIGHZ_PAD IOMUXC_SW_PAD_CTL_PAD_DRAM_DQM1
+    PM_SET_HIGHZ_PAD IOMUXC_SW_PAD_CTL_GRP_ADDDS
+    PM_SET_HIGHZ_PAD IOMUXC_SW_PAD_CTL_GRP_B0DS
+    PM_SET_HIGHZ_PAD IOMUXC_SW_PAD_CTL_GRP_B1DS
+    PM_SET_HIGHZ_PAD IOMUXC_SW_PAD_CTL_GRP_B2DS
+    PM_SET_HIGHZ_PAD IOMUXC_SW_PAD_CTL_GRP_B3DS
+	/* use DSE=1 for CKE pin,when DDR is in self-refresh */
+    ldr     r4, =1
+    PM_SET_HIGHZ_PAD IOMUXC_SW_PAD_CTL_GRP_CTLDS
+mx5x_wfi:
+    /*
+     * PLL1 workaround as the following: For mx51 only.
+     * Before enter WFI
+     *      (1) switch DDR and ARM to PLL2
+     *      (2) Disable AREN bit to avoid PLL1 restart during MFN change)
+     *      (3) set PLL1 to ~864Mhz with MFI = 8, MFN = 180, MFD = 179
+     *          thus the equation |MFN/(MFD+1)|  < 1
+     *      (4) Manual restart PLL1
+     *      (5) Wait PLL1 lock
+     * After CPU out of WFI
+     *      (6) Set PLL1 to 800Mhz with only change MFN to 60, others keep
+     *      (7) Wait MFN change complete by delay 4.6us,
+     *      (8) Switch DDR and ARM back to PLL1
+     */
+	cmp r6, #SUSPEND_ID_MX51
+
+	bne WFI
+	PM_SET_ADDR_REG MX51_PLL1_BASE, r3
+	PM_SET_ADDR_REG MX51_CCM_BASE, r4
+
+	/* step 1 */
+	ldr     r0, [r4, #0x14]
+	bic     r0, r0, #(0x1 << 30)
+	str     r0, [r4, #0x14]
+
+1:
+	ldr     r0, [r4, #0x48]
+	ands    r0, r0, #(1 << 8)
+	bne     1b
+
+	ldr     r0, [r4, #0x0c]
+	bic     r0, r0, #(0xf << 5)
+	orr     r0, r0, #(0x1 << 8)
+	str     r0, [r4, #0x0c]
+
+	orr     r0, r0, #(1 << 2)
+	str     r0, [r4, #0x0c]
+
+	/* step 2 */
+	ldr r0, [r3, #0x4]
+	bic r0, r0, #0x2
+	str r0, [r3, #0x4] /* disable auto-restart AREN bit */
+
+	/* step 3 */
+	mov r0, #0x80
+	mov r1, #179
+	mov r2, #180
+	str r0, [r3, #0x08]
+	str r0, [r3, #0x1c]
+
+	str r1, [r3, #0x0c]
+	str r1, [r3, #0x20]
+
+	str r2, [r3, #0x10]
+	str r2, [r3, #0x24]
+
+	/* step 4 */
+	ldr r0, =0x00001236 /* Set PLM =1, manual restart and enable PLL*/
+	str r0, [r3, #0x0]
+1:      ldr r0, [r3, #0x0]
+	ands r0, r0, #0x1
+	beq 1b
+WFI:
+	mov r0,#0x0
+	.long     0xe320f003              @ Opcode for WFI
+
+	cmp     r6, #SUSPEND_ID_MX51
+	bne      wfi_done
+
+	/* step 5 */
+	ldr     r0, =60
+	str     r0, [r3, #0x10]
+
+	/* step 6 */
+	/* Load MFN by setting LDREQ */
+	ldr     r0, [r3, #0x04]
+	orr     r0, r0, #0x1
+	str     r0, [r3, #0x04]
+
+      /* Wait for LDREQ bit to clear. */
+2:    ldr     r0, [r3, #0x4]
+       tst    r0, #1
+       bne   2b
+
+	mov r0, #100 /* delay more than 4.6 us */
+3:      subs r0, r0, #1
+	bge 3b
+
+	/* step 8 */
+	ldr     r0, [r4, #0x0c]
+	bic     r0, r0, #(1 << 2)
+	str     r0, [r4, #0x0c]
+
+	/* Source step_clk from LPAPM. */
+	ldr     r0, [r4, #0x0c]
+	bic     r0, r0, #(3 << 7)
+	str     r0, [r4, #0x0c]
+
+	ldr     r0, [r4, #0x14]
+	orr     r0, r0, #(0x1 << 30)
+	str     r0, [r4, #0x14]
+
+3:
+	ldr     r0, [r4, #0x48]
+	ands    r0, r0, #(1 << 8)
+	bne     3b
+
+wfi_done:
+    cmp     r6, #SUSPEND_ID_NONE
+    beq     mx5x_post_wfi
+
+    /*Set the DDR drive strength to max */
+    cmp     r6, #SUSPEND_ID_MX51
+    bne     mx53_restore_ddr_drive_strength
+	ldr r0, =MX51_DRAM_SDCLK_PAD_CTRL_ADDR
+	ldr r1, __mx5x_temp_stack
+	str r1, [r0]
+mx53_restore_ddr_drive_strength:
+    cmp     r6, #SUSPEND_ID_MX53
+    bne     mx5x_post_wfi
 
-    .long     0xe320f003              @ Opcode for WFI
+    ldr     r0, =M4IF_BASE_ADDR_VIRT
+    ldr     r1, =IOMUXC_BASE_ADDR_VIRT
+    adr     r3, __mx5x_temp_stack
+	PM_SET_RESTORE_REG IOMUXC_SW_PAD_CTL_PAD_DRAM_DQM3,		0
+	PM_SET_RESTORE_REG IOMUXC_SW_PAD_CTL_PAD_DRAM_SDQS3, 	1
+	PM_SET_RESTORE_REG IOMUXC_SW_PAD_CTL_PAD_DRAM_DQM2,		2
+	PM_SET_RESTORE_REG IOMUXC_SW_PAD_CTL_PAD_DRAM_SDODT1,	3
+	PM_SET_RESTORE_REG IOMUXC_SW_PAD_CTL_PAD_DRAM_SDQS2, 	4
+	PM_SET_RESTORE_REG IOMUXC_SW_PAD_CTL_PAD_DRAM_SDCLK_1,	5
+	PM_SET_RESTORE_REG IOMUXC_SW_PAD_CTL_PAD_DRAM_CAS,		6
+	PM_SET_RESTORE_REG IOMUXC_SW_PAD_CTL_PAD_DRAM_SDCLK_0,	7
+	PM_SET_RESTORE_REG IOMUXC_SW_PAD_CTL_PAD_DRAM_SDQS0,	8
+	PM_SET_RESTORE_REG IOMUXC_SW_PAD_CTL_PAD_DRAM_SDODT0,	9
+	PM_SET_RESTORE_REG IOMUXC_SW_PAD_CTL_PAD_DRAM_DQM0,		10
+	PM_SET_RESTORE_REG IOMUXC_SW_PAD_CTL_PAD_DRAM_RAS,		11
+	PM_SET_RESTORE_REG IOMUXC_SW_PAD_CTL_PAD_DRAM_SDQS1, 	12
+	PM_SET_RESTORE_REG IOMUXC_SW_PAD_CTL_PAD_DRAM_DQM1,		13
+	PM_SET_RESTORE_REG IOMUXC_SW_PAD_CTL_GRP_ADDDS,			14
+	PM_SET_RESTORE_REG IOMUXC_SW_PAD_CTL_GRP_B0DS,			15
+	PM_SET_RESTORE_REG IOMUXC_SW_PAD_CTL_GRP_B1DS, 			16
+	PM_SET_RESTORE_REG IOMUXC_SW_PAD_CTL_GRP_CTLDS,			17
+	PM_SET_RESTORE_REG IOMUXC_SW_PAD_CTL_GRP_B2DS, 			18
+	PM_SET_RESTORE_REG IOMUXC_SW_PAD_CTL_GRP_B3DS, 			19
 
-#if 0
-	/*Set the DDR drive strength to max */
-	orr       r10, r10, #0x06        @ set bits 2-1
-	str        r10, [r6]
-#endif
+    /* Clear FDVFS bit of M4IF_MCR0 to request DDR to exit self-refresh */
+    ldr     r2,[r0, #M4IF_MCR0_OFFSET]
+    bic     r2, r2, #M4IF_MCR0_FDVFS
+    str     r2,[r0, #M4IF_MCR0_OFFSET]
 
-	ldr     r11, =0x0000fFFF
-TestLoop:
-	subs    r11,r11, #1              @ Decrement the index
-	bge     TestLoop
+    /* Poll FDVACK bit of M4IF_MCR to wait for DDR to exit self-refresh */
+WAIT_AR_ACK:
+    ldr     r2,[r0, #M4IF_MCR0_OFFSET]
+    ands    r2, r2, #M4IF_MCR0_FDVACK
+    bne     WAIT_AR_ACK
 
+    cmp     r6, #SUSPEND_ID_MX51
+    bne     mx53_end
+mx5x_post_wfi:
     mov     r0, #0
     mcr     p15, 0, r0, c7, c5, 0   @ Invalidate inst cache
 
@@ -152,9 +442,12 @@ FinishedInvalidate:
     orr     r0, r0, #ARM_CTRL_DCACHE @ Enable DCache
     mcr     p15, 0, r0, c1, c0, 0    @ Update system control reg
 
+mx53_end:
     /* Restore registers */
     ldmfd sp!, {r4,r5,r6,r7,r9,r10,r11}
     mov		pc, lr
+__mx5x_temp_stack:
+	.space 128
 
 	.type	cpu_do_suspend, #object
 ENTRY(cpu_do_suspend)
diff --git a/arch/arm/mach-mx5/system.c b/arch/arm/mach-mx5/system.c
index 90f1f6f90cfb..7a6918089e71 100644
--- a/arch/arm/mach-mx5/system.c
+++ b/arch/arm/mach-mx5/system.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2011 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright (C) 2011-2012 Freescale Semiconductor, Inc. All Rights Reserved.
  */
 
 /*
@@ -47,34 +47,41 @@ extern int dvfs_core_is_active;
 extern void __iomem *ccm_base;
 extern void __iomem *databahn_base;
 extern int low_bus_freq_mode;
-extern void (*wait_in_iram)(void *ccm_addr, void *databahn_addr);
+extern void (*wait_in_iram)(void *ccm_addr, void *databahn_addr,
+							u32 sys_clk_count);
 extern void mx50_wait(u32 ccm_base, u32 databahn_addr);
 extern void stop_dvfs(void);
 extern void *wait_in_iram_base;
 extern void __iomem *apll_base;
+extern void __iomem *arm_plat_base;
+extern void (*suspend_in_iram)(void *param1, void *param2, void* param3);
+extern void __iomem *suspend_param1;
 
 static struct clk *gpc_dvfs_clk;
-static struct regulator *vpll;
 static struct clk *pll1_sw_clk;
 static struct clk *osc;
 static struct clk *pll1_main_clk;
 static struct clk *ddr_clk ;
-static int dvfs_core_paused;
+static struct clk *sys_clk ;
 
 /* set cpu low power mode before WFI instruction */
 void mxc_cpu_lp_set(enum mxc_cpu_pwr_mode mode)
 {
 	u32 plat_lpc, arm_srpgcr, ccm_clpcr;
-	u32 empgc0, empgc1;
+	u32 empgc0 = 0, empgc1 = 0;
 	int stop_mode = 0;
 
 	/* always allow platform to issue a deep sleep mode request */
-	plat_lpc = __raw_readl(MXC_CORTEXA8_PLAT_LPC) &
+	plat_lpc = __raw_readl(arm_plat_base + MXC_CORTEXA8_PLAT_LPC) &
 	    ~(MXC_CORTEXA8_PLAT_LPC_DSM);
 	ccm_clpcr = __raw_readl(MXC_CCM_CLPCR) & ~(MXC_CCM_CLPCR_LPM_MASK);
 	arm_srpgcr = __raw_readl(MXC_SRPG_ARM_SRPGCR) & ~(MXC_SRPGCR_PCR);
-	empgc0 = __raw_readl(MXC_SRPG_EMPGC0_SRPGCR) & ~(MXC_SRPGCR_PCR);
-	empgc1 = __raw_readl(MXC_SRPG_EMPGC1_SRPGCR) & ~(MXC_SRPGCR_PCR);
+	if (!cpu_is_mx53()) {
+		empgc0 = __raw_readl(MXC_SRPG_EMPGC0_SRPGCR) &
+							~(MXC_SRPGCR_PCR);
+		empgc1 = __raw_readl(MXC_SRPG_EMPGC1_SRPGCR) &
+							~(MXC_SRPGCR_PCR);
+	}
 
 	switch (mode) {
 	case WAIT_CLOCKED:
@@ -111,7 +118,7 @@ void mxc_cpu_lp_set(enum mxc_cpu_pwr_mode mode)
 		return;
 	}
 
-	__raw_writel(plat_lpc, MXC_CORTEXA8_PLAT_LPC);
+	__raw_writel(plat_lpc, arm_plat_base + MXC_CORTEXA8_PLAT_LPC);
 	__raw_writel(ccm_clpcr, MXC_CCM_CLPCR);
 	__raw_writel(arm_srpgcr, MXC_SRPG_ARM_SRPGCR);
 
@@ -119,7 +126,7 @@ void mxc_cpu_lp_set(enum mxc_cpu_pwr_mode mode)
 	if (mx50_revision() != IMX_CHIP_REVISION_1_0)
 		__raw_writel(arm_srpgcr, MXC_SRPG_NEON_SRPGCR);
 
-	if (stop_mode) {
+	if (stop_mode && !cpu_is_mx53()) {
 		empgc0 |= MXC_SRPGCR_PCR;
 		empgc1 |= MXC_SRPGCR_PCR;
 
@@ -149,6 +156,9 @@ void arch_idle(void)
 		mxc_cpu_lp_set(arch_idle_mode);
 
 		if (cpu_is_mx50() && (clk_get_usecount(ddr_clk) == 0)) {
+			if (sys_clk == NULL)
+				sys_clk = clk_get(NULL, "sys_clk");
+
 			memcpy(wait_in_iram_base, mx50_wait, SZ_4K);
 			wait_in_iram = (void *)wait_in_iram_base;
 			if (low_bus_freq_mode) {
@@ -178,7 +188,8 @@ void arch_idle(void)
 				cpu_podf = __raw_readl(MXC_CCM_CACRR);
 				__raw_writel(0x01, MXC_CCM_CACRR);
 
-				wait_in_iram(ccm_base, databahn_base);
+				wait_in_iram(ccm_base, databahn_base,
+					clk_get_usecount(sys_clk));
 
 				/* Set the ARM-POD divider back
 				 * to the original.
@@ -186,7 +197,11 @@ void arch_idle(void)
 				__raw_writel(cpu_podf, MXC_CCM_CACRR);
 				clk_set_parent(pll1_sw_clk, pll1_main_clk);
 			} else
-				wait_in_iram(ccm_base, databahn_base);
+				wait_in_iram(ccm_base, databahn_base,
+					clk_get_usecount(sys_clk));
+		} else if (cpu_is_mx53() && (clk_get_usecount(ddr_clk) == 0)
+				&& low_bus_freq_mode) {
+			suspend_in_iram(suspend_param1, NULL, NULL);
 		} else
 			cpu_do_idle();
 		clk_disable(gpc_dvfs_clk);
@@ -272,7 +287,7 @@ static int __mxs_reset_block(void __iomem *hwreg, int just_enable)
 int mxs_reset_block(void __iomem *hwreg, int just_enable)
 {
 	int try = 10;
-	int r;
+	int r = 0;
 
 	while (try--) {
 		r = __mxs_reset_block(hwreg, just_enable);
diff --git a/arch/arm/plat-mxc/cpufreq.c b/arch/arm/plat-mxc/cpufreq.c
index 15060e737c5d..e3c19c539439 100644
--- a/arch/arm/plat-mxc/cpufreq.c
+++ b/arch/arm/plat-mxc/cpufreq.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2010-2011 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright (C) 2010-2012 Freescale Semiconductor, Inc. All Rights Reserved.
  */
 
 /*
@@ -29,11 +29,15 @@
 #include <mach/hardware.h>
 #include <mach/clock.h>
 
+#define dprintk(msg...) cpufreq_debug_printk(CPUFREQ_DEBUG_CORE, \
+						"cpufreq-core", msg)
+
 #define CLK32_FREQ	32768
 #define NANOSECOND	(1000 * 1000 * 1000)
 
 static int cpu_freq_khz_min;
 static int cpu_freq_khz_max;
+int cpufreq_suspended;
 
 static struct clk *cpu_clk;
 static struct cpufreq_frequency_table *imx_freq_table;
@@ -122,8 +126,17 @@ static int mxc_set_target(struct cpufreq_policy *policy,
 	if (policy->cpu > num_cpus)
 		return 0;
 
-	if (dvfs_core_is_active) {
-		printk(KERN_DEBUG"DVFS-CORE is active, cannot change frequency using CPUFREQ\n");
+	if (dvfs_core_is_active || cpufreq_suspended) {
+		struct cpufreq_freqs freqs;
+
+		freqs.old = policy->cur;
+		freqs.new = clk_get_rate(cpu_clk) / 1000;
+		freqs.cpu = policy->cpu;
+		freqs.flags = 0;
+		cpufreq_notify_transition(&freqs, CPUFREQ_PRECHANGE);
+		cpufreq_notify_transition(&freqs, CPUFREQ_POSTCHANGE);
+
+		dprintk("DVFS is active, cannot change FREQ using CPUFREQ\n");
 		return ret;
 	}
 
diff --git a/arch/arm/plat-mxc/devices/platform-imx-ocotp.c b/arch/arm/plat-mxc/devices/platform-imx-ocotp.c
index 2279aa7b8f91..d3901b6d37ca 100644
--- a/arch/arm/plat-mxc/devices/platform-imx-ocotp.c
+++ b/arch/arm/plat-mxc/devices/platform-imx-ocotp.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2011 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright (C) 2011-2012 Freescale Semiconductor, Inc. All Rights Reserved.
  *
  * This program is free software; you can redistribute it and/or modify it under
  * the terms of the GNU General Public License version 2 as published by the
@@ -26,7 +26,7 @@ static const char *bank_reg_desc[BANKS][BANK_ITEMS] = {
 	BANK(SJC0, SJC1, MAC0, MAC1, HWCAP0, HWCAP1, HWCAP2, SWCAP),
 };
 
-static const struct mxc_otp_platform_data imx50_otp_platform_data = {
+static struct mxc_otp_platform_data imx50_otp_platform_data = {
 	.fuse_name = (char **)bank_reg_desc,
 	.clock_name = "ocotp_ctrl_ahb_clk",
 	.fuse_num = BANKS * BANK_ITEMS,
diff --git a/arch/arm/plat-mxc/dvfs_core.c b/arch/arm/plat-mxc/dvfs_core.c
index dca974df45be..52cfc5686194 100644
--- a/arch/arm/plat-mxc/dvfs_core.c
+++ b/arch/arm/plat-mxc/dvfs_core.c
@@ -1,5 +1,5 @@
 /*
- * Copyright 2008-2011 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2008-2012 Freescale Semiconductor, Inc. All Rights Reserved.
  */
 
 /*
@@ -114,6 +114,7 @@ static struct delayed_work dvfs_core_handler;
  */
 static struct clk *pll1_sw_clk;
 static struct clk *cpu_clk;
+static struct clk *gpu_clk;
 static struct clk *dvfs_clk;
 
 static int cpu_op_nr;
@@ -189,6 +190,7 @@ static int mx5_set_cpu_freq(int op)
 	int podf;
 	int vinc = 0;
 	int ret = 0;
+	int retry_count = 0;
 	int org_cpu_rate;
 	unsigned long rate = 0;
 	int gp_volt = 0;
@@ -276,6 +278,15 @@ static int mx5_set_cpu_freq(int op)
 		podf = cpu_op_tbl[op].cpu_podf;
 		gp_volt = cpu_op_tbl[op].cpu_voltage;
 
+		/* Get ARM_PODF */
+		reg = __raw_readl(ccm_base + dvfs_data->ccm_cacrr_offset);
+		arm_podf = reg & 0x07;
+		if (podf == arm_podf) {
+			printk(KERN_DEBUG
+			       "No need to change freq and voltage!!!!\n");
+			return 0;
+		}
+
 		/* Change arm_podf only */
 		/* set ARM_FREQ_SHIFT_DIVIDER */
 		reg = __raw_readl(ccm_base + dvfs_data->ccm_cdcr_offset);
@@ -290,14 +301,6 @@ static int mx5_set_cpu_freq(int op)
 		reg |= CCM_CDCR_ARM_FREQ_SHIFT_DIVIDER;
 		__raw_writel(reg, ccm_base + dvfs_data->ccm_cdcr_offset);
 
-		/* Get ARM_PODF */
-		reg = __raw_readl(ccm_base + dvfs_data->ccm_cacrr_offset);
-		arm_podf = reg & 0x07;
-		if (podf == arm_podf) {
-			printk(KERN_DEBUG
-			       "No need to change freq and voltage!!!!\n");
-			return 0;
-		}
 		/* Check if FSVAI indicate freq up */
 		if (podf < arm_podf) {
 			ret = regulator_set_voltage(cpu_regulator, gp_volt,
@@ -355,7 +358,10 @@ static int mx5_set_cpu_freq(int op)
 		/* Wait for arm podf Enable */
 		while ((__raw_readl(gpc_base + dvfs_data->gpc_cntr_offset) &
 			MXC_GPCCNTR_STRT) == MXC_GPCCNTR_STRT) {
-			printk(KERN_DEBUG "Waiting arm_podf enabled!\n");
+			if (retry_count)
+				printk(KERN_DEBUG "Waiting arm_podf enabled!\n");
+
+			retry_count++;
 			udelay(10);
 		}
 		spin_unlock_irqrestore(&mxc_dvfs_core_lock, flags);
@@ -577,6 +583,7 @@ static irqreturn_t dvfs_irq(int irq, void *dev_id)
 	return IRQ_HANDLED;
 }
 
+extern int clk_get_usecount(struct clk *clk);
 static void dvfs_core_work_handler(struct work_struct *work)
 {
 	u32 fsvai;
@@ -584,8 +591,11 @@ static void dvfs_core_work_handler(struct work_struct *work)
 	u32 curr_cpu = 0;
 	int ret = 0;
 	int low_freq_bus_ready = 0;
+	int disable_dvfs_irq = 0;
 	int bus_incr = 0, cpu_dcr = 0;
+#ifdef CONFIG_CPU_FREQ
 	int cpu;
+#endif
 
 	low_freq_bus_ready = low_freq_bus_used();
 
@@ -598,6 +608,29 @@ static void dvfs_core_work_handler(struct work_struct *work)
 		goto END;
 	}
 	curr_cpu = clk_get_rate(cpu_clk);
+
+	if (clk_get_usecount(gpu_clk)) {
+		maxf = 1;
+		if (curr_cpu != cpu_op_tbl[0].cpu_rate) {
+			curr_op = 0;
+			minf = 0;
+			dvfs_load_config(0);
+			if (!high_bus_freq_mode)
+				set_high_bus_freq(1);
+			set_cpu_freq(curr_op);
+		}
+		/* If we enable DVFS's irq, the irq will keep coming,
+		 * and will consume about 3-40% cpu usage, we disable
+		 * dvfs 's irq here, and let it check the status every
+		 * 100 msecs.  If gpu clk have count to 0, it will
+		 * enable dvfs's irq let it do what it want.*/
+		schedule_delayed_work(&dvfs_core_handler,
+						msecs_to_jiffies(100));
+		disable_dvfs_irq = 1;
+		goto END;
+	} else
+		disable_dvfs_irq = 0;
+
 	/* If FSVAI indicate freq down,
 	   check arm-clk is not in lowest frequency*/
 	if (fsvai == FSVAI_FREQ_DECREASE) {
@@ -687,8 +720,10 @@ END:
 
 	/* Enable DVFS interrupt */
 	/* FSVAIM=0 */
-	reg = (reg & ~MXC_DVFSCNTR_FSVAIM);
-	reg |= FSVAI_FREQ_NOCHANGE;
+	if (!disable_dvfs_irq) {
+		reg = (reg & ~MXC_DVFSCNTR_FSVAIM);
+		reg |= FSVAI_FREQ_NOCHANGE;
+	}
 	/* LBFL=1 */
 	reg = (reg & ~MXC_DVFSCNTR_LBFL);
 	reg |= MXC_DVFSCNTR_LBFL;
@@ -709,7 +744,10 @@ void stop_dvfs(void)
 	u32 reg = 0;
 	unsigned long flags;
 	u32 curr_cpu;
+	u32 old_loops_per_jiffy;
+#ifdef CONFIG_CPU_FREQ
 	int cpu;
+#endif
 
 	if (dvfs_core_is_active) {
 
@@ -736,7 +774,7 @@ void stop_dvfs(void)
 				dvfs_cpu_jiffies(per_cpu(cpu_data, cpu).loops_per_jiffy,
 					curr_cpu/1000, clk_get_rate(cpu_clk) / 1000);
 #else
-		u32 old_loops_per_jiffy = loops_per_jiffy;
+		old_loops_per_jiffy = loops_per_jiffy;
 
 		loops_per_jiffy =
 			dvfs_cpu_jiffies(old_loops_per_jiffy,
@@ -948,9 +986,17 @@ static int __devinit mxc_dvfs_core_probe(struct platform_device *pdev)
 		return PTR_ERR(cpu_clk);
 	}
 	if (!cpu_is_mx6q()) {
+		gpu_clk = clk_get(NULL, "gpu3d_clk");
+		if (IS_ERR(cpu_clk)) {
+			printk(KERN_ERR "%s: failed to get gpu clock\n",
+								__func__);
+			return PTR_ERR(gpu_clk);
+		}
+
 		dvfs_clk = clk_get(NULL, dvfs_data->clk2_id);
 		if (IS_ERR(dvfs_clk)) {
-			printk(KERN_ERR "%s: failed to get dvfs clock\n", __func__);
+			printk(KERN_ERR "%s: failed to get dvfs clock\n",
+								__func__);
 			return PTR_ERR(dvfs_clk);
 		}
 	}
diff --git a/arch/arm/plat-mxc/include/mach/check_fuse.h b/arch/arm/plat-mxc/include/mach/check_fuse.h
new file mode 100644
index 000000000000..00c6116a8209
--- /dev/null
+++ b/arch/arm/plat-mxc/include/mach/check_fuse.h
@@ -0,0 +1,35 @@
+/*
+ * Copyright (C) 2011-2012 Freescale Semiconductor, Inc. All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+
+#ifndef __ARCH_ARM_MACH_MX5_FUSE_CHECK_H__
+#define __ARCH_ARM_MACH_MX5_FUSE_CHECK_H__
+
+#define MXC_IIM_MX5_DISABLERS_OFFSET 0x8
+#define MXC_IIM_MX5_DISABLERS_GPU_MASK 0x4
+#define MXC_IIM_MX5_DISABLERS_GPU_SHIFT 0x2
+#define MXC_IIM_MX5_DISABLERS_VPU_MASK 0x2
+#define MXC_IIM_MX5_DISABLERS_VPU_SHIFT 0x1
+
+#define FSL_OCOTP_MX5_CFG2_OFFSET 0x060
+#define FSL_OCOTP_MX5_DISABLERS_GPU_MASK 0x2000000
+#define FSL_OCOTP_MX5_DISABLERS_GPU_SHIFT 0x19
+
+int mxc_fuse_get_gpu_status(void);
+int mxc_fuse_get_vpu_status(void);
+
+#endif
diff --git a/arch/arm/plat-mxc/include/mach/mxc_edid.h b/arch/arm/plat-mxc/include/mach/mxc_edid.h
index 59688f657a25..39c7641a9f1c 100644
--- a/arch/arm/plat-mxc/include/mach/mxc_edid.h
+++ b/arch/arm/plat-mxc/include/mach/mxc_edid.h
@@ -1,5 +1,5 @@
 /*
- * Copyright 2009-2011 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2009-2012 Freescale Semiconductor, Inc. All Rights Reserved.
  */
 
 /*
@@ -28,6 +28,7 @@
 
 #define FB_VMODE_ASPECT_4_3	0x10
 #define FB_VMODE_ASPECT_16_9	0x20
+#define FB_VMODE_ASPECT_MASK	(FB_VMODE_ASPECT_4_3 | FB_VMODE_ASPECT_16_9)
 
 struct mxc_edid_cfg {
 	bool cea_underscan;