15 files changed, 261 insertions, 97 deletions

diff --git a/arch/arm/mach-imx/busfreq-imx6.c b/arch/arm/mach-imx/busfreq-imx6.c
index 8e640d39013e..fdd0c40d6b66 100644
--- a/arch/arm/mach-imx/busfreq-imx6.c
+++ b/arch/arm/mach-imx/busfreq-imx6.c
@@ -39,6 +39,7 @@
 #include <linux/module.h>
 #include <linux/mutex.h>
 #include <linux/of.h>
+#include <linux/of_fdt.h>
 #include <linux/platform_device.h>
 #include <linux/proc_fs.h>
 #include <linux/reboot.h>
@@ -63,6 +64,9 @@ int low_bus_freq_mode;
 int ultra_low_bus_freq_mode;
 unsigned int ddr_med_rate;
 unsigned int ddr_normal_rate;
+unsigned long ddr_freq_change_total_size;
+unsigned long ddr_freq_change_iram_base;
+unsigned long ddr_freq_change_iram_phys;
 
 static int bus_freq_scaling_initialized;
 static struct device *busfreq_dev;
@@ -72,6 +76,9 @@ static int bus_freq_scaling_is_active;
 static int high_bus_count, med_bus_count, audio_bus_count, low_bus_count;
 static unsigned int ddr_low_rate;
 
+extern unsigned long iram_tlb_phys_addr;
+extern int unsigned long iram_tlb_base_addr;
+
 extern int init_mmdc_lpddr2_settings(struct platform_device *dev);
 extern int init_mmdc_ddr3_settings_imx6q(struct platform_device *dev);
 extern int init_mmdc_ddr3_settings_imx6sx(struct platform_device *dev);
@@ -684,6 +691,63 @@ void release_bus_freq(enum bus_freq_mode mode)
 }
 EXPORT_SYMBOL(release_bus_freq);
 
