ENGR00280101-1 [iMX6SL] Add busfreq support

Add support to scale the DDR frequency between 400MHz and 24MHz.
Add support to scale AHB between 132MHz and 24MHz.

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

7 files changed, 1073 insertions, 143 deletions

diff --git a/arch/arm/mach-imx/Makefile b/arch/arm/mach-imx/Makefile
index 9b80b2328785..50887de73cde 100644
--- a/arch/arm/mach-imx/Makefile
+++ b/arch/arm/mach-imx/Makefile
@@ -108,6 +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
 endif
 
 
diff --git a/arch/arm/mach-imx/busfreq-imx6.c b/arch/arm/mach-imx/busfreq-imx6.c
index c7a7cce30315..53f4ebff52c4 100644
--- a/arch/arm/mach-imx/busfreq-imx6.c
+++ b/arch/arm/mach-imx/busfreq-imx6.c
@@ -41,33 +41,37 @@
 #include <linux/of.h>
 #include <linux/platform_device.h>
 #include <linux/proc_fs.h>
+#include <linux/reboot.h>
 #include <linux/regulator/consumer.h>
 #include <linux/sched.h>
 #include <linux/suspend.h>
 #include "hardware.h"
 
 #define LPAPM_CLK               24000000
-#define DDR_AUDIO_CLK           50000000
+#define DDR_AUDIO_CLK           100000000
 
 int high_bus_freq_mode;
 int med_bus_freq_mode;
 int audio_bus_freq_mode;
 int low_bus_freq_mode;
+unsigned int ddr_med_rate;
+unsigned int ddr_normal_rate;
 
 static int bus_freq_scaling_initialized;
 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 unsigned int ddr_low_rate;
-unsigned int ddr_med_rate;
-unsigned int ddr_normal_rate;
 
-extern int init_mmdc_settings(struct platform_device *dev);
+extern int init_mmdc_lpddr2_settings(struct platform_device *dev);
+extern int init_mmdc_ddr3_settings(struct platform_device *dev);
 extern int update_ddr_freq(int ddr_rate);
+extern int update_lpddr2_freq(int ddr_rate);
 
 DEFINE_MUTEX(bus_freq_mutex);
+static DEFINE_SPINLOCK(freq_lock);
 
 static struct clk *pll2_400;
 static struct clk *periph_clk;
@@ -79,55 +83,181 @@ static struct clk *cpu_clk;
 static struct clk *pll3;
 static struct clk *pll2;
 static struct clk *pll2_200;
-
+static struct clk *pll1_sys;
+static struct clk *periph2_clk;
+static struct clk *ocram_clk;
+static struct clk *ahb_clk;
+static struct clk *pll1_sw_clk;
+static struct clk *periph2_pre_clk;
+static struct clk *periph2_clk2_sel;
+static struct clk *periph2_clk2;
+
+static u32 pll2_org_rate;
 static struct delayed_work low_bus_freq_handler;
 static struct delayed_work bus_freq_daemon;
 
