ENGR00297285-1 [MX6x] 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.

Signed-off-by: Ranjani Vaidyanathan <Ranjani.Vaidyanathan@freescale.com>

10 files changed, 448 insertions, 389 deletions

diff --git a/arch/arm/mach-imx/busfreq_ddr3.c b/arch/arm/mach-imx/busfreq_ddr3.c
index 3121e38efaea..c59fd928c385 100644
--- a/arch/arm/mach-imx/busfreq_ddr3.c
+++ b/arch/arm/mach-imx/busfreq_ddr3.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.
  */
 
 /*
@@ -49,8 +49,6 @@ static unsigned long (*iram_iomux_settings)[2];
 
 static void __iomem *mmdc_base;
 static void __iomem *iomux_base;
-static void __iomem *ccm_base;
-static void __iomem *l2_base;
 static void __iomem *gic_dist_base;
 static u32 *irqs_used;
 
@@ -70,10 +68,11 @@ extern int low_bus_freq_mode;
 extern int audio_bus_freq_mode;
 extern void mx6_ddr3_freq_change(u32 freq, void *ddr_settings,
 	bool dll_mode, void *iomux_offsets);
+extern unsigned long save_ttbr1(void);
+extern void restore_ttbr1(unsigned long ttbr1);
 
 #define MIN_DLL_ON_FREQ		333000000
 #define MAX_DLL_OFF_FREQ		125000000
-#define DDR_FREQ_CHANGE_SIZE	0x2000
 
 unsigned long ddr3_dll_mx6q[][2] = {
 	{0x0c, 0x0},
@@ -173,6 +172,7 @@ int update_ddr_freq(int ddr_rate)
 	unsigned int online_cpus = 0;
 	int cpu = 0;
 	int me;
+	unsigned long ttbr1;
 
 	if (!can_change_ddr_freq())
 		return -1;
@@ -229,15 +229,11 @@ int update_ddr_freq(int ddr_rate)
 	while (cpus_in_wfe != online_cpus)
 		udelay(5);
 
-	/*
-	 * Flush the TLB, to ensure no TLB maintenance occurs
-	 * when DDR is in self-refresh.
-	 */
-	local_flush_tlb_all();
+	ttbr1 = save_ttbr1();
 	/* Now we can change the DDR frequency. */
 	mx6_change_ddr_freq(ddr_rate, iram_ddr_settings,
 		dll_off, iram_iomux_settings);
-
+	restore_ttbr1(ttbr1);
 	curr_ddr_rate = ddr_rate;
 
 	/* DDR frequency change is done . */
@@ -258,13 +254,10 @@ int update_ddr_freq(int ddr_rate)
 int init_mmdc_ddr3_settings(struct platform_device *busfreq_pdev)
 {
 	struct device *dev = &busfreq_pdev->dev;
-	struct platform_device *ocram_dev;
 	unsigned int iram_paddr;
 	int i, err;
 	u32 cpu;
 	struct device_node *node;
-	struct gen_pool *iram_pool;
-	void *iram_addr;
 
 	node = of_find_compatible_node(NULL, NULL, "fsl,imx6q-mmdc-combine");
 	if (!node) {
@@ -287,22 +280,6 @@ int init_mmdc_ddr3_settings(struct platform_device *busfreq_pdev)
 	iomux_base = of_iomap(node, 0);
 	WARN(!iomux_base, "unable to map iomux registers\n");
 
-	node = of_find_compatible_node(NULL, NULL, "fsl,imx6q-ccm");
-	if (!node) {
-		printk(KERN_ERR "failed to find imx6q-ccm device tree data!\n");
-		return -EINVAL;
-	}
-	ccm_base = of_iomap(node, 0);
-	WARN(!ccm_base, "unable to map mmdc registers\n");
-
-	node = of_find_compatible_node(NULL, NULL, "arm,pl310-cache");
-	if (!node) {
-		printk(KERN_ERR "failed to find imx6q-pl310-cache device tree data!\n");
-		return -EINVAL;
-	}
-	l2_base = of_iomap(node, 0);
-	WARN(!ccm_base, "unable to map mmdc registers\n");
-
 	node = NULL;
 	node = of_find_compatible_node(NULL, NULL, "arm,cortex-a9-gic");
 	if (!node) {
@@ -374,61 +351,11 @@ int init_mmdc_ddr3_settings(struct platform_device *busfreq_pdev)
 		}
 		irqs_used[cpu] = irq;
 	}
-
-	node = NULL;
-	node = of_find_compatible_node(NULL, NULL, "mmio-sram");
-	if (!node) {
-		dev_err(dev, "%s: failed to find ocram node\n",
-			__func__);
-		return -EINVAL;
-	}
-
-	ocram_dev = of_find_device_by_node(node);
-	if (!ocram_dev) {
-		dev_err(dev, "failed to find ocram device!\n");
-		return -EINVAL;
-	}
-
-	iram_pool = dev_get_gen_pool(&ocram_dev->dev);
-	if (!iram_pool) {
-		dev_err(dev, "iram pool unavailable!\n");
-		return -EINVAL;
-	}
-
 	iomux_settings_size = ARRAY_SIZE(iomux_offsets_mx6q);
-	iram_addr = (void *)gen_pool_alloc(iram_pool,
-						(iomux_settings_size * 8) + 8);
-	iram_iomux_settings = iram_addr;
-	if (!iram_iomux_settings) {
-		dev_err(dev, "unable to alloc iram for IOMUX settings!\n");
-		return -ENOMEM;
-	}
-
-	/*
-	 * Allocate extra space to store the number of entries in the
-	 * ddr_settings plus 4 extra regsiter information that needs
-	 * to be passed to the frequency change code.
-	 * sizeof(iram_ddr_settings) = sizeof(ddr_settings) +
-	 *				entries in ddr_settings + 16.
-	 * The last 4 enties store the addresses of the registers:
-	 * CCM_BASE_ADDR
-	 * MMDC_BASE_ADDR
-	 * IOMUX_BASE_ADDR
-	 * L2X0_BASE_ADDR
-	 */
-	iram_addr = (void *)gen_pool_alloc(iram_pool,
-					(ddr_settings_size * 8) + 8 + 32);
-	iram_ddr_settings = iram_addr;
-	if (!iram_ddr_settings) {
-		dev_err(dev, "unable to alloc iram for ddr settings!\n");
-		return -ENOMEM;
-	}
 
-	i = ddr_settings_size + 1;
-	iram_ddr_settings[i][0] = (unsigned long)mmdc_base;
-	iram_ddr_settings[i+1][0] = (unsigned long)ccm_base;
-	iram_ddr_settings[i+2][0] = (unsigned long)iomux_base;
-	iram_ddr_settings[i+3][0] = (unsigned long)l2_base;
+	iram_iomux_settings = (void *)IMX_IO_P2V(MX6Q_IRAM_TLB_BASE_ADDR) +
+			(DDR3_IOMUX_SETTINGS_ADDR - MX6Q_IRAM_TLB_BASE_ADDR);
+	iram_ddr_settings = iram_iomux_settings + (iomux_settings_size * 8) + 8;
 
 	if (cpu_is_imx6q()) {
 		/* store the IOMUX settings at boot. */
@@ -451,24 +378,13 @@ int init_mmdc_ddr3_settings(struct platform_device *busfreq_pdev)
 		}
 	}
 
