ENGR00281017 [MX6SL]Low power IDLE mode optimizations

Added a new bus freq mode - ultra_low_bus_freq_mode.
In this mode the ARM is the only bus master that is active and
the system is already in low power idle mode.
And when ARM executes WFI in this mode, we do some aggressive
power savings techinques like:
1. Drop DDR freq to 1MHz
2. Drop AHB freq to 3MHz
3. Float the DDR IO pads
4. If all PLLs are in bypass (which should be the case), do
some analog power saving options like reducing the OSC-bias current,
turning off the regular bandgap, disabling the regular 2P5, enabling
the weak 2p5 etc.

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

8 files changed, 800 insertions, 51 deletions

diff --git a/arch/arm/boot/dts/imx6sl.dtsi b/arch/arm/boot/dts/imx6sl.dtsi
index c64207a6fa58..f03f2e1a92c6 100644
--- a/arch/arm/boot/dts/imx6sl.dtsi
+++ b/arch/arm/boot/dts/imx6sl.dtsi
@@ -104,10 +104,11 @@
                                         <&clks IMX6SL_CLK_PLL1_SYS>, <&clks IMX6SL_CLK_PERIPH2>,
                                         <&clks IMX6SL_CLK_AHB>, <&clks IMX6SL_CLK_OCRAM>,
                                         <&clks IMX6SL_CLK_PLL1_SW>, <&clks IMX6SL_CLK_PRE_PERIPH2_SEL>,
-                                        <&clks IMX6SL_CLK_PERIPH2_CLK2_SEL>, <&clks IMX6SL_CLK_PERIPH2_CLK2>;
+                                        <&clks IMX6SL_CLK_PERIPH2_CLK2_SEL>, <&clks IMX6SL_CLK_PERIPH2_CLK2>,
+                                        <&clks IMX6SL_CLK_STEP>;
                         clock-names = "pll2_bus", "pll2_pfd2_396m", "pll2_198m", "arm", "pll3_usb_otg", "periph",
                                 "periph_pre", "periph_clk2", "periph_clk2_sel", "osc", "pll1_sys", "periph2", "ahb", "ocram", "pll1_sw",
-                                "periph2_pre", "periph2_clk2_sel", "periph2_clk2";
+                                "periph2_pre", "periph2_clk2_sel", "periph2_clk2", "step";
                         fsl,max_ddr_freq = <400000000>;
                 };
 
diff --git a/arch/arm/mach-imx/Makefile b/arch/arm/mach-imx/Makefile
index 50887de73cde..87a323b0f0f8 100644
--- a/arch/arm/mach-imx/Makefile
+++ b/arch/arm/mach-imx/Makefile
@@ -108,7 +108,7 @@ obj-$(CONFIG_PM) += pm-imx6.o headsmp.o suspend-imx6.o
 ifeq ($(CONFIG_ARM_IMX6_CPUFREQ),y)
 obj-y += busfreq-imx6.o
 obj-$(CONFIG_SOC_IMX6Q) += ddr3_freq_imx6.o busfreq_ddr3.o
-obj-$(CONFIG_SOC_IMX6SL) += lpddr2_freq_imx6.o busfreq_lpddr2.o
+obj-$(CONFIG_SOC_IMX6SL) += lpddr2_freq_imx6.o busfreq_lpddr2.o imx6sl_wfi.o
 endif
 
 
diff --git a/arch/arm/mach-imx/busfreq-imx6.c b/arch/arm/mach-imx/busfreq-imx6.c
index 58ecef05e95a..a32b1d25fbc1 100644
--- a/arch/arm/mach-imx/busfreq-imx6.c
+++ b/arch/arm/mach-imx/busfreq-imx6.c
@@ -54,6 +54,7 @@ int high_bus_freq_mode;
 int med_bus_freq_mode;
 int audio_bus_freq_mode;
 int low_bus_freq_mode;
+int ultra_low_bus_freq_mode;
 unsigned int ddr_med_rate;
 unsigned int ddr_normal_rate;
 
@@ -62,7 +63,7 @@ static struct device *busfreq_dev;
 static int busfreq_suspended;
 static u32 org_arm_rate;
 static int bus_freq_scaling_is_active;
-static int high_bus_count, med_bus_count, audio_bus_count;
+static int high_bus_count, med_bus_count, audio_bus_count, low_bus_count;
 static unsigned int ddr_low_rate;
 
 extern int init_mmdc_lpddr2_settings(struct platform_device *dev);
@@ -91,6 +92,7 @@ static struct clk *pll1_sw_clk;
 static struct clk *periph2_pre_clk;
 static struct clk *periph2_clk2_sel;
 static struct clk *periph2_clk2;
+static struct clk *step_clk;
 
 static u32 pll2_org_rate;
 static struct delayed_work low_bus_freq_handler;
@@ -120,7 +122,7 @@ static void enter_lpm_imx6sl(void)
                 clk_set_parent(periph2_pre_clk, pll2_200);
                 clk_set_parent(periph2_clk, periph2_pre_clk);
 
