arm: imx: Add busfreq support imx6sl

Add busfreq support for i.MX6SL.

Signed-off-by: Jacky Bai <ping.bai@nxp.com>

4 files changed, 853 insertions, 2 deletions

diff --git a/arch/arm/mach-imx/Makefile b/arch/arm/mach-imx/Makefile
index 84e8112afb2b..998b07de577d 100644
--- a/arch/arm/mach-imx/Makefile
+++ b/arch/arm/mach-imx/Makefile
@@ -85,7 +85,7 @@ obj-$(CONFIG_HOTPLUG_CPU) += hotplug.o
 endif
 obj-$(CONFIG_SOC_IMX6Q) += mach-imx6q.o ddr3_freq_imx6.o smp_wfe_imx6.o \
 				lpddr2_freq_imx6q.o
-obj-$(CONFIG_SOC_IMX6SL) += mach-imx6sl.o
+obj-$(CONFIG_SOC_IMX6SL) += mach-imx6sl.o lpddr2_freq_imx6.o
 obj-$(CONFIG_SOC_IMX6SLL) += mach-imx6sl.o
 obj-$(CONFIG_SOC_IMX6SX) += mach-imx6sx.o ddr3_freq_imx6sx.o smp_wfe_imx6.o lpddr2_freq_imx6sx.o
 obj-$(CONFIG_SOC_IMX6UL) += mach-imx6ul.o
@@ -100,6 +100,7 @@ AFLAGS_smp_wfe_imx6.o :=-Wa,-march=armv7-a
 AFLAGS_ddr3_freq_imx7d.o :=-Wa,-march=armv7-a
 AFLAGS_lpddr3_freq_imx.o :=-Wa,-march=armv7-a
 AFLAGS_ddr3_freq_imx6.o :=-Wa,-march=armv7-a
+AFLAGS_lpddr2_freq_imx6.o :=-Wa,-march=armv7-a
 AFLAGS_lpddr2_freq_imx6q.o :=-Wa,-march=armv7-a
 AFLAGS_lpddr2_freq_imx6sx.o :=-Wa,-march=armv7-a
 AFLAGS_ddr3_freq_imx6sx.o :=-Wa,-march=armv7-a
diff --git a/arch/arm/mach-imx/busfreq-imx.c b/arch/arm/mach-imx/busfreq-imx.c
index c4aae89ced16..f948ca65ffb3 100644
--- a/arch/arm/mach-imx/busfreq-imx.c
+++ b/arch/arm/mach-imx/busfreq-imx.c
@@ -66,6 +66,7 @@ static int bus_freq_scaling_is_active;
 static int high_bus_count, med_bus_count, audio_bus_count, low_bus_count;
 static unsigned int ddr_low_rate;
 static int cur_bus_freq_mode;
+static u32 org_arm_rate;
 
 extern unsigned long iram_tlb_phys_addr;
 extern int unsigned long iram_tlb_base_addr;
@@ -119,6 +120,17 @@ static struct clk *mmdc_clk;
 static struct clk *periph_clk2_clk;
 static struct clk *pll2_bus_clk;
 
+static struct clk *pll2_bypass_src_clk;
+static struct clk *pll2_bypass_clk;
+static struct clk *pll2_clk;
+static struct clk *arm_clk;
+static struct clk *step_clk;
+static struct clk *pll1_clk;
+static struct clk *pll1_bypass_src_clk;
+static struct clk *pll1_bypass_clk;
+static struct clk *pll1_sys_clk;
+static struct clk *pll1_sw_clk;
+
 static struct clk *pll3_pfd1_540m_clk;
 
 static struct clk *ocram_clk;
@@ -370,6 +382,186 @@ static void exit_lpm_imx6_smp(void)
 		clk_disable_unprepare(pll2_400_clk);
 }
 
