ENGR00154917 [MX6]Enable MMDC low-power mode

1. Set MMDC pad ctrl to low-power mode when dormant;
2. DRAM_RESET can't be changed due to hardware design;
3. GPR_CTLDS should be changed to lower the MMDC IO
power to 0mA, but it needs hardware change, will add it
in next hardware version after we figure out how to
change the hardware.

Current MMDC data in dormant is:
IO: 28mA@1.5V;
DDR: 35mA@1.5V.

4. Change the suspend code to run in iRAM;

Signed-off-by: Anson Huang <b20788@freescale.com>

2 files changed, 331 insertions, 128 deletions

diff --git a/arch/arm/mach-mx6/mx6q_suspend.S b/arch/arm/mach-mx6/mx6q_suspend.S
index 4c304c9b10c..5d2aca1c9dd 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 2a94689bbf6..420eb929db9 100644
--- a/arch/arm/mach-mx6/pm.c
+++ b/arch/arm/mach-mx6/pm.c
@@ -73,6 +73,11 @@ static void __iomem *pl310_base;
 static void __iomem *gic_dist_base;
 static void __iomem *gic_cpu_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;
 static u32 local_timer[4];
@@ -144,9 +149,8 @@ static int mx6_suspend_enter(suspend_state_t state)
 		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);
+		suspend_in_iram(state, (unsigned long)iram_paddr,
+			(unsigned long)suspend_iram_base);
 
 		if (state == PM_SUSPEND_MEM) {
 			/* need to re-init irq */
@@ -262,6 +266,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__);