-                if (low_bus_freq_mode) {
+                if (low_bus_freq_mode || ultra_low_bus_freq_mode) {
                         /*
                          * Swtich ARM to run off PLL2_PFD2_400MHz
                          * since DDR is anyway at 100MHz.
@@ -133,46 +135,73 @@ static void enter_lpm_imx6sl(void)
                         clk_set_rate(cpu_clk, org_arm_rate);
                 }
                 low_bus_freq_mode = 0;
+                ultra_low_bus_freq_mode = 0;
                 audio_bus_freq_mode = 1;
         } else {
                 u32 arm_div, pll1_rate;
                 org_arm_rate = clk_get_rate(cpu_clk);
-                /*
-                 * Set DDR to 24MHz.
-                 * Since we are going to bypass PLL2, we need
-                 * to move ARM clk off PLL2_PFD2 to PLL1.
-                 * Make sure the PLL1 is running at the lowest possible freq.
-                 */
-                clk_set_rate(pll1_sys, clk_round_rate(pll1_sys, org_arm_rate));
-                pll1_rate = clk_get_rate(pll1_sys);
-                arm_div = pll1_rate / org_arm_rate + 1;
-                /* Ensure ARM CLK is lower before changing the parent. */
-                clk_set_rate(cpu_clk, org_arm_rate / arm_div);
-                /* Now set the ARM clk parent to PLL1_SYS. */
-                clk_set_parent(pll1_sw_clk, pll1_sys);
-
-                /* Now set DDR to 24MHz. */
-                spin_lock_irqsave(&freq_lock, flags);
-                update_lpddr2_freq(LPAPM_CLK);
-                spin_unlock_irqrestore(&freq_lock, flags);
-
-                /*
-                 * Fix the clock tree in kernel.
-                 * Make sure PLL2 rate is updated as it gets
-                 * bypassed in the DDR freq change code.
-                 */
-                clk_set_rate(pll2, LPAPM_CLK);
-                clk_set_parent(periph2_clk2_sel, pll2);
-                clk_set_parent(periph2_clk, periph2_clk2_sel);
-
-                low_bus_freq_mode = 1;
-                audio_bus_freq_mode = 0;
+                if (low_bus_freq_mode && low_bus_count == 0) {
+                        /*
+                         * We are already in DDR @ 24MHz state, but
+                         * no one but ARM needs the DDR. In this case,
+                         * we can lower the DDR freq to 1MHz when ARM
+                         * enters WFI in this state. Keep track of this state.
+                         */
+                        ultra_low_bus_freq_mode = 1;
+                        low_bus_freq_mode = 0;
+                        audio_bus_freq_mode = 0;
+                } else {
+                        if (!ultra_low_bus_freq_mode && !low_bus_freq_mode) {
+                                /*
+                                 * Set DDR to 24MHz.
+                                 * Since we are going to bypass PLL2,
+                                 * we need to move ARM clk off PLL2_PFD2
+                                 * to PLL1. Make sure the PLL1 is running
+                                 * at the lowest possible freq.
+                                 */
+                                clk_set_rate(pll1_sys,
+                                        clk_round_rate(pll1_sys, org_arm_rate));
+                                pll1_rate = clk_get_rate(pll1_sys);
+                                arm_div = pll1_rate / org_arm_rate + 1;
+                                /*
+                                 * Ensure ARM CLK is lower before
+                                 * changing the parent.
+                                 */
+                                clk_set_rate(cpu_clk, org_arm_rate / arm_div);
+                                /* Now set the ARM clk parent to PLL1_SYS. */
+                                clk_set_parent(pll1_sw_clk, pll1_sys);
+
+                                /* Now set DDR to 24MHz. */
+                                spin_lock_irqsave(&freq_lock, flags);
+                                update_lpddr2_freq(LPAPM_CLK);
+                                spin_unlock_irqrestore(&freq_lock, flags);
+
+                                /*
+                                 * Fix the clock tree in kernel.
+                                 * Make sure PLL2 rate is updated as it gets
+                                 * bypassed in the DDR freq change code.
+                                 */
+                                clk_set_rate(pll2, LPAPM_CLK);
+                                clk_set_parent(periph2_clk2_sel, pll2);
+                                clk_set_parent(periph2_clk, periph2_clk2_sel);
+
+                        }
+                        if (low_bus_count == 0) {
+                                ultra_low_bus_freq_mode = 1;
+                                low_bus_freq_mode = 0;
+                        } else {
+                                ultra_low_bus_freq_mode = 0;
+                                low_bus_freq_mode = 1;
+                        }
+                        audio_bus_freq_mode = 0;
+                }
         }
 }
 
 static void exit_lpm_imx6sl(void)
 {
         unsigned long flags;
+
         spin_lock_irqsave(&freq_lock, flags);
         /* Change DDR freq in IRAM. */
         update_lpddr2_freq(ddr_normal_rate);
@@ -197,10 +226,12 @@ static void exit_lpm_imx6sl(void)
         clk_set_rate(ocram_clk, LPAPM_CLK / 2);
         clk_set_parent(periph_clk, periph_pre_clk);
 
-        if (low_bus_freq_mode) {
+        if (low_bus_freq_mode || ultra_low_bus_freq_mode) {
                 /* Move ARM from PLL1_SW_CLK to PLL2_400. */
-                clk_set_parent(pll1_sw_clk, pll2_400);
+                clk_set_parent(step_clk, pll2_400);
+                clk_set_parent(pll1_sw_clk, step_clk);
                 clk_set_rate(cpu_clk, org_arm_rate);
+                ultra_low_bus_freq_mode = 0;
         }
 }
 
@@ -287,7 +318,7 @@ static void reduce_bus_freq_handler(struct work_struct *work)
  * This mode will be activated only when none of the modules that
  * need a higher DDR or AHB frequency are active.
  */
-int set_low_bus_freq(int low_bus_mode)
+int set_low_bus_freq(void)
 {
         if (busfreq_suspended)
                 return 0;
@@ -417,6 +448,8 @@ void request_bus_freq(enum bus_freq_mode mode)
                 med_bus_count++;
         else if (mode == BUS_FREQ_AUDIO)
                 audio_bus_count++;
+        else if (mode == BUS_FREQ_LOW)
+                low_bus_count++;
 
         if (busfreq_suspended || !bus_freq_scaling_initialized ||
                 !bus_freq_scaling_is_active) {
@@ -448,7 +481,7 @@ void request_bus_freq(enum bus_freq_mode mode)
         }
         if ((mode == BUS_FREQ_AUDIO) && (!high_bus_freq_mode) &&
                 (!med_bus_freq_mode) && (!audio_bus_freq_mode)) {
-                set_low_bus_freq(1);
+                set_low_bus_freq();
                 mutex_unlock(&bus_freq_mutex);
                 return;
         }
@@ -485,6 +518,14 @@ void release_bus_freq(enum bus_freq_mode mode)
                         return;
                 }
                 audio_bus_count--;
+        } else if (mode == BUS_FREQ_LOW) {
+                if (low_bus_count == 0) {
+                        dev_err(busfreq_dev, "low bus count mismatch!\n");
+                        dump_stack();
+                        mutex_unlock(&bus_freq_mutex);
+                        return;
+                }
+                low_bus_count--;
         }
 
         if (busfreq_suspended || !bus_freq_scaling_initialized ||
@@ -503,13 +544,24 @@ void release_bus_freq(enum bus_freq_mode mode)
 
         if ((!audio_bus_freq_mode) && (high_bus_count == 0) &&
                 (med_bus_count == 0) && (audio_bus_count != 0)) {
-                set_low_bus_freq(1);
+                set_low_bus_freq();
                 mutex_unlock(&bus_freq_mutex);
                 return;
         }
         if ((!low_bus_freq_mode) && (high_bus_count == 0) &&
-                (med_bus_count == 0) && (audio_bus_count == 0))
-                set_low_bus_freq(0);
+                (med_bus_count == 0) && (audio_bus_count == 0) &&
+                (low_bus_count != 0)) {
+                set_low_bus_freq();
+                mutex_unlock(&bus_freq_mutex);
+                return;
+        }
+        if ((!ultra_low_bus_freq_mode) && (high_bus_count == 0) &&
+                (med_bus_count == 0) && (audio_bus_count == 0) &&
+                (low_bus_count == 0)) {
+                set_low_bus_freq();
+                mutex_unlock(&bus_freq_mutex);
+                return;
+        }
 
         mutex_unlock(&bus_freq_mutex);
         return;