+static void enter_lpm_imx6sl(void)
+{
+	if (high_bus_freq_mode) {
+		/* Set periph_clk to be sourced from OSC_CLK */
+		clk_set_parent(periph_clk2_sel_clk, osc_clk);
+		clk_set_parent(periph_clk, periph_clk2_clk);
+		/* 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 AHB to 8MHz to lower pwer.*/
+		clk_set_rate(ahb_clk, LPAPM_CLK / 3);
+
+		/* Set up DDR to 100MHz. */
+		busfreq_func.update(HIGH_AUDIO_CLK);
+
+		/* Fix the clock tree in kernel */
+		clk_set_parent(periph2_pre_clk, pll2_200_clk);
+		clk_set_parent(periph2_clk, periph2_pre_clk);
+
+		if (low_bus_freq_mode || ultra_low_bus_freq_mode) {
+			/*
+			 * Fix the clock tree in kernel, make sure
+			 * pll2_bypass is updated as it is
+			 * sourced from PLL2.
+			 */
+			clk_set_parent(pll2_bypass_clk, pll2_clk);
+			/*
+			 * Swtich ARM to run off PLL2_PFD2_400MHz
+			 * since DDR is anyway at 100MHz.
+			 */
+			clk_set_parent(step_clk, pll2_400_clk);
+			clk_set_parent(pll1_sw_clk, step_clk);
+
+			/*
+			 * Need to ensure that PLL1 is bypassed and enabled
+			 * before ARM-PODF is set.
+			 */
+			clk_set_parent(pll1_bypass_clk, pll1_bypass_src_clk);
+
+			/*
+			 * Ensure that the clock will be
+			 * at original speed.
+			 */
+			clk_set_rate(arm_clk, org_arm_rate);
+		}
+		low_bus_freq_mode = 0;
+		ultra_low_bus_freq_mode = 0;
+		audio_bus_freq_mode = 1;
+		cur_bus_freq_mode = BUS_FREQ_AUDIO;
+	} else {
+		u32 arm_div, pll1_rate;
+		org_arm_rate = clk_get_rate(arm_clk);
+		if (org_arm_rate == 0) {
+			WARN_ON(1);
+			return;
+		}
+		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;
+			cur_bus_freq_mode = BUS_FREQ_ULTRA_LOW;
+		} else {
+			if (!ultra_low_bus_freq_mode && !low_bus_freq_mode) {
+				/*
+				 * Anyway, make sure the AHB is running at 24MHz
+				 * in low_bus_freq_mode.
+				 */
+				if (audio_bus_freq_mode)
+					clk_set_rate(ahb_clk, LPAPM_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.
+				 * To work well with CPUFREQ we want to ensure that
+				 * the CPU freq does not change, so attempt to
+				 * get a freq as close to 396MHz as possible.
+				 */
+				clk_set_rate(pll1_clk,
+					clk_round_rate(pll1_clk, (org_arm_rate * 2)));
+				pll1_rate = clk_get_rate(pll1_clk);
+				arm_div = pll1_rate / org_arm_rate;
+				if (pll1_rate / arm_div > org_arm_rate)
+					arm_div++;
+				/*
+				  * Need to ensure that PLL1 is bypassed and enabled
+				  * before ARM-PODF is set.
+				  */
+				clk_set_parent(pll1_bypass_clk, pll1_clk);
+				/*
+				 * Ensure ARM CLK is lower before
+				 * changing the parent.
+				 */
+				clk_set_rate(arm_clk, org_arm_rate / arm_div);
+				/* Now set the ARM clk parent to PLL1_SYS. */
+				clk_set_parent(pll1_sw_clk, pll1_sys_clk);
+
+				/*
+				 * Set STEP_CLK back to OSC to save power and
+				 * also to maintain the parent.The WFI iram code
+				 * will switch step_clk to osc, but the clock API
+				 * is not aware of the change and when a new request
+				 * to change the step_clk parent to pll2_pfd2_400M
+				 * is requested sometime later, the change is ignored.
+				 */
+				clk_set_parent(step_clk, osc_clk);
+
+				/* Now set DDR to 24MHz. */
+				busfreq_func.update(LPAPM_CLK);
+
+				/*
+				 * Fix the clock tree in kernel.
+				 * Make sure PLL2 rate is updated as it gets
+				 * bypassed in the DDR freq change code.
+				 */
+				clk_set_parent(pll2_bypass_clk, pll2_bypass_src_clk);
+				clk_set_parent(periph2_clk2_sel_clk, pll2_bus_clk);
+				clk_set_parent(periph2_clk, periph2_clk2_clk);
+			}
+			if (low_bus_count == 0) {
+				ultra_low_bus_freq_mode = 1;
+				low_bus_freq_mode = 0;
+				cur_bus_freq_mode = BUS_FREQ_ULTRA_LOW;
+			} else {
+				ultra_low_bus_freq_mode = 0;
+				low_bus_freq_mode = 1;
+				cur_bus_freq_mode = BUS_FREQ_LOW;
+			}
+			audio_bus_freq_mode = 0;
+		}
+	}
+}
+
+static void exit_lpm_imx6sl(void)
+{
+	/* Change DDR freq in IRAM. */
+	busfreq_func.update(ddr_normal_rate);
+
+	/*
+	 * 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_parent(pll2_bypass_clk, pll2_clk);
+	clk_set_parent(periph2_pre_clk, pll2_400_clk);
+	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_clk);
+	/*
+	 * 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 || ultra_low_bus_freq_mode) {
+		/* Move ARM from PLL1_SW_CLK to PLL2_400. */
+		clk_set_parent(step_clk, pll2_400_clk);
+		clk_set_parent(pll1_sw_clk, step_clk);
+		/*
+		  * Need to ensure that PLL1 is bypassed and enabled
+		  * before ARM-PODF is set.
+		  */
+		clk_set_parent(pll1_bypass_clk, pll1_bypass_src_clk);
+		clk_set_rate(arm_clk, org_arm_rate);
+		ultra_low_bus_freq_mode = 0;
+	}
+}
+
 static void enter_lpm_imx7d(void)
 {
 	/*
@@ -441,6 +633,8 @@ static void reduce_bus_freq(void)
 		enter_lpm_imx6_up();
 	else if (cpu_is_imx6q() || cpu_is_imx6dl())
 		enter_lpm_imx6_smp();
+	else if (cpu_is_imx6sl())
+		enter_lpm_imx6sl();
 
 	med_bus_freq_mode = 0;
 	high_bus_freq_mode = 0;
@@ -543,6 +737,8 @@ static int set_high_bus_freq(int high_bus_freq)
 		exit_lpm_imx6_up();
 	else if (cpu_is_imx6q() || cpu_is_imx6dl())
 		exit_lpm_imx6_smp();
+	else if (cpu_is_imx6sl())
+		exit_lpm_imx6sl();
 
 	high_bus_freq_mode = 1;
 	med_bus_freq_mode = 0;
@@ -895,7 +1091,7 @@ static int busfreq_probe(struct platform_device *pdev)
 		}
 	}
 
-	if (cpu_is_imx6sx()) {
+	if (cpu_is_imx6sx() || cpu_is_imx6sl()) {
 		ahb_clk = devm_clk_get(&pdev->dev, "ahb");
 		ocram_clk = devm_clk_get(&pdev->dev, "ocram");
 		periph2_clk = devm_clk_get(&pdev->dev, "periph2");
@@ -936,6 +1132,35 @@ static int busfreq_probe(struct platform_device *pdev)
 			return -EINVAL;
 		}
 	}
+
+	if (cpu_is_imx6sl()) {
+		pll2_bypass_src_clk = devm_clk_get(&pdev->dev, "pll2_bypass_src");
+		pll2_bypass_clk = devm_clk_get(&pdev->dev, "pll2_bypass");
+		pll2_clk = devm_clk_get(&pdev->dev, "pll2");
+		if (IS_ERR(pll2_bypass_src_clk) || IS_ERR(pll2_bypass_clk)
+			|| IS_ERR(pll2_clk)) {
+			dev_err(busfreq_dev,
+				"%s failed to get busfreq clk for imx6sl.\n", __func__);
+			return -EINVAL;
+		}
+	}
+
+	if (cpu_is_imx6sl()) {
+		arm_clk = devm_clk_get(&pdev->dev, "arm");
+		step_clk = devm_clk_get(&pdev->dev, "step");
+		pll1_clk = devm_clk_get(&pdev->dev, "pll1");
+		pll1_bypass_src_clk = devm_clk_get(&pdev->dev, "pll1_bypass_src");
+		pll1_bypass_clk = devm_clk_get(&pdev->dev, "pll1_bypass");
+		pll1_sys_clk = devm_clk_get(&pdev->dev, "pll1_sys");
+		pll1_sw_clk = devm_clk_get(&pdev->dev, "pll1_sw");
+		if (IS_ERR(arm_clk) || IS_ERR(step_clk) || IS_ERR(pll1_clk)
+			|| IS_ERR(pll1_bypass_src_clk) || IS_ERR(pll1_bypass_clk)
+			|| IS_ERR(pll1_sys_clk) || IS_ERR(pll1_sw_clk)) {
+			dev_err(busfreq_dev, "%s failed to get busfreq clk for imx6ull/sl.\n", __func__);
+			return -EINVAL;
+		}
+	}
+
 	if (cpu_is_imx7d()) {
 		osc_clk = devm_clk_get(&pdev->dev, "osc");
 		axi_sel_clk = devm_clk_get(&pdev->dev, "axi_sel");
@@ -1044,6 +1269,9 @@ static int busfreq_probe(struct platform_device *pdev)
 			busfreq_func.init   = &init_mmdc_lpddr2_settings_mx6q;
 			busfreq_func.update = &update_lpddr2_freq_smp;
 		}
+	} else if (cpu_is_imx6sl()) {
+		busfreq_func.init   = &init_mmdc_lpddr2_settings;
+		busfreq_func.update = &update_lpddr2_freq;
 	}
 
 	if (busfreq_func.init)
diff --git a/arch/arm/mach-imx/busfreq_lpddr2.c b/arch/arm/mach-imx/busfreq_lpddr2.c
index 72c947370f51..8c7793c01f3a 100644
--- a/arch/arm/mach-imx/busfreq_lpddr2.c
+++ b/arch/arm/mach-imx/busfreq_lpddr2.c
@@ -162,6 +162,10 @@ int init_mmdc_lpddr2_settings(struct platform_device *busfreq_pdev)
 
 	ddr_code_size = SZ_4K;
 
+	if (cpu_is_imx6sl())
+		mx6_change_lpddr2_freq = (void *)fncpy(
+			(void *)ddr_freq_change_iram_base,
+			&mx6_lpddr2_freq_change, ddr_code_size);
 	if (cpu_is_imx6sx())
 		mx6_change_lpddr2_freq = (void *)fncpy(
 			(void *)ddr_freq_change_iram_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..21179fb2cb72
--- /dev/null
+++ b/arch/arm/mach-imx/lpddr2_freq_imx6.S
@@ -0,0 +1,618 @@
+/*
+ * Copyright (C) 2012-2016 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.
+ */
+
+#include <linux/linkage.h>
+#include "hardware.h"
+
+#define PL310_AUX_CTRL          0x104
+#define PL310_DCACHE_LOCKDOWN_BASE 0x900
+#define PL310_AUX_16WAY_BIT	0x10000
+#define PL310_LOCKDOWN_NBREGS   8
+#define PL310_LOCKDOWN_SZREG    4
+#define PL310_8WAYS_MASK        0x00FF
+#define PL310_16WAYS_UPPERMASK  0xFF00
+
+.globl imx6_lpddr2_freq_change_start
+.globl imx6_lpddr2_freq_change_end
+
+	.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]
+
+	.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
+ */
+	.align 3
+ENTRY(mx6_lpddr2_freq_change)
+imx6_lpddr2_freq_change_start:
+	push {r4-r10}
+
+	/*
+	 * To ensure no page table walks occur in DDR, we
+	 * have a another page table stored in IRAM that only
+	 * contains entries pointing to IRAM, AIPS1 and AIPS2.
+	 * We need to set the TTBR1 to the new IRAM TLB.
+	 * Do the following steps:
+	 * 1. Flush the Branch Target Address Cache (BTAC)
+	 * 2. Set TTBR1 to point to IRAM page table.
+	 * 3. Disable page table walks in TTBR0 (PD0 = 1)
+	 * 4. Set TTBR0.N=1, implying 0-2G is translated by TTBR0
+	 *     and 2-4G is translated by TTBR1.
+	 */
+
+	ldr	r6, =iram_tlb_phys_addr
+	ldr	r7, [r6]
+
+	/* Disable Branch Prediction, Z bit in SCTLR. */
+	mrc	p15, 0, r6, c1, c0, 0
+	bic	r6, r6, #0x800
+	mcr	p15, 0, r6, c1, c0, 0
+
+	/* Flush the Branch Target Address Cache (BTAC) */
+	ldr	r6, =0x0
+	mcr	p15, 0, r6, c7, c1, 6
+
+	dsb
+	isb
+	/* Store the IRAM table in TTBR1 */
+	mcr	p15, 0, r7, c2, c0, 1
+
+	/* Read TTBCR and set PD0=1, N = 1 */
+	mrc	p15, 0, r6, c2, c0, 2
+	orr	r6, r6, #0x11
+	mcr	p15, 0, r6, c2, c0, 2
+
+	dsb
+	isb
+
+	/* flush the TLB */
+	ldr	r6, =0x0
+	mcr	p15, 0, r6, c8, c3, 0
+
+	/* Disable L1 data cache. */
+	mrc	p15, 0, r6, c1, c0, 0
+	bic	r6, r6, #0x4
+	mcr	p15, 0, r6, c1, c0, 0
+
+	dsb
+	isb
+
+
+#ifdef CONFIG_CACHE_L2X0
+	/*
+	 * Need to make sure the buffers in L2 are drained.
+	 * Performing a sync operation does this.
+	 */
+	ldr	r7, =IMX_IO_P2V(MX6Q_L2_BASE_ADDR)
+
+	/* Wait for background operations to complete. */
+wait_for_l2_to_idle:
+	ldr	r6, [r7, #0x730]
+	cmp	r6, #0x0
+	bne	wait_for_l2_to_idle
+
+	mov	r6, #0x0
+	str	r6, [r7, #0x730]
+
+	/*
+	 * The second dsb might be needed to keep cache sync (device write)
+	 * ordering with the memory accesses before it.
+	 */
+	dsb
+	isb
+
+	ldr	r3, [r7, #PL310_AUX_CTRL]
+	tst	r3, #PL310_AUX_16WAY_BIT
+	mov	r3, #PL310_8WAYS_MASK
+	orrne	r3, #PL310_16WAYS_UPPERMASK
+	mov	r6, #PL310_LOCKDOWN_NBREGS
+	add	r5, r7, #PL310_DCACHE_LOCKDOWN_BASE
+1:	/* lock Dcache and Icache */
+	str	r3, [r5], #PL310_LOCKDOWN_SZREG
+	str	r3, [r5], #PL310_LOCKDOWN_SZREG
+	subs	r6, r6, #1
+	bne	1b
+#endif
+
+	ldr	r3, =IMX_IO_P2V(MX6Q_ANATOP_BASE_ADDR)
+	ldr	r2, =IMX_IO_P2V(MX6Q_CCM_BASE_ADDR)
+	ldr	r8, =IMX_IO_P2V(MX6Q_MMDC_P0_BASE_ADDR)
+
+	/* 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]
+
+#ifdef CONFIG_CACHE_L2X0
+	ldr	r7, =IMX_IO_P2V(MX6Q_L2_BASE_ADDR)
+	ldr	r3, [r7, #PL310_AUX_CTRL]
+	tst	r3, #PL310_AUX_16WAY_BIT
+	mov	r6, #PL310_LOCKDOWN_NBREGS
+	mov	r3, #0x00	/* 8 ways mask */
+	orrne	r3, #0x0000	/* 16 ways mask */
+	add	r5, r7, #PL310_DCACHE_LOCKDOWN_BASE
+1:	/* lock Dcache and Icache */
+	str	r3, [r5], #PL310_LOCKDOWN_SZREG
+	str	r3, [r5], #PL310_LOCKDOWN_SZREG
+	subs	r6, r6, #1
+	bne	1b
+#endif
+
+	/* Enable L1 data cache. */
+	mrc	p15, 0, r6, c1, c0, 0
+	orr	r6, r6, #0x4
+	mcr	p15, 0, r6, c1, c0, 0
+
+	/* Restore the TTBCR */
+	dsb
+	isb
+
+	/* Read TTBCR and set PD0=0, N = 0 */
+	mrc	p15, 0, r6, c2, c0, 2
+	bic	r6, r6, #0x11
+	mcr	p15, 0, r6, c2, c0, 2
+	dsb
+	isb
+
+	/* flush the TLB */
+	ldr	r6, =0x0
+	mcr	p15, 0, r6, c8, c3, 0
+
+	dsb
+	isb
+
+	/* Enable Branch Prediction, Z bit in SCTLR. */
+	mrc	p15, 0, r6, c1, c0, 0
+	orr	r6, r6, #0x800
+	mcr	p15, 0, r6, c1, c0, 0
+
+	/* Flush the Branch Target Address Cache (BTAC) */
+	ldr	r6, =0x0
+	mcr	p15, 0, r6, c7, c1, 6
+
+	nop
+	nop
+	nop
+	nop
+	nop
+
+	nop
+	nop
+	nop
+	nop
+	nop
+
+	nop
+	nop
+	nop
+	nop
+	nop
+
+	nop
+	nop
+	nop
+	nop
+	nop
+
+	nop
+	nop
+	nop
+	nop
+	nop
+
+	pop {r4-r10}
+
+	/* Restore registers */
+	mov	pc, lr
+
+	/*
+	 * Add ltorg here to ensure that all
+	 * literals are stored here and are
+	 * within the text space.
+	 */
+	.ltorg
+imx6_lpddr2_freq_change_end: