ARM: imx: add i.MX6SX bus-freq support

This patch adds i.MX6SX bus-freq support.

Signed-off-by: Anson Huang <Anson.Huang@nxp.com>

8 files changed, 1584 insertions, 3 deletions

diff --git a/arch/arm/mach-imx/Makefile b/arch/arm/mach-imx/Makefile
index a3db687d8c07..c24cdd8af09a 100644
--- a/arch/arm/mach-imx/Makefile
+++ b/arch/arm/mach-imx/Makefile
@@ -78,24 +78,26 @@ AFLAGS_headsmp.o :=-Wa,-march=armv7-a
 obj-$(CONFIG_SMP) += headsmp.o platsmp.o
 obj-$(CONFIG_HOTPLUG_CPU) += hotplug.o
 endif
-obj-$(CONFIG_SOC_IMX6Q) += mach-imx6q.o ddr3_freq_imx6.o smp_wfe_imx6.o busfreq_lpddr2.o \
+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_IMX6SLL) += mach-imx6sl.o
-obj-$(CONFIG_SOC_IMX6SX) += mach-imx6sx.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
 obj-$(CONFIG_SOC_IMX7D_CA7) += mach-imx7d.o pm-imx7.o ddr3_freq_imx7d.o smp_wfe.o \
 			   lpddr3_freq_imx.o
 obj-$(CONFIG_SOC_IMX7D_CM4) += mach-imx7d-cm4.o
 obj-$(CONFIG_SOC_IMX7ULP) += mach-imx7ulp.o pm-imx7ulp.o
 
-obj-y += busfreq-imx.o busfreq_ddr3.o
+obj-y += busfreq-imx.o busfreq_ddr3.o busfreq_lpddr2.o
 AFLAGS_smp_wfe.o :=-Wa,-march=armv7-a
 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_imx6q.o :=-Wa,-march=armv7-a
+AFLAGS_lpddr2_freq_imx6sx.o :=-Wa,-march=armv7-a
+AFLAGS_ddr3_freq_imx6sx.o :=-Wa,-march=armv7-a
 
 ifeq ($(CONFIG_SUSPEND),y)
 AFLAGS_suspend-imx6.o :=-Wa,-march=armv7-a
diff --git a/arch/arm/mach-imx/busfreq-imx.c b/arch/arm/mach-imx/busfreq-imx.c
index 62b36e94a12c..0dea0fff3665 100644
--- a/arch/arm/mach-imx/busfreq-imx.c
+++ b/arch/arm/mach-imx/busfreq-imx.c
@@ -120,6 +120,12 @@ static struct clk *pll2_bus_clk;
 
 static struct clk *pll3_pfd1_540m_clk;
 
+static struct clk *ocram_clk;
+static struct clk *periph2_clk;
+static struct clk *periph2_pre_clk;
+static struct clk *periph2_clk2_clk;
+static struct clk *periph2_clk2_sel_clk;
+
 static struct delayed_work low_bus_freq_handler;
 static struct delayed_work bus_freq_daemon;
 
@@ -155,6 +161,65 @@ int unregister_busfreq_notifier(struct notifier_block *nb)
 }
 EXPORT_SYMBOL(unregister_busfreq_notifier);
 
+/*
+ * enter_lpm_imx6_up and exit_lpm_imx6_up is used by
+ * i.MX6SX/i.MX6UL for entering and exiting lpm mode.
+ */
+static void enter_lpm_imx6_up(void)
+{
+	/* set periph_clk2 to source from OSC for periph */
+	clk_set_parent(periph_clk2_sel_clk, osc_clk);
+	clk_set_parent(periph_clk, periph_clk2_clk);
+	/* set ahb/ocram to 24MHz */
+	clk_set_rate(ahb_clk, LPAPM_CLK);
+	clk_set_rate(ocram_clk, LPAPM_CLK);
+
+	if (audio_bus_count) {
+		/* Need to ensure that PLL2_PFD_400M is kept ON. */
+		clk_prepare_enable(pll2_400_clk);
+		if (ddr_type == IMX_DDR_TYPE_DDR3)
+			busfreq_func.update(LOW_AUDIO_CLK);
+		else if (ddr_type == IMX_DDR_TYPE_LPDDR2 ||
+			 ddr_type == IMX_MMDC_DDR_TYPE_LPDDR3)
+			busfreq_func.update(HIGH_AUDIO_CLK);
+		clk_set_parent(periph2_clk2_sel_clk, pll3_clk);
+		clk_set_parent(periph2_pre_clk, pll2_400_clk);
+		clk_set_parent(periph2_clk, periph2_pre_clk);
+		/*
+		 * As periph2_clk's parent is not changed from
+		 * high mode to audio mode, so clk framework
+		 * will not update its children's freq, but we
+		 * change the mmdc's podf in asm code, so here
+		 * need to update mmdc rate to make sure clk
+		 * tree is right, although it will not do any
+		 * change to hardware.
+		 */
+		if (high_bus_freq_mode) {
+			if (ddr_type == IMX_DDR_TYPE_DDR3)
+				clk_set_rate(mmdc_clk, LOW_AUDIO_CLK);
+			else if (ddr_type == IMX_DDR_TYPE_LPDDR2 ||
+				 ddr_type == IMX_MMDC_DDR_TYPE_LPDDR3)
+				clk_set_rate(mmdc_clk, HIGH_AUDIO_CLK);
+		}
+
+		audio_bus_freq_mode = 1;
+		low_bus_freq_mode = 0;
+		cur_bus_freq_mode = BUS_FREQ_AUDIO;
+	} else {
+		busfreq_func.update(LPAPM_CLK);
+
+		clk_set_parent(periph2_clk2_sel_clk, osc_clk);
+		clk_set_parent(periph2_clk, periph2_clk2_clk);
+
+		if (audio_bus_freq_mode)
+			clk_disable_unprepare(pll2_400_clk);
+
+		low_bus_freq_mode = 1;
+		audio_bus_freq_mode = 0;
+		cur_bus_freq_mode = BUS_FREQ_LOW;
+	}
+}
+
 static void enter_lpm_imx6_smp(void)
 {
 	if (cpu_is_imx6dl())
@@ -208,6 +273,48 @@ static void enter_lpm_imx6_smp(void)
 	}
 }
 
+static void exit_lpm_imx6_up(void)
+{
+	clk_prepare_enable(pll2_400_clk);
+
+	/*
+	 * lower ahb/ocram's freq first to avoid too high
+	 * freq during parent switch from OSC to pll3.
+	 */
+	clk_set_rate(ahb_clk, LPAPM_CLK / 3);
+
+	clk_set_rate(ocram_clk, LPAPM_CLK / 2);
+	/* set periph clk to from pll2_400 */
+	clk_set_parent(periph_pre_clk, pll2_400_clk);
+	clk_set_parent(periph_clk, periph_pre_clk);
+	/* set periph_clk2 to pll3 */
+	clk_set_parent(periph_clk2_sel_clk, pll3_clk);
+
+	busfreq_func.update(ddr_normal_rate);
+
+	/* correct parent info after ddr freq change in asm code */
+	clk_set_parent(periph2_pre_clk, pll2_400_clk);
+	clk_set_parent(periph2_clk, periph2_pre_clk);
+	clk_set_parent(periph2_clk2_sel_clk, pll3_clk);
+
+	/*
+	 * As periph2_clk's parent is not changed from
+	 * audio mode to high mode, so clk framework
+	 * will not update its children's freq, but we
+	 * change the mmdc's podf in asm code, so here
+	 * need to update mmdc rate to make sure clk
+	 * tree is right, although it will not do any
+	 * change to hardware.
+	 */
+	if (audio_bus_freq_mode)
+		clk_set_rate(mmdc_clk, ddr_normal_rate);
+
+	clk_disable_unprepare(pll2_400_clk);
+
+	if (audio_bus_freq_mode)
+		clk_disable_unprepare(pll2_400_clk);
+}
+
 static void exit_lpm_imx6_smp(void)
 {
 	struct clk *periph_clk_parent;
@@ -315,6 +422,8 @@ static void reduce_bus_freq(void)
 
 	if (cpu_is_imx7d())
 		enter_lpm_imx7d();
+	else if (cpu_is_imx6sx())
+		enter_lpm_imx6_up();
 	else if (cpu_is_imx6q() || cpu_is_imx6dl())
 		enter_lpm_imx6_smp();
 
@@ -415,6 +524,8 @@ static int set_high_bus_freq(int high_bus_freq)
 
 	if (cpu_is_imx7d())
 		exit_lpm_imx7d();
+	else if (cpu_is_imx6sx())
+		exit_lpm_imx6_up();
 	else if (cpu_is_imx6q() || cpu_is_imx6dl())
 		exit_lpm_imx6_smp();
 
@@ -764,6 +875,34 @@ static int busfreq_probe(struct platform_device *pdev)
 			return -EINVAL;
 		}
 	}
+
+	if (cpu_is_imx6sx()) {
+		ahb_clk = devm_clk_get(&pdev->dev, "ahb");
+		ocram_clk = devm_clk_get(&pdev->dev, "ocram");
+		periph2_clk = devm_clk_get(&pdev->dev, "periph2");
+		periph2_pre_clk = devm_clk_get(&pdev->dev, "periph2_pre");
+		periph2_clk2_clk = devm_clk_get(&pdev->dev, "periph2_clk2");
+		periph2_clk2_sel_clk =
+			devm_clk_get(&pdev->dev, "periph2_clk2_sel");
+		if (IS_ERR(ahb_clk) || IS_ERR(ocram_clk)
+			|| IS_ERR(periph2_clk) || IS_ERR(periph2_pre_clk)
+			|| IS_ERR(periph2_clk2_clk)
+			|| IS_ERR(periph2_clk2_sel_clk)) {
+			dev_err(busfreq_dev,
+				"%s: failed to get busfreq clk for imx6ul/sx/sl.\n", __func__);
+			return -EINVAL;
+		}
+	}
+
+	if (cpu_is_imx6sx()) {
+		mmdc_clk = devm_clk_get(&pdev->dev, "mmdc");
+		if (IS_ERR(mmdc_clk)) {
+			dev_err(busfreq_dev,
+				"%s: failed to get mmdc clk for imx6sx/ul.\n", __func__);
+			return -EINVAL;
+		}
+	}
+
 	if (cpu_is_imx6q()) {
 		mmdc_clk = devm_clk_get(&pdev->dev, "mmdc");
 		if (IS_ERR(mmdc_clk)) {
@@ -860,6 +999,16 @@ static int busfreq_probe(struct platform_device *pdev)
 		}
 		busfreq_func.init   = &init_ddrc_ddr_settings;
 		busfreq_func.update = &update_ddr_freq_imx_smp;
+	} else if (cpu_is_imx6sx()) {
+		ddr_type = imx_mmdc_get_ddr_type();
+		if (ddr_type == IMX_DDR_TYPE_DDR3) {
+			busfreq_func.init   = &init_mmdc_ddr3_settings_imx6_up;
+			busfreq_func.update = &update_ddr_freq_imx6_up;
+		} else if (ddr_type == IMX_DDR_TYPE_LPDDR2 ||
+			 ddr_type == IMX_MMDC_DDR_TYPE_LPDDR3) {
+			busfreq_func.init   = &init_mmdc_lpddr2_settings;
+			busfreq_func.update = &update_lpddr2_freq;
+		}
 	} else if (cpu_is_imx6q() || cpu_is_imx6dl()) {
 		ddr_type = imx_mmdc_get_ddr_type();
 		if (ddr_type == MMDC_MDMISC_DDR_TYPE_DDR3) {
diff --git a/arch/arm/mach-imx/busfreq_ddr3.c b/arch/arm/mach-imx/busfreq_ddr3.c
index 53c9d3b3462c..3aa28f9e4853 100644
--- a/arch/arm/mach-imx/busfreq_ddr3.c
+++ b/arch/arm/mach-imx/busfreq_ddr3.c
@@ -72,6 +72,8 @@ struct imx6_busfreq_info {
 	u32 mu_delay_val;
 } __aligned(8);
 
+static struct imx6_busfreq_info *imx6_busfreq_info;
+
 /* DDR settings */
 static unsigned long (*iram_ddr_settings)[2];
 static unsigned long (*normal_mmdc_settings)[2];
@@ -85,6 +87,8 @@ static int ddr_settings_size;
 static int iomux_settings_size;
 static int curr_ddr_rate;
 
+void (*imx6_up_change_ddr_freq)(struct imx6_busfreq_info *busfreq_info);
+extern void imx6_up_ddr3_freq_change(struct imx6_busfreq_info *busfreq_info);
 void (*imx7d_change_ddr_freq)(u32 freq) = NULL;
 extern void imx7d_ddr3_freq_change(u32 freq);
 extern void imx_lpddr3_freq_change(u32 freq);
@@ -107,6 +111,9 @@ extern unsigned long iram_tlb_phys_addr;
 
 extern unsigned long mx6_ddr3_freq_change_start asm("mx6_ddr3_freq_change_start");
 extern unsigned long mx6_ddr3_freq_change_end asm("mx6_ddr3_freq_change_end");
+extern unsigned long imx6_up_ddr3_freq_change_start asm("imx6_up_ddr3_freq_change_start");
+extern unsigned long imx6_up_ddr3_freq_change_end asm("imx6_up_ddr3_freq_change_end");
+
 #ifdef CONFIG_SMP
 static unsigned long wfe_freq_change_iram_base;
 volatile u32 *wait_for_ddr_freq_update;
@@ -122,6 +129,30 @@ extern unsigned long wfe_smp_freq_change_end asm("wfe_smp_freq_change_end");
 extern void __iomem *scu_base;
 #endif
 
+unsigned long ddr3_dll_mx6sx[][2] = {
+	{0x0c, 0x0},
+	{0x10, 0x0},
+	{0x1C, 0x04008032},
+	{0x1C, 0x00048031},
+	{0x1C, 0x05208030},
+	{0x1C, 0x04008040},
+	{0x818, 0x0},
+	{0x18, 0x0},
+};
+
+unsigned long ddr3_calibration_mx6sx[][2] = {
+	{0x83c, 0x0},
+	{0x840, 0x0},
+	{0x848, 0x0},
+	{0x850, 0x0},
+};
+
+unsigned long iomux_offsets_mx6sx[][2] = {
+	{0x330, 0x0},
+	{0x334, 0x0},
+	{0x338, 0x0},
+	{0x33c, 0x0},
+};
 unsigned long ddr3_dll_mx6q[][2] = {
 	{0x0c, 0x0},
 	{0x10, 0x0},
@@ -338,6 +369,52 @@ int update_ddr_freq_imx_smp(int ddr_rate)
 	return 0;
 }
 
+/* Used by i.MX6SX/i.MX6UL for updating the ddr frequency */
+int update_ddr_freq_imx6_up(int ddr_rate)
+{
+	int i;
+	bool dll_off = false;
+	unsigned long ttbr1;
+	int mode = get_bus_freq_mode();
+
+	if (ddr_rate == curr_ddr_rate)
+		return 0;
+
+	printk(KERN_DEBUG "\nBus freq set to %d start...\n", ddr_rate);
+
+	if ((mode == BUS_FREQ_LOW) || (mode == BUS_FREQ_AUDIO))
+		dll_off = true;
+
+	imx6_busfreq_info->dll_off = dll_off;
+	iram_ddr_settings[0][0] = ddr_settings_size;
+	iram_iomux_settings[0][0] = iomux_settings_size;
+	for (i = 0; i < iram_ddr_settings[0][0]; i++) {
+		iram_ddr_settings[i + 1][0] =
+				normal_mmdc_settings[i][0];
+		iram_ddr_settings[i + 1][1] =
+				normal_mmdc_settings[i][1];
+	}
+
+	local_irq_disable();
+
+	ttbr1 = save_ttbr1();
+	imx6_busfreq_info->freq = ddr_rate;
+	imx6_busfreq_info->ddr_settings = iram_ddr_settings;
+	imx6_busfreq_info->iomux_offsets = iram_iomux_settings;
+	imx6_busfreq_info->mu_delay_val  = ((readl_relaxed(mmdc_base + MMDC0_MPMUR0)
+		>> MMDC0_MPMUR0_OFFSET) & MMDC0_MPMUR0_MASK);
+
+	imx6_up_change_ddr_freq(imx6_busfreq_info);
+	restore_ttbr1(ttbr1);
+	curr_ddr_rate = ddr_rate;
+
+	local_irq_enable();
+
+	printk(KERN_DEBUG "Bus freq set to %d done!\n", ddr_rate);
+
+	return 0;
+}
+
 int init_ddrc_ddr_settings(struct platform_device *busfreq_pdev)
 {
 	int ddr_type = imx_ddrc_get_ddr_type();
@@ -409,6 +486,89 @@ int init_ddrc_ddr_settings(struct platform_device *busfreq_pdev)
 	return 0;
 }
 
+/* Used by i.MX6SX/i.MX6UL for mmdc setting init. */
+int init_mmdc_ddr3_settings_imx6_up(struct platform_device *busfreq_pdev)
+{
+	int i;
+	struct device_node *node;
+	unsigned long ddr_code_size;
+
+	node = of_find_compatible_node(NULL, NULL, "fsl,imx6q-mmdc");
+	if (!node) {
+		printk(KERN_ERR "failed to find mmdc device tree data!\n");
+		return -EINVAL;
+	}
+	mmdc_base = of_iomap(node, 0);
+	WARN(!mmdc_base, "unable to map mmdc registers\n");
+
+	if (cpu_is_imx6sx())
+		node = of_find_compatible_node(NULL, NULL, "fsl,imx6sx-iomuxc");
+	if (!node) {
+		printk(KERN_ERR "failed to find iomuxc device tree data!\n");
+		return -EINVAL;
+	}
+	iomux_base = of_iomap(node, 0);
+	WARN(!iomux_base, "unable to map iomux registers\n");
+
+	ddr_settings_size = ARRAY_SIZE(ddr3_dll_mx6sx) +
+		ARRAY_SIZE(ddr3_calibration_mx6sx);
+
+	normal_mmdc_settings = kmalloc((ddr_settings_size * 8), GFP_KERNEL);
+	memcpy(normal_mmdc_settings, ddr3_dll_mx6sx,
+		sizeof(ddr3_dll_mx6sx));
+	memcpy(((char *)normal_mmdc_settings + sizeof(ddr3_dll_mx6sx)),
+		ddr3_calibration_mx6sx, sizeof(ddr3_calibration_mx6sx));
+
+	/* store the original DDR settings at boot. */
+	for (i = 0; i < ddr_settings_size; i++) {
+		/*
+		 * writes via command mode register cannot be read back.
+		 * hence hardcode them in the initial static array.
+		 * this may require modification on a per customer basis.
+		 */
+		if (normal_mmdc_settings[i][0] != 0x1C)
+			normal_mmdc_settings[i][1] =
+				readl_relaxed(mmdc_base
+				+ normal_mmdc_settings[i][0]);
+	}
+
+	iomux_settings_size = ARRAY_SIZE(iomux_offsets_mx6sx);
+
+	ddr_code_size = (&imx6_up_ddr3_freq_change_end -&imx6_up_ddr3_freq_change_start) *4 +
+			sizeof(*imx6_busfreq_info);
+	imx6_busfreq_info = (struct imx6_busfreq_info *)ddr_freq_change_iram_base;
+
+	imx6_up_change_ddr_freq = (void *)fncpy((void *)ddr_freq_change_iram_base + sizeof(*imx6_busfreq_info),
+		&imx6_up_ddr3_freq_change, ddr_code_size - sizeof(*imx6_busfreq_info));
+
+	/*
+	 * Store the size of the array in iRAM also,
+	 * increase the size by 8 bytes.
+	 */
+	iram_iomux_settings = (void *)(ddr_freq_change_iram_base + ddr_code_size);
+	iram_ddr_settings = iram_iomux_settings + (iomux_settings_size * 8) + 8;
+
+	if ((ddr_code_size + (iomux_settings_size + ddr_settings_size) * 8 + 16)
+		> ddr_freq_change_total_size) {
+		printk(KERN_ERR "Not enough memory allocated for DDR Frequency change code.\n");
+		return EINVAL;
+	}
+
+	for (i = 0; i < iomux_settings_size; i++) {
+		iomux_offsets_mx6sx[i][1] =
+			readl_relaxed(iomux_base +
+			iomux_offsets_mx6sx[i][0]);
+		iram_iomux_settings[i + 1][0] =
+			iomux_offsets_mx6sx[i][0];
+		iram_iomux_settings[i + 1][1] =
+			iomux_offsets_mx6sx[i][1];
+	}
+
+	curr_ddr_rate = ddr_normal_rate;
+
+	return 0;
+}
+
 int init_mmdc_ddr3_settings_imx6_smp(struct platform_device *busfreq_pdev)
 {
 	int i;
diff --git a/arch/arm/mach-imx/busfreq_lpddr2.c b/arch/arm/mach-imx/busfreq_lpddr2.c
index 1d2eda2389b9..72c947370f51 100644
--- a/arch/arm/mach-imx/busfreq_lpddr2.c
+++ b/arch/arm/mach-imx/busfreq_lpddr2.c
@@ -56,6 +56,7 @@ void (*mx6_change_lpddr2_freq)(u32 ddr_freq, int bus_freq_mode) = NULL;
 
 extern unsigned int ddr_normal_rate;
 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, void *ddr_settings);
@@ -161,6 +162,10 @@ int init_mmdc_lpddr2_settings(struct platform_device *busfreq_pdev)
 
 	ddr_code_size = SZ_4K;
 
+	if (cpu_is_imx6sx())
+		mx6_change_lpddr2_freq = (void *)fncpy(
+			(void *)ddr_freq_change_iram_base,
+			&imx6_up_lpddr2_freq_change, ddr_code_size);
 	curr_ddr_rate = ddr_normal_rate;
 
 	return 0;
diff --git a/arch/arm/mach-imx/ddr3_freq_imx6sx.S b/arch/arm/mach-imx/ddr3_freq_imx6sx.S
new file mode 100644
index 000000000000..1ac0d017bdf9
--- /dev/null
+++ b/arch/arm/mach-imx/ddr3_freq_imx6sx.S
@@ -0,0 +1,764 @@
+/*
+ * Copyright (C) 2011-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 "hardware.h"
+
+.globl imx6_up_ddr3_freq_change_start
+.globl imx6_up_ddr3_freq_change_end
+
+#define MMDC0_MDPDC		0x4
+#define MMDC0_MDCF0		0xc
+#define MMDC0_MDCF1		0x10
+#define MMDC0_MDMISC		0x18
+#define MMDC0_MDSCR		0x1c
+#define MMDC0_MAPSR		0x404
+#define MMDC0_MADPCR0		0x410
+#define MMDC0_MPZQHWCTRL	0x800
+#define MMDC0_MPODTCTRL		0x818
+#define MMDC0_MPDGCTRL0		0x83c
+#define MMDC0_MPMUR0		0x8b8
+
+#define CCM_CBCDR		0x14
+#define CCM_CBCMR		0x18
+#define CCM_CSCMR1		0x1c
+#define CCM_CDHIPR		0x48
+
+#define L2_CACHE_SYNC		0x730
+#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
+
+#define BUSFREQ_INFO_FREQ_OFFSET		0x0
+#define BUSFREQ_INFO_DDR_SETTINGS_OFFSET	0x4
+#define BUSFREQ_INFO_DLL_OFF_OFFSET		0x8
+#define BUSFREQ_INFO_IOMUX_OFFSETS_OFFSET	0xc
+#define BUSFREQ_INFO_MU_DELAY_OFFSET		0x10
+
+.extern iram_tlb_phys_addr
+
+	.align 3
+
+	/* Check if the cpu is cortex-a7 */
+	.macro is_ca7
+
+	/* Read the primary cpu number is MPIDR */
+	mrc	p15, 0, r7, c0, c0, 0
+	ldr	r8, =0xfff0
+	and	r7, r7, r8
+	ldr	r8, =0xc070
+	cmp	r7, r8
+
+	.endm
+
+	.macro do_delay
+
+1:
+	ldr	r9, =0
+2:
+	ldr	r10, [r4, r9]
+	add	r9, r9, #4
+	cmp	r9, #16
+	bne	2b
+	sub	r8, r8, #1
+	cmp	r8, #0
+	bgt	1b
+
+	.endm
+
+	.macro wait_for_ccm_handshake
+
+3:
+	ldr	r8, [r5, #CCM_CDHIPR]
+	cmp	r8, #0
+	bne	3b
+
+	.endm
+
+	.macro	switch_to_400MHz
+
+	/* check whether periph2_clk is already from top path */
+	ldr	r8, [r5, #CCM_CBCDR]
+	ands	r8, #(1 << 26)
+	beq	skip_periph2_clk2_switch_400m
+
+	/* now switch periph2_clk back. */
+	ldr	r8, [r5, #CCM_CBCDR]
+	bic	r8, r8, #(1 << 26)
+	str	r8, [r5, #CCM_CBCDR]
+
+	wait_for_ccm_handshake
+
+	/*
+	 * on i.MX6SX, pre_periph2_clk will be always from
+	 * pll2_pfd2, so no need to set pre_periph2_clk
+	 * parent, just set the mmdc divider directly.
+	 */
+skip_periph2_clk2_switch_400m:
+
+	/* fabric_mmdc_podf to 0 */
+	ldr	r8, [r5, #CCM_CBCDR]
+	bic	r8, r8, #(0x7 << 3)
+	str	r8, [r5, #CCM_CBCDR]
+
+	wait_for_ccm_handshake
+
+	.endm
+
+	.macro	switch_to_50MHz
+
+	/* check whether periph2_clk is already from top path */
+	ldr	r8, [r5, #CCM_CBCDR]
+	ands	r8, #(1 << 26)
+	beq	skip_periph2_clk2_switch_50m
+
+	/* now switch periph2_clk back. */
+	ldr	r8, [r5, #CCM_CBCDR]
+	bic	r8, r8, #(1 << 26)
+	str	r8, [r5, #CCM_CBCDR]
+
+	wait_for_ccm_handshake
+
+	/*
+	 * on i.MX6SX, pre_periph2_clk will be always from
+	 * pll2_pfd2, so no need to set pre_periph2_clk
+	 * parent, just set the mmdc divider directly.
+	 */
+skip_periph2_clk2_switch_50m:
+
+	/* fabric_mmdc_podf to 7 so that mmdc is 400 / 8 = 50MHz */
+	ldr	r8, [r5, #CCM_CBCDR]
+	orr	r8, r8, #(0x7 << 3)
+	str	r8, [r5, #CCM_CBCDR]
+
+	wait_for_ccm_handshake
+
+	.endm
+
+	.macro	switch_to_24MHz
+
+	/* periph2_clk2 sel to OSC_CLK */
+	ldr	r8, [r5, #CCM_CBCMR]
+	orr	r8, r8, #(1 << 20)
+	str	r8, [r5, #CCM_CBCMR]
+
+	/* periph2_clk2_podf to 0 */
+	ldr	r8, [r5, #CCM_CBCDR]
+	bic	r8, r8, #0x7
+	str	r8, [r5, #CCM_CBCDR]
+
+	/* periph2_clk sel to periph2_clk2 */
+	ldr	r8, [r5, #CCM_CBCDR]
+	orr	r8, r8, #(0x1 << 26)
+	str	r8, [r5, #CCM_CBCDR]
+
+	wait_for_ccm_handshake
+
+	/* fabric_mmdc_podf to 0 */
+	ldr	r8, [r5, #CCM_CBCDR]
+	bic	r8, r8, #(0x7 << 3)
+	str	r8, [r5, #CCM_CBCDR]
+
+	wait_for_ccm_handshake
+
+	.endm
+
+/*
+ *  imx6_up_ddr3_freq_change
+ *  Below code can be used by i.MX6SX and i.MX6UL.
+ *
+ *  idle the processor (eg, wait for interrupt).
+ *  make sure DDR is in self-refresh.
+ *  IRQs are already disabled.
+ */
+ENTRY(imx6_up_ddr3_freq_change)
+
+imx6_up_ddr3_freq_change_start:
+	stmfd	sp!, {r4 - r11}
+
+	ldr	r1, [r0, #BUSFREQ_INFO_DDR_SETTINGS_OFFSET]
+	ldr	r2, [r0, #BUSFREQ_INFO_DLL_OFF_OFFSET]
+	ldr	r3, [r0, #BUSFREQ_INFO_IOMUX_OFFSETS_OFFSET]
+
+	/*
+	 * 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
+
+	dsb
+	isb
+
+	/* Disable L1 data cache. */
+	mrc	p15, 0, r6, c1, c0, 0
+	bic	r6, r6, #0x4
+	mcr	p15, 0, r6, c1, c0, 0
+
+	ldr	r4, =IMX_IO_P2V(MX6Q_MMDC_P0_BASE_ADDR)
+	ldr	r5, =IMX_IO_P2V(MX6Q_CCM_BASE_ADDR)
+	ldr	r6, =IMX_IO_P2V(MX6Q_IOMUXC_BASE_ADDR)
+
+	is_ca7
+	beq	skip_disable_l2
+
+#ifdef CONFIG_CACHE_L2X0
+	/*
+	 * make sure the L2 buffers are drained,
+	 * sync operation on L2 drains the buffers.
+	 */
+	ldr	r8, =IMX_IO_P2V(MX6Q_L2_BASE_ADDR)
+
+	/* Wait for background operations to complete. */
+wait_for_l2_to_idle:
+	ldr	r7, [r8, #0x730]
+	cmp	r7, #0x0
+	bne	wait_for_l2_to_idle
+
+	mov	r7, #0x0
+	str	r7, [r8, #L2_CACHE_SYNC]
+
+	/* Lock L2. */
+
+	ldr	r9, [r8, #PL310_AUX_CTRL]
+	tst	r9, #PL310_AUX_16WAY_BIT
+	mov	r9, #PL310_8WAYS_MASK
+	orrne	r9, #PL310_16WAYS_UPPERMASK
+	mov	r10, #PL310_LOCKDOWN_NBREGS
+	add	r11, r8, #PL310_DCACHE_LOCKDOWN_BASE
+1:	/* lock Dcache and Icache */
+	str	r9, [r11], #PL310_LOCKDOWN_SZREG
+	str	r9, [r11], #PL310_LOCKDOWN_SZREG
+	subs	r10, r10, #1
+	bne	1b
+
+	/*
+	 * The second dsb might be needed to keep cache sync (device write)
+	 * ordering with the memory accesses before it.
+	 */
+	dsb
+	isb
+#endif
+
+skip_disable_l2:
+	/* disable automatic power saving. */
+	ldr	r8, [r4, #MMDC0_MAPSR]
+	orr	r8, r8, #0x1
+	str	r8, [r4, #MMDC0_MAPSR]
+
+	/* disable MMDC power down timer. */
+	ldr	r8, [r4, #MMDC0_MDPDC]
+	bic	r8, r8, #(0xff << 8)
+	str	r8, [r4, #MMDC0_MDPDC]
+
+	/* delay for a while */
+	ldr	r8, =4
+	do_delay
+
+	/* set CON_REG */
+	ldr	r8, =0x8000
+	str	r8, [r4, #MMDC0_MDSCR]
+poll_conreq_set_1:
+	ldr	r8, [r4, #MMDC0_MDSCR]
+	and	r8, r8, #(0x4 << 12)
+	cmp	r8, #(0x4 << 12)
+	bne	poll_conreq_set_1
+
+	/*
+	 * if requested frequency is greater than
+	 * 300MHz go to DLL on mode.
+	 */
+	ldr	r8, [r0, #BUSFREQ_INFO_FREQ_OFFSET]
+	ldr	r9, =300000000
+	cmp	r8, r9
+	bge	dll_on_mode
+
+dll_off_mode:
+	/* if DLL is currently on, turn it off. */
+	cmp	r2, #1
+	beq	continue_dll_off_1
+
+	ldr	r8, =0x00018031
+	str	r8, [r4, #MMDC0_MDSCR]
+
+	ldr	r8, =0x00018039
+	str	r8, [r4, #MMDC0_MDSCR]
+
+	ldr	r8, =10
+	do_delay
+
+continue_dll_off_1:
+	/* set DVFS - enter self refresh mode */
+	ldr	r8, [r4, #MMDC0_MAPSR]
+	orr	r8, r8, #(1 << 21)
+	str	r8, [r4, #MMDC0_MAPSR]
+
+	/* de-assert con_req */
+	mov	r8, #0x0
+	str	r8, [r4, #MMDC0_MDSCR]
+
+poll_dvfs_set_1:
+	ldr	r8, [r4, #MMDC0_MAPSR]
+	and	r8, r8, #(1 << 25)
+	cmp	r8, #(1 << 25)
+	bne	poll_dvfs_set_1
+
+	ldr	r8, [r0, #BUSFREQ_INFO_FREQ_OFFSET]
+	ldr	r9, =24000000
+	cmp	r8, r9
+	beq	switch_freq_24
+
+	switch_to_50MHz
+	b	continue_dll_off_2
+
+switch_freq_24:
+	switch_to_24MHz
+
+continue_dll_off_2:
+	/* set SBS - block ddr accesses */
+	ldr	r8, [r4, #MMDC0_MADPCR0]
+	orr	r8, r8, #(1 << 8)
+	str	r8, [r4, #MMDC0_MADPCR0]
+
+	/* clear DVFS - exit from self refresh mode */
+	ldr	r8, [r4, #MMDC0_MAPSR]
+	bic	r8, r8, #(1 << 21)
+	str	r8, [r4, #MMDC0_MAPSR]
+
+poll_dvfs_clear_1:
+	ldr	r8, [r4, #MMDC0_MAPSR]
+	and	r8, r8, #(1 << 25)
+	cmp	r8, #(1 << 25)
+	beq	poll_dvfs_clear_1
+
+	/* if DLL was previously on, continue DLL off routine. */
+	cmp    	r2, #1
+	beq 	continue_dll_off_3
+
+	ldr	r8, =0x00018031
+	str	r8, [r4, #MMDC0_MDSCR]
+
+	ldr	r8, =0x00018039
+	str	r8, [r4, #MMDC0_MDSCR]
+
+	ldr	r8, =0x04208030
+	str	r8, [r4, #MMDC0_MDSCR]
+
+	ldr	r8, =0x04208038
+	str	r8, [r4, #MMDC0_MDSCR]
+
+	ldr	r8, =0x00088032
+	str	r8, [r4, #MMDC0_MDSCR]
+
+	ldr	r8, =0x0008803A
+	str	r8, [r4, #MMDC0_MDSCR]
+
+	/* delay for a while. */
+	ldr	r8, =4
+	do_delay
+
+	ldr	r8, [r4, #MMDC0_MDCF0]
+	bic	r8, r8, #0xf
+	orr	r8, r8, #0x3
+	str	r8, [r4, #MMDC0_MDCF0]
+
+	ldr	r8, [r4, #MMDC0_MDCF1]
+	bic	r8, r8, #0x7
+	orr	r8, r8, #0x4
+	str	r8, [r4, #MMDC0_MDCF1]
+
+	ldr	r8, [r4, #MMDC0_MDMISC]
+	bic	r8, r8, #(0x3 << 16) /* walat = 0x1 */
+	orr	r8, r8, #(0x1 << 16)
+	bic	r8, r8, #(0x7 << 6)  /* ralat = 0x2 */
+	orr	r8, r8, #(0x2 << 6)
+	str	r8, [r4, #MMDC0_MDMISC]
+
+	/* enable dqs pull down in the IOMUX. */
+	ldr	r8, [r3]
+	add	r3, r3, #8
+	ldr	r9, =0x3028
+update_iomux:
+	ldr	r10, [r3]
+	ldr	r11, [r6, r10]
+	bic	r11, r11, r9
+	orr	r11, r11, #(0x3 << 12)
+	orr	r11, r11, #0x28
+	str	r11, [r6, r10]
+	add	r3, r3, #8
+	sub	r8, r8, #1
+	cmp	r8, #0
+	bgt	update_iomux
+
+	/*  ODT disabled. */
+	ldr	r8, =0x0
+	str	r8, [r4, #MMDC0_MPODTCTRL]
+
+	/* DQS gating disabled. */
+	ldr	r8, [r4, #MMDC0_MPDGCTRL0]
+	orr	r8, r8, #(1 << 29)
+	str	r8, [r4, #MMDC0_MPDGCTRL0]
+
+	/* Add workaround for ERR005778.*/
+	/* double the original MU_UNIT_DEL_NUM. */
+	ldr	r8, [r0, #BUSFREQ_INFO_MU_DELAY_OFFSET]
+	lsl	r8, r8, #1
+
+	/* Bypass the automatic MU by setting the mu_byp_en */
+	ldr	r10, [r4, #MMDC0_MPMUR0]
+	orr	r10, r10, #0x400
+	/* Set the MU_BYP_VAL */
+	orr	r10, r10, r8
+	str	r10, [r4, #MMDC0_MPMUR0]
+
+	/* Now perform a force measure */
+	ldr	r8, [r4, #MMDC0_MPMUR0]
+	orr	r8, r8, #0x800
+	str	r8, [r4, #MMDC0_MPMUR0]
+	/* Wait for FRC_MSR to clear. */
+1:
+	ldr	r8, [r4, #MMDC0_MPMUR0]
+	and	r8, r8, #0x800
+	cmp	r8, #0x0
+	bne	1b
+
+continue_dll_off_3:
+	/* clear SBS - unblock accesses to DDR. */
+	ldr	r8, [r4, #MMDC0_MADPCR0]
+	bic	r8, r8, #(0x1 << 8)
+	str	r8, [r4, #MMDC0_MADPCR0]
+
+	mov	r8, #0x0
+	str	r8, [r4, #MMDC0_MDSCR]
+poll_conreq_clear_1:
+	ldr	r8, [r4, #MMDC0_MDSCR]
+	and	r8, r8, #(0x4 << 12)
+	cmp	r8, #(0x4 << 12)
+	beq	poll_conreq_clear_1
+
+	b	done
+
+dll_on_mode:
+	/* assert DVFS - enter self refresh mode. */
+	ldr	r8, [r4, #MMDC0_MAPSR]
+	orr	r8, r8, #(1 << 21)
+	str	r8, [r4, #MMDC0_MAPSR]
+
+	/* de-assert CON_REQ. */
+	mov	r8, #0x0
+	str	r8, [r4, #MMDC0_MDSCR]
+
+	/* poll DVFS ack. */
+poll_dvfs_set_2:
+	ldr	r8, [r4, #MMDC0_MAPSR]
+	and	r8, r8, #(1 << 25)
+	cmp	r8, #(1 << 25)
+	bne	poll_dvfs_set_2
+
+	switch_to_400MHz
+
+	/* set SBS step-by-step mode. */
+	ldr	r8, [r4, #MMDC0_MADPCR0]
+	orr	r8, r8, #(1 << 8)
+	str	r8, [r4, #MMDC0_MADPCR0]
+
+	/* clear DVFS - exit self refresh mode. */
+	ldr	r8, [r4, #MMDC0_MAPSR]
+	bic	r8, r8, #(1 << 21)
+	str	r8, [r4, #MMDC0_MAPSR]
+
+poll_dvfs_clear_2:
+	ldr	r8, [r4, #MMDC0_MAPSR]
+	ands	r8, r8, #(1 << 25)
+	bne	poll_dvfs_clear_2
+
+	/* if DLL is currently off, turn it back on. */
+	cmp	r2, #0
+	beq	update_calibration_only
+
+	/* issue zq calibration command */
+	ldr	r8, [r4, #MMDC0_MPZQHWCTRL]
+	orr	r8, r8, #0x3
+	str	r8, [r4, #MMDC0_MPZQHWCTRL]
+
+	/* enable DQS gating. */
+	ldr	r10, =MMDC0_MPDGCTRL0
+	ldr	r8, [r4, r10]
+	bic	r8, r8, #(1 << 29)
+	str	r8, [r4, r10]
+
+	/* Now perform a force measure */
+	ldr	r8, =0x00000800
+	str	r8, [r4, #MMDC0_MPMUR0]
+	/* Wait for FRC_MSR to clear. */
+1:
+	ldr	r8, [r4, #MMDC0_MPMUR0]
+	and	r8, r8, #0x800
+	cmp	r8, #0x0
+	bne	1b
+
+	/* disable dqs pull down in the IOMUX. */
+	ldr	r8, [r3]
+	add	r3, r3, #8
+update_iomux1:
+	ldr	r10, [r3, #0x0]
+	ldr	r11, [r3, #0x4]
+	str	r11, [r6, r10]
+	add	r3, r3, #8
+	sub	r8, r8, #1
+	cmp	r8, #0
+	bgt	update_iomux1
+
+	/* config MMDC timings to 400MHz. */
+	ldr	r1, [r0, #BUSFREQ_INFO_DDR_SETTINGS_OFFSET]
+	ldr	r7, [r1]
+	add	r1, r1, #8
+	ldr	r10, [r1, #0x0]
+	ldr	r11, [r1, #0x4]
+	str	r11, [r4, r10]
+	add	r1, r1, #8
+
+	ldr	r10, [r1, #0x0]
+	ldr	r11, [r1, #0x4]
+	str	r11, [r4, r10]
+	add	r1, r1, #8
+
+	/* configure ddr devices to dll on, odt. */
+	ldr	r8, =0x00028031
+	str	r8, [r4, #MMDC0_MDSCR]
+
+	ldr	r8, =0x00028039
+	str	r8, [r4, #MMDC0_MDSCR]
+
+	/* delay for while. */
+	ldr	r8, =4
+	do_delay
+
+	/* reset dll. */
+	ldr	r8, =0x09208030
+	str	r8, [r4, #MMDC0_MDSCR]
+
+	ldr	r8, =0x09208038
+	str	r8, [r4, #MMDC0_MDSCR]
+
+	/* delay for while. */
+	ldr	r8, =100
+	do_delay
+
+	ldr	r10, [r1, #0x0]
+	ldr	r11, [r1, #0x4]
+	str	r11, [r4, r10]
+	add	r1, r1, #8
+
+	ldr	r10, [r1, #0x0]
+	ldr	r11, [r1, #0x4]
+	str	r11, [r4, r10]
+	add	r1, r1, #8
+
+	ldr	r8, =0x00428031
+	str	r8, [r4, #MMDC0_MDSCR]
+
+	ldr	r8, =0x00428039
+	str	r8, [r4, #MMDC0_MDSCR]
+
+	ldr	r10, [r1, #0x0]
+	ldr	r11, [r1, #0x4]
+	str	r11, [r4, r10]
+	add	r1, r1, #8
+
+	ldr	r10, [r1, #0x0]
+	ldr	r11, [r1, #0x4]
+	str	r11, [r4, r10]
+	add	r1, r1, #8
+
+	/* issue a zq command. */
+	ldr	r8, =0x04008040
+	str	r8, [r4, #MMDC0_MDSCR]
+
+	ldr	r8, =0x04008048
+	str	r8, [r4, #MMDC0_MDSCR]
+
+	/* MMDC ODT enable. */
+	ldr	r10, [r1, #0x0]
+	ldr	r11, [r1, #0x4]
+	str	r11, [r4, r10]
+	add	r1, r1, #8
+
+	/* delay for while. */
+	ldr	r8, =40
+	do_delay
+
+	/* enable MMDC power down timer. */
+	ldr	r8, [r4, #MMDC0_MDPDC]
+	orr	r8, r8, #(0x55 << 8)
+	str	r8, [r4, #MMDC0_MDPDC]
+
+	b	update_calibration
+
+update_calibration_only:
+	ldr	r8, [r1]
+	sub	r8, r8, #7
+	add	r1, r1, #64
+	b	update_calib
+
+update_calibration:
+	/* write the new calibration values. */
+	mov	r8, r7
+	sub	r8, r8, #7
+
+update_calib:
+	ldr	r10, [r1, #0x0]
+	ldr	r11, [r1, #0x4]
+	str	r11, [r4, r10]
+	add	r1, r1, #8
+	sub	r8, r8, #1
+	cmp	r8, #0
+	bgt	update_calib
+
+	/* perform a force measurement. */
+	ldr	r8, =0x800
+	str	r8, [r4, #MMDC0_MPMUR0]
+	/* Wait for FRC_MSR to clear. */
+1:
+	ldr	r8, [r4, #MMDC0_MPMUR0]
+	and	r8, r8, #0x800
+	cmp	r8, #0x0
+	bne	1b
+
+	/* clear SBS - unblock DDR accesses. */
+	ldr	r8, [r4, #MMDC0_MADPCR0]
+	bic	r8, r8, #(1 << 8)
+	str	r8, [r4, #MMDC0_MADPCR0]
+
+	mov	r8, #0x0
+	str	r8, [r4, #MMDC0_MDSCR]
+poll_conreq_clear_2:
+	ldr	r8, [r4, #MMDC0_MDSCR]
+	and	r8, r8, #(0x4 << 12)
+	cmp	r8, #(0x4 << 12)
+	beq	poll_conreq_clear_2
+
+done:
+
+	/* MMDC0_MAPSR adopt power down enable. */
+	ldr	r8, [r4, #MMDC0_MAPSR]
+	bic	r8, r8, #0x01
+	str	r8, [r4, #MMDC0_MAPSR]
+
+	is_ca7
+	beq	skip_enable_l2
+
+#ifdef CONFIG_CACHE_L2X0
+	/* Unlock L2. */
+	ldr	r8, =IMX_IO_P2V(MX6Q_L2_BASE_ADDR)
+	ldr	r9, [r8, #PL310_AUX_CTRL]
+	tst	r9, #PL310_AUX_16WAY_BIT
+	mov	r10, #PL310_LOCKDOWN_NBREGS
+	mov	r9, #0x00	/* 8 ways mask */
+	orrne	r9, #0x0000	/* 16 ways mask */
+	add	r11, r8, #PL310_DCACHE_LOCKDOWN_BASE
+1:	/* lock Dcache and Icache */
+	str	r9, [r11], #PL310_LOCKDOWN_SZREG
+	str	r9, [r11], #PL310_LOCKDOWN_SZREG
+	subs	r10, r10, #1
+	bne	1b
+
+#endif
+
+skip_enable_l2:
+	/* Enable L1 data cache. */
+	mrc	p15, 0, r7, c1, c0, 0
+	orr	r7, r7, #0x4
+	mcr	p15, 0, r7, 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, r7, c1, c0, 0
+	orr	r7, r7, #0x800
+	mcr	p15, 0, r7, c1, c0, 0
+
+	/* Flush the Branch Target Address Cache (BTAC) */
+	ldr	r7, =0x0
+	mcr	p15, 0, r7, c7, c1, 6
+
+	/* restore registers */
+	ldmfd	sp!, {r4 - r11}
+	mov	pc, lr
+
+	/*
+	 * Add ltorg here to ensure that all
+	 * literals are stored here and are
+	 * within the text space.
+	 */
+	.ltorg
+imx6_up_ddr3_freq_change_end:
+ENDPROC(imx6_up_ddr3_freq_change)
diff --git a/arch/arm/mach-imx/lpddr2_freq_imx6sx.S b/arch/arm/mach-imx/lpddr2_freq_imx6sx.S
new file mode 100644
index 000000000000..ba3488cad9d4
--- /dev/null
+++ b/arch/arm/mach-imx/lpddr2_freq_imx6sx.S
@@ -0,0 +1,492 @@
+/*
+ * Copyright (C) 2014-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 CCM_CBCDR	0x14
+#define CCM_CBCMR	0x18
+#define CCM_CSCMR1	0x1c
+#define CCM_CDHIPR	0x48
+
+#define L2_CACHE_SYNC	0x730
+#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
+
+#define MMDC0_MDPDC	0x4
+#define MMDC0_MAPSR	0x404
+#define MMDC0_MADPCR0	0x410
+
+#define HIGH_BUS_MODE	0x0
+
+	/* Check if the cpu is cortex-a7 */
+	.macro is_ca7
+
+	/* Read the primary cpu number is MPIDR */
+	mrc	p15, 0, r6, c0, c0, 0
+	ldr	r7, =0xfff0
+	and	r6, r6, r7
+	ldr	r7, =0xc070
+	cmp	r6, r7
+
+	.endm
+
+	.macro	wait_for_ccm_handshake
+
+1:
+	ldr	r8, [r2, #CCM_CDHIPR]
+	cmp	r8, #0
+	bne	1b
+
+	.endm
+
+	.macro	switch_to_24MHz
+
+	/* periph2_clk2 sel to OSC_CLK */
+	ldr	r8, [r2, #CCM_CBCMR]
+	orr	r8, r8, #(1 << 20)
+	str	r8, [r2, #CCM_CBCMR]
+
+	/* periph2_clk2_podf to 0 */
+	ldr	r8, [r2, #CCM_CBCDR]
+	bic	r8, r8, #0x7
+	str	r8, [r2, #CCM_CBCDR]
+
+	/* periph2_clk sel to periph2_clk2 */
+	ldr	r8, [r2, #CCM_CBCDR]
+	orr	r8, r8, #(0x1 << 26)
+	str	r8, [r2, #CCM_CBCDR]
+
+	wait_for_ccm_handshake
+
+	/* fabric_mmdc_podf to 0 */
+	ldr	r8, [r2, #CCM_CBCDR]
+	bic	r8, r8, #(0x7 << 3)
+	str	r8, [r2, #CCM_CBCDR]
+
+	wait_for_ccm_handshake
+
+	.endm
+
+	.macro	switch_to_100MHz
+
+	/* check whether periph2_clk is from top path */
+	ldr	r8, [r2, #CCM_CBCDR]
+	ands	r8, #(1 << 26)
+	beq	skip_periph2_clk2_switch_100m
+
+	/* now switch periph2_clk back. */
+	ldr	r8, [r2, #CCM_CBCDR]
+	bic	r8, r8, #(1 << 26)
+	str	r8, [r2, #CCM_CBCDR]
+
+	wait_for_ccm_handshake
+
+	/*
+	 * on i.MX6SX, pre_periph2_clk will be always from
+	 * pll2_pfd2, so no need to set pre_periph2_clk
+	 * parent, just set the mmdc divider directly.
+	 */
+skip_periph2_clk2_switch_100m:
+
+	/* fabric_mmdc_podf to 3 so that mmdc is 400 / 4 = 100MHz */
+	ldr	r8, [r2, #CCM_CBCDR]
+	bic	r8, r8, #(0x7 << 3)
+	orr	r8, r8, #(0x3 << 3)
+	str	r8, [r2, #CCM_CBCDR]
+
+	wait_for_ccm_handshake
+
+	.endm
+
+	.macro	switch_to_400MHz
+
+	/* check whether periph2_clk is from top path */
+	ldr	r8, [r2, #CCM_CBCDR]
+	ands	r8, #(1 << 26)
+	beq	skip_periph2_clk2_switch_400m
+
+	/* now switch periph2_clk back. */
+	ldr	r8, [r2, #CCM_CBCDR]
+	bic	r8, r8, #(1 << 26)
+	str	r8, [r2, #CCM_CBCDR]
+
+	wait_for_ccm_handshake
+
+	/*
+	 * on i.MX6SX, pre_periph2_clk will be always from
+	 * pll2_pfd2, so no need to set pre_periph2_clk
+	 * parent, just set the mmdc divider directly.
+	 */
+skip_periph2_clk2_switch_400m:
+
+	/* fabric_mmdc_podf to 0 */
+	ldr	r8, [r2, #CCM_CBCDR]
+	bic	r8, r8, #(0x7 << 3)
+	str	r8, [r2, #CCM_CBCDR]
+
+	wait_for_ccm_handshake
+
+	.endm
+
+	.macro	mmdc_clk_lower_100MHz
+	/* if MMDC is not in 400MHz mode, skip double mu count */
+	cmp	r1, #HIGH_BUS_MODE
+	bne	1f
+
+	/*
+	 * Prior to reducing the DDR frequency (at 528/400 MHz),
+	 * read the Measure unit count bits (MU_UNIT_DEL_NUM)
+	 */
+	ldr	r8, =0x8B8
+	ldr	r6, [r5, r8]
+	/* 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, [r5, r8]
+	orr	r6, r6, #0x400
+	str	r6, [r5, r8]
+	/*
+	 * 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, [r5, r8]
+	ldr	r4, =0x3FF
+	bic	r6, r6, r4
+	orr	r6, r6, r7
+	str	r6, [r5, r8]
+
+	/* For freq lower than 100MHz, need to set RALAT to 2 */
+	ldr	r6, [r5, #0x18]
+	bic	r6, r6, #(0x7 << 6)
+	orr	r6, r6, #(0x2 << 6)
+	str	r6, [r5, #0x18]
+1:
+	.endm
+
+	.macro	mmdc_clk_above_100MHz
+
+	/* Make sure that the PHY measurement unit is NOT in bypass mode */
+	ldr	r8, =0x8B8
+	ldr	r6, [r5, r8]
+	bic	r6, r6, #0x400
+	str	r6, [r5, r8]
+	/* Now perform a Force Measurement. */
+	ldr	r6, [r5, r8]
+	orr	r6, r6, #0x800
+	str	r6, [r5, r8]
+	/* Wait for FRC_MSR to clear. */
+force_measure1:
+	ldr	r6, [r5, r8]
+	and	r6, r6, #0x800
+	cmp	r6, #0x0
+	bne	force_measure1
+
+	/* For freq higher than 100MHz, need to set RALAT to 5 */
+	ldr	r6, [r5, #0x18]
+	bic	r6, r6, #(0x7 << 6)
+	orr	r6, r6, #(0x5 << 6)
+	str	r6, [r5, #0x18]
+
+	.endm
+
+	.align 3
+/*
+ * Below code can be used by i.MX6SX and i.MX6UL when changing the
+ * frequency of MMDC. the MMDC is the same on these two SOCs.
+ */
+ENTRY(imx6_up_lpddr2_freq_change)
+
+	push	{r2 - r8}
+
+	/*
+	 * 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
+
+	is_ca7
+	beq	skip_disable_l2
+
+#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)
+	mov	r6, #0x0
+	str	r6, [r7, #L2_CACHE_SYNC]
+
+	/*
+	 * 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
+
+skip_disable_l2:
+	ldr	r2, =IMX_IO_P2V(MX6Q_CCM_BASE_ADDR)
+	ldr	r3, =IMX_IO_P2V(MX6Q_ANATOP_BASE_ADDR)
+	ldr	r5, =IMX_IO_P2V(MX6Q_MMDC_P0_BASE_ADDR)
+
+	/* Disable Automatic power savings. */
+	ldr	r6, [r5, #MMDC0_MAPSR]
+	orr	r6, r6, #0x1
+	str	r6, [r5, #MMDC0_MAPSR]
+
+	/* MMDC0_MDPDC disable power down timer */
+	ldr	r6, [r5, #MMDC0_MDPDC]
+	bic	r6, r6, #0xff00
+	str	r6, [r5, #MMDC0_MDPDC]
+
+	/* Delay for a while */
+	ldr	r8, =10
+delay:
+	ldr	r7, =0
+cont:
+	ldr	r6, [r5, r7]
+	add	r7, r7, #4
+	cmp	r7, #16
+	bne	cont
+	sub	r8, r8, #1
+	cmp	r8, #0
+	bgt	delay
+
+	/* Make the DDR explicitly enter self-refresh. */
+	ldr	r6, [r5, #MMDC0_MAPSR]
+	orr	r6, r6, #0x200000
+	str	r6, [r5, #MMDC0_MAPSR]
+
+poll_dvfs_set_1:
+	ldr	r6, [r5, #MMDC0_MAPSR]
+	and	r6, r6, #0x2000000
+	cmp	r6, #0x2000000
+	bne	poll_dvfs_set_1
+
+	/* set SBS step-by-step mode */
+	ldr	r6, [r5, #MMDC0_MADPCR0]
+	orr	r6, r6, #0x100
+	str	r6, [r5, #MMDC0_MADPCR0]
+
+	ldr	r6, =100000000
+	cmp	r0, r6
+	bgt	set_ddr_mu_above_100
+	mmdc_clk_lower_100MHz
+
+set_ddr_mu_above_100:
+	ldr	r6, =24000000
+	cmp	r0, r6
+	beq	set_to_24MHz
+
+	ldr     r6, =100000000
+	cmp	r0, r6
+	beq	set_to_100MHz
+
+	switch_to_400MHz
+
+	mmdc_clk_above_100MHz
+
+	b	done
+
+set_to_24MHz:
+	switch_to_24MHz
+	b	done
+set_to_100MHz:
+	switch_to_100MHz
+done:
+	/* clear DVFS - exit from self refresh mode */
+	ldr	r6, [r5, #MMDC0_MAPSR]
+	bic	r6, r6, #0x200000
+	str	r6, [r5, #MMDC0_MAPSR]
+
+poll_dvfs_clear_1:
+	ldr	r6, [r5, #MMDC0_MAPSR]
+	and	r6, r6, #0x2000000
+	cmp	r6, #0x2000000
+	beq	poll_dvfs_clear_1
+
+	/* Enable Automatic power savings. */
+	ldr	r6, [r5, #MMDC0_MAPSR]
+	bic	r6, r6, #0x1
+	str	r6, [r5, #MMDC0_MAPSR]
+
+	ldr	r6, =24000000
+	cmp	r0, r6
+	beq	skip_power_down
+
+	/* Enable MMDC power down timer. */
+	ldr	r6, [r5, #MMDC0_MDPDC]
+	orr	r6, r6, #0x5500
+	str	r6, [r5, #MMDC0_MDPDC]
+
+skip_power_down:
+	/* clear SBS - unblock DDR accesses */
+	ldr	r6, [r5, #MMDC0_MADPCR0]
+	bic	r6, r6, #0x100
+	str	r6, [r5, #MMDC0_MADPCR0]
+
+	is_ca7
+	beq	skip_enable_l2
+
+#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
+
+skip_enable_l2:
+	/* 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
+
+	/* Restore registers */
+	pop	{r2 - r8}
+	mov	pc, lr
diff --git a/arch/arm/mach-imx/mach-imx6sx.c b/arch/arm/mach-imx/mach-imx6sx.c
index d5310bf307ff..35ef73346047 100644
--- a/arch/arm/mach-imx/mach-imx6sx.c
+++ b/arch/arm/mach-imx/mach-imx6sx.c
@@ -86,6 +86,13 @@ static void __init imx6sx_init_irq(void)
 	imx6_pm_ccm_init("fsl,imx6sx-ccm");
 }
 
+static void __init imx6sx_map_io(void)
+{
+	debug_ll_io_init();
+	imx6_pm_map_io();
+	imx_busfreq_map_io();
+}
+
 static void __init imx6sx_init_late(void)
 {
 	imx6sx_cpuidle_init();
@@ -102,6 +109,7 @@ static const char * const imx6sx_dt_compat[] __initconst = {
 DT_MACHINE_START(IMX6SX, "Freescale i.MX6 SoloX (Device Tree)")
 	.l2c_aux_val 	= 0,
 	.l2c_aux_mask	= ~0,
+	.map_io		= imx6sx_map_io,
 	.init_irq	= imx6sx_init_irq,
 	.init_machine	= imx6sx_init_machine,
 	.dt_compat	= imx6sx_dt_compat,
diff --git a/arch/arm/mach-imx/mxc.h b/arch/arm/mach-imx/mxc.h
index 848d96b2dd77..fc22dcd0a591 100644
--- a/arch/arm/mach-imx/mxc.h
+++ b/arch/arm/mach-imx/mxc.h
@@ -36,6 +36,7 @@
 #define IMX_DDR_TYPE_DDR3		0
 #define IMX_DDR_TYPE_LPDDR2		1
 #define IMX_DDR_TYPE_LPDDR3		2
+#define IMX_MMDC_DDR_TYPE_LPDDR3	3
 
 #ifndef __ASSEMBLY__
 extern unsigned int __mxc_cpu_type;