Merge branch 'imx_2.6.38' into imx_2.6.38_android

24 files changed, 775 insertions, 168 deletions

diff --git a/arch/arm/configs/imx6_defconfig b/arch/arm/configs/imx6_defconfig
index 3ef30a90b404..7c19d2bc8840 100644
--- a/arch/arm/configs/imx6_defconfig
+++ b/arch/arm/configs/imx6_defconfig
@@ -1,7 +1,7 @@
 #
 # Automatically generated make config: don't edit
 # Linux/arm 2.6.38 Kernel Configuration
-# Wed Jul 27 13:18:10 2011
+# Tue Aug 16 14:53:37 2011
 #
 CONFIG_ARM=y
 CONFIG_HAVE_PWM=y
@@ -270,6 +270,7 @@ CONFIG_IMX_HAVE_PLATFORM_IMX_IPUV3=y
 CONFIG_IMX_HAVE_PLATFORM_IMX_VPU=y
 CONFIG_IMX_HAVE_PLATFORM_AHCI=y
 CONFIG_IMX_HAVE_PLATFORM_IMX_OCOTP=y
+CONFIG_IMX_HAVE_PLATFORM_IMX_VIIM=y
 CONFIG_IMX_HAVE_PLATFORM_LDB=y
 CONFIG_IMX_HAVE_PLATFORM_VIV_GPU=y
 CONFIG_IMX_HAVE_PLATFORM_MXC_HDMI=y
@@ -290,6 +291,7 @@ CONFIG_IMX_HAVE_PLATFORM_IMX_ASRC=y
 # CONFIG_ARCH_MX5 is not set
 CONFIG_ARCH_MX6=y
 CONFIG_FORCE_MAX_ZONEORDER=13
+CONFIG_DMA_ZONE_SIZE=96
 CONFIG_ARCH_MX6Q=y
 CONFIG_SOC_IMX6Q=y
 CONFIG_MACH_MX6Q_SABREAUTO=y
@@ -303,7 +305,6 @@ CONFIG_MXC_PWM=y
 # CONFIG_MXC_DEBUG_BOARD is not set
 CONFIG_ARCH_MXC_IOMUX_V3=y
 CONFIG_IRAM_ALLOC=y
-CONFIG_DMA_ZONE_SIZE=96
 CONFIG_CLK_DEBUG=y
 
 #
@@ -1023,6 +1024,7 @@ CONFIG_HW_RANDOM=y
 # CONFIG_RAW_DRIVER is not set
 # CONFIG_TCG_TPM is not set
 # CONFIG_RAMOOPS is not set
+CONFIG_MXS_VIIM=y
 CONFIG_I2C=y
 CONFIG_I2C_BOARDINFO=y
 CONFIG_I2C_COMPAT=y
diff --git a/arch/arm/mach-mx5/Kconfig b/arch/arm/mach-mx5/Kconfig
index 6ab106d7aa6f..2ee02647136c 100644
--- a/arch/arm/mach-mx5/Kconfig
+++ b/arch/arm/mach-mx5/Kconfig
@@ -229,6 +229,7 @@ config MACH_MX50_RDP
 	select IMX_HAVE_PLATFORM_FEC
 	select IMX_HAVE_PLATFORM_GPMI_NFC
 	select IMX_HAVE_PLATFORM_IMX_OCOTP
+	select IMX_HAVE_PLATFORM_IMX_VIIM
 	select IMX_HAVE_PLATFORM_IMX_DCP
 	select IMX_HAVE_PLATFORM_RANDOM_RNGC
     select IMX_HAVE_PLATFORM_PERFMON
diff --git a/arch/arm/mach-mx5/board-mx50_rdp.c b/arch/arm/mach-mx5/board-mx50_rdp.c
index 498f320d94f7..8ea5a4a9bbaa 100644
--- a/arch/arm/mach-mx5/board-mx50_rdp.c
+++ b/arch/arm/mach-mx5/board-mx50_rdp.c
@@ -846,6 +846,7 @@ static void __init mx50_rdp_board_init(void)
 	imx50_add_sdhci_esdhc_imx(1, &mx50_rdp_sd2_data);
 	imx50_add_sdhci_esdhc_imx(2, &mx50_rdp_sd3_data);
 	imx50_add_otp();
+	imx50_add_viim();
 	imx50_add_dcp();
 	imx50_add_rngb();
 	imx50_add_perfmon();
diff --git a/arch/arm/mach-mx5/board-mx53_ard.c b/arch/arm/mach-mx5/board-mx53_ard.c
index 7ef675e50369..e68f7e08e8ef 100644
--- a/arch/arm/mach-mx5/board-mx53_ard.c
+++ b/arch/arm/mach-mx5/board-mx53_ard.c
@@ -22,12 +22,16 @@
 #include <linux/clk.h>
 #include <linux/delay.h>
 #include <linux/gpio.h>
+#include <linux/mxcfb.h>
+#include <linux/ipu.h>
+#include <linux/pwm_backlight.h>
 #include <linux/smsc911x.h>
 
 #include <mach/common.h>
 #include <mach/hardware.h>
 #include <mach/imx-uart.h>
 #include <mach/iomux-mx53.h>
+#include <mach/ipu-v3.h>
 
 #include <asm/mach-types.h>
 #include <asm/mach/arch.h>
@@ -126,6 +130,20 @@ static iomux_v3_cfg_t mx53_ard_pads[] = {
 
 	/* MAINBRD_SPDIF_IN */
 	MX53_PAD_KEY_COL3__SPDIF_IN1,
+	/* LVDS */
+	MX53_PAD_LVDS0_TX3_P__LDB_LVDS0_TX3,
+	MX53_PAD_LVDS0_CLK_P__LDB_LVDS0_CLK,
+	MX53_PAD_LVDS0_TX2_P__LDB_LVDS0_TX2,
+	MX53_PAD_LVDS0_TX1_P__LDB_LVDS0_TX1,
+	MX53_PAD_LVDS0_TX0_P__LDB_LVDS0_TX0,
+	MX53_PAD_LVDS1_TX3_P__LDB_LVDS1_TX3,
+	MX53_PAD_LVDS1_TX2_P__LDB_LVDS1_TX2,
+	MX53_PAD_LVDS1_CLK_P__LDB_LVDS1_CLK,
+	MX53_PAD_LVDS1_TX1_P__LDB_LVDS1_TX1,
+	MX53_PAD_LVDS1_TX0_P__LDB_LVDS1_TX0,
+	/* PWM */
+	MX53_PAD_DISP0_DAT8__PWM1_PWMO,
+	MX53_PAD_DISP0_DAT9__PWM2_PWMO
 };
 
 /* Config CS1 settings for ethernet controller */
@@ -230,6 +248,52 @@ static inline void mx53_ard_init_uart(void)
 	imx53_add_imx_uart(2, &mx53_ard_uart_data);
 }
 
+static struct ipuv3_fb_platform_data ard_fb_data[] = {
+	{
+	.disp_dev = "ldb",
+	.interface_pix_fmt = IPU_PIX_FMT_RGB666,
+	.mode_str = "LDB-XGA",
+	.default_bpp = 16,
+	.int_clk = false,
+	},
+	{
+	.disp_dev = "vga",
+	.interface_pix_fmt = IPU_PIX_FMT_GBR24,
+	.mode_str = "VGA-XGA",
+	.default_bpp = 16,
+	.int_clk = false,
+	},
+};
+
+static struct imx_ipuv3_platform_data ipu_data = {
+	.rev = 3,
+};
+
+static struct platform_pwm_backlight_data ard_pwm1_backlight_data = {
+	.pwm_id = 0,
+	.max_brightness = 255,
+	.dft_brightness = 128,
+	.pwm_period_ns = 50000,
+};
+
+static struct platform_pwm_backlight_data ard_pwm2_backlight_data = {
+	.pwm_id = 1,
+	.max_brightness = 255,
+	.dft_brightness = 128,
+	.pwm_period_ns = 50000,
+};
+
+static struct fsl_mxc_tve_platform_data tve_data = {
+	.dac_reg = "LDO4",
+};
+
+static struct fsl_mxc_ldb_platform_data ldb_data = {
+	.ipu_id = 0,
+	.disp_id = 0,
+	.ext_ref = 1,
+	.mode = LDB_SIN0,
+};
+
 static void __init mx53_ard_io_init(void)
 {
 	/* MX53 ARD board */
@@ -237,13 +301,48 @@ static void __init mx53_ard_io_init(void)
 	gpio_request(ARD_ETHERNET_INT_B, "eth-int-b");
 	gpio_direction_input(ARD_ETHERNET_INT_B);
 }
+
+static int __initdata enable_ard_vga = { 0 };
+static int __init ard_vga_setup(char *__unused)
+{
+	enable_ard_vga = 1;
+	printk(KERN_INFO "Enable MX53 ARD VGA\n");
+	return cpu_is_mx53();
+}
+__setup("ard-vga", ard_vga_setup);
+
 static void __init mx53_ard_board_init(void)
 {
+	int i;
 	mxc_iomux_v3_setup_multiple_pads(mx53_ard_pads,
 					ARRAY_SIZE(mx53_ard_pads));
+	/* setup VGA PINs */
+	if (enable_ard_vga) {
+			iomux_v3_cfg_t vga;
+			vga = MX53_PAD_EIM_OE__IPU_DI1_PIN7;
+			mxc_iomux_v3_setup_pad(vga);
+			vga = MX53_PAD_EIM_RW__IPU_DI1_PIN8;
+			mxc_iomux_v3_setup_pad(vga);
+	}
+
 	mxc_spdif_data.spdif_core_clk = clk_get(NULL, "spdif_xtal_clk");
 	clk_put(mxc_spdif_data.spdif_core_clk);
 	mx53_ard_init_uart();
+
+	imx53_add_ipuv3(0, &ipu_data);
+	for (i = 0; i < ARRAY_SIZE(ard_fb_data); i++)
+			imx53_add_ipuv3fb(i, &ard_fb_data[i]);
+
+	imx53_add_vpu();
+	imx53_add_ldb(&ldb_data);
+	imx53_add_tve(&tve_data);
+	imx53_add_v4l2_output(0);
+
+	imx53_add_mxc_pwm(0);
+	imx53_add_mxc_pwm_backlight(0, &ard_pwm1_backlight_data);
+	imx53_add_mxc_pwm(1);
+	imx53_add_mxc_pwm_backlight(1, &ard_pwm2_backlight_data);
+
 	imx53_add_srtc();
 	imx53_add_imx2_wdt(0, NULL);
 	imx53_add_sdhci_esdhc_imx(0, &mx53_ard_sd1_data);
diff --git a/arch/arm/mach-mx5/devices-imx50.h b/arch/arm/mach-mx5/devices-imx50.h
index 74b4988f6ab3..09df1a288676 100644
--- a/arch/arm/mach-mx5/devices-imx50.h
+++ b/arch/arm/mach-mx5/devices-imx50.h
@@ -63,6 +63,10 @@ extern const struct imx_otp_data imx50_otp_data __initconst;
 #define imx50_add_otp() \
 	imx_add_otp(&imx50_otp_data);
 
+extern const struct imx_viim_data imx50_viim_data  __initconst;
+#define imx50_add_viim() \
+	imx_add_viim(&imx50_viim_data)
+
 extern const struct imx_dcp_data imx50_dcp_data __initconst;
 #define imx50_add_dcp() \
 	imx_add_dcp(&imx50_dcp_data);
diff --git a/arch/arm/mach-mx6/Kconfig b/arch/arm/mach-mx6/Kconfig
index 847e6ab0af1b..8d0d3c766046 100644
--- a/arch/arm/mach-mx6/Kconfig
+++ b/arch/arm/mach-mx6/Kconfig
@@ -40,6 +40,7 @@ config MACH_MX6Q_SABREAUTO
 	select IMX_HAVE_PLATFORM_FSL_USB_WAKEUP
 	select IMX_HAVE_PLATFORM_AHCI
 	select IMX_HAVE_PLATFORM_IMX_OCOTP
+	select IMX_HAVE_PLATFORM_IMX_VIIM
 	select IMX_HAVE_PLATFORM_IMX2_WDT
 	select IMX_HAVE_PLATFORM_IMX_SNVS_RTC
 	select IMX_HAVE_PLATFORM_IMX_PM
diff --git a/arch/arm/mach-mx6/board-mx6q_sabreauto.c b/arch/arm/mach-mx6/board-mx6q_sabreauto.c
index 9c6b07cd7a7b..5ae5a8d41145 100644
--- a/arch/arm/mach-mx6/board-mx6q_sabreauto.c
+++ b/arch/arm/mach-mx6/board-mx6q_sabreauto.c
@@ -826,6 +826,7 @@ static void __init mx6_board_init(void)
 	gpio_request(MX6Q_SABREAUTO_LDB_BACKLIGHT, "ldb-backlight");
 	gpio_direction_output(MX6Q_SABREAUTO_LDB_BACKLIGHT, 1);
 	imx6q_add_otp();
+	imx6q_add_viim();
 	imx6q_add_imx2_wdt(0, NULL);
 	imx6q_add_dma();
 	imx6q_add_gpmi(&mx6q_gpmi_nfc_platform_data);
diff --git a/arch/arm/mach-mx6/clock.c b/arch/arm/mach-mx6/clock.c
index 1c192b7bee69..4e1ee83d7a83 100644
--- a/arch/arm/mach-mx6/clock.c
+++ b/arch/arm/mach-mx6/clock.c
@@ -36,6 +36,7 @@
 #define __INIT_CLK_DEBUG(n)
 #endif
 
+extern int mxc_jtag_enabled;
 void __iomem *apll_base;
 static struct clk pll1_sys_main_clk;
 static struct clk pll2_528_bus_main_clk;
@@ -4132,14 +4133,31 @@ int __init mx6_clocks_init(unsigned long ckil, unsigned long osc,
 	/* set the NAND to 11MHz. Too fast will cause dma timeout. */
 	clk_set_rate(&enfc_clk, enfc_clk.round_rate(&enfc_clk, 11000000));
 
-	/* Make sure all clocks are ON initially */
-	__raw_writel(0xFFFFFFFF, MXC_CCM_CCGR0);
-	__raw_writel(0xFFFFFFFF, MXC_CCM_CCGR1);
-	__raw_writel(0xFFFFFFFF, MXC_CCM_CCGR2);
-	__raw_writel(0xFFFFFFFF, MXC_CCM_CCGR3);
-	__raw_writel(0xFFFFFFFF, MXC_CCM_CCGR4);
-	__raw_writel(0xFFFFFFFF, MXC_CCM_CCGR5);
-	__raw_writel(0xFFFFFFFF, MXC_CCM_CCGR6);
+	/* Gate off all possible clocks */
+	if (mxc_jtag_enabled) {
+		__raw_writel(3 << MXC_CCM_CCGRx_CG11_OFFSET |
+			     3 << MXC_CCM_CCGRx_CG2_OFFSET |
+			     3 << MXC_CCM_CCGRx_CG1_OFFSET |
+			     3 << MXC_CCM_CCGRx_CG0_OFFSET, MXC_CCM_CCGR0);
+	} else {
+		__raw_writel(3 << MXC_CCM_CCGRx_CG2_OFFSET |
+			     3 << MXC_CCM_CCGRx_CG1_OFFSET |
+			     3 << MXC_CCM_CCGRx_CG0_OFFSET, MXC_CCM_CCGR0);
+	}
+	__raw_writel(3 << MXC_CCM_CCGRx_CG10_OFFSET, MXC_CCM_CCGR1);
+	__raw_writel(3 << MXC_CCM_CCGRx_CG10_OFFSET |
+		     3 << MXC_CCM_CCGRx_CG9_OFFSET |
+		     3 << MXC_CCM_CCGRx_CG8_OFFSET, MXC_CCM_CCGR2);
+	__raw_writel(3 << MXC_CCM_CCGRx_CG14_OFFSET |
+		     3 << MXC_CCM_CCGRx_CG13_OFFSET |
+		     3 << MXC_CCM_CCGRx_CG12_OFFSET |
+		     3 << MXC_CCM_CCGRx_CG11_OFFSET |
+		     3 << MXC_CCM_CCGRx_CG10_OFFSET, MXC_CCM_CCGR3);
+	__raw_writel(3 << MXC_CCM_CCGRx_CG7_OFFSET |
+		     3 << MXC_CCM_CCGRx_CG4_OFFSET, MXC_CCM_CCGR4);
+	__raw_writel(3 << MXC_CCM_CCGRx_CG3_OFFSET |
+		     3 << MXC_CCM_CCGRx_CG0_OFFSET, MXC_CCM_CCGR5);
+	__raw_writel(0, MXC_CCM_CCGR6);
 
 	base = ioremap(GPT_BASE_ADDR, SZ_4K);
 	mxc_timer_init(&gpt_clk[0], base, MXC_INT_GPT);
diff --git a/arch/arm/mach-mx6/devices-imx6q.h b/arch/arm/mach-mx6/devices-imx6q.h
index 8e790c72f423..c9cf454629f1 100644
--- a/arch/arm/mach-mx6/devices-imx6q.h
+++ b/arch/arm/mach-mx6/devices-imx6q.h
@@ -117,7 +117,11 @@ extern const struct imx_vpu_data imx6q_vpu_data __initconst;
 
 extern const struct imx_otp_data imx6q_otp_data __initconst;
 #define imx6q_add_otp() \
-	imx_add_otp(&imx6q_otp_data);
+	imx_add_otp(&imx6q_otp_data)
+
+extern const struct imx_viim_data imx6q_viim_data __initconst;
+#define imx6q_add_viim() \
+	imx_add_viim(&imx6q_viim_data)
 
 extern const struct imx_imx2_wdt_data imx6q_imx2_wdt_data[] __initconst;
 #define imx6q_add_imx2_wdt(id, pdata)   \
diff --git a/arch/arm/mach-mx6/devices.c b/arch/arm/mach-mx6/devices.c
index ab5b05d16cee..a03b4ae1b07d 100644
--- a/arch/arm/mach-mx6/devices.c
+++ b/arch/arm/mach-mx6/devices.c
@@ -109,7 +109,7 @@ static struct mxc_gpio_port mxc_gpio_ports[] = {
 	},
 };
 
-int __init mx6q_register_gpios(void)
+int mx6q_register_gpios(void)
 {
 	/* 7 ports for Mx6 */
 	return mxc_gpio_init(mxc_gpio_ports, 7);
diff --git a/arch/arm/mach-mx6/irq.c b/arch/arm/mach-mx6/irq.c
index 7cc93962de7c..4633607c1e87 100644
--- a/arch/arm/mach-mx6/irq.c
+++ b/arch/arm/mach-mx6/irq.c
@@ -43,7 +43,7 @@ static int mx6_gic_irq_set_wake(struct irq_data *d, unsigned int enable)
 	}
 	return 0;
 }
-void __init mx6_init_irq(void)
+void mx6_init_irq(void)
 {
 	struct irq_desc *desc;
 	unsigned int i;
diff --git a/arch/arm/mach-mx6/mm.c b/arch/arm/mach-mx6/mm.c
index f1d6d6d76924..bbaa6ae15c4b 100644
--- a/arch/arm/mach-mx6/mm.c
+++ b/arch/arm/mach-mx6/mm.c
@@ -64,7 +64,7 @@ void __init mx6_map_io(void)
 	mxc_arch_reset_init(IO_ADDRESS(MX6Q_WDOG1_BASE_ADDR));
 }
 #ifdef CONFIG_CACHE_L2X0
-static int mxc_init_l2x0(void)
+int mxc_init_l2x0(void)
 {
 	unsigned int val;
 
diff --git a/arch/arm/mach-mx6/mx6q_suspend.S b/arch/arm/mach-mx6/mx6q_suspend.S
index 4c304c9b10c3..5d2aca1c9dd1 100644
--- a/arch/arm/mach-mx6/mx6q_suspend.S
+++ b/arch/arm/mach-mx6/mx6q_suspend.S
@@ -21,13 +21,17 @@
 #include <asm/memory.h>
 #include "src-reg.h"
 
-#define ARM_CTRL_DCACHE     1 << 2
-#define ARM_CTRL_ICACHE     1 << 12
-#define ARM_AUXCR_L2EN      1 << 1
+#define ARM_CTRL_DCACHE		(1 << 2)
+#define ARM_CTRL_ICACHE		(1 << 12)
+#define ARM_AUXCR_L2EN		(1 << 1)
 #define TTRBIT_MASK 		0xffffc000
 #define TABLE_INDEX_MASK 	0xfff00000
 #define TABLE_ENTRY 		0x00000c02
 #define CACHE_DISABLE_MASK 	0xffffe7fb
+#define MMDC_MAPSR_OFFSET 	0x404
+#define MMDC_MAPSR_PSS 		(1 << 4)
+#define MMDC_MAPSR_PSD 		(1 << 0)
+#define IRAM_SUSPEND_SIZE 	(1 << 12)
 
 /*************************************************************
 mx6q_suspend:
@@ -42,16 +46,18 @@ passed in r0:
 see define in include/linux/suspend.h
 1 -> cpu enter stop mode;
 3 -> cpu enter dormant mode.
+r1: iram_paddr
+r2: suspend_iram_base
 *************************************************************/
 
 ENTRY(mx6q_suspend)
-	stmfd   sp!, {r0-r12}     @ Save registers
+	stmfd	sp!, {r0-r12}     @ Save registers
 /*************************************************************
 suspend mode entry
 *************************************************************/
 
-	cmp r0, #0x1
-	bne dormant		/* dormant mode */
+	cmp	r0, #0x1
+	bne	dormant		/* dormant mode */
 
 	dsb
 	wfi
@@ -64,17 +70,21 @@ suspend mode entry
 /***********************************************************
 never run to here
 ************************************************************/
-	b out	/* exit standby */
+	b 	out	/* exit standby */
 
 /************************************************************
 dormant entry, data save in stack, save sp in the src_gpr2
 ************************************************************/
 dormant:
-	ldr r1, =SRC_BASE_ADDR
-	add r1, r1, #PERIPBASE_VIRT
-	str sp, [r1, #SRC_GPR2_OFFSET]	/* save sp in src_gpr2 */
-/**********************************************************
+	mov 	r3, r1
+	mov 	r0, r1
+	add 	r0, r0, #IRAM_SUSPEND_SIZE /* 4K */
+	ldr 	r4, =SRC_BASE_ADDR
+	add 	r4, r4, #PERIPBASE_VIRT
+	str 	r0, [r4, #SRC_GPR2_OFFSET] /* set src_gpr2 */
+/************************************************************
 saved register and context as below:
+	ddr_iomux set
 	sp
 	spsr
 	lr
@@ -92,47 +102,106 @@ saved register and context as below:
 	CPSR
 	SCTLR
 ************************************************************/
-	/*
-	 * Save only needed CPU CP15 registers. VFP, breakpoint,
-	 * performance monitor registers are not saved. Generic
-	 * code suppose to take care of those.
-	 */
-	mrs	r5, spsr			@ Store spsr
-	mov	r6, lr				@ Store lr
-	stmfd	sp!, {r5-r6}
+ddr_iomux_save:
+	/* save mmdc iomux setting, stack is from the tail of
+	iram_suspend base */
+
+	mov	r0, r2 		/* get suspend_iram_base */
+	add	r0, r0, #IRAM_SUSPEND_SIZE	/* 4K */
+	ldr	r1, =MX6Q_IOMUXC_BASE_ADDR
+	add	r1, r1, #PERIPBASE_VIRT
+
+	ldr	r4, [r1, #0x5ac] /* DRAM_DQM0 */
+	ldr	r5, [r1, #0x5b4] /* DRAM_DQM1 */
+	ldr	r6, [r1, #0x528] /* DRAM_DQM2 */
+	ldr	r7, [r1, #0x520] /* DRAM_DQM3 */
+	stmfd	r0!, {r4-r7}
+
+	ldr	r4, [r1, #0x514] /* DRAM_DQM4 */
+	ldr	r5, [r1, #0x510] /* DRAM_DQM5 */
+	ldr	r6, [r1, #0x5bc] /* DRAM_DQM6 */
+	ldr	r7, [r1, #0x5c4] /* DRAM_DQM7 */
+	stmfd	r0!, {r4-r7}
+
+	ldr	r4, [r1, #0x56c] /* DRAM_CAS */
+	ldr	r5, [r1, #0x578] /* DRAM_RAS */
+	ldr	r6, [r1, #0x588] /* DRAM_SDCLK_0 */
+	ldr	r7, [r1, #0x594] /* DRAM_SDCLK_1 */
+	stmfd	r0!, {r4-r7}
+
+	ldr	r5, [r1, #0x750] /* DDRMODE_CTL */
+	ldr	r6, [r1, #0x774] /* DDRMODE */
+	stmfd	r0!, {r5-r6}
+
+	ldr	r4, [r1, #0x5a8] /* DRAM_SDQS0 */
+	ldr	r5, [r1, #0x5b0] /* DRAM_SDQS1 */
+	ldr	r6, [r1, #0x524] /* DRAM_SDQS2 */
+	ldr	r7, [r1, #0x51c] /* DRAM_SDQS3 */
+	stmfd	r0!, {r4-r7}
+
+	ldr	r4, [r1, #0x518] /* DRAM_SDQS4 */
+	ldr	r5, [r1, #0x50c] /* DRAM_SDQS5 */
+	ldr	r6, [r1, #0x5b8] /* DRAM_SDQS6 */
+	ldr	r7, [r1, #0x5c0] /* DRAM_SDQS7 */
+	stmfd	r0!, {r4-r7}
+
+	ldr	r4, [r1, #0x784] /* GPR_B0DS */
+	ldr	r5, [r1, #0x788] /* GPR_B1DS */
+	ldr	r6, [r1, #0x794] /* GPR_B2DS */
+	ldr	r7, [r1, #0x79c] /* GPR_B3DS */
+	stmfd	r0!, {r4-r7}
+
+	ldr	r4, [r1, #0x7a0] /* GPR_B4DS */
+	ldr	r5, [r1, #0x7a4] /* GPR_B5DS */
+	ldr	r6, [r1, #0x7a8] /* GPR_B6DS */
+	ldr	r7, [r1, #0x748] /* GPR_B7DS */
+	stmfd	r0!, {r4-r7}
+
+	ldr	r5, [r1, #0x74c] /* GPR_ADDS*/
+	ldr	r6, [r1, #0x59c] /* DRAM_SODT0*/
+	ldr	r7, [r1, #0x5a0] /* DRAM_SODT1*/
+	stmfd	r0!, {r5-r7}
+ddr_iomux_save_done:
+
+	mov	r4, sp		@ Store sp
+	mrs	r5, spsr	@ Store spsr
+	mov	r6, lr		@ Store lr
+	stmfd	r0!, {r4-r6}
 
 	/* c1 and c2 registers */
-	mrc	p15, 0, r4, c1, c0, 2		@ CPACR
-	mrc	p15, 0, r5, c2, c0, 0		@ TTBR0
-	mrc	p15, 0, r6, c2, c0, 1		@ TTBR1
-	mrc	p15, 0, r7, c2, c0, 2		@ TTBCR
-	stmfd	sp!, {r4-r7}
+	mrc	p15, 0, r4, c1, c0, 2	@ CPACR
+	mrc	p15, 0, r5, c2, c0, 0	@ TTBR0
+	mrc	p15, 0, r6, c2, c0, 1	@ TTBR1
+	mrc	p15, 0, r7, c2, c0, 2	@ TTBCR
+	stmfd	r0!, {r4-r7}
 
 	/* c3 and c10 registers */
-	mrc	p15, 0, r4, c3, c0, 0		@ DACR
-	mrc	p15, 0, r5, c10, c2, 0		@ PRRR
-	mrc	p15, 0, r6, c10, c2, 1		@ NMRR
-	mrc	p15, 0, r7, c1, c0, 1		@ ACTLR
-	stmfd	sp!,{r4-r7}
+	mrc	p15, 0, r4, c3, c0, 0	@ DACR
+	mrc	p15, 0, r5, c10, c2, 0	@ PRRR
+	mrc	p15, 0, r6, c10, c2, 1	@ NMRR
+	mrc	p15, 0, r7, c1, c0, 1	@ ACTLR
+	stmfd	r0!,{r4-r7}
 
 	/* c12, c13 and CPSR registers */
-	mrc	p15, 0, r4, c13, c0, 1		@ Context ID
-	mrc	p15, 0, r5, c13, c0, 2		@ User r/w thread ID
-	mrc	p15, 0, r6, c12, c0, 0		@ Secure or NS VBAR
-	mrs	r7, cpsr					@ Store CPSR
-	stmfd	sp!, {r4-r7}
+	mrc	p15, 0, r4, c13, c0, 1	@ Context ID
+	mrc	p15, 0, r5, c13, c0, 2	@ User r/w thread ID
+	mrc	p15, 0, r6, c12, c0, 0	@ Secure or NS VBAR
+	mrs	r7, cpsr		@ Store CPSR
+	stmfd	r0!, {r4-r7}
 
 	/* c1 control register */
-	mrc	p15, 0, r4, c1, c0, 0		@ SCTLR
-	stmfd	sp!, {r4}
+	mrc	p15, 0, r4, c1, c0, 0	@ SCTLR
+	stmfd	r0!, {r4}
 
 	/*
 	 * Flush all data from the L1 data cache before disabling
 	 * SCTLR.C bit.
 	 */
-	push {r0-r12}
-	bl v7_flush_dcache_all
-	pop {r0-r12}
+	push	{r0-r12}
+	ldr	r0, =v7_flush_dcache_all
+	mov	lr, pc
+	mov	pc, r0
+	pop	{r0-r12}
 
 	/*
 	 * Clear the SCTLR.C bit to prevent further data cache
@@ -140,7 +209,7 @@ saved register and context as below:
 	 * strongly ordered and would not hit the cache.
 	 */
 	mrc	p15, 0, r0, c1, c0, 0
-	bic	r0, r0, #(1 << 2)		@ Disable the C bit
+	bic	r0, r0, #(1 << 2)	@ Disable the C bit
 	mcr	p15, 0, r0, c1, c0, 0
 	isb
 
@@ -149,9 +218,11 @@ saved register and context as below:
 	 * necessary exported flush API is used here. Doing clean
 	 * on already clean cache would be almost NOP.
 	 */
-	push {r0-r12}
-	bl v7_flush_dcache_all
-	pop {r0-r12}
+	push	{r0-r12}
+	ldr	r0, =v7_flush_dcache_all
+	mov	lr, pc
+	mov	pc, r0
+	pop	{r0-r12}
 
 	/*
 	 * Execute an ISB instruction to ensure that all of the
@@ -167,16 +238,77 @@ saved register and context as below:
 	dsb
 	dmb
 
-
+/****************************************************************
+set ddr iomux to low power mode
+****************************************************************/
+ddr_iomux_set_lpm:
+	ldr	r1, =MMDC_P0_BASE_ADDR
+	add	r1, r1, #PERIPBASE_VIRT
+	ldr	r0, [r1, #MMDC_MAPSR_OFFSET]
+	bic	r0, #MMDC_MAPSR_PSD /* enable power saving */
+	str	r0, [r1, #MMDC_MAPSR_OFFSET]
+refresh:
+	ldr	r0, [r1, #MMDC_MAPSR_OFFSET] /* MMDC_MAPSR */
+	and	r0, r0, #MMDC_MAPSR_PSS /* PSS bit */
+	cmp	r0, #0
+	beq	refresh
+
+	/* set mmdc iomux to low power mode */
+	ldr	r1, =MX6Q_IOMUXC_BASE_ADDR
+	add	r1, r1, #PERIPBASE_VIRT
+	mov	r0 , #0
+	str	r0, [r1, #0x5ac] /* DRAM_DQM0 */
+	str	r0, [r1, #0x5b4] /* DRAM_DQM1 */
+	str	r0, [r1, #0x528] /* DRAM_DQM2 */
+	str	r0, [r1, #0x520] /* DRAM_DQM3 */
+
+	str	r0, [r1, #0x514] /* DRAM_DQM4 */
+	str	r0, [r1, #0x510] /* DRAM_DQM5 */
+	str	r0, [r1, #0x5bc] /* DRAM_DQM6 */
+	str	r0, [r1, #0x5c4] /* DRAM_DQM7 */
+
+	str	r0, [r1, #0x56c] /* DRAM_CAS */
+	str	r0, [r1, #0x578] /* DRAM_RAS */
+	str	r0, [r1, #0x588] /* DRAM_SDCLK_0 */
+	str	r0, [r1, #0x594] /* DRAM_SDCLK_1 */
+
+	str	r0, [r1, #0x750] /* DDRMODE_CTL */
+	str	r0, [r1, #0x774] /* DDRMODE */
+
+	str	r0, [r1, #0x5a8] /* DRAM_SDQS0 */
+	str	r0, [r1, #0x5b0] /* DRAM_SDQS1 */
+	str	r0, [r1, #0x524] /* DRAM_SDQS2 */
+	str	r0, [r1, #0x51c] /* DRAM_SDQS3 */
+
+	str	r0, [r1, #0x518] /* DRAM_SDQS4 */
+	str	r0, [r1, #0x50c] /* DRAM_SDQS5 */
+	str	r0, [r1, #0x5b8] /* DRAM_SDQS6 */
+	str	r0, [r1, #0x5c0] /* DRAM_SDQS7 */
+
+	str	r0, [r1, #0x784] /* GPR_B0DS */
+	str	r0, [r1, #0x788] /* GPR_B1DS */
+	str	r0, [r1, #0x794] /* GPR_B2DS */
+	str	r0, [r1, #0x79c] /* GPR_B3DS */
+
+	str	r0, [r1, #0x7a0] /* GPR_B4DS */
+	str	r0, [r1, #0x7a4] /* GPR_B5DS */
+	str	r0, [r1, #0x7a8] /* GPR_B6DS */
+	str	r0, [r1, #0x748] /* GPR_B7DS */
+
+	str	r0, [r1, #0x74c] /* GPR_ADDS*/
+	str	r0, [r1, #0x59c] /* DRAM_SODT0*/
+	str	r0, [r1, #0x5a0] /* DRAM_SODT1*/
+ddr_iomux_set_lpm_done:
 /****************************************************************
 save resume pointer into SRC_GPR1
 ****************************************************************/
-	ldr r0, =resume
-	ldr r1, =va2pa_offset
-	sub r0, r0, r1
-	ldr r1, =SRC_BASE_ADDR
-	add r1, r1, #PERIPBASE_VIRT
-	str r0, [r1, #SRC_GPR1_OFFSET]
+	ldr	r0, =mx6q_suspend
+	ldr	r1, =resume
+	sub	r1, r1, r0
+	add	r3, r3, r1
+	ldr	r1, =SRC_BASE_ADDR
+	add	r1, r1, #PERIPBASE_VIRT
+	str	r3, [r1, #SRC_GPR1_OFFSET]
 /****************************************************************
 execute a wfi instruction to let SOC go into stop mode.
 ****************************************************************/
@@ -202,83 +334,142 @@ when SOC exit stop mode, arm core restart from here, currently
 are running with MMU off.
 ****************************************************************/
 resume:
-	mov r1, #0
-	ldr r0, =SRC_BASE_ADDR
-	str r1, [r0, #SRC_GPR1_OFFSET] /* clear SRC_GPR1 */
-	ldr sp, [r0, #SRC_GPR2_OFFSET]
-	ldr r1, =va2pa_offset
-	sub sp, sp, r1
+	mov	r1, #0x0
+	ldr	r0, =SRC_BASE_ADDR
+	str	r1, [r0, #SRC_GPR1_OFFSET] /* clear SRC_GPR1 */
+	ldr	r0, [r0, #SRC_GPR2_OFFSET]
+ddr_iomux_restore:
+	ldr	r1, =MX6Q_IOMUXC_BASE_ADDR
+	ldmea	r0!, {r4-r7}
+	str	r4, [r1, #0x5ac] /* DRAM_DQM0 */
+	str	r5, [r1, #0x5b4] /* DRAM_DQM1 */
+	str	r6, [r1, #0x528] /* DRAM_DQM2 */
+	str	r7, [r1, #0x520] /* DRAM_DQM3 */
+
+	ldmea	r0!, {r4-r7}
+	str	r4, [r1, #0x514] /* DRAM_DQM4 */
+	str	r5, [r1, #0x510] /* DRAM_DQM5 */
+	str	r6, [r1, #0x5bc] /* DRAM_DQM6 */
+	str	r7, [r1, #0x5c4] /* DRAM_DQM7 */
+
+	ldmea	r0!, {r4-r7}
+	str	r4, [r1, #0x56c] /* DRAM_CAS */
+	str	r5, [r1, #0x578] /* DRAM_RAS */
+	str	r6, [r1, #0x588] /* DRAM_SDCLK_0 */
+	str	r7, [r1, #0x594] /* DRAM_SDCLK_1 */
+
+	ldmea	r0!, {r5-r6}
+	@str 	r4, [r1, #0x57c] /* DRAM_RESET */
+	str	r5, [r1, #0x750] /* DDRMODE_CTL */
+	str	r6, [r1, #0x774] /* DDRMODE */
+
+	ldmea	r0!, {r4-r7}
+	str	r4, [r1, #0x5a8] /* DRAM_SDQS0 */
+	str	r5, [r1, #0x5b0] /* DRAM_SDQS1 */
+	str	r6, [r1, #0x524] /* DRAM_SDQS2 */
+	str	r7, [r1, #0x51c] /* DRAM_SDQS3 */
+
+	ldmea	r0!, {r4-r7}
+	str	r4, [r1, #0x518] /* DRAM_SDQS4 */
+	str	r5, [r1, #0x50c] /* DRAM_SDQS5 */
+	str	r6, [r1, #0x5b8] /* DRAM_SDQS6 */
+	str	r7, [r1, #0x5c0] /* DRAM_SDQS7 */
+
+	ldmea	r0!, {r4-r7}
+	str	r4, [r1, #0x784] /* GPR_B0DS */
+	str	r5, [r1, #0x788] /* GPR_B1DS */
+	str	r6, [r1, #0x794] /* GPR_B2DS */
+	str	r7, [r1, #0x79c] /* GPR_B3DS */
+
+	ldmea	r0!, {r4-r7}
+	str	r4, [r1, #0x7a0] /* GPR_B4DS */
+	str	r5, [r1, #0x7a4] /* GPR_B5DS */
+	str	r6, [r1, #0x7a8] /* GPR_B6DS */
+	str	r7, [r1, #0x748] /* GPR_B7DS */
+
+	ldmea	r0!, {r5-r7}
+	str	r5, [r1, #0x74c] /* GPR_ADDS*/
+	str	r6, [r1, #0x59c] /* DRAM_SODT0*/
+	str	r7, [r1, #0x5a0] /* DRAM_SODT1*/
+ddr_iomux_restore_done:
+
+	ldr	r2, =ddr
+	ldr	r1, =va2pa_offset
+	sub	r2, r2, r1
+	mov	pc, r2
+ddr:
 	/* Restore cp15 registers */
-	ldmea	sp!, {r5-r6}
+	ldmea	r0!, {r4-r6}
+	mov	sp, r4
 	msr	spsr_cxsf, r5		@ Restore spsr
-	mov	lr, r6				@ Restore lr
+	mov	lr, r6			@ Restore lr
 
 	/* c1 and c2 registers */
-	ldmea	sp!, {r4-r7}
-	mcr	p15, 0, r4, c1, c0, 2		@ CPACR
-	mcr	p15, 0, r5, c2, c0, 0		@ TTBR0
-	mcr	p15, 0, r6, c2, c0, 1		@ TTBR1
-	mcr	p15, 0, r7, c2, c0, 2		@ TTBCR
+	ldmea	r0!, {r4-r7}
+	mcr	p15, 0, r4, c1, c0, 2	@ CPACR
+	mcr	p15, 0, r5, c2, c0, 0	@ TTBR0
+	mcr	p15, 0, r6, c2, c0, 1	@ TTBR1
+	mcr	p15, 0, r7, c2, c0, 2	@ TTBCR
 
 	/* c3 and c10 registers */
-	ldmea	sp!,{r4-r7}
-	mcr	p15, 0, r4, c3, c0, 0		@ DACR
-	mcr	p15, 0, r5, c10, c2, 0		@ PRRR
-	mcr	p15, 0, r6, c10, c2, 1		@ NMRR
-	mcr	p15, 0, r7, c1, c0, 1		@ ACTLR
+	ldmea	r0!,{r4-r7}
+	mcr	p15, 0, r4, c3, c0, 0	@ DACR
+	mcr	p15, 0, r5, c10, c2, 0	@ PRRR
+	mcr	p15, 0, r6, c10, c2, 1	@ NMRR
+	mcr	p15, 0, r7, c1, c0, 1	@ ACTLR
 
 	/* c12, c13 and CPSR registers */
-	ldmea	sp!,{r4-r7}
-	mcr	p15, 0, r4, c13, c0, 1		@ Context ID
-	mcr	p15, 0, r5, c13, c0, 2		@ User r/w thread ID
-	mrc	p15, 0, r6, c12, c0, 0		@ Secure or NS VBAR
-	msr	cpsr, r7			@ store cpsr
+	ldmea	r0!,{r4-r7}
+	mcr	p15, 0, r4, c13, c0, 1	@ Context ID
+	mcr	p15, 0, r5, c13, c0, 2	@ User r/w thread ID
+	mrc	p15, 0, r6, c12, c0, 0	@ Secure or NS VBAR
+	msr	cpsr, r7		@ store cpsr
 
 	/*
 	 * Enabling MMU here. Page entry needs to be altered
 	 * to create temporary 1:1 map and then resore the entry
 	 * ones MMU is enabled
 	 */
-	mrc	p15, 0, r7, c2, c0, 2		@ Read TTBRControl
-	and	r7, #0x7			@ Extract N (0:2) to decide
-	cmp	r7, #0x0			@ TTBR0/TTBR1
+	mrc	p15, 0, r7, c2, c0, 2	@ Read TTBRControl
+	and	r7, #0x7		@ Extract N (0:2) to decide
+	cmp	r7, #0x0		@ TTBR0/TTBR1
 	beq	use_ttbr0
 ttbr_error:
-	b	ttbr_error			@ Only N = 0 supported
+	b	ttbr_error		@ Only N = 0 supported
 use_ttbr0:
-	mrc	p15, 0, r2, c2, c0, 0		@ Read TTBR0
+	mrc	p15, 0, r2, c2, c0, 0	@ Read TTBR0
 	ldr	r5, =TTRBIT_MASK
 	and	r2, r5
 	mov	r4, pc
 	ldr	r5, =TABLE_INDEX_MASK
-	and	r4, r5				@ r4 = 31 to 20 bits of pc
+	and	r4, r5			@ r4 = 31 to 20 bits of pc
 	ldr	r1, =TABLE_ENTRY
-	add	r1, r1, r4			@ r1 has value of table entry
-	lsr	r4, #18				@ Address of table entry
-	add	r2, r4				@ r2 - location to be modified
+	add	r1, r1, r4		@ r1 has value of table entry
+	lsr	r4, #18			@ Address of table entry
+	add	r2, r4			@ r2 - location to be modified
 
 	/* Storing previous entry of location being modified */
 	ldr	r4, [r2]
-	mov r9, r4
+	mov	r9, r4
 	str	r1, [r2]
 
 	/*
 	 * Storing address of entry being modified
 	 * It will be restored after enabling MMU
 	 */
-	mov r10, r2
+	mov 	r10, r2
 
 	mov	r1, #0
-	mcr	p15, 0, r1, c7, c5, 4		@ Flush prefetch buffer
-	mcr	p15, 0, r1, c7, c5, 6		@ Invalidate BTB
-	mcr	p15, 0, r1, c8, c5, 0		@ Invalidate ITLB
-	mcr	p15, 0, r1, c8, c6, 0		@ Invalidate DTLB
+	mcr	p15, 0, r1, c7, c5, 4	@ Flush prefetch buffer
+	mcr	p15, 0, r1, c7, c5, 6	@ Invalidate BTB
+	mcr	p15, 0, r1, c8, c5, 0	@ Invalidate ITLB
+	mcr	p15, 0, r1, c8, c6, 0	@ Invalidate DTLB
 
 	/*
 	 * Restore control register  but don't enable Data caches here.
 	 * Caches will be enabled after restoring MMU table entry.
 	 */
-	ldmea	sp!, {r4}
+	ldmea	r0!, {r4}
 	mov r11, r4
 	ldr	r2, =CACHE_DISABLE_MASK
 	and	r4, r4, r2
@@ -289,24 +480,22 @@ use_ttbr0:
 	bx	r1
 mmu_on_label:
 	/* Set up the per-CPU stacks */
-	ldr r1, =va2pa_offset
-	add sp, sp, r1
-	mov r5, lr
+	mov 	r5, lr
 	bl	cpu_init
 
 	/*
 	 * Restore the MMU table entry that was modified for
 	 * enabling MMU.
 	 */
-	mov r0, r9
-	mov r10, r0
+	mov	r0, r9
+	mov	r10, r0
 
 	mov	r0, #0
-	mcr	p15, 0, r0, c7, c1, 6		@ flush TLB and issue barriers
-	mcr	p15, 0, r0, c7, c5, 4		@ Flush prefetch buffer
-	mcr	p15, 0, r0, c7, c5, 6		@ Invalidate BTB
-	mcr	p15, 0, r0, c8, c5, 0		@ Invalidate ITLB
-	mcr	p15, 0, r0, c8, c6, 0		@ Invalidate DTLB
+	mcr	p15, 0, r0, c7, c1, 6	@ flush TLB and issue barriers
+	mcr	p15, 0, r0, c7, c5, 4	@ Flush prefetch buffer
+	mcr	p15, 0, r0, c7, c5, 6	@ Invalidate BTB
+	mcr	p15, 0, r0, c8, c5, 0	@ Invalidate ITLB
+	mcr	p15, 0, r0, c8, c6, 0	@ Invalidate DTLB
 	dsb
 	isb
 
@@ -322,22 +511,22 @@ invalidate l1 dcache, r0-r4, r6, r7 used
 
 	ldr     r1, =0x3ff
 
-	and     r3, r1, r0, lsr #3  @ NumWays - 1
-	add     r2, r2, #1          @ NumSets
+	and     r3, r1, r0, lsr #3	@ NumWays - 1
+	add     r2, r2, #1		@ NumSets
 
 	and     r0, r0, #0x7
-	add     r0, r0, #4          @ SetShift
+	add     r0, r0, #4		@ SetShift
 
-	clz     r1, r3              @ WayShift
-	add     r4, r3, #1          @ NumWays
+	clz     r1, r3			@ WayShift
+	add     r4, r3, #1		@ NumWays
 1:
-	sub     r2, r2, #1          @ NumSets--
-	mov     r3, r4              @ Temp = NumWays
+	sub     r2, r2, #1		@ NumSets--
+	mov     r3, r4			@ Temp = NumWays
 2:
-	subs    r3, r3, #1          @ Temp--
+	subs    r3, r3, #1		@ Temp--
 	mov     r7, r3, lsl r1
 	mov     r6, r2, lsl r0
-	orr     r7, r7, r6          @ Reg = (Temp<<WayShift)|(NumSets<<SetShift)
+	orr     r7, r7, r6
 	mcr     p15, 0, r7, c7, c6, 2
 	bgt     2b
 	cmp     r2, #0
@@ -348,34 +537,27 @@ invalidate l1 dcache, r0-r4, r6, r7 used
 /************************************************************
 restore control register to enable cache
 ************************************************************/
-	mov r0, r11
-	mcr     p15, 0, r0, c1, c0, 0	@ with caches enabled.
+	mov	r0, r11
+	mcr	p15, 0, r0, c1, c0, 0	@ with caches enabled.
 	isb
 
-/************************************************************
-clear src register we used
-************************************************************/
-	ldr r8, =SRC_BASE_ADDR
-	add r8, r8, #PERIPBASE_VIRT
-	ldr sp, [r8, #SRC_GPR2_OFFSET]
-
 /***********************************************************
 return back to mx6_suspend_enter for dormant
 ***********************************************************/
-	mov lr, r5
-	ldmfd   sp!, {r0-r12}
-	mov pc, lr
+	mov	lr, r5
+	ldmfd	sp!, {r0-r12}
+	mov	pc, lr
 
 
 /************************************************
 return back to mx6_suspend_enter for suspend
 *************************************************/
 out:
-	ldmfd   sp!, {r0-r12}
-	mov pc, lr
+	ldmfd	sp!, {r0-r12}
+	mov	pc, lr
 
 	.equ	va2pa_offset, (PAGE_OFFSET - MX6_PHYS_OFFSET)
-	.type   mx6q_do_suspend, #object
+	.type	mx6q_do_suspend, #object
 ENTRY(mx6q_do_suspend)
-	.word   mx6q_suspend
-	.size   mx6q_suspend, . - mx6q_suspend
+	.word	mx6q_suspend
+	.size	mx6q_suspend, . - mx6q_suspend
diff --git a/arch/arm/mach-mx6/pm.c b/arch/arm/mach-mx6/pm.c
index 4c53bb44d5ef..622ec5a241f9 100644
--- a/arch/arm/mach-mx6/pm.c
+++ b/arch/arm/mach-mx6/pm.c
@@ -42,6 +42,10 @@
 #define GPC_IMR2_OFFSET				0x0c
 #define GPC_IMR3_OFFSET				0x10
 #define GPC_IMR4_OFFSET				0x14
+#define GPC_ISR1_OFFSET				0x18
+#define GPC_ISR2_OFFSET				0x1c
+#define GPC_ISR3_OFFSET				0x20
+#define GPC_ISR4_OFFSET				0x24
 #define GPC_PGC_CPU_PDN_OFFSET		0x2a0
 #define GPC_PGC_CPU_PUPSCR_OFFSET	0x2a4
 #define GPC_PGC_CPU_PDNSCR_OFFSET	0x2a8
@@ -61,6 +65,9 @@ static int org_freq;
 extern int set_cpu_freq(int wp);
 #endif
 extern void mx6q_suspend(suspend_state_t state);
+extern void mx6_init_irq(void);
+extern int mxc_init_l2x0(void);
+extern unsigned int gpc_wake_irq[4];
 static struct device *pm_dev;
 struct clk *gpc_dvfs_clk;
 
@@ -68,9 +75,14 @@ static void __iomem *scu_base;
 static void __iomem *gpc_base;
 static void __iomem *src_base;
 static void __iomem *local_twd_base;
-static void __iomem *pl310_base;
 static void __iomem *gic_dist_base;
 static void __iomem *gic_cpu_base;
+static void __iomem *uart4_base;
+
+static void *suspend_iram_base;
+static void (*suspend_in_iram)(suspend_state_t state,
+	unsigned long iram_paddr, unsigned long suspend_iram_base) = NULL;
+static unsigned long iram_paddr, cpaddr;
 
 static u32 ccm_clpcr, scu_ctrl;
 static u32 gpc_imr[4], gpc_cpu_pup, gpc_cpu_pdn, gpc_cpu;
@@ -115,8 +127,28 @@ static void mx6_suspend_restore(void)
 	__raw_writel(local_timer[3], local_twd_base + LOCAL_TWD_INT_OFFSET);
 #endif
 }
+
 static int mx6_suspend_enter(suspend_state_t state)
 {
+	unsigned int wake_irq_isr[4];
+
+	wake_irq_isr[0] = __raw_readl(gpc_base +
+			GPC_ISR1_OFFSET) & gpc_wake_irq[0];
+	wake_irq_isr[1] = __raw_readl(gpc_base +
+			GPC_ISR1_OFFSET) & gpc_wake_irq[1];
+	wake_irq_isr[2] = __raw_readl(gpc_base +
+			GPC_ISR1_OFFSET) & gpc_wake_irq[2];
+	wake_irq_isr[3] = __raw_readl(gpc_base +
+			GPC_ISR1_OFFSET) & gpc_wake_irq[3];
+	if (wake_irq_isr[0] | wake_irq_isr[1] |
+			wake_irq_isr[2] | wake_irq_isr[3]) {
+		printk(KERN_INFO "There are wakeup irq pending,system resume!\n");
+		printk(KERN_INFO "wake_irq_isr[0-3]: 0x%x, 0x%x, 0x%x, 0x%x\n",
+				wake_irq_isr[0], wake_irq_isr[1],
+				wake_irq_isr[2], wake_irq_isr[3]);
+		return 0;
+	}
+
 	mx6_suspend_store();
 
 	switch (state) {
@@ -136,19 +168,19 @@ static int mx6_suspend_enter(suspend_state_t state)
 
 		local_flush_tlb_all();
 		flush_cache_all();
+#ifdef CONFIG_CACHE_L2X0
 		outer_cache.flush_all();
 
 		/* for dormant mode, we need to disable l2 cache */
 		if (state == PM_SUSPEND_MEM)
 			outer_cache.disable();
-
-		/* mx6q mmdc can enter self-refresh when ARM enter wfi
-		 * , so no need to run the code in the iram */
-		mx6q_suspend(state);
+#endif
+		suspend_in_iram(state, (unsigned long)iram_paddr,
+			(unsigned long)suspend_iram_base);
 
 		if (state == PM_SUSPEND_MEM) {
-			/* need to re-init gic */
-			gic_init(0, 29, gic_dist_base, gic_cpu_base);
+			/* need to re-init irq */
+			mx6_init_irq();
 
 #ifdef CONFIG_LOCAL_TIMERS
 			gic_enable_ppi(IRQ_LOCALTIMER);
@@ -162,8 +194,10 @@ static int mx6_suspend_enter(suspend_state_t state)
 					(MXC_INT_GPT / 32) * 4);
 
 			flush_cache_all();
+#ifdef CONFIG_CACHE_L2X0
 			/* init l2 cache, pl310 */
-			l2x0_init(pl310_base, 0x0, ~0x00000000);
+			mxc_init_l2x0();
+#endif
 		}
 
 		mx6_suspend_restore();
@@ -247,10 +281,10 @@ static int __init pm_init(void)
 	scu_base = IO_ADDRESS(SCU_BASE_ADDR);
 	gpc_base = IO_ADDRESS(GPC_BASE_ADDR);
 	src_base = IO_ADDRESS(SRC_BASE_ADDR);
-	pl310_base = IO_ADDRESS(L2_BASE_ADDR);
 	gic_dist_base = IO_ADDRESS(IC_DISTRIBUTOR_BASE_ADDR);
 	gic_cpu_base = IO_ADDRESS(IC_INTERFACES_BASE_ADDR);
 	local_twd_base = IO_ADDRESS(LOCAL_TWD_ADDR);
+	uart4_base = IO_ADDRESS(0x21f0000);
 
 	pr_info("Static Power Management for Freescale i.MX6\n");
 
@@ -260,6 +294,23 @@ static int __init pm_init(void)
 	}
 
 	suspend_set_ops(&mx6_suspend_ops);
+	/* Move suspend routine into iRAM */
+	cpaddr = (unsigned long)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_MEMORY);
+	pr_info("cpaddr = %x suspend_iram_base=%x\n",
+		(unsigned int)cpaddr, (unsigned int)suspend_iram_base);
+
+	/*
+	 * Need to run the suspend code from IRAM as the DDR needs
+	 * to be put into low power mode manually.
+	 */
+	memcpy((void *)cpaddr, mx6q_suspend, SZ_4K);
+
+	suspend_in_iram = (void *)suspend_iram_base;
+
 	cpu_clk = clk_get(NULL, "cpu_clk");
 	if (IS_ERR(cpu_clk)) {
 		printk(KERN_DEBUG "%s: failed to get cpu_clk\n", __func__);
diff --git a/arch/arm/plat-mxc/devices/Kconfig b/arch/arm/plat-mxc/devices/Kconfig
index 3d6c53fe2974..06b383aa32ec 100644
--- a/arch/arm/plat-mxc/devices/Kconfig
+++ b/arch/arm/plat-mxc/devices/Kconfig
@@ -110,6 +110,9 @@ config IMX_HAVE_PLATFORM_MXC_GPU
 config IMX_HAVE_PLATFORM_IMX_OCOTP
 	bool
 
+config IMX_HAVE_PLATFORM_IMX_VIIM
+	bool
+
 config IMX_HAVE_PLATFORM_IMX_DCP
 	bool
 
diff --git a/arch/arm/plat-mxc/devices/Makefile b/arch/arm/plat-mxc/devices/Makefile
index 00d5f5160895..d4c1b24c9bad 100644
--- a/arch/arm/plat-mxc/devices/Makefile
+++ b/arch/arm/plat-mxc/devices/Makefile
@@ -35,6 +35,7 @@ obj-$(CONFIG_IMX_HAVE_PLATFORM_AHCI) +=  platform-ahci-imx.o
 obj-$(CONFIG_IMX_HAVE_PLATFORM_IMX_IIM)	+= platform-imx-iim.o
 obj-$(CONFIG_IMX_HAVE_PLATFORM_MXC_GPU) +=  platform-mxc_gpu.o
 obj-$(CONFIG_IMX_HAVE_PLATFORM_IMX_OCOTP) += platform-imx-ocotp.o
+obj-$(CONFIG_IMX_HAVE_PLATFORM_IMX_VIIM) += platform-imx-viim.o
 obj-$(CONFIG_IMX_HAVE_PLATFORM_IMX_DCP) += platform-imx-dcp.o
 obj-$(CONFIG_IMX_HAVE_PLATFORM_RANDOM_RNGC) += platform-imx-rngb.o
 obj-$(CONFIG_IMX_HAVE_PLATFORM_PERFMON) += platform-imx-perfmon.o
diff --git a/arch/arm/plat-mxc/devices/platform-imx-viim.c b/arch/arm/plat-mxc/devices/platform-imx-viim.c
new file mode 100644
index 000000000000..1abc2acb0fdd
--- /dev/null
+++ b/arch/arm/plat-mxc/devices/platform-imx-viim.c
@@ -0,0 +1,45 @@
+/*
+ * Copyright (C) 2011 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
+ * Free Software Foundation.
+ */
+#include <mach/hardware.h>
+#include <mach/devices-common.h>
+#include <linux/fsl_devices.h>
+
+#ifdef CONFIG_SOC_IMX51
+#define GPT_REG_BASE_ADDR (MX50_GPT1_BASE_ADDR)
+const struct imx_viim_data imx50_viim_data = {
+	.iobase = MX50_OCOTP_CTRL_BASE_ADDR,
+};
+#endif
+
+#ifdef CONFIG_SOC_IMX6Q
+#define GPT_REG_BASE_ADDR (GPT_BASE_ADDR)
+const struct imx_viim_data imx6q_viim_data = {
+	.iobase = OCOTP_BASE_ADDR,
+};
+#endif
+
+struct platform_device *__init imx_add_viim(
+		const struct imx_viim_data *data)
+{
+	struct resource res[] = {
+		[0] = {
+			.start = GPT_REG_BASE_ADDR,
+			.end   = GPT_REG_BASE_ADDR + PAGE_SIZE - 1,
+			.flags = IORESOURCE_MEM,
+		},
+		[1] = {
+			.start  = data->iobase,
+			.end    = data->iobase + PAGE_SIZE - 1,
+			.flags  = IORESOURCE_MEM,
+		},
+	};
+
+	return imx_add_platform_device("mxs-viim", 0,
+			res, ARRAY_SIZE(res), NULL, 0);
+}
+
diff --git a/arch/arm/plat-mxc/gpio.c b/arch/arm/plat-mxc/gpio.c
index 66630785e02b..5b72ff40914a 100644
--- a/arch/arm/plat-mxc/gpio.c
+++ b/arch/arm/plat-mxc/gpio.c
@@ -299,9 +299,10 @@ static int mxc_gpio_direction_output(struct gpio_chip *chip,
 	return 0;
 }
 
-int __init mxc_gpio_init(struct mxc_gpio_port *port, int cnt)
+int mxc_gpio_init(struct mxc_gpio_port *port, int cnt)
 {
 	int i, j;
+	static bool initialed;
 
 	/* save for local usage */
 	mxc_gpio_ports = port;
@@ -330,8 +331,9 @@ int __init mxc_gpio_init(struct mxc_gpio_port *port, int cnt)
 
 		spin_lock_init(&port[i].lock);
 
-		/* its a serious configuration bug when it fails */
-		BUG_ON( gpiochip_add(&port[i].chip) < 0 );
+		if (!initialed)
+			/* its a serious configuration bug when it fails */
+			BUG_ON(gpiochip_add(&port[i].chip) < 0);
 
 		if (cpu_is_mx1() || cpu_is_mx3() || cpu_is_mx25() ||
 			cpu_is_mx51() || cpu_is_mx53() || cpu_is_mx6q()) {
@@ -346,7 +348,7 @@ int __init mxc_gpio_init(struct mxc_gpio_port *port, int cnt)
 			}
 		}
 	}
-
+	initialed = true;
 	if (cpu_is_mx2()) {
 		/* setup one handler for all GPIO interrupts */
 		set_irq_chained_handler(port[0].irq, mx2_gpio_irq_handler);
diff --git a/arch/arm/plat-mxc/include/mach/devices-common.h b/arch/arm/plat-mxc/include/mach/devices-common.h
index b06cc6b6f292..b1803be4db94 100644
--- a/arch/arm/plat-mxc/include/mach/devices-common.h
+++ b/arch/arm/plat-mxc/include/mach/devices-common.h
@@ -394,6 +394,13 @@ struct imx_otp_data {
 struct platform_device *__init imx_add_otp(
 		const struct imx_otp_data *data);
 
+struct imx_viim_data {
+	resource_size_t iobase;
+};
+
+struct platform_device *__init imx_add_viim(
+		const struct imx_viim_data *data);
+
 struct imx_dcp_data {
 	resource_size_t iobase;
 	resource_size_t irq1;
diff --git a/drivers/char/Kconfig b/drivers/char/Kconfig
index c557419c176f..efb0c2bd3cc4 100644
--- a/drivers/char/Kconfig
+++ b/drivers/char/Kconfig
@@ -1179,5 +1179,11 @@ config MXC_IIM
 	help
 	  Support for access to MXC IIM device, most people should say N here.
 
+config MXS_VIIM
+	tristate "MXS Virtual IIM device driver"
+	depends on (ARCH_MX50 || ARCH_MX6)
+	help
+	  Support for access to MXS Virtual IIM device, most people should say N here.
+
 endmenu
 
diff --git a/drivers/char/Makefile b/drivers/char/Makefile
index f97d5a0a2810..044d61b7121b 100644
--- a/drivers/char/Makefile
+++ b/drivers/char/Makefile
@@ -86,6 +86,7 @@ obj-$(CONFIG_PS3_FLASH)		+= ps3flash.o
 obj-$(CONFIG_RAMOOPS)		+= ramoops.o
 
 obj-$(CONFIG_MXC_IIM)           += mxc_iim.o
+obj-$(CONFIG_MXS_VIIM)		+= mxs_viim.o
 
 obj-$(CONFIG_JS_RTC)		+= js-rtc.o
 js-rtc-y = rtc.o
diff --git a/drivers/char/fsl_otp.h b/drivers/char/fsl_otp.h
index fa2b21e12a88..5f779e53e41a 100755
--- a/drivers/char/fsl_otp.h
+++ b/drivers/char/fsl_otp.h
@@ -190,7 +190,7 @@ static int set_otp_timing(struct mxc_otp_platform_data *otp_data)
 /* IMX5 does not need to open the bank anymore */
 static int otp_read_prepare(struct mxc_otp_platform_data *otp_data)
 {
-	return set_otp_timing();
+	return set_otp_timing(otp_data);
 }
 static int otp_read_post(struct mxc_otp_platform_data *otp_data)
 {
@@ -202,7 +202,7 @@ static int otp_write_prepare(struct mxc_otp_platform_data *otp_data)
 	int ret = 0;
 
 	/* [1] set timing */
-	ret = set_otp_timing();
+	ret = set_otp_timing(otp_data);
 	if (ret)
 		return ret;
 
diff --git a/drivers/char/mxs_viim.c b/drivers/char/mxs_viim.c
new file mode 100644
index 000000000000..31195b39aa42
--- /dev/null
+++ b/drivers/char/mxs_viim.c
@@ -0,0 +1,175 @@
+/*
+ * Copyright (C) 2009-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/fs.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/err.h>
+#include <linux/mm.h>
+#include <linux/miscdevice.h>
+
+static unsigned long iim_reg_base0, iim_reg_end0, iim_reg_size0;
+static unsigned long iim_reg_base1, iim_reg_end1, iim_reg_size1;
+static struct device *iim_dev;
+
+/*!
+ * MXS Virtual IIM interface - memory map function
+ * This function maps one page size VIIM registers from VIIM base address0
+ * if the size of the required virtual memory space is less than or equal to
+ * one page size, otherwise this function will also map one page size VIIM
+ * registers from VIIM base address1.
+ *
+ * @param file	     struct file *
+ * @param vma	     structure vm_area_struct *
+ *
+ * @return	     Return 0 on success or negative error code on error
+ */
+static int mxs_viim_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	size_t size = vma->vm_end - vma->vm_start;
+
+	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
+
+	/* Remap-pfn-range will mark the range VM_IO and VM_RESERVED */
+	if (remap_pfn_range(vma,
+			    vma->vm_start,
+			    iim_reg_base0 >> PAGE_SHIFT,
+			    iim_reg_size0,
+			    vma->vm_page_prot))
+		return -EAGAIN;
+
+	if (size > iim_reg_size0) {
+		if (remap_pfn_range(vma,
+				    vma->vm_start + iim_reg_size0,
+				    iim_reg_base1 >> PAGE_SHIFT,
+				    iim_reg_size1,
+				    vma->vm_page_prot))
+			return -EAGAIN;
+	}
+
+	return 0;
+}
+
+/*!
+ * MXS Virtual IIM interface - open function
+ *
+ * @param inode	     struct inode *
+ * @param filp	     struct file *
+ *
+ * @return	     Return 0 on success or negative error code on error
+ */
+static int mxs_viim_open(struct inode *inode, struct file *filp)
+{
+	return 0;
+}
+
+/*!
+ * MXS Virtual IIM interface - release function
+ *
+ * @param inode	     struct inode *
+ * @param filp	     struct file *
+ *
+ * @return	     Return 0 on success or negative error code on error
+ */
+static int mxs_viim_release(struct inode *inode, struct file *filp)
+{
+	return 0;
+}
+
+static const struct file_operations mxs_viim_fops = {
+	.mmap = mxs_viim_mmap,
+	.open = mxs_viim_open,
+	.release = mxs_viim_release,
+};
+
+static struct miscdevice mxs_viim_miscdev = {
+	.minor = MISC_DYNAMIC_MINOR,
+	.name = "mxs_viim",
+	.fops = &mxs_viim_fops,
+};
+
+/*!
+ * This function is called by the driver framework to get virtual iim base/end
+ * address and register iim misc device.
+ *
+ * @param	dev	The device structure for Virtual IIM passed in by the
+ *			driver framework.
+ *
+ * @return      Returns 0 on success or negative error code on error
+ */
+static int mxs_viim_probe(struct platform_device *pdev)
+{
+	struct resource *res;
+	int ret;
+
+	iim_dev = &pdev->dev;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (IS_ERR(res)) {
+		dev_err(iim_dev, "Unable to get Virtual IIM resource 0\n");
+		return -ENODEV;
+	}
+
+	iim_reg_base0 = res->start;
+	iim_reg_end0 = res->end;
+	iim_reg_size0 = iim_reg_end0 - iim_reg_base0 + 1;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+	if (IS_ERR(res)) {
+		dev_err(iim_dev, "Unable to get Virtual IIM resource 1\n");
+		return -ENODEV;
+	}
+
+	iim_reg_base1 = res->start;
+	iim_reg_end1 = res->end;
+	iim_reg_size1 = iim_reg_end1 - iim_reg_base1 + 1;
+
+	ret = misc_register(&mxs_viim_miscdev);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static int mxs_viim_remove(struct platform_device *pdev)
+{
+	misc_deregister(&mxs_viim_miscdev);
+	return 0;
+}
+
+static struct platform_driver mxs_viim_driver = {
+	.driver = {
+		   .owner = THIS_MODULE,
+		   .name = "imx_viim",
+		   },
+	.probe = mxs_viim_probe,
+	.remove = mxs_viim_remove,
+};
+
+static int __init mxs_viim_dev_init(void)
+{
+	return platform_driver_register(&mxs_viim_driver);
+}
+
+static void __exit mxs_viim_dev_cleanup(void)
+{
+	platform_driver_unregister(&mxs_viim_driver);
+}
+
+module_init(mxs_viim_dev_init);
+module_exit(mxs_viim_dev_cleanup);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("IMX Virtual IIM driver");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS_MISCDEV(MISC_DYNAMIC_MINOR);
diff --git a/sound/soc/codecs/cs42888.c b/sound/soc/codecs/cs42888.c
index f64459cd9f07..ee1c8a88d587 100644
--- a/sound/soc/codecs/cs42888.c
+++ b/sound/soc/codecs/cs42888.c
@@ -810,20 +810,6 @@ static int cs42888_probe(struct snd_soc_codec *codec)
 	}
 	msleep(1);
 
-	/* Verify that we have a CS42888 */
-	val = snd_soc_read(codec, CS42888_CHIPID);
-	if (val < 0) {
-		pr_err("Device with ID register %x is not a CS42888", val);
-		return -ENODEV;
-	}
-	/* The top four bits of the chip ID should be 0000. */
-	if ((val & CS42888_CHIPID_ID_MASK) != 0x00) {
-		dev_err(codec->dev, "device is not a CS42888\n");
-		return -ENODEV;
-	}
-
-	dev_info(codec->dev, "hardware revision %X\n", val & 0xF);
-
 	/* The I2C interface is set up, so pre-fill our register cache */
 	ret = cs42888_fill_cache(codec);
 	if (ret < 0) {
@@ -912,6 +898,23 @@ static int cs42888_i2c_probe(struct i2c_client *i2c_client,
 {
 	struct cs42888_private *cs42888;
 	int ret;
+	int val;
+
+	/* Verify that we have a CS42888 */
+	val = i2c_smbus_read_byte_data(i2c_client, CS42888_CHIPID);
+	if (val < 0) {
+		pr_err("Device with ID register %x is not a CS42888", val);
+		return -ENODEV;
+	}
+	/* The top four bits of the chip ID should be 0000. */
+	if ((val & CS42888_CHIPID_ID_MASK) != 0x00) {
+		dev_err(&i2c_client->dev, "device is not a CS42888\n");
+		return -ENODEV;
+	}
+
+	dev_info(&i2c_client->dev, "found device at i2c address %X\n",
+		i2c_client->addr);
+	dev_info(&i2c_client->dev, "hardware revision %X\n", val & 0xF);
 
 	/* Allocate enough space for the snd_soc_codec structure
 	   and our private data together. */