ENGR00316180: [iMX6x] Support IRAM page table when DDR is in self-refresh.

Whenever DDR is explicitly put into self-refresh, we need to ensure
that no access are made to the DDR. All the bus masters excpet ARM
are shutdown gracefully.
The ARM core can continue to access the DDR due to:
1. Speculative accesses
   This can be prevented by flushing the Branch Target Address Cache
2. Aggressive Prefetching
   This can be minimized by adding nops.
Apart from this the TLB architecture in ARM does not guarantee that
an entry remains in the TLB unless its explicitly locked. Even if
free slots are available an entry maybe evicted. So flushing the TLB
does not guarantee a page table walk will not happen.

The solution is to put a minimized page table in IRAM that can be used when
DDR is in self-refresh. The IRAM page tables should have entries for IRAM,
AIPS1 and AIPS2 as these entries will be needed by the code that puts DDR
into self-refresh. It should not contain any entries that point to the DDR.

This patch set accomplishes the following:
1. Set the IRAM to be mapped as 1M sections in the high mem region.
   This makes it possible to create entries for the IRAM code in the
   IRAM page table. We need to ensure that both the DDR and IRAM page
   table have mapping for the IRAM code.
2. Ensure the IRAM, AIPS1, AIPS2 have entries in the IRAM page table.
3. Save TTBR1
4. Set TTBR1 to point to the page tables stored in IRAM. Switch to using
TTBR1 before DDR is put into self-refresh. Ensure the following settings:
    a. TTBCR.N = 1
     This means the 0-2G virtual address space is translated using TTBR0
     and 2G-4G is translated using TTBR1.
    b. Set TTBCR.PD0 = 1
      With this setting page table walks using TTBR0 are disabled.
4. After the DDR has exited self-refresh, reset TTBCR to 0 (TTBR0 will
be used for translations now).
5. Restore TTBR1

Even though TTBR1 is only used to decode the top 2G of virtual address
space, ARM requires that we allocate the entire 16KB for the page table.
To minimize IRAM/OCRAM required, we put the code in the bottom 8K and
page table entries in the top 8K.
This requires the low power code be optimized to occupy as little space
as possible. Only the WFI and suspend code that puts DDR into self-refresh
is located in this 8k. The DDR frequency code that also puts the DDR into
self-refresh need not be located in this 8K region. Currently this patch
allocates a separate 4K region in IRAM for the DDR frequency change code.

Additional conditions to be met:
1. Disable L2 when DDR is in self-refresh.
2. Ensure that L1 and branch prediction are disabled when we
are switching to IRAM page tables, based on recommendation from
ARM.

This patch also dynamically calculate size of all the code that puts
DDR into self-refresh (low power and ddr freq change).

Signed-off-by: Ranjani Vaidyanathan <Ranjani.Vaidyanathan@freescale.com>
(cherry picked from commit 67779bf67ebf4daddea33693f42cf01eced14bd9)

12 files changed, 946 insertions, 378 deletions

diff --git a/arch/arm/mach-mx6/bus_freq.c b/arch/arm/mach-mx6/bus_freq.c
index d0e90bd29963..3031b0645087 100644
--- a/arch/arm/mach-mx6/bus_freq.c
+++ b/arch/arm/mach-mx6/bus_freq.c
@@ -53,8 +53,6 @@
 #define GPC_PGC_GPU_PGCR_OFFSET	0x260
 #define GPC_CNTR_OFFSET		0x0
 
-static DEFINE_SPINLOCK(freq_lock);
-
 int low_bus_freq_mode;
 int audio_bus_freq_mode;
 int high_bus_freq_mode;
@@ -78,16 +76,23 @@ unsigned int ddr_normal_rate;
 int low_freq_bus_used(void);
 void set_ddr_freq(int ddr_freq);
 void *mx6sl_wfi_iram_base;
+unsigned long mx6sl_wfi_iram_phys_addr;
+unsigned long total_wfi_lpddr2_iram_size;
+
+
 void (*mx6sl_wfi_iram)(int arm_podf, unsigned long wfi_iram_addr,\
 		int audio_mode) = NULL;
 extern void mx6sl_wait (int arm_podf, unsigned long wfi_iram_addr);
 
-
 extern int init_mmdc_settings(void);
 extern struct cpu_op *(*get_cpu_op)(int *op);
 extern int update_ddr_freq(int ddr_rate);
+extern unsigned long total_suspend_size;
 extern int chip_rev;
 
+extern unsigned long mx6sl_wfi_iram_end asm("mx6sl_wfi_iram_end");
+extern unsigned long mx6sl_wfi_iram_start asm("mx6sl_wfi_iram_start");
+
 DEFINE_MUTEX(bus_freq_mutex);
 
 struct timeval start_time;
@@ -151,7 +156,6 @@ void reduce_bus_freq(void)
 	} else {
 		u32 reg;
 		u32  div;
-		unsigned long flags;
 
 		if (high_bus_freq_mode) {
 			/* Set periph_clk to be sourced from OSC_CLK */
@@ -164,20 +168,19 @@ void reduce_bus_freq(void)
 				clk_round_rate(ahb_clk, LPAPM_CLK));
 		}
 		if (lp_audio_freq) {
-			/* PLL2 is on in this mode, as DDR is at 50MHz. */
+			/* PLL2 is on in this mode, as DDR is at 100MHz. */
 			/* Now change DDR freq while running from IRAM. */
 
 			/* Set AHB to 24MHz. */
 			clk_set_rate(ahb_clk,
 				clk_round_rate(ahb_clk, LPAPM_CLK / 3));
 
-			spin_lock_irqsave(&freq_lock, flags);
+
 			update_ddr_freq(DDR_AUDIO_CLK);
-			spin_unlock_irqrestore(&freq_lock, flags);
 
 			if (low_bus_freq_mode) {
 				/* Swtich ARM to run off PLL2_PFD2_400MHz
-				 * since DDR is anway at 50MHz.
+				 * since DDR is anyway at 100MHz.
 				 */
 				clk_set_parent(pll1_sw_clk, pll2_400);
 
@@ -215,10 +218,8 @@ void reduce_bus_freq(void)
 				;
 			clk_set_parent(pll1_sw_clk, pll1);
 
-			spin_lock_irqsave(&freq_lock, flags);
-			/* Now change DDR freq while running from IRAM. */
+
 			update_ddr_freq(LPAPM_CLK);
-			spin_unlock_irqrestore(&freq_lock, flags);
 
 			low_bus_freq_mode = 1;
 			audio_bus_freq_mode = 0;
@@ -324,12 +325,9 @@ int set_high_bus_freq(int high_bus_freq)
 
 	if (cpu_is_mx6sl()) {
 		u32 reg;
-		unsigned long flags;
 
-		spin_lock_irqsave(&freq_lock, flags);
 		/* Change DDR freq in IRAM. */
 		update_ddr_freq(ddr_normal_rate);
-		spin_unlock_irqrestore(&freq_lock, flags);
 
 		/* Set periph_clk to be sourced from pll2_pfd2_400M */
 		/* First need to set the divider before changing the */
@@ -716,24 +714,29 @@ static int __devinit busfreq_probe(struct platform_device *pdev)
 	INIT_DELAYED_WORK(&low_bus_freq_handler, reduce_bus_freq_handler);
 	register_pm_notifier(&imx_bus_freq_pm_notifier);
 
-
 	if (cpu_is_mx6sl()) {
-		unsigned long iram_paddr;
+		u32 wfi_code_size;
 
-		/* Allocate IRAM for WFI code when system is
-		  * in low freq mode.
-		  */
-		iram_alloc(SZ_4K, &iram_paddr);
-		/* Need to remap the area here since we want
-		   * the memory region to be executable.
-		   */
-		mx6sl_wfi_iram_base = __arm_ioremap(iram_paddr,
-						SZ_4K, MT_MEMORY_NONCACHED);
-		memcpy(mx6sl_wfi_iram_base, mx6sl_wait, SZ_4K);
-		mx6sl_wfi_iram = (void *)mx6sl_wfi_iram_base;
+		/* Use preallocated memory */
+		mx6sl_wfi_iram_phys_addr = MX6_SUSPEND_IRAM_CODE + total_suspend_size;
+
+		wfi_code_size = (&mx6sl_wfi_iram_end -&mx6sl_wfi_iram_start) *4;
+		total_wfi_lpddr2_iram_size = wfi_code_size + MX6_LPDDR2_WFI_DATA_SIZE;
 
+		/*
+		 * Don't ioremap the address, we have fixed the IRAM address
+		 * at IRAM_BASE_ADDR_VIRT
+		 */
+		mx6sl_wfi_iram_base = (void *)IRAM_BASE_ADDR_VIRT +
+			(mx6sl_wfi_iram_phys_addr - IRAM_BASE_ADDR);
+
+		memcpy(mx6sl_wfi_iram_base, mx6sl_wait, wfi_code_size);
+		mx6sl_wfi_iram = (void *)mx6sl_wfi_iram_base;
 	}
-	init_mmdc_settings();
+
+	err = init_mmdc_settings();
+	if (err)
+		return err;
 
 	return 0;
 }
diff --git a/arch/arm/mach-mx6/cpu.c b/arch/arm/mach-mx6/cpu.c
index d71c02fd97eb..495f62e2ca11 100644
--- a/arch/arm/mach-mx6/cpu.c
+++ b/arch/arm/mach-mx6/cpu.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2011-2013 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright (C) 2011-2014 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
@@ -39,6 +39,8 @@ u32 arm_max_freq = CPU_AT_1_2GHz;
 bool mem_clk_on_in_wait;
 bool enet_to_gpio_6;
 int chip_rev;
+unsigned long iram_tlb_base_addr;
+unsigned long iram_tlb_phys_addr;
 
 void __iomem *gpc_base;
 void __iomem *ccm_base;
@@ -49,6 +51,28 @@ static int cpu_silicon_rev = -1;
 #define MX6_USB_ANALOG_DIGPROG  0x260
 #define MX6SL_USB_ANALOG_DIGPROG  0x280
 
+unsigned long save_ttbr1(void)
+{
+	unsigned long lttbr1;
+
+	asm volatile(
+		".align 4\n"
+		"mrc p15, 0, %0, c2, c0, 1\n"
+	: "=r" (lttbr1)
+	);
+
+	return lttbr1;
+}
+
+void restore_ttbr1(u32 ttbr1)
+{
+	asm volatile(
+		".align 4\n"
+		"mcr p15, 0, %0, c2, c0, 1\n"
+	: : "r" (ttbr1)
+	);
+}
+
 static int mx6_get_srev(void)
 {
 	void __iomem *anatop = MX6_IO_ADDRESS(ANATOP_BASE_ADDR);
@@ -129,6 +153,7 @@ static int __init post_cpu_init(void)
 	unsigned int reg;
 	void __iomem *base;
 	u32	iram_size;
+	int i;
 
 	if (cpu_is_mx6q())
 		iram_size = MX6Q_IRAM_SIZE;
@@ -233,6 +258,34 @@ static int __init post_cpu_init(void)
 		reg |= IOMUXC_GPR1_TEST_POWERDOWN;
 		__raw_writel(reg, IOMUXC_GPR1);
 	}
+
+	/*
+	 * Allocate the bottom 16K of IRAM page tables that
+	 * will be used when DDR is in self-refresh.
+	 */
+	iram_tlb_phys_addr = MX6_IRAM_TLB_BASE_ADDR;
+	iram_tlb_base_addr = (unsigned long)__arm_ioremap(iram_tlb_phys_addr,
+					MX6_IRAM_TLB_SIZE, MT_MEMORY_NONCACHED);
+	/* Set all entries to 0. */
+	memset((void *)iram_tlb_base_addr, 0, MX6_IRAM_TLB_SIZE);
+
+	/* Make sure the IRAM virtual address has a mapping in the IRAM page table. */
+	i = ((IRAM_BASE_ADDR_VIRT >> 20) << 2) / 4;
+	*((unsigned long *)iram_tlb_base_addr + i) =
+		(IRAM_BASE_ADDR & 0xFFF00000) | TT_ATTRIB_NON_CACHEABLE_1M;
+	/* Make sure the AIPS1 virtual address has a mapping in the IRAM page table. */
+	i = ((AIPS1_BASE_ADDR_VIRT >> 20) << 2) / 4;
+	*((unsigned long *)iram_tlb_base_addr + i) =
+		(AIPS1_ARB_BASE_ADDR & 0xFFF00000) | TT_ATTRIB_NON_CACHEABLE_1M;
+	/* Make sure the AIPS2 virtual address has a mapping in the IRAM page table. */
+	i = ((AIPS2_BASE_ADDR_VIRT >> 20) << 2) / 4;
+	*((unsigned long *)iram_tlb_base_addr + i) =
+		(AIPS2_ARB_BASE_ADDR & 0xFFF00000) | TT_ATTRIB_NON_CACHEABLE_1M;
+	/* Make sure the AIPS2 virtual address has a mapping in the IRAM page table. */
+	i = ((L2_BASE_ADDR_VIRT >> 20) << 2) / 4;
+	*((unsigned long *)iram_tlb_base_addr + i) =
+		(L2_BASE_ADDR & 0xFFF00000) | TT_ATTRIB_NON_CACHEABLE_1M;
+
 	return 0;
 }
 postcore_initcall(post_cpu_init);
diff --git a/arch/arm/mach-mx6/mm.c b/arch/arm/mach-mx6/mm.c
index bdec4f879da9..ea9d5f277523 100644
--- a/arch/arm/mach-mx6/mm.c
+++ b/arch/arm/mach-mx6/mm.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2011-2013 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright (C) 2011-2014 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
@@ -56,6 +56,11 @@ static struct map_desc mx6_io_desc[] __initdata = {
 	.pfn = __phys_to_pfn(ARM_PERIPHBASE),
 	.length = ARM_PERIPHBASE_SIZE,
 	.type = MT_DEVICE},
+	{
+	.virtual = IRAM_BASE_ADDR_VIRT,
+	.pfn = __phys_to_pfn(IRAM_BASE_ADDR),
+	.length = IRAM_VIRT_SIZE,
+	.type = MT_MEMORY_NONCACHED},
 };
 
 static void mx6_set_cpu_type(void)