+static struct map_desc ddr_iram_io_desc __initdata = {
+        /* .virtual and .pfn are run-time assigned */
+        .length         = SZ_1M,
+        .type           = MT_MEMORY_NONCACHED,
+};
+
+const static char *ddr_freq_iram_match[] __initconst = {
+        "fsl,ddr-lpm-sram",
+        NULL
+};
+
+static int __init imx6_dt_find_ddr_sram(unsigned long node,
+                const char *uname, int depth, void *data)
+{
+        unsigned long ddr_iram_addr;
+        __be32 *prop;
+
+        if (of_flat_dt_match(node, ddr_freq_iram_match)) {
+                unsigned long len;
+                prop = of_get_flat_dt_prop(node, "reg", &len);
+                if (prop == NULL || len != (sizeof(unsigned long) * 2))
+                        return EINVAL;
+                ddr_iram_addr = be32_to_cpu(prop[0]);
+                ddr_freq_change_total_size = be32_to_cpu(prop[1]);
+
+                if ((iram_tlb_phys_addr & 0xFFF00000) != (ddr_iram_addr & 0xFFF00000)) {
+                        unsigned long i;
+
+                        /* We need to create a 1M page table entry. */
+                        ddr_iram_io_desc.virtual = IMX_IO_P2V(ddr_iram_addr & 0xFFF00000);
+                        ddr_iram_io_desc.pfn = __phys_to_pfn(ddr_iram_addr & 0xFFF00000);
+                        iotable_init(&ddr_iram_io_desc, 1);
+
+                        /*
+                         * Make sure the ddr_iram virtual address has a mapping
+                         * in the IRAM page table.
+                         */
+                        i = ((IMX_IO_P2V(ddr_iram_addr) >> 20) << 2) / 4;
+                        *((unsigned long *)iram_tlb_base_addr + i) =
+                                (ddr_iram_addr  & 0xFFF00000) | TT_ATTRIB_NON_CACHEABLE_1M;
+
+                }
+                ddr_freq_change_iram_phys = (void *)ddr_iram_addr;
+                ddr_freq_change_iram_base = IMX_IO_P2V(ddr_iram_addr);
+        }
+        return 0;
+}
+
+void __init imx6_busfreq_map_io(void)
+{
+        /*
+         * Get the address of IRAM to be used by the ddr frequency
+         * change code from the device tree.
+         */
+        WARN_ON(of_scan_flat_dt(imx6_dt_find_ddr_sram, NULL));
+}
+
 static void bus_freq_daemon_handler(struct work_struct *work)
 {
         mutex_lock(&bus_freq_mutex);
@@ -782,6 +846,10 @@ static int busfreq_probe(struct platform_device *pdev)
 
         busfreq_dev = &pdev->dev;
 
+        /* Return if no IRAM space is allocated for ddr freq change code. */
+        if (!ddr_freq_change_iram_base)
+                return ENOMEM;
+
         pll2_400 = devm_clk_get(&pdev->dev, "pll2_pfd2_396m");
         if (IS_ERR(pll2_400)) {
                 dev_err(busfreq_dev, "%s: failed to get pll2_pfd2_396m\n",
diff --git a/arch/arm/mach-imx/busfreq_ddr3.c b/arch/arm/mach-imx/busfreq_ddr3.c
index e19249dd2265..f2719205097b 100644
--- a/arch/arm/mach-imx/busfreq_ddr3.c
+++ b/arch/arm/mach-imx/busfreq_ddr3.c
@@ -70,7 +70,6 @@ static void __iomem *iomux_base;
 static void __iomem *gic_dist_base;
 static u32 *irqs_used;
 
-static void *ddr_freq_change_iram_base;
 static int ddr_settings_size;
 static int iomux_settings_size;
 static volatile unsigned int cpus_in_wfe;
@@ -91,7 +90,12 @@ 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);
-extern unsigned long iram_tlb_phys_addr;
+extern unsigned long ddr_freq_change_iram_base;
+extern unsigned long ddr_freq_change_total_size;
+extern unsigned long mx6_ddr3_freq_change_start asm("mx6_ddr3_freq_change_start");
+extern unsigned long mx6_ddr3_freq_change_end asm("mx6_ddr3_freq_change_end");
+extern unsigned long imx6sx_ddr3_freq_change_start asm("imx6sx_ddr3_freq_change_start");
+extern unsigned long imx6sx_ddr3_freq_change_end asm("imx6sx_ddr3_freq_change_end");
 
 #define MIN_DLL_ON_FREQ         333000000
 #define MAX_DLL_OFF_FREQ                125000000
@@ -343,6 +347,7 @@ int init_mmdc_ddr3_settings_imx6sx(struct platform_device *busfreq_pdev)
 {
         int i;
         struct device_node *node;
+        unsigned long ddr_code_size;
 
         node = of_find_compatible_node(NULL, NULL, "fsl,imx6q-mmdc");
         if (!node) {
@@ -384,10 +389,27 @@ int init_mmdc_ddr3_settings_imx6sx(struct platform_device *busfreq_pdev)
 
         iomux_settings_size = ARRAY_SIZE(iomux_offsets_mx6sx);
 
-        iram_iomux_settings = (void *)IMX_IO_P2V(iram_tlb_phys_addr) +
-                        DDR3_IOMUX_SETTINGS_ADDR_OFFSET;
+        ddr_code_size = (&imx6sx_ddr3_freq_change_end -&imx6sx_ddr3_freq_change_start) *4 +
+                        sizeof(*imx6sx_busfreq_info);
 
+        imx6sx_busfreq_info = (struct imx6_busfreq_info *)ddr_freq_change_iram_base;
+
+        imx6sx_change_ddr_freq = (void *)fncpy((void *)ddr_freq_change_iram_base + sizeof(*imx6sx_busfreq_info),
+                &imx6sx_ddr3_freq_change, ddr_code_size - sizeof(*imx6sx_busfreq_info));
+
+        /*
+         * Store the size of the array in iRAM also,
+         * increase the size by 8 bytes.
+         */
+        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)
+                > ddr_freq_change_total_size) {
+                printk(KERN_ERR "Not enough memory allocated for DDR Frequency change code.\n");
+                return EINVAL;
+        }
+
         for (i = 0; i < iomux_settings_size; i++) {
                 iomux_offsets_mx6sx[i][1] =
                         readl_relaxed(iomux_base +
@@ -398,18 +420,6 @@ int init_mmdc_ddr3_settings_imx6sx(struct platform_device *busfreq_pdev)
                                 iomux_offsets_mx6sx[i][1];
         }
 
-        /* Calculate the virtual address of the code */
-        ddr_freq_change_iram_base =
-                        (void *)IMX_IO_P2V(iram_tlb_phys_addr) +
-                        DDR3_FREQ_CODE_ADDR_OFFSET;
-
-        imx6sx_busfreq_info = (struct imx6_busfreq_info *)ddr_freq_change_iram_base;
-
-        imx6sx_change_ddr_freq = (void *)fncpy(ddr_freq_change_iram_base +
-                sizeof(*imx6sx_busfreq_info),
-                &imx6sx_ddr3_freq_change,
-                DDR3_FREQ_CODE_SIZE - sizeof(*imx6sx_busfreq_info));
-
         curr_ddr_rate = ddr_normal_rate;
 
         return 0;
@@ -421,6 +431,7 @@ int init_mmdc_ddr3_settings_imx6q(struct platform_device *busfreq_pdev)
         int i, err;
         u32 cpu;
         struct device_node *node;
+        unsigned long ddr_code_size;
 
         node = of_find_compatible_node(NULL, NULL, "fsl,imx6q-mmdc-combine");
         if (!node) {
@@ -516,12 +527,24 @@ int init_mmdc_ddr3_settings_imx6q(struct platform_device *busfreq_pdev)
         }
         iomux_settings_size = ARRAY_SIZE(iomux_offsets_mx6q);
 
+        ddr_code_size = (&mx6_ddr3_freq_change_end -&mx6_ddr3_freq_change_start) *4;
 
-        iram_iomux_settings = (void *)IMX_IO_P2V(iram_tlb_phys_addr) +
-                        DDR3_IOMUX_SETTINGS_ADDR_OFFSET;
+        mx6_change_ddr_freq = (void *)fncpy((void *)ddr_freq_change_iram_base,
+                &mx6_ddr3_freq_change, ddr_code_size);
 
+        /*
+         * Store the size of the array in iRAM also,
+         * increase the size by 8 bytes.
+         */
+        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)
+                > ddr_freq_change_total_size) {
+                printk(KERN_ERR "Not enough memory allocated for DDR Frequency change code.\n");
+                return EINVAL;
+        }
+
         if (cpu_is_imx6q()) {
                 /* store the IOMUX settings at boot. */
                 for (i = 0; i < iomux_settings_size; i++) {
@@ -543,14 +566,6 @@ int init_mmdc_ddr3_settings_imx6q(struct platform_device *busfreq_pdev)
                 }
         }
 
-        /* Calculate the virtual address of the code */
-        ddr_freq_change_iram_base =
-                        (void *)IMX_IO_P2V(iram_tlb_phys_addr) +
-                        DDR3_FREQ_CODE_ADDR_OFFSET;
-
-        mx6_change_ddr_freq = (void *)fncpy(ddr_freq_change_iram_base,
-                &mx6_ddr3_freq_change, DDR3_FREQ_CODE_SIZE);
-
         curr_ddr_rate = ddr_normal_rate;
 
         return 0;
diff --git a/arch/arm/mach-imx/busfreq_lpddr2.c b/arch/arm/mach-imx/busfreq_lpddr2.c
index 1e332bf66cbf..5c96885f9fc5 100644
--- a/arch/arm/mach-imx/busfreq_lpddr2.c
+++ b/arch/arm/mach-imx/busfreq_lpddr2.c
@@ -44,7 +44,6 @@
 
 
 static struct device *busfreq_dev;
-static void *ddr_freq_change_iram_base;
 static int curr_ddr_rate;
 static DEFINE_SPINLOCK(freq_lock);
 
@@ -55,10 +54,11 @@ 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);
 extern void imx6sx_lpddr2_freq_change(u32 freq, int bus_freq_mode);
