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.

This commit is cherry-picked:
93ae491d9dbe34a91e2dd5832b02b0f0a390ddbe

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

10 files changed, 446 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 314fd9f38c55..3f7449633c65 100644
--- a/arch/arm/mach-imx/pm-imx6.c
+++ b/arch/arm/mach-imx/pm-imx6.c
@@ -73,15 +73,37 @@
 #define BM_ROMPATCHENL_0D               (0x1 << 0)
 #define ROM_ADDR_FOR_INTERNAL_RAM_BASE  0x10d7c
 
-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;
 struct regmap *romcp;
 
+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() >
@@ -231,8 +253,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;
 }
 
@@ -323,11 +348,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)
@@ -349,46 +417,14 @@ 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;
-
-        if (cpu_is_imx6sx())
-                node = of_find_compatible_node(NULL, NULL, "fsl,lpm-sram");
-        else
-                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 0c3bdfc79c0b..86b3998e002e 100644
--- a/arch/arm/mach-imx/suspend-imx6.S
+++ b/arch/arm/mach-imx/suspend-imx6.S
@@ -536,6 +536,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
@@ -737,6 +772,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