@@ -521,7 +573,7 @@ static void bus_freq_daemon_handler(struct work_struct *work)
         mutex_lock(&bus_freq_mutex);
         if ((!low_bus_freq_mode) && (high_bus_count == 0) &&
                 (med_bus_count == 0) && (audio_bus_count == 0))
-                set_low_bus_freq(0);
+                set_low_bus_freq();
         mutex_unlock(&bus_freq_mutex);
 }
 
@@ -744,6 +796,14 @@ static int busfreq_probe(struct platform_device *pdev)
                         return PTR_ERR(periph2_clk2_sel);
                 }
 
+                step_clk = devm_clk_get(&pdev->dev, "step");
+                if (IS_ERR(step_clk)) {
+                        dev_err(busfreq_dev,
+                                "%s: failed to get step_clk\n",
+                                __func__);
+                        return PTR_ERR(periph2_clk2_sel);
+                }
+
         }
 
         err = sysfs_create_file(&busfreq_dev->kobj, &dev_attr_enable.attr);
@@ -763,6 +823,7 @@ static int busfreq_probe(struct platform_device *pdev)
         med_bus_freq_mode = 0;
         low_bus_freq_mode = 0;
         audio_bus_freq_mode = 0;
+        ultra_low_bus_freq_mode = 0;
 
         bus_freq_scaling_is_active = 1;
         bus_freq_scaling_initialized = 1;