-int low_bus_freq;
-
-int reduce_bus_freq(void)
+static void enter_lpm_imx6sl(void)
 {
-        int ret = 0;
-        clk_prepare_enable(pll3);
-        if (low_bus_freq) {
-                /* Need to ensure that PLL2_PFD_400M is kept ON. */
-                clk_prepare_enable(pll2_400);
-                update_ddr_freq(DDR_AUDIO_CLK);
-                /* Make sure periph clk's parent also got updated */
-                ret = clk_set_parent(periph_clk2_sel, pll3);
-                if (ret)
-                        dev_WARN(busfreq_dev, "%s: %d: clk set parent fail!\n",
-                                __func__, __LINE__);
-                ret = clk_set_parent(periph_pre_clk, pll2_200);
-                if (ret)
-                        dev_WARN(busfreq_dev, "%s: %d: clk set parent fail!\n",
-                                __func__, __LINE__);
-                ret = clk_set_parent(periph_clk, periph_pre_clk);
-                if (ret)
-                        dev_WARN(busfreq_dev, "%s: %d: clk set parent fail!\n",
-                                __func__, __LINE__);
-                audio_bus_freq_mode = 1;
+        unsigned long flags;
+
+        if (high_bus_freq_mode) {
+                pll2_org_rate = clk_get_rate(pll2);
+                /* Set periph_clk to be sourced from OSC_CLK */
+                clk_set_parent(periph_clk2_sel, osc_clk);
+                clk_set_parent(periph_clk, periph_clk2);
+                /* Ensure AHB/AXI clks are at 24MHz. */
+                clk_set_rate(ahb_clk, LPAPM_CLK);
+                clk_set_rate(ocram_clk, LPAPM_CLK);
+        }
+        if (audio_bus_count) {
+                /* Set up DDR to 100MHz. */
+                spin_lock_irqsave(&freq_lock, flags);
+                update_lpddr2_freq(DDR_AUDIO_CLK);
+                spin_unlock_irqrestore(&freq_lock, flags);
+
+                /* Fix the clock tree in kernel */
+                clk_set_rate(pll2, pll2_org_rate);
+                clk_set_parent(periph2_pre_clk, pll2_200);
+                clk_set_parent(periph2_clk, periph2_pre_clk);
+
+                if (low_bus_freq_mode) {
+                        /*
+                         * Swtich ARM to run off PLL2_PFD2_400MHz
+                         * since DDR is anyway at 100MHz.
+                         */
+                        clk_set_parent(pll1_sw_clk, pll2_400);
+                        /*
+                         * Ensure that the clock will be
+                         * at original speed.
+                         */
+                        clk_set_rate(cpu_clk, org_arm_rate);
+                }
                 low_bus_freq_mode = 0;
+                audio_bus_freq_mode = 1;
         } else {
-                update_ddr_freq(LPAPM_CLK);
-                /* Make sure periph clk's parent also got updated */
-                ret = clk_set_parent(periph_clk2_sel, osc_clk);
-                if (ret)
-                        dev_WARN(busfreq_dev, "%s: %d: clk set parent fail!\n",
-                                __func__, __LINE__);
-                ret = clk_set_parent(periph_clk, periph_clk2);
-                if (ret)
-                        dev_WARN(busfreq_dev, "%s: %d: clk set parent fail!\n",
-                                __func__, __LINE__);
-                if (audio_bus_freq_mode)
-                        clk_disable_unprepare(pll2_400);
+                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 (high_bus_freq_mode && cpu_is_imx6dl())
-                clk_disable_unprepare(pll2_400);
+}
+
+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);
+        spin_unlock_irqrestore(&freq_lock, flags);
 
+        /*
+         * Fix the clock tree in kernel.
+         * Make sure PLL2 rate is updated as it gets
+         * un-bypassed in the DDR freq change code.
+         */
+        clk_set_rate(pll2, pll2_org_rate);
+        clk_set_parent(periph2_pre_clk, pll2_400);
+        clk_set_parent(periph2_clk, periph2_pre_clk);
+
+        /* Ensure that periph_clk is sourced from PLL2_400. */
+        clk_set_parent(periph_pre_clk, pll2_400);
+        /*
+         * Before switching the perhiph_clk, ensure that the
+         * AHB/AXI will not be too fast.
+         */
+        clk_set_rate(ahb_clk, LPAPM_CLK / 3);
+        clk_set_rate(ocram_clk, LPAPM_CLK / 2);
+        clk_set_parent(periph_clk, periph_pre_clk);
+
+        if (low_bus_freq_mode) {
+                /* Move ARM from PLL1_SW_CLK to PLL2_400. */
+                clk_set_parent(pll1_sw_clk, pll2_400);
+                clk_set_rate(cpu_clk, org_arm_rate);
+        }
+}
+
+int reduce_bus_freq(void)
+{
+        int ret = 0;
+        clk_prepare_enable(pll3);
+        if (cpu_is_imx6sl())
+                enter_lpm_imx6sl();
+        else {
+                if (audio_bus_count) {
+                        /* Need to ensure that PLL2_PFD_400M is kept ON. */
+                        clk_prepare_enable(pll2_400);
+                        update_ddr_freq(DDR_AUDIO_CLK);
+                        /* Make sure periph clk's parent also got updated */
+                        ret = clk_set_parent(periph_clk2_sel, pll3);
+                        if (ret)
+                                dev_WARN(busfreq_dev,
+                                        "%s: %d: clk set parent fail!\n",
+                                        __func__, __LINE__);
+                        ret = clk_set_parent(periph_pre_clk, pll2_200);
+                        if (ret)
+                                dev_WARN(busfreq_dev,
+                                        "%s: %d: clk set parent fail!\n",
+                                        __func__, __LINE__);
+                        ret = clk_set_parent(periph_clk, periph_pre_clk);
+                        if (ret)
+                                dev_WARN(busfreq_dev,
+                                        "%s: %d: clk set parent fail!\n",
+                                        __func__, __LINE__);
+                        audio_bus_freq_mode = 1;
+                        low_bus_freq_mode = 0;
+                } else {
+                        update_ddr_freq(LPAPM_CLK);
+                        /* Make sure periph clk's parent also got updated */
+                        ret = clk_set_parent(periph_clk2_sel, osc_clk);
+                        if (ret)
+                                dev_WARN(busfreq_dev,
+                                        "%s: %d: clk set parent fail!\n",
+                                        __func__, __LINE__);
+                        /* Set periph_clk parent to OSC via periph_clk2_sel */
+                        ret = clk_set_parent(periph_clk, periph_clk2);
+                        if (ret)
+                                dev_WARN(busfreq_dev,
+                                        "%s: %d: clk set parent fail!\n",
+                                        __func__, __LINE__);
+                        if (audio_bus_freq_mode)
+                                clk_disable_unprepare(pll2_400);
+                        low_bus_freq_mode = 1;
+                        audio_bus_freq_mode = 0;
+                }
+                if (high_bus_freq_mode && cpu_is_imx6dl())
+                        clk_disable_unprepare(pll2_400);
+
+        }
         clk_disable_unprepare(pll3);
+
         med_bus_freq_mode = 0;
         high_bus_freq_mode = 0;
 
@@ -153,10 +283,10 @@ static void reduce_bus_freq_handler(struct work_struct *work)
 }
 
 /*
-  * Set the DDR, AHB to 24MHz.
-  * This mode will be activated only when none of the modules that
-  * need a higher DDR or AHB frequency are active.
-  */
+ * Set the DDR, AHB to 24MHz.
+ * 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)
 {
         if (busfreq_suspended)
@@ -166,23 +296,31 @@ int set_low_bus_freq(int low_bus_mode)
                 return 0;
 
         /*
-          * Don't lower the frequency immediately. Instead
-          * scheduled a delayed work and drop the freq if
-          * the conditions still remain the same.
-          */
-        low_bus_freq = low_bus_mode;
-        schedule_delayed_work(&low_bus_freq_handler,
-                                usecs_to_jiffies(3000000));
+         * Check to see if we need to got from
+         * low bus freq mode to audio bus freq mode.
+         * If so, the change needs to be done immediately.
+         */
+        if (audio_bus_count && low_bus_freq_mode)
+                reduce_bus_freq();
+        else
+                /*
+                 * Don't lower the frequency immediately. Instead
+                 * scheduled a delayed work and drop the freq if
+                 * the conditions still remain the same.
+                 */
+                schedule_delayed_work(&low_bus_freq_handler,
+                                        usecs_to_jiffies(3000000));
         return 0;
 }
 
 /*
-  * Set the DDR to either 528MHz or 400MHz for iMX6qd
-  * or 400MHz for iMX6dl.
-  */
+ * Set the DDR to either 528MHz or 400MHz for iMX6qd
+ * or 400MHz for iMX6dl.
+ */
 int set_high_bus_freq(int high_bus_freq)
 {
         int ret = 0;
+        struct clk *periph_clk_parent;
 
         if (bus_freq_scaling_initialized && bus_freq_scaling_is_active)
                 cancel_delayed_work_sync(&low_bus_freq_handler);
@@ -190,11 +328,10 @@ int set_high_bus_freq(int high_bus_freq)
         if (busfreq_suspended)
                 return 0;
 
-        /* for high setpoint, i.MX6Q is 528MHz, i.MX6DL is 400MHz */
         if (cpu_is_imx6q())
-                high_bus_freq = 1;
+                periph_clk_parent = pll2;
         else
-                high_bus_freq = 0;
+                periph_clk_parent = pll2_400;
 
         if (!bus_freq_scaling_initialized || !bus_freq_scaling_is_active)
                 return 0;
@@ -207,42 +344,49 @@ int set_high_bus_freq(int high_bus_freq)
                 return 0;
 
         clk_prepare_enable(pll3);
-        if (high_bus_freq) {
-                update_ddr_freq(ddr_normal_rate);
-                /* Make sure periph clk's parent also got updated */
-                ret = clk_set_parent(periph_clk2_sel, pll3);
-                if (ret)
-                        dev_WARN(busfreq_dev, "%s: %d: clk set parent fail!\n",
-                                __func__, __LINE__);
-                ret = clk_set_parent(periph_pre_clk, pll2);
-                if (ret)
-                        dev_WARN(busfreq_dev, "%s: %d: clk set parent fail!\n",
-                                __func__, __LINE__);
-                ret = clk_set_parent(periph_clk, periph_pre_clk);
-                if (ret)
-                        dev_WARN(busfreq_dev, "%s: %d: clk set parent fail!\n",
-                                __func__, __LINE__);
-                if (med_bus_freq_mode)
+        if (cpu_is_imx6sl())
+                exit_lpm_imx6sl();
+        else {
+                if (high_bus_freq) {
+                        update_ddr_freq(ddr_normal_rate);
+                        /* Make sure periph clk's parent also got updated */
+                        ret = clk_set_parent(periph_clk2_sel, pll3);
+                        if (ret)
+                                dev_WARN(busfreq_dev,
+                                        "%s: %d: clk set parent fail!\n",
+                                        __func__, __LINE__);
+                        ret = clk_set_parent(periph_pre_clk, periph_clk_parent);
+                        if (ret)
+                                dev_WARN(busfreq_dev,
+                                        "%s: %d: clk set parent fail!\n",
+                                        __func__, __LINE__);
+                        ret = clk_set_parent(periph_clk, periph_pre_clk);
+                        if (ret)
+                                dev_WARN(busfreq_dev,
+                                        "%s: %d: clk set parent fail!\n",
+                                        __func__, __LINE__);
+                } else {
+                        update_ddr_freq(ddr_med_rate);
+                        /* Make sure periph clk's parent also got updated */
+                        ret = clk_set_parent(periph_clk2_sel, pll3);
+                        if (ret)
+                                dev_WARN(busfreq_dev,
+                                        "%s: %d: clk set parent fail!\n",
+                                        __func__, __LINE__);
+                        ret = clk_set_parent(periph_pre_clk, pll2_400);
+                        if (ret)
+                                dev_WARN(busfreq_dev,
+                                        "%s: %d: clk set parent fail!\n",
+                                        __func__, __LINE__);
+                        ret = clk_set_parent(periph_clk, periph_pre_clk);
+                        if (ret)
+                                dev_WARN(busfreq_dev,
+                                        "%s: %d: clk set parent fail!\n",
+                                        __func__, __LINE__);
+                }
+                if (audio_bus_freq_mode)
                         clk_disable_unprepare(pll2_400);
-        } else {
-                clk_prepare_enable(pll2_400);
-                update_ddr_freq(ddr_med_rate);
-                /* Make sure periph clk's parent also got updated */
-                ret = clk_set_parent(periph_clk2_sel, pll3);
-                if (ret)
-                        dev_WARN(busfreq_dev, "%s: %d: clk set parent fail!\n",
-                                __func__, __LINE__);
-                ret = clk_set_parent(periph_pre_clk, pll2_400);
-                if (ret)
-                        dev_WARN(busfreq_dev, "%s: %d: clk set parent fail!\n",
-                                __func__, __LINE__);
-                ret = clk_set_parent(periph_clk, periph_pre_clk);
-                if (ret)
-                        dev_WARN(busfreq_dev, "%s: %d: clk set parent fail!\n",
-                                __func__, __LINE__);
         }
-        if (audio_bus_freq_mode)
-                clk_disable_unprepare(pll2_400);
 
         high_bus_freq_mode = 1;
         med_bus_freq_mode = 0;
@@ -281,6 +425,15 @@ void request_bus_freq(enum bus_freq_mode mode)
         }
 
         cancel_delayed_work_sync(&low_bus_freq_handler);
+
+        if (cpu_is_imx6dl()) {
+                /* No support for medium setpoint on MX6DL. */
+                if (mode == BUS_FREQ_MED) {
+                        high_bus_count++;
+                        mode = BUS_FREQ_HIGH;
+                }
+        }
+
         if ((mode == BUS_FREQ_HIGH) && (!high_bus_freq_mode)) {
                 set_high_bus_freq(1);
                 mutex_unlock(&bus_freq_mutex);
@@ -340,6 +493,14 @@ void release_bus_freq(enum bus_freq_mode mode)
                 return;
         }
 
+        if (cpu_is_imx6dl()) {
+                /* No support for medium setpoint on MX6DL. */
+                if (mode == BUS_FREQ_MED) {
+                        high_bus_count--;
+                        mode = BUS_FREQ_HIGH;
+                }
+        }
+
         if ((!audio_bus_freq_mode) && (high_bus_count == 0) &&
                 (med_bus_count == 0) && (audio_bus_count != 0)) {
                 set_low_bus_freq(1);
@@ -417,10 +578,24 @@ static int bus_freq_pm_notify(struct notifier_block *nb, unsigned long event,
         return NOTIFY_OK;
 }
 
+static int busfreq_reboot_notifier_event(struct notifier_block *this,
+                                                 unsigned long event, void *ptr)
+{
+        /* System is rebooting. Set the system into high_bus_freq_mode. */
+        request_bus_freq(BUS_FREQ_HIGH);
+
+        return 0;
+}
+
 static struct notifier_block imx_bus_freq_pm_notifier = {
         .notifier_call = bus_freq_pm_notify,
 };
 
+static struct notifier_block imx_busfreq_reboot_notifier = {
+        .notifier_call = busfreq_reboot_notifier_event,
+};
+
+
 static DEVICE_ATTR(enable, 0644, bus_freq_scaling_enable_show,
                         bus_freq_scaling_enable_store);
 
@@ -442,73 +617,135 @@ static int busfreq_probe(struct platform_device *pdev)
         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",
-                       __func__);
+                __func__);
                 return PTR_ERR(pll2_400);
         }
 
         pll2_200 = devm_clk_get(&pdev->dev, "pll2_198m");
         if (IS_ERR(pll2_200)) {
                 dev_err(busfreq_dev, "%s: failed to get pll2_198m\n",
-                       __func__);
+                        __func__);
                 return PTR_ERR(pll2_200);
         }
 
         pll2 = devm_clk_get(&pdev->dev, "pll2_bus");
         if (IS_ERR(pll2)) {
                 dev_err(busfreq_dev, "%s: failed to get pll2_bus\n",
-                       __func__);
+                        __func__);
                 return PTR_ERR(pll2);
         }
 
         cpu_clk = devm_clk_get(&pdev->dev, "arm");
         if (IS_ERR(cpu_clk)) {
                 dev_err(busfreq_dev, "%s: failed to get cpu_clk\n",
-                       __func__);
+                        __func__);
                 return PTR_ERR(cpu_clk);
         }
 
         pll3 = devm_clk_get(&pdev->dev, "pll3_usb_otg");
         if (IS_ERR(pll3)) {
                 dev_err(busfreq_dev, "%s: failed to get pll3_usb_otg\n",
-                       __func__);
+                        __func__);
                 return PTR_ERR(pll3);
         }
 
         periph_clk = devm_clk_get(&pdev->dev, "periph");
         if (IS_ERR(periph_clk)) {
                 dev_err(busfreq_dev, "%s: failed to get periph\n",
-                       __func__);
+                        __func__);
                 return PTR_ERR(periph_clk);
         }
 
         periph_pre_clk = devm_clk_get(&pdev->dev, "periph_pre");
         if (IS_ERR(periph_pre_clk)) {
                 dev_err(busfreq_dev, "%s: failed to get periph_pre\n",
-                       __func__);
+                        __func__);
                 return PTR_ERR(periph_pre_clk);
         }
 
         periph_clk2 = devm_clk_get(&pdev->dev, "periph_clk2");
         if (IS_ERR(periph_clk2)) {
                 dev_err(busfreq_dev, "%s: failed to get periph_clk2\n",
-                       __func__);
+                        __func__);
                 return PTR_ERR(periph_clk2);
         }
 
         periph_clk2_sel = devm_clk_get(&pdev->dev, "periph_clk2_sel");
         if (IS_ERR(periph_clk2_sel)) {
                 dev_err(busfreq_dev, "%s: failed to get periph_clk2_sel\n",
-                       __func__);
+                        __func__);
                 return PTR_ERR(periph_clk2_sel);
         }
 
         osc_clk = devm_clk_get(&pdev->dev, "osc");
         if (IS_ERR(osc_clk)) {
                 dev_err(busfreq_dev, "%s: failed to get osc_clk\n",
-                       __func__);
+                        __func__);
                 return PTR_ERR(osc_clk);
         }
 
+        if (cpu_is_imx6sl()) {
+                pll1_sys = devm_clk_get(&pdev->dev, "pll1_sys");
+                if (IS_ERR(pll1_sys)) {
+                        dev_err(busfreq_dev, "%s: failed to get pll1_sys\n",
+                                __func__);
+                        return PTR_ERR(pll1_sys);
+                }
+
+                ahb_clk = devm_clk_get(&pdev->dev, "ahb");
+                if (IS_ERR(ahb_clk)) {
+                        dev_err(busfreq_dev, "%s: failed to get ahb_clk\n",
+                                __func__);
+                        return PTR_ERR(ahb_clk);
+                }
+
+                ocram_clk = devm_clk_get(&pdev->dev, "ocram");
+                if (IS_ERR(ocram_clk)) {
+                        dev_err(busfreq_dev, "%s: failed to get ocram_clk\n",
+                                __func__);
+                        return PTR_ERR(ocram_clk);
+                }
+
+                pll1_sw_clk = devm_clk_get(&pdev->dev, "pll1_sw");
+                if (IS_ERR(pll1_sw_clk)) {
+                        dev_err(busfreq_dev, "%s: failed to get pll1_sw_clk\n",
+                                __func__);
+                        return PTR_ERR(pll1_sw_clk);
+                }
+
+                periph2_clk = devm_clk_get(&pdev->dev, "periph2");
+                if (IS_ERR(periph2_clk)) {
+                        dev_err(busfreq_dev, "%s: failed to get periph2\n",
+                                __func__);
+                        return PTR_ERR(periph2_clk);
+                }
+
+                periph2_pre_clk = devm_clk_get(&pdev->dev, "periph2_pre");
+                if (IS_ERR(periph2_pre_clk)) {
+                        dev_err(busfreq_dev,
+                                "%s: failed to get periph2_pre_clk\n",
+                                __func__);
+                        return PTR_ERR(periph2_pre_clk);
+                }
+
+                periph2_clk2 = devm_clk_get(&pdev->dev, "periph2_clk2");
+                if (IS_ERR(periph2_clk2)) {
+                        dev_err(busfreq_dev,
+                                "%s: failed to get periph2_clk2\n",
+                                __func__);
+                        return PTR_ERR(periph2_clk2);
+                }
+
+                periph2_clk2_sel = devm_clk_get(&pdev->dev, "periph2_clk2_sel");
+                if (IS_ERR(periph2_clk2_sel)) {
+                        dev_err(busfreq_dev,
+                                "%s: failed to get periph2_clk2_sel\n",
+                                __func__);
+                        return PTR_ERR(periph2_clk2_sel);
+                }
+
+        }
+
         err = sysfs_create_file(&busfreq_dev->kobj, &dev_attr_enable.attr);
         if (err) {
                 dev_err(busfreq_dev,
@@ -534,7 +771,7 @@ static int busfreq_probe(struct platform_device *pdev)
         if (cpu_is_imx6q()) {
                 if (of_property_read_u32(pdev->dev.of_node, "fsl,med_ddr_freq",
                                 &ddr_med_rate)) {
-                        dev_err(busfreq_dev,
+                        dev_WARN(busfreq_dev,
                                         "DDR medium rate not supported.\n");
                         ddr_med_rate = ddr_normal_rate;
                 }
@@ -543,8 +780,12 @@ static int busfreq_probe(struct platform_device *pdev)
         INIT_DELAYED_WORK(&low_bus_freq_handler, reduce_bus_freq_handler);
         INIT_DELAYED_WORK(&bus_freq_daemon, bus_freq_daemon_handler);
         register_pm_notifier(&imx_bus_freq_pm_notifier);
+        register_reboot_notifier(&imx_busfreq_reboot_notifier);
 
-        err = init_mmdc_settings(pdev);
+        if (cpu_is_imx6sl())
+                err = init_mmdc_lpddr2_settings(pdev);
+        else
+                err = init_mmdc_ddr3_settings(pdev);
         if (err) {
                 dev_err(busfreq_dev, "Busfreq init of MMDC failed\n");
                 return err;
diff --git a/arch/arm/mach-imx/busfreq_ddr3.c b/arch/arm/mach-imx/busfreq_ddr3.c
index 6ecd2f8a4407..6ac7d27fe86f 100644
--- a/arch/arm/mach-imx/busfreq_ddr3.c
+++ b/arch/arm/mach-imx/busfreq_ddr3.c
@@ -54,6 +54,13 @@ static void __iomem *l2_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;
+static volatile bool wait_for_ddr_freq_update;
+static int curr_ddr_rate;
+
 void (*mx6_change_ddr_freq)(u32 freq, void *ddr_settings,
         bool dll_mode, void *iomux_offsets) = NULL;
 
@@ -64,13 +71,6 @@ extern int audio_bus_freq_mode;
 extern void mx6_ddr3_freq_change(u32 freq, void *ddr_settings,
         bool dll_mode, void *iomux_offsets);
 
-static void *ddr_freq_change_iram_base;
-static int ddr_settings_size;
-static int iomux_settings_size;
-static volatile unsigned int cpus_in_wfe;
-static volatile bool wait_for_ddr_freq_update;
-static int curr_ddr_rate;
-
 #define MIN_DLL_ON_FREQ         333000000
 #define MAX_DLL_OFF_FREQ                125000000
 #define DDR_FREQ_CHANGE_SIZE    0x2000
@@ -250,7 +250,7 @@ int update_ddr_freq(int ddr_rate)
         return 0;
 }
 
-int init_mmdc_settings(struct platform_device *busfreq_pdev)
+int init_mmdc_ddr3_settings(struct platform_device *busfreq_pdev)
 {
         struct device *dev = &busfreq_pdev->dev;
         struct platform_device *ocram_dev;
@@ -400,17 +400,17 @@ int init_mmdc_settings(struct platform_device *busfreq_pdev)
         }
 
         /*
-          * 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
-          */
+         * 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;
@@ -456,7 +456,7 @@ int init_mmdc_settings(struct platform_device *busfreq_pdev)
         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
+         * Need to remap the area here since we want
          * the memory region to be executable.
          */
         ddr_freq_change_iram_base = __arm_ioremap(iram_paddr,
diff --git a/arch/arm/mach-imx/busfreq_lpddr2.c b/arch/arm/mach-imx/busfreq_lpddr2.c
new file mode 100644
index 000000000000..f15b8290669c
--- /dev/null
+++ b/arch/arm/mach-imx/busfreq_lpddr2.c
@@ -0,0 +1,172 @@
+/*
+ * Copyright (C) 2011-2013 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file busfreq_lpddr2.c
+ *
+ * @brief iMX6 LPDDR2 frequency change specific file.
+ *
+ * @ingroup PM
+ */
+#include <asm/cacheflush.h>
+#include <asm/fncpy.h>
+#include <asm/io.h>
+#include <asm/mach/map.h>
+#include <asm/mach-types.h>
+#include <asm/tlb.h>
+#include <linux/clk.h>
+#include <linux/cpumask.h>
+#include <linux/delay.h>
+#include <linux/genalloc.h>
+#include <linux/interrupt.h>
+#include <linux/irqchip/arm-gic.h>
+#include <linux/kernel.h>
+#include <linux/mutex.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_device.h>
+#include <linux/platform_device.h>
+#include <linux/proc_fs.h>
+#include <linux/sched.h>
+#include <linux/smp.h>
+
+#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;
+static 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 void mx6_lpddr2_freq_change(u32 freq, int bus_freq_mode,
+        void *iram_addr);
+
+
+#define LPDDR2_FREQ_CHANGE_SIZE 0x1000
+
+
+/* change the DDR frequency. */
+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);
+
+        curr_ddr_rate = ddr_rate;
+
+        dev_dbg(busfreq_dev, "\nBus freq set to %d start...\n", ddr_rate);
+
+        return 0;
+}
+
+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);
+        mx6_change_lpddr2_freq = (void *)fncpy(ddr_freq_change_iram_base,
+                &mx6_lpddr2_freq_change, LPDDR2_FREQ_CHANGE_SIZE);
+
+        curr_ddr_rate = ddr_normal_rate;
+
+        return 0;
+}
diff --git a/arch/arm/mach-imx/clk-imx6sl.c b/arch/arm/mach-imx/clk-imx6sl.c
index 5a7e76af7698..660656c449bc 100644
--- a/arch/arm/mach-imx/clk-imx6sl.c
+++ b/arch/arm/mach-imx/clk-imx6sl.c
@@ -7,9 +7,13 @@
  *
  */
 
+#define CCM_CCDR_OFFSET 0x4
+#define CCDR_CH0_HS_BYP 17
+
 #include <linux/clk.h>
 #include <linux/clkdev.h>
 #include <linux/err.h>
+#include <linux/io.h>
 #include <linux/of.h>
 #include <linux/of_address.h>
 #include <linux/of_irq.h>
@@ -25,10 +29,11 @@ static const char const *ocram_sels[]		= { "periph", "ocram_alt_sels", };
 static const char const *pre_periph_sels[]      = { "pll2_bus", "pll2_pfd2", "pll2_pfd0", "pll2_198m", };
 static const char const *periph_clk2_sels[]     = { "pll3_usb_otg", "osc", "osc", "dummy", };
 static const char const *periph2_clk2_sels[]    = { "pll3_usb_otg", "pll2_bus", };
-static const char const *periph_sels[]          = { "pre_periph_sel", "periph_clk2_podf", };
-static const char const *periph2_sels[]         = { "pre_periph2_sel", "periph2_clk2_podf", };
+static const char const *periph_sels[]          = { "pre_periph_sel", "periph_clk2", };
+static const char const *periph2_sels[]         = { "pre_periph2_sel", "periph2_clk2", };
 static const char const *csi_sels[]             = { "osc", "pll2_pfd2", "pll3_120m", "pll3_pfd1", };
 static const char const *lcdif_axi_sels[]       = { "pll2_bus", "pll2_pfd2", "pll3_usb_otg", "pll3_pfd1", };
+static const char const *csi_lcdif_sels[]       = { "mmdc", "pll2_pfd2", "pll3_120m", "pll3_pfd1", };
 static const char const *usdhc_sels[]           = { "pll2_pfd2", "pll2_pfd0", };
 static const char const *ssi_sels[]             = { "pll3_pfd2", "pll3_pfd3", "pll4_audio_div", "dummy", };
 static const char const *perclk_sels[]          = { "ipg", "osc", };
@@ -69,27 +74,49 @@ static struct clk *clks[IMX6SL_CLK_CLK_END];
 static struct clk_onecell_data clk_data;
 static u32 cur_arm_podf;
 
+extern int low_bus_freq_mode;
+
 /*
-  * On MX6SL, need to ensure that the ARM:IPG clock ratio is maintained
-  * within 12:5 when the clocks to ARM are gated when the SOC enters
-  * WAIT mode. This is necessary to avoid WAIT mode issue (an early
-  * interrupt waking up the ARM).
-  * This function will set the ARM clk to max value within the 12:5 limit.
-  */
+ * On MX6SL, need to ensure that the ARM:IPG clock ratio is maintained
+ * within 12:5 when the clocks to ARM are gated when the SOC enters
+ * WAIT mode. This is necessary to avoid WAIT mode issue (an early
+ * interrupt waking up the ARM).
+ * This function will set the ARM clk to max value within the 12:5 limit.
+ */
 void imx6sl_set_wait_clk(bool enter)
 {
-        u32 parent_rate = clk_get_rate(clk_get_parent(clks[IMX6SL_CLK_ARM]));
+        u32 parent_rate;
 
         if (enter) {
+                u32 wait_podf, new_parent_rate;
                 u32 ipg_rate = clk_get_rate(clks[IMX6SL_CLK_IPG]);
                 u32 max_arm_wait_clk = (12 * ipg_rate) / 5;
-                u32 wait_podf = (parent_rate + max_arm_wait_clk - 1) /
+                parent_rate = clk_get_rate(clks[IMX6SL_CLK_PLL1_SW]);
+                cur_arm_podf = parent_rate / clk_get_rate(clks[IMX6SL_CLK_ARM]);
+                if (low_bus_freq_mode) {
+                        /*
+                         * IPG clk is at 12MHz at this point, we can only run
+                         * ARM at a max of 28.8MHz. So we need to set ARM
+                         * to run from the 24MHz OSC, as there is no way to
+                         * get 28.8MHz when ARM is sourced from PLL1.
+                         */
+                        clk_set_parent(clks[IMX6SL_CLK_STEP],
+                                                        clks[IMX6SL_CLK_OSC]);
+                        clk_set_parent(clks[IMX6SL_CLK_PLL1_SW],
+                                                        clks[IMX6SL_CLK_STEP]);
+                }
+                new_parent_rate = clk_get_rate(clks[IMX6SL_CLK_PLL1_SW]);
+                wait_podf = (new_parent_rate + max_arm_wait_clk - 1) /
                                                 max_arm_wait_clk;
 
-                cur_arm_podf = parent_rate / clk_get_rate(clks[IMX6SL_CLK_ARM]);
-                clk_set_rate(clks[IMX6SL_CLK_ARM], parent_rate / wait_podf);
-        } else
+                clk_set_rate(clks[IMX6SL_CLK_ARM], new_parent_rate / wait_podf);
+        } else {
+                if (low_bus_freq_mode)
+                        /* Move ARM back to PLL1. */
+                        clk_set_parent(clks[IMX6SL_CLK_PLL1_SW], clks[IMX6SL_CLK_PLL1_SYS]);
+                parent_rate = clk_get_rate(clks[IMX6SL_CLK_PLL1_SW]);
                 clk_set_rate(clks[IMX6SL_CLK_ARM], parent_rate / cur_arm_podf);
+        }
 }
 
 static void __init imx6sl_clocks_init(struct device_node *ccm_node)
@@ -99,6 +126,7 @@ static void __init imx6sl_clocks_init(struct device_node *ccm_node)
         int irq;
         int ret;
         int i;
+        u32 reg;
 
         clks[IMX6SL_CLK_DUMMY] = imx_clk_fixed("dummy", 0);
         clks[IMX6SL_CLK_CKIL] = imx_obtain_fixed_clock("ckil", 0);
@@ -193,8 +221,8 @@ static void __init imx6sl_clocks_init(struct device_node *ccm_node)
 
         /*                                                   name                 parent_name          reg       shift width */
         clks[IMX6SL_CLK_OCRAM_PODF]        = imx_clk_divider("ocram_podf",        "ocram_sel",         base + 0x14, 16, 3);
-        clks[IMX6SL_CLK_PERIPH_CLK2_PODF]  = imx_clk_divider("periph_clk2_podf",  "periph_clk2_sel",   base + 0x14, 27, 3);
-        clks[IMX6SL_CLK_PERIPH2_CLK2_PODF] = imx_clk_divider("periph2_clk2_podf", "periph2_clk2_sel",  base + 0x14, 0,  3);
+        clks[IMX6SL_CLK_PERIPH_CLK2]  = imx_clk_divider("periph_clk2",  "periph_clk2_sel",   base + 0x14, 27, 3);
+        clks[IMX6SL_CLK_PERIPH2_CLK2] = imx_clk_divider("periph2_clk2", "periph2_clk2_sel",  base + 0x14, 0,  3);
         clks[IMX6SL_CLK_IPG]               = imx_clk_divider("ipg",               "ahb",               base + 0x14, 8,  2);
         clks[IMX6SL_CLK_CSI_PODF]          = imx_clk_divider("csi_podf",          "csi_sel",           base + 0x3c, 11, 3);
         clks[IMX6SL_CLK_LCDIF_AXI_PODF]    = imx_clk_divider("lcdif_axi_podf",    "lcdif_axi_sel",     base + 0x3c, 16, 3);
@@ -332,6 +360,11 @@ static void __init imx6sl_clocks_init(struct device_node *ccm_node)
         /* Set initial power mode */
         imx6_set_lpm(WAIT_CLOCKED);
 
+        /* Ensure that CH0 handshake is bypassed. */
+        reg = readl_relaxed(base + CCM_CCDR_OFFSET);
+        reg |= 1 << CCDR_CH0_HS_BYP;
+        writel_relaxed(reg, base + CCM_CCDR_OFFSET);
+
         np = of_find_compatible_node(NULL, NULL, "fsl,imx6sl-gpt");
         base = of_iomap(np, 0);
         WARN_ON(!base);
diff --git a/arch/arm/mach-imx/lpddr2_freq_imx6.S b/arch/arm/mach-imx/lpddr2_freq_imx6.S
new file mode 100644
index 000000000000..091bfae96478
--- /dev/null
+++ b/arch/arm/mach-imx/lpddr2_freq_imx6.S
@@ -0,0 +1,483 @@
+/*
+ * 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>
+
+        .macro    mx6sl_switch_to_24MHz
+
+        /*
+         * Set MMDC clock to be sourced from PLL3.
+         * Ensure first periph2_clk2 is sourced from PLL3.
+         * Set the PERIPH2_CLK2_PODF to divide by 2.
+         */
+        ldr     r6, [r2, #0x14]
+        bic     r6, r6, #0x7
+        orr     r6, r6, #0x1
+        str     r6, [r2, #0x14]
+
+        /* Select PLL3 to source MMDC. */
+        ldr     r6, [r2, #0x18]
+        bic     r6, r6, #0x100000
+        str     r6, [r2, #0x18]
+
+        /* Swtich periph2_clk_sel to run from PLL3. */
+        ldr     r6, [r2, #0x14]
+        orr     r6, r6, #0x4000000
+        str     r6, [r2, #0x14]
+
+periph2_clk_switch1:
+        ldr     r6, [r2, #0x48]
+        cmp     r6, #0
+        bne     periph2_clk_switch1
+
+        /*
+         * Need to clock gate the 528 PFDs before
+         * powering down PLL2.
+         * Only the PLL2_PFD2_400M should be ON
+         * at this time, so only clock gate that one.
+         */
+        ldr     r6, [r3, #0x100]
+        orr     r6, r6, #0x800000
+        str     r6, [r3, #0x100]
+
+        /*
+         * Set PLL2 to bypass state. We should be here
+         * only if MMDC is not sourced from PLL2.
+         */
+        ldr     r6, [r3, #0x30]
+        orr     r6, r6, #0x10000
+        str     r6, [r3, #0x30]
+
+        ldr     r6, [r3, #0x30]
+        orr     r6, r6, #0x1000
+        str     r6, [r3, #0x30]
+
+        /* Ensure pre_periph2_clk_mux is set to pll2 */
+        ldr     r6, [r2, #0x18]
+        bic     r6, r6, #0x600000
+        str     r6, [r2, #0x18]
+
+        /* Set MMDC clock to be sourced from the bypassed PLL2. */
+        ldr     r6, [r2, #0x14]
+        bic     r6, r6, #0x4000000
+        str     r6, [r2, #0x14]
+
+periph2_clk_switch2:
+        ldr     r6, [r2, #0x48]
+        cmp     r6, #0
+        bne     periph2_clk_switch2
+
+        /*
+         * Now move MMDC back to periph2_clk2 source.
+         * after selecting PLL2 as the option.
+         * Select PLL2 as the source.
+         */
+        ldr     r6, [r2, #0x18]
+        orr     r6, r6, #0x100000
+        str     r6, [r2, #0x18]
+
+        /* set periph2_clk2_podf to divide by 1. */
+        ldr     r6, [r2, #0x14]
+        bic     r6, r6, #0x7
+        str     r6, [r2, #0x14]
+
+        /* Now move periph2_clk to periph2_clk2 source */
+        ldr     r6, [r2, #0x14]
+        orr     r6, r6, #0x4000000
+        str     r6, [r2, #0x14]
+
+periph2_clk_switch3:
+        ldr     r6, [r2, #0x48]
+        cmp     r6, #0
+        bne     periph2_clk_switch3
+
+        /* Now set the MMDC PODF back to 1.*/
+        ldr     r6, [r2, #0x14]
+        bic     r6, r6, #0x38
+        str     r6, [r2, #0x14]
+
+mmdc_podf0:
+        ldr     r6, [r2, #0x48]
+        cmp     r6, #0
+        bne     mmdc_podf0
+
+        .endm
+
+          .macro        ddr_switch_400MHz
+
+        /* Set MMDC divider first, in case PLL3 is at 480MHz. */
+        ldr     r6, [r3, #0x10]
+        and     r6, r6, #0x10000
+        cmp     r6, #0x10000
+        beq     pll3_in_bypass
+
+        /* Set MMDC divder to divide by 2. */
+        ldr     r6, [r2, #0x14]
+        bic     r6, r6, #0x38
+        orr     r6, r6, #0x8
+        str     r6, [r2, #0x14]
+
+mmdc_podf:
+        ldr     r6, [r2, #0x48]
+        cmp     r6, #0
+        bne     mmdc_podf
+
+pll3_in_bypass:
+        /*
+         * Check if we are switching between
+         * 400Mhz <-> 100MHz.If so, we should
+         * try to source MMDC from PLL2_200M.
+         */
+        cmp     r1, #0
+        beq     not_low_bus_freq
+
+        /* Ensure that MMDC is sourced from PLL2 mux first. */
+        ldr     r6, [r2, #0x14]
+        bic     r6, r6, #0x4000000
+        str     r6, [r2, #0x14]
+
+periph2_clk_switch4:
+        ldr     r6, [r2, #0x48]
+        cmp     r6, #0
+        bne     periph2_clk_switch4
+
+not_low_bus_freq:
+        /* Now ensure periph2_clk2_sel mux is set to PLL3 */
+        ldr     r6, [r2, #0x18]
+        bic     r6, r6, #0x100000
+        str     r6, [r2, #0x18]
+
+        /* Now switch MMDC to PLL3. */
+        ldr     r6, [r2, #0x14]
+        orr     r6, r6, #0x4000000
+        str     r6, [r2, #0x14]
+
+periph2_clk_switch5:
+        ldr     r6, [r2, #0x48]
+        cmp     r6, #0
+        bne     periph2_clk_switch5
+
+        /*
+         * Check if PLL2 is already unlocked.
+         * If so do nothing with PLL2.
+         */
+        cmp     r1, #0
+        beq     pll2_already_on
+
+        /* Now power up PLL2 and unbypass it. */
+        ldr     r6, [r3, #0x30]
+        bic     r6, r6, #0x1000
+        str     r6, [r3, #0x30]
+
+        /* Make sure PLL2 has locked.*/
+wait_for_pll_lock:
+        ldr     r6, [r3, #0x30]
+        and     r6, r6, #0x80000000
+        cmp     r6, #0x80000000
+        bne     wait_for_pll_lock
+
+        ldr     r6, [r3, #0x30]
+        bic     r6, r6, #0x10000
+        str     r6, [r3, #0x30]
+
+        /*
+         * Need to enable the 528 PFDs after
+         * powering up PLL2.
+         * Only the PLL2_PFD2_400M should be ON
+         * as it feeds the MMDC. Rest should have
+         * been managed by clock code.
+         */
+        ldr     r6, [r3, #0x100]
+        bic     r6, r6, #0x800000
+        str     r6, [r3, #0x100]
+
+pll2_already_on:
+        /*
+         * Now switch MMDC clk back to pll2_mux option.
+         * Ensure pre_periph2_clk2 is set to pll2_pfd_400M.
+         * If switching to audio DDR freq, set the
+         * pre_periph2_clk2 to PLL2_PFD_200M
+         */
+        ldr     r6, =400000000
+        cmp     r6, r0
+        bne     use_pll2_pfd_200M
+
+        ldr     r6, [r2, #0x18]
+        bic     r6, r6, #0x600000
+        orr     r6, r6, #0x200000
+        str     r6, [r2, #0x18]
+        ldr     r6, =400000000
+        b       cont2
+
+use_pll2_pfd_200M:
+        ldr     r6, [r2, #0x18]
+        orr     r6, r6, #0x600000
+        str     r6, [r2, #0x18]
+        ldr     r6, =200000000
+
+cont2:
+        ldr     r4, [r2, #0x14]
+        bic     r4, r4, #0x4000000
+        str     r4, [r2, #0x14]
+
+periph2_clk_switch6:
+        ldr     r4, [r2, #0x48]
+        cmp     r4, #0
+        bne     periph2_clk_switch6
+
+change_divider_only:
+        /*
+         * Calculate the MMDC divider
+         * based on the requested freq.
+         */
+        ldr     r4, =0
+Loop2:
+        sub     r6, r6, r0
+        cmp     r6, r0
+        blt     Div_Found
+        add     r4, r4, #1
+        bgt     Loop2
+
+        /* Shift divider into correct offset. */
+        lsl     r4, r4, #3
+Div_Found:
+        /* Set the MMDC PODF. */
+        ldr     r6, [r2, #0x14]
+        bic     r6, r6, #0x38
+        orr     r6, r6, r4
+        str     r6, [r2, #0x14]
+
+mmdc_podf1:
+        ldr     r6, [r2, #0x48]
+        cmp     r6, #0
+        bne     mmdc_podf1
+
+        .endm
+
+        .macro  mmdc_clk_lower_100MHz
+
+        /*
+         * Prior to reducing the DDR frequency (at 528/400 MHz),
+         * read the Measure unit count bits (MU_UNIT_DEL_NUM)
+         */
+        ldr     r5, =0x8B8
+        ldr     r6, [r8, r5]
+        /* Original MU unit count */
+        mov     r6, r6, LSR #16
+        ldr     r4, =0x3FF
+        and     r6, r6, r4
+        /* Original MU unit count * 2 */
+        mov     r7, r6, LSL #1
+        /*
+         * Bypass the automatic measure unit when below 100 MHz
+         * by setting the Measure unit bypass enable bit (MU_BYP_EN)
+         */
+        ldr     r6, [r8, r5]
+        orr     r6, r6, #0x400
+        str     r6, [r8, r5]
+        /*
+         * Double the measure count value read in step 1 and program it in the
+         * measurement bypass bits (MU_BYP_VAL) of the MMDC PHY Measure Unit
+         * Register for the reduced frequency operation below 100 MHz
+         */
+        ldr     r6, [r8, r5]
+        ldr     r4, =0x3FF
+        bic     r6, r6, r4
+        orr     r6, r6, r7
+        str     r6, [r8, r5]
+        /* Now perform a Force Measurement. */
+        ldr     r6, [r8, r5]
+        orr     r6, r6, #0x800
+        str     r6, [r8, r5]
+        /* Wait for FRC_MSR to clear. */
+force_measure:
+        ldr     r6, [r8, r5]
+        and     r6, r6, #0x800
+        cmp     r6, #0x0
+        bne     force_measure
+
+        .endm
+
+        .macro  mmdc_clk_above_100MHz
+
+        /* Make sure that the PHY measurement unit is NOT in bypass mode */
+        ldr     r5, =0x8B8
+        ldr     r6, [r8, r5]
+        bic     r6, r6, #0x400
+        str     r6, [r8, r5]
+        /* Now perform a Force Measurement. */
+        ldr     r6, [r8, r5]
+        orr     r6, r6, #0x800
+        str     r6, [r8, r5]
+        /* Wait for FRC_MSR to clear. */
+force_measure1:
+        ldr     r6, [r8, r5]
+        and     r6, r6, #0x800
+        cmp     r6, #0x0
+        bne     force_measure1
+        .endm
+
+/*
+ *  mx6_lpddr2_freq_change
+ *
+ *  Make sure DDR is in self-refresh.
+ *  IRQs are already disabled.
+ * r0 : DDR freq.
+ * r1: low_bus_freq_mode flag
+ * r2: Pointer to array containing addresses of registers.
+ */
+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, #0x8]          @L2_BASE_ADDR
+
+lpddr2_freq_change:
+        adr     r9, lpddr2_freq_change
+
+        /* Prime all TLB entries. */
+        ldr     r6, [r9]
+        ldr     r6, [r8]
+        ldr     r6, [r3]
+        ldr     r6, [r2]
+
+         /* 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
+
+        /* Disable Automatic power savings. */
+        ldr     r6, [r8, #0x404]
+        orr     r6, r6, #0x01
+        str     r6, [r8, #0x404]
+
+        /* MMDC0_MDPDC disable power down timer */
+        ldr     r6, [r8, #0x4]
+        bic     r6, r6, #0xff00
+        str     r6, [r8, #0x4]
+
+        /* Delay for a while */
+        ldr     r10, =10
+delay1:
+        ldr     r7, =0
+cont1:
+        ldr     r6, [r8, r7]
+        add     r7, r7, #4
+        cmp     r7, #16
+        bne     cont1
+        sub     r10, r10, #1
+        cmp     r10, #0
+        bgt     delay1
+
+        /* 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]
+
+        ldr     r10, =100000000
+        cmp     r0, r10
+        bgt     set_ddr_mu_above_100
+        mmdc_clk_lower_100MHz
+
+set_ddr_mu_above_100:
+        ldr     r10, =24000000
+        cmp     r0, r10
+        beq     set_to_24MHz
+
+        ddr_switch_400MHz
+
+        ldr     r10,=100000000
+        cmp     r0, r10
+        blt     done
+        mmdc_clk_above_100MHz
+
+        b       done
+
+set_to_24MHz:
+        mx6sl_switch_to_24MHz
+
+done:
+        /* 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
+
+        /* Enable Automatic power savings. */
+        ldr     r6, [r8, #0x404]
+        bic     r6, r6, #0x01
+        str     r6, [r8, #0x404]
+
+        ldr     r10, =24000000
+        cmp     r0, r10
+        beq     skip_power_down
+
+        /* Enable MMDC power down timer. */
+        ldr     r6, [r8, #0x4]
+        orr     r6, r6, #0x5500
+        str     r6, [r8, #0x4]
+
+skip_power_down:
+        /* 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
+
+        .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
diff --git a/include/dt-bindings/clock/imx6sl-clock.h b/include/dt-bindings/clock/imx6sl-clock.h
index 118b730bc635..b04e64a2ec2a 100644
--- a/include/dt-bindings/clock/imx6sl-clock.h
+++ b/include/dt-bindings/clock/imx6sl-clock.h
@@ -71,8 +71,8 @@
 #define IMX6SL_CLK_PERIPH               58
 #define IMX6SL_CLK_PERIPH2              59
 #define IMX6SL_CLK_OCRAM_PODF           60
-#define IMX6SL_CLK_PERIPH_CLK2_PODF     61
-#define IMX6SL_CLK_PERIPH2_CLK2_PODF    62
+#define IMX6SL_CLK_PERIPH_CLK2          61
+#define IMX6SL_CLK_PERIPH2_CLK2 62
 #define IMX6SL_CLK_IPG                  63
 #define IMX6SL_CLK_CSI_PODF             64
 #define IMX6SL_CLK_LCDIF_AXI_PODF       65