diff --git a/arch/arm/mach-mx6/mx6_ddr_freq.S b/arch/arm/mach-mx6/mx6_ddr_freq.S
index de20f0c3248a..6061fbbd7619 100644
--- a/arch/arm/mach-mx6/mx6_ddr_freq.S
+++ b/arch/arm/mach-mx6/mx6_ddr_freq.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2011-2013 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright (C) 2011-2014 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
@@ -19,6 +19,10 @@
 #include <linux/linkage.h>
 #include <mach/hardware.h>
 
+.extern iram_tlb_phys_addr
+.globl mx6_ddr3_iram_start
+.globl mx6_ddr3_iram_end
+
 	.macro   switch_to_528MHz
 
 	/* DDR freq change to 528MHz */
@@ -378,15 +382,9 @@ skip_gpt_workaround4:
  *  IRQs are already disabled.
  */
 ENTRY(mx6_ddr_freq_change)
+mx6_ddr3_iram_start:
 
-	stmfd   sp!, {r4,r5,r6, r7, r8, r9, r10, r11}      @ Save registers
-
-	ldr     r6, =CCM_BASE_ADDR
-	add     r6, r6, #PERIPBASE_VIRT
-	ldr     r5, =MMDC_P0_BASE_ADDR
-	add     r5, r5, #PERIPBASE_VIRT
-	ldr     r7, =MX6Q_IOMUXC_BASE_ADDR
-	add     r7, r7, #PERIPBASE_VIRT
+	stmfd   sp!, {r4 - r11}      @ Save registers
 
 	mov     r4, r0				@save new freq requested
 	mov     r8, r1				@save the ddr settings for the new rate
@@ -394,46 +392,80 @@ ENTRY(mx6_ddr_freq_change)
 	mov     r11, r3			@save iomux offsets
 
 ddr_freq_change:
-	/* Make sure no TLB miss will occur when the DDR is in self refresh. */
-	/* Invalidate TLB single entry to ensure that the address is not
-	  * already in the TLB.
-	  */
+	/*
+	 * To ensure no page table walks occur in DDR, we
+	 * have a another page table stored in IRAM that only
+	 * contains entries pointing to IRAM, AIPS1 and AIPS2.
+	 * We need to set the TTBR1 to the new IRAM TLB.
+	 * Do the following steps:
+	 * 1. Flush the Branch Target Address Cache (BTAC)
+	 * 2. Set TTBR1 to point to IRAM page table.
+	 * 3. Disable page table walks in TTBR0 (PD0 = 1)
+	 * 4. Set TTBR0.N=1, implying 0-2G is translated by TTBR0
+	 *     and 2-4G is translated by TTBR1.
+	 */
+	ldr    r6, =iram_tlb_phys_addr
+	ldr    r7, [r6]
 
-	adr   r10, ddr_freq_change               @Address in this function.
+	/* Disable Branch Prediction, Z bit in SCTLR. */
+	mrc    p15, 0, r6, c1, c0, 0
+	bic    r6, r6, #0x800
+	mcr    p15, 0, r6, c1, c0, 0
 
-	ldr    r2, [r6]                        @ TLB will miss,
-	                                         @CCM address will be loaded
-	ldr    r2, [r5]                        @ TLB will miss,
-	                                         @MMDC address will be loaded
-	ldr    r2, [r7]                        @ TLB will miss,
-	                                         @IOMUX will be loaded
+	/* Flush the Branch Target Address Cache (BTAC) */
+	ldr    r6, =0x0
+	mcr    p15, 0, r6, c7, c1, 6
 
-	ldr    r2, [r8]                        @ Get the DDR settings.
-	ldr    r2, [r10]                        @ TLB will miss
-	ldr    r2, [r11]				@Get the IOMUX settings
+	dsb
+	isb
+	/* Store the IRAM table in TTBR1 */
+	mcr    p15, 0, r7, c2, c0, 1
+
+	/* Read TTBCR and set PD0=1, N = 1 */
+	mrc    p15, 0, r6, c2, c0, 2
+	orr    r6, r6, #0x11
+	mcr    p15, 0, r6, c2, c0, 2
+
+	dsb
+	isb
+
+	/* flush the TLB */
+	ldr    r6, =0x0
+	mcr    p15, 0, r6, c8, c3, 0
+
+	/* Disable L1 data cache. */
+	mrc    p15, 0, r6, c1, c0, 0
+	bic    r6, r6, #0x4
+	mcr    p15, 0, r6, c1, c0, 0
 
-	/* Make sure all the L1 & L2 buffers are drained, as
-	  * we don't want any writes to the DDR when it is
-	  * in self-refresh.
-	  */
-	/* Make sure the L1 buffers are drained. */
 	dsb
 
 #ifdef CONFIG_CACHE_L2X0
-	/* Make sure the L2 buffers are drained.
-	  * Sync operation on L2 drains the buffers.
-	  */
-	ldr      r0, =L2_BASE_ADDR
-	add     r0, r0, #PERIPBASE_VIRT
-	mov    r1, #0x0
-	str      r1, [r0, #0x730]
-#endif
+	/*
+	 * Sync L2 and then disable it.
+	 */
+	ldr   r7, =L2_BASE_ADDR
+	add   r7, r7, #PERIPBASE_VIRT
+	/* Wait for background operations to complete. */
+wait_for_l2_to_idle:
+	ldr   r6, [r7, #0x730]
+	cmp   r6, #0x0
+	bne   wait_for_l2_to_idle
+	ldr   r6, =0x0
+	str   r6, [r7, #0x730]
+	/* Disable L2. */
+	str   r6, [r7, #0x100]
 
-     /* The second dsb might be needed to keep cache sync (device write)
-       * ordering with the memory accesses before it.
-       */
 	dsb
 	isb
+#endif
+
+	ldr     r6, =CCM_BASE_ADDR
+	add     r6, r6, #PERIPBASE_VIRT
+	ldr     r5, =MMDC_P0_BASE_ADDR
+	add     r5, r5, #PERIPBASE_VIRT
+	ldr     r7, =MX6Q_IOMUXC_BASE_ADDR
+	add     r7, r7, #PERIPBASE_VIRT
 
 	/* Disable automatic power saving. */
 	ldr    r0, [r5, #0x404]
@@ -997,14 +1029,54 @@ poll_conreq_clear_2:
 	beq     poll_conreq_clear_2
 
 done:
+#ifdef CONFIG_CACHE_L2X0
+	/* Enable L2. */
+	ldr   r1, =L2_BASE_ADDR
+	add   r1, r1, #PERIPBASE_VIRT
+	ldr   r6, =0x1
+	str   r6, [r1, #0x100]
+#endif
 
+	/* Enable L1 data cache. */
+	mrc    p15, 0, r6, c1, c0, 0
+	orr    r6, r6, #0x4
+	mcr    p15, 0, r6, c1, c0, 0
+
+	/* Restore the TTBCR */
+	dsb
+	isb
+
+	/* Read TTBCR and set PD0=0, N = 0 */
+	mrc    p15, 0, r6, c2, c0, 2
+	bic    r6, r6, #0x11
+	mcr    p15, 0, r6, c2, c0, 2
+	dsb
+	isb
+
+	/* flush the TLB */
+	ldr    r6, =0x0
+	mcr    p15, 0, r6, c8, c3, 0
+
+	dsb
+	isb
+
+	/* Enable Branch Prediction, Z bit in SCTLR. */
+	mrc    p15, 0, r6, c1, c0, 0
+	orr    r6, r6, #0x800
+	mcr    p15, 0, r6, c1, c0, 0
+
+	/* Flush the Branch Target Address Cache (BTAC) */
+	ldr    r6, =0x0
+	mcr    p15, 0, r6, c7, c1, 6
 	/* Restore registers */
 
-	ldmfd sp!, {r4,r5,r6, r7, r8, r9, r10, r11}
+	ldmfd sp!, {r4 - r11}
 
 	mov     pc, lr
-
-	.type   mx6_do_ddr_freq_change, #object
-ENTRY(mx6_do_ddr_freq_change)
-	.word   mx6_ddr_freq_change
-	.size    mx6_ddr_freq_change, . - mx6_ddr_freq_change
+	/*
+	 * Add ltorg here to ensure that all
+	 * literals are stored here and are
+	 * within the text space.
+	 */
+	.ltorg
+mx6_ddr3_iram_end:
diff --git a/arch/arm/mach-mx6/mx6_mmdc.c b/arch/arm/mach-mx6/mx6_mmdc.c
index 2675a9f8b290..907e8aaee294 100644
--- a/arch/arm/mach-mx6/mx6_mmdc.c
+++ b/arch/arm/mach-mx6/mx6_mmdc.c
@@ -65,10 +65,19 @@ extern int mmdc_med_rate;
 extern void __iomem *ccm_base;
 extern void mx6_ddr_freq_change(u32 freq, void *ddr_settings, bool dll_mode, void *iomux_offsets);
 extern void mx6sl_ddr_iram(int ddr_freq, int low_bus_freq_mode, void *ddr_settings);
+extern void mx6sl_ddr3_freq_change(u32 freq, void *ddr_settings, bool dll_mode, void *iomux_offsets, int low_bus_freq_mode);
+extern unsigned long save_ttbr1(void);
+extern void restore_ttbr1(u32 ttbr1);
 
+extern unsigned long mx6_ddr3_iram_end asm("mx6_ddr3_iram_end");
+extern unsigned long mx6_ddr3_iram_start asm("mx6_ddr3_iram_start");
+extern unsigned long mx6sl_lpddr2_iram_end asm("mx6sl_lpddr2_iram_end");
+extern unsigned long mx6sl_lpddr2_iram_start asm("mx6sl_lpddr2_iram_start");
+extern unsigned long mx6sl_ddr3_iram_end asm("mx6sl_ddr3_iram_end");
+extern unsigned long mx6sl_ddr3_iram_start asm("mx6sl_ddr3_iram_start");
 
-extern void mx6l_ddr3_freq_change(u32 freq, void *ddr_settings, bool dll_mode, void *iomux_offsets, int low_bus_freq_mode);
 
+unsigned long ddr_freq_change_iram_phys_addr;
 static void *ddr_freq_change_iram_base;
 static int ddr_settings_size;
 static int iomux_settings_size;
@@ -235,6 +244,7 @@ irqreturn_t wait_in_wfe_irq(int irq, void *dev_id)
 	return IRQ_HANDLED;
 }
 
+extern unsigned long iram_tlb_phys_addr;
 /* Change the DDR frequency. */
 int update_ddr_freq(int ddr_rate)
 {
@@ -244,6 +254,7 @@ int update_ddr_freq(int ddr_rate)
 	unsigned int online_cpus = 0;
 	int cpu = 0;
 	int me;
+	u32 ttbr1;
 
 	if (!can_change_ddr_freq())
 		return -1;
@@ -339,6 +350,8 @@ int update_ddr_freq(int ddr_rate)
 	while (cpus_in_wfe != online_cpus)
 		udelay(5);
 
+	/* Save TTBR1 */
+	ttbr1 = save_ttbr1();
 	/* Now we can change the DDR frequency. */
 	if (cpu_is_mx6sl())
 		if (ddr_type == MX6_DDR3)
@@ -348,6 +361,7 @@ int update_ddr_freq(int ddr_rate)
 	else
 		mx6_change_ddr_freq(ddr_rate, iram_ddr_settings, dll_off, iram_iomux_settings);
 
+	restore_ttbr1(ttbr1);
 
 	curr_ddr_rate = ddr_rate;
 
@@ -366,47 +380,73 @@ int update_ddr_freq(int ddr_rate)
 
 int init_mmdc_settings(void)
 {
-	unsigned long iram_paddr;
 	int i, err, cpu;
+	unsigned long ddr_code_size = 0;
 
 	mmdc_base = ioremap(MMDC_P0_BASE_ADDR, SZ_32K);
 	iomux_base = ioremap(MX6Q_IOMUXC_BASE_ADDR, SZ_16K);
 	gic_dist_base = ioremap(IC_DISTRIBUTOR_BASE_ADDR, SZ_16K);
 	gic_cpu_base = ioremap(IC_INTERFACES_BASE_ADDR, SZ_16K);
 
-
 	ddr_type = (__raw_readl(MMDC_MDMISC_OFFSET) & MMDC_MDMISC_DDR_TYPE_MASK) >> MMDC_MDMISC_DDR_TYPE_OFFSET;
 	printk(KERN_NOTICE "DDR type is %s\n", ddr_type == 0 ? "DDR3" : "LPDDR2");
 
 	if (ddr_type == MX6_DDR3) {
-		if (cpu_is_mx6q())
-			ddr_settings_size = ARRAY_SIZE(ddr3_dll_mx6q) + ARRAY_SIZE(ddr3_calibration);
-		if (cpu_is_mx6dl())
-			ddr_settings_size = ARRAY_SIZE(ddr3_dll_mx6dl) + ARRAY_SIZE(ddr3_calibration);
-		if (cpu_is_mx6sl())
+		if (cpu_is_mx6sl()) {
 			ddr_settings_size = ARRAY_SIZE(ddr3_dll_mx6sl) + ARRAY_SIZE(ddr3_calibration_mx6sl);
+			iomux_settings_size = ARRAY_SIZE(iomux_offsets_mx6sl);
+			ddr_code_size = (&mx6sl_ddr3_iram_end -&mx6sl_ddr3_iram_start) *4;
+		} else {
+			ddr_code_size = (&mx6_ddr3_iram_end -&mx6_ddr3_iram_start) *4;
+			if (cpu_is_mx6q()) {
+				ddr_settings_size = ARRAY_SIZE(ddr3_dll_mx6q) + ARRAY_SIZE(ddr3_calibration);
+				iomux_settings_size = ARRAY_SIZE(iomux_offsets_mx6q);
+			}
+			if (cpu_is_mx6dl()) {
+				ddr_settings_size = ARRAY_SIZE(ddr3_dll_mx6dl) + ARRAY_SIZE(ddr3_calibration);
+				iomux_settings_size = ARRAY_SIZE(iomux_offsets_mx6dl);
+			}
+		}
 	} else {
-		if (cpu_is_mx6sl())
+		if (cpu_is_mx6sl()) {
 			ddr_settings_size = ARRAY_SIZE(lpddr2_400M_6sl);
+			ddr_code_size = (&mx6sl_lpddr2_iram_end -&mx6sl_lpddr2_iram_start) *4;
+		}
 	}
 
+	/* Use preallocated IRAM memory */
+	ddr_freq_change_iram_phys_addr = MX6_IRAM_DDR_FREQ_ADDR;
+	/*
+	 * Don't ioremap the address, we have fixed the IRAM address
+	 * at IRAM_BASE_ADDR_VIRT
+	 */
+	ddr_freq_change_iram_base = (void *)IRAM_BASE_ADDR_VIRT +
+		(ddr_freq_change_iram_phys_addr - IRAM_BASE_ADDR);
 
 	normal_mmdc_settings = kmalloc((ddr_settings_size * 8), GFP_KERNEL);
-	if (cpu_is_mx6q()) {
-		memcpy(normal_mmdc_settings, ddr3_dll_mx6q, sizeof(ddr3_dll_mx6q));
-		memcpy(((char *)normal_mmdc_settings + sizeof(ddr3_dll_mx6q)), ddr3_calibration, sizeof(ddr3_calibration));
-	}
-	if (cpu_is_mx6dl()) {
-		memcpy(normal_mmdc_settings, ddr3_dll_mx6dl, sizeof(ddr3_dll_mx6dl));
-		memcpy(((char *)normal_mmdc_settings + sizeof(ddr3_dll_mx6dl)), ddr3_calibration, sizeof(ddr3_calibration));
-	}
 	if (cpu_is_mx6sl()) {
 		if (ddr_type == MX6_DDR3) {
 			memcpy(normal_mmdc_settings, ddr3_dll_mx6sl, sizeof(ddr3_dll_mx6sl));
 			memcpy(((char *)normal_mmdc_settings + sizeof(ddr3_dll_mx6sl)),
 				ddr3_calibration_mx6sl, sizeof(ddr3_calibration_mx6sl));
-		} else
+			memcpy(ddr_freq_change_iram_base, mx6sl_ddr3_freq_change, ddr_code_size);
+			mx6sl_ddr3_change_freq = (void *)ddr_freq_change_iram_base;
+		} else {
 			memcpy(normal_mmdc_settings, lpddr2_400M_6sl, sizeof(lpddr2_400M_6sl));
+			memcpy(ddr_freq_change_iram_base, mx6sl_ddr_iram, ddr_code_size);
+			mx6sl_lpddr2_change_freq = (void *)ddr_freq_change_iram_base;
+		}
+	} else {
+		memcpy(ddr_freq_change_iram_base, mx6_ddr_freq_change, ddr_code_size);
+		mx6_change_ddr_freq = (void *)ddr_freq_change_iram_base;
+		if (cpu_is_mx6q()) {
+			memcpy(normal_mmdc_settings, ddr3_dll_mx6q, sizeof(ddr3_dll_mx6q));
+			memcpy(((char *)normal_mmdc_settings + sizeof(ddr3_dll_mx6q)), ddr3_calibration, sizeof(ddr3_calibration));
+		}
+		if (cpu_is_mx6dl()) {
+			memcpy(normal_mmdc_settings, ddr3_dll_mx6dl, sizeof(ddr3_dll_mx6dl));
+			memcpy(((char *)normal_mmdc_settings + sizeof(ddr3_dll_mx6dl)), ddr3_calibration, sizeof(ddr3_calibration));
+		}
 	}
 
 	/* Store the original DDR settings at boot. */
@@ -424,30 +464,16 @@ int init_mmdc_settings(void)
 	/* Store the size of the array in iRAM also,
 	 * increase the size by 8 bytes.
 	 */
-	iram_ddr_settings = iram_alloc((ddr_settings_size * 8) + 8, &iram_paddr);
-	if (iram_ddr_settings == NULL) {
-			printk(KERN_DEBUG
-			"%s: failed to allocate iRAM memory for ddr settings\n",
-			__func__);
-			return ENOMEM;
+	iram_iomux_settings = ddr_freq_change_iram_base + ddr_code_size;
+	iram_ddr_settings = iram_iomux_settings + (iomux_settings_size * 8) + 8;
+
+	if ((ddr_code_size + (iomux_settings_size + ddr_settings_size) * 8 + 16)
+		> MX6_IRAM_DDR_FREQ_CODE_SIZE) {
+		printk(KERN_ERR "Not enough memory allocated for DDR Frequency change code.\n");
+		return -EINVAL;
 	}
 
 	if (ddr_type == MX6_DDR3) {
-		if (cpu_is_mx6sl())
-			iomux_settings_size = ARRAY_SIZE(iomux_offsets_mx6sl);
-		else
-			iomux_settings_size = ARRAY_SIZE(iomux_offsets_mx6q);
-		/* Store the size of the iomux settings in iRAM also,
-		 * increase the size by 8 bytes.
-		 */
-		iram_iomux_settings = iram_alloc((iomux_settings_size * 8) + 8, &iram_paddr);
-		if (iram_iomux_settings == NULL) {
-				printk(KERN_DEBUG
-				"%s: failed to allocate iRAM memory for iomuxr settings\n",
-				__func__);
-				return ENOMEM;
-		}
-
 		/* Store the IOMUX settings at boot. */
 		if (cpu_is_mx6q()) {
 			for (i = 0; i < iomux_settings_size; i++) {
@@ -479,27 +505,6 @@ int init_mmdc_settings(void)
 			}
 		}
 	}
-	/* Allocate IRAM for the DDR freq change code. */
-	iram_alloc(SZ_8K, &iram_paddr);
-	/* Need to remap the area here since we want the memory region
-		 to be executable. */
-	ddr_freq_change_iram_base = __arm_ioremap(iram_paddr,
-						SZ_8K, MT_MEMORY_NONCACHED);
-
-	if (cpu_is_mx6sl()) {
-		if (ddr_type == MX6_DDR3) {
-			memcpy(ddr_freq_change_iram_base, mx6l_ddr3_freq_change, SZ_8K);
-			mx6sl_ddr3_change_freq = (void *)ddr_freq_change_iram_base;
-		} else {
-			memcpy(ddr_freq_change_iram_base, mx6sl_ddr_iram, SZ_8K);
-			mx6sl_lpddr2_change_freq = (void *)ddr_freq_change_iram_base;
-		}
-	} else {
-		memcpy(ddr_freq_change_iram_base, mx6_ddr_freq_change, SZ_8K);
-		mx6_change_ddr_freq = (void *)ddr_freq_change_iram_base;
-	}
-
-
 	curr_ddr_rate = ddr_normal_rate;
 
 	if (!cpu_is_mx6sl()) {
diff --git a/arch/arm/mach-mx6/mx6_suspend.S b/arch/arm/mach-mx6/mx6_suspend.S
index a96490a19237..1f0d95ab5584 100644
--- a/arch/arm/mach-mx6/mx6_suspend.S
+++ b/arch/arm/mach-mx6/mx6_suspend.S
@@ -29,10 +29,21 @@
 #define TABLE_INDEX_MASK 	0xfff00000
 #define TABLE_ENTRY 		0x00000c02
 #define CACHE_DISABLE_MASK 	0xfffffffb
+
 #define MMDC_MAPSR_OFFSET 	0x404
 #define MMDC_MAPSR_PSS 		(1 << 4)
 #define MMDC_MAPSR_PSD 		(1 << 0)
-#define IRAM_SUSPEND_SIZE 	(1 << 13)
+/*
+ * The code size is limited to 5K, as we may need to store
+ * this code  * along with the low power WFI and DDR freq
+ * change code within 8K of IRAM.
+ */
+#define IRAM_SUSPEND_SIZE 	MX6_SUSPEND_CODE_SIZE
+
+.extern iram_tlb_phys_addr
+.extern total_suspend_size
+.globl mx6_suspend_end
+.globl mx6_suspend_start
 
 #define MX6_TYPE_DDR3	0
 
@@ -824,7 +835,6 @@ Invalidate l1 dcache, r0-r4, r6, r7 used
 	and 	r2, r1, r0, lsr #13
 
 	ldr 	r1, =0x3ff
-
 	and 	r3, r1, r0, lsr #3	@ NumWays - 1
 	add 	r2, r2, #1		@ NumSets
 
@@ -937,7 +947,9 @@ Clean L2 cache
 #endif
 	.endm
 
+	.align 3
 ENTRY(mx6_suspend)
+mx6_suspend_start:
 	stmfd	sp!, {r0-r12}     @ Save registers
 /*************************************************************
 suspend mode entry
@@ -1011,12 +1023,14 @@ never run to here
 dormant entry, data save in stack, save sp in the src_gpr2
 ************************************************************/
 dormant:
-	mov 	r3, r1
-	mov 	r0, r1
-	add 	r0, r0, #IRAM_SUSPEND_SIZE /* 8K */
-	ldr 	r4, =SRC_BASE_ADDR
-	add 	r4, r4, #PERIPBASE_VIRT
-	str 	r0, [r4, #SRC_GPR2_OFFSET] /* set src_gpr2 */
+	mov r3, r1
+	mov r0, r1
+	ldr r4,=total_suspend_size
+	ldr r4, [r4]
+	add r0, r0, r4
+	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
@@ -1040,18 +1054,20 @@ saved register and context as below:
 	/* 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	/* 8K */
+	mov r0, r2 		/* get suspend_iram_base */
+	ldr r4,=total_suspend_size
+	ldr r4, [r4]
+	add r0, r0, r4
 
-	mov	r4, r12			@ Store cpu type
-	stmfd	r0!, {r4}
-	mov	r4, r11			@ Store state entered
-	stmfd	r0!, {r4}
+	mov r4, r12			@ Store cpu type
+	stmfd r0!, {r4}
+	mov r4, r11			@ Store state entered
+	stmfd r0!, {r4}
 	mov r4, r10			@ Store ddr type
-	stmfd	r0!, {r4}
+	stmfd r0!, {r4}
 
-	ldr	r1, =MX6Q_IOMUXC_BASE_ADDR
-	add	r1, r1, #PERIPBASE_VIRT
+	ldr r1, =MX6Q_IOMUXC_BASE_ADDR
+	add r1, r1, #PERIPBASE_VIRT
 
 	cmp r12, #MXC_CPU_MX6Q
 	bne dl_io_save
@@ -1066,7 +1082,6 @@ sl_io_save:
 	sl_ddr_io_save
 
 ddr_io_save_done:
-
 #ifdef CONFIG_CACHE_L2X0
 	ldr r1, =L2_BASE_ADDR
 	add r1, r1, #PERIPBASE_VIRT
@@ -1118,32 +1133,64 @@ ddr_io_save_done:
 	/* Need to clean L2 dcache*/
 	clean_l2_cache
 
-/****************************************************************
-set ddr iomux to low power mode
-****************************************************************/
-	/* Make sure TLBs are primed. */
-	ldr	r1,  =MX6Q_IOMUXC_BASE_ADDR
-	add	r1, r1, #PERIPBASE_VIRT
-	ldr	r0, [r1]
-	ldr	r1, =SRC_BASE_ADDR
-	add	r1, r1, #PERIPBASE_VIRT
-	ldr	r0, [r1]
-	ldr	r1, =CCM_BASE_ADDR
-	add	r1, r1, #PERIPBASE_VIRT
-	ldr	r0, [r1]
-	ldr	r1, =GPC_BASE_ADDR
-	add	r1, r1, #PERIPBASE_VIRT
-	ldr	r0, [r1]
-	ldr	r1, =CCM_BASE_ADDR
-	add	r1, r1, #PERIPBASE_VIRT
-	ldr	r0, [r1]
-	ldr	r1, =ANATOP_BASE_ADDR
-	add	r1, r1, #PERIPBASE_VIRT
-	ldr	r0, [r1]
+	/*
+	 * To ensure no page table walks occur in DDR, we
+	 * have a another page table stored in IRAM that only
+	 * contains entries pointing to IRAM, AIPS1 and AIPS2.
+	 * We need to set the TTBR1 to the new IRAM TLB.
+	 * Do the following steps:
+	 * 1. Flush the Branch Target Address Cache (BTAC)
+	 * 2. Set TTBR1 to point to IRAM page table.
+	 * 3. Disable page table walks in TTBR0 (PD0 = 1)
+	 * 4. Set TTBR0.N=1, implying 0-2G is translated by TTBR0
+	 *     and 2-4G is translated by TTBR1.
+	 */
+	/* Disable Branch Prediction, Z bit in SCTLR. */
+	mrc    p15, 0, r6, c1, c0, 0
+	bic    r6, r6, #0x800
+	mcr    p15, 0, r6, c1, c0, 0
+
+	/* Flush the BTAC. */
+	ldr r6, =0x0
+	mcr p15, 0, r6, c7, c1, 6
 
-	/* Do a DSB to drain the buffers. */
+	ldr r6, =iram_tlb_phys_addr
+	ldr r6, [r6]
 	dsb
+	isb
+
+	/* Store the IRAM table in TTBR1 */
+	mcr    p15, 0, r6, c2, c0, 1
+	/* Read TTBCR and set PD0=1, N = 1 */
+	mrc    p15, 0, r6, c2, c0, 2
+	orr    r6, r6, #0x11
+	mcr    p15, 0, r6, c2, c0, 2
+
+	dsb
+	isb
+
+	/* flush the TLB */
+	ldr r6, =0x0
+	mcr p15, 0, r6, c8, c3, 0
+
+	/* Disable L1 data cache. */
+	mrc p15, 0, r6, c1, c0, 0
+	bic r6, r6, #0x4
+	mcr p15, 0, r6, c1, c0, 0
+
+    dsb
+
+	/* Disable L2 cache */
+#ifdef CONFIG_CACHE_L2X0
+	ldr r2, =L2_BASE_ADDR
+	add r2, r2, #PERIPBASE_VIRT
+	mov r4, #0x0
+	str r4, [r2, #L2X0_CTRL]
+#endif
 
+/****************************************************************
+set ddr iomux to low power mode
+****************************************************************/
 	ldr	r1, =MMDC_P0_BASE_ADDR
 	add	r1, r1, #PERIPBASE_VIRT
 
@@ -1211,7 +1258,6 @@ sl_io_set_lpm:
 	sl_ddr_io_set_lpm
 
 ddr_io_set_lpm_done:
-
 	/* Do Analog Power Optimizations
 	  * for MX6SL in standby mode.
 	  */
@@ -1238,15 +1284,16 @@ save resume pointer into SRC_GPR1
 	sub	r1, r1, r0
 	add	r3, r3, r1
 	ldr	r1, =SRC_BASE_ADDR
+
 	add	r1, r1, #PERIPBASE_VIRT
 	str	r3, [r1, #SRC_GPR1_OFFSET]
 
 	/* Mask all GPC interrupts before
-	  * enabling the RBC counters to
-	  * avoid the counter starting too
-	  * early if an interupt is already
-	  * pending.
-	  */
+	 * enabling the RBC counters to
+	 * avoid the counter starting too
+	 * early if an interupt is already
+	 * pending.
+	 */
 	ldr	r3, =GPC_BASE_ADDR
 	add	r3, r3, #PERIPBASE_VIRT
 	ldr	r4, [r3, #0x08]
@@ -1287,14 +1334,15 @@ save resume pointer into SRC_GPR1
 	str	r6, [r3, #0x10]
 	str	r7, [r3, #0x14]
 
+
 	/* Now delay for a short while (3usec)
-	  * ARM is at 1GHz at this point
-	  * so a short loop should be enough.
-	  * This delay is required to ensure that
-	  * the RBC counter can start counting in case an
-	  * interrupt is already pending or in case an interrupt
-	  * arrives just as ARM is about to assert DSM_request.
-	  */
+	 * ARM is at 1GHz at this point
+	 * so a short loop should be enough.
+	 * This delay is required to ensure that
+	 * the RBC counter can start counting in case an
+	 * interrupt is already pending or in case an interrupt
+	 * arrives just as ARM is about to assert DSM_request.
+	 */
 	ldr	r4, =2000
 rbc_loop:
 	sub	r4, r4, #0x1
@@ -1342,8 +1390,13 @@ ldo_bypass_restore:
 	orr	r3, r3, #0x1f
 	str	r3, [r1, #0x140]
 ldo_check_done2:
-	mov	r0, r2 		/* get suspend_iram_base */
-	add	r0, r0, #IRAM_SUSPEND_SIZE	/* 8K */
+	/*
+	 * Get the address of IRAM stack
+	 */
+	ldr	r0, =SRC_BASE_ADDR
+	add	r0, r0, #PERIPBASE_VIRT
+	ldr	r0, [r0, #SRC_GPR2_OFFSET]
+	add	r0, r0, #PERIPBASE_VIRT
 
 	ldmea	r0!, {r12}	@ get cpu type to make ddr io
 				@ offset right
@@ -1384,6 +1437,7 @@ sl_io_restore:
 	sl_ddr_io_restore
 	ldr	r1, =MMDC_P0_BASE_ADDR
 	add	r1, r1, #PERIPBASE_VIRT
+
 	/* reset read FIFO, RST_RD_FIFO */
 	ldr	r7, =0x83c
 	ldr	r6, [r1, r7]
@@ -1426,10 +1480,10 @@ poll_dvfs_clear_1:
 	str	r6, [r1, #0x404]
 
 	/* Add enough nops so that the
-	  * prefetcher will not get instructions
-	  * from DDR before its IO pads
-	  * are restored.
-	  */
+	 * prefetcher will not get instructions
+	 * from DDR before its IO pads
+	 * are restored.
+	 */
 	nop
 	nop
 	nop
@@ -1464,6 +1518,37 @@ poll_dvfs_clear_1:
 	orr	r1, r1, #(1 << 2)	@ Enable the C bit
 	mcr	p15, 0, r1, c1, c0, 0
 
+	/* Enable L2 cache here */
+#ifdef CONFIG_CACHE_L2X0
+	ldr r2, =L2_BASE_ADDR
+	add r2, r2, #PERIPBASE_VIRT
+	mov r4, #0x1
+	str r4, [r2, #L2X0_CTRL]
+#endif
+
+	/* Restore TTBCR */
+	dsb
+	isb
+	/* Read TTBCR and set PD0=0, N = 0 */
+	mrc    p15, 0, r6, c2, c0, 2
+	bic    r6, r6, #0x11
+	mcr    p15, 0, r6, c2, c0, 2
+	dsb
+	isb
+
+	/* flush the TLB */
+	ldr	r6, =0x0
+	mcr	p15, 0, r6, c8, c3, 0
+
+	/* Enable Branch Prediction, Z bit in SCTLR. */
+	mrc	p15, 0, r6, c1, c0, 0
+	orr	r6, r6, #0x800
+	mcr	p15, 0, r6, c1, c0, 0
+
+	/* Flush the Branch Target Address Cache (BTAC) */
+	ldr	r6, =0x0
+	mcr	p15, 0, r6, c7, c1, 6
+
 	b 	out	/* exit standby */
 	.ltorg
 
@@ -1472,6 +1557,7 @@ when SOC exit stop mode, arm core restart from here, currently
 are running with MMU off.
 ****************************************************************/
 resume:
+
 	/*restore it with 0x1f if use ldo bypass mode.*/
 	ldr	r1, =ANATOP_BASE_ADDR
 	ldr	r3, [r1, #0x140]
@@ -1534,7 +1620,9 @@ dl_io_dsm_restore:
 	b	ddr_io_restore_dsm_done
 sl_io_dsm_restore:
 	sl_ddr_io_restore
+
 	ldr	r1, =MMDC_P0_BASE_ADDR
+
 	/* reset read FIFO, RST_RD_FIFO */
 	ldr	r7, =0x83c
 	ldr	r6, [r1, r7]
@@ -1672,10 +1760,12 @@ use_ttbr0:
 	mcr	p15, 0, r4, c1, c0, 0
 	isb
 	dsb
+
 	ldr	r1, =mmu_on_label
 	bx	r1
 mmu_on_label:
 
+
 /************************************************************
 restore control register to enable cache
 ************************************************************/
@@ -1739,7 +1829,14 @@ out:
 	ldmfd	sp!, {r0-r12}
 	mov	pc, lr
 
-	.type	mx6_do_suspend, #object
-ENTRY(mx6_do_suspend)
-	.word	mx6_suspend
-	.size	mx6_suspend, . - mx6_suspend
+	/*
+	 * Add ltorg here to ensure that all
+	 * literals are stored here and are
+	 * within the text space.
+	 */
+	.ltorg
+mx6_suspend_end:
+
+ENDPROC(mx6_suspend)
+
+
diff --git a/arch/arm/mach-mx6/mx6sl_ddr.S b/arch/arm/mach-mx6/mx6sl_ddr.S
index 6f6e1a68bfca..b139dd5920db 100644
--- a/arch/arm/mach-mx6/mx6sl_ddr.S
+++ b/arch/arm/mach-mx6/mx6sl_ddr.S
@@ -18,6 +18,9 @@
 
 #include <linux/linkage.h>
 #include <mach/hardware.h>
+.extern iram_tlb_phys_addr
+.globl mx6sl_lpddr2_iram_start
+.globl mx6sl_lpddr2_iram_end
 
 
 	 .macro    mx6sl_switch_to_24MHz
@@ -213,6 +216,7 @@ pll2_already_on:
      bic     r6, r6, #0x600000
      orr     r6, r6, #0x200000
      str     r6, [r2, #0x18]
+
      ldr     r6, =400000000
      b       cont2
 
@@ -318,6 +322,7 @@ force_measure1:
       bne    force_measure1
       .endm
 
+    .align 3
 /*
  *  mx6sl_ddr_iram
  *
@@ -329,46 +334,85 @@ force_measure1:
  * r2: MMDC array
  */
 ENTRY(mx6sl_ddr_iram)
-
+mx6sl_lpddr2_iram_start:
     push {r4-r10}
 
-mx6sl_ddr_freq_change:
-
     mov  r9, r2                      @save MMDC array
-    ldr     r3, =ANATOP_BASE_ADDR
-    add   r3, r3, #PERIPBASE_VIRT
 
-    ldr     r2, =CCM_BASE_ADDR
-    add   r2, r2, #PERIPBASE_VIRT
+    /*
+     * To ensure no page table walks occur in DDR, we
+     * have a another page table stored in IRAM that only
+     * contains entries pointing to IRAM, AIPS1 and AIPS2.
+     * We need to set the TTBR1 to the new IRAM TLB.
+     * Do the following steps:
+     * 1. Flush the Branch Target Address Cache (BTAC)
+     * 2. Set TTBR1 to point to IRAM page table.
+     * 3. Disable page table walks in TTBR0 (PD0 = 1)
+     * 4. Set TTBR0.N=1, implying 0-2G is translated by TTBR0
+     *     and 2-4G is translated by TTBR1.
+     */
+    ldr    r6, =iram_tlb_phys_addr
+    ldr    r7, [r6]
 
-    ldr     r8, =MMDC_P0_BASE_ADDR
-    add   r8, r8, #PERIPBASE_VIRT
+    /* Disable Branch Prediction, Z bit in SCTLR. */
+    mrc    p15, 0, r6, c1, c0, 0
+    bic    r6, r6, #0x800
+    mcr    p15, 0, r6, c1, c0, 0
+
+    /* Flush the Branch Target Address Cache (BTAC) */
+    ldr    r6, =0x0
+    mcr    p15, 0, r6, c7, c1, 6
+
+    dsb
+    isb
+    /* Store the IRAM table in TTBR1 */
+    mcr    p15, 0, r7, c2, c0, 1
 
-    /* Prime all TLB entries. */
-    adr   r7, mx6sl_ddr_freq_change               @Address in this function.
+    /* Read TTBCR and set PD0=1, N = 1 */
+    mrc    p15, 0, r6, c2, c0, 2
+    orr    r6, r6, #0x11
+    mcr    p15, 0, r6, c2, c0, 2
+
+    dsb
+    isb
+
+mx6sl_ddr_freq_change:
+    /* flush the TLB */
+    ldr    r6, =0x0
+    mcr    p15, 0, r6, c8, c3, 0
 
-    ldr     r6, [r7]
-    ldr     r6, [r8]
-    ldr     r6, [r3]
-    ldr     r6, [r2]
 
-     /* Drain all the L1 buffers. */
-     dsb
+    /* Disable L1 data cache. */
+    mrc    p15, 0, r6, c1, c0, 0
+    bic    r6, r6, #0x4
+    mcr    p15, 0, r6, c1, c0, 0
 
+    dsb
 #ifdef CONFIG_CACHE_L2X0
-    /* Need to make sure the buffers in L2 are drained.
-      * Performing a sync operation does this. */
-     ldr     r7, =L2_BASE_ADDR
-     add   r7, r7, #PERIPBASE_VIRT
-     mov  r6, #0x0
-     str    r6, [r7, #0x730]
+    /*
+     * Sync L2 and then disable it.
+     */
+    ldr   r4, =L2_BASE_ADDR
+    add   r4, r4, #PERIPBASE_VIRT
+    /* Wait for background operations to complete. */
+wait_for_l2_to_idle:
+    ldr   r6, [r4, #0x730]
+    cmp   r6, #0x0
+    bne   wait_for_l2_to_idle
+    ldr   r6, =0x0
+    str   r6, [r4, #0x730]
+    /* Disable L2. */
+    str   r6, [r4, #0x100]
+
+    dsb
+    isb
 #endif
-
-     /* The second dsb might be needed to keep cache sync (device write)
-       * ordering with the memory accesses before it.
-       */
-     dsb
-     isb
+    ldr     r3, =ANATOP_BASE_ADDR
+    add     r3, r3, #PERIPBASE_VIRT
+    ldr     r2, =CCM_BASE_ADDR
+    add     r2, r2, #PERIPBASE_VIRT
+    ldr     r8, =MMDC_P0_BASE_ADDR
+    add     r8, r8, #PERIPBASE_VIRT
 
      /* Disable Automatic power savings. */
      ldr    r6, [r8, #0x404]
@@ -461,6 +505,7 @@ poll_dvfs_clear_1:
      str    r6, [r8, #0x404]
 
      ldr     r10, =24000000
+
      cmp    r0, r10
      beq     skip_power_down
 
@@ -475,12 +520,86 @@ skip_power_down:
      bic     r6, r6, #0x100
      str     r6, [r8, #0x410]
 
+#ifdef CONFIG_CACHE_L2X0
+    /* Enable L2. */
+    ldr   r1, =L2_BASE_ADDR
+    add   r1, r1, #PERIPBASE_VIRT
+    ldr   r6, =0x1
+    str   r6, [r1, #0x100]
+#endif
+
+    /* Enable L1 data cache. */
+    mrc    p15, 0, r6, c1, c0, 0
+    orr    r6, r6, #0x4
+    mcr    p15, 0, r6, c1, c0, 0
+
+    /* Restore the TTBCR */
+    dsb
+    isb
+    /* Read TTBCR and set PD0=0, N = 0 */
+    mrc    p15, 0, r6, c2, c0, 2
+    bic    r6, r6, #0x11
+    mcr    p15, 0, r6, c2, c0, 2
+    dsb
+    isb
+
+    /* flush the TLB */
+    ldr    r6, =0x0
+    mcr    p15, 0, r6, c8, c3, 0
+
+    dsb
+    isb
+
+    /* Enable Branch Prediction, Z bit in SCTLR. */
+    mrc    p15, 0, r6, c1, c0, 0
+    orr    r6, r6, #0x800
+    mcr    p15, 0, r6, c1, c0, 0
+
+    /* Flush the Branch Target Address Cache (BTAC) */
+    ldr    r6, =0x0
+    mcr    p15, 0, r6, c7, c1, 6
+
+    nop
+    nop
+    nop
+    nop
+    nop
+
+    nop
+    nop
+    nop
+    nop
+    nop
+
+    nop
+    nop
+    nop
+    nop
+    nop
+
+    nop
+    nop
+    nop
+    nop
+    nop
+
+    nop
+    nop
+    nop
+    nop
+    nop
+
     pop {r4-r10}
 
     /* Restore registers */
     mov     pc, lr
 
-    .type   mx6sl_ddr_do_iram, #object
-ENTRY(mx6sl_ddr_do_iram)
-    .word   mx6sl_ddr_iram
-    .size    mx6sl_ddr_iram, . - mx6sl_ddr_iram
+    /*
+     * Add ltorg here to ensure that all
+     * literals are stored here and are
+     * within the text space.
+     */
+   .ltorg
+mx6sl_lpddr2_iram_end:
+
+ENDPROC(mx6sl_ddr_iram)
diff --git a/arch/arm/mach-mx6/mx6sl_ddr3.S b/arch/arm/mach-mx6/mx6sl_ddr3.S
index 6ca088df6484..b55ff7f7c6e8 100644
--- a/arch/arm/mach-mx6/mx6sl_ddr3.S
+++ b/arch/arm/mach-mx6/mx6sl_ddr3.S
@@ -19,6 +19,10 @@
 #include <linux/linkage.h>
 #include <mach/hardware.h>
 
+.extern iram_tlb_phys_addr
+.globl mx6sl_ddr3_iram_start
+.globl mx6sl_ddr3_iram_end
+
 	  .macro	ddr_switch_400MHz
 
     /* Set MMDC divider first, in case PLL3 is at 480MHz. */
@@ -325,25 +329,16 @@ mmdc_podf0:
 	.endm
 
 /*
- *  mx6l_ddr3_freq_change
+ *  mx6sl_ddr3_freq_change
  *
  *  Idle the processor (eg, wait for interrupt).
  *  Make sure DDR is in self-refresh.
  *  IRQs are already disabled.
  */
-ENTRY(mx6l_ddr3_freq_change)
-
+ENTRY(mx6sl_ddr3_freq_change)
+mx6sl_ddr3_iram_start:
 	stmfd   sp!, {r4,r5,r6, r7, r8, r9, r10, r11,r12}      @ Save registers
 
-	ldr     r6, =CCM_BASE_ADDR
-	add     r6, r6, #PERIPBASE_VIRT
-	ldr     r5, =MMDC_P0_BASE_ADDR
-	add     r5, r5, #PERIPBASE_VIRT
-	ldr     r7, =MX6Q_IOMUXC_BASE_ADDR
-	add     r7, r7, #PERIPBASE_VIRT
-  ldr     r12, =ANATOP_BASE_ADDR
-  add   	r12, r12, #PERIPBASE_VIRT
-
 	mov			r10, r4				@save low_bus_freq_mode
 	mov     r4, r0				@save new freq requested
 	mov     r8, r1				@save the ddr settings for the new rate
@@ -351,47 +346,83 @@ ENTRY(mx6l_ddr3_freq_change)
 	mov     r11, r3			@save iomux offsets
 
 ddr_freq_change:
-	/* Make sure no TLB miss will occur when the DDR is in self refresh. */
-	/* Invalidate TLB single entry to ensure that the address is not
-	  * already in the TLB.
-	  */
-
-	adr   r3, ddr_freq_change               @Address in this function.
-
-
-	mcr   p15, 0, r3, c8, c7, 1      @//@ Make sure freq code address
-	                                       @//  @ is not already in TLB.
-	mcr   p15, 0, r6, c8, c7, 1      @//@ Make sure CCM address
-	                                         @//@ is not already in TLB.
-	mcr   p15, 0, r5, c8, c7, 1      @//@ make sure MMDC address
-	                                         @//@ is not already in TLB.
-	mcr   p15, 0, r7, c8, c7, 1      @//@ make sure IOMUX address
-	                                         @//@ is not already in TLB.
-	mcr   p15, 0, r12, c8, c7, 1      @//@ make sure ANATOP address
-	                                         @//@ is not already in TLB.
-
-	mrc   p15, 0, r0, c10, c0, 0    @//@ Read the TLB lockdown register
-	orr    r0, r0, #1                    @//@ Set the Preserve bit.
-	mcr   p15, 0, r0, c10, c0, 0    @//@ Write to the lockdown register
-
-	ldr    r2, [r6]                        @ TLB will miss,
-	                                         @CCM address will be loaded
-	ldr    r2, [r5]                        @ TLB will miss,
-	                                         @MMDC address will be loaded
-	ldr    r2, [r7]                        @ TLB will miss,
-	                                         @IOMUX will be loaded
-
-	ldr    r2, [r8]                        @ Get the DDR settings.
-
-	ldr    r2, [r3]                        @ TLB will miss
-
-	ldr    r2, [r11]				@Get the IOMUX settings
-	ldr    r2, [r12]
+	/*
+	 * To ensure no page table walks occur in DDR, we
+	 * have a another page table stored in IRAM that only
+	 * contains entries pointing to IRAM, AIPS1 and AIPS2.
+	 * We need to set the TTBR1 to the new IRAM TLB.
+	 * Do the following steps:
+	 * 1. Flush the Branch Target Address Cache (BTAC)
+	 * 2. Set TTBR1 to point to IRAM page table.
+	 * 3. Disable page table walks in TTBR0 (PD0 = 1)
+	 * 4. Set TTBR0.N=1, implying 0-2G is translated by TTBR0
+	 *     and 2-4G is translated by TTBR1.
+	 */
+	ldr    r6, =iram_tlb_phys_addr
+	ldr    r7, [r6]
+
+	/* Disable Branch Prediction, Z bit in SCTLR. */
+	mrc    p15, 0, r6, c1, c0, 0
+	bic    r6, r6, #0x800
+	mcr    p15, 0, r6, c1, c0, 0
+
+	/* Flush the Branch Target Address Cache (BTAC) */
+	ldr    r6, =0x0
+	mcr    p15, 0, r6, c7, c1, 6
+
+	dsb
+	isb
+
+	/* Store the IRAM table in TTBR1 */
+	mcr    p15, 0, r7, c2, c0, 1
+
+	/* Read TTBCR and set PD0=1, N = 1 */
+	mrc    p15, 0, r6, c2, c0, 2
+	orr    r6, r6, #0x11
+	mcr    p15, 0, r6, c2, c0, 2
+
+	dsb
+	isb
+
+	/* flush the TLB */
+	ldr    r6, =0x0
+	mcr    p15, 0, r6, c8, c3, 0
+
+	/* Disable L1 data cache. */
+	mrc    p15, 0, r6, c1, c0, 0
+	bic    r6, r6, #0x4
+	mcr    p15, 0, r6, c1, c0, 0
+
+	dsb
+
+#ifdef CONFIG_CACHE_L2X0
+	/*
+	 * Sync L2 and then disable it.
+	 */
+	ldr   r7, =L2_BASE_ADDR
+	add   r7, r7, #PERIPBASE_VIRT
+	/* Wait for background operations to complete. */
+wait_for_l2_to_idle:
+	ldr   r6, [r7, #0x730]
+	cmp   r6, #0x0
+	bne   wait_for_l2_to_idle
+	ldr   r6, =0x0
+	str   r6, [r7, #0x730]
+	/* Disable L2. */
+	str   r6, [r7, #0x100]
+
+	dsb
+	isb
+#endif
 
-	mrc  p15, 0, r0, c10, c0, 0      @//@ Read the lockdown register
-	                                             @//@ (victim will be incremented)
-	bic   r0, r0, #1                     @//@ Clear the preserve bit
-	mcr   p15, 0, r0, c10, c0, 0     @//@ Write to the lockdown register
+	ldr     r6, =CCM_BASE_ADDR
+	add     r6, r6, #PERIPBASE_VIRT
+	ldr     r5, =MMDC_P0_BASE_ADDR
+	add     r5, r5, #PERIPBASE_VIRT
+	ldr     r7, =MX6Q_IOMUXC_BASE_ADDR
+	add     r7, r7, #PERIPBASE_VIRT
+	ldr     r12, =ANATOP_BASE_ADDR
+	add     r12, r12, #PERIPBASE_VIRT
 
 	/* Disable automatic power saving. */
 
@@ -819,13 +850,58 @@ poll_conreq_clear_2:
 
 done:
 
+#ifdef CONFIG_CACHE_L2X0
+	/* Enable L2. */
+	ldr   r1, =L2_BASE_ADDR
+	add   r1, r1, #PERIPBASE_VIRT
+	ldr   r6, =0x1
+	str   r6, [r1, #0x100]
+#endif
+
+	/* Enable L1 data cache. */
+	mrc    p15, 0, r6, c1, c0, 0
+	orr    r6, r6, #0x4
+	mcr    p15, 0, r6, c1, c0, 0
+
+	/* Restore the TTBCR */
+	dsb
+	isb
+
+	/* Read TTBCR and set PD0=0, N = 0 */
+	mrc    p15, 0, r6, c2, c0, 2
+	bic    r6, r6, #0x11
+	mcr    p15, 0, r6, c2, c0, 2
+	dsb
+	isb
+
+	/* flush the TLB */
+	ldr    r6, =0x0
+	mcr    p15, 0, r6, c8, c3, 0
+
+	dsb
+	isb
+
+	/* Enable Branch Prediction, Z bit in SCTLR. */
+	mrc    p15, 0, r6, c1, c0, 0
+	orr    r6, r6, #0x800
+	mcr    p15, 0, r6, c1, c0, 0
+
+	/* Flush the Branch Target Address Cache (BTAC) */
+	ldr    r6, =0x0
+	mcr    p15, 0, r6, c7, c1, 6
+
 	/* Restore registers */
 
 	ldmfd sp!, {r4,r5,r6, r7, r8, r9, r10, r11, r12}
 
 	mov     pc, lr
+	/*
+	 * Add ltorg here to ensure that all
+	 * literals are stored here and are
+	 * within the text space.
+	 */
+	.ltorg
+mx6sl_ddr3_iram_end:
+
+ENDPROC(mx6sl_ddr3_freq_change)
 
-	.type   mx6l_do_ddr_freq_change, #object
-ENTRY(mx6l_do_ddr_freq_change)
-	.word   mx6l_ddr3_freq_change
-	.size    mx6l_ddr3_freq_change, . - mx6l_ddr3_freq_change
diff --git a/arch/arm/mach-mx6/mx6sl_wfi.S b/arch/arm/mach-mx6/mx6sl_wfi.S
index bd5a00c67828..11bbadbf9200 100644
--- a/arch/arm/mach-mx6/mx6sl_wfi.S
+++ b/arch/arm/mach-mx6/mx6sl_wfi.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2012-2013 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright (C) 2012-2014 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
@@ -18,7 +18,18 @@
 
 #include <linux/linkage.h>
 #include <mach/hardware.h>
-#define IRAM_WAIT_SIZE 	(1 << 11)
+/*
+ * The code size is limited to 1.5K
+ * as we may need to store this code
+ * along with the suspend and DDR freq change
+ * code within 8K of IRAM.
+ */
+
+#define IRAM_WAIT_SIZE 	MX6SL_WFI_IRAM_CODE_SIZE
+
+.extern iram_tlb_phys_addr
+.globl mx6sl_wfi_iram_start
+.globl mx6sl_wfi_iram_end
 
 
 	.macro	sl_ddr_io_save
@@ -147,6 +158,7 @@ fifo_reset2_wait:
 
 	.endm
 
+	.align 3
 /*
  *  mx6sl_wait
  *
@@ -157,62 +169,99 @@ fifo_reset2_wait:
  * r1: WFI IRAMcode base address.
  */
 ENTRY(mx6sl_wait)
+mx6sl_wfi_iram_start:
+    push {r4-r12}
 
-    push {r4-r11}
-
-mx6sl_lpm_wfi:
     mov    r11, r2
 
     /* Get the IRAM data storage address. */
     mov    r10, r1
-    mov	r9, r1 		/* get suspend_iram_base */
-     add	r9, r9, #IRAM_WAIT_SIZE	/* 4K */
+    adrl    r9, mx6sl_wfi_iram_end
+    add    r9, r9, #MX6_LPDDR2_WFI_DATA_SIZE
+
+    /*
+     * To ensure no page table walks occur in DDR, we
+     * have a another page table stored in IRAM that only
+     * contains entries pointing to IRAM, AIPS1 and AIPS2.
+     * We need to set the TTBR1 to the new IRAM TLB.
+     * Do the following steps:
+     * 1. Flush the Branch Target Address Cache (BTAC)
+     * 2. Set TTBR1 to point to IRAM page table.
+     * 3. Disable page table walks in TTBR0 (PD0 = 1)
+     * 4. Set TTBR0.N=1, implying 0-2G is translated by TTBR0
+     *     and 2-4G is translated by TTBR1.
+     */
 
-    ldr     r1, =MX6Q_IOMUXC_BASE_ADDR
-    add   r1, r1, #PERIPBASE_VIRT
+    ldr    r6, =iram_tlb_phys_addr
+    ldr    r7, [r6]
 
-      /* Save the DDR IO state. */
-     sl_ddr_io_save
+    /* Disable Branch Prediction, Z bit in SCTLR. */
+    mrc    p15, 0, r6, c1, c0, 0
+    bic    r6, r6, #0x800
+    mcr    p15, 0, r6, c1, c0, 0
 
-    ldr     r3, =ANATOP_BASE_ADDR
-    add   r3, r3, #PERIPBASE_VIRT
+    /* Flush the BTAC. */
+    ldr    r6, =0x0
+    mcr    p15, 0, r6, c7, c1, 6
 
-    ldr     r2, =CCM_BASE_ADDR
-    add   r2, r2, #PERIPBASE_VIRT
+    dsb
+    isb
 
-    ldr     r8, =MMDC_P0_BASE_ADDR
-    add   r8, r8, #PERIPBASE_VIRT
+    /* Store the IRAM table in TTBR1 */
+    mcr    p15, 0, r7, c2, c0, 1
 
+    /* Read TTBCR and set PD0=1, N = 1 */
+    mrc    p15, 0, r6, c2, c0, 2
+    orr    r6, r6, #0x11
+    mcr    p15, 0, r6, c2, c0, 2
 
-    /* Prime all TLB entries. */
-    adr   r7, mx6sl_lpm_wfi               @Address in this function.
+    dsb
+    isb
 
-    ldr     r6, [r7]
+    /* flush the TLB */
+    ldr    r6, =0x0
+    mcr    p15, 0, r6, c8, c3, 0
 
-    ldr     r6, [r8]
-    ldr     r6, [r3]
-    ldr     r6, [r2]
-    ldr     r6, [r1]
+    /* Disable L1 data cache. */
+    mrc    p15, 0, r6, c1, c0, 0
+    bic    r6, r6, #0x4
+    mcr    p15, 0, r6, c1, c0, 0
 
-     /* Drain all the L1 buffers. */
-     dsb
+    dsb
 
 #ifdef CONFIG_CACHE_L2X0
-    /* Need to make sure the buffers in L2 are drained.
-      * Performing a sync operation does this. */
-     ldr     r7, =L2_BASE_ADDR
-     add   r7, r7, #PERIPBASE_VIRT
-     mov  r6, #0x0
-     str    r6, [r7, #0x730]
+    /*
+     * Sync L2 and then disable it.
+     */
+    ldr   r1, =L2_BASE_ADDR
+    add   r1, r1, #PERIPBASE_VIRT
+    /* Wait for background operations to complete. */
+wait_for_l2_to_idle:
+    ldr   r6, [r1, #0x730]
+    cmp   r6, #0x0
+    bne   wait_for_l2_to_idle
+    ldr   r6, =0x0
+    str   r6, [r1, #0x730]
+    /* Disable L2. */
+    str   r6, [r1, #0x100]
+    dsb
+    isb
 #endif
 
-     /* The second dsb might be needed to keep cache sync (device write)
-       * ordering with the memory accesses before it.
-       */
-     dsb
-     isb
+    ldr     r1, =MX6Q_IOMUXC_BASE_ADDR
+    add   r1, r1, #PERIPBASE_VIRT
+
+     /* Save the DDR IO state. */
+     sl_ddr_io_save
 
-     /* Disable Automatic power savings. */
+    ldr     r3, =ANATOP_BASE_ADDR
+    add     r3, r3, #PERIPBASE_VIRT
+    ldr     r2, =CCM_BASE_ADDR
+    add     r2, r2, #PERIPBASE_VIRT
+    ldr     r8, =MMDC_P0_BASE_ADDR
+    add     r8, r8, #PERIPBASE_VIRT
+
+    /* Disable Automatic power savings. */
      ldr    r6, [r8, #0x404]
      orr    r6, r6, #0x01
      str    r6, [r8, #0x404]
@@ -316,10 +365,10 @@ no_analog_saving:
 cont:
      /*Set the AHB to 3MHz. AXI to 3MHz. */
      ldr    r9, [r2, #0x14]
-     mov   r6, r9
+     mov    r6, r9
      orr    r6, r6, #0x1c00
      orr    r6, r6, #0x70000
-     str     r6, [r2, #0x14]
+     str    r6, [r2, #0x14]
 
     /* Loop till podf is accepted. */
 ahb_podf:
@@ -414,14 +463,16 @@ podf_loop:
      b      do_wfi
 
 do_audio_arm_clk:
-    /* ARM is from PLL2_PFD2_400M here.
-      * Switch ARM to bypassed PLL1.
-      */
+    /*
+     * ARM is from PLL2_PFD2_400M here.
+     * Switch ARM to bypassed PLL1.
+     */
      ldr    r6, [r2, #0xC]
      bic   r6, r6, #0x4
      str   r6, [r2, #0xC]
 
-     /* Set the ARM_PODF to divide by 2
+     /*
+      * Set the ARM_PODF to divide by 2
       * as IPG is at 4MHz, we cannot run
       * ARM_CLK above 9.6MHz when
       * system enters WAIT mode.
@@ -451,20 +502,21 @@ podf_loop1:
      cmp    r11, #1
      beq    audio_arm_clk_restore
 
-      /* Check if powered down
-        * analog components.
-        */
-      cmp     r7, #0x1
-      bne     skip_analog_restore
+     /*
+      * Check if powered down
+      * analog components.
+      */
+     cmp     r7, #0x1
+     bne     skip_analog_restore
 
-     /*Power up the regular bandgap. */
+     /* Power up the regular bandgap. */
      ldr     r6, [r3, #0x150]
      bic      r6, r6, #0x1
      str     r6, [r3, #0x150]
 
     /* turn on the bias current
-      * from the regular bandgap.
-      */
+     * from the regular bandgap.
+     */
      ldr     r6, [r3, #0x260]
      bic     r6, r6, #0x80
      str     r6, [r3, #0x260]
@@ -474,7 +526,8 @@ podf_loop1:
      bic     r6, r6, #0x20
      str     r6, [r3, #0x260]
 
-    /* Set the OSC bias current to max
+    /*
+     * Set the OSC bias current to max
      * value for normal operation.
      */
      ldr     r6, [r3, #0x150]
@@ -542,8 +595,8 @@ audio_arm_clk_restore:
      str   r6, [r2, #0xC]
 
 wfi_restore:
-     mov	r9, r10 		/* get suspend_iram_base */
-     add	r9, r9, #IRAM_WAIT_SIZE	/* 4K */
+     adrl    r9, mx6sl_wfi_iram_end
+     add    r9, r9, #MX6_LPDDR2_WFI_DATA_SIZE
 
     /* Restore the DDR IO before exiting self-refresh. */
      sl_ddr_io_restore
@@ -583,6 +636,57 @@ poll_dvfs_clear_1:
      cmp     r6, #0x2000000
      beq     poll_dvfs_clear_1
 
+     /* Enable Automatic power savings. */
+     ldr    r6, [r8, #0x404]
+     bic    r6, r6, #0x01
+     str    r6, [r8, #0x404]
+
+     /* clear SBS - unblock DDR accesses */
+     ldr     r6, [r8, #0x410]
+     bic     r6, r6, #0x100
+     str     r6, [r8, #0x410]
+
+#ifdef CONFIG_CACHE_L2X0
+     /* Enable L2. */
+     ldr   r1, =L2_BASE_ADDR
+     add   r1, r1, #PERIPBASE_VIRT
+     ldr   r6, =0x1
+     str   r6, [r1, #0x100]
+#endif
+
+    /* Enable L1 data cache. */
+    mrc    p15, 0, r6, c1, c0, 0
+    orr    r6, r6, #0x4
+    mcr    p15, 0, r6, c1, c0, 0
+
+    /* Restore the TTBCR */
+
+    dsb
+    isb
+    /* Read TTBCR and set PD0=0, N = 0 */
+    mrc    p15, 0, r6, c2, c0, 2
+    bic    r6, r6, #0x11
+    mcr    p15, 0, r6, c2, c0, 2
+
+    dsb
+    isb
+
+    /* flush the TLB */
+    ldr    r6, =0x0
+    mcr    p15, 0, r6, c8, c3, 0
+    dsb
+    isb
+
+    /* Enable Branch Prediction, Z bit in SCTLR. */
+    mrc    p15, 0, r6, c1, c0, 0
+    orr    r6, r6, #0x800
+    mcr    p15, 0, r6, c1, c0, 0
+
+    /* Flush the Branch Target Address Cache (BTAC) */
+    ldr    r6, =0x0
+    mcr    p15, 0, r6, c7, c1, 6
+
+
      /* Add these nops so that the
       * prefetcher will not try to get
       * any instructions from DDR.
@@ -619,23 +723,16 @@ poll_dvfs_clear_1:
      nop
      nop
 
-     /* Enable Automatic power savings. */
-     ldr    r6, [r8, #0x404]
-     bic    r6, r6, #0x01
-     str    r6, [r8, #0x404]
-
-     /* clear SBS - unblock DDR accesses */
-     ldr     r6, [r8, #0x410]
-     bic     r6, r6, #0x100
-     str     r6, [r8, #0x410]
-
-
-    pop {r4-r11}
+    pop {r4-r12}
 
     /* Restore registers */
     mov     pc, lr
 
-    .type   mx6sl_do_wait, #object
-ENTRY(mx6sl_do_wait)
-    .word   mx6sl_wait
-    .size    mx6sl_wait, . - mx6sl_wait
+    /*
+     * Add ltorg here to ensure that all
+     * literals are stored here and are
+     * within the text space.
+     */
+   .ltorg
+mx6sl_wfi_iram_end:
+
diff --git a/arch/arm/mach-mx6/pm.c b/arch/arm/mach-mx6/pm.c
index d9c04e11bed0..da1debbe7ecf 100644
--- a/arch/arm/mach-mx6/pm.c
+++ b/arch/arm/mach-mx6/pm.c
@@ -1,5 +1,5 @@
 /*
- *  Copyright (C) 2011-2013 Freescale Semiconductor, Inc. All Rights Reserved.
+ *  Copyright (C) 2011-2014 Freescale Semiconductor, Inc. All Rights Reserved.
  */
 
 /*
@@ -85,8 +85,13 @@ extern int set_cpu_freq(int wp);
 extern void mx6_suspend(suspend_state_t state);
 extern void mx6_init_irq(void);
 extern unsigned int gpc_wake_irq[4];
-
 extern bool enable_wait_mode;
+extern unsigned long save_ttbr1(void);
+extern void restore_ttbr1(u32 ttbr1);
+extern unsigned long mx6_suspend_end asm("mx6_suspend_end");
+extern unsigned long mx6_suspend_start asm("mx6_suspend_start");
+
+
 static struct device *pm_dev;
 struct clk *gpc_dvfs_clk;
 static void __iomem *scu_base;
@@ -97,10 +102,9 @@ static void __iomem *gic_dist_base;
 static void __iomem *gic_cpu_base;
 static void __iomem *anatop_base;
 
-static void *suspend_iram_base;
 static void (*suspend_in_iram)(suspend_state_t state,
 	unsigned long iram_paddr, unsigned long suspend_iram_base, unsigned int cpu_type) = NULL;
-static unsigned long iram_paddr, cpaddr;
+static unsigned long cpaddr;
 
 static u32 ccm_ccr, ccm_clpcr, scu_ctrl;
 static u32 gpc_imr[4], gpc_cpu_pup, gpc_cpu_pdn, gpc_cpu, gpc_ctr, gpc_disp;
@@ -110,6 +114,10 @@ static u32 ccm_analog_pll3_480;
 static u32 ccm_anadig_ana_misc2;
 static bool usb_vbus_wakeup_enabled;
 
+void *suspend_iram_base;
+unsigned long suspend_iram_phys_addr;
+unsigned long total_suspend_size;
+
 /*
  * The USB VBUS wakeup should be disabled to avoid vbus wake system
  * up due to vbus comparator is closed at weak 2p5 mode.
@@ -346,8 +354,8 @@ static int mx6_suspend_enter(suspend_state_t state)
 	__raw_writel(__raw_readl(IOMUXC_GPR1) | (1 << 18), IOMUXC_GPR1);
 
 	if (state == PM_SUSPEND_MEM || state == PM_SUSPEND_STANDBY) {
+		u32 ttbr1;
 
-		local_flush_tlb_all();
 		flush_cache_all();
 
 		if (arm_pg) {
@@ -359,8 +367,10 @@ static int mx6_suspend_enter(suspend_state_t state)
 		if (pm_data && pm_data->suspend_enter)
 			pm_data->suspend_enter();
 
-		suspend_in_iram(state, (unsigned long)iram_paddr,
+		ttbr1 = save_ttbr1();
+		suspend_in_iram(state, (unsigned long)suspend_iram_phys_addr,
 			(unsigned long)suspend_iram_base, cpu_type);
+		restore_ttbr1(ttbr1);
 
 		if (pm_data && pm_data->suspend_exit)
 			pm_data->suspend_exit();
@@ -487,6 +497,8 @@ static struct platform_driver mx6_pm_driver = {
 static int __init pm_init(void)
 {
 	int ret = 0;
+	unsigned long suspend_code_size;
+
 	scu_base = IO_ADDRESS(SCU_BASE_ADDR);
 	gpc_base = IO_ADDRESS(GPC_BASE_ADDR);
 	src_base = IO_ADDRESS(SRC_BASE_ADDR);
@@ -506,20 +518,25 @@ static int __init pm_init(void)
 	}
 
 	suspend_set_ops(&mx6_suspend_ops);
-	/* Move suspend routine into iRAM */
-	cpaddr = (unsigned long)iram_alloc(SZ_8K, &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_8K,
-					  MT_MEMORY_NONCACHED);
+	/* Use preallocated IRAM memory. */
+	suspend_iram_phys_addr = MX6_SUSPEND_IRAM_CODE;
+
+	/* Dont ioremap the address, we have fixed the IRAM address at IRAM_BASE_ADDR_VIRT */
+	suspend_iram_base = (void *)IRAM_BASE_ADDR_VIRT + (suspend_iram_phys_addr - IRAM_BASE_ADDR);
+
 	pr_info("cpaddr = %x suspend_iram_base=%x\n",
 		(unsigned int)cpaddr, (unsigned int)suspend_iram_base);
 
+
+	suspend_code_size = (&mx6_suspend_end -&mx6_suspend_start) *4;
 	/*
 	 * Need to run the suspend code from IRAM as the DDR needs
 	 * to be put into low power mode manually.
 	 */
-	memcpy((void *)cpaddr, mx6_suspend, SZ_8K);
+	memcpy((void *)suspend_iram_base, mx6_suspend, suspend_code_size);
+
+	/* Now add the space used for storing various registers and IO in suspend. */
+	total_suspend_size = suspend_code_size + MX6_SUSPEND_DATA_SIZE;
 
 	suspend_in_iram = (void *)suspend_iram_base;
 
diff --git a/arch/arm/mach-mx6/system.c b/arch/arm/mach-mx6/system.c
index 15c942f892a1..98b7914cdd30 100644
--- a/arch/arm/mach-mx6/system.c
+++ b/arch/arm/mach-mx6/system.c
@@ -65,6 +65,9 @@ extern void (*mx6sl_wfi_iram)(int arm_podf, unsigned long wfi_iram_addr, \
 			int audio_mode);
 extern void mx6_wait(void *num_cpu_idle_lock, void *num_cpu_idle, \
 				int wait_arm_podf, int cur_arm_podf);
+extern unsigned long save_ttbr1(void);
+extern void restore_ttbr1(u32 ttbr1);
+
 extern bool enable_wait_mode;
 extern int low_bus_freq_mode;
 extern int audio_bus_freq_mode;
@@ -320,13 +323,17 @@ void arch_idle_single_core(void)
 				  * reduce power.
 				  */
 				u32 org_arm_podf = __raw_readl(MXC_CCM_CACRR);
+				u32 ttbr1;
 
+				outer_sync();
 				/* Need to run WFI code from IRAM so that
-				  * we can lower DDR freq.
-				  */
+				 * we can lower DDR freq.
+				 */
+				ttbr1 = save_ttbr1();
 				mx6sl_wfi_iram(org_arm_podf,
 					(unsigned long)mx6sl_wfi_iram_base,
 					audio_bus_freq_mode);
+				restore_ttbr1(ttbr1);
 			} else {
 				/* Need to set ARM to run at 24MHz since IPG
 				  * is at 12MHz. This is valid for audio mode on
diff --git a/arch/arm/plat-mxc/include/mach/mx6.h b/arch/arm/plat-mxc/include/mach/mx6.h
index 6e5c289c8609..c599dc5ee1ca 100644
--- a/arch/arm/plat-mxc/include/mach/mx6.h
+++ b/arch/arm/plat-mxc/include/mach/mx6.h
@@ -72,12 +72,26 @@
 #define PCIE_ARB_BASE_ADDR		0x01000000
 #define PCIE_ARB_END_ADDR		0x01FFFFFF
 
-/* IRAM
+/*
+ * IRAM page table setup.
+ */
+#define MX6_IRAM_TLB_BASE_ADDR		IRAM_BASE_ADDR
+#define MX6_IRAM_TLB_SIZE			SZ_16K
+#define MX6_IRAM_DDR_FREQ_ADDR		(IRAM_BASE_ADDR + MX6_IRAM_TLB_SIZE)
+/*
+ * The size defined here includes the code size plus the memory
+ * needed to store DDR IOMUX pad settings.
  */
-#define MX6Q_IRAM_BASE_ADDR		IRAM_BASE_ADDR
+#define MX6_IRAM_DDR_FREQ_CODE_SIZE		SZ_4K
+#define TT_ATTRIB_NON_CACHEABLE_1M		0x802
+#define MX6Q_IRAM_BASE_ADDR			(IRAM_BASE_ADDR + MX6_IRAM_TLB_SIZE + MX6_IRAM_DDR_FREQ_CODE_SIZE)
+#define MX6_SUSPEND_IRAM_CODE			IRAM_BASE_ADDR
+#define MX6_SUSPEND_DATA_SIZE			256
+#define MX6_LPDDR2_WFI_DATA_SIZE		100
+
 /* The last 4K is for cpu hotplug to workaround wdog issue*/
-#define MX6Q_IRAM_SIZE			(SZ_256K - SZ_4K)
-#define MX6DL_MX6SL_IRAM_SIZE			(SZ_128K - SZ_4K)
+#define MX6Q_IRAM_SIZE			(SZ_256K - SZ_4K - MX6_IRAM_TLB_SIZE - MX6_IRAM_DDR_FREQ_CODE_SIZE)
+#define MX6DL_MX6SL_IRAM_SIZE		(SZ_128K - SZ_4K - MX6_IRAM_TLB_SIZE - MX6_IRAM_DDR_FREQ_CODE_SIZE)
 
 /* Blocks connected via pl301periph */
 #define ROMCP_ARB_BASE_ADDR		0x00000000
@@ -296,10 +310,13 @@
 #define AIPS1_BASE_ADDR_VIRT		(PERIPBASE_VIRT + AIPS1_ARB_BASE_ADDR)
 #define AIPS2_BASE_ADDR_VIRT		(PERIPBASE_VIRT + AIPS2_ARB_BASE_ADDR)
 #define ARM_PERIPHBASE_VIRT		(PERIPBASE_VIRT + ARM_PERIPHBASE)
+#define IRAM_BASE_ADDR_VIRT		(PERIPBASE_VIRT + IRAM_BASE_ADDR)
+#define L2_BASE_ADDR_VIRT		(PERIPBASE_VIRT + L2_BASE_ADDR)
 #define ROMCP_SIZE			SZ_1M
 #define AIPS1_SIZE			SZ_1M
 #define AIPS2_SIZE			SZ_1M
 #define ARM_PERIPHBASE_SIZE		(SZ_8K + SZ_4K)
+#define IRAM_VIRT_SIZE			SZ_1M
 
 #define SRC_GPR9			0x40
 #define SRC_GPR10			0x44