MLK-12023-3: arm: imx6q: add lpddr2 bus frequency support

Add busfreq support for imx6q lpddr2 pop target platform
DDR scaling support for low bus frequency and high bus
frequency mode (24Mhz/400Mhz)
Update Copyrigth year info

Signed-off-by: Adrian Alonso <aalonso@freescale.com>
Signed-off-by: Anson Huang <b20788@freescale.com>
(cherry picked from commit 91cff834d4f5d065fe8e7e60c1c1799f00990654)

5 files changed, 865 insertions, 9 deletions

diff --git a/arch/arm/mach-imx/Makefile b/arch/arm/mach-imx/Makefile
index 432be7d8e609..d32d2aae1492 100644
--- a/arch/arm/mach-imx/Makefile
+++ b/arch/arm/mach-imx/Makefile
@@ -99,7 +99,9 @@ endif
 obj-y += busfreq_lpddr2.o busfreq-imx.o busfreq_ddr3.o
 AFLAGS_ddr3_freq_imx6.o :=-Wa,-march=armv7-a
 AFLAGS_smp_wfe_imx6.o :=-Wa,-march=armv7-a
-obj-$(CONFIG_SOC_IMX6Q) += clk-imx6q.o mach-imx6q.o ddr3_freq_imx6.o smp_wfe_imx6.o
+AFLAGS_lpddr2_freq_imx6q.o :=-Wa,-march=armv7-a
+obj-$(CONFIG_SOC_IMX6Q) += clk-imx6q.o mach-imx6q.o ddr3_freq_imx6.o \
+			smp_wfe_imx6.o lpddr2_freq_imx6q.o
 AFLAGS_lpddr2_freq_imx6.o :=-Wa,-march=armv7-a
 obj-$(CONFIG_SOC_IMX6SL) += clk-imx6sl.o mach-imx6sl.o lpddr2_freq_imx6.o
 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 49eca5e96db5..89a5bd6815a1 100644
--- a/arch/arm/mach-imx/busfreq-imx.c
+++ b/arch/arm/mach-imx/busfreq-imx.c
@@ -40,6 +40,9 @@
 #define LOW_AUDIO_CLK		50000000
 #define HIGH_AUDIO_CLK		100000000
 
+#define MMDC_MDMISC_DDR_TYPE_DDR3	0
+#define MMDC_MDMISC_DDR_TYPE_LPDDR2	1
+
 unsigned int ddr_med_rate;
 unsigned int ddr_normal_rate;
 unsigned long ddr_freq_change_total_size;
@@ -65,12 +68,14 @@ extern unsigned long iram_tlb_phys_addr;
 extern int unsigned long iram_tlb_base_addr;
 
 extern int init_mmdc_lpddr2_settings(struct platform_device *dev);
+extern int init_mmdc_lpddr2_settings_mx6q(struct platform_device *dev);
 extern int init_mmdc_ddr3_settings_imx6_up(struct platform_device *dev);
 extern int init_mmdc_ddr3_settings_imx6_smp(struct platform_device *dev);
 extern int init_ddrc_ddr_settings(struct platform_device *dev);
 extern int update_ddr_freq_imx_smp(int ddr_rate);
 extern int update_ddr_freq_imx6_up(int ddr_rate);
 extern int update_lpddr2_freq(int ddr_rate);
+extern int update_lpddr2_freq_smp(int ddr_rate);
 
 DEFINE_MUTEX(bus_freq_mutex);
 