-	ddr_freq_change_iram_base = (void *)gen_pool_alloc(iram_pool,
-						DDR_FREQ_CHANGE_SIZE);
-	if (!ddr_freq_change_iram_base) {
-		dev_err(dev, "Cannot alloc iram for ddr freq change code!\n");
-		return -ENOMEM;
-	}
-
-	iram_paddr = gen_pool_virt_to_phys(iram_pool,
-				(unsigned long)ddr_freq_change_iram_base);
-	/*
-	 * Need to remap the area here since we want
-	 * the memory region to be executable.
-	 */
-	ddr_freq_change_iram_base = __arm_ioremap(iram_paddr,
-						DDR_FREQ_CHANGE_SIZE,
-						MT_MEMORY_NONCACHED);
+	iram_paddr = DDR3_FREQ_CODE_ADDR;
+	/* Calculate the virtual address of the code */
+	ddr_freq_change_iram_base =
+			(void *)IMX_IO_P2V(MX6Q_IRAM_TLB_BASE_ADDR) +
+			(iram_paddr - MX6Q_IRAM_TLB_BASE_ADDR);
 	mx6_change_ddr_freq = (void *)fncpy(ddr_freq_change_iram_base,
-		&mx6_ddr3_freq_change, DDR_FREQ_CHANGE_SIZE);
+		&mx6_ddr3_freq_change, DDR3_FREQ_CODE_SIZE);
 
 	curr_ddr_rate = ddr_normal_rate;
 
diff --git a/arch/arm/mach-imx/busfreq_lpddr2.c b/arch/arm/mach-imx/busfreq_lpddr2.c
index a6c4f4e9ae93..ac209e2a3cd0 100644
--- a/arch/arm/mach-imx/busfreq_lpddr2.c
+++ b/arch/arm/mach-imx/busfreq_lpddr2.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.
  */
 
 /*
@@ -42,33 +42,27 @@
 
 #include "hardware.h"
 
-/* DDR settings */
-static void __iomem *mmdc_base;
-static void __iomem *anatop_base;
-static void __iomem *ccm_base;
-static void __iomem *l2_base;
+
 static struct device *busfreq_dev;
 static void *ddr_freq_change_iram_base;
 static int curr_ddr_rate;
 
-unsigned long reg_addrs[4];
+unsigned long ddr_freq_change_iram_paddr;
 
-void (*mx6_change_lpddr2_freq)(u32 ddr_freq, int bus_freq_mode,
-	void *iram_addr) = NULL;
+void (*mx6_change_lpddr2_freq)(u32 ddr_freq, int bus_freq_mode) = NULL;
 
 extern unsigned int ddr_normal_rate;
 extern int low_bus_freq_mode;
 extern int ultra_low_bus_freq_mode;
-extern void mx6_lpddr2_freq_change(u32 freq, int bus_freq_mode,
-	void *iram_addr);
-
-
-#define LPDDR2_FREQ_CHANGE_SIZE	0x1000
+extern void mx6_lpddr2_freq_change(u32 freq, int bus_freq_mode);
 
+extern unsigned long save_ttbr1(void);
+extern void restore_ttbr1(unsigned long ttbr1);
 
 /* change the DDR frequency. */
 int update_lpddr2_freq(int ddr_rate)
 {
+	unsigned long ttbr1;
 	if (ddr_rate == curr_ddr_rate)
 		return 0;
 
@@ -78,11 +72,12 @@ int update_lpddr2_freq(int ddr_rate)
 	 * Flush the TLB, to ensure no TLB maintenance occurs
 	 * when DDR is in self-refresh.
 	 */
-	local_flush_tlb_all();
+	ttbr1 = save_ttbr1();
+
 	/* Now change DDR frequency. */
 	mx6_change_lpddr2_freq(ddr_rate,
-		(low_bus_freq_mode | ultra_low_bus_freq_mode),
-		reg_addrs);
+		(low_bus_freq_mode | ultra_low_bus_freq_mode));
+	restore_ttbr1(ttbr1);
 
 	curr_ddr_rate = ddr_rate;
 
@@ -93,89 +88,16 @@ int update_lpddr2_freq(int ddr_rate)
 
 int init_mmdc_lpddr2_settings(struct platform_device *busfreq_pdev)
 {
-	struct platform_device *ocram_dev;
-	unsigned int iram_paddr;
-	struct device_node *node;
-	struct gen_pool *iram_pool;
-
 	busfreq_dev = &busfreq_pdev->dev;
-	node = of_find_compatible_node(NULL, NULL, "fsl,imx6sl-mmdc");
-	if (!node) {
-		printk(KERN_ERR "failed to find imx6sl-mmdc device tree data!\n");
-		return -EINVAL;
-	}
-	mmdc_base = of_iomap(node, 0);
-	WARN(!mmdc_base, "unable to map mmdc registers\n");
-
-	node = NULL;
-	node = of_find_compatible_node(NULL, NULL, "fsl,imx6sl-ccm");
-	if (!node) {
-		printk(KERN_ERR "failed to find imx6sl-ccm device tree data!\n");
-		return -EINVAL;
-	}
-	ccm_base = of_iomap(node, 0);
-	WARN(!ccm_base, "unable to map ccm registers\n");
-
-	node = of_find_compatible_node(NULL, NULL, "arm,pl310-cache");
-	if (!node) {
-		printk(KERN_ERR "failed to find imx6sl-pl310-cache device tree data!\n");
-		return -EINVAL;
-	}
-	l2_base = of_iomap(node, 0);
-	WARN(!l2_base, "unable to map PL310 registers\n");
-
-	node = of_find_compatible_node(NULL, NULL, "fsl,imx6sl-anatop");
-	if (!node) {
-		printk(KERN_ERR "failed to find imx6sl-pl310-cache device tree data!\n");
-		return -EINVAL;
-	}
-	anatop_base = of_iomap(node, 0);
-	WARN(!anatop_base, "unable to map anatop registers\n");
-
-	node = NULL;
-	node = of_find_compatible_node(NULL, NULL, "mmio-sram");
-	if (!node) {
-		dev_err(busfreq_dev, "%s: failed to find ocram node\n",
-			__func__);
-		return -EINVAL;
-	}
-
-	ocram_dev = of_find_device_by_node(node);
-	if (!ocram_dev) {
-		dev_err(busfreq_dev, "failed to find ocram device!\n");
-		return -EINVAL;
-	}
-
-	iram_pool = dev_get_gen_pool(&ocram_dev->dev);
-	if (!iram_pool) {
-		dev_err(busfreq_dev, "iram pool unavailable!\n");
-		return -EINVAL;
-	}
-
-	reg_addrs[0] = (unsigned long)anatop_base;
-	reg_addrs[1] = (unsigned long)ccm_base;
-	reg_addrs[2] = (unsigned long)mmdc_base;
-	reg_addrs[3] = (unsigned long)l2_base;
-
-	ddr_freq_change_iram_base = (void *)gen_pool_alloc(iram_pool,
-						LPDDR2_FREQ_CHANGE_SIZE);
-	if (!ddr_freq_change_iram_base) {
-		dev_err(busfreq_dev,
-			"Cannot alloc iram for ddr freq change code!\n");
-		return -ENOMEM;
-	}
-
-	iram_paddr = gen_pool_virt_to_phys(iram_pool,
-				(unsigned long)ddr_freq_change_iram_base);
-	/*
-	 * Need to remap the area here since we want
-	 * the memory region to be executable.
-	 */
-	ddr_freq_change_iram_base = __arm_ioremap(iram_paddr,
-						LPDDR2_FREQ_CHANGE_SIZE,
-						MT_MEMORY_NONCACHED);
+
+	ddr_freq_change_iram_paddr = MX6SL_LPDDR2_FREQ_ADDR;
+	/* Calculate the virtual address of the code */
+	ddr_freq_change_iram_base =
+			(void *)IMX_IO_P2V(MX6Q_IRAM_TLB_BASE_ADDR) +
+			(ddr_freq_change_iram_paddr - MX6Q_IRAM_TLB_BASE_ADDR);
+
 	mx6_change_lpddr2_freq = (void *)fncpy(ddr_freq_change_iram_base,
-		&mx6_lpddr2_freq_change, LPDDR2_FREQ_CHANGE_SIZE);
+		&mx6_lpddr2_freq_change, LPDDR2_FREQ_CODE_SIZE);
 
 	curr_ddr_rate = ddr_normal_rate;
 
diff --git a/arch/arm/mach-imx/cpuidle-imx6sl.c b/arch/arm/mach-imx/cpuidle-imx6sl.c
index c542f56efc74..6b98392d10e8 100644
--- a/arch/arm/mach-imx/cpuidle-imx6sl.c
+++ b/arch/arm/mach-imx/cpuidle-imx6sl.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2012-2013 Freescale Semiconductor, Inc.
+ * Copyright (C) 2012-2014 Freescale Semiconductor, Inc.
  *
  * 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
@@ -20,36 +20,38 @@
 
 #include "common.h"
 #include "cpuidle.h"
+#include "hardware.h"
 
 extern u32 audio_bus_freq_mode;
 extern u32 ultra_low_bus_freq_mode;
 extern unsigned long reg_addrs[];
 extern void imx6sl_low_power_wfi(void);
 
+extern unsigned long save_ttbr1(void);
+extern void restore_ttbr1(unsigned long ttbr1);
+
 static void __iomem *iomux_base;
 static void *wfi_iram_base;
 
 void (*imx6sl_wfi_in_iram_fn)(void *wfi_iram_base,
-	void *iomux_addr, void *regs_addr, u32 audio_mode) = NULL;
+	void *iomux_addr, u32 audio_mode) = NULL;
 
-#define WFI_IN_IRAM_SIZE	0x1000
 
 static int imx6sl_enter_wait(struct cpuidle_device *dev,
 			    struct cpuidle_driver *drv, int index)
 {
 	imx6_set_lpm(WAIT_UNCLOCKED);
 	if (ultra_low_bus_freq_mode || audio_bus_freq_mode) {
-		/*
-		 * Flush the TLB, to ensure no TLB maintenance occurs
-		 * when DDR is in self-refresh.
-		 */
-		local_flush_tlb_all();
+		unsigned long ttbr1;
 		/*
 		 * Run WFI code from IRAM.
 		 * Drop the DDR freq to 1MHz and AHB to 3MHz
 		 * Also float DDR IO pads.
 		 */
-		imx6sl_wfi_in_iram_fn(wfi_iram_base, iomux_base, reg_addrs, audio_bus_freq_mode);
+		ttbr1 = save_ttbr1();
+		imx6sl_wfi_in_iram_fn(wfi_iram_base, iomux_base,
+					audio_bus_freq_mode);
+		restore_ttbr1(ttbr1);
 	} else {
 		imx6sl_set_wait_clk(true);
 		cpu_do_idle();
@@ -83,10 +85,8 @@ static struct cpuidle_driver imx6sl_cpuidle_driver = {
 
 int __init imx6sl_cpuidle_init(void)
 {
-	struct platform_device *ocram_dev;
 	unsigned int iram_paddr;
 	struct device_node *node;
-	struct gen_pool *iram_pool;
 
 	node = of_find_compatible_node(NULL, NULL, "fsl,imx6sl-iomuxc");
 	if (!node) {
@@ -96,50 +96,13 @@ int __init imx6sl_cpuidle_init(void)
 	iomux_base = of_iomap(node, 0);
 	WARN(!iomux_base, "unable to map iomux registers\n");
 
-	node = NULL;
-	node = of_find_compatible_node(NULL, NULL, "mmio-sram");
-	if (!node) {
-		pr_err("%s: failed to find ocram node\n",
-			__func__);
-		return -EINVAL;
-	}
-
-	ocram_dev = of_find_device_by_node(node);
-	if (!ocram_dev) {
-		pr_err("failed to find ocram device!\n");
-		return -EINVAL;
-	}
-
-	iram_pool = dev_get_gen_pool(&ocram_dev->dev);
-	if (!iram_pool) {
-		pr_err("iram pool unavailable!\n");
-		return -EINVAL;
-	}
-	/*
-	 * Allocate IRAM memory when ARM executes WFI in
-	 * ultra_low_power_mode.
-	 */
-	wfi_iram_base = (void *)gen_pool_alloc(iram_pool,
-						WFI_IN_IRAM_SIZE);
-	if (!wfi_iram_base) {
-		pr_err("Cannot alloc iram for wfi code!\n");
-		return -ENOMEM;
-	}
-
-	iram_paddr = gen_pool_virt_to_phys(iram_pool,
-				(unsigned long)wfi_iram_base);
-	/*
-	 * Need to remap the area here since we want
-	 * the memory region to be executable.
-	 */
-	wfi_iram_base = __arm_ioremap(iram_paddr,
-					WFI_IN_IRAM_SIZE,
-					MT_MEMORY_NONCACHED);
-	if (!wfi_iram_base)
-		pr_err("wfi_ram_base NOT remapped\n");
+	iram_paddr = MX6SL_WFI_IRAM_ADDR;
+	/* Calculate the virtual address of the code */
+	wfi_iram_base = (void *)IMX_IO_P2V(MX6Q_IRAM_TLB_BASE_ADDR) +
+				(iram_paddr - MX6Q_IRAM_TLB_BASE_ADDR);
 
 	imx6sl_wfi_in_iram_fn = (void *)fncpy(wfi_iram_base,
-		&imx6sl_low_power_wfi, WFI_IN_IRAM_SIZE);
+		&imx6sl_low_power_wfi, MX6SL_WFI_IRAM_CODE_SIZE);
 
 	return cpuidle_register(&imx6sl_cpuidle_driver, NULL);
 }
diff --git a/arch/arm/mach-imx/ddr3_freq_imx6.S b/arch/arm/mach-imx/ddr3_freq_imx6.S
index 699f32fbe118..cbba4c2a9322 100644
--- a/arch/arm/mach-imx/ddr3_freq_imx6.S
+++ b/arch/arm/mach-imx/ddr3_freq_imx6.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
@@ -17,6 +17,7 @@
  */
 
 #include <linux/linkage.h>
+#include "hardware.h"
 
 #define MMDC0_MDPDC		0x4
 #define MMDC0_MDCF0			0x0c
@@ -41,6 +42,8 @@
 
 #define L2_CACHE_SYNC		0x730
 
+.extern iram_tlb_phys_addr
+
 	.align 3
 
 	.macro	switch_to_528MHz
@@ -331,56 +334,58 @@ ENTRY(mx6_ddr3_freq_change)
 	stmfd	sp!, {r4-r12}
 
 	/*
-	  * r5 -> mmdc_base
-	  * r6 -> ccm_base
-	  * r7 -> iomux_base
-	  * r12 -> l2_base
-	  */
+	 * r5 -> mmdc_base
+	 * r6 -> ccm_base
+	 * r7 -> iomux_base
+	 * r12 -> l2_base
+	 */
 	mov	r4, r0
 	mov	r8, r1
 	mov	r9, r2
 	mov	r11, r3
 
-	/*
-	  * Get the addresses of the registers.
-	  * They are last few entries in the
-	  * ddr_settings parameter.
-	  * The first entry contains the count,
-	  * and each entry is 2 words.
-	  */
-	ldr	r0, [r1]
-	add	r0, r0, #1
-	lsl	r0, r0, #3
-	add	r1, r0, r1
-	/* mmdc_base. */
-	ldr	r5, [r1]
-	add	r1, #8
-	/* ccm_base */
-	ldr	r6, [r1]
-	add	r1, #8
-	/*iomux_base */
-	ldr	r7, [r1]
-	add	r1, #8
-	/*l2_base */
-	ldr	r12, [r1]
-
 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.
 	 */
 
-	adr	r10, ddr_freq_change
-
-	ldr	r2, [r6]
-	ldr	r2, [r5]
-	ldr	r2, [r7]
-	ldr	r2, [r8]
-	ldr	r2, [r10]
-	ldr	r2, [r11]
-	ldr	r2, [r12]
+	/* Flush the Branch Target Address Cache (BTAC) */
+	ldr	r6, =0x0
+	mcr	p15, 0, r6, c7, c1, 6
+	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
+	dsb
+	isb
+
+	ldr	r5, =IMX_IO_P2V(MX6Q_MMDC_P0_BASE_ADDR)
+	ldr	r6, =IMX_IO_P2V(MX6Q_CCM_BASE_ADDR)
+	ldr	r7, =IMX_IO_P2V(MX6Q_IOMUXC_BASE_ADDR)
+	ldr	r12, =IMX_IO_P2V(MX6Q_L2_BASE_ADDR)
 
 #ifdef CONFIG_CACHE_L2X0
 	/*
@@ -882,6 +887,22 @@ poll_conreq_clear_2:
 	beq	poll_conreq_clear_2
 
 done:
+	/* 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
+
 	/* restore registers */
 
 	ldmfd	sp!, {r4-r12}
diff --git a/arch/arm/mach-imx/hardware.h b/arch/arm/mach-imx/hardware.h
index 2b3516d0d238..89dc771baadf 100644
--- a/arch/arm/mach-imx/hardware.h
+++ b/arch/arm/mach-imx/hardware.h
@@ -1,5 +1,5 @@
 /*
- * Copyright 2004-2013 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2004-2014 Freescale Semiconductor, Inc.
  * Copyright 2008 Juergen Beisert, kernel@pengutronix.de
  *
  * This program is free software; you can redistribute it and/or
@@ -94,12 +94,22 @@
  *	ANATOP	0x020c8000+0x004000	->	0xf42c8000+0x004000
  *	UART4	0x021f0000+0x004000	->	0xf42f0000+0x004000
  */
+ #if defined(CONFIG_SOC_IMX6Q) || defined(CONFIG_SOC_IMX6SL)
 #define IMX_IO_P2V(x)	(						\
 			(((x) & 0x80000000) >> 7) |			\
 			(0xf4000000 +					\
 			(((x) & 0x50000000) >> 6) +			\
 			(((x) & 0x0b000000) >> 4) +			\
+			(((x) & 0x00100000) << 5) +			\
 			(((x) & 0x000fffff))))
+#else
+#define IMX_IO_P2V(x)	(						\
+			(((x) & 0x80000000) >> 7) |			\
+			(0xf4000000 +					\
+			(((x) & 0x50000000) >> 6) +			\
+			(((x) & 0x0b000000) >> 4) +			\
+			(((x) & 0x000fffff))))
+#endif
 
 #define IMX_IO_ADDRESS(x)	IOMEM(IMX_IO_P2V(x))
 
diff --git a/arch/arm/mach-imx/imx6sl_wfi.S b/arch/arm/mach-imx/imx6sl_wfi.S
index 0399e704f6c2..20afe0256f5f 100644
--- a/arch/arm/mach-imx/imx6sl_wfi.S
+++ b/arch/arm/mach-imx/imx6sl_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
@@ -17,7 +17,9 @@
  */
 
 #include <linux/linkage.h>
-#define IRAM_WAIT_SIZE 	(1 << 11)
+#include "hardware.h"
+
+.extern iram_tlb_phys_addr
 
 	.macro	sl_ddr_io_save
 
@@ -154,8 +156,7 @@ fifo_reset2_wait:
  *  IRQs are already disabled.
  * r0: WFI IRAMcode base address.
  * r1: IOMUX base address
- * r2: Base address of CCM, ANATOP and MMDC
- * r3: 1 if in audio_bus_freq_mode
+ * r2: 1 if in audio_bus_freq_mode
  */
 	.align 3
 ENTRY(imx6sl_low_power_wfi)
@@ -164,27 +165,57 @@ ENTRY(imx6sl_low_power_wfi)
 
 mx6sl_lpm_wfi:
 	/* Store audio_bus_freq_mode */
-	mov	r11, r3
+	mov	r11, r2
 
-	mov	r4,r2
 	/* Get the IRAM data storage address. */
 	mov	r10, r0
-	mov	r9, r0		/* get suspend_iram_base */
-	add	r9, r9, #IRAM_WAIT_SIZE
-
-	/* Anatop Base address in r3. */
-	ldr	r3, [r4]
-	/* CCM Base Address in r2 */
-	ldr	r2, [r4, #0x4]
-	/* MMDC Base Address in r8 */
-	ldr	r8, [r4, #0x8]
-	/* L2 Base Address in r7 */
-	ldr	r7, [r4, #0xC]
-
-	ldr	r6, [r8]
-	ldr	r6, [r3]
-	ldr	r6, [r2]
-	ldr	r6, [r1]
+	mov	r9, r0		/* get wfi_iram_base */
+	add	r9, r9, #MX6SL_WFI_IRAM_CODE_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.
+	 */
+	/* Flush the BTAC. */
+	ldr	r6, =0x0
+	mcr	p15, 0, r6, c7, c1, 6
+
+	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
+
+	dsb
+	isb
+
+	ldr	r1, =IMX_IO_P2V(MX6Q_IOMUXC_BASE_ADDR)
+	ldr	r3, =IMX_IO_P2V(MX6Q_ANATOP_BASE_ADDR)
+	ldr	r2, =IMX_IO_P2V(MX6Q_CCM_BASE_ADDR)
+	ldr	r8, =IMX_IO_P2V(MX6Q_MMDC_P0_BASE_ADDR)
+	ldr	r7, =IMX_IO_P2V(MX6Q_L2_BASE_ADDR)
 
 	/* Store the original ARM PODF. */
 	ldr	r0, [r2, #0x10]
@@ -546,7 +577,7 @@ ahb_podf1:
 
 	b	wfi_restore
 
-	audio_arm_clk_restore:
+audio_arm_clk_restore:
 	/* Move ARM back to PLL2_PFD2_400M */
 	ldr	r6, [r2, #0xC]
 	orr	r6, r6, #0x4
@@ -555,7 +586,7 @@ ahb_podf1:
 wfi_restore:
 	/* get suspend_iram_base */
 	mov	r9, r10
-	add	r9, r9, #IRAM_WAIT_SIZE
+	add	r9, r9, #MX6SL_WFI_IRAM_CODE_SIZE
 
 	/* Restore the DDR IO before exiting self-refresh. */
 	sl_ddr_io_restore
@@ -585,6 +616,35 @@ 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]
+
+	/* 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
+
 	/*
 	 * Add these nops so that the
 	 * prefetcher will not try to get
@@ -622,16 +682,6 @@ 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}
 
diff --git a/arch/arm/mach-imx/lpddr2_freq_imx6.S b/arch/arm/mach-imx/lpddr2_freq_imx6.S
index a126f110b0d3..3c1c076112ef 100644
--- a/arch/arm/mach-imx/lpddr2_freq_imx6.S
+++ b/arch/arm/mach-imx/lpddr2_freq_imx6.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
@@ -17,6 +17,7 @@
  */
 
 #include <linux/linkage.h>
+#include "hardware.h"
 
 	.macro    mx6sl_switch_to_24MHz
 
@@ -339,30 +340,55 @@ force_measure1:
  *  IRQs are already disabled.
  * r0 : DDR freq.
  * r1: low_bus_freq_mode flag
- * r2: Pointer to array containing addresses of registers.
  */
 	.align 3
 ENTRY(mx6_lpddr2_freq_change)
 
 	push {r4-r10}
 
-	mov	r4, r2
-	ldr	r3, [r4]			@ANATOP_BASE_ADDR
-	ldr	r2, [r4, #0x4]		@CCM_BASE_ADDR
-	ldr	r8, [r4, #0x8]		@MMDC_P0_BASE_ADDR
-	ldr	r7, [r4, #0xC]		@L2_BASE_ADDR
+	/*
+	 * 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.
+	 */
 
-lpddr2_freq_change:
-	adr	r9, lpddr2_freq_change
+	/* Flush the Branch Target Address Cache (BTAC) */
+	ldr	r6, =0x0
+	mcr	p15, 0, r6, c7, c1, 6
 
-	/* Prime all TLB entries. */
-	ldr	r6, [r9]
-	ldr	r6, [r8]
-	ldr	r6, [r3]
-	ldr	r6, [r2]
+	ldr	r6, =iram_tlb_phys_addr
+	ldr	r6, [r6]
+	dsb
+	isb
+	/* Store the IRAM table in TTBR1 */
+	mcr	p15, 0, r6, c2, c0, 1
 
-	 /* Drain all the L1 buffers. */
-	 dsb
+	/* 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
+
+	dsb
+	isb
+
+	ldr	r3, =IMX_IO_P2V(MX6Q_ANATOP_BASE_ADDR)
+	ldr	r2, =IMX_IO_P2V(MX6Q_CCM_BASE_ADDR)
+	ldr	r8, =IMX_IO_P2V(MX6Q_MMDC_P0_BASE_ADDR)
+	ldr	r7, =IMX_IO_P2V(MX6Q_L2_BASE_ADDR)
 
 #ifdef CONFIG_CACHE_L2X0
 	/*
@@ -371,14 +397,13 @@ lpddr2_freq_change:
 	 */
 	mov	r6, #0x0
 	str	r6, [r7, #0x730]
-#endif
-
 	/*
 	 * The second dsb might be needed to keep cache sync (device write)
 	 * ordering with the memory accesses before it.
 	 */
 	dsb
 	isb
+#endif
 
 	/* Disable Automatic power savings. */
 	ldr	r6, [r8, #0x404]
@@ -473,6 +498,53 @@ skip_power_down:
 	bic	r6, r6, #0x100
 	str	r6, [r8, #0x410]
 
+	/* 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
+
+	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 */
diff --git a/arch/arm/mach-imx/mx6.h b/arch/arm/mach-imx/mx6.h
index 16f7eeac2299..2cf91c7e04aa 100644
--- a/arch/arm/mach-imx/mx6.h
+++ b/arch/arm/mach-imx/mx6.h
@@ -1,5 +1,5 @@
 /*
- * Copyright 2004-2013 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2004-2014 Freescale Semiconductor, Inc. All Rights Reserved.
  */
 
 /*
@@ -14,7 +14,7 @@
 #define MX6Q_IO_P2V(x)                  IMX_IO_P2V(x)
 #define MX6Q_IO_ADDRESS(x)              IOMEM(MX6Q_IO_P2V(x))
 
-#define MX6Q_L2_BASE_ADDR               0x00a02000
+#define MX6Q_L2_BASE_ADDR		0x00a02000
 #define MX6Q_L2_SIZE			0x1000
 #define MX6Q_IOMUXC_BASE_ADDR		0x020e0000
 #define MX6Q_IOMUXC_SIZE		0x4000
@@ -26,10 +26,27 @@
 #define MX6Q_ANATOP_SIZE		0x1000
 #define MX6Q_GPC_BASE_ADDR		0x020dc000
 #define MX6Q_GPC_SIZE			0x4000
-#define MX6Q_MMDC_P0_BASE_ADDR		0x021b0000
+#define MX6Q_MMDC_P0_BASE_ADDR	0x021b0000
 #define MX6Q_MMDC_P0_SIZE		0x4000
-#define MX6Q_MMDC_P1_BASE_ADDR		0x021b4000
+#define MX6Q_MMDC_P1_BASE_ADDR	0x021b4000
 #define MX6Q_MMDC_P1_SIZE		0x4000
+#define MX6Q_AIPS1_BASE_ADDR		0x02000000
+#define MX6Q_AIPS1_SIZE		0x100000
+#define MX6Q_AIPS2_BASE_ADDR		0x02100000
+#define MX6Q_AIPS2_SIZE		0x100000
+
+#define MX6Q_IRAM_TLB_BASE_ADDR	0x00900000
+#define MX6Q_IRAM_TLB_SIZE		0x100000
+#define TT_ATTRIB_NON_CACHEABLE_1M	0x802
 
 #define MX6_SUSPEND_IRAM_SIZE		0x1000
+#define LPDDR2_FREQ_CODE_SIZE		0x600
+#define DDR3_FREQ_CODE_SIZE		0xC00
+#define DDR3_IOMUX_SETTINGS_SIZE	0x400
+#define MX6SL_WFI_IRAM_CODE_SIZE	0x600
+#define MX6_SUSPEND_IRAM_ADDR		MX6Q_IRAM_TLB_BASE_ADDR
+#define DDR3_FREQ_CODE_ADDR		(MX6_SUSPEND_IRAM_ADDR + MX6_SUSPEND_IRAM_SIZE)
+#define DDR3_IOMUX_SETTINGS_ADDR	(DDR3_FREQ_CODE_ADDR + DDR3_FREQ_CODE_SIZE)
+#define MX6SL_LPDDR2_FREQ_ADDR	(MX6_SUSPEND_IRAM_ADDR + MX6_SUSPEND_IRAM_SIZE)
+#define MX6SL_WFI_IRAM_ADDR		(MX6SL_LPDDR2_FREQ_ADDR + LPDDR2_FREQ_CODE_SIZE)
 #endif
diff --git a/arch/arm/mach-imx/pm-imx6.c b/arch/arm/mach-imx/pm-imx6.c
index a4af0c539199..de46b87ba139 100644
--- a/arch/arm/mach-imx/pm-imx6.c
+++ b/arch/arm/mach-imx/pm-imx6.c
@@ -1,5 +1,5 @@
 /*
- * Copyright 2011-2013 Freescale Semiconductor, Inc.
+ * Copyright 2011-2014 Freescale Semiconductor, Inc.
  * Copyright 2011 Linaro Ltd.
  *
  * The code contained herein is licensed under the GNU General Public
@@ -64,14 +64,36 @@
 
 #define MX6_INT_IOMUXC			32
 
-static struct gen_pool *iram_pool;
+unsigned long iram_tlb_base_addr;
+unsigned long iram_tlb_phys_addr;
+
 static void *suspend_iram_base;
-static unsigned long iram_size, iram_paddr;
+static unsigned long iram_paddr;
 static int (*suspend_in_iram_fn)(void *iram_vbase,
 	unsigned long iram_pbase, unsigned int cpu_type);
 static unsigned int cpu_type;
 static void __iomem *ccm_base;
 
+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(unsigned long ttbr1)
+{
+	asm volatile(
+		".align 4\n"
+		"mcr p15, 0, %0, c2, c0, 1\n"
+	: : "r" (ttbr1)
+	);
+}
+
 void imx6_set_cache_lpm_in_wait(bool enable)
 {
 	if ((cpu_is_imx6q() && imx_get_soc_revision() >
@@ -219,8 +241,11 @@ static int imx6_suspend_finish(unsigned long val)
 	 * call low level suspend function in iram,
 	 * as we need to float DDR IO.
 	 */
-	local_flush_tlb_all();
+	u32 ttbr1;
+
+	ttbr1 = save_ttbr1();
 	suspend_in_iram_fn(suspend_iram_base, iram_paddr, cpu_type);
+	restore_ttbr1(ttbr1);
 	return 0;
 }
 
@@ -303,11 +328,54 @@ static struct map_desc imx6_pm_io_desc[] __initdata = {
 	imx_map_entry(MX6Q, ANATOP, MT_DEVICE),
 	imx_map_entry(MX6Q, GPC, MT_DEVICE),
 	imx_map_entry(MX6Q, L2, MT_DEVICE),
+	imx_map_entry(MX6Q, IRAM_TLB, MT_MEMORY_NONCACHED),
 };
 
 void __init imx6_pm_map_io(void)
 {
+	unsigned long i;
+
 	iotable_init(imx6_pm_io_desc, ARRAY_SIZE(imx6_pm_io_desc));
+
+	/*
+	 * Allocate IRAM for page tables to be used
+	 * when DDR is in self-refresh.
+	 */
+	iram_tlb_phys_addr = MX6Q_IRAM_TLB_BASE_ADDR;
+	iram_tlb_base_addr = IMX_IO_P2V(MX6Q_IRAM_TLB_BASE_ADDR);
+
+	/* Set all entries to 0. */
+	memset((void *)iram_tlb_base_addr, 0, SZ_16K);
+
+	/*
+	 * Make sure the IRAM virtual address has a mapping
+	 * in the IRAM page table.
+	 */
+	i = (IMX_IO_P2V(MX6Q_IRAM_TLB_BASE_ADDR) >> 18) / 4;
+	*((unsigned long *)iram_tlb_base_addr + i) =
+		MX6Q_IRAM_TLB_BASE_ADDR | TT_ATTRIB_NON_CACHEABLE_1M;
+	/*
+	 * Make sure the AIPS1 virtual address has a mapping
+	 * in the IRAM page table.
+	 */
+	i = (IMX_IO_P2V(MX6Q_AIPS1_BASE_ADDR) >> 18) / 4;
+	*((unsigned long *)iram_tlb_base_addr + i) =
+		MX6Q_AIPS1_BASE_ADDR | TT_ATTRIB_NON_CACHEABLE_1M;
+	/*
+	* Make sure the AIPS2 virtual address has a mapping
+	* in the IRAM page table.
+	*/
+	i = (IMX_IO_P2V(MX6Q_AIPS2_BASE_ADDR) >> 18) / 4;
+	*((unsigned long *)iram_tlb_base_addr + i) =
+		MX6Q_AIPS2_BASE_ADDR | TT_ATTRIB_NON_CACHEABLE_1M;
+	/*
+	 * Make sure the AIPS2 virtual address has a mapping
+	 * in the IRAM page table.
+	 */
+	i = (IMX_IO_P2V(MX6Q_L2_BASE_ADDR) >> 18) / 4;
+	*((unsigned long *)iram_tlb_base_addr + i) =
+		MX6Q_L2_BASE_ADDR | TT_ATTRIB_NON_CACHEABLE_1M;
+
 }
 
 static int imx6_pm_valid(suspend_state_t state)
@@ -329,43 +397,13 @@ void imx6_pm_set_ccm_base(void __iomem *base)
 
 void __init imx6_pm_init(void)
 {
-	struct device_node *node;
-	unsigned long iram_base;
-	struct platform_device *pdev;
-
-	node = of_find_compatible_node(NULL, NULL, "mmio-sram");
-	if (!node) {
-		pr_err("failed to find ocram node!\n");
-		return;
-	}
-
-	pdev = of_find_device_by_node(node);
-	if (!pdev) {
-		pr_err("failed to find ocram device!\n");
-		return;
-	}
-
-	iram_pool = dev_get_gen_pool(&pdev->dev);
-	if (!iram_pool) {
-		pr_err("iram pool unavailable!\n");
-		return;
-	}
-
-	iram_size = MX6_SUSPEND_IRAM_SIZE;
-
-	iram_base = gen_pool_alloc(iram_pool, iram_size);
-	if (!iram_base) {
-		pr_err("unable to alloc iram!\n");
-		return;
-	}
-
-	iram_paddr = gen_pool_virt_to_phys(iram_pool, iram_base);
-
-	suspend_iram_base = __arm_ioremap(iram_paddr, iram_size,
-			MT_MEMORY_NONCACHED);
+	iram_paddr = MX6_SUSPEND_IRAM_ADDR;
+	/* Get the virtual address of the suspend code. */
+	suspend_iram_base = (void *)IMX_IO_P2V(MX6Q_IRAM_TLB_BASE_ADDR) +
+			(iram_paddr - MX6Q_IRAM_TLB_BASE_ADDR);
 
 	suspend_in_iram_fn = (void *)fncpy(suspend_iram_base,
-		&imx6_suspend, iram_size);
+		&imx6_suspend, MX6_SUSPEND_IRAM_SIZE);
 
 	suspend_set_ops(&imx6_pm_ops);
 
diff --git a/arch/arm/mach-imx/suspend-imx6.S b/arch/arm/mach-imx/suspend-imx6.S
index a620560a5e26..0861966b4ca8 100644
--- a/arch/arm/mach-imx/suspend-imx6.S
+++ b/arch/arm/mach-imx/suspend-imx6.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2010-2013 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright (C) 2010-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
@@ -519,6 +519,41 @@ ddr_io_save_dsm_done:
 	/* need to sync L2 cache before DSM. */
 	sync_l2_cache
 
+	/*
+	 * 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.
+	 */
+	/* Flush the BTAC. */
+	ldr r6, =0x0
+	mcr p15, 0, r6, c7, c1, 6
+
+	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
+
 	ldr	r8, =IMX_IO_P2V(MX6Q_MMDC_P0_BASE_ADDR)
 	/*
 	 * put DDR explicitly into self-refresh and
@@ -710,6 +745,21 @@ poll_dvfs_clear_2:
 	ldr	r7, [r8, #MX6Q_MMDC_MAPSR]
 	bic	r7, r7, #0x1
 	str	r7, [r8, #MX6Q_MMDC_MAPSR]
+
+	/* 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
+
 	/* return to suspend finish */
 	mov	pc, lr