diff --git a/arch/arm/mach-imx/busfreq_lpddr2.c b/arch/arm/mach-imx/busfreq_lpddr2.c
index f15b8290669c..900d7bdf469f 100644
--- a/arch/arm/mach-imx/busfreq_lpddr2.c
+++ b/arch/arm/mach-imx/busfreq_lpddr2.c
@@ -50,14 +50,15 @@ static void __iomem *l2_base;
 static struct device *busfreq_dev;
 static void *ddr_freq_change_iram_base;
 static int curr_ddr_rate;
-static unsigned long reg_addrs[4];
+
+unsigned long reg_addrs[4];
 
 void (*mx6_change_lpddr2_freq)(u32 ddr_freq, int bus_freq_mode,
         void *iram_addr) = NULL;
 
 extern unsigned int ddr_normal_rate;
 extern int low_bus_freq_mode;
-extern int audio_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);
 
@@ -71,7 +72,9 @@ int update_lpddr2_freq(int ddr_rate)
         if (ddr_rate == curr_ddr_rate)
                 return 0;
 
-        mx6_change_lpddr2_freq(ddr_rate, low_bus_freq_mode, reg_addrs);
+        mx6_change_lpddr2_freq(ddr_rate,
+                (low_bus_freq_mode | ultra_low_bus_freq_mode),
+                reg_addrs);
 
         curr_ddr_rate = ddr_rate;
 
diff --git a/arch/arm/mach-imx/clk-imx6sl.c b/arch/arm/mach-imx/clk-imx6sl.c
index 36a5d00c10f2..9ee6a168c705 100644
--- a/arch/arm/mach-imx/clk-imx6sl.c
+++ b/arch/arm/mach-imx/clk-imx6sl.c
@@ -74,6 +74,7 @@ static struct clk_onecell_data clk_data;
 static u32 cur_arm_podf;
 
 extern int low_bus_freq_mode;
+extern int audio_bus_freq_mode;
 
 /*
  * On MX6SL, need to ensure that the ARM:IPG clock ratio is maintained
@@ -103,6 +104,15 @@ void imx6sl_set_wait_clk(bool enter)
                                                         clks[IMX6SL_CLK_OSC]);
                         clk_set_parent(clks[IMX6SL_CLK_PLL1_SW],
                                                         clks[IMX6SL_CLK_STEP]);
+                } else if (audio_bus_freq_mode) {
+                        /*
+                         * In this mode ARM is from PLL2_PFD2 (396MHz),
+                         * but IPG is at 12MHz. Need to switch ARM to run
+                         * from the bypassed PLL1 clocks so that we can run
+                         * ARM at 24MHz.
+                         */
+                        clk_set_parent(clks[IMX6SL_CLK_PLL1_SW],
+                                clks[IMX6SL_CLK_PLL1_SYS]);
                 }
                 new_parent_rate = clk_get_rate(clks[IMX6SL_CLK_PLL1_SW]);
                 wait_podf = (new_parent_rate + max_arm_wait_clk - 1) /
@@ -112,7 +122,12 @@ void imx6sl_set_wait_clk(bool enter)
         } else {
                 if (low_bus_freq_mode)
                         /* Move ARM back to PLL1. */
-                        clk_set_parent(clks[IMX6SL_CLK_PLL1_SW], clks[IMX6SL_CLK_PLL1_SYS]);
+                        clk_set_parent(clks[IMX6SL_CLK_PLL1_SW],
+                                clks[IMX6SL_CLK_PLL1_SYS]);
+                else if (audio_bus_freq_mode)
+                        /* Move ARM back to PLL2_PFD2 via STEP_CLK. */
+                        clk_set_parent(clks[IMX6SL_CLK_PLL1_SW],
+                                clks[IMX6SL_CLK_STEP]);
                 parent_rate = clk_get_rate(clks[IMX6SL_CLK_PLL1_SW]);
                 clk_set_rate(clks[IMX6SL_CLK_ARM], parent_rate / cur_arm_podf);
         }
diff --git a/arch/arm/mach-imx/cpuidle-imx6sl.c b/arch/arm/mach-imx/cpuidle-imx6sl.c
index 7d8ed7c90043..841b65749c5b 100644
--- a/arch/arm/mach-imx/cpuidle-imx6sl.c
+++ b/arch/arm/mach-imx/cpuidle-imx6sl.c
@@ -7,21 +7,48 @@
  */
 
 #include <linux/cpuidle.h>
+#include <linux/genalloc.h>
 #include <linux/module.h>
 #include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_device.h>
 #include <asm/cpuidle.h>
+#include <asm/fncpy.h>
+#include <asm/mach/map.h>
 #include <asm/proc-fns.h>
 
 #include "common.h"
 #include "cpuidle.h"
 
+extern u32 low_bus_freq_mode;
+extern u32 ultra_low_bus_freq_mode;
+extern unsigned long reg_addrs[];
+extern void imx6sl_low_power_wfi(void);
+
+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) = 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);
-        imx6sl_set_wait_clk(true);
-        cpu_do_idle();
-        imx6sl_set_wait_clk(false);
+        if (ultra_low_bus_freq_mode)
+                /*
+                 * 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);
+        else {
+                imx6sl_set_wait_clk(true);
+                cpu_do_idle();
+                imx6sl_set_wait_clk(false);
+        }
         imx6_set_lpm(WAIT_CLOCKED);
 
         return index;
@@ -50,5 +77,63 @@ 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) {
+                pr_err("failed to find imx6sl-iomuxc device tree data!\n");
+                return -EINVAL;
+        }
+        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");
+
+        imx6sl_wfi_in_iram_fn = (void *)fncpy(wfi_iram_base,
+                &imx6sl_low_power_wfi, WFI_IN_IRAM_SIZE);
+
         return cpuidle_register(&imx6sl_cpuidle_driver, NULL);
 }
diff --git a/arch/arm/mach-imx/imx6sl_wfi.S b/arch/arm/mach-imx/imx6sl_wfi.S
new file mode 100644
index 000000000000..2fe8bae1d968
--- /dev/null
+++ b/arch/arm/mach-imx/imx6sl_wfi.S
@@ -0,0 +1,584 @@
+/*
+ * Copyright (C) 2012-2013 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
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+
+#include <linux/linkage.h>
+#define IRAM_WAIT_SIZE  (1 << 11)
+
+        .macro  sl_ddr_io_save
+
+        ldr     r4, [r1, #0x30c] /* DRAM_DQM0 */
+        ldr     r5, [r1, #0x310] /* DRAM_DQM1 */
+        ldr     r6, [r1, #0x314] /* DRAM_DQM2 */
+        ldr     r7, [r1, #0x318] /* DRAM_DQM3 */
+        stmfd   r9!, {r4-r7}
+
+        ldr     r4, [r1, #0x5c4] /* GPR_B0DS */
+        ldr     r5, [r1, #0x5cc] /* GPR_B1DS */
+        ldr     r6, [r1, #0x5d4] /* GPR_B2DS */
+        ldr     r7, [r1, #0x5d8] /* GPR_B3DS */
+        stmfd   r9!, {r4-r7}
+
+        ldr     r4, [r1, #0x300] /* DRAM_CAS */
+        ldr     r5, [r1, #0x31c] /* DRAM_RAS */
+        ldr     r6, [r1, #0x338] /* DRAM_SDCLK_0 */
+        ldr r7, [r1, #0x5ac] /* GPR_ADDS*/
+        stmfd   r9!, {r4-r7}
+
+        ldr     r4, [r1, #0x5b0] /* DDRMODE_CTL */
+        ldr     r5, [r1, #0x5c0] /* DDRMODE */
+        ldr     r6, [r1, #0x33c] /* DRAM_SODT0*/
+        ldr     r7, [r1, #0x340] /* DRAM_SODT1*/
+        stmfd   r9!, {r4-r7}
+
+        ldr     r4, [r1, #0x330] /* DRAM_SDCKE0 */
+        ldr     r5, [r1, #0x334] /* DRAM_SDCKE1 */
+        ldr     r6, [r1, #0x320] /* DRAM_RESET */
+        stmfd   r9!, {r4-r6}
+
+        .endm
+
+        .macro  sl_ddr_io_restore
+
+        /*
+         * r9 points to IRAM stack.
+         * r1 points to IOMUX base address.
+         * r8 points to MMDC  base address.
+         */
+        ldmea   r9!, {r4-r7}
+        str     r4, [r1, #0x30c] /* DRAM_DQM0 */
+        str     r5, [r1, #0x310] /* DRAM_DQM1 */
+        str     r6, [r1, #0x314] /* DRAM_DQM2 */
+        str     r7, [r1, #0x318] /* DRAM_DQM3 */
+
+        ldmea   r9!, {r4-r7}
+        str     r4, [r1, #0x5c4] /* GPR_B0DS */
+        str     r5, [r1, #0x5cc] /* GPR_B1DS */
+        str r6, [r1, #0x5d4] /* GPR_B2DS */
+        str r7, [r1, #0x5d8] /* GPR_B3DS */
+
+        ldmea   r9!, {r4-r7}
+        str     r4, [r1, #0x300] /* DRAM_CAS */
+        str     r5, [r1, #0x31c] /* DRAM_RAS */
+        str     r6, [r1, #0x338] /* DRAM_SDCLK_0 */
+        str r7, [r1, #0x5ac] /* GPR_ADDS*/
+
+        ldmea   r9!, {r4-r7}
+        str     r4, [r1, #0x5b0] /* DDRMODE_CTL */
+        str     r5, [r1, #0x5c0] /* DDRMODE */
+        str     r6, [r1, #0x33c] /* DRAM_SODT0*/
+        str     r7, [r1, #0x340] /* DRAM_SODT1*/
+
+        ldmea   r9!, {r4-r6}
+        str     r4, [r1, #0x330] /* DRAM_SDCKE0 */
+        str     r5, [r1, #0x334] /* DRAM_SDCKE1 */
+        str     r6, [r1, #0x320] /* DRAM_RESET */
+
+        /*
+         * Need to reset the FIFO to avoid MMDC lockup
+         * caused because of floating/changing the
+         * configuration of many DDR IO pads.
+         */
+        ldr     r7, =0x83c
+        ldr     r6, [r8, r7]
+        orr     r6, r6, #0x80000000
+        str     r6, [r8, r7]
+fifo_reset1_wait:
+        ldr     r6, [r8, r7]
+        and     r6, r6, #0x80000000
+        cmp     r6, #0
+        bne     fifo_reset1_wait
+
+        /* reset FIFO a second time */
+        ldr     r6, [r8, r7]
+        orr     r6, r6, #0x80000000
+        str     r6, [r8, r7]
+fifo_reset2_wait:
+        ldr     r6, [r8, r7]
+        and     r6, r6, #0x80000000
+        cmp     r6, #0
+        bne     fifo_reset2_wait
+
+        .endm
+
+        .macro  sl_ddr_io_set_lpm
+
+        mov     r4, #0
+        str     r4, [r1, #0x30c] /* DRAM_DQM0 */
+        str     r4, [r1, #0x310] /* DRAM_DQM1 */
+        str     r4, [r1, #0x314] /* DRAM_DQM2 */
+        str     r4, [r1, #0x318] /* DRAM_DQM3 */
+
+        str     r4, [r1, #0x5c4] /* GPR_B0DS */
+        str     r4, [r1, #0x5cc] /* GPR_B1DS */
+        str r4, [r1, #0x5d4] /* GPR_B2DS */
+        str r4, [r1, #0x5d8] /* GPR_B3DS */
+
+        str     r4, [r1, #0x300] /* DRAM_CAS */
+        str     r4, [r1, #0x31c] /* DRAM_RAS */
+        str     r4, [r1, #0x338] /* DRAM_SDCLK_0 */
+        str r4, [r1, #0x5ac] /* GPR_ADDS*/
+
+        str     r4, [r1, #0x5b0] /* DDRMODE_CTL */
+        str     r4, [r1, #0x5c0] /* DDRMODE */
+        str     r4, [r1, #0x33c] /* DRAM_SODT0*/
+        str     r4, [r1, #0x340] /* DRAM_SODT1*/
+
+        mov     r4, #0x80000
+        str     r4, [r1, #0x320] /* DRAM_RESET */
+        mov     r4, #0x1000
+        str     r4, [r1, #0x330] /* DRAM_SDCKE0 */
+        str     r4, [r1, #0x334] /* DRAM_SDCKE1 */
+
+        .endm
+
+/*
+ *  imx6sl_low_power_wfi
+ *
+ *  Idle the processor (eg, wait for interrupt).
+ *  Make sure DDR is in self-refresh.
+ *  IRQs are already disabled.
+ * r0: WFI IRAMcode base address.
+ * r1: IOMUX base address
+ * r2: Base address of CCM, ANATOP and MMDC
+ */
+        .align 3
+ENTRY(imx6sl_low_power_wfi)
+
+        push {r4-r10}
+
+mx6sl_lpm_wfi:
+        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]
+
+        /* Store the original ARM PODF. */
+        ldr     r0, [r2, #0x10]
+
+        /* Drain all the L1 buffers. */
+        dsb
+
+#ifdef CONFIG_CACHE_L2X0
+        /*
+         * Need to make sure the buffers in L2 are drained.
+         * Performing a sync operation does this.
+         */
+        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
+
+        /* Save the DDR IO state. */
+        sl_ddr_io_save
+
+        /* Disable Automatic power savings. */
+        ldr     r6, [r8, #0x404]
+        orr     r6, r6, #0x01
+        str     r6, [r8, #0x404]
+
+        /* Make the DDR explicitly enter self-refresh. */
+        ldr     r6, [r8, #0x404]
+        orr     r6, r6, #0x200000
+        str     r6, [r8, #0x404]
+
+poll_dvfs_set_1:
+        ldr     r6, [r8, #0x404]
+        and     r6, r6, #0x2000000
+        cmp     r6, #0x2000000
+        bne     poll_dvfs_set_1
+
+        /* set SBS step-by-step mode */
+        ldr     r6, [r8, #0x410]
+        orr     r6, r6, #0x100
+        str     r6, [r8, #0x410]
+
+        /*
+         * Now set DDR rate to 1MHz.
+         * DDR is from bypassed PLL2 on periph2_clk2 path.
+         * Set the periph2_clk2_podf to divide by 8.
+         */
+        ldr     r6, [r2, #0x14]
+        orr     r6, r6, #0x07
+        str     r6, [r2, #0x14]
+
+        /* Now set MMDC PODF to divide by 3. */
+        ldr     r6, [r2, #0x14]
+        bic     r6, r6, #0x38
+        orr     r6, r6, #0x10
+        str     r6, [r2, #0x14]
+
+        /* Loop till podf is accepted. */
+mmdc_podf:
+        ldr     r6, [r2, #0x48]
+        cmp     r6, #0x0
+        bne     mmdc_podf
+
+        /* Set the DDR IO in LPM state. */
+        sl_ddr_io_set_lpm
+
+        /*
+         * Check if none of the PLLs are
+         * locked, except PLL1 which will get
+         * bypassed below.
+         * We should not be here if PLL2 is not
+         * bypassed.
+         */
+        ldr     r7, =1
+        /* USB1 PLL3 */
+        ldr     r6, [r3, #0x10]
+        and     r6, r6, #0x80000000
+        cmp     r6, #0x80000000
+        beq     no_analog_saving
+
+        /* USB2 PLL7 */
+        ldr     r6, [r3, #0x20]
+        and     r6, r6, #0x80000000
+        cmp     r6, #0x80000000
+        beq     no_analog_saving
+
+        /* Audio PLL4 */
+        ldr     r6, [r3, #0x70]
+        and     r6, r6, #0x80000000
+        cmp     r6, #0x80000000
+        beq     no_analog_saving
+
+        /* Video PLL5 */
+        ldr     r6, [r3, #0xA0]
+        and     r6, r6, #0x80000000
+        cmp     r6, #0x80000000
+        beq     no_analog_saving
+
+        /* ENET PLL8 */
+        ldr     r6, [r3, #0xE0]
+        and     r6, r6, #0x80000000
+        cmp     r6, #0x80000000
+        beq     no_analog_saving
+
+        b       cont
+
+no_analog_saving:
+        ldr     r7, =0
+
+cont:
+        /* Set the AHB to 3MHz. AXI to 3MHz. */
+        ldr     r9, [r2, #0x14]
+        mov     r6, r9
+        orr     r6, r6, #0x1c00
+        orr     r6, r6, #0x70000
+        str     r6, [r2, #0x14]
+
+        /* Loop till podf is accepted. */
+ahb_podf:
+        ldr     r6, [r2, #0x48]
+        cmp     r6, #0x0
+        bne     podf_loop
+
+        /*
+         * Now set ARM to 24MHz.
+         * Move ARM to be sourced from STEP_CLK
+         * after setting STEP_CLK to 24MHz.
+         */
+        ldr     r6, [r2, #0xc]
+        bic     r6, r6, #0x100
+        str     r6, [r2, #0x0c]
+        /* Now PLL1_SW_CLK to step_clk. */
+        ldr     r6, [r2, #0x0c]
+        orr     r6, r6, #0x4
+        str     r6, [r2, #0x0c]
+
+        /* Bypass PLL1 and power it down. */
+        ldr     r6, =(1 << 16)
+        orr     r6, r6, #0x1000
+        str     r6, [r3, #0x04]
+
+        /*
+         * Set the ARM PODF to divide by 8.
+         * IPG is at 1.5MHz here, we need ARM to
+         * run at the 12:5 ratio (WAIT mode issue).
+         */
+        ldr     r6, =0x7
+        str     r6, [r2, #0x10]
+
+        /* Loop till podf is accepted. */
+podf_loop:
+        ldr     r6, [r2, #0x48]
+        cmp     r6, #0x0
+        bne   podf_loop
+
+        /*
+         * Check if we can save some
+         * power in the Analog section.
+         */
+        cmp     r7, #0x1
+        bne     do_wfi
+
+        /* Disable 1p1 brown out. */
+        ldr     r6, [r3, #0x110]
+        bic     r6, r6, #0x2
+        str     r6, [r3, #0x110]
+
+        /* Enable the weak 2P5 */
+        ldr     r6, [r3, #0x130]
+        orr     r6, r6, #0x40000
+        str     r6, [r3, #0x130]
+
+        /* Disable main 2p5. */
+        ldr     r6, [r3, #0x130]
+        bic     r6, r6, #0x1
+        str     r6, [r3, #0x130]
+
+        /*
+         * Set the OSC bias current to -37.5%
+         * to drop the power on VDDHIGH.
+         */
+        ldr     r6, [r3, #0x150]
+        orr     r6, r6, #0xC000
+        str     r6, [r3, #0x150]
+
+        /* Enable low power bandgap */
+        ldr     r6, [r3, #0x260]
+        orr     r6, r6, #0x20
+        str     r6, [r3, #0x260]
+
+        /*
+         * Turn off the bias current
+         * from the regular bandgap.
+         */
+        ldr     r6, [r3, #0x260]
+        orr     r6, r6, #0x80
+        str     r6, [r3, #0x260]
+
+        /*
+         * Clear the REFTOP_SELFBIASOFF,
+         * self-bias circuit of the band gap.
+         * Per RM, should be cleared when
+         * band gap is powered down.
+         */
+        ldr     r6, [r3, #0x150]
+        bic     r6, r6, #0x8
+        str     r6, [r3, #0x150]
+
+        /* Power down the regular bandgap. */
+        ldr     r6, [r3, #0x150]
+        orr     r6, r6, #0x1
+        str     r6, [r3, #0x150]
+
+do_wfi:
+        /* Now do WFI. */
+        wfi
+
+        /* Set original ARM PODF back. */
+        str     r0, [r2, #0x10]
+
+        /* Loop till podf is accepted. */
+podf_loop1:
+        ldr     r6, [r2, #0x48]
+        cmp     r6, #0x0
+        bne     podf_loop1
+
+        /*
+         * Check if powered down
+         * analog components.
+         */
+        cmp     r7, #0x1
+        bne     skip_analog_restore
+
+        /* Power up the regular bandgap. */
+        ldr     r6, [r3, #0x150]
+        bic     r6, r6, #0x1
+        str     r6, [r3, #0x150]
+
+        /*
+         * Turn on the bias current
+         * from the regular bandgap.
+         */
+        ldr     r6, [r3, #0x260]
+        bic     r6, r6, #0x80
+        str     r6, [r3, #0x260]
+
+        /* Disable the low power bandgap */
+        ldr     r6, [r3, #0x260]
+        bic     r6, r6, #0x20
+        str     r6, [r3, #0x260]
+
+        /*
+         * Set the OSC bias current to max
+         * value for normal operation.
+         */
+        ldr     r6, [r3, #0x150]
+        bic     r6, r6, #0xC000
+        str     r6, [r3, #0x150]
+
+        /* Enable main 2p5. */
+        ldr     r6, [r3, #0x130]
+        orr     r6, r6, #0x1
+        str     r6, [r3, #0x130]
+
+        /* Ensure the 2P5 is up. */
+loop_2p5:
+        ldr     r6, [r3, #0x130]
+        and     r6, r6, #0x20000
+        cmp     r6, #0x20000
+        bne     loop_2p5
+
+        /* Disable the  weak 2P5 */
+        ldr     r6, [r3, #0x130]
+        bic     r6, r6, #0x40000
+        str     r6, [r3, #0x130]
+
+        /* Enable 1p1 brown out. */
+        ldr     r6, [r3, #0x110]
+        orr     r6, r6, #0x2
+        str     r6, [r3, #0x110]
+
+skip_analog_restore:
+
+        /* Power up PLL1 and un-bypass it. */
+        ldr     r6, =(1 << 12)
+        str     r6, [r3, #0x08]
+
+        /* Wait for PLL1 to relock. */
+wait_for_pll_lock:
+        ldr     r6, [r3, #0x0]
+        and     r6, r6, #0x80000000
+        cmp     r6, #0x80000000
+        bne     wait_for_pll_lock
+
+        ldr     r6, =(1 << 16)
+        str     r6, [r3, #0x08]
+
+        /* Set PLL1_sw_clk back to PLL1. */
+        ldr     r6, [r2, #0x0c]
+        bic     r6, r6, #0x4
+        str     r6, [r2, #0xc]
+
+        /* Restore AHB/AXI back. */
+        str     r9, [r2, #0x14]
+
+        /* Loop till podf is accepted. */
+ahb_podf1:
+        ldr     r6, [r2, #0x48]
+        cmp     r6, #0x0
+        bne     podf_loop1
+
+wfi_restore:
+        /* get suspend_iram_base */
+        mov     r9, r10
+        add     r9, r9, #IRAM_WAIT_SIZE
+
+        /* Restore the DDR IO before exiting self-refresh. */
+        sl_ddr_io_restore
+
+        /*
+         * Set MMDC back to 24MHz.
+         * Set periph2_clk2_podf to divide by 1
+         * Now set MMDC PODF to divide by 1.
+         */
+        ldr     r6, [r2, #0x14]
+        bic     r6, r6, #0x3f
+        str     r6, [r2, #0x14]
+
+mmdc_podf1:
+        ldr     r6, [r2, #0x48]
+        cmp     r6, #0x0
+        bne     mmdc_podf1
+
+        /* clear DVFS - exit from self refresh mode */
+        ldr     r6, [r8, #0x404]
+        bic     r6, r6, #0x200000
+        str     r6, [r8, #0x404]
+
+poll_dvfs_clear_1:
+        ldr     r6, [r8, #0x404]
+        and     r6, r6, #0x2000000
+        cmp     r6, #0x2000000
+        beq     poll_dvfs_clear_1
+
+        /*
+         * Add these nops so that the
+         * prefetcher will not try to get
+         * any instructions from DDR.
+         * The prefetch depth is about 23
+         * on A9, so adding 25 nops.
+         */
+        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
+
+        /* 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-r10}
+
+        /* Restore registers */
+        mov     pc, lr
diff --git a/arch/arm/mach-imx/lpddr2_freq_imx6.S b/arch/arm/mach-imx/lpddr2_freq_imx6.S
index 975582ec2468..a126f110b0d3 100644
--- a/arch/arm/mach-imx/lpddr2_freq_imx6.S
+++ b/arch/arm/mach-imx/lpddr2_freq_imx6.S
@@ -341,7 +341,7 @@ force_measure1:
  * r1: low_bus_freq_mode flag
  * r2: Pointer to array containing addresses of registers.
  */
-        .align 8
+        .align 3
 ENTRY(mx6_lpddr2_freq_change)
 
         push {r4-r10}