-
 extern unsigned long save_ttbr1(void);
 extern void restore_ttbr1(unsigned long ttbr1);
-extern unsigned long iram_tlb_phys_addr;
+extern unsigned long ddr_freq_change_iram_base;
+extern unsigned long imx6_lpddr2_freq_change_start asm("imx6_lpddr2_freq_change_start");
+extern unsigned long imx6_lpddr2_freq_change_end asm("imx6_lpddr2_freq_change_end");
 
 /* change the DDR frequency. */
 int update_lpddr2_freq(int ddr_rate)
@@ -93,20 +93,18 @@ int update_lpddr2_freq(int ddr_rate)
 int init_mmdc_lpddr2_settings(struct platform_device *busfreq_pdev)
 {
         busfreq_dev = &busfreq_pdev->dev;
+        unsigned long ddr_code_size;
 
-        /* Calculate the virtual address of the code */
-        ddr_freq_change_iram_base =
-                        (void *)IMX_IO_P2V(iram_tlb_phys_addr) +
-                        MX6SL_LPDDR2_FREQ_ADDR_OFFSET;
+        ddr_code_size = (&imx6_lpddr2_freq_change_end -&imx6_lpddr2_freq_change_start) *4;
 
         if (cpu_is_imx6sl())
                 mx6_change_lpddr2_freq = (void *)fncpy(
                         ddr_freq_change_iram_base,
-                        &mx6_lpddr2_freq_change, LPDDR2_FREQ_CODE_SIZE);
+                        &mx6_lpddr2_freq_change, ddr_code_size);
         else if (cpu_is_imx6sx())
                 mx6_change_lpddr2_freq = (void *)fncpy(
                         ddr_freq_change_iram_base,
-                        &imx6sx_lpddr2_freq_change, LPDDR2_FREQ_CODE_SIZE);
+                        &imx6sx_lpddr2_freq_change, ddr_code_size);
 
         curr_ddr_rate = ddr_normal_rate;
 
diff --git a/arch/arm/mach-imx/common.h b/arch/arm/mach-imx/common.h
index b0872d986b7a..6f8bac300ca6 100644
--- a/arch/arm/mach-imx/common.h
+++ b/arch/arm/mach-imx/common.h
@@ -131,6 +131,7 @@ static inline void imx_smp_prepare(void) {}
 static inline void imx_scu_standby_enable(void) {}
 #endif
 extern void imx6_pm_map_io(void);
+extern void imx6_busfreq_map_io(void);
 extern void imx6_suspend(void);
 extern void imx_src_init(void);
 #ifdef CONFIG_HAVE_IMX_SRC
diff --git a/arch/arm/mach-imx/cpuidle-imx6sl.c b/arch/arm/mach-imx/cpuidle-imx6sl.c
index cd4e1fda4e35..412b86aacb86 100644
--- a/arch/arm/mach-imx/cpuidle-imx6sl.c
+++ b/arch/arm/mach-imx/cpuidle-imx6sl.c
@@ -33,6 +33,9 @@ extern void imx6sl_low_power_wfi(void);
 extern unsigned long save_ttbr1(void);
 extern void restore_ttbr1(unsigned long ttbr1);
 extern unsigned long iram_tlb_phys_addr;
+extern unsigned long total_suspend_size;
+extern unsigned long mx6sl_lpm_wfi_start asm("mx6sl_lpm_wfi_start");
+extern unsigned long mx6sl_lpm_wfi_end asm("mx6sl_lpm_wfi_end");
 
 static void __iomem *iomux_base;
 static void *wfi_iram_base;
@@ -99,6 +102,7 @@ static struct cpuidle_driver imx6sl_cpuidle_driver = {
 int __init imx6sl_cpuidle_init(void)
 {
         struct device_node *node;
+        u32 wfi_code_size;
 
         node = of_find_compatible_node(NULL, NULL, "fsl,imx6sl-iomuxc");
         if (!node) {
@@ -112,12 +116,14 @@ int __init imx6sl_cpuidle_init(void)
         if (IS_ERR(vbus_ldo))
                 vbus_ldo = NULL;
 
-        /* Calculate the virtual address of the code */
+        wfi_code_size = (&mx6sl_lpm_wfi_end -&mx6sl_lpm_wfi_start) *4;
+
+        /* Get the virtual address of the wfi iram code. */
         wfi_iram_base = (void *)IMX_IO_P2V(iram_tlb_phys_addr) +
-                                MX6SL_WFI_IRAM_ADDR_OFFSET;
+                        total_suspend_size;
 
         imx6sl_wfi_in_iram_fn = (void *)fncpy(wfi_iram_base,
-                &imx6sl_low_power_wfi, MX6SL_WFI_IRAM_CODE_SIZE);
+                &imx6sl_low_power_wfi, wfi_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 26a601805b8a..25406d45ccb8 100644
--- a/arch/arm/mach-imx/ddr3_freq_imx6.S
+++ b/arch/arm/mach-imx/ddr3_freq_imx6.S
@@ -43,6 +43,8 @@
 #define L2_CACHE_SYNC           0x730
 
 .extern iram_tlb_phys_addr
+.globl mx6_ddr3_freq_change_start
+.globl mx6_ddr3_freq_change_end
 
         .align 3
 
@@ -331,6 +333,7 @@ wait_div_update:
  */
 ENTRY(mx6_ddr3_freq_change)
 
+mx6_ddr3_freq_change_start:
         stmfd   sp!, {r4-r12}
 
         /*
@@ -344,7 +347,6 @@ ENTRY(mx6_ddr3_freq_change)
         mov     r9, r2
         mov     r11, r3
 
-ddr_freq_change:
         /*
          * To ensure no page table walks occur in DDR, we
          * have a another page table stored in IRAM that only
@@ -361,15 +363,15 @@ ddr_freq_change:
         ldr     r6, =iram_tlb_phys_addr
         ldr     r7, [r6]
 
-        /* Flush the Branch Target Address Cache (BTAC) */
-        ldr     r6, =0x0
-        mcr     p15, 0, r6, c7, c1, 6
-
         /* 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
 
@@ -402,6 +404,13 @@ ddr_freq_change:
          * Sync operation on L2 drains the buffers.
          */
         ldr     r12, =IMX_IO_P2V(MX6Q_L2_BASE_ADDR)
+
+        /* Wait for background operations to complete. */
+wait_for_l2_to_idle:
+        ldr     r1, [r12, #L2_CACHE_SYNC]
+        cmp     r1, #0x0
+        bne     wait_for_l2_to_idle
+
         mov     r1, #0x0
         str     r1, [r12, #L2_CACHE_SYNC]
 
@@ -955,7 +964,10 @@ done:
         ldmfd   sp!, {r4-r12}
         mov     pc, lr
 
-        .type   mx6_do_ddr3_freq_change, #object
-ENTRY(mx6_do_ddr_freq_change)
-        .word   mx6_ddr3_freq_change
-        .size   mx6_ddr3_freq_change, . - mx6_ddr3_freq_change
+        /*
+         * Add ltorg here to ensure that all
+         * literals are stored here and are
+         * within the text space.
+         */
+        .ltorg
+mx6_ddr3_freq_change_end:
diff --git a/arch/arm/mach-imx/ddr3_freq_imx6sx.S b/arch/arm/mach-imx/ddr3_freq_imx6sx.S
index 152740fd3ffd..c63bf570100d 100644
--- a/arch/arm/mach-imx/ddr3_freq_imx6sx.S
+++ b/arch/arm/mach-imx/ddr3_freq_imx6sx.S
@@ -19,6 +19,9 @@
 #include <linux/linkage.h>
 #include "hardware.h"
 
+.globl imx6sx_ddr3_freq_change_start
+.globl imx6sx_ddr3_freq_change_end
+
 #define MMDC0_MDPDC             0x4
 #define MMDC0_MDCF0             0xc
 #define MMDC0_MDCF1             0x10
@@ -204,13 +207,13 @@ skip_periph2_clk2_switch_50m:
  */
 ENTRY(imx6sx_ddr3_freq_change)
 
+imx6sx_ddr3_freq_change_start:
         stmfd   sp!, {r4 - r11}
 
         ldr     r1, [r0, #BUSFREQ_INFO_DDR_SETTINGS_OFFSET]
         ldr     r2, [r0, #BUSFREQ_INFO_DLL_OFF_OFFSET]
         ldr     r3, [r0, #BUSFREQ_INFO_IOMUX_OFFSETS_OFFSET]
 
-ddr_freq_change:
         /*
          * To ensure no page table walks occur in DDR, we
          * have a another page table stored in IRAM that only
@@ -224,17 +227,17 @@ ddr_freq_change:
          *    and 2-4G is translated by TTBR1.
          */
 
+        /* Disable Branch Prediction, Z bit in SCTLR. */
+        mrc     p15, 0, r7, c1, c0, 0
+        bic     r7, r7, #0x800
+        mcr     p15, 0, r7, c1, c0, 0
+
         /* 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]
 
-        /* Disable Branch Prediction, Z bit in SCTLR. */
-        mrc     p15, 0, r7, c1, c0, 0
-        bic     r7, r7, #0x800
-        mcr     p15, 0, r7, c1, c0, 0
-
         dsb
         isb
 
@@ -270,12 +273,15 @@ ddr_freq_change:
          * sync operation on L2 drains the buffers.
          */
         ldr     r8, =IMX_IO_P2V(MX6Q_L2_BASE_ADDR)
+
+        /* Wait for background operations to complete. */
+wait_for_l2_to_idle:
+        ldr     r7, [r8, #0x730]
+        cmp     r7, #0x0
+        bne     wait_for_l2_to_idle
+
         mov     r7, #0x0
         str     r7, [r8, #L2_CACHE_SYNC]
-l2:
-        ldr     r7, [r8, #L2_CACHE_SYNC]
-        ands    r7, r7, #0x1
-        bne     l2
 
         /* Disable L2. */
         mov     r7, #0x0
@@ -719,4 +725,11 @@ done:
         ldmfd   sp!, {r4 - r11}
         mov     pc, lr
 
+        /*
+         * Add ltorg here to ensure that all
+         * literals are stored here and are
+         * within the text space.
+         */
+        .ltorg
+imx6sx_ddr3_freq_change_end:
 ENDPROC(imx6sx_ddr3_freq_change)
diff --git a/arch/arm/mach-imx/imx6sl_wfi.S b/arch/arm/mach-imx/imx6sl_wfi.S
index e4ce82faaa8e..4bd0a86f11b4 100644
--- a/arch/arm/mach-imx/imx6sl_wfi.S
+++ b/arch/arm/mach-imx/imx6sl_wfi.S
@@ -20,6 +20,8 @@
 #include "hardware.h"
 
 .extern iram_tlb_phys_addr
+.globl mx6sl_lpm_wfi_start
+.globl mx6sl_lpm_wfi_end
 
         .macro  sl_ddr_io_save
 
@@ -161,9 +163,10 @@ fifo_reset2_wait:
         .align 3
 ENTRY(imx6sl_low_power_wfi)
 
+mx6sl_lpm_wfi_start:
+
         push {r4-r12}
 
-mx6sl_lpm_wfi:
         /* Can LDO2p5 be disabled */
         mov     r12, r1
         /* Store audio_bus_freq_mode */
@@ -172,7 +175,8 @@ mx6sl_lpm_wfi:
         /* Get the IRAM data storage address. */
         mov     r10, r0
         mov     r9, r0          /* get wfi_iram_base */
-        add     r9, r9, #MX6SL_WFI_IRAM_CODE_SIZE
+        adrl    r4, mx6sl_lpm_wfi_end
+        add     r9, r4, #MX6SL_WFI_IRAM_DATA_SIZE
 
         /*
          * To ensure no page table walks occur in DDR, we
@@ -190,15 +194,15 @@ mx6sl_lpm_wfi:
         ldr     r6, =iram_tlb_phys_addr
         ldr     r7, [r6]
 
-        /* Flush the BTAC. */
-        ldr     r6, =0x0
-        mcr     p15, 0, r6, c7, c1, 6
-
         /* 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
+
         dsb
         isb
 
@@ -231,6 +235,13 @@ mx6sl_lpm_wfi:
          * Performing a sync operation does this.
          */
         ldr     r7, =IMX_IO_P2V(MX6Q_L2_BASE_ADDR)
+
+        /* Wait for background operations to complete. */
+wait_for_l2_to_idle:
+        ldr     r6, [r7, #0x730]
+        cmp     r6, #0x0
+        bne     wait_for_l2_to_idle
+
         mov     r6, #0x0
         str     r6, [r7, #0x730]
 
@@ -609,8 +620,8 @@ audio_arm_clk_restore:
 
 wfi_restore:
         /* get wfi_iram_base */
-        mov     r9, r10
-        add     r9, r9, #MX6SL_WFI_IRAM_CODE_SIZE
+        adrl    r9, mx6sl_lpm_wfi_end
+        add     r9, r9, #MX6SL_WFI_IRAM_DATA_SIZE
 
         /* Restore the DDR IO before exiting self-refresh. */
         sl_ddr_io_restore
@@ -732,3 +743,10 @@ poll_dvfs_clear_1:
 
         /* Restore registers */
         mov     pc, lr
+        /*
+         * Add ltorg here to ensure that all
+         * literals are stored here and are
+         * within the text space.
+         */
+        .ltorg
+mx6sl_lpm_wfi_end:
diff --git a/arch/arm/mach-imx/lpddr2_freq_imx6.S b/arch/arm/mach-imx/lpddr2_freq_imx6.S
index 121a88bb62e8..8ed95fe57c1d 100644
--- a/arch/arm/mach-imx/lpddr2_freq_imx6.S
+++ b/arch/arm/mach-imx/lpddr2_freq_imx6.S
@@ -19,6 +19,9 @@
 #include <linux/linkage.h>
 #include "hardware.h"
 
+.globl imx6_lpddr2_freq_change_start
+.globl imx6_lpddr2_freq_change_end
+
         .macro    mx6sl_switch_to_24MHz
 
         /*
@@ -343,7 +346,7 @@ force_measure1:
  */
         .align 3
 ENTRY(mx6_lpddr2_freq_change)
-
+imx6_lpddr2_freq_change_start:
         push {r4-r10}
 
         /*
@@ -362,16 +365,15 @@ ENTRY(mx6_lpddr2_freq_change)
         ldr     r6, =iram_tlb_phys_addr
         ldr     r7, [r6]
 
-
-        /* Flush the Branch Target Address Cache (BTAC) */
-        ldr     r6, =0x0
-        mcr     p15, 0, r6, c7, c1, 6
-
         /* 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 */
@@ -404,6 +406,13 @@ ENTRY(mx6_lpddr2_freq_change)
          * Performing a sync operation does this.
          */
         ldr     r7, =IMX_IO_P2V(MX6Q_L2_BASE_ADDR)
+
+        /* Wait for background operations to complete. */
+wait_for_l2_to_idle:
+        ldr     r6, [r7, #0x730]
+        cmp     r6, #0x0
+        bne     wait_for_l2_to_idle
+
         mov     r6, #0x0
         str     r6, [r7, #0x730]
 
@@ -589,7 +598,10 @@ skip_power_down:
         /* Restore registers */
         mov     pc, lr
 
-        .type   mx6_lpddr2_do_iram, #object
-ENTRY(mx6_lpddr2_do_iram)
-        .word   mx6_lpddr2_freq_change
-        .size           mx6_lpddr2_freq_change, . - mx6_lpddr2_freq_change
+        /*
+         * Add ltorg here to ensure that all
+         * literals are stored here and are
+         * within the text space.
+         */
+        .ltorg
+imx6_lpddr2_freq_change_end:
diff --git a/arch/arm/mach-imx/mach-imx6q.c b/arch/arm/mach-imx/mach-imx6q.c
index 4808ebbff9ac..05366ad2e9e8 100644
--- a/arch/arm/mach-imx/mach-imx6q.c
+++ b/arch/arm/mach-imx/mach-imx6q.c
@@ -489,6 +489,7 @@ static void __init imx6q_map_io(void)
         debug_ll_io_init();
         imx_scu_map_io();
         imx6_pm_map_io();
+        imx6_busfreq_map_io();
 }
 
 static void __init imx6q_init_irq(void)
diff --git a/arch/arm/mach-imx/mach-imx6sl.c b/arch/arm/mach-imx/mach-imx6sl.c
index 78090b430dd7..283031d28aa8 100644
--- a/arch/arm/mach-imx/mach-imx6sl.c
+++ b/arch/arm/mach-imx/mach-imx6sl.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2013 Freescale Semiconductor, Inc.
+ * Copyright (C) 2013-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
@@ -117,6 +117,7 @@ static void __init imx6sl_map_io(void)
 {
         debug_ll_io_init();
         imx6_pm_map_io();
+        imx6_busfreq_map_io();
 }
 
 static void __init imx6sl_init_irq(void)
diff --git a/arch/arm/mach-imx/mach-imx6sx.c b/arch/arm/mach-imx/mach-imx6sx.c
index 5d6e50d28b2a..1c9b005cc56f 100644
--- a/arch/arm/mach-imx/mach-imx6sx.c
+++ b/arch/arm/mach-imx/mach-imx6sx.c
@@ -310,6 +310,7 @@ static void __init imx6sx_map_io(void)
 {
         debug_ll_io_init();
         imx6_pm_map_io();
+        imx6_busfreq_map_io();
 }
 
 #define MX6SX_SRC_SMBR_L2_AS_OCRAM_MASK 0x100
diff --git a/arch/arm/mach-imx/mx6.h b/arch/arm/mach-imx/mx6.h
index 477c523745a2..54aa28e47151 100644
--- a/arch/arm/mach-imx/mx6.h
+++ b/arch/arm/mach-imx/mx6.h
@@ -37,19 +37,11 @@
 
 #define MX6SX_IRAM_TLB_BASE_ADDR        0x008f8000
 #define MX6Q_IRAM_TLB_BASE_ADDR 0x00900000
-#define MX6Q_IRAM_TLB_SIZE              0x100000
+#define MX6Q_IRAM_TLB_SIZE              0x4000
 #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        0x700
+#define MX6_SUSPEND_IRAM_DATA_SIZE      256
+#define MX6SL_WFI_IRAM_DATA_SIZE        100
 
 #define MX6_SUSPEND_IRAM_ADDR_OFFSET            0
-#define DDR3_FREQ_CODE_ADDR_OFFSET              (MX6_SUSPEND_IRAM_ADDR_OFFSET + MX6_SUSPEND_IRAM_SIZE)
-#define DDR3_IOMUX_SETTINGS_ADDR_OFFSET (DDR3_FREQ_CODE_ADDR_OFFSET + DDR3_FREQ_CODE_SIZE)
-#define MX6SL_LPDDR2_FREQ_ADDR_OFFSET   (MX6_SUSPEND_IRAM_ADDR_OFFSET + MX6_SUSPEND_IRAM_SIZE)
-#define MX6SL_WFI_IRAM_ADDR_OFFSET              (MX6SL_LPDDR2_FREQ_ADDR_OFFSET + LPDDR2_FREQ_CODE_SIZE)
-
 #endif
diff --git a/arch/arm/mach-imx/pm-imx6.c b/arch/arm/mach-imx/pm-imx6.c
index 674a1537fae4..b03defb21473 100644
--- a/arch/arm/mach-imx/pm-imx6.c
+++ b/arch/arm/mach-imx/pm-imx6.c
@@ -85,6 +85,10 @@ static unsigned int cpu_type;
 static void __iomem *ccm_base;
 struct regmap *romcp;
 
+unsigned long total_suspend_size;
+extern unsigned long imx6_suspend_start asm("imx6_suspend_start");
+extern unsigned long imx6_suspend_end asm("imx6_suspend_end");
+
 unsigned long save_ttbr1(void)
 {
         unsigned long lttbr1;
@@ -415,7 +419,7 @@ void __init imx6_pm_map_io(void)
                 return;
 
         /* Set all entries to 0. */
-        memset((void *)iram_tlb_base_addr, 0, SZ_16K);
+        memset((void *)iram_tlb_base_addr, 0, MX6Q_IRAM_TLB_SIZE);
 
         /*
          * Make sure the IRAM virtual address has a mapping
@@ -470,6 +474,8 @@ void imx6_pm_set_ccm_base(void __iomem *base)
 
 void __init imx6_pm_init(void)
 {
+        unsigned long suspend_code_size;
+
         if (!iram_tlb_base_addr) {
                 pr_warn("No IRAM/OCRAM memory allocated for suspend/resume code. \
 Please ensure device tree has an entry fsl,lpm-sram\n");
@@ -481,8 +487,11 @@ Please ensure device tree has an entry fsl,lpm-sram\n");
         suspend_iram_base = (void *)IMX_IO_P2V(iram_tlb_phys_addr) +
                         MX6_SUSPEND_IRAM_ADDR_OFFSET;
 
+        suspend_code_size = (&imx6_suspend_end -&imx6_suspend_start) *4;
         suspend_in_iram_fn = (void *)fncpy(suspend_iram_base,
-                &imx6_suspend, MX6_SUSPEND_IRAM_SIZE);
+                &imx6_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_IRAM_DATA_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 620181a4886b..da9bd589f5ea 100644
--- a/arch/arm/mach-imx/suspend-imx6.S
+++ b/arch/arm/mach-imx/suspend-imx6.S
@@ -31,6 +31,9 @@
 #define MX6Q_CCM_CCR    0x0
 #define MX6Q_ANATOP_CORE        0x140
 
+.globl imx6_suspend_start
+.globl imx6_suspend_end
+
         .align 3
 
         .macro  imx6sx_ddr_io_save
@@ -557,7 +560,7 @@
         .endm
 
 ENTRY(imx6_suspend)
-
+imx6_suspend_start:
         push {r4-r12}
 
         /*
@@ -590,15 +593,19 @@ ENTRY(imx6_suspend)
          * is for the jump when we finish DDR IO
          * restore.
          */
+        ldr r7,=total_suspend_size
+        ldr r7, [r7]
+        add r10, r0, r7
+
         ldr     r5, [r8, #MX6Q_SRC_GPR1]
-        add     r10, r0, #MX6_SUSPEND_IRAM_SIZE
         stmfd   r10!, {r5}
 
         /* save cpu type */
         stmfd   r10!, {r2}
 
         str     r9, [r8, #MX6Q_SRC_GPR1]
-        add     r3, r1, #MX6_SUSPEND_IRAM_SIZE
+        add     r3, r1, r7
+
         str     r3, [r8, #MX6Q_SRC_GPR2]
 
         ldr     r8, =IMX_IO_P2V(MX6Q_IOMUXC_BASE_ADDR)
@@ -636,15 +643,16 @@ ddr_io_save_dsm_done:
          * 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
-
         /* 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
+
+
         ldr r6, =iram_tlb_phys_addr
         ldr r6, [r6]
         dsb
@@ -820,8 +828,9 @@ ldo_bypass_restore:
         orr     r7, r7, #0x1f
         str     r7, [r8, #MX6Q_ANATOP_CORE]
 ldo_check_done2:
+        adrl    r7, imx6_suspend_end
+        add     r10, r7, #MX6_SUSPEND_IRAM_DATA_SIZE
 
-        add     r10, r0, #MX6_SUSPEND_IRAM_SIZE
         /* skip the lr saved in iram */
         sub     r10, r10, #0x4
         /* skip the cpu type saved in iram */
@@ -1014,3 +1023,11 @@ poll_dvfs_clear_1:
         bic     r7, r7, #0x1
         str     r7, [r8, #MX6Q_MMDC_MAPSR]
         mov     pc, lr
+
+        /*
+         * Add ltorg here to ensure that all
+         * literals are stored here and are
+         * within the text space.
+         */
+        .ltorg
+imx6_suspend_end: