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

Add i.MX6Q bus-freq support.

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

14 files changed, 2652 insertions, 22 deletions

diff --git a/arch/arm/mach-imx/Makefile b/arch/arm/mach-imx/Makefile
index 8703165f4d64..a3db687d8c07 100644
--- a/arch/arm/mach-imx/Makefile
+++ b/arch/arm/mach-imx/Makefile
@@ -78,7 +78,8 @@ 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
+obj-$(CONFIG_SOC_IMX6Q) += mach-imx6q.o ddr3_freq_imx6.o smp_wfe_imx6.o busfreq_lpddr2.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
@@ -90,8 +91,11 @@ obj-$(CONFIG_SOC_IMX7ULP) += mach-imx7ulp.o pm-imx7ulp.o
 
 obj-y += busfreq-imx.o busfreq_ddr3.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
 
 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 976ba3ca7f29..5a43b1521917 100644
--- a/arch/arm/mach-imx/busfreq-imx.c
+++ b/arch/arm/mach-imx/busfreq-imx.c
@@ -108,6 +108,16 @@ static struct clk *pll_dram;
 static struct clk *ahb_sel_clk;
 static struct clk *axi_clk;
 
+static struct clk *pll3_clk;
+static struct clk *pll2_400_clk;
+static struct clk *periph_clk2_sel_clk;
+static struct clk *periph_pre_clk;
+static struct clk *pll2_200_clk;
+static struct clk *periph_clk;
+static struct clk *mmdc_clk;
+static struct clk *periph_clk2_clk;
+static struct clk *pll2_bus_clk;
+
 static struct delayed_work low_bus_freq_handler;
 static struct delayed_work bus_freq_daemon;
 
@@ -143,6 +153,92 @@ int unregister_busfreq_notifier(struct notifier_block *nb)
 }
 EXPORT_SYMBOL(unregister_busfreq_notifier);
 
+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);
+		if (ddr_type == MMDC_MDMISC_DDR_TYPE_DDR3)
+			busfreq_func.update(LOW_AUDIO_CLK);
+		else if (ddr_type == MMDC_MDMISC_DDR_TYPE_LPDDR2)
+			busfreq_func.update(HIGH_AUDIO_CLK);
+		/* Make sure periph clk's parent also got updated */
+		clk_set_parent(periph_clk2_sel_clk, pll3_clk);
+		if (ddr_type == MMDC_MDMISC_DDR_TYPE_DDR3)
+			clk_set_parent(periph_pre_clk, pll2_200_clk);
+		else if (ddr_type == MMDC_MDMISC_DDR_TYPE_LPDDR2)
+			clk_set_parent(periph_pre_clk, pll2_400_clk);
+		clk_set_parent(periph_clk, periph_pre_clk);
+
+		/*
+		 * As periph_pre_clk's parent is not changed from
+		 * high mode to audio mode on lpddr2, the clk framework
+		 * will not update its children's freq, but we
+		 * change the mmdc_ch0_axi 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. Calling get_rate will only call
+		 * the .rate_recalc which is all we need.
+		 */
+		if (high_bus_freq_mode && mmdc_clk)
+			if (ddr_type == IMX_DDR_TYPE_LPDDR2)
+				clk_get_rate(mmdc_clk);
+
+		audio_bus_freq_mode = 1;
+		low_bus_freq_mode = 0;
+		cur_bus_freq_mode = BUS_FREQ_AUDIO;
+	} else {
+		busfreq_func.update(LPAPM_CLK);
+
+		/* Make sure periph clk's parent also got updated */
+		clk_set_parent(periph_clk2_sel_clk, osc_clk);
+		/* Set periph_clk parent to OSC via periph_clk2_sel */
+		clk_set_parent(periph_clk, periph_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 exit_lpm_imx6_smp(void)
+{
+	struct clk *periph_clk_parent;
+
+	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);
+
+	busfreq_func.update(ddr_normal_rate);
+
+	/* Make sure periph clk's parent also got updated */
+	clk_set_parent(periph_clk2_sel_clk, pll3_clk);
+	clk_set_parent(periph_pre_clk, periph_clk_parent);
+	clk_set_parent(periph_clk, periph_pre_clk);
+
+	/*
+	 * As periph_pre_clk's parent is not changed from
+	 * high mode to audio mode on lpddr2, the clk framework
+	 * will not update its children's freq, but we
+	 * change the mmdc_ch0_axi 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. Calling get_rate will only call
+	 * the .rate_recalc which is all we need.
+	 */
+	if (audio_bus_freq_mode && mmdc_clk)
+		if (ddr_type == IMX_DDR_TYPE_LPDDR2)
+			clk_get_rate(mmdc_clk);
+
+	clk_disable_unprepare(pll2_400_clk);
+	if (audio_bus_freq_mode)
+		clk_disable_unprepare(pll2_400_clk);
+}
+
 static void enter_lpm_imx7d(void)
 {
 	/*
@@ -200,6 +296,9 @@ static void exit_lpm_imx7d(void)
 
 static void reduce_bus_freq(void)
 {
+	if (cpu_is_imx6())
+		clk_prepare_enable(pll3_clk);
+
 	if (audio_bus_count && (low_bus_freq_mode || ultra_low_bus_freq_mode))
 		busfreq_notify(LOW_BUSFREQ_EXIT);
 	else if (!audio_bus_count)
@@ -207,10 +306,15 @@ static void reduce_bus_freq(void)
 
 	if (cpu_is_imx7d())
 		enter_lpm_imx7d();
+	else if (cpu_is_imx6q())
+		enter_lpm_imx6_smp();
 
 	med_bus_freq_mode = 0;
 	high_bus_freq_mode = 0;
 
+	if (cpu_is_imx6())
+		clk_disable_unprepare(pll3_clk);
+
 	if (audio_bus_freq_mode)
 		dev_dbg(busfreq_dev,
 			"Bus freq set to audio mode. Count: high %d, med %d, audio %d\n",
@@ -297,8 +401,13 @@ static int set_high_bus_freq(int high_bus_freq)
 	if (low_bus_freq_mode || ultra_low_bus_freq_mode)
 		busfreq_notify(LOW_BUSFREQ_EXIT);
 
+	if (cpu_is_imx6())
+		clk_prepare_enable(pll3_clk);
+
 	if (cpu_is_imx7d())
 		exit_lpm_imx7d();
+	else if (cpu_is_imx6q())
+		exit_lpm_imx6_smp();
 
 	high_bus_freq_mode = 1;
 	med_bus_freq_mode = 0;
@@ -306,6 +415,9 @@ static int set_high_bus_freq(int high_bus_freq)
 	audio_bus_freq_mode = 0;
 	cur_bus_freq_mode = BUS_FREQ_HIGH;
 
+	if (cpu_is_imx6())
+		clk_disable_unprepare(pll3_clk);
+
 	if (high_bus_freq_mode)
 		dev_dbg(busfreq_dev,
 			"Bus freq set to high mode. Count: high %d, med %d, audio %d\n",
@@ -612,6 +724,35 @@ static int busfreq_probe(struct platform_device *pdev)
 	if (!ddr_freq_change_iram_base)
 		return -ENOMEM;
 
+	if (cpu_is_imx6()) {
+		osc_clk = devm_clk_get(&pdev->dev, "osc");
+		pll2_400_clk = devm_clk_get(&pdev->dev, "pll2_pfd2_396m");
+		pll2_200_clk = devm_clk_get(&pdev->dev, "pll2_198m");
+		pll2_bus_clk = devm_clk_get(&pdev->dev, "pll2_bus");
+		pll3_clk = devm_clk_get(&pdev->dev, "pll3_usb_otg");
+		periph_clk = devm_clk_get(&pdev->dev, "periph");
+		periph_pre_clk = devm_clk_get(&pdev->dev, "periph_pre");
+		periph_clk2_clk = devm_clk_get(&pdev->dev, "periph_clk2");
+		periph_clk2_sel_clk = devm_clk_get(&pdev->dev,
+			"periph_clk2_sel");
+		if (IS_ERR(osc_clk) || IS_ERR(pll2_400_clk)
+			|| IS_ERR(pll2_200_clk) || IS_ERR(pll2_bus_clk)
+			|| IS_ERR(pll3_clk) || IS_ERR(periph_clk)
+			|| IS_ERR(periph_pre_clk) || IS_ERR(periph_clk2_clk)
+			|| IS_ERR(periph_clk2_sel_clk)) {
+			dev_err(busfreq_dev,
+				"%s: failed to get busfreq clk\n", __func__);
+			return -EINVAL;
+		}
+	}
+
+	if (cpu_is_imx6q()) {
+		mmdc_clk = devm_clk_get(&pdev->dev, "mmdc");
+		if (IS_ERR(mmdc_clk)) {
+			mmdc_clk = NULL;
+		}
+	}
+
 	if (cpu_is_imx7d()) {
 		osc_clk = devm_clk_get(&pdev->dev, "osc");
 		axi_sel_clk = devm_clk_get(&pdev->dev, "axi_sel");
@@ -701,6 +842,15 @@ 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_imx6q()) {
+		ddr_type = imx_mmdc_get_ddr_type();
+		if (ddr_type == MMDC_MDMISC_DDR_TYPE_DDR3) {
+			busfreq_func.init   = &init_mmdc_ddr3_settings_imx6_smp;
+			busfreq_func.update = &update_ddr_freq_imx_smp;
+		} else if (ddr_type == MMDC_MDMISC_DDR_TYPE_LPDDR2) {
+			busfreq_func.init   = &init_mmdc_lpddr2_settings_mx6q;
+			busfreq_func.update = &update_lpddr2_freq_smp;
+		}
 	}
 
 	if (busfreq_func.init)
diff --git a/arch/arm/mach-imx/busfreq_ddr3.c b/arch/arm/mach-imx/busfreq_ddr3.c
index 3bd03dab05e6..612dd0be512f 100644
--- a/arch/arm/mach-imx/busfreq_ddr3.c
+++ b/arch/arm/mach-imx/busfreq_ddr3.c
@@ -73,19 +73,30 @@ struct imx6_busfreq_info {
 } __aligned(8);
 
 /* DDR settings */
+static unsigned long (*iram_ddr_settings)[2];
+static unsigned long (*normal_mmdc_settings)[2];
 static unsigned long (*iram_iomux_settings)[2];
 
+static void __iomem *mmdc_base;
+static void __iomem *iomux_base;
 static void __iomem *gic_dist_base;
 
+static int ddr_settings_size;
+static int iomux_settings_size;
 static int curr_ddr_rate;
 
 void (*imx7d_change_ddr_freq)(u32 freq) = NULL;
 extern void imx7d_ddr3_freq_change(u32 freq);
 extern void imx_lpddr3_freq_change(u32 freq);
 
+void (*mx6_change_ddr_freq)(u32 freq, void *ddr_settings,
+	bool dll_mode, void *iomux_offsets) = NULL;
+
 extern unsigned int ddr_normal_rate;
 extern int low_bus_freq_mode;
 extern int audio_bus_freq_mode;
+extern void mx6_ddr3_freq_change(u32 freq, void *ddr_settings,
+	bool dll_mode, void *iomux_offsets);
 
 extern unsigned long save_ttbr1(void);
 extern void restore_ttbr1(unsigned long ttbr1);
@@ -94,7 +105,10 @@ extern unsigned long ddr_freq_change_iram_base;
 extern unsigned long ddr_freq_change_total_size;
 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");
 #ifdef CONFIG_SMP
+static unsigned long wfe_freq_change_iram_base;
 volatile u32 *wait_for_ddr_freq_update;
 static unsigned int online_cpus;
 static u32 *irqs_used;
@@ -105,9 +119,41 @@ extern void wfe_smp_freq_change(u32 cpuid, u32 *ddr_freq_change_done);
 extern void imx7_smp_wfe(u32 cpuid, u32 ocram_base);
 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;
+extern void __iomem *scu_base;
 #endif
 
+unsigned long ddr3_dll_mx6q[][2] = {
+	{0x0c, 0x0},
+	{0x10, 0x0},
+	{0x1C, 0x04088032},
+	{0x1C, 0x0408803a},
+	{0x1C, 0x08408030},
+	{0x1C, 0x08408038},
+	{0x818, 0x0},
+	{0x18, 0x0},
+};
+
+unsigned long ddr3_calibration[][2] = {
+	{0x83c, 0x0},
+	{0x840, 0x0},
+	{0x483c, 0x0},
+	{0x4840, 0x0},
+	{0x848, 0x0},
+	{0x4848, 0x0},
+	{0x850, 0x0},
+	{0x4850, 0x0},
+};
+
+unsigned long iomux_offsets_mx6q[][2] = {
+	{0x5A8, 0x0},
+	{0x5B0, 0x0},
+	{0x524, 0x0},
+	{0x51C, 0x0},
+	{0x518, 0x0},
+	{0x50C, 0x0},
+	{0x5B8, 0x0},
+	{0x5C0, 0x0},
+};
 int can_change_ddr_freq(void)
 {
 	return 1;
@@ -149,10 +195,13 @@ int update_ddr_freq_imx_smp(int ddr_rate)
 {
 	int me = 0;
 	unsigned long ttbr1;
+	bool dll_off = false;
+	int i;
 #ifdef CONFIG_SMP
 	unsigned int reg = 0;
 	int cpu = 0;
 #endif
+	int mode = get_bus_freq_mode();
 
 	if (!can_change_ddr_freq())
 		return -1;
@@ -162,6 +211,22 @@ int update_ddr_freq_imx_smp(int ddr_rate)
 
 	printk(KERN_DEBUG "\nBus freq set to %d start...\n", ddr_rate);
 
+	if (cpu_is_imx6()) {
+		if ((mode == BUS_FREQ_LOW) || (mode == BUS_FREQ_AUDIO))
+			dll_off = true;
+
+		iram_ddr_settings[0][0] = ddr_settings_size;
+		iram_iomux_settings[0][0] = iomux_settings_size;
+		if (ddr_rate == ddr_normal_rate) {
+			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];
+			}
+		}
+	}
+
 	/* ensure that all Cores are in WFE. */
 	local_irq_disable();
 
@@ -176,6 +241,8 @@ int update_ddr_freq_imx_smp(int ddr_rate)
 		for_each_online_cpu(cpu) {
 			if (cpu_is_imx7d())
 				reg = *(wait_for_ddr_freq_update + 1);
+			else if (cpu_is_imx6())
+				reg = __raw_readl(scu_base + 0x08);
 
 			if (reg & (0x02 << (cpu * 8)))
 				not_exited_busfreq = true;
@@ -189,6 +256,8 @@ int update_ddr_freq_imx_smp(int ddr_rate)
 	dsb();
 	if (cpu_is_imx7d())
 		online_cpus = *(wait_for_ddr_freq_update + 1);
+	else if (cpu_is_imx6())
+		online_cpus = readl_relaxed(scu_base + 0x08);
 	for_each_online_cpu(cpu) {
 		*((char *)(&online_cpus) + (u8)cpu) = 0x02;
 		if (cpu != me) {
@@ -204,6 +273,8 @@ int update_ddr_freq_imx_smp(int ddr_rate)
 
 		if (cpu_is_imx7d())
 			reg = *(wait_for_ddr_freq_update + 1);
+		else if (cpu_is_imx6())
+			reg = readl_relaxed(scu_base + 0x08);
 		reg |= (0x02 << (me * 8));
 		if (reg == online_cpus)
 			break;
@@ -213,11 +284,17 @@ int update_ddr_freq_imx_smp(int ddr_rate)
 	/* Ensure iram_tlb_phys_addr is flushed to DDR. */
 	__cpuc_flush_dcache_area(&iram_tlb_phys_addr,
 		sizeof(iram_tlb_phys_addr));
+	if (cpu_is_imx6())
+		outer_clean_range(__pa(&iram_tlb_phys_addr),
+			__pa(&iram_tlb_phys_addr + 1));
 
 	ttbr1 = save_ttbr1();
 	/* Now we can change the DDR frequency. */
 	if (cpu_is_imx7d())
 		imx7d_change_ddr_freq(ddr_rate);
+	else if (cpu_is_imx6())
+		mx6_change_ddr_freq(ddr_rate, iram_ddr_settings,
+			dll_off, iram_iomux_settings);
 	restore_ttbr1(ttbr1);
 	curr_ddr_rate = ddr_rate;
 
@@ -308,3 +385,160 @@ int init_ddrc_ddr_settings(struct platform_device *busfreq_pdev)
 
 	return 0;
 }
+
+int init_mmdc_ddr3_settings_imx6_smp(struct platform_device *busfreq_pdev)
+{
+	int i;
+	struct device_node *node;
+	unsigned long ddr_code_size;
+	unsigned long wfe_code_size = 0;
+#ifdef CONFIG_SMP
+	u32 cpu;
+	struct device *dev = &busfreq_pdev->dev;
+	int err;
+	struct irq_data *d;
+#endif
+
+	node = of_find_compatible_node(NULL, NULL, "fsl,imx6q-mmdc-combine");
+	if (!node) {
+		printk(KERN_ERR "failed to find imx6q-mmdc device tree data!\n");
+		return -EINVAL;
+	}
+	mmdc_base = of_iomap(node, 0);
+	WARN(!mmdc_base, "unable to map mmdc registers\n");
+
+	node = NULL;
+	if (cpu_is_imx6q())
+		node = of_find_compatible_node(NULL, NULL, "fsl,imx6q-iomuxc");
+	if (!node) {
+		printk(KERN_ERR "failed to find imx6q-iomux device tree data!\n");
+		return -EINVAL;
+	}
+	iomux_base = of_iomap(node, 0);
+	WARN(!iomux_base, "unable to map iomux registers\n");
+
+	node = NULL;
+	node = of_find_compatible_node(NULL, NULL, "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");
+
+	if (cpu_is_imx6q())
+		ddr_settings_size = ARRAY_SIZE(ddr3_dll_mx6q) +
+			ARRAY_SIZE(ddr3_calibration);
+
+	normal_mmdc_settings = kmalloc((ddr_settings_size * 8), GFP_KERNEL);
+	if (cpu_is_imx6q()) {
+		memcpy(normal_mmdc_settings, ddr3_dll_mx6q,
+			sizeof(ddr3_dll_mx6q));
+		memcpy(((char *)normal_mmdc_settings + sizeof(ddr3_dll_mx6q)),
+			ddr3_calibration, sizeof(ddr3_calibration));
+	}
+	/* 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]);
+	}
+
+#ifdef CONFIG_SMP
+	irqs_used = devm_kzalloc(dev, sizeof(u32) * num_present_cpus(),
+					GFP_KERNEL);
+
+	for_each_online_cpu(cpu) {
+		int irq;
+
+		/*
+		 * set up a reserved interrupt to get all
+		 * the active cores into a WFE state
+		 * before changing the DDR frequency.
+		 */
+		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;
+	}
+#endif
+	iomux_settings_size = ARRAY_SIZE(iomux_offsets_mx6q);
+
+	ddr_code_size = (&mx6_ddr3_freq_change_end -
+		&mx6_ddr3_freq_change_start) * 4;
+
+	mx6_change_ddr_freq = (void *)fncpy((void *)ddr_freq_change_iram_base,
+		&mx6_ddr3_freq_change, ddr_code_size);
+
+	/*
+	 * 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;
+#ifdef CONFIG_SMP
+	wfe_freq_change_iram_base = (unsigned long)((u32 *)iram_ddr_settings +
+		(ddr_settings_size * 8) + 8);
+
+	if (wfe_freq_change_iram_base & (FNCPY_ALIGN - 1))
+			wfe_freq_change_iram_base += FNCPY_ALIGN -
+			((uintptr_t)wfe_freq_change_iram_base % (FNCPY_ALIGN));
+
+	wfe_code_size = (&wfe_smp_freq_change_end -
+		&wfe_smp_freq_change_start) *4;
+
+	wfe_change_ddr_freq = (void *)fncpy((void *)wfe_freq_change_iram_base,
+		&wfe_smp_freq_change, wfe_code_size);
+
+	/*
+	 * Store the variable used to communicate
+	 * between cores in a non-cacheable IRAM area
+	 */
+	wait_for_ddr_freq_update = (u32 *)&iram_iomux_settings[0][1];
+#endif
+
+	if ((ddr_code_size + wfe_code_size + (iomux_settings_size +
+		ddr_settings_size) * 8 + 16)
+		> ddr_freq_change_total_size) {
+		printk(KERN_ERR "Not enough memory for DDR Freq scale.\n");
+		return EINVAL;
+	}
+
+	if (cpu_is_imx6q()) {
+		/* store the IOMUX settings at boot. */
+		for (i = 0; i < iomux_settings_size; i++) {
+			iomux_offsets_mx6q[i][1] =
+				readl_relaxed(iomux_base +
+					iomux_offsets_mx6q[i][0]);
+			iram_iomux_settings[i + 1][0] =
+				iomux_offsets_mx6q[i][0];
+			iram_iomux_settings[i + 1][1] =
+				iomux_offsets_mx6q[i][1];
+		}
+	}
+
+	curr_ddr_rate = ddr_normal_rate;
+
+	return 0;
+}
diff --git a/arch/arm/mach-imx/busfreq_lpddr2.c b/arch/arm/mach-imx/busfreq_lpddr2.c
index 2ef1806bbc35..1d2eda2389b9 100644
--- a/arch/arm/mach-imx/busfreq_lpddr2.c
+++ b/arch/arm/mach-imx/busfreq_lpddr2.c
@@ -56,8 +56,6 @@ 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 void imx6sll_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);
@@ -98,7 +96,7 @@ 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;
+extern void __iomem *scu_base;
 static void __iomem *gic_dist_base;
 #endif
 
@@ -163,19 +161,6 @@ 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() || cpu_is_imx6ul() || cpu_is_imx6ull())
-		mx6_change_lpddr2_freq = (void *)fncpy(
-			(void *)ddr_freq_change_iram_base,
-			&imx6_up_lpddr2_freq_change, ddr_code_size);
-	if (cpu_is_imx6sll())
-		mx6_change_lpddr2_freq = (void *)fncpy(
-			(void *)ddr_freq_change_iram_base,
-			&imx6sll_lpddr2_freq_change, ddr_code_size);
-
 	curr_ddr_rate = ddr_normal_rate;
 
 	return 0;
@@ -214,7 +199,7 @@ int update_lpddr2_freq_smp(int ddr_rate)
 	while (1) {
 		bool not_exited_busfreq = false;
 		for_each_online_cpu(cpu) {
-			reg = __raw_readl(imx_scu_base + 0x08);
+			reg = __raw_readl(scu_base + 0x08);
 			if (reg & (0x02 << (cpu * 8)))
 				not_exited_busfreq = true;
 		}
@@ -225,7 +210,7 @@ int update_lpddr2_freq_smp(int ddr_rate)
 	wmb();
 	*wait_for_lpddr2_freq_update = 1;
 	dsb();
-	online_cpus = readl_relaxed(imx_scu_base + 0x08);
+	online_cpus = readl_relaxed(scu_base + 0x08);
 	for_each_online_cpu(cpu) {
 		*((char *)(&online_cpus) + (u8)cpu) = 0x02;
 		if (cpu != me) {
@@ -238,7 +223,7 @@ int update_lpddr2_freq_smp(int ddr_rate)
 	/* Wait for the other active CPUs to idle */
 	while (1) {
 		reg = 0;
-		reg = readl_relaxed(imx_scu_base + 0x08);
+		reg = readl_relaxed(scu_base + 0x08);
 		reg |= (0x02 << (me * 8));
 		if (reg == online_cpus)
 			break;
diff --git a/arch/arm/mach-imx/common.h b/arch/arm/mach-imx/common.h
index 60add3295015..b6395f68d483 100644
--- a/arch/arm/mach-imx/common.h
+++ b/arch/arm/mach-imx/common.h
@@ -106,6 +106,7 @@ int imx_mmdc_get_ddr_type(void);
 int imx7ulp_set_lpm(enum ulp_cpu_pwr_mode mode);
 void imx_busfreq_map_io(void);
 void imx7_pm_map_io(void);
+void imx6_pm_map_io(void);
 
 void imx_cpu_die(unsigned int cpu);
 int imx_cpu_kill(unsigned int cpu);
diff --git a/arch/arm/mach-imx/ddr3_freq_imx6.S b/arch/arm/mach-imx/ddr3_freq_imx6.S
new file mode 100644
index 000000000000..e6f7f74f7d32
--- /dev/null
+++ b/arch/arm/mach-imx/ddr3_freq_imx6.S
@@ -0,0 +1,1103 @@
+/*
+ * 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 <asm/smp_scu.h>
+#include "hardware.h"
+
+#define MMDC0_MDPDC		0x4
+#define MMDC0_MDCF0			0x0c
+#define MMDC0_MDCF1			0x10
+#define MMDC0_MDMISC		0x18
+#define MMDC0_MDSCR		0x1c
+#define MMDC0_MAARCR		0x400
+#define MMDC0_MAPSR			0x404
+#define MMDC0_MADPCR0		0x410
+#define MMDC0_MPZQHWCTRL	0x800
+#define MMDC1_MPZQHWCTRL	0x4800
+#define MMDC0_MPODTCTRL	0x818
+#define MMDC1_MPODTCTRL	0x4818
+#define MMDC0_MPDGCTRL0	0x83c
+#define MMDC1_MPDGCTRL0	0x483c
+#define MMDC0_MPMUR0		0x8b8
+#define MMDC1_MPMUR0		0x48b8
+
+#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 IMX6QP_REVISION_ID 0x630100
+#define ANADIG_DIGPROG     0x260
+
+.extern iram_tlb_phys_addr
+
+.globl mx6_ddr3_freq_change_start
+.globl mx6_ddr3_freq_change_end
+
+	.align 3
+
+	.macro is_mx6qp
+
+	/* check if the SOC is i.MX6QP */
+	ldr	r0, =IMX_IO_P2V(MX6Q_ANATOP_BASE_ADDR)
+	ldr	r1, [r0, #ANADIG_DIGPROG]
+	ldr	r2, =IMX6QP_REVISION_ID
+	cmp	r1, r2
+
+	.endm
+
+	.macro	switch_to_528MHz
+
+	/* check if periph_clk_sel is already set */
+	ldr	r0, [r6, #CCM_CBCDR]
+	and	r0, r0, #(1 << 25)
+	cmp	r0, #(1 << 25)
+	beq	set_ahb_podf_before_switch
+
+	/* change periph_clk to be sourced from pll3_clk. */
+	ldr	r0, [r6, #CCM_CBCMR]
+	bic	r0, r0, #(3 << 12)
+	str	r0, [r6, #CCM_CBCMR]
+
+	ldr	r0, [r6, #CCM_CBCDR]
+	bic	r0, r0, #(0x38 << 20)
+	str	r0, [r6, #CCM_CBCDR]
+
+	/*
+	 * set the AHB dividers before the switch,
+	 * don't change AXI clock divider,
+	 * set the MMDC_DIV=1, AXI_DIV = 2, AHB_DIV=4,
+	 */
+	ldr	r0, [r6, #CCM_CBCDR]
+	ldr	r2, =0x3f1f00
+	bic	r0, r0, r2
+	orr	r0, r0, #0xd00
+	orr	r0, r0, #(1 << 16)
+	str	r0, [r6, #CCM_CBCDR]
+
+wait_div_update528:
+	ldr	r0, [r6, #CCM_CDHIPR]
+	cmp	r0, #0
+	bne	wait_div_update528
+
+	/* now switch periph_clk to pll3_main_clk. */
+	ldr	r0, [r6, #CCM_CBCDR]
+	orr	r0, r0, #(1 << 25)
+	str	r0, [r6, #CCM_CBCDR]
+
+periph_clk_switch3:
+	ldr	r0, [r6, #CCM_CDHIPR]
+	cmp	r0, #0
+	bne	periph_clk_switch3
+
+	b	switch_pre_periph_clk_528
+
+set_ahb_podf_before_switch:
+	/*
+	 * set the MMDC_DIV=1, AXI_DIV = 2, AHB_DIV=4,
+	 */
+	ldr	r0, [r6, #CCM_CBCDR]
+	ldr	r2, =0x3f1f00
+	bic	r0, r0, r2
+	orr	r0, r0, #0xd00
+	orr	r0, r0, #(1 << 16)
+	str	r0, [r6, #CCM_CBCDR]
+
+wait_div_update528_1:
+	ldr	r0, [r6, #CCM_CDHIPR]
+	cmp	r0, #0
+	bne	wait_div_update528_1
+
+switch_pre_periph_clk_528:
+
+	/* now switch pre_periph_clk to PLL2_528MHz. */
+	ldr	r0, [r6, #CCM_CBCMR]
+	bic	r0, r0, #(0xc << 16)
+	str	r0, [r6, #CCM_CBCMR]
+
+	/* now switch periph_clk back. */
+	ldr	r0, [r6, #CCM_CBCDR]
+	bic	r0, r0, #(1 << 25)
+	str	r0, [r6, #CCM_CBCDR]
+
+periph_clk_switch4:
+	ldr	r0, [r6, #CCM_CDHIPR]
+	cmp	r0, #0
+	bne	periph_clk_switch4
+
+	.endm
+
+	.macro	switch_to_400MHz
+
+	/* check if periph_clk_sel is already set. */
+	ldr	r0, [r6, #CCM_CBCDR]
+	and	r0, r0, #(1 << 25)
+	cmp	r0, #(1 << 25)
+	beq	set_ahb_podf_before_switch1
+
+	/* change periph_clk to be sourced from pll3_clk. */
+	ldr	r0, [r6, #CCM_CBCMR]
+	bic	r0, r0, #(3 << 12)
+	str	r0, [r6, #CCM_CBCMR]
+
+	ldr	r0, [r6, #CCM_CBCDR]
+	bic	r0, r0, #(0x38 << 24)
+	str	r0, [r6, #CCM_CBCDR]
+
+	/* now switch periph_clk to pll3_main_clk. */
+	ldr	r0, [r6, #CCM_CBCDR]
+	orr	r0, r0, #(1 << 25)
+	str	r0, [r6, #CCM_CBCDR]
+
+periph_clk_switch5:
+	ldr	r0, [r6, #CCM_CDHIPR]
+	cmp	r0, #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	r0, [r6, #CCM_CBCDR]
+	ldr	r2, =0x3f1f00
+	bic	r0, r0, r2
+	orr	r0, r0, #(0x9 << 8)
+	orr	r0, r0, #(1 << 16)
+	str	r0, [r6, #CCM_CBCDR]
+
+wait_div_update400_1:
+	ldr	r0, [r6, #CCM_CDHIPR]
+	cmp	r0, #0
+	bne	wait_div_update400_1
+
+switch_pre_periph_clk_400:
+
+	/* now switch pre_periph_clk to PFD_400MHz. */
+	ldr	r0, [r6, #CCM_CBCMR]
+	bic	r0, r0, #(0xc << 16)
+	orr	r0, r0, #(0x4 << 16)
+	str	r0, [r6, #CCM_CBCMR]
+
+	/* now switch periph_clk back. */
+	ldr	r0, [r6, #CCM_CBCDR]
+	bic	r0, r0, #(1 << 25)
+	str	r0, [r6, #CCM_CBCDR]
+
+periph_clk_switch6:
+	ldr	r0, [r6, #CCM_CDHIPR]
+	cmp	r0, #0
+	bne	periph_clk_switch6
+
+	/*
+	 * change AHB divider so that we are at 400/3=133MHz.
+	 * don't change AXI clock divider.
+	 * set the MMDC_DIV=1, AXI_DIV=2, AHB_DIV=3,
+	 */
+	ldr	r0, [r6, #CCM_CBCDR]
+	ldr	r2, =0x3f1f00
+	bic	r0, r0, r2
+	orr	r0, r0, #(0x9 << 8)
+	orr	r0, r0, #(1 << 16)
+	str	r0, [r6, #CCM_CBCDR]
+
+wait_div_update400_2:
+	ldr	r0, [r6, #CCM_CDHIPR]
+	cmp	r0, #0
+	bne	wait_div_update400_2
+
+	.endm
+
+	.macro	switch_to_50MHz
+
+	/* check if periph_clk_sel is already set. */
+	ldr	r0, [r6, #CCM_CBCDR]
+	and	r0, r0, #(1 << 25)
+	cmp	r0, #(1 << 25)
+	beq	switch_pre_periph_clk_50
+
+	/*
+	 * set the periph_clk to be sourced from PLL2_PFD_200M
+	 * change periph_clk to be sourced from pll3_clk.
+	 * ensure PLL3 is the source and set the divider to 1.
+	 */
+	ldr	r0, [r6, #CCM_CBCMR]
+	bic	r0, r0, #(0x3 << 12)
+	str	r0, [r6, #CCM_CBCMR]
+
+	ldr	r0, [r6, #CCM_CBCDR]
+	bic	r0, r0, #(0x38 << 24)
+	str	r0, [r6, #CCM_CBCDR]
+
+	/* now switch periph_clk to pll3_main_clk. */
+	ldr	r0, [r6, #CCM_CBCDR]
+	orr	r0, r0, #(1 << 25)
+	str	r0, [r6, #CCM_CBCDR]
+
+periph_clk_switch_50:
+	ldr	r0, [r6, #CCM_CDHIPR]
+	cmp	r0, #0
+	bne	periph_clk_switch_50
+
+switch_pre_periph_clk_50:
+
+	/* now switch pre_periph_clk to PFD_200MHz. */
+	ldr	r0, [r6, #CCM_CBCMR]
+	orr	r0, r0, #(0xc << 16)
+	str	r0, [r6, #CCM_CBCMR]
+
+	/*
+	 * set the MMDC_DIV=4, AXI_DIV = 4, AHB_DIV=8,
+	 */
+	ldr	r0, [r6, #CCM_CBCDR]
+	ldr	r2, =0x3f1f00
+	bic	r0, r0, r2
+	orr	r0, r0, #(0x18 << 16)
+	orr	r0, r0, #(0x3 << 16)
+
+	/*
+	 * if changing AHB divider remember to change
+	 * the IPGPER divider too below.
+	 */
+	orr	r0, r0, #0x1d00
+	str	r0, [r6, #CCM_CBCDR]
+
+wait_div_update_50:
+	ldr	r0, [r6, #CCM_CDHIPR]
+	cmp	r0, #0
+	bne	wait_div_update_50
+
+	/* now switch periph_clk back. */
+	ldr	r0, [r6, #CCM_CBCDR]
+	bic	r0, r0, #(1 << 25)
+	str	r0, [r6, #CCM_CBCDR]
+
+periph_clk_switch2:
+	ldr	r0, [r6, #CCM_CDHIPR]
+	cmp	r0, #0
+	bne	periph_clk_switch2
+
+	.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	r0, [r6, #CCM_CBCMR]
+	bic	r0, r0, #(0x3 << 12)
+	orr	r0, r0, #(1 << 12)
+	str	r0, [r6, #CCM_CBCMR]
+
+	ldr	r0, [r6, #CCM_CBCDR]
+	bic	r0, r0, #(0x38 << 24)
+	str	r0, [r6, #CCM_CBCDR]
+
+	/* now switch periph_clk to 24MHz. */
+	ldr	r0, [r6, #CCM_CBCDR]
+	orr	r0, r0, #(1 << 25)
+	str	r0, [r6, #CCM_CBCDR]
+
+periph_clk_switch1:
+	ldr	r0, [r6, #CCM_CDHIPR]
+	cmp	r0, #0
+	bne	periph_clk_switch1
+
+	/* change all the dividers to 1. */
+	ldr	r0, [r6, #CCM_CBCDR]
+	ldr	r2, =0x3f1f00
+	bic	r0, r0, r2
+	orr	r0, r0, #(1 << 8)
+	str	r0, [r6, #CCM_CBCDR]
+
+	/* Wait for the divider to change. */
+wait_div_update:
+	ldr	r0, [r6, #CCM_CDHIPR]
+	cmp	r0, #0
+	bne	wait_div_update
+
+	.endm
+
+	.macro	disable_l1_dcache
+
+	/*
+	 * Flush all data from the L1 data cache before disabling
+	 * SCTLR.C bit.
+	 */
+	push	{r0 - r11, lr}
+
+	ldr	r7, =v7_flush_kern_cache_all
+	mov	lr, pc
+	mov	pc, r7
+	pop	{r0 - r11, lr}
+
+	/* disable d-cache */
+	mrc	p15, 0, r6, c1, c0, 0
+	bic	r6, r6, #0x4
+	mcr	p15, 0, r6, c1, c0, 0
+	dsb
+	isb
+
+	push	{r0 - r11, lr}
+
+	ldr	r7, =v7_flush_kern_cache_all
+	mov	lr, pc
+	mov	pc, r7
+	pop	{r0 - r11, lr}
+
+	.endm
+
+/*
+ *  mx6_ddr3_freq_change
+ *
+ *  idle the processor (eg, wait for interrupt).
+ *  make sure DDR is in self-refresh.
+ *  IRQs are already disabled.
+ */
+ENTRY(mx6_ddr3_freq_change)
+
+mx6_ddr3_freq_change_start:
+	stmfd	sp!, {r4-r12}
+
+	/*
+	 * r5 -> mmdc_base
+	 * r6 -> ccm_base
+	 * r7 -> iomux_base
+	 * r12 -> l2_base
+	 */
+	mov	r4, r0
+	mov	r8, r1
+	mov	r9, r2
+	mov	r11, r3
+
+	/* flush the TLB */
+	ldr	r6, =0x0
+	mcr	p15, 0, r6, c8, c3, 0
+
+	ldr	r6, =iram_tlb_phys_addr
+	ldr	r7, [r6]
+
+	/*
+	  * Need to flush and disable L1 before
+	  * disabling L2, we need data to
+	  * coherent. Flushing L1 pushes
+	  * everyhting to L2. We sync L2 later, but
+	  * it can still have dirty lines.
+	  * While exiting, we need to enable L2 first
+	  * and then L1.
+	  */
+	disable_l1_dcache
+
+#ifdef CONFIG_CACHE_L2X0
+	/*
+	 * Make sure the L2 buffers are drained.
+	 * Sync operation on L2 drains the buffers.
+	 */
+	ldr	r12, =IMX_IO_P2V(MX6Q_L2_BASE_ADDR)
+
+	/* Wait for background operations to complete. */
+wait_for_l2_to_idle:
+	ldr	r1, [r12, #L2_CACHE_SYNC]
+	cmp	r1, #0x0
+	bne	wait_for_l2_to_idle
+
+	mov	r1, #0x0
+	str	r1, [r12, #L2_CACHE_SYNC]
+
+	dsb
+	isb
+
+	ldr	r1, [r12, #PL310_AUX_CTRL]
+	tst	r1, #PL310_AUX_16WAY_BIT
+	mov	r1, #PL310_8WAYS_MASK
+	orrne	r1, #PL310_16WAYS_UPPERMASK
+	mov	r6, #PL310_LOCKDOWN_NBREGS
+	add	r5, r12, #PL310_DCACHE_LOCKDOWN_BASE
+1:	/* lock Dcache and Icache */
+	str	r1, [r5], #PL310_LOCKDOWN_SZREG
+	str	r1, [r5], #PL310_LOCKDOWN_SZREG
+	subs	r6, r6, #1
+	bne	1b
+#endif
+
+	/*
+	 * 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.
+	 */
+
+
+	/* Now switch the TTBR. */
+	/* 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
+
+
+	ldr	r5, =IMX_IO_P2V(MX6Q_MMDC_P0_BASE_ADDR)
+	ldr	r6, =IMX_IO_P2V(MX6Q_CCM_BASE_ADDR)
+	ldr	r7, =IMX_IO_P2V(MX6Q_IOMUXC_BASE_ADDR)
+
+	/* Read the Original MU delay value */
+	ldr	r1, [r5, #MMDC0_MPMUR0]
+	mov	r10, r1, lsr #16
+	ldr	r1, =0x3ff
+	and	r10, r10, r1
+
+	/* disable automatic power saving. */
+	ldr	r0, [r5, #MMDC0_MAPSR]
+	orr	r0, r0, #0x01
+	str	r0, [r5, #MMDC0_MAPSR]
+
+	/* disable MMDC power down timer. */
+	ldr	r0, [r5, #MMDC0_MDPDC]
+	bic	r0, r0, #(0xff << 8)
+	str	r0, [r5, #MMDC0_MDPDC]
+
+	/* delay for a while */
+	ldr	r1, =4
+delay1:
+	ldr	r2, =0
+cont1:
+	ldr	r0, [r5, r2]
+	add	r2, r2, #4
+	cmp	r2, #16
+	bne	cont1
+	sub	r1, r1, #1
+	cmp	r1, #0
+	bgt	delay1
+
+	/* set CON_REG */
+	ldr	r0, =0x8000
+	str	r0, [r5, #MMDC0_MDSCR]
+poll_conreq_set_1:
+	ldr	r0, [r5, #MMDC0_MDSCR]
+	and	r0, r0, #(0x4 << 12)
+	cmp	r0, #(0x4 << 12)
+	bne	poll_conreq_set_1
+
+	/*
+	 * if requested frequency is great than
+	 * 300MHz, skip setting bypass adopt mode.
+	 */
+	ldr	r1, =300000000
+	cmp	r4, r1
+	bge	1f
+
+	is_mx6qp
+	bne	1f
+	/* Switch to adopt mode, set MMDC0_MAARCR bit25~26 to 2b'01 */
+	ldr	r0, [r5, #MMDC0_MAARCR]
+	bic	r0, r0, #(0x3 << 25)
+	orr	r0, #(0x01 << 25)
+	str	r0 , [r5, #MMDC0_MAARCR]
+1:
+	ldr	r0, =0x00008050
+	str	r0, [r5, #MMDC0_MDSCR]
+	ldr	r0, =0x00008058
+	str	r0, [r5, #MMDC0_MDSCR]
+
+	/*
+	 * if requested frequency is greater than
+	 * 300MHz go to DLL on mode.
+	 */
+	ldr	r1, =300000000
+	cmp	r4, r1
+	bge	dll_on_mode
+
+dll_off_mode:
+
+	/* if DLL is currently on, turn it off. */
+	cmp	r9, #1
+	beq	continue_dll_off_1
+
+	ldr	r0, =0x00018031
+	str	r0, [r5, #MMDC0_MDSCR]
+
+	ldr	r0, =0x00018039
+	str	r0, [r5, #MMDC0_MDSCR]
+
+	ldr	r1, =10
+delay1a:
+	ldr	r2, =0
+cont1a:
+	ldr	r0, [r5, r2]
+	add	r2, r2, #4
+	cmp	r2, #16
+	bne	cont1a
+	sub	r1, r1, #1
+	cmp	r1, #0
+	bgt	delay1a
+
+continue_dll_off_1:
+	/* set DVFS - enter self refresh mode */
+	ldr	r0, [r5, #MMDC0_MAPSR]
+	orr	r0, r0, #(1 << 21)
+	str	r0, [r5, #MMDC0_MAPSR]
+
+	/* de-assert con_req */
+	mov	r0, #0x0
+	str	r0, [r5, #MMDC0_MDSCR]
+
+poll_dvfs_set_1:
+	ldr	r0, [r5, #MMDC0_MAPSR]
+	and	r0, r0, #(1 << 25)
+	cmp	r0, #(1 << 25)
+	bne	poll_dvfs_set_1
+
+	ldr	r1, =24000000
+	cmp	r4, r1
+	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	r0, [r5, #MMDC0_MADPCR0]
+	orr	r0, r0, #(1 << 8)
+	str	r0, [r5, #MMDC0_MADPCR0]
+
+	/* clear DVFS - exit from self refresh mode */
+	ldr	r0, [r5, #MMDC0_MAPSR]
+	bic	r0, r0, #(1 << 21)
+	str	r0, [r5, #MMDC0_MAPSR]
+
+poll_dvfs_clear_1:
+	ldr	r0, [r5, #MMDC0_MAPSR]
+	and	r0, r0, #(1 << 25)
+	cmp	r0, #(1 << 25)
+	beq	poll_dvfs_clear_1
+
+	/* if DLL was previously on, continue DLL off routine. */
+	cmp    	r9, #1
+	beq 	continue_dll_off_3
+
+	ldr	r0, =0x00018031
+	str	r0, [r5, #MMDC0_MDSCR]
+
+	ldr	r0, =0x00018039
+	str	r0, [r5, #MMDC0_MDSCR]
+
+	ldr	r0, =0x08208030
+	str	r0, [r5, #MMDC0_MDSCR]
+
+	ldr	r0, =0x08208038
+	str	r0, [r5, #MMDC0_MDSCR]
+
+	ldr	r0, =0x00088032
+	str	r0, [r5, #MMDC0_MDSCR]
+
+	ldr	r0, =0x0008803A
+	str	r0, [r5, #MMDC0_MDSCR]
+
+	/* delay for a while. */
+	ldr	r1, =4
+delay_1:
+	ldr	r2, =0
+cont_1:
+	ldr	r0, [r5, r2]
+	add	r2, r2, #4
+	cmp	r2, #16
+	bne	cont_1
+	sub	r1, r1, #1
+	cmp	r1, #0
+	bgt	delay_1
+
+	ldr	r0, [r5, #MMDC0_MDCF0]
+	bic	r0, r0, #0xf
+	orr	r0, r0, #0x3
+	str	r0, [r5, #MMDC0_MDCF0]
+
+	ldr	r0, [r5, #MMDC0_MDCF1]
+	bic	r0, r0, #0x7
+	orr	r0, r0, #0x4
+	str	r0, [r5, #MMDC0_MDCF1]
+
+	ldr	r0, [r5, #MMDC0_MDMISC]
+	bic	r0, r0, #(0x3 << 16) /* walat = 0x1 */
+	orr	r0, r0, #(0x1 << 16)
+	bic	r0, r0, #(0x7 << 6)  /* ralat = 0x2 */
+	orr	r0, r0, #(0x2 << 6)
+	str	r0, [r5, #MMDC0_MDMISC]
+
+	/* enable dqs pull down in the IOMUX. */
+	ldr	r1, [r11]
+	add	r11, r11, #8
+	ldr	r2, =0x3028
+update_iomux:
+	ldr	r0, [r11, #0x0]
+	ldr	r3, [r7, r0]
+	bic	r3, r3, r2
+	orr	r3, r3, #(0x3 << 12)
+	orr	r3, r3, #0x28
+	str	r3, [r7, r0]
+	add	r11, r11, #8
+	sub	r1, r1, #1
+	cmp	r1, #0
+	bgt	update_iomux
+
+	/*  ODT disabled. */
+	ldr	r0, =0x0
+	ldr	r2, =MMDC0_MPODTCTRL
+	str	r0, [r5, r2]
+	ldr	r2, =MMDC1_MPODTCTRL
+	str	r0, [r5, r2]
+
+	/* DQS gating disabled. */
+	ldr	r2, =MMDC0_MPDGCTRL0
+	ldr	r0, [r5, r2]
+	orr	r0, r0, #(1 << 29)
+	str	r0, [r5, r2]
+
+	ldr	r2, =MMDC1_MPDGCTRL0
+	ldr	r0, [r5, r2]
+	orr	r0, r0, #(0x1 << 29)
+	str	r0, [r5, r2]
+
+	/* Add workaround for ERR005778.*/
+	/* double the original MU_UNIT_DEL_NUM. */
+	lsl	r10, r10, #1
+
+	/* Bypass the automatic MU by setting the mu_byp_en */
+	ldr	r2, [r5, #MMDC0_MPMUR0]
+	orr	r2, r2, #0x400
+	orr	r2, r2, r10
+	str	r2, [r5, #MMDC0_MPMUR0]
+	ldr	r0, =MMDC1_MPMUR0
+	str	r2, [r5, r0]
+
+	/* Now perform a force measure */
+	ldr	r0, [r5, #MMDC0_MPMUR0]
+	orr	r0, r0, #0x800
+	str	r0, [r5, #MMDC0_MPMUR0]
+	ldr	r2, =MMDC1_MPMUR0
+	str	r0, [r5, r2]
+	/* Wait for FRC_MSR to clear. */
+1:
+	ldr	r0, [r5, #MMDC0_MPMUR0]
+	and	r0, r0, #0x800
+	ldr	r1, [r5, r2]
+	and	r1, r1, #0x800
+	orr	r0, r0, r1
+	cmp	r0, #0x0
+	bne	1b
+
+continue_dll_off_3:
+	/* clear SBS - unblock accesses to DDR. */
+	ldr	r0, [r5, #MMDC0_MADPCR0]
+	bic	r0, r0, #(0x1 << 8)
+	str	r0, [r5, #MMDC0_MADPCR0]
+
+	mov	r0, #0x0
+	str	r0, [r5, #MMDC0_MDSCR]
+poll_conreq_clear_1:
+	ldr	r0, [r5, #MMDC0_MDSCR]
+	and	r0, r0, #(0x4 << 12)
+	cmp	r0, #(0x4 << 12)
+	beq	poll_conreq_clear_1
+
+	b	done
+
+dll_on_mode:
+	/* assert DVFS - enter self refresh mode. */
+	ldr	r0, [r5, #MMDC0_MAPSR]
+	orr	r0, r0, #(1 << 21)
+	str	r0, [r5, #MMDC0_MAPSR]
+
+	/* de-assert CON_REQ. */
+	mov	r0, #0x0
+	str	r0, [r5, #MMDC0_MDSCR]
+
+	/* poll DVFS ack. */
+poll_dvfs_set_2:
+	ldr	r0, [r5, #MMDC0_MAPSR]
+	and	r0, r0, #(1 << 25)
+	cmp	r0, #(1 << 25)
+	bne	poll_dvfs_set_2
+
+	ldr	r1, =528000000
+	cmp	r4, r1
+	beq	switch_freq_528
+
+	switch_to_400MHz
+
+	b	continue_dll_on
+
+switch_freq_528:
+	switch_to_528MHz
+
+continue_dll_on:
+
+	/* set SBS step-by-step mode. */
+	ldr	r0, [r5, #MMDC0_MADPCR0]
+	orr	r0, r0, #( 1 << 8)
+	str	r0, [r5, #MMDC0_MADPCR0]
+
+	/* clear DVFS - exit self refresh mode. */
+	ldr	r0, [r5, #MMDC0_MAPSR]
+	bic	r0, r0, #(1 << 21)
+	str	r0, [r5, #MMDC0_MAPSR]
+
+poll_dvfs_clear_2:
+	ldr	r0, [r5, #MMDC0_MAPSR]
+	and	r0, r0, #(1 << 25)
+	cmp	r0, #(1 << 25)
+	beq	poll_dvfs_clear_2
+
+	/* if DLL is currently off, turn it back on. */
+	cmp	r9, #0
+	beq	update_calibration_only
+
+	/* issue zq calibration command */
+	ldr	r0, [r5, #MMDC0_MPZQHWCTRL]
+	orr	r0, r0, #0x3
+	str	r0, [r5, #MMDC0_MPZQHWCTRL]
+	ldr	r2, =MMDC1_MPZQHWCTRL
+	str	r0, [r5, r2]
+
+	/* enable DQS gating. */
+	ldr	r2, =MMDC0_MPDGCTRL0
+	ldr	r0, [r5, r2]
+	bic	r0, r0, #(1 << 29)
+	str	r0, [r5, r2]
+
+	ldr	r2, =MMDC1_MPDGCTRL0
+	ldr	r0, [r5, r2]
+	bic	r0, r0, #(1 << 29)
+	str	r0, [r5, r2]
+
+	/* force measure. */
+	ldr	r0, =0x00000800
+	str	r0, [r5, #MMDC0_MPMUR0]
+	ldr	r2, =MMDC1_MPMUR0
+	str	r0, [r5, r2]
+
+	/* Wait for FRC_MSR to clear. */
+1:
+	ldr	r0, [r5, #MMDC0_MPMUR0]
+	and	r0, r0, #0x800
+	ldr	r1, [r5, r2]
+	and	r1, r1, #0x800
+	orr	r0, r0, r1
+	cmp	r0, #0x0
+	bne	1b
+
+	/* disable dqs pull down in the IOMUX. */
+	ldr	r1, [r11]
+	add	r11, r11, #8
+update_iomux1:
+	ldr	r0, [r11, #0x0]
+	ldr	r3, [r11, #0x4]
+	str	r3, [r7, r0]
+	add	r11, r11, #8
+	sub	r1, r1, #1
+	cmp	r1, #0
+	bgt	update_iomux1
+
+	/* config MMDC timings to 528MHz. */
+	ldr	r9, [r8]
+	add	r8, r8, #8
+	ldr	r0, [r8, #0x0]
+	ldr	r3, [r8, #0x4]
+	str	r3, [r5, r0]
+	add	r8, r8, #8
+
+	ldr	r0, [r8, #0x0]
+	ldr	r3, [r8, #0x4]
+	str	r3, [r5, r0]
+	add	r8, r8, #8
+
+	/* configure ddr devices to dll on, odt. */
+	ldr	r0, =0x00048031
+	str	r0, [r5, #MMDC0_MDSCR]
+
+	ldr	r0, =0x00048039
+	str	r0, [r5, #MMDC0_MDSCR]
+
+	/* delay for while. */
+	ldr	r1, =4
+delay7:
+	ldr	r2, =0
+cont7:
+	ldr	r0, [r5, r2]
+	add	r2, r2, #4
+	cmp	r2, #16
+	bne	cont7
+	sub	r1, r1, #1
+	cmp	r1, #0
+	bgt	delay7
+
+	/* reset dll. */
+	ldr	r0, =0x09408030
+	str	r0, [r5, #MMDC0_MDSCR]
+
+	ldr	r0, =0x09408038
+	str	r0, [r5, #MMDC0_MDSCR]
+
+	/* delay for while. */
+	ldr	r1, =100
+delay8:
+	ldr	r2, =0
+cont8:
+	ldr	r0, [r5, r2]
+	add	r2, r2, #4
+	cmp	r2, #16
+	bne	cont8
+	sub	r1, r1, #1
+	cmp	r1, #0
+	bgt	delay8
+
+	ldr	r0, [r8, #0x0]
+	ldr	r3, [r8, #0x4]
+	str	r3, [r5, r0]
+	add	r8, r8, #8
+
+	ldr	r0, [r8, #0x0]
+	ldr	r3, [r8, #0x4]
+	str	r3, [r5, r0]
+	add	r8, r8, #8
+
+	ldr	r0, =0x00428031
+	str	r0, [r5, #MMDC0_MDSCR]
+
+	ldr	r0, =0x00428039
+	str	r0, [r5, #MMDC0_MDSCR]
+
+	ldr	r0, [r8, #0x0]
+	ldr	r3, [r8, #0x4]
+	str	r3, [r5, r0]
+	add	r8, r8, #8
+
+	ldr	r0, [r8, #0x0]
+	ldr	r3, [r8, #0x4]
+	str	r3, [r5, r0]
+	add	r8, r8, #8
+
+	/* issue a zq command. */
+	ldr	r0, =0x04008040
+	str	r0, [r5, #MMDC0_MDSCR]
+
+	ldr	r0, =0x04008048
+	str	r0, [r5, #MMDC0_MDSCR]
+
+	/* MMDC ODT enable. */
+	ldr	r0, [r8, #0x0]
+	ldr	r3, [r8, #0x4]
+	str	r3, [r5, r0]
+	add	r8, r8, #8
+
+	ldr	r2, =0x4818
+	str	r3, [r5, r2]
+
+	/* delay for while. */
+	ldr	r1, =40
+delay15:
+	ldr	r2, =0
+cont15:
+	ldr	r0, [r5, r2]
+	add	r2, r2, #4
+	cmp	r2, #16
+	bne	cont15
+	sub	r1, r1, #1
+	cmp	r1, #0
+	bgt	delay15
+
+	/* enable MMDC power down timer. */
+	ldr	r0, [r5, #MMDC0_MDPDC]
+	orr	r0, r0, #(0x55 << 8)
+	str	r0, [r5, #MMDC0_MDPDC]
+
+	b	update_calibration
+
+update_calibration_only:
+	ldr	r1, [r8]
+	sub	r1, r1, #7
+	add	r8, r8, #64
+	b	update_calib
+
+update_calibration:
+	/* write the new calibration values. */
+	mov	r1, r9
+	sub	r1, r1, #7
+
+update_calib:
+	ldr	r0, [r8, #0x0]
+	ldr	r3, [r8, #0x4]
+	str	r3, [r5, r0]
+	add	r8, r8, #8
+	sub	r1, r1, #1
+	cmp	r1, #0
+	bgt	update_calib
+
+	/* perform a force measurement. */
+	ldr	r0, =0x800
+	str	r0, [r5, #MMDC0_MPMUR0]
+	ldr	r2, =MMDC1_MPMUR0
+	str	r0, [r5, r2]
+
+	/* Wait for FRC_MSR to clear. */
+1:
+	ldr	r0, [r5, #MMDC0_MPMUR0]
+	and	r0, r0, #0x800
+	ldr	r1, [r5, r2]
+	and	r1, r1, #0x800
+	orr	r0, r0, r1
+	cmp	r0, #0x0
+	bne	1b
+
+	/* clear SBS - unblock DDR accesses. */
+	ldr	r0, [r5, #MMDC0_MADPCR0]
+	bic	r0, r0, #(1 << 8)
+	str	r0, [r5, #MMDC0_MADPCR0]
+
+	is_mx6qp
+	bne	3f
+	/*
+	 * Switch back to adopt_bp mode, set MMDC0_MAARCR
+	 * bit25~26 to 2b'10.
+	 */
+	ldr	r0, [r5, #MMDC0_MAARCR]
+	bic	r0, r0, #(0x3 << 25)
+	orr	r0, r0, #(0x2 << 25)
+	str	r0, [r5, #MMDC0_MAARCR]
+3:
+	mov	r0, #0x0
+	str	r0, [r5, #MMDC0_MDSCR]
+poll_conreq_clear_2:
+	ldr	r0, [r5, #MMDC0_MDSCR]
+	and	r0, r0, #(0x4 << 12)
+	cmp	r0, #(0x4 << 12)
+	beq	poll_conreq_clear_2
+
+done:
+	/* MMDC0_MAPSR adopt power down enable. */
+	ldr	r0, [r5, #MMDC0_MAPSR]
+	bic	r0, r0, #0x01
+	str	r0, [r5, #MMDC0_MAPSR]
+
+#ifdef CONFIG_CACHE_L2X0
+	ldr	r1, [r12, #PL310_AUX_CTRL]
+	tst	r1, #PL310_AUX_16WAY_BIT
+	mov	r6, #PL310_LOCKDOWN_NBREGS
+	mov	r1, #0x00	/* 8 ways mask */
+	orrne	r1, #0x0000	/* 16 ways mask */
+	add	r5, r12, #PL310_DCACHE_LOCKDOWN_BASE
+1:	/* lock Dcache and Icache */
+	str	r1, [r5], #PL310_LOCKDOWN_SZREG
+	str	r1, [r5], #PL310_LOCKDOWN_SZREG
+	subs	r6, r6, #1
+	bne	1b
+
+	isb
+	dsb
+#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
+
+	isb
+
+	/* Flush the Branch Target Address Cache (BTAC) */
+	ldr	r6, =0x0
+	mcr	p15, 0, r6, c7, c1, 6
+	isb
+	dsb
+
+	/* restore registers */
+	ldmfd	sp!, {r4-r12}
+	mov	pc, lr
+
+	/*
+	 * Add ltorg here to ensure that all
+	 * literals are stored here and are
+	 * within the text space.
+	 */
+	.ltorg
+mx6_ddr3_freq_change_end:
diff --git a/arch/arm/mach-imx/hardware.h b/arch/arm/mach-imx/hardware.h
index ce8e8a39f051..468fe53600f5 100644
--- a/arch/arm/mach-imx/hardware.h
+++ b/arch/arm/mach-imx/hardware.h
@@ -102,6 +102,7 @@
 #include "mx2x.h"
 #include "mx21.h"
 #include "mx27.h"
+#include "mx6.h"
 #include "mx7.h"
 
 #define imx_map_entry(soc, name, _type)	{				\
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..6c9aac07df16
--- /dev/null
+++ b/arch/arm/mach-imx/lpddr2_freq_imx6q.S
@@ -0,0 +1,765 @@
+/*
+ * Copyright (C) 2015-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 <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	wait_for_ccm_handshake
+	/* wait for div update */
+1:
+	ldr	r9, [r2, #CCM_CDHIPR]
+	cmp	r9, #0
+	bne	1b
+
+	.endm
+
+	.macro set_mmdc_misc_ralat_2_cycles
+
+	/* Set MMDCx_MISC[RALAT] = 2 cycles */
+	ldr	r6, [r8, #0x18]
+	bic	r6, r6, #(0x7 << 6)
+	orr	r6, r6, #(0x2 << 6)
+	str	r6, [r8, #0x18]
+
+	/* Check if lpddr2 channel 1 is enabled */
+	ldr 	r6, [r8, #0x18]
+	ands	r6, r6, #(1 << 2)
+	beq	1f
+
+	ldr	r6, [r4, #0x18]
+	bic	r6, r6, #(0x7 << 6)
+	orr	r6, r6, #(0x2 << 6)
+	str	r6, [r4, #0x18]
+1:
+	.endm
+
+	.macro	switch_to_400MHz
+	/* set the MMDC_DIV=1, AXI_DIV=2, AHB_DIV=3 */
+	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_for_ccm_handshake
+
+	/* check periph_clk_sel */
+	ldr	r9, [r2, #CCM_CBCDR]
+	and	r9, r9, #(1 << 25)
+	cmp	r9, #(1 << 25)
+	bne	skip_periph_clk_switch_400m
+
+	/* now switch periph_clk back. */
+	ldr	r9, [r2, #CCM_CBCDR]
+	bic	r9, r9, #(1 << 25)
+	str	r9, [r2, #CCM_CBCDR]
+
+	wait_for_ccm_handshake
+
+skip_periph_clk_switch_400m:
+
+	.endm
+
+	.macro	switch_to_100MHz
+	/* set the MMDC_DIV=4, AXI_DIV=8, AHB_DIV=8 */
+	ldr	r9, [r2, #CCM_CBCDR]
+	ldr	r6, =0x3f1f00
+	bic	r9, r9, r6
+	orr	r9, r9, #(0x1F << 16)
+	orr	r9, r9, #(0x1D << 8)
+	str	r9, [r2, #CCM_CBCDR]
+
+	wait_for_ccm_handshake
+
+	/* check if periph_clk_sel is already set. */
+	ldr	r9, [r2, #CCM_CBCDR]
+	and	r9, r9, #(1 << 25)
+	cmp	r9, #(1 << 25)
+	bne	skip_periph_clk_switch_100m
+
+	/* now switch periph_clk back. */
+	ldr	r9, [r2, #CCM_CBCDR]
+	bic	r9, r9, #(1 << 25)
+	str	r9, [r2, #CCM_CBCDR]
+
+	wait_for_ccm_handshake
+
+skip_periph_clk_switch_100m:
+
+	.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, #(0x7 << 27)
+	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]
+
+	wait_for_ccm_handshake
+
+	/* 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_for_ccm_handshake
+
+	.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]
+
+	wait_for_ccm_handshake
+
+	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_for_ccm_handshake
+
+	.endm
+
+	.macro	set_timings_below_100MHz_operation
+	set_mmdc_misc_ralat_2_cycles
+
+	/* Adjust LPDDR2 timings for 24Mhz operation */
+	ldr	r5, =0x03162073
+	str	r5, [r8, #0xC]  	/* MMDC0_MDCFG0 */
+	ldr	r7, =0x00020482
+	str	r7, [r8, #0x10] 	/* MMDC0_MDCFG1 */
+	ldr	r9, =0x00000049
+	str	r9, [r8, #0x14] 	/* MMDC0_MDCFG2 */
+	ldr	r10, =0x00020333
+	str	r10, [r8, #0x38] 	/* MMDC0_MDCFG3LP */
+
+	/* Check if lpddr2 channel 1 is enabled */
+	ldr	r6, [r8, #0x18]
+	ands	r6, r6, #(1 << 2)
+	beq	skip_below_100Mhz_ch1_timings
+
+	str	r5, [r4, #0xC]  	/* MMDC1_MDCFG0 */
+	str	r7, [r4, #0x10] 	/* MMDC1_MDCFG1 */
+	str	r9, [r4, #0x14] 	/* MMDC1_MDCFG2 */
+	str	r10, [r4, #0x38] 	/* MMDC1_MDCFG3LP */
+
+skip_below_100Mhz_ch1_timings:
+
+	.endm
+
+	.macro restore_mmdc_settings_info
+	/* restore timing from mmdc_settings_info */
+	ldr	r6, [r1, #0x0]
+	ldr	r7, [r1, #0x4]
+1:
+	ldr	r9, [r7], #0x4
+	ldr	r10, [r7], #0x4
+	str	r10, [r8, r9]
+	subs	r6, r6, #0x1
+	bne	1b
+
+	/* Check if lpddr2 channel 1 is enabled */
+	ldr	r6, [r8, #0x18]
+	ands	r6, r6, #(1 << 2)
+	beq	3f
+
+	ldr	r6, [r1, #0x0]
+	ldr	r7, [r1, #0x4]
+2:
+	ldr	r9, [r7], #0x4
+	ldr	r10, [r7], #0x4
+	str	r10, [r4, r9]
+	subs	r6, r6, #0x1
+	bne	2b
+3:
+
+	.endm
+
+	.macro	mmdc_clk_lower_equal_100MHz
+
+	ldr     r10, =100000000
+	cmp     r0, r10
+	beq	set_timmings_100MHz
+	set_timings_below_100MHz_operation
+	b	common_to_lower_equal_100MHz
+
+set_timmings_100MHz:
+	restore_mmdc_settings_info
+	set_mmdc_misc_ralat_2_cycles
+
+common_to_lower_equal_100MHz:
+
+	/* if MMDC is not in 400MHz mode, skip double mu count */
+	ldr	r5, [r1, #0x8]
+	ldr	r6, =400000000
+	cmp	r5, r6
+	bne	skip_lower_force_measure_ch1
+
+	/*
+	 * 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
+
+	/* Check if lpddr2 channel 2 is enabled */
+	ldr	r6, [r8, #0x18]
+	ands	r6, r6, #(1 << 2)
+	beq	skip_lower_force_measure_ch1
+
+	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
+
+skip_lower_force_measure_ch1:
+
+	.endm
+
+	.macro	mmdc_clk_above_100MHz
+
+	restore_mmdc_settings_info
+
+	/* 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
+
+	/* Check if lpddr2 channel 2 is enabled */
+	ldr	r6, [r8, #0x18]
+	ands	r6, r6, #(1 << 2)
+	beq	skip_above_force_measure_ch1
+
+	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
+
+skip_above_force_measure_ch1:
+
+	.endm
+
+	.macro	disable_l1_dcache
+
+	/*
+	 * Flush all data from the L1 data cache before disabling
+	 * SCTLR.C bit.
+	 */
+	push	{r0 - r11, lr}
+
+	ldr	r7, =v7_flush_kern_cache_all
+	mov	lr, pc
+	mov	pc, r7
+	pop	{r0 - r11, lr}
+
+	/* disable d-cache */
+	mrc	p15, 0, r6, c1, c0, 0
+	bic	r6, r6, #0x4
+	mcr	p15, 0, r6, c1, c0, 0
+	dsb
+	isb
+
+	push	{r0 - r11, lr}
+
+	ldr	r7, =v7_flush_kern_cache_all
+	mov	lr, pc
+	mov	pc, r7
+	pop	{r0 - r11, lr}
+
+	.endm
+
+/*
+ *  mx6_lpddr2_freq_change
+ *
+ *  Make sure DDR is in self-refresh.
+ *  IRQs are already disabled.
+ * r0 : DDR freq.
+ * r1 : mmdc_settings_info
+ */
+	.align 3
+ENTRY(mx6q_lpddr2_freq_change)
+mx6q_lpddr2_freq_change_start:
+	push	{r2-r10}
+
+	/*
+	  * Need to flush and disable L1 before
+	  * disabling L2, we need data to
+	  * coherent. Flushing L1 pushes
+	  * everyhting to L2. We sync L2 later, but
+	  * it can still have dirty lines.
+	  * While exiting, we need to enable L2 first
+	  * and then L1.
+	  */
+	disable_l1_dcache
+
+	/*
+	 * 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
+
+#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]
+
+	/* MMDC0_MDPDC disable power down timer */
+	ldr	r6, [r8, #0x4]
+	bic	r6, r6, #0xff00
+	str	r6, [r8, #0x4]
+
+	/* Check if lpddr2 channel 2 is enabled */
+	ldr	r6, [r8, #0x18]
+	ands	r6, r6, #(1 << 2)
+	beq	skip_psd_ch1
+
+	ldr	r6, [r4, #0x404]
+	orr	r6, r6, #0x01
+	str	r6, [r4, #0x404]
+
+	ldr	r6, [r4, #0x4]
+	bic	r6, r6, #0xff00
+	str	r6, [r4, #0x4]
+
+skip_psd_ch1:
+	/* 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]
+
+	/* Check if lpddr2 channel 2 is enabled */
+	ldr	r6, [r8, #0x18]
+	ands	r6, r6, #(1 << 2)
+	beq	skip_sbs_ch1
+
+	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
+
+	ldr	r6, [r4, #0x410]
+	orr	r6, r6, #0x100
+	str	r6, [r4, #0x410]
+
+skip_sbs_ch1:
+	ldr	r10, =100000000
+	cmp	r0, r10
+	bgt	set_ddr_mu_above_100
+	mmdc_clk_lower_equal_100MHz
+
+set_ddr_mu_above_100:
+	ldr	r10, =24000000
+	cmp	r0, r10
+	beq	set_to_24MHz
+
+	ldr	r10, =100000000
+	cmp	r0, r10
+	beq set_to_100MHz
+
+	ldr r10, =400000000
+	cmp r0, r10
+	switch_to_400MHz
+	b	done
+
+set_to_24MHz:
+/*
+	switch_to_24MHZ_from_pll2
+*/
+	switch_to_24MHz
+	b	done
+
+set_to_100MHz:
+	switch_to_100MHz
+
+done:
+
+	ldr	r10,=100000000
+	cmp	r0, r10
+	ble	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
+
+	/* Enable Automatic power savings. */
+	ldr	r6, [r8, #0x404]
+	bic	r6, r6, #0x01
+	str	r6, [r8, #0x404]
+
+	/* Check if lpddr2 channel 2 is enabled */
+	ldr	r6, [r8, #0x18]
+	ands	r6, r6, #(1 << 2)
+	beq	skip_enable_psd_ch1
+
+	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
+
+	ldr	r6, [r4, #0x404]
+	bic	r6, r6, #0x01
+	str	r6, [r4, #0x404]
+
+skip_enable_psd_ch1:
+	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]
+
+	/* Check if lpddr2 channel 2 is enabled */
+	ldr	r6, [r8, #0x18]
+	ands	r6, r6, #(1 << 2)
+	beq	skip_power_down
+
+	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]
+
+	/* Check if lpddr2 channel 2 is enabled */
+	ldr	r6, [r8, #0x18]
+	ands	r6, r6, #(1 << 2)
+	beq	skip_disable_sbs_ch1
+
+	ldr	r6, [r4, #0x410]
+	bic	r6, r6, #0x100
+	str	r6, [r4, #0x410]
+
+skip_disable_sbs_ch1:
+#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:
diff --git a/arch/arm/mach-imx/mach-imx6q.c b/arch/arm/mach-imx/mach-imx6q.c
index edd26e0ffeec..54c89bdad39f 100644
--- a/arch/arm/mach-imx/mach-imx6q.c
+++ b/arch/arm/mach-imx/mach-imx6q.c
@@ -300,6 +300,8 @@ static void __init imx6q_map_io(void)
 {
 	debug_ll_io_init();
 	imx_scu_map_io();
+	imx6_pm_map_io();
+	imx_busfreq_map_io();
 }
 
 static void __init imx6q_init_irq(void)
diff --git a/arch/arm/mach-imx/mx6.h b/arch/arm/mach-imx/mx6.h
new file mode 100644
index 000000000000..06b8135a9954
--- /dev/null
+++ b/arch/arm/mach-imx/mx6.h
@@ -0,0 +1,51 @@
+/*
+ * Copyright 2004-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 version 2 as
+ *    * published by the Free Software Foundation.
+ *     */
+
+#ifndef __ASM_ARCH_MXC_IOMAP_H__
+#define __ASM_ARCH_MXC_IOMAP_H__
+
+#define MX6Q_IO_P2V(x)                  IMX_IO_P2V(x)
+#define MX6Q_IO_ADDRESS(x)              IOMEM(MX6Q_IO_P2V(x))
+
+#define MX6Q_L2_BASE_ADDR       0x00a02000
+#define MX6Q_L2_SIZE            0x1000
+#define MX6Q_IOMUXC_BASE_ADDR       0x020e0000
+#define MX6Q_IOMUXC_SIZE        0x4000
+#define MX6Q_SRC_BASE_ADDR      0x020d8000
+#define MX6Q_SRC_SIZE           0x4000
+#define MX6Q_CCM_BASE_ADDR      0x020c4000
+#define MX6Q_CCM_SIZE           0x4000
+#define MX6Q_ANATOP_BASE_ADDR       0x020c8000
+#define MX6Q_ANATOP_SIZE        0x1000
+#define MX6Q_GPC_BASE_ADDR      0x020dc000
+#define MX6Q_GPC_SIZE           0x4000
+#define MX6Q_SEMA4_BASE_ADDR	0x02290000
+#define MX6Q_SEMA4_SIZE		0x4000
+#define MX6Q_MMDC_P0_BASE_ADDR  0x021b0000
+#define MX6Q_MMDC_P0_SIZE       0x4000
+#define MX6Q_MMDC_P1_BASE_ADDR  0x021b4000
+#define MX6Q_MMDC_P1_SIZE       0x4000
+#define MX6Q_AIPS1_BASE_ADDR        0x02000000
+#define MX6Q_AIPS1_SIZE     0x100000
+#define MX6Q_AIPS2_BASE_ADDR        0x02100000
+#define MX6Q_AIPS2_SIZE     0x100000
+#define MX6Q_AIPS3_BASE_ADDR	0x02200000
+#define MX6Q_AIPS3_SIZE		0x100000
+
+#define MX6SX_IRAM_TLB_BASE_ADDR    0x008f8000
+#define MX6Q_IRAM_TLB_BASE_ADDR 0x00900000
+#define MX6Q_IRAM_TLB_SIZE      0x4000
+#define TT_ATTRIB_NON_CACHEABLE_1M  0x802
+#define MX6_SUSPEND_IRAM_DATA_SIZE  256
+#define MX6SL_WFI_IRAM_DATA_SIZE    100
+
+#define MX6_SUSPEND_IRAM_ADDR_OFFSET        0
+#define MX6_CPUIDLE_IRAM_ADDR_OFFSET        0x1000
+#endif
diff --git a/arch/arm/mach-imx/mxc.h b/arch/arm/mach-imx/mxc.h
index 33d560c6f0b1..848d96b2dd77 100644
--- a/arch/arm/mach-imx/mxc.h
+++ b/arch/arm/mach-imx/mxc.h
@@ -87,6 +87,18 @@ static inline bool cpu_is_imx6q(void)
 	return __mxc_cpu_type == MXC_CPU_IMX6Q;
 }
 
+static inline bool cpu_is_imx6(void)
+{
+	return __mxc_cpu_type == MXC_CPU_IMX6Q ||
+		__mxc_cpu_type == MXC_CPU_IMX6DL ||
+		__mxc_cpu_type == MXC_CPU_IMX6SL ||
+		__mxc_cpu_type == MXC_CPU_IMX6SX ||
+		__mxc_cpu_type == MXC_CPU_IMX6UL ||
+		__mxc_cpu_type == MXC_CPU_IMX6ULL ||
+		__mxc_cpu_type == MXC_CPU_IMX6SLL ||
+		__mxc_cpu_type == MXC_CPU_IMX6ULZ;
+}
+
 static inline bool cpu_is_imx7d(void)
 {
 	return __mxc_cpu_type == MXC_CPU_IMX7D;
diff --git a/arch/arm/mach-imx/platsmp.c b/arch/arm/mach-imx/platsmp.c
index 2aa26928221d..7be31e7e91b3 100644
--- a/arch/arm/mach-imx/platsmp.c
+++ b/arch/arm/mach-imx/platsmp.c
@@ -18,7 +18,7 @@
 #include "hardware.h"
 
 u32 g_diag_reg;
-static void __iomem *scu_base;
+void __iomem *scu_base;
 
 static struct map_desc scu_io_desc __initdata = {
 	/* .virtual and .pfn are run-time assigned */
diff --git a/arch/arm/mach-imx/pm-imx6.c b/arch/arm/mach-imx/pm-imx6.c
index 1c0ecad3620e..e36d0acfda8a 100644
--- a/arch/arm/mach-imx/pm-imx6.c
+++ b/arch/arm/mach-imx/pm-imx6.c
@@ -13,11 +13,13 @@
 #include <linux/mfd/syscon/imx6q-iomuxc-gpr.h>
 #include <linux/of.h>
 #include <linux/of_address.h>
+#include <linux/of_fdt.h>
 #include <linux/of_platform.h>
 #include <linux/regmap.h>
 #include <linux/suspend.h>
 #include <asm/cacheflush.h>
 #include <asm/fncpy.h>
+#include <asm/mach/map.h>
 #include <asm/proc-fns.h>
 #include <asm/suspend.h>
 #include <asm/tlb.h>
@@ -58,6 +60,9 @@
 #define MX6Q_SUSPEND_OCRAM_SIZE		0x1000
 #define MX6_MAX_MMDC_IO_NUM		33
 
+extern unsigned long iram_tlb_base_addr;
+extern unsigned long iram_tlb_phys_addr;
+
 static void __iomem *ccm_base;
 static void __iomem *suspend_ocram_base;
 static void (*imx6_suspend_in_ocram_fn)(void __iomem *ocram_vbase);
@@ -206,6 +211,37 @@ static const struct imx6_pm_socdata imx6ul_pm_data __initconst = {
 	.mmdc_io_offset = imx6ul_mmdc_io_offset,
 };
 
+static struct map_desc iram_tlb_io_desc __initdata = {
+	/* .virtual and .pfn are run-time assigned */
+	.length     = SZ_1M,
+	.type       = MT_MEMORY_RWX_NONCACHED,
+};
+
+/*
+ * AIPS1 and AIPS2 is not used, because it will trigger a BUG_ON if
+ * lowlevel debug and earlyprintk are configured.
+ *
+ * it is because there is a vm conflict because UART1 is mapped early if
+ * AIPS1 is mapped using 1M size.
+ *
+ * Thus no use AIPS1 and AIPS2 to avoid kernel BUG_ON.
+ */
+static struct map_desc imx6_pm_io_desc[] __initdata = {
+	imx_map_entry(MX6Q, MMDC_P0, MT_DEVICE),
+	imx_map_entry(MX6Q, MMDC_P1, MT_DEVICE),
+	imx_map_entry(MX6Q, SRC, MT_DEVICE),
+	imx_map_entry(MX6Q, IOMUXC, MT_DEVICE),
+	imx_map_entry(MX6Q, CCM, MT_DEVICE),
+	imx_map_entry(MX6Q, ANATOP, MT_DEVICE),
+	imx_map_entry(MX6Q, GPC, MT_DEVICE),
+	imx_map_entry(MX6Q, L2,	MT_DEVICE),
+};
+
+static const char * const low_power_ocram_match[] __initconst = {
+	"fsl,lpm-sram",
+	NULL
+};
+
 /*
  * This structure is for passing necessary data for low level ocram
  * suspend code(arch/arm/mach-imx/suspend-imx6.S), if this struct
@@ -433,6 +469,106 @@ static const struct platform_suspend_ops imx6q_pm_ops = {
 	.valid = imx6q_pm_valid,
 };
 
+static int __init imx6_dt_find_lpsram(unsigned long node, const char *uname,
+				      int depth, void *data)
+{
+	unsigned long lpram_addr;
+	const __be32 *prop = of_get_flat_dt_prop(node, "reg", NULL);
+
+	if (of_flat_dt_match(node, low_power_ocram_match)) {
+		if (!prop)
+			return -EINVAL;
+
+		lpram_addr = be32_to_cpup(prop);
+
+		/* We need to create a 1M page table entry. */
+		iram_tlb_io_desc.virtual = IMX_IO_P2V(lpram_addr & 0xFFF00000);
+		iram_tlb_io_desc.pfn = __phys_to_pfn(lpram_addr & 0xFFF00000);
+		iram_tlb_phys_addr = lpram_addr;
+		iram_tlb_base_addr = IMX_IO_P2V(lpram_addr);
+
+		iotable_init(&iram_tlb_io_desc, 1);
+	}
+
+	return 0;
+}
+
+void __init imx6_pm_map_io(void)
+{
+	unsigned long i;
+
+	iotable_init(imx6_pm_io_desc, ARRAY_SIZE(imx6_pm_io_desc));
+
+	/*
+	 * Get the address of IRAM or OCRAM to be used by the low
+	 * power code from the device tree.
+	 */
+	WARN_ON(of_scan_flat_dt(imx6_dt_find_lpsram, NULL));
+
+	/*
+	 * We moved suspend/resume and lowpower idle to TEE,
+	 * But busfreq now still in Linux, this table is still needed
+	 * If we later decide to move busfreq to TEE, we could drop this.
+	 */
+	/* Return if no IRAM space is allocated for suspend/resume code. */
+	if (!iram_tlb_base_addr) {
+		pr_warn("No IRAM/OCRAM memory allocated for suspend/resume \
+			 code. Please ensure device tree has an entry for \
+			 fsl,lpm-sram.\n");
+		return;
+	}
+
+	/* Set all entries to 0. */
+	memset((void *)iram_tlb_base_addr, 0, MX6Q_IRAM_TLB_SIZE);
+
+	/*
+	 * Make sure the IRAM virtual address has a mapping in the IRAM
+	 * page table.
+	 *
+	 * Only use the top 11 bits [31-20] when storing the physical
+	 * address in the page table as only these bits are required
+	 * for 1M mapping.
+	 */
+	i = ((iram_tlb_base_addr >> 20) << 2) / 4;
+	*((unsigned long *)iram_tlb_base_addr + i) =
+		(iram_tlb_phys_addr & 0xFFF00000) | TT_ATTRIB_NON_CACHEABLE_1M;
+
+	/*
+	 * Make sure the AIPS1 virtual address has a mapping in the
+	 * IRAM page table.
+	 */
+	i = ((IMX_IO_P2V(MX6Q_AIPS1_BASE_ADDR) >> 20) << 2) / 4;
+	*((unsigned long *)iram_tlb_base_addr + i) =
+		(MX6Q_AIPS1_BASE_ADDR & 0xFFF00000) |
+		TT_ATTRIB_NON_CACHEABLE_1M;
+
+	/*
+	 * Make sure the AIPS2 virtual address has a mapping in the
+	 * IRAM page table.
+	 */
+	i = ((IMX_IO_P2V(MX6Q_AIPS2_BASE_ADDR) >> 20) << 2) / 4;
+	*((unsigned long *)iram_tlb_base_addr + i) =
+		(MX6Q_AIPS2_BASE_ADDR & 0xFFF00000) |
+		TT_ATTRIB_NON_CACHEABLE_1M;
+
+	/*
+	 * Make sure the AIPS3 virtual address has a mapping
+	 * in the IRAM page table.
+	 */
+	i = ((IMX_IO_P2V(MX6Q_AIPS3_BASE_ADDR) >> 20) << 2) / 4;
+		*((unsigned long *)iram_tlb_base_addr + i) =
+		(MX6Q_AIPS3_BASE_ADDR & 0xFFF00000) |
+		TT_ATTRIB_NON_CACHEABLE_1M;
+
+	/*
+	 * Make sure the L2 controller virtual address has a mapping
+	 * in the IRAM page table.
+	 */
+	i = ((IMX_IO_P2V(MX6Q_L2_BASE_ADDR) >> 20) << 2) / 4;
+	*((unsigned long *)iram_tlb_base_addr + i) =
+		(MX6Q_L2_BASE_ADDR & 0xFFF00000) | TT_ATTRIB_NON_CACHEABLE_1M;
+}
+
 static int __init imx6_pm_get_base(struct imx6_pm_base *base,
 				const char *compat)
 {
diff --git a/arch/arm/mach-imx/smp_wfe_imx6.S b/arch/arm/mach-imx/smp_wfe_imx6.S
new file mode 100644
index 000000000000..791e93ce98da
--- /dev/null
+++ b/arch/arm/mach-imx/smp_wfe_imx6.S
@@ -0,0 +1,186 @@
+/*
+ * 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.
+
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+
+#include <linux/linkage.h>
+#include <asm/smp_scu.h>
+#include "hardware.h"
+
+#ifdef CONFIG_SMP
+.extern scu_base
+#endif
+
+.globl wfe_smp_freq_change_start
+.globl wfe_smp_freq_change_end
+
+#ifdef CONFIG_SMP
+
+	.align 3
+
+	.macro	disable_l1_dcache
+
+	/*
+	 * Flush all data from the L1 data cache before disabling
+	 * SCTLR.C bit.
+	 */
+	push	{r0 - r11, lr}
+
+	ldr	r7, =v7_flush_kern_cache_all
+	mov	lr, pc
+	mov	pc, r7
+	pop	{r0 - r11, lr}
+
+	/* disable d-cache */
+	mrc	p15, 0, r6, c1, c0, 0
+	bic	r6, r6, #0x4
+	mcr	p15, 0, r6, c1, c0, 0
+	dsb
+	isb
+
+	push	{r0 - r11, lr}
+
+	ldr	r7, =v7_flush_kern_cache_all
+	mov	lr, pc
+	mov	pc, r7
+	pop	{r0 - r11, lr}
+
+	.endm
+
+ENTRY(wfe_smp_freq_change)
+wfe_smp_freq_change_start:
+	push	{r4 - r11, lr}
+
+	mov	r6, r0
+	mov	r7, r1
+
+	dsb
+	isb
+
+	disable_l1_dcache
+
+	isb
+
+	/* Turn off SMP bit. */
+	mrc	p15, 0, r8, c1, c0, 1
+	bic	r8, r8, #0x40
+	mcr	p15, 0, r8, c1, c0, 1
+
+	isb
+
+	/* Inform the SCU we are going to enter WFE. */
+	push	{r0 - r11, lr}
+
+	ldr	r0,=scu_base
+	ldr	r0, [r0]
+	mov	r1, #SCU_PM_DORMANT
+	ldr	r3, =scu_power_mode
+	mov	lr, pc
+	mov	pc, r3
+
+	pop	{r0 - r11, lr}
+
+go_back_wfe:
+	wfe
+
+	ldr	r3, [r7]
+	cmp	r3, #1
+	beq	go_back_wfe
+
+	/* Turn ON SMP bit. */
+	mrc	p15, 0, r8, c1, c0, 1
+	orr	r8, r8, #0x40
+	mcr	p15, 0, r8, c1, c0, 1
+
+	isb
+	/* Enable L1 data cache. */
+	mrc	p15, 0, r8, c1, c0, 0
+	orr	r8, r8, #0x4
+	mcr	p15, 0, r8, c1, c0, 0
+	isb
+
+	/* Inform the SCU we have exited WFE. */
+	push	{r0 - r11, lr}
+
+	ldr	r0,=scu_base
+	ldr	r0, [r0]
+	mov	r1, #SCU_PM_NORMAL
+	ldr	r3, =scu_power_mode
+	mov	lr, pc
+	mov	pc, r3
+
+	pop	{r0 - r11, lr}
+
+	/* Pop all saved registers. */
+	pop	{r4 - r11, lr}
+	mov	pc, lr
+	.ltorg
+wfe_smp_freq_change_end:
+ENDPROC(wfe_smp_freq_change)
+
+#ifdef CONFIG_OPTEE
+/**
+ * @brief  Switch CPU in WFE mode while bus frequency change
+ *         on-going
+ *
+ * @param[in] r0  CPU in WFE Status
+ * @param[in] r1  Bus frequency change status
+ */
+
+.globl imx_smp_wfe_optee_end
+
+ENTRY(imx_smp_wfe_optee)
+	push  {r4-r11, lr}
+
+	dsb
+	isb
+
+	disable_l1_dcache
+	isb
+
+	/* Set flag CPU entering WFE. */
+	mov   r4, #1
+	str   r4, [r0]
+
+	dsb
+	isb
+
+1:
+	wfe
+
+	/* Check if busfreq is done, else loop */
+	ldr   r4, [r1]
+	cmp   r4, #1
+	beq   1b
+
+	/* Enable L1 data cache. */
+	mrc   p15, 0, r4, c1, c0, 0
+	orr   r4, r4, #0x4
+	mcr   p15, 0, r4, c1, c0, 0
+	isb
+
+	/* Set flag CPU exiting WFE. */
+	mov   r4, #0
+	str   r4, [r0]
+
+	/* Pop all saved registers. */
+	pop   {r4-r11, lr}
+	mov   pc, lr
+	.ltorg
+imx_smp_wfe_optee_end:
+ENDPROC(imx_smp_wfe_optee)
+#endif
+#endif