@@ -221,7 +226,10 @@ static void enter_lpm_imx6_smp(void)
 	if (audio_bus_count) {
 		/* Need to ensure that PLL2_PFD_400M is kept ON. */
 		clk_prepare_enable(pll2_400_clk);
-		update_ddr_freq_imx_smp(LOW_AUDIO_CLK);
+		if (ddr_type == MMDC_MDMISC_DDR_TYPE_DDR3)
+			update_ddr_freq_imx_smp(LOW_AUDIO_CLK);
+		else if (ddr_type == MMDC_MDMISC_DDR_TYPE_LPDDR2)
+			update_lpddr2_freq(LOW_AUDIO_CLK);
 		/* Make sure periph clk's parent also got updated */
 		imx_clk_set_parent(periph_clk2_sel_clk, pll3_clk);
 		imx_clk_set_parent(periph_pre_clk, pll2_200_clk);
@@ -230,7 +238,10 @@ static void enter_lpm_imx6_smp(void)
 		low_bus_freq_mode = 0;
 		cur_bus_freq_mode = BUS_FREQ_AUDIO;
 	} else {
-		update_ddr_freq_imx_smp(LPAPM_CLK);
+		if (ddr_type == MMDC_MDMISC_DDR_TYPE_DDR3)
+			update_ddr_freq_imx_smp(LPAPM_CLK);
+		else if (ddr_type == MMDC_MDMISC_DDR_TYPE_LPDDR2)
+			update_lpddr2_freq_smp(LPAPM_CLK);
 		/* Make sure periph clk's parent also got updated */
 		imx_clk_set_parent(periph_clk2_sel_clk, osc_clk);
 		/* Set periph_clk parent to OSC via periph_clk2_sel */
@@ -298,13 +309,16 @@ static void exit_lpm_imx6_smp(void)
 {
 	struct clk *periph_clk_parent;
 
-	if (cpu_is_imx6q())
+	if (cpu_is_imx6q() && ddr_type == MMDC_MDMISC_DDR_TYPE_DDR3)
 		periph_clk_parent = pll2_bus_clk;
 	else
 		periph_clk_parent = pll2_400_clk;
 
 	clk_prepare_enable(pll2_400_clk);
-	update_ddr_freq_imx_smp(ddr_normal_rate);
+	if (ddr_type == MMDC_MDMISC_DDR_TYPE_DDR3)
+		update_ddr_freq_imx_smp(ddr_normal_rate);
+	else if (ddr_type == MMDC_MDMISC_DDR_TYPE_LPDDR2)
+		update_lpddr2_freq_smp(ddr_normal_rate);
 	/* Make sure periph clk's parent also got updated */
 	imx_clk_set_parent(periph_clk2_sel_clk, pll3_clk);
 	imx_clk_set_parent(periph_pre_clk, periph_clk_parent);
@@ -1144,9 +1158,11 @@ static int busfreq_probe(struct platform_device *pdev)
 		else if (ddr_type == IMX_DDR_TYPE_LPDDR2)
 			err = init_mmdc_lpddr2_settings(pdev);
 	} else if (cpu_is_imx6q() || cpu_is_imx6dl()) {
-		err = init_mmdc_ddr3_settings_imx6_smp(pdev);
-	} else if (cpu_is_imx6q() || cpu_is_imx6dl()) {
-		err = init_mmdc_ddr3_settings_imx6_smp(pdev);
+		ddr_type = imx_mmdc_get_ddr_type();
+		if (ddr_type == MMDC_MDMISC_DDR_TYPE_DDR3)
+			err = init_mmdc_ddr3_settings_imx6_smp(pdev);
+		else if (ddr_type == MMDC_MDMISC_DDR_TYPE_LPDDR2)
+			err = init_mmdc_lpddr2_settings_mx6q(pdev);
 	} else if (cpu_is_imx6sl()) {
 		err = init_mmdc_lpddr2_settings(pdev);
 	}
diff --git a/arch/arm/mach-imx/busfreq_lpddr2.c b/arch/arm/mach-imx/busfreq_lpddr2.c
index 076ec8400a76..8b6de0839352 100644
--- a/arch/arm/mach-imx/busfreq_lpddr2.c
+++ b/arch/arm/mach-imx/busfreq_lpddr2.c
@@ -26,10 +26,12 @@
 #include <asm/tlb.h>
 #include <linux/busfreq-imx.h>
 #include <linux/clk.h>
+#include <linux/clockchips.h>
 #include <linux/cpumask.h>
 #include <linux/delay.h>
 #include <linux/genalloc.h>
 #include <linux/interrupt.h>
+#include <linux/irq.h>
 #include <linux/irqchip/arm-gic.h>
 #include <linux/kernel.h>
 #include <linux/mutex.h>
@@ -41,9 +43,9 @@
 #include <linux/sched.h>
 #include <linux/smp.h>
 
+#include "common.h"
 #include "hardware.h"
 
-
 static struct device *busfreq_dev;
 static int curr_ddr_rate;
 static DEFINE_SPINLOCK(freq_lock);
@@ -55,9 +57,45 @@ extern void mx6_lpddr2_freq_change(u32 freq, int bus_freq_mode);
 extern void imx6_up_lpddr2_freq_change(u32 freq, int bus_freq_mode);
 extern unsigned long save_ttbr1(void);
 extern void restore_ttbr1(unsigned long ttbr1);
+extern void mx6q_lpddr2_freq_change(u32 freq, int bus_freq_mode);
 extern unsigned long ddr_freq_change_iram_base;
 extern unsigned long imx6_lpddr2_freq_change_start asm("imx6_lpddr2_freq_change_start");
 extern unsigned long imx6_lpddr2_freq_change_end asm("imx6_lpddr2_freq_change_end");
+extern unsigned long mx6q_lpddr2_freq_change_start asm("mx6q_lpddr2_freq_change_start");
+extern unsigned long mx6q_lpddr2_freq_change_end asm("mx6q_lpddr2_freq_change_end");
+extern unsigned long iram_tlb_phys_addr;
+
+#ifdef CONFIG_SMP
+volatile u32 *wait_for_lpddr2_freq_update;
+static unsigned int online_cpus;
+static u32 *irqs_used;
+void (*wfe_change_lpddr2_freq)(u32 cpuid, u32 *ddr_freq_change_done);
+extern void wfe_smp_freq_change(u32 cpuid, u32 *ddr_freq_change_done);
+extern unsigned long wfe_smp_freq_change_start asm("wfe_smp_freq_change_start");
+extern unsigned long wfe_smp_freq_change_end asm("wfe_smp_freq_change_end");
+extern void __iomem *imx_scu_base;
+static void __iomem *gic_dist_base;
+#endif
+
+#define SMP_WFE_CODE_SIZE 0x400
+
+#ifdef CONFIG_SMP
+static irqreturn_t wait_in_wfe_irq(int irq, void *dev_id)
+{
+	u32 me;
+
+	me = smp_processor_id();
+#ifdef CONFIG_LOCAL_TIMERS
+	clockevents_notify(CLOCK_EVT_NOTIFY_BROADCAST_ENTER, &me);
+#endif
+	wfe_change_lpddr2_freq(0xff << (me * 8),
+			(u32 *)ddr_freq_change_iram_base);
+#ifdef CONFIG_LOCAL_TIMERS
+	clockevents_notify(CLOCK_EVT_NOTIFY_BROADCAST_EXIT, &me);
+#endif
+	return IRQ_HANDLED;
+}
+#endif
 
 /* change the DDR frequency. */
 int update_lpddr2_freq(int ddr_rate)
@@ -110,3 +148,152 @@ int init_mmdc_lpddr2_settings(struct platform_device *busfreq_pdev)
 
 	return 0;
 }
+
+int update_lpddr2_freq_smp(int ddr_rate)
+{
+	unsigned long ttbr1;
+	int mode = get_bus_freq_mode();
+#ifdef CONFIG_SMP
+	int cpu = 0;
+	int me = 0;
+	u32 reg;
+#endif
+
+	if (ddr_rate == curr_ddr_rate)
+		return 0;
+
+	printk(KERN_DEBUG "Bus freq set to %d start...\n", ddr_rate);
+
+	/* ensure that all Cores are in WFE. */
+	local_irq_disable();
+
+#ifdef CONFIG_SMP
+	me = smp_processor_id();
+
+	/* Make sure all the online cores are active */
+	while (1) {
+		bool not_exited_busfreq = false;
+		for_each_online_cpu(cpu) {
+			reg = __raw_readl(imx_scu_base + 0x08);
+			if (reg & (0x02 << (cpu * 8)))
+				not_exited_busfreq = true;
+		}
+		if (!not_exited_busfreq)
+			break;
+	}
+
+	wmb();
+	*wait_for_lpddr2_freq_update = 1;
+	dsb();
+	online_cpus = readl_relaxed(imx_scu_base + 0x08);
+	for_each_online_cpu(cpu) {
+		*((char *)(&online_cpus) + (u8)cpu) = 0x02;
+		if (cpu != me) {
+			reg = 1 << (irqs_used[cpu] % 32);
+			writel_relaxed(reg, gic_dist_base + GIC_DIST_PENDING_SET
+					+ (irqs_used[cpu] / 32) * 4);
+		}
+	}
+
+	/* Wait for the other active CPUs to idle */
+	while (1) {
+		reg = readl_relaxed(imx_scu_base + 0x08);
+		reg |= (0x02 << (me * 8));
+		if (reg == online_cpus)
+			break;
+	}
+#endif
+
+	/* Ensure iram_tlb_phys_addr is flushed to DDR. */
+	__cpuc_flush_dcache_area(&iram_tlb_phys_addr,
+			sizeof(iram_tlb_phys_addr));
+	outer_clean_range(__pa(&iram_tlb_phys_addr),
+			__pa(&iram_tlb_phys_addr + 1));
+	/*
+	 * Flush the TLB, to ensure no TLB maintenance occurs
+	 * when DDR is in self-refresh.
+	 */
+	ttbr1 = save_ttbr1();
+
+	/* Now change DDR frequency. */
+	mx6_change_lpddr2_freq(ddr_rate,
+		(mode == BUS_FREQ_LOW || mode == BUS_FREQ_ULTRA_LOW) ? 1 : 0);
+
+	restore_ttbr1(ttbr1);
+
+	curr_ddr_rate = ddr_rate;
+
+#ifdef CONFIG_SMP
+	wmb();
+	/* DDR frequency change is done . */
+	*wait_for_lpddr2_freq_update = 0;
+	dsb();
+	/* wake up all the cores. */
+	sev();
+#endif
+
+	local_irq_enable();
+	printk(KERN_DEBUG "Bus freq set to %d done! cpu=%d\n", ddr_rate, me);
+
+	return 0;
+}
+
+int init_mmdc_lpddr2_settings_mx6q(struct platform_device *busfreq_pdev)
+{
+	unsigned long ddr_code_size;
+#ifdef CONFIG_SMP
+	struct device *dev = &busfreq_pdev->dev;
+	struct device_node *node;
+	struct irq_data *d;
+	u32 cpu;
+	int err;
+
+	node = of_find_compatible_node(NULL, NULL, "arm,cortex-a9-gic");
+	if (!node) {
+		printk(KERN_ERR "failed to find imx6q-a9-gic device tree data!\n");
+		return -EINVAL;
+	}
+
+	gic_dist_base = of_iomap(node, 0);
+	WARN(!gic_dist_base, "unable to map gic dist registers\n");
+
+	irqs_used = devm_kzalloc(dev, sizeof(u32) * num_present_cpus(),
+			GFP_KERNEL);
+
+	for_each_online_cpu(cpu) {
+		int irq = platform_get_irq(busfreq_pdev, cpu);
+		err = request_irq(irq, wait_in_wfe_irq, IRQF_PERCPU,
+				"mmdc_1", NULL);
+		if (err) {
+			dev_err(dev,
+				"Busfreq:request_irq failed %d, err = %d\n",
+				irq, err);
+			return err;
+		}
+		err = irq_set_affinity(irq, cpumask_of(cpu));
+		if (err) {
+			dev_err(dev,
+				"Busfreq: Cannot set irq affinity irq=%d,\n",
+				irq);
+			return err;
+		}
+		d = irq_get_irq_data(irq);
+		irqs_used[cpu] = d->hwirq + 32;
+	}
+
+	/* Stoange_iram_basee the variable used to communicate between cores in
+	 * a non-cacheable IRAM area */
+	wait_for_lpddr2_freq_update = (u32 *)ddr_freq_change_iram_base;
+	wfe_change_lpddr2_freq = (void *)fncpy((void *)ddr_freq_change_iram_base + 0x8,
+			&wfe_smp_freq_change, SMP_WFE_CODE_SIZE - 0x8);
+#endif
+	ddr_code_size = (&mx6q_lpddr2_freq_change_end -&mx6q_lpddr2_freq_change_start) *4;
+
+	mx6_change_lpddr2_freq = (void *)fncpy(
+			(void *)ddr_freq_change_iram_base + SMP_WFE_CODE_SIZE,
+			&mx6q_lpddr2_freq_change, ddr_code_size);
+
+	curr_ddr_rate = ddr_normal_rate;
+
+	return 0;
+}
diff --git a/arch/arm/mach-imx/common.c b/arch/arm/mach-imx/common.c
index 397a2e7dfc7e..b240b449d889 100644
--- a/arch/arm/mach-imx/common.c
+++ b/arch/arm/mach-imx/common.c
@@ -135,9 +135,11 @@ void imx6_up_lpddr2_freq_change(u32 freq, int bus_freq_mode) {}
 
 #if !defined(CONFIG_SOC_IMX6Q)
 u32 mx6_ddr3_freq_change_start, mx6_ddr3_freq_change_end;
+u32 mx6q_lpddr2_freq_change_start, mx6q_lpddr2_freq_change_end;
 u32 wfe_smp_freq_change_start, wfe_smp_freq_change_end;
 void mx6_ddr3_freq_change(u32 freq, void *ddr_settings,
 	bool dll_mode, void *iomux_offsets) {}
+void mx6q_lpddr2_freq_change(u32 freq, int bus_freq_mode) {}
 void wfe_smp_freq_change(u32 cpuid, u32 *ddr_freq_change_done) {}
 #endif
 
diff --git a/arch/arm/mach-imx/lpddr2_freq_imx6q.S b/arch/arm/mach-imx/lpddr2_freq_imx6q.S
new file mode 100644
index 000000000000..8d1605765985
--- /dev/null
+++ b/arch/arm/mach-imx/lpddr2_freq_imx6q.S
@@ -0,0 +1,649 @@
+/*
+ * Copyright (C) 2015 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 <asm/smp_scu.h>
+#include "hardware.h"
+
+#define CCM_CBCDR 		0x14
+#define CCM_CBCMR 		0x18
+#define CCM_CSCMR1 		0x1c
+#define CCM_CDHIPR 		0x48
+
+.globl mx6q_lpddr2_freq_change_start
+.globl mx6q_lpddr2_freq_change_end
+
+	.macro	switch_to_400MHz
+
+	/* check if periph_clk_sel is already set. */
+	ldr	r9, [r2, #CCM_CBCDR]
+	and	r9, r9, #(1 << 25)
+	cmp	r9, #(1 << 25)
+	beq	set_ahb_podf_before_switch1
+
+	/* change periph_clk to be sourced from pll3_clk. */
+	ldr	r9, [r2, #CCM_CBCMR]
+	bic	r9, r9, #(3 << 12)
+	str	r9, [r2, #CCM_CBCMR]
+
+	ldr	r9, [r2, #CCM_CBCDR]
+	bic	r9, r9, #(0x38 << 24)
+	str	r9, [r2, #CCM_CBCDR]
+
+	/* now switch periph_clk to pll3_main_clk. */
+	ldr	r9, [r2, #CCM_CBCDR]
+	orr	r9, r9, #(1 << 25)
+	str	r9, [r2, #CCM_CBCDR]
+
+periph_clk_switch5:
+	ldr	r9, [r2, #CCM_CDHIPR]
+	cmp	r9, #0
+	bne	periph_clk_switch5
+
+	b	switch_pre_periph_clk_400
+
+set_ahb_podf_before_switch1:
+	/*
+	 * set the MMDC_DIV=1, AXI_DIV = 2, AHB_DIV=4,
+	 */
+	ldr	r9, [r2, #CCM_CBCDR]
+	ldr	r6, =0x3f1f00
+	bic	r9, r9, r6
+	orr	r9, r9, #(0x9 << 8)
+	orr	r9, r9, #(1 << 16)
+	str	r9, [r2, #CCM_CBCDR]
+
+wait_div_update400_1:
+	ldr	r9, [r2, #CCM_CDHIPR]
+	cmp	r9, #0
+	bne	wait_div_update400_1
+
+switch_pre_periph_clk_400:
+
+	/* now switch pre_periph_clk to PFD_400MHz. */
+	ldr	r9, [r2, #CCM_CBCMR]
+	bic	r9, r9, #(0xc << 16)
+	orr	r9, r9, #(0x4 << 16)
+	str	r9, [r2, #CCM_CBCMR]
+
+	/* now switch periph_clk back. */
+	ldr	r9, [r2, #CCM_CBCDR]
+	bic	r9, r9, #(1 << 25)
+	str	r9, [r2, #CCM_CBCDR]
+
+periph_clk_switch6:
+	ldr	r9, [r2, #CCM_CDHIPR]
+	cmp	r9, #0
+	bne	periph_clk_switch6
+
+	.endm
+
+	.macro	switch_to_24MHz
+	/*
+	 * change the freq now try setting DDR to 24MHz.
+	 * source it from the periph_clk2 ensure the
+	 * periph_clk2 is sourced from 24MHz and the
+	 * divider is 1.
+	 */
+
+	ldr	r9, [r2, #CCM_CBCMR]
+	bic	r9, r9, #(0x3 << 12)
+	orr	r9, r9, #(1 << 12)
+	str	r9, [r2, #CCM_CBCMR]
+
+	ldr	r9, [r2, #CCM_CBCDR]
+	bic	r9, r9, #(0x38 << 24)
+	str	r9, [r2, #CCM_CBCDR]
+
+	/* now switch periph_clk to 24MHz. */
+	ldr	r9, [r2, #CCM_CBCDR]
+	orr	r9, r9, #(1 << 25)
+	str	r9, [r2, #CCM_CBCDR]
+
+periph_clk_switch1:
+	ldr	r9, [r2, #CCM_CDHIPR]
+	cmp	r9, #0
+	bne	periph_clk_switch1
+
+	/* change all the dividers to 1. */
+	ldr	r9, [r2, #CCM_CBCDR]
+	ldr	r6, =0x3f1f00
+	bic	r9, r9, r6
+	orr	r9, r9, #(1 << 8)
+	str	r9, [r2, #CCM_CBCDR]
+
+	/* Wait for the divider to change. */
+wait_div_update:
+	ldr	r9, [r2, #CCM_CDHIPR]
+	cmp	r9, #0
+	bne	wait_div_update
+
+	.endm
+
+	.macro 	switch_to_24MHZ_from_pll2
+	/* Change DDR freq settings from pll2_pfd2 (div 2) */
+
+	ldr	r9, [r2, #CCM_CBCMR]
+	bic	r9, r9, #(0x3 << 18)
+	orr	r9, r9, #(0x3 << 18)
+	str	r9, [r2, #CCM_CBCMR]
+
+	ldr	r9, [r2, #CCM_CBCDR]
+	bic	r9, r9, #(1 << 25)
+	str	r9, [r2, #CCM_CBCDR]
+
+periph_clk_switch_pll2_pfd2:
+	ldr	r9, [r2, #CCM_CDHIPR]
+	cmp	r9, #0
+	bne periph_clk_switch_pll2_pfd2
+
+	ldr	r9, [r2, #CCM_CBCDR]
+	ldr	r6, =0x3f1f00
+	bic	r9, r9, r6
+	orr	r9, r9, #(1 << 8)
+	orr	r9, r9, #(0x7 << 19)
+	str	r9, [r2, #CCM_CBCDR]
+
+wait_div_update2:
+	ldr	r9, [r2, #CCM_CDHIPR]
+	cmp	r9, #0
+	bne	wait_div_update2
+
+	.endm
+
+	.macro	mmdc_clk_lower_100MHz
+
+	/* Set MMDCx_MISC[RALAT] = 2 cycles */
+	ldr	r6, [r8, #0x18]
+	bic	r6, r6, #(0x7 << 6)
+	orr	r6, r6, #(0x2 << 6)
+	str	r6, [r8, #0x18]
+
+	ldr	r6, [r4, #0x18]
+	bic	r6, r6, #(0x7 << 6)
+	orr	r6, r6, #(0x2 << 6)
+	str	r6, [r4, #0x18]
+
+	/* Adjust LPDDR2 timmings for 24Mhz operation */
+	ldr	r6, =0x03032073
+	str	r6, [r8, #0xC]  	/* MMDC0_MDCFG0 */
+	str	r6, [r4, #0xC]  	/* MMDC1_MDCFG0 */
+	ldr	r6, =0x00020482
+	str	r6, [r8, #0x10] 	/* MMDC0_MDCFG1 */
+	str	r6, [r4, #0x10] 	/* MMDC1_MDCFG1 */
+	ldr	r6, =0x00000049
+	str	r6, [r8, #0x14] 	/* MMDC0_MDCFG2 */
+	str	r6, [r4, #0x14] 	/* MMDC1_MDCFG2 */
+	ldr	r6, =0x00020333
+	str	r6, [r8, #0x38] 	/* MMDC0_MDCFG3LP */
+	str	r6, [r4, #0x38] 	/* MMDC1_MDCFG3LP */
+
+	/*
+	 * 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	r9, =0x3FF
+	and	r6, r6, r9
+	/* 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	r9, =0x3FF
+	bic	r6, r6, r9
+	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
+
+	ldr	r5, =0x8B8
+	ldr	r6, [r4, r5]
+	/* Original MU unit count */
+	mov	r6, r6, LSR #16
+	ldr	r9, =0x3FF
+	and	r6, r6, r9
+	/* 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, [r4, r5]
+	orr	r6, r6, #0x400
+	str	r6, [r4, 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, [r4, r5]
+	ldr	r9, =0x3FF
+	bic	r6, r6, r9
+	orr	r6, r6, r7
+	str	r6, [r4, r5]
+	/* Now perform a Force Measurement. */
+	ldr	r6, [r4, r5]
+	orr	r6, r6, #0x800
+	str	r6, [r4, r5]
+	/* Wait for FRC_MSR to clear. */
+force_measure_ch1:
+	ldr	r6, [r4, r5]
+	and	r6, r6, #0x800
+	cmp	r6, #0x0
+	bne	force_measure_ch1
+
+	.endm
+
+	.macro	mmdc_clk_above_100MHz
+
+	/* Set MMDCx_MISC[RALAT] = 5 cycles */
+	ldr	r6, [r8, #0x18]
+	bic	r6, r6, #(0x7 << 6)
+	orr	r6, r6, #(0x5 << 6)
+	str	r6, [r8, #0x18]
+
+	ldr	r6, [r4, #0x18]
+	bic	r6, r6, #(0x7 << 6)
+	orr	r6, r6, #(0x5 << 6)
+	str	r6, [r4, #0x18]
+
+	/* Adjust LPDDR2 timmings for 400Mhz operation */
+	ldr	r6, =0x33374133
+	str	r6, [r8, #0xC]  	/* MMDC0_MDCFG0 */
+	str	r6, [r4, #0xC]  	/* MMDC1_MDCFG0 */
+	ldr	r6, =0x00100A82
+	str	r6, [r8, #0x10] 	/* MMDC0_MDCFG1 */
+	str	r6, [r4, #0x10] 	/* MMDC1_MDCFG1 */
+	ldr	r6, =0x00000093
+	str	r6, [r8, #0x14] 	/* MMDC0_MDCFG2 */
+	str	r6, [r4, #0x14] 	/* MMDC1_MDCFG2 */
+	ldr	r6, =0x001A0889
+	str	r6, [r8, #0x38] 	/* MMDC0_MDCFG3LP */
+	str	r6, [r4, #0x38] 	/* MMDC1_MDCFG3LP */
+
+	/* 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
+
+	ldr	r5, =0x8B8
+	ldr	r6, [r4, r5]
+	bic	r6, r6, #0x400
+	str	r6, [r4, r5]
+	/* Now perform a Force Measurement. */
+	ldr	r6, [r4, r5]
+	orr	r6, r6, #0x800
+	str	r6, [r4, r5]
+	/* Wait for FRC_MSR to clear. */
+force_measure1_ch1:
+	ldr	r6, [r4, r5]
+	and	r6, r6, #0x800
+	cmp	r6, #0x0
+	bne	force_measure1_ch1
+	.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(mx6q_lpddr2_freq_change)
+mx6q_lpddr2_freq_change_start:
+	push	{r2-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]
+
+	/* Flush the Branch Target Address Cache (BTAC) */
+	ldr	r6, =0x0
+	mcr	p15, 0, r6, c7, c1, 6
+
+	/* Disable Branch Prediction, Z bit in SCTLR. */
+	mrc	p15, 0, r6, c1, c0, 0
+	bic	r6, r6, #0x800
+	mcr	p15, 0, r6, c1, c0, 0
+
+	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
+
+	/* Disable L2. */
+	str	r6, [r7, #0x100]
+#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)
+	ldr r4, =IMX_IO_P2V(MX6Q_MMDC_P1_BASE_ADDR)
+
+	/* Disable Automatic power savings. */
+	ldr	r6, [r8, #0x404]
+	orr	r6, r6, #0x01
+	str	r6, [r8, #0x404]
+
+	ldr	r6, [r4, #0x404]
+	orr	r6, r6, #0x01
+	str	r6, [r4, #0x404]
+
+	/* MMDC0_MDPDC disable power down timer */
+	ldr	r6, [r8, #0x4]
+	bic	r6, r6, #0xff00
+	str	r6, [r8, #0x4]
+
+	ldr	r6, [r4, #0x4]
+	bic	r6, r6, #0xff00
+	str	r6, [r4, #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
+
+	ldr	r6, [r4, #0x404]
+	orr	r6, r6, #0x200000
+	str	r6, [r4, #0x404]
+
+poll_dvfs_set_2:
+	ldr	r6, [r4, #0x404]
+	and	r6, r6, #0x2000000
+	cmp	r6, #0x2000000
+	bne	poll_dvfs_set_2
+
+
+	/* set SBS step-by-step mode */
+	ldr	r6, [r8, #0x410]
+	orr	r6, r6, #0x100
+	str	r6, [r8, #0x410]
+
+	ldr	r6, [r4, #0x410]
+	orr	r6, r6, #0x100
+	str	r6, [r4, #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
+
+	ldr r10, =400000000
+	cmp r0, r10
+	switch_to_400MHz
+	b	done
+
+set_to_24MHz:
+/*
+	switch_to_24MHZ_from_pll2
+*/
+	switch_to_24MHz
+
+done:
+
+	ldr	r10,=100000000
+	cmp	r0, r10
+	blt	skip_mmdc_clk_check
+	mmdc_clk_above_100MHz
+
+skip_mmdc_clk_check:
+
+	/* 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
+
+	ldr	r6, [r4, #0x404]
+	bic	r6, r6, #0x200000
+	str	r6, [r4, #0x404]
+
+poll_dvfs_clear_2:
+	ldr	r6, [r4, #0x404]
+	and	r6, r6, #0x2000000
+	cmp	r6, #0x2000000
+	beq	poll_dvfs_clear_2
+
+	/* Enable Automatic power savings. */
+	ldr	r6, [r8, #0x404]
+	bic	r6, r6, #0x01
+	str	r6, [r8, #0x404]
+
+	ldr	r6, [r4, #0x404]
+	bic	r6, r6, #0x01
+	str	r6, [r4, #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]
+
+	ldr	r6, [r4, #0x4]
+	orr	r6, r6, #0x5500
+	str	r6, [r4, #0x4]
+
+skip_power_down:
+	/* clear SBS - unblock DDR accesses */
+	ldr	r6, [r8, #0x410]
+	bic	r6, r6, #0x100
+	str	r6, [r8, #0x410]
+
+	ldr	r6, [r4, #0x410]
+	bic	r6, r6, #0x100
+	str	r6, [r4, #0x410]
+
+#ifdef CONFIG_CACHE_L2X0
+	/* Enable L2. */
+	ldr	r7, =IMX_IO_P2V(MX6Q_L2_BASE_ADDR)
+	ldr	r6, =0x1
+	str	r6, [r7, #0x100]
+#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	{r2-r10}
+
+	/* Restore registers */
+	mov	pc, lr
+
+	/*
+	 * Add ltorg here to ensure that all
+	 * literals are stored here and are
+	 * within the text space.
+	 */
+	.ltorg
+mx6q_lpddr2_freq_change_end: