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

Add i.MX7D bus-freq support, it supports High/Low/Audio bus
mode.

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

16 files changed, 3084 insertions, 6 deletions

diff --git a/arch/arm/mach-imx/Kconfig b/arch/arm/mach-imx/Kconfig
index 593bf1519608..29b15098f26b 100644
--- a/arch/arm/mach-imx/Kconfig
+++ b/arch/arm/mach-imx/Kconfig
@@ -43,6 +43,13 @@ config HAVE_IMX_GPC
 config HAVE_IMX_MMDC
 	bool
 
+config HAVE_IMX_DDRC
+       bool
+       select HAVE_IMX_BUSFREQ
+
+config HAVE_IMX_BUSFREQ
+       bool
+
 config HAVE_IMX_SRC
 	def_bool y if SMP
 	select ARCH_HAS_RESET_CONTROLLER
@@ -546,6 +553,7 @@ config SOC_IMX7D_CA7
 	select HAVE_IMX_MMDC
 	select HAVE_IMX_SRC
 	select IMX_GPCV2
+	select HAVE_IMX_DDRC
 
 config SOC_IMX7D_CM4
 	bool
diff --git a/arch/arm/mach-imx/Makefile b/arch/arm/mach-imx/Makefile
index 35ff620537e6..8703165f4d64 100644
--- a/arch/arm/mach-imx/Makefile
+++ b/arch/arm/mach-imx/Makefile
@@ -1,5 +1,5 @@
 # SPDX-License-Identifier: GPL-2.0
-obj-y := cpu.o system.o irq-common.o
+obj-y := cpu.o system.o irq-common.o common.o
 
 obj-$(CONFIG_SOC_IMX21) += mm-imx21.o
 
@@ -72,6 +72,7 @@ obj-$(CONFIG_HAVE_IMX_ANATOP) += anatop.o
 obj-$(CONFIG_HAVE_IMX_GPC) += gpc.o
 obj-$(CONFIG_HAVE_IMX_MMDC) += mmdc.o
 obj-$(CONFIG_HAVE_IMX_SRC) += src.o
+obj-$(CONFIG_HAVE_IMX_DDRC) += ddrc.o
 ifneq ($(CONFIG_SOC_IMX6)$(CONFIG_SOC_LS1021A),)
 AFLAGS_headsmp.o :=-Wa,-march=armv7-a
 obj-$(CONFIG_SMP) += headsmp.o platsmp.o
@@ -82,10 +83,16 @@ obj-$(CONFIG_SOC_IMX6SL) += mach-imx6sl.o
 obj-$(CONFIG_SOC_IMX6SLL) += mach-imx6sl.o
 obj-$(CONFIG_SOC_IMX6SX) += mach-imx6sx.o
 obj-$(CONFIG_SOC_IMX6UL) += mach-imx6ul.o
-obj-$(CONFIG_SOC_IMX7D_CA7) += mach-imx7d.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
+AFLAGS_smp_wfe.o :=-Wa,-march=armv7-a
+AFLAGS_ddr3_freq_imx7d.o :=-Wa,-march=armv7-a
+AFLAGS_lpddr3_freq_imx.o :=-Wa,-march=armv7-a
+
 ifeq ($(CONFIG_SUSPEND),y)
 AFLAGS_suspend-imx6.o :=-Wa,-march=armv7-a
 obj-$(CONFIG_SOC_IMX6) += suspend-imx6.o
diff --git a/arch/arm/mach-imx/busfreq-imx.c b/arch/arm/mach-imx/busfreq-imx.c
new file mode 100644
index 000000000000..976ba3ca7f29
--- /dev/null
+++ b/arch/arm/mach-imx/busfreq-imx.c
@@ -0,0 +1,764 @@
+/*
+ * Copyright (C) 2011-2016 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2017 NXP.
+ * Copyright 2018 NXP.
+ *
+ * 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 <asm/cacheflush.h>
+#include <asm/fncpy.h>
+#include <asm/io.h>
+#include <asm/mach/map.h>
+#include <asm/mach-types.h>
+#include <asm/tlb.h>
+#include <linux/busfreq-imx.h>
+#include <linux/clk.h>
+#include <linux/clk-provider.h>
+#include <linux/delay.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/of.h>
+#include <linux/of_fdt.h>
+#include <linux/platform_device.h>
+#include <linux/proc_fs.h>
+#include <linux/reboot.h>
+#include <linux/regulator/consumer.h>
+#include <linux/sched.h>
+#include <linux/suspend.h>
+#include "hardware.h"
+#include "common.h"
+
+#define LPAPM_CLK		24000000
+#define LOW_AUDIO_CLK		50000000
+#define HIGH_AUDIO_CLK		100000000
+
+#define LOW_POWER_RUN_VOLTAGE	950000
+
+#define MMDC_MDMISC_DDR_TYPE_DDR3	0
+#define MMDC_MDMISC_DDR_TYPE_LPDDR2	1
+
+unsigned int ddr_med_rate;
+unsigned int ddr_normal_rate;
+unsigned long ddr_freq_change_total_size;
+unsigned long ddr_freq_change_iram_base;
+unsigned long ddr_freq_change_iram_phys;
+
+static int ddr_type;
+static int low_bus_freq_mode;
+static int audio_bus_freq_mode;
+static int ultra_low_bus_freq_mode;
+static int high_bus_freq_mode;
+static int med_bus_freq_mode;
+static int bus_freq_scaling_initialized;
+static bool cancel_reduce_bus_freq;
+static struct device *busfreq_dev;
+static int busfreq_suspended;
+static int bus_freq_scaling_is_active;
+static int high_bus_count, med_bus_count, audio_bus_count, low_bus_count;
+static unsigned int ddr_low_rate;
+static int cur_bus_freq_mode;
+
+extern unsigned long iram_tlb_phys_addr;
+extern int unsigned long iram_tlb_base_addr;
+
+/*
+ * Bus frequency management by Linux
+ */
+extern int init_mmdc_lpddr2_settings(struct platform_device *dev);
+extern int init_mmdc_lpddr2_settings_mx6q(struct platform_device *dev);
+extern int init_mmdc_ddr3_settings_imx6_up(struct platform_device *dev);
+extern int init_mmdc_ddr3_settings_imx6_smp(struct platform_device *dev);
+extern int init_ddrc_ddr_settings(struct platform_device *dev);
+extern int update_ddr_freq_imx_smp(int ddr_rate);
+extern int update_ddr_freq_imx6_up(int ddr_rate);
+extern int update_lpddr2_freq(int ddr_rate);
+extern int update_lpddr2_freq_smp(int ddr_rate);
+
+
+/**
+ * @brief  Functions to init and update the busfreq function of
+ *         device and memory type
+ */
+static struct busfreq_func {
+	int (*init)(struct platform_device *dev);
+	int (*update)(int ddr_rate);
+} busfreq_func = {NULL, NULL};
+
+DEFINE_MUTEX(bus_freq_mutex);
+
+static struct clk *osc_clk;
+static struct clk *ahb_clk;
+static struct clk *axi_sel_clk;
+static struct clk *dram_root;
+static struct clk *dram_alt_sel;
+static struct clk *dram_alt_root;
+static struct clk *pfd0_392m;
+static struct clk *pfd2_270m;
+static struct clk *pfd1_332m;
+static struct clk *pll_dram;
+static struct clk *ahb_sel_clk;
+static struct clk *axi_clk;
+
+static struct delayed_work low_bus_freq_handler;
+static struct delayed_work bus_freq_daemon;
+
+static RAW_NOTIFIER_HEAD(busfreq_notifier_chain);
+
+static bool busfreq_notified_low = false;
+
+static int busfreq_notify(enum busfreq_event event)
+{
+	int ret;
+
+	if (event == LOW_BUSFREQ_ENTER) {
+		WARN_ON(busfreq_notified_low);
+		busfreq_notified_low = true;
+	} else if (event == LOW_BUSFREQ_EXIT) {
+		WARN_ON(!busfreq_notified_low);
+		busfreq_notified_low = false;
+	}
+	ret = raw_notifier_call_chain(&busfreq_notifier_chain, event, NULL);
+
+	return notifier_to_errno(ret);
+}
+
+int register_busfreq_notifier(struct notifier_block *nb)
+{
+	return raw_notifier_chain_register(&busfreq_notifier_chain, nb);
+}
+EXPORT_SYMBOL(register_busfreq_notifier);
+
+int unregister_busfreq_notifier(struct notifier_block *nb)
+{
+	return raw_notifier_chain_unregister(&busfreq_notifier_chain, nb);
+}
+EXPORT_SYMBOL(unregister_busfreq_notifier);
+
+static void enter_lpm_imx7d(void)
+{
+	/*
+	 * The AHB clock parent switch and divider change
+	 * needs to keep previous/current parent enabled
+	 * per design requirement, but when we switch the
+	 * clock parent, previous AHB clock parent may be
+	 * disabled by common clock framework, so here we
+	 * have to make sure AHB's previous parent pfd2_270m
+	 * is enabled during AHB set rate.
+	 */
+	clk_prepare_enable(pfd2_270m);
+	if (audio_bus_count) {
+		clk_prepare_enable(pfd0_392m);
+		busfreq_func.update(HIGH_AUDIO_CLK);
+
+		clk_set_parent(dram_alt_sel, pfd0_392m);
+		clk_set_parent(dram_root, dram_alt_root);
+		if (high_bus_freq_mode) {
+			clk_set_parent(axi_sel_clk, osc_clk);
+			clk_set_parent(ahb_sel_clk, osc_clk);
+			clk_set_rate(ahb_clk, LPAPM_CLK);
+		}
+		clk_disable_unprepare(pfd0_392m);
+		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(dram_alt_sel, osc_clk);
+		clk_set_parent(dram_root, dram_alt_root);
+		if (high_bus_freq_mode) {
+			clk_set_parent(axi_sel_clk, osc_clk);
+			clk_set_parent(ahb_sel_clk, osc_clk);
+			clk_set_rate(ahb_clk, LPAPM_CLK);
+		}
+		low_bus_freq_mode = 1;
+		audio_bus_freq_mode = 0;
+		cur_bus_freq_mode = BUS_FREQ_LOW;
+	}
+	clk_disable_unprepare(pfd2_270m);
+}
+
+static void exit_lpm_imx7d(void)
+{
+	clk_set_parent(axi_sel_clk, pfd1_332m);
+	clk_set_rate(ahb_clk, LPAPM_CLK / 2);
+	clk_set_parent(ahb_sel_clk, pfd2_270m);
+
+	busfreq_func.update(ddr_normal_rate);
+
+	clk_set_parent(dram_root, pll_dram);
+}
+
+static void reduce_bus_freq(void)
+{
+	if (audio_bus_count && (low_bus_freq_mode || ultra_low_bus_freq_mode))
+		busfreq_notify(LOW_BUSFREQ_EXIT);
+	else if (!audio_bus_count)
+		busfreq_notify(LOW_BUSFREQ_ENTER);
+
+	if (cpu_is_imx7d())
+		enter_lpm_imx7d();
+
+	med_bus_freq_mode = 0;
+	high_bus_freq_mode = 0;
+
+	if (audio_bus_freq_mode)
+		dev_dbg(busfreq_dev,
+			"Bus freq set to audio mode. Count: high %d, med %d, audio %d\n",
+			high_bus_count, med_bus_count, audio_bus_count);
+	if (low_bus_freq_mode)
+		dev_dbg(busfreq_dev,
+			"Bus freq set to low mode. Count: high %d, med %d, audio %d\n",
+			high_bus_count, med_bus_count, audio_bus_count);
+}
+
+static inline void cancel_low_bus_freq_handler(void)
+{
+	cancel_delayed_work(&low_bus_freq_handler);
+	cancel_reduce_bus_freq = true;
+}
+
+static void reduce_bus_freq_handler(struct work_struct *work)
+{
+	mutex_lock(&bus_freq_mutex);
+
+	if (!cancel_reduce_bus_freq) {
+		reduce_bus_freq();
+		cancel_low_bus_freq_handler();
+	}
+
+	mutex_unlock(&bus_freq_mutex);
+}
+
+/*
+ * Set the DDR, AHB to 24MHz.
+ * This mode will be activated only when none of the modules that
+ * need a higher DDR or AHB frequency are active.
+ */
+static int set_low_bus_freq(void)
+{
+	if (busfreq_suspended)
+		return 0;
+
+	if (!bus_freq_scaling_initialized || !bus_freq_scaling_is_active)
+		return 0;
+
+	cancel_reduce_bus_freq = false;
+
+	/*
+	 * Check to see if we need to got from
+	 * low bus freq mode to audio bus freq mode.
+	 * If so, the change needs to be done immediately.
+	 */
+	if (audio_bus_count && (low_bus_freq_mode || ultra_low_bus_freq_mode))
+		reduce_bus_freq();
+	else
+		/*
+		 * Don't lower the frequency immediately. Instead
+		 * scheduled a delayed work and drop the freq if
+		 * the conditions still remain the same.
+		 */
+		schedule_delayed_work(&low_bus_freq_handler,
+					usecs_to_jiffies(3000000));
+	return 0;
+}
+
+/*
+ * Set the DDR to either 528MHz or 400MHz for iMX6qd
+ * or 400MHz for iMX6dl.
+ */
+static int set_high_bus_freq(int high_bus_freq)
+{
+	if (bus_freq_scaling_initialized && bus_freq_scaling_is_active)
+		cancel_low_bus_freq_handler();
+
+	if (busfreq_suspended)
+		return 0;
+
+	if (!bus_freq_scaling_initialized || !bus_freq_scaling_is_active)
+		return 0;
+
+	if (high_bus_freq_mode)
+		return 0;
+
+	/* medium bus freq is only supported for MX6DQ */
+	if (med_bus_freq_mode && !high_bus_freq)
+		return 0;
+
+	if (low_bus_freq_mode || ultra_low_bus_freq_mode)
+		busfreq_notify(LOW_BUSFREQ_EXIT);
+
+	if (cpu_is_imx7d())
+		exit_lpm_imx7d();
+
+	high_bus_freq_mode = 1;
+	med_bus_freq_mode = 0;
+	low_bus_freq_mode = 0;
+	audio_bus_freq_mode = 0;
+	cur_bus_freq_mode = BUS_FREQ_HIGH;
+
+	if (high_bus_freq_mode)
+		dev_dbg(busfreq_dev,
+			"Bus freq set to high mode. Count: high %d, med %d, audio %d\n",
+			high_bus_count, med_bus_count, audio_bus_count);
+	if (med_bus_freq_mode)
+		dev_dbg(busfreq_dev,
+			"Bus freq set to med mode. Count: high %d, med %d, audio %d\n",
+			high_bus_count, med_bus_count, audio_bus_count);
+
+	return 0;
+}
+
+void request_bus_freq(enum bus_freq_mode mode)
+{
+	mutex_lock(&bus_freq_mutex);
+
+	if (mode == BUS_FREQ_ULTRA_LOW) {
+		dev_dbg(busfreq_dev, "This mode cannot be requested!\n");
+		mutex_unlock(&bus_freq_mutex);
+		return;
+	}
+
+	if (mode == BUS_FREQ_HIGH)
+		high_bus_count++;
+	else if (mode == BUS_FREQ_MED)
+		med_bus_count++;
+	else if (mode == BUS_FREQ_AUDIO)
+		audio_bus_count++;
+	else if (mode == BUS_FREQ_LOW)
+		low_bus_count++;
+
+	if (busfreq_suspended || !bus_freq_scaling_initialized ||
+		!bus_freq_scaling_is_active) {
+		mutex_unlock(&bus_freq_mutex);
+		return;
+	}
+
+	cancel_low_bus_freq_handler();
+
+	if ((mode == BUS_FREQ_HIGH) && (!high_bus_freq_mode)) {
+		set_high_bus_freq(1);
+		mutex_unlock(&bus_freq_mutex);
+		return;
+	}
+
+	if ((mode == BUS_FREQ_MED) && (!high_bus_freq_mode) &&
+		(!med_bus_freq_mode)) {
+		set_high_bus_freq(0);
+		mutex_unlock(&bus_freq_mutex);
+		return;
+	}
+	if ((mode == BUS_FREQ_AUDIO) && (!high_bus_freq_mode) &&
+		(!med_bus_freq_mode) && (!audio_bus_freq_mode)) {
+		set_low_bus_freq();
+		mutex_unlock(&bus_freq_mutex);
+		return;
+	}
+	mutex_unlock(&bus_freq_mutex);
+}
+EXPORT_SYMBOL(request_bus_freq);
+
+void release_bus_freq(enum bus_freq_mode mode)
+{
+	mutex_lock(&bus_freq_mutex);
+
+	if (mode == BUS_FREQ_ULTRA_LOW) {
+		dev_dbg(busfreq_dev,
+			"This mode cannot be released!\n");
+		mutex_unlock(&bus_freq_mutex);
+		return;
+	}
+
+	if (mode == BUS_FREQ_HIGH) {
+		if (high_bus_count == 0) {
+			dev_err(busfreq_dev, "high bus count mismatch!\n");
+			dump_stack();
+			mutex_unlock(&bus_freq_mutex);
+			return;
+		}
+		high_bus_count--;
+	} else if (mode == BUS_FREQ_MED) {
+		if (med_bus_count == 0) {
+			dev_err(busfreq_dev, "med bus count mismatch!\n");
+			dump_stack();
+			mutex_unlock(&bus_freq_mutex);
+			return;
+		}
+		med_bus_count--;
+	} else if (mode == BUS_FREQ_AUDIO) {
+		if (audio_bus_count == 0) {
+			dev_err(busfreq_dev, "audio bus count mismatch!\n");
+			dump_stack();
+			mutex_unlock(&bus_freq_mutex);
+			return;
+		}
+		audio_bus_count--;
+	} else if (mode == BUS_FREQ_LOW) {
+		if (low_bus_count == 0) {
+			dev_err(busfreq_dev, "low bus count mismatch!\n");
+			dump_stack();
+			mutex_unlock(&bus_freq_mutex);
+			return;
+		}
+		low_bus_count--;
+	}
+
+	if (busfreq_suspended || !bus_freq_scaling_initialized ||
+		!bus_freq_scaling_is_active) {
+		mutex_unlock(&bus_freq_mutex);
+		return;
+	}
+
+	if ((!audio_bus_freq_mode) && (high_bus_count == 0) &&
+		(med_bus_count == 0) && (audio_bus_count != 0)) {
+		set_low_bus_freq();
+		mutex_unlock(&bus_freq_mutex);
+		return;
+	}
+	if ((!low_bus_freq_mode) && (high_bus_count == 0) &&
+		(med_bus_count == 0) && (audio_bus_count == 0) &&
+		(low_bus_count != 0)) {
+		set_low_bus_freq();
+		mutex_unlock(&bus_freq_mutex);
+		return;
+	}
+	if ((!ultra_low_bus_freq_mode) && (high_bus_count == 0) &&
+		(med_bus_count == 0) && (audio_bus_count == 0) &&
+		(low_bus_count == 0)) {
+		set_low_bus_freq();
+		mutex_unlock(&bus_freq_mutex);
+		return;
+	}
+
+	mutex_unlock(&bus_freq_mutex);
+}
+EXPORT_SYMBOL(release_bus_freq);
+
+int get_bus_freq_mode(void)
+{
+	return cur_bus_freq_mode;
+}
+EXPORT_SYMBOL(get_bus_freq_mode);
+
+static struct map_desc ddr_iram_io_desc __initdata = {
+	/* .virtual and .pfn are run-time assigned */
+	.length		= SZ_1M,
+	.type		= MT_MEMORY_RWX_NONCACHED,
+};
+
+const static char *ddr_freq_iram_match[] __initconst = {
+	"fsl,ddr-lpm-sram",
+	NULL
+};
+
+static int __init imx_dt_find_ddr_sram(unsigned long node,
+		const char *uname, int depth, void *data)
+{
+	unsigned long ddr_iram_addr;
+	const __be32 *prop;
+
+	if (of_flat_dt_match(node, ddr_freq_iram_match)) {
+		unsigned int len;
+
+		prop = of_get_flat_dt_prop(node, "reg", &len);
+		if (prop == NULL || len != (sizeof(unsigned long) * 2))
+			return -EINVAL;
+		ddr_iram_addr = be32_to_cpu(prop[0]);
+		ddr_freq_change_total_size = be32_to_cpu(prop[1]);
+		ddr_freq_change_iram_phys = ddr_iram_addr;
+
+		/* Make sure ddr_freq_change_iram_phys is 8 byte aligned. */
+		if ((uintptr_t)(ddr_freq_change_iram_phys) & (FNCPY_ALIGN - 1))
+			ddr_freq_change_iram_phys += FNCPY_ALIGN -
+				((uintptr_t)ddr_freq_change_iram_phys %
+				(FNCPY_ALIGN));
+	}
+	return 0;
+}
+
+void __init imx_busfreq_map_io(void)
+{
+	/*
+	 * Get the address of IRAM to be used by the ddr frequency
+	 * change code from the device tree.
+	 */
+	WARN_ON(of_scan_flat_dt(imx_dt_find_ddr_sram, NULL));
+	if (ddr_freq_change_iram_phys) {
+		ddr_freq_change_iram_base = IMX_IO_P2V(
+			ddr_freq_change_iram_phys);
+		if ((iram_tlb_phys_addr & 0xFFF00000) !=
+			(ddr_freq_change_iram_phys & 0xFFF00000)) {
+			/* We need to create a 1M page table entry. */
+			ddr_iram_io_desc.virtual = IMX_IO_P2V(
+				ddr_freq_change_iram_phys & 0xFFF00000);
+			ddr_iram_io_desc.pfn = __phys_to_pfn(
+				ddr_freq_change_iram_phys & 0xFFF00000);
+			iotable_init(&ddr_iram_io_desc, 1);
+		}
+		memset((void *)ddr_freq_change_iram_base, 0,
+			ddr_freq_change_total_size);
+	}
+}
+
+static void bus_freq_daemon_handler(struct work_struct *work)
+{
+	mutex_lock(&bus_freq_mutex);
+	if ((!low_bus_freq_mode) && (!ultra_low_bus_freq_mode)
+		&& (high_bus_count == 0) &&
+		(med_bus_count == 0) && (audio_bus_count == 0))
+		set_low_bus_freq();
+	mutex_unlock(&bus_freq_mutex);
+}
+
+static ssize_t bus_freq_scaling_enable_show(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	if (bus_freq_scaling_is_active)
+		return sprintf(buf, "Bus frequency scaling is enabled\n");
+	else
+		return sprintf(buf, "Bus frequency scaling is disabled\n");
+}
+
+static ssize_t bus_freq_scaling_enable_store(struct device *dev,
+				 struct device_attribute *attr,
+				 const char *buf, size_t size)
+{
+	if (strncmp(buf, "1", 1) == 0) {
+		bus_freq_scaling_is_active = 1;
+		set_high_bus_freq(1);
+		/*
+		 * We set bus freq to highest at the beginning,
+		 * so we use this daemon thread to make sure system
+		 * can enter low bus mode if
+		 * there is no high bus request pending
+		 */
+		schedule_delayed_work(&bus_freq_daemon,
+			usecs_to_jiffies(5000000));
+	} else if (strncmp(buf, "0", 1) == 0) {
+		if (bus_freq_scaling_is_active)
+			set_high_bus_freq(1);
+		bus_freq_scaling_is_active = 0;
+	}
+	return size;
+}
+
+static int bus_freq_pm_notify(struct notifier_block *nb, unsigned long event,
+	void *dummy)
+{
+	mutex_lock(&bus_freq_mutex);
+
+	if (event == PM_SUSPEND_PREPARE) {
+		high_bus_count++;
+		set_high_bus_freq(1);
+		busfreq_suspended = 1;
+	} else if (event == PM_POST_SUSPEND) {
+		busfreq_suspended = 0;
+		high_bus_count--;
+		schedule_delayed_work(&bus_freq_daemon,
+			usecs_to_jiffies(5000000));
+	}
+
+	mutex_unlock(&bus_freq_mutex);
+
+	return NOTIFY_OK;
+}
+
+static int busfreq_reboot_notifier_event(struct notifier_block *this,
+						 unsigned long event, void *ptr)
+{
+	/* System is rebooting. Set the system into high_bus_freq_mode. */
+	request_bus_freq(BUS_FREQ_HIGH);
+
+	return 0;
+}
+
+static struct notifier_block imx_bus_freq_pm_notifier = {
+	.notifier_call = bus_freq_pm_notify,
+};
+
+static struct notifier_block imx_busfreq_reboot_notifier = {
+	.notifier_call = busfreq_reboot_notifier_event,
+};
+
+
+static DEVICE_ATTR(enable, 0644, bus_freq_scaling_enable_show,
+			bus_freq_scaling_enable_store);
+
+/*!
+ * This is the probe routine for the bus frequency driver.
+ *
+ * @param   pdev   The platform device structure
+ *
+ * @return         The function returns 0 on success
+ *
+ */
+
+static int busfreq_probe(struct platform_device *pdev)
+{
+	u32 err;
+
+	busfreq_dev = &pdev->dev;
+
+	/* Return if no IRAM space is allocated for ddr freq change code. */
+	if (!ddr_freq_change_iram_base)
+		return -ENOMEM;
+
+	if (cpu_is_imx7d()) {
+		osc_clk = devm_clk_get(&pdev->dev, "osc");
+		axi_sel_clk = devm_clk_get(&pdev->dev, "axi_sel");
+		ahb_sel_clk = devm_clk_get(&pdev->dev, "ahb_sel");
+		pfd0_392m = devm_clk_get(&pdev->dev, "pfd0_392m");
+		dram_root = devm_clk_get(&pdev->dev, "dram_root");
+		dram_alt_sel = devm_clk_get(&pdev->dev, "dram_alt_sel");
+		pll_dram = devm_clk_get(&pdev->dev, "pll_dram");
+		dram_alt_root = devm_clk_get(&pdev->dev, "dram_alt_root");
+		pfd1_332m = devm_clk_get(&pdev->dev, "pfd1_332m");
+		pfd2_270m = devm_clk_get(&pdev->dev, "pfd2_270m");
+		ahb_clk = devm_clk_get(&pdev->dev, "ahb");
+		axi_clk = devm_clk_get(&pdev->dev, "axi");
+		if (IS_ERR(osc_clk) || IS_ERR(axi_sel_clk) || IS_ERR(ahb_clk)
+			|| IS_ERR(pfd0_392m) || IS_ERR(dram_root)
+			|| IS_ERR(dram_alt_sel) || IS_ERR(pll_dram)
+			|| IS_ERR(dram_alt_root) || IS_ERR(pfd1_332m)
+			|| IS_ERR(ahb_clk) || IS_ERR(axi_clk)
+			|| IS_ERR(pfd2_270m)) {
+			dev_err(busfreq_dev,
+				"%s: failed to get busfreq clk\n", __func__);
+			return -EINVAL;
+		}
+	}
+
+	err = sysfs_create_file(&busfreq_dev->kobj, &dev_attr_enable.attr);
+	if (err) {
+		dev_err(busfreq_dev,
+		       "Unable to register sysdev entry for BUSFREQ");
+		return err;
+	}
+
+	if (of_property_read_u32(pdev->dev.of_node, "fsl,max_ddr_freq",
+			&ddr_normal_rate)) {
+		dev_err(busfreq_dev, "max_ddr_freq entry missing\n");
+		return -EINVAL;
+	}
+
+	high_bus_freq_mode = 1;
+	med_bus_freq_mode = 0;
+	low_bus_freq_mode = 0;
+	audio_bus_freq_mode = 0;
+	ultra_low_bus_freq_mode = 0;
+	cur_bus_freq_mode = BUS_FREQ_HIGH;
+
+	bus_freq_scaling_is_active = 1;
+	bus_freq_scaling_initialized = 1;
+
+	ddr_low_rate = LPAPM_CLK;
+
+	INIT_DELAYED_WORK(&low_bus_freq_handler, reduce_bus_freq_handler);
+	INIT_DELAYED_WORK(&bus_freq_daemon, bus_freq_daemon_handler);
+	register_pm_notifier(&imx_bus_freq_pm_notifier);
+	register_reboot_notifier(&imx_busfreq_reboot_notifier);
+
+	/* enter low bus mode if no high speed device enabled */
+	schedule_delayed_work(&bus_freq_daemon,
+		msecs_to_jiffies(20000));
+
+	/*
+	 * Need to make sure to an entry for the ddr freq change code
+	 * address in the IRAM page table.
+	 * This is only required if the DDR freq code and suspend/idle
+	 * code are in different OCRAM spaces.
+	 */
+	if ((iram_tlb_phys_addr & 0xFFF00000) !=
+		(ddr_freq_change_iram_phys & 0xFFF00000)) {
+		unsigned long i;
+
+		/*
+		 * Make sure the ddr_iram virtual address has a mapping
+		 * in the IRAM page table.
+		 */
+		i = ((IMX_IO_P2V(ddr_freq_change_iram_phys) >> 20) << 2) / 4;
+		*((unsigned long *)iram_tlb_base_addr + i) =
+			(ddr_freq_change_iram_phys  & 0xFFF00000) |
+			TT_ATTRIB_NON_CACHEABLE_1M;
+	}
+
+	if (cpu_is_imx7d()) {
+		ddr_type = imx_ddrc_get_ddr_type();
+		/* reduce ddr3 normal rate to 400M due to CKE issue on TO1.1 */
+		if (imx_get_soc_revision() == IMX_CHIP_REVISION_1_1 &&
+			ddr_type == IMX_DDR_TYPE_DDR3) {
+			ddr_normal_rate = 400000000;
+			pr_info("ddr3 normal rate changed to 400MHz for TO1.1.\n");
+		}
+		busfreq_func.init   = &init_ddrc_ddr_settings;
+		busfreq_func.update = &update_ddr_freq_imx_smp;
+	}
+
+	if (busfreq_func.init)
+		err = busfreq_func.init(pdev);
+	else
+		err = -EINVAL;
+
+	if (err) {
+		dev_err(busfreq_dev, "Busfreq init of ddr controller failed\n");
+		return err;
+	}
+
+	return 0;
+}
+
+static const struct of_device_id imx_busfreq_ids[] = {
+	{ .compatible = "fsl,imx_busfreq", },
+	{ /* sentinel */ }
+};
+
+static struct platform_driver busfreq_driver = {
+	.driver = {
+		.name = "imx_busfreq",
+		.owner  = THIS_MODULE,
+		.of_match_table = imx_busfreq_ids,
+		},
+	.probe = busfreq_probe,
+};
+
+/*!
+ * Initialise the busfreq_driver.
+ *
+ * @return  The function always returns 0.
+ */
+
+static int __init busfreq_init(void)
+{
+#ifndef CONFIG_MX6_VPU_352M
+	if (platform_driver_register(&busfreq_driver) != 0)
+		return -ENODEV;
+
+	pr_info("Bus freq driver module loaded\n");
+#endif
+	return 0;
+}
+
+static void __exit busfreq_cleanup(void)
+{
+	sysfs_remove_file(&busfreq_dev->kobj, &dev_attr_enable.attr);
+
+	/* Unregister the device structure */
+	platform_driver_unregister(&busfreq_driver);
+	bus_freq_scaling_initialized = 0;
+}
+
+module_init(busfreq_init);
+module_exit(busfreq_cleanup);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("BusFreq driver");
+MODULE_LICENSE("GPL");
diff --git a/arch/arm/mach-imx/busfreq_ddr3.c b/arch/arm/mach-imx/busfreq_ddr3.c
new file mode 100644
index 000000000000..3bd03dab05e6
--- /dev/null
+++ b/arch/arm/mach-imx/busfreq_ddr3.c
@@ -0,0 +1,310 @@
+/*
+ * Copyright (C) 2011-2016 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file busfreq_ddr3.c
+ *
+ * @brief iMX6 DDR3 frequency change specific file.
+ *
+ * @ingroup PM
+ */
+#include <asm/cacheflush.h>
+#include <asm/fncpy.h>
+#include <asm/io.h>
+#include <asm/mach/map.h>
+#include <asm/mach-types.h>
+#include <asm/tlb.h>
+#include <linux/busfreq-imx.h>
+#include <linux/clk.h>
+#include <linux/clockchips.h>
+#include <linux/cpumask.h>
+#include <linux/delay.h>
+#include <linux/genalloc.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/irqchip/arm-gic.h>
+#include <linux/kernel.h>
+#include <linux/mutex.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_device.h>
+#include <linux/platform_device.h>
+#include <linux/proc_fs.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/smp.h>
+
+#include "hardware.h"
+#include "common.h"
+
+#define SMP_WFE_CODE_SIZE	0x400
+
+#define MIN_DLL_ON_FREQ		333000000
+#define MAX_DLL_OFF_FREQ	125000000
+#define MMDC0_MPMUR0		0x8b8
+#define MMDC0_MPMUR0_OFFSET	16
+#define MMDC0_MPMUR0_MASK	0x3ff
+
+/*
+ * This structure is for passing necessary data for low level ocram
+ * busfreq code(arch/arm/mach-imx/ddr3_freq_imx6.S), if this struct
+ * definition is changed, the offset definition in
+ * arch/arm/mach-imx/ddr3_freq_imx6.S must be also changed accordingly,
+ * otherwise, the busfreq change function will be broken!
+ *
+ * This structure will be placed in front of the asm code on ocram.
+ */
+struct imx6_busfreq_info {
+	u32 freq;
+	void *ddr_settings;
+	u32 dll_off;
+	void *iomux_offsets;
+	u32 mu_delay_val;
+} __aligned(8);
+
+/* DDR settings */
+static unsigned long (*iram_iomux_settings)[2];
+
+static void __iomem *gic_dist_base;
+
+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);
+
+extern unsigned int ddr_normal_rate;
+extern int low_bus_freq_mode;
+extern int audio_bus_freq_mode;
+
+extern unsigned long save_ttbr1(void);
+extern void restore_ttbr1(unsigned long ttbr1);
+extern unsigned long ddr_freq_change_iram_base;
+
+extern unsigned long ddr_freq_change_total_size;
+extern unsigned long iram_tlb_phys_addr;
+
+#ifdef CONFIG_SMP
+volatile u32 *wait_for_ddr_freq_update;
+static unsigned int online_cpus;
+static u32 *irqs_used;
+
+void (*wfe_change_ddr_freq)(u32 cpuid, u32 *ddr_freq_change_done);
+void (*imx7_wfe_change_ddr_freq)(u32 cpuid, u32 ocram_base);
+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;
+#endif
+
+int can_change_ddr_freq(void)
+{
+	return 1;
+}
+
+#ifdef CONFIG_SMP
+/*
+ * each active core apart from the one changing
+ * the DDR frequency will execute this function.
+ * the rest of the cores have to remain in WFE
+ * state until the frequency is changed.
+ */
+static irqreturn_t wait_in_wfe_irq(int irq, void *dev_id)
+{
+	u32 me;
+
+	me = smp_processor_id();
+#ifdef CONFIG_LOCAL_TIMERS
+	clockevents_notify(CLOCK_EVT_NOTIFY_BROADCAST_ENTER,
+		&me);
+#endif
+	if (cpu_is_imx7d())
+		imx7_wfe_change_ddr_freq(0x8 * me,
+			(u32)ddr_freq_change_iram_base);
+	else
+		wfe_change_ddr_freq(0xff << (me * 8),
+			(u32 *)&iram_iomux_settings[0][1]);
+#ifdef CONFIG_LOCAL_TIMERS
+	clockevents_notify(CLOCK_EVT_NOTIFY_BROADCAST_EXIT,
+		&me);
+#endif
+
+	return IRQ_HANDLED;
+}
+#endif
+
+/* change the DDR frequency. */
+int update_ddr_freq_imx_smp(int ddr_rate)
+{
+	int me = 0;
+	unsigned long ttbr1;
+#ifdef CONFIG_SMP
+	unsigned int reg = 0;
+	int cpu = 0;
+#endif
+
+	if (!can_change_ddr_freq())
+		return -1;
+
+	if (ddr_rate == curr_ddr_rate)
+		return 0;
+
+	printk(KERN_DEBUG "\nBus freq set to %d start...\n", ddr_rate);
+
+	/* ensure that all Cores are in WFE. */
+	local_irq_disable();
+
+#ifdef CONFIG_SMP
+	me = smp_processor_id();
+
+	/* Make sure all the online cores are active */
+	while (1) {
+		bool not_exited_busfreq = false;
+		u32 reg = 0;
+
+		for_each_online_cpu(cpu) {
+			if (cpu_is_imx7d())
+				reg = *(wait_for_ddr_freq_update + 1);
+
+			if (reg & (0x02 << (cpu * 8)))
+				not_exited_busfreq = true;
+		}
+		if (!not_exited_busfreq)
+			break;
+	}
+
+	wmb();
+	*wait_for_ddr_freq_update = 1;
+	dsb();
+	if (cpu_is_imx7d())
+		online_cpus = *(wait_for_ddr_freq_update + 1);
+	for_each_online_cpu(cpu) {
+		*((char *)(&online_cpus) + (u8)cpu) = 0x02;
+		if (cpu != me) {
+			/* set the interrupt to be pending in the GIC. */
+			reg = 1 << (irqs_used[cpu] % 32);
+			writel_relaxed(reg, gic_dist_base + GIC_DIST_PENDING_SET
+				+ (irqs_used[cpu] / 32) * 4);
+		}
+	}
+	/* Wait for the other active CPUs to idle */
+	while (1) {
+		u32 reg = 0;
+
+		if (cpu_is_imx7d())
+			reg = *(wait_for_ddr_freq_update + 1);
+		reg |= (0x02 << (me * 8));
+		if (reg == online_cpus)
+			break;
+	}
+#endif
+
+	/* Ensure iram_tlb_phys_addr is flushed to DDR. */
+	__cpuc_flush_dcache_area(&iram_tlb_phys_addr,
+		sizeof(iram_tlb_phys_addr));
+
+	ttbr1 = save_ttbr1();
+	/* Now we can change the DDR frequency. */
+	if (cpu_is_imx7d())
+		imx7d_change_ddr_freq(ddr_rate);
+	restore_ttbr1(ttbr1);
+	curr_ddr_rate = ddr_rate;
+
+#ifdef CONFIG_SMP
+	wmb();
+	/* DDR frequency change is done . */
+	*wait_for_ddr_freq_update = 0;
+	dsb();
+
+	/* wake up all the cores. */
+	sev();
+#endif
+
+	local_irq_enable();
+
+	printk(KERN_DEBUG "Bus freq set to %d done! cpu=%d\n", ddr_rate, me);
+
+	return 0;
+}
+
+int init_ddrc_ddr_settings(struct platform_device *busfreq_pdev)
+{
+	int ddr_type = imx_ddrc_get_ddr_type();
+#ifdef CONFIG_SMP
+	struct device_node *node;
+	u32 cpu;
+	struct device *dev = &busfreq_pdev->dev;
+	int err;
+	struct irq_data *d;
+
+	node = of_find_compatible_node(NULL, NULL, "arm,cortex-a7-gic");
+	if (!node) {
+		printk(KERN_ERR "failed to find imx7d-a7-gic device tree data!\n");
+		return -EINVAL;
+	}
+	gic_dist_base = of_iomap(node, 0);
+	WARN(!gic_dist_base, "unable to map gic dist registers\n");
+
+	irqs_used = devm_kzalloc(dev, sizeof(u32) * num_present_cpus(),
+					GFP_KERNEL);
+	for_each_online_cpu(cpu) {
+		int irq;
+		/*
+		 * 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, "ddrc", 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;
+	}
+
+	/* Store the variable used to communicate between cores */
+	wait_for_ddr_freq_update = (u32 *)ddr_freq_change_iram_base;
+	imx7_wfe_change_ddr_freq = (void *)fncpy(
+		(void *)ddr_freq_change_iram_base + 0x8,
+		&imx7_smp_wfe, SMP_WFE_CODE_SIZE - 0x8);
+#endif
+	if (ddr_type == IMX_DDR_TYPE_DDR3)
+		imx7d_change_ddr_freq = (void *)fncpy(
+			(void *)ddr_freq_change_iram_base + SMP_WFE_CODE_SIZE,
+			&imx7d_ddr3_freq_change,
+			MX7_BUSFREQ_OCRAM_SIZE - SMP_WFE_CODE_SIZE);
+	else if (ddr_type == IMX_DDR_TYPE_LPDDR3
+		|| ddr_type == IMX_DDR_TYPE_LPDDR2)
+		imx7d_change_ddr_freq = (void *)fncpy(
+			(void *)ddr_freq_change_iram_base +
+			SMP_WFE_CODE_SIZE,
+			&imx_lpddr3_freq_change,
+			MX7_BUSFREQ_OCRAM_SIZE - SMP_WFE_CODE_SIZE);
+
+	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
new file mode 100644
index 000000000000..2ef1806bbc35
--- /dev/null
+++ b/arch/arm/mach-imx/busfreq_lpddr2.c
@@ -0,0 +1,373 @@
+/*
+ * Copyright (C) 2011-2016 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2017 NXP.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file busfreq_lpddr2.c
+ *
+ * @brief iMX6 LPDDR2 frequency change specific file.
+ *
+ * @ingroup PM
+ */
+#include <asm/cacheflush.h>
+#include <asm/fncpy.h>
+#include <asm/io.h>
+#include <asm/mach/map.h>
+#include <asm/mach-types.h>
+#include <asm/tlb.h>
+#include <linux/busfreq-imx.h>
+#include <linux/clk.h>
+#include <linux/clockchips.h>
+#include <linux/cpumask.h>
+#include <linux/delay.h>
+#include <linux/genalloc.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/irqchip/arm-gic.h>
+#include <linux/kernel.h>
+#include <linux/mutex.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_device.h>
+#include <linux/platform_device.h>
+#include <linux/proc_fs.h>
+#include <linux/sched.h>
+#include <linux/smp.h>
+#include <linux/slab.h>
+
+#include "common.h"
+#include "hardware.h"
+
+static struct device *busfreq_dev;
+static int curr_ddr_rate;
+static DEFINE_SPINLOCK(freq_lock);
+
+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);
+extern unsigned long ddr_freq_change_iram_base;
+extern unsigned long imx6_lpddr2_freq_change_start asm("imx6_lpddr2_freq_change_start");
+extern unsigned long imx6_lpddr2_freq_change_end asm("imx6_lpddr2_freq_change_end");
+extern unsigned long mx6q_lpddr2_freq_change_start asm("mx6q_lpddr2_freq_change_start");
+extern unsigned long mx6q_lpddr2_freq_change_end asm("mx6q_lpddr2_freq_change_end");
+extern unsigned long iram_tlb_phys_addr;
+
+struct mmdc_settings_info {
+	u32 size;
+	void *settings;
+	int freq;
+} __aligned(8);
+static struct mmdc_settings_info *mmdc_settings_info;
+void (*mx6_change_lpddr2_freq_smp)(u32 ddr_freq, struct mmdc_settings_info
+		*mmdc_settings_info) = NULL;
+
+static int mmdc_settings_size;
+static unsigned long (*mmdc_settings)[2];
+static unsigned long (*iram_mmdc_settings)[2];
+static unsigned long *iram_settings_size;
+static unsigned long *iram_ddr_freq_chage;
+unsigned long mmdc_timing_settings[][2] = {
+	{0x0C, 0x0},	/* mmdc_mdcfg0 */
+	{0x10, 0x0},	/* mmdc_mdcfg1 */
+	{0x14, 0x0},	/* mmdc_mdcfg2 */
+	{0x18, 0x0},	/* mmdc_mdmisc */
+	{0x38, 0x0},	/* mmdc_mdcfg3lp */
+};
+
+#ifdef CONFIG_SMP
+volatile u32 *wait_for_lpddr2_freq_update;
+static unsigned int online_cpus;
+static u32 *irqs_used;
+void (*wfe_change_lpddr2_freq)(u32 cpuid, u32 *ddr_freq_change_done);
+extern void wfe_smp_freq_change(u32 cpuid, u32 *ddr_freq_change_done);
+extern unsigned long wfe_smp_freq_change_start asm("wfe_smp_freq_change_start");
+extern unsigned long wfe_smp_freq_change_end asm("wfe_smp_freq_change_end");
+extern void __iomem *imx_scu_base;
+static void __iomem *gic_dist_base;
+#endif
+
+#ifdef CONFIG_SMP
+static irqreturn_t wait_in_wfe_irq(int irq, void *dev_id)
+{
+	u32 me;
+
+	me = smp_processor_id();
+#ifdef CONFIG_LOCAL_TIMERS
+	clockevents_notify(CLOCK_EVT_NOTIFY_BROADCAST_ENTER, &me);
+#endif
+	wfe_change_lpddr2_freq(0xff << (me * 8),
+			(u32 *)ddr_freq_change_iram_base);
+#ifdef CONFIG_LOCAL_TIMERS
+	clockevents_notify(CLOCK_EVT_NOTIFY_BROADCAST_EXIT, &me);
+#endif
+	return IRQ_HANDLED;
+}
+#endif
+
+/* change the DDR frequency. */
+int update_lpddr2_freq(int ddr_rate)
+{
+	unsigned long ttbr1, flags;
+	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);
+
+	spin_lock_irqsave(&freq_lock, flags);
+	/*
+	 * Flush the TLB, to ensure no TLB maintenance occurs
+	 * when DDR is in self-refresh.
+	 */
+	ttbr1 = save_ttbr1();
+
+	/* Now change DDR frequency. */
+	if (cpu_is_imx6sl())
+		mx6_change_lpddr2_freq(ddr_rate,
+			(mode == BUS_FREQ_LOW || mode == BUS_FREQ_ULTRA_LOW) ? 1 : 0);
+	else
+		mx6_change_lpddr2_freq(ddr_rate,
+			(mode == BUS_FREQ_LOW || mode == BUS_FREQ_AUDIO) ? 1 : 0);
+
+	restore_ttbr1(ttbr1);
+
+	curr_ddr_rate = ddr_rate;
+	spin_unlock_irqrestore(&freq_lock, flags);
+
+	printk(KERN_DEBUG "\nBus freq set to %d done...\n", ddr_rate);
+
+	return 0;
+}
+
+int init_mmdc_lpddr2_settings(struct platform_device *busfreq_pdev)
+{
+	unsigned long ddr_code_size;
+	busfreq_dev = &busfreq_pdev->dev;
+
+	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;
+}
+
+int update_lpddr2_freq_smp(int ddr_rate)
+{
+	unsigned long ttbr1;
+	int i, me = 0;
+#ifdef CONFIG_SMP
+	int cpu = 0;
+	u32 reg = 0;
+#endif
+
+	if (ddr_rate == curr_ddr_rate)
+		return 0;
+
+	printk(KERN_DEBUG "Bus freq set to %d start...\n", ddr_rate);
+
+	for (i=0; i < mmdc_settings_size; i++) {
+		iram_mmdc_settings[i][0] = mmdc_settings[i][0];
+		iram_mmdc_settings[i][1] = mmdc_settings[i][1];
+	}
+
+	mmdc_settings_info->size = mmdc_settings_size;
+	mmdc_settings_info->settings = iram_mmdc_settings;
+	mmdc_settings_info->freq = curr_ddr_rate;
+
+	/* ensure that all Cores are in WFE. */
+	local_irq_disable();
+
+#ifdef CONFIG_SMP
+	me = smp_processor_id();
+
+	/* Make sure all the online cores are active */
+	while (1) {
+		bool not_exited_busfreq = false;
+		for_each_online_cpu(cpu) {
+			reg = __raw_readl(imx_scu_base + 0x08);
+			if (reg & (0x02 << (cpu * 8)))
+				not_exited_busfreq = true;
+		}
+		if (!not_exited_busfreq)
+			break;
+	}
+
+	wmb();
+	*wait_for_lpddr2_freq_update = 1;
+	dsb();
+	online_cpus = readl_relaxed(imx_scu_base + 0x08);
+	for_each_online_cpu(cpu) {
+		*((char *)(&online_cpus) + (u8)cpu) = 0x02;
+		if (cpu != me) {
+			reg = 1 << (irqs_used[cpu] % 32);
+			writel_relaxed(reg, gic_dist_base + GIC_DIST_PENDING_SET
+					+ (irqs_used[cpu] / 32) * 4);
+		}
+	}
+
+	/* Wait for the other active CPUs to idle */
+	while (1) {
+		reg = 0;
+		reg = readl_relaxed(imx_scu_base + 0x08);
+		reg |= (0x02 << (me * 8));
+		if (reg == online_cpus)
+			break;
+	}
+#endif
+
+	/* Ensure iram_tlb_phys_addr is flushed to DDR. */
+	__cpuc_flush_dcache_area(&iram_tlb_phys_addr,
+			sizeof(iram_tlb_phys_addr));
+	outer_clean_range(__pa(&iram_tlb_phys_addr),
+			__pa(&iram_tlb_phys_addr + 1));
+	/*
+	 * Flush the TLB, to ensure no TLB maintenance occurs
+	 * when DDR is in self-refresh.
+	 */
+	ttbr1 = save_ttbr1();
+
+	curr_ddr_rate = ddr_rate;
+
+	/* Now change DDR frequency. */
+	mx6_change_lpddr2_freq_smp(ddr_rate, mmdc_settings_info);
+
+	restore_ttbr1(ttbr1);
+
+#ifdef CONFIG_SMP
+	wmb();
+	/* DDR frequency change is done . */
+	*wait_for_lpddr2_freq_update = 0;
+	dsb();
+	/* wake up all the cores. */
+	sev();
+#endif
+
+	local_irq_enable();
+
+	printk(KERN_DEBUG "Bus freq set to %d done! cpu=%d\n", ddr_rate, me);
+
+	return 0;
+}
+
+int init_mmdc_lpddr2_settings_mx6q(struct platform_device *busfreq_pdev)
+{
+	struct device *dev = &busfreq_pdev->dev;
+	unsigned long ddr_code_size = 0;
+	unsigned long wfe_code_size = 0;
+	struct device_node *node;
+	void __iomem *mmdc_base;
+	int i;
+#ifdef CONFIG_SMP
+	struct irq_data *d;
+	u32 cpu;
+	int err;
+#endif
+
+	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);
+	if (!mmdc_base) {
+		dev_err(dev, "unable to map mmdc registers\n");
+		return -EINVAL;
+	}
+
+	mmdc_settings_size = ARRAY_SIZE(mmdc_timing_settings);
+	mmdc_settings = kmalloc((mmdc_settings_size * 8), GFP_KERNEL);
+	memcpy(mmdc_settings, mmdc_timing_settings,
+			sizeof(mmdc_timing_settings));
+
+#ifdef CONFIG_SMP
+	node = of_find_compatible_node(NULL, NULL, "arm,cortex-a9-gic");
+	if (!node) {
+		printk(KERN_ERR "failed to find imx6q-a9-gic device tree data!\n");
+		return -EINVAL;
+	}
+
+	gic_dist_base = of_iomap(node, 0);
+	WARN(!gic_dist_base, "unable to map gic dist registers\n");
+
+	irqs_used = devm_kzalloc(dev, sizeof(u32) * num_present_cpus(),
+			GFP_KERNEL);
+
+	for_each_online_cpu(cpu) {
+		int irq = platform_get_irq(busfreq_pdev, cpu);
+		err = request_irq(irq, wait_in_wfe_irq, IRQF_PERCPU,
+				"mmdc_1", NULL);
+		if (err) {
+			dev_err(dev,
+				"Busfreq:request_irq failed %d, err = %d\n",
+				irq, err);
+			return err;
+		}
+		err = irq_set_affinity(irq, cpumask_of(cpu));
+		if (err) {
+			dev_err(dev,
+				"Busfreq: Cannot set irq affinity irq=%d,\n",
+				irq);
+			return err;
+		}
+		d = irq_get_irq_data(irq);
+		irqs_used[cpu] = d->hwirq + 32;
+	}
+
+	/* Stoange_iram_basee the variable used to communicate between cores in
+	 * a non-cacheable IRAM area */
+	wait_for_lpddr2_freq_update = (u32 *)ddr_freq_change_iram_base;
+	wfe_code_size = (&wfe_smp_freq_change_end - &wfe_smp_freq_change_start) *4;
+
+	wfe_change_lpddr2_freq = (void *)fncpy((void *)ddr_freq_change_iram_base + 0x8,
+			&wfe_smp_freq_change, wfe_code_size);
+#endif
+	iram_settings_size = (void *)ddr_freq_change_iram_base + wfe_code_size + 0x8;
+	iram_mmdc_settings = (void *)iram_settings_size + sizeof(*mmdc_settings_info);
+	iram_ddr_freq_chage = (void *)iram_mmdc_settings + (mmdc_settings_size * 8) + 0x8;
+	mmdc_settings_info = (struct mmdc_settings_info *)iram_settings_size;
+
+	ddr_code_size = (&mx6q_lpddr2_freq_change_end -&mx6q_lpddr2_freq_change_start) *4;
+
+	mx6_change_lpddr2_freq_smp = (void *)fncpy(iram_ddr_freq_chage,
+			&mx6q_lpddr2_freq_change, ddr_code_size);
+
+	/* save initial mmdc boot timing settings */
+	for (i=0; i < mmdc_settings_size; i++)
+		mmdc_settings[i][1] = readl_relaxed(mmdc_base +
+				mmdc_settings[i][0]);
+
+	curr_ddr_rate = ddr_normal_rate;
+
+	return 0;
+}
diff --git a/arch/arm/mach-imx/common.c b/arch/arm/mach-imx/common.c
new file mode 100644
index 000000000000..cf57b55a369b
--- /dev/null
+++ b/arch/arm/mach-imx/common.c
@@ -0,0 +1,166 @@
+/*
+ * Copyright (C) 2015-2016 Freescale Semiconductor, Inc.
+ *
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#include <linux/io.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_net.h>
+#include <linux/slab.h>
+
+#include "hardware.h"
+
+unsigned long iram_tlb_base_addr;
+unsigned long iram_tlb_phys_addr;
+
+unsigned long save_ttbr1(void)
+{
+	unsigned long lttbr1;
+	asm volatile(
+		".align 4\n"
+		"mrc p15, 0, %0, c2, c0, 1\n"
+		: "=r" (lttbr1)
+	);
+	return lttbr1;
+}
+
+void restore_ttbr1(unsigned long ttbr1)
+{
+	asm volatile(
+		".align 4\n"
+		"mcr p15, 0, %0, c2, c0, 1\n"
+		: : "r" (ttbr1)
+	);
+}
+
+#define OCOTP_MAC_OFF	(cpu_is_imx7d() ? 0x640 : 0x620)
+#define OCOTP_MACn(n)	(OCOTP_MAC_OFF + (n) * 0x10)
+void __init imx6_enet_mac_init(const char *enet_compat, const char *ocotp_compat)
+{
+	struct device_node *ocotp_np, *enet_np, *from = NULL;
+	void __iomem *base;
+	struct property *newmac;
+	u32 macaddr_low;
+	u32 macaddr_high = 0;
+	u32 macaddr1_high = 0;
+	u8 *macaddr;
+	int i, id;
+
+	for (i = 0; i < 2; i++) {
+		enet_np = of_find_compatible_node(from, NULL, enet_compat);
+		if (!enet_np)
+			return;
+
+		from = enet_np;
+
+		if (of_get_mac_address(enet_np))
+			goto put_enet_node;
+
+		id = of_alias_get_id(enet_np, "ethernet");
+		if (id < 0)
+			id = i;
+
+		ocotp_np = of_find_compatible_node(NULL, NULL, ocotp_compat);
+		if (!ocotp_np) {
+			pr_warn("failed to find ocotp node\n");
+			goto put_enet_node;
+		}
+
+		base = of_iomap(ocotp_np, 0);
+		if (!base) {
+			pr_warn("failed to map ocotp\n");
+			goto put_ocotp_node;
+		}
+
+		macaddr_low = readl_relaxed(base + OCOTP_MACn(1));
+		if (id)
+			macaddr1_high = readl_relaxed(base + OCOTP_MACn(2));
+		else
+			macaddr_high = readl_relaxed(base + OCOTP_MACn(0));
+
+		newmac = kzalloc(sizeof(*newmac) + 6, GFP_KERNEL);
+		if (!newmac)
+			goto put_ocotp_node;
+
+		newmac->value = newmac + 1;
+		newmac->length = 6;
+		newmac->name = kstrdup("local-mac-address", GFP_KERNEL);
+		if (!newmac->name) {
+			kfree(newmac);
+			goto put_ocotp_node;
+		}
+
+		macaddr = newmac->value;
+		if (id) {
+			macaddr[5] = (macaddr_low >> 16) & 0xff;
+			macaddr[4] = (macaddr_low >> 24) & 0xff;
+			macaddr[3] = macaddr1_high & 0xff;
+			macaddr[2] = (macaddr1_high >> 8) & 0xff;
+			macaddr[1] = (macaddr1_high >> 16) & 0xff;
+			macaddr[0] = (macaddr1_high >> 24) & 0xff;
+		} else {
+			macaddr[5] = macaddr_high & 0xff;
+			macaddr[4] = (macaddr_high >> 8) & 0xff;
+			macaddr[3] = (macaddr_high >> 16) & 0xff;
+			macaddr[2] = (macaddr_high >> 24) & 0xff;
+			macaddr[1] = macaddr_low & 0xff;
+			macaddr[0] = (macaddr_low >> 8) & 0xff;
+		}
+
+		of_update_property(enet_np, newmac);
+
+put_ocotp_node:
+	of_node_put(ocotp_np);
+put_enet_node:
+	of_node_put(enet_np);
+	}
+}
+
+#ifndef CONFIG_HAVE_IMX_GPC
+int imx_gpc_mf_request_on(unsigned int irq, unsigned int on) { return 0; }
+EXPORT_SYMBOL_GPL(imx_gpc_mf_request_on);
+#endif
+
+#if !defined(CONFIG_SOC_IMX6SL)
+u32 imx6_lpddr2_freq_change_start, imx6_lpddr2_freq_change_end;
+void mx6_lpddr2_freq_change(u32 freq, int bus_freq_mode) {}
+#endif
+
+#if !defined(CONFIG_SOC_IMX6SLL)
+void imx6sll_lpddr2_freq_change(u32 freq, int bus_freq_mode) {}
+#endif
+
+#if !defined(CONFIG_SOC_IMX6SX) && !defined(CONFIG_SOC_IMX6UL)
+u32 imx6_up_ddr3_freq_change_start, imx6_up_ddr3_freq_change_end;
+struct imx6_busfreq_info {
+} __aligned(8);
+void imx6_up_ddr3_freq_change(struct imx6_busfreq_info *busfreq_info) {}
+void imx6_up_lpddr2_freq_change(u32 freq, int bus_freq_mode) {}
+#endif
+
+#if !defined(CONFIG_SOC_IMX6ULL)
+u32 mx6ull_lpm_wfi_start, mx6ull_lpm_wfi_end;
+void imx6ull_low_power_idle(void) {}
+#endif
+
+#if !defined(CONFIG_SOC_IMX6Q)
+u32 mx6_ddr3_freq_change_start, mx6_ddr3_freq_change_end;
+u32 mx6q_lpddr2_freq_change_start, mx6q_lpddr2_freq_change_end;
+u32 wfe_smp_freq_change_start, wfe_smp_freq_change_end;
+void mx6_ddr3_freq_change(u32 freq, void *ddr_settings,
+	bool dll_mode, void *iomux_offsets) {}
+void mx6q_lpddr2_freq_change(u32 freq, void *ddr_settings) {}
+void wfe_smp_freq_change(u32 cpuid, u32 *ddr_freq_change_done) {}
+#endif
+
+#if !defined(CONFIG_SOC_IMX7D)
+void imx7_smp_wfe(u32 cpuid, u32 ocram_base) {}
+void imx7d_ddr3_freq_change(u32 freq) {}
+#endif
+
diff --git a/arch/arm/mach-imx/common.h b/arch/arm/mach-imx/common.h
index 912aeceb4ff8..60add3295015 100644
--- a/arch/arm/mach-imx/common.h
+++ b/arch/arm/mach-imx/common.h
@@ -104,6 +104,8 @@ void imx6_set_int_mem_clk_lpm(bool enable);
 void imx6sl_set_wait_clk(bool enter);
 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 imx_cpu_die(unsigned int cpu);
 int imx_cpu_kill(unsigned int cpu);
@@ -120,6 +122,12 @@ static const u32 imx53_suspend_sz;
 static inline void imx6_suspend(void __iomem *ocram_vbase) {}
 #endif
 
+#ifdef CONFIG_HAVE_IMX_DDRC
+int imx_ddrc_get_ddr_type(void);
+#else
+static inline int imx_ddrc_get_ddr_type(void) { return 0; }
+#endif
+
 void imx6_pm_ccm_init(const char *ccm_compat);
 void imx6q_pm_init(void);
 void imx6dl_pm_init(void);
diff --git a/arch/arm/mach-imx/ddr3_freq_imx7d.S b/arch/arm/mach-imx/ddr3_freq_imx7d.S
new file mode 100644
index 000000000000..9342e0d83f5e
--- /dev/null
+++ b/arch/arm/mach-imx/ddr3_freq_imx7d.S
@@ -0,0 +1,586 @@
+/*
+ * 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 "hardware.h"
+
+#define DDRC_MSTR		0x0
+#define DDRC_STAT		0x4
+#define DDRC_MRCTRL0		0x10
+#define DDRC_MRCTRL1		0x14
+#define DDRC_MRSTAT		0x18
+#define DDRC_PWRCTL		0x30
+#define DDRC_RFSHCTL3		0x60
+#define DDRC_RFSHTMG		0x64
+#define DDRC_DBG1		0x304
+#define DDRC_SWCTL		0x320
+#define DDRC_SWSTAT		0x324
+#define DDRC_PSTAT		0x3fc
+#define DDRC_PCTRL_0		0x490
+#define DDRC_ZQCTL0		0x180
+#define DDRC_DFIMISC		0x1b0
+#define DDRC_DBGCAM		0x308
+#define DDRPHY_LP_CON0		0x18
+#define IOMUXC_GPR8		0x20
+#define DDRPHY_MDLL_CON0	0xb0
+#define DDRPHY_MDLL_CON1	0xb4
+#define DDRPHY_OFFSETD_CON0	0x50
+#define DDRPHY_OFFSETR_CON0	0x20
+#define DDRPHY_OFFSETR_CON1	0x24
+#define DDRPHY_OFFSETR_CON2	0x28
+#define DDRPHY_OFFSETW_CON0	0x30
+#define DDRPHY_OFFSETW_CON1	0x34
+#define DDRPHY_OFFSETW_CON2	0x38
+#define DDRPHY_CA_WLDSKEW_CON0	0x6c
+#define DDRPHY_CA_DSKEW_CON0	0x7c
+#define DDRPHY_CA_DSKEW_CON1	0x80
+#define DDRPHY_CA_DSKEW_CON2	0x84
+
+#define ANADIG_DIGPROG		0x800
+
+	.align 3
+
+	.macro	switch_to_below_100m
+
+	ldr	r7, =0x2
+	str	r7, [r4, #DDRC_DBG1]
+
+	ldr	r6, =0x36000000
+1:
+	ldr	r7, [r4, #DDRC_DBGCAM]
+	and	r7, r7, r6
+	cmp	r7, r6
+	bne	1b
+
+	ldr	r6, =0x1
+2:
+	ldr	r7, [r4, #DDRC_MRSTAT]
+	and	r7, r7, r6
+	cmp	r7, r6
+	beq	2b
+
+	ldr	r7, =0x10f0
+	str	r7, [r4, #DDRC_MRCTRL0]
+	ldr	r7, =0x0
+	str	r7, [r4, #DDRC_MRCTRL1]
+	ldr	r7, =0x800010f0
+	str	r7, [r4, #DDRC_MRCTRL0]
+
+	ldr	r6, =0x1
+3:
+	ldr	r7, [r4, #DDRC_MRSTAT]
+	and	r7, r7, r6
+	cmp	r7, r6
+	beq	3b
+
+	ldr	r7, =0x20f0
+	str	r7, [r4, #DDRC_MRCTRL0]
+	ldr	r7, =0x8
+	str	r7, [r4, #DDRC_MRCTRL1]
+	ldr	r7, =0x800020f0
+	str	r7, [r4, #DDRC_MRCTRL0]
+
+	ldr	r6, =0x1
+4:
+	ldr	r7, [r4, #DDRC_MRSTAT]
+	and	r7, r7, r6
+	cmp	r7, r6
+	beq	4b
+
+	ldr	r7, =0x10f0
+	str	r7, [r4, #DDRC_MRCTRL0]
+	ldr	r7, =0x1
+	str	r7, [r4, #DDRC_MRCTRL1]
+	ldr	r7, =0x800010f0
+	str	r7, [r4, #DDRC_MRCTRL0]
+
+	ldr	r7, =0x20
+	str	r7, [r4, #DDRC_PWRCTL]
+
+	ldr	r6, =0x23
+5:
+	ldr	r7, [r4, #DDRC_STAT]
+	and	r7, r7, r6
+	cmp	r7, r6
+	bne	5b
+
+	ldr	r7, =0x0
+	str	r7, [r4, #DDRC_SWCTL]
+
+	ldr	r7, =0x03048001
+	str	r7, [r4, #DDRC_MSTR]
+
+	ldr	r7, =0x1
+	str	r7, [r4, #DDRC_SWCTL]
+
+	ldr	r6, =0x1
+6:
+	ldr	r7, [r4, #DDRC_SWSTAT]
+	and	r7, r7, r6
+	cmp	r7, r6
+	bne	6b
+
+	ldr	r7, =0x10010100
+	str	r7, [r5, #0x4]
+
+	ldr	r6, =24000000
+	cmp	r0, r6
+	beq	25f
+
+	ldr	r7, =0x000B000D
+	str	r7,[r4, #DDRC_RFSHTMG]
+	b	7f
+
+25:
+	ldr	r7, =0x00030004
+	str	r7,[r4, #DDRC_RFSHTMG]
+
+	/* dram alt sel set to OSC */
+	ldr	r7, =0x10000000
+	ldr	r8, =0xa080
+	str	r7, [r2, r8]
+	/* dram root set to from dram alt, div by 1 */
+	ldr	r7, =0x11000000
+	ldr	r8, =0x9880
+	str	r7, [r2, r8]
+	b	8f
+7:
+	/* dram alt sel set to pfd0_392m */
+	ldr	r7, =0x15000000
+	ldr	r8, =0xa080
+	str	r7, [r2, r8]
+	/* dram root set to from dram alt, div by 4 */
+	ldr	r7, =0x11000003
+	ldr	r8, =0x9880
+	str	r7, [r2, r8]
+8:
+	ldr	r7, =0x202ffd0
+	str	r7, [r5, #DDRPHY_MDLL_CON0]
+
+	ldr	r7, =0x1000007f
+	str	r7, [r5, #DDRPHY_OFFSETD_CON0]
+
+	ldr	r7, =0x7f7f7f7f
+	str	r7, [r5, #DDRPHY_OFFSETR_CON0]
+	str	r7, [r5, #DDRPHY_OFFSETR_CON1]
+	ldr	r7, =0x7f
+	str	r7, [r5, #DDRPHY_OFFSETR_CON2]
+
+	ldr	r7, =0x7f7f7f7f
+	str	r7, [r5, #DDRPHY_OFFSETW_CON0]
+	str	r7, [r5, #DDRPHY_OFFSETW_CON1]
+	ldr	r7, =0x7f
+	str	r7, [r5, #DDRPHY_OFFSETW_CON2]
+
+	ldr	r7, [r9, #ANADIG_DIGPROG]
+	and	r7, r7, #0x11
+	cmp	r7, #0x11
+	bne	20f
+
+	ldr	r7, =0x0
+	str	r7, [r5, #DDRPHY_CA_WLDSKEW_CON0]
+	ldr	r7, =0x60606060
+	str	r7, [r5, #DDRPHY_CA_DSKEW_CON0]
+	str	r7, [r5, #DDRPHY_CA_DSKEW_CON1]
+	ldr	r7, =0x00006060
+	str	r7, [r5, #DDRPHY_CA_DSKEW_CON2]
+	b	21f
+20:
+	ldr	r7, =0x0
+	str	r7, [r5, #DDRPHY_CA_DSKEW_CON0]
+	str	r7, [r5, #DDRPHY_CA_DSKEW_CON1]
+	str	r7, [r5, #DDRPHY_CA_DSKEW_CON2]
+21:
+	ldr	r7, =0x1100007f
+	str	r7, [r5, #DDRPHY_OFFSETD_CON0]
+	ldr	r7, =0x1000007f
+	str	r7, [r5, #DDRPHY_OFFSETD_CON0]
+
+	ldr	r7, =0x0
+	str	r7, [r4, #DDRC_PWRCTL]
+
+	ldr	r6, =0x1
+9:
+	ldr	r7, [r4, #DDRC_MRSTAT]
+	and	r7, r7, r6
+	cmp	r7, r6
+	beq	9b
+
+	ldr	r7, =0xf0
+	str	r7, [r4, #DDRC_MRCTRL0]
+	ldr	r7, =0x820
+	str	r7, [r4, #DDRC_MRCTRL1]
+	ldr	r7, =0x800000f0
+	str	r7, [r4, #DDRC_MRCTRL0]
+
+	ldr	r6, =0x1
+10:
+	ldr	r7, [r4, #DDRC_MRSTAT]
+	and	r7, r7, r6
+	cmp	r7, r6
+	beq	10b
+
+	ldr	r7, =0x800020
+	str	r7, [r4, #DDRC_ZQCTL0]
+
+	ldr	r7, =0x0
+	str	r7, [r4, #DDRC_DBG1]
+
+	/* enable auto self-refresh */
+	ldr	r7, [r4, #DDRC_PWRCTL]
+	orr	r7, r7, #(1 << 0)
+	str	r7, [r4, #DDRC_PWRCTL]
+
+	.endm
+
+	.macro	switch_to_533m
+
+	ldr	r7, =0x2
+	str	r7, [r4, #DDRC_DBG1]
+
+	ldr	r7, =0x78
+	str	r7, [r3, #IOMUXC_GPR8]
+	orr	r7, r7, #0x100
+	str	r7, [r3, #IOMUXC_GPR8]
+
+	ldr	r6, =0x30000000
+11:
+	ldr	r7, [r4, #DDRC_DBGCAM]
+	and	r7, r7, r6
+	cmp	r7, r6
+	bne	11b
+
+	ldr	r6, =0x1
+12:
+	ldr	r7, [r4, #DDRC_MRSTAT]
+	and	r7, r7, r6
+	cmp	r7, r6
+	beq	12b
+
+	ldr	r7, =0x10f0
+	str	r7, [r4, #DDRC_MRCTRL0]
+	ldr	r7, =0x1
+	str	r7, [r4, #DDRC_MRCTRL1]
+	ldr	r7, =0x800010f0
+	str	r7, [r4, #DDRC_MRCTRL0]
+
+	ldr	r7, =0x20
+	str	r7, [r4, #DDRC_PWRCTL]
+
+	ldr	r6, =0x23
+13:
+	ldr	r7, [r4, #DDRC_STAT]
+	and	r7, r7, r6
+	cmp	r7, r6
+	bne	13b
+
+	ldr	r7, =0x03040001
+	str	r7, [r4, #DDRC_MSTR]
+
+	ldr	r7, =0x40800020
+	str	r7, [r4, #DDRC_ZQCTL0]
+
+
+	ldr	r7, =0x10210100
+	str	r7, [r5, #0x4]
+
+	ldr	r7, =0x00040046
+	str	r7, [r4, #DDRC_RFSHTMG]
+
+	/* dram root set to from dram main, div by 2 */
+	ldr	r7, =0x10000001
+	ldr	r8, =0x9880
+	str	r7, [r2, r8]
+
+	ldr	r7, =0x1010007e
+	str	r7, [r5, #DDRPHY_MDLL_CON0]
+
+	ldr	r7, =0x10000008
+	str	r7, [r5, #DDRPHY_OFFSETD_CON0]
+
+	ldr	r7, =0x08080808
+	str	r7, [r5, #DDRPHY_OFFSETR_CON0]
+	str	r7, [r5, #DDRPHY_OFFSETR_CON1]
+	ldr	r7, =0x8
+	str	r7, [r5, #DDRPHY_OFFSETR_CON2]
+
+	ldr	r7, =0x08080808
+	str	r7, [r5, #DDRPHY_OFFSETW_CON0]
+	str	r7, [r5, #DDRPHY_OFFSETW_CON1]
+	ldr	r7, =0x8
+	str	r7, [r5, #DDRPHY_OFFSETW_CON2]
+
+	ldr	r7, [r9, #ANADIG_DIGPROG]
+	and	r7, r7, #0x11
+	cmp	r7, #0x11
+	bne	22f
+
+	ldr	r7, =0x40404040
+	str	r7, [r5, #DDRPHY_CA_WLDSKEW_CON0]
+	ldr	r7, =0x18181818
+	str	r7, [r5, #DDRPHY_CA_DSKEW_CON0]
+	str	r7, [r5, #DDRPHY_CA_DSKEW_CON1]
+	ldr	r7, =0x40401818
+	str	r7, [r5, #DDRPHY_CA_DSKEW_CON2]
+	b	23f
+22:
+	ldr	r7, =0x0
+	str	r7, [r5, #DDRPHY_CA_DSKEW_CON0]
+	str	r7, [r5, #DDRPHY_CA_DSKEW_CON1]
+	str	r7, [r5, #DDRPHY_CA_DSKEW_CON2]
+23:
+	ldr	r7, =0x11000008
+	str	r7, [r5, #DDRPHY_OFFSETD_CON0]
+	ldr	r7, =0x10000008
+	str	r7, [r5, #DDRPHY_OFFSETD_CON0]
+
+	ldr	r6, =0x4
+14:
+	ldr	r7, [r5, #DDRPHY_MDLL_CON1]
+	and	r7, r7, r6
+	cmp	r7, r6
+	bne	14b
+
+	ldr	r7, =0x1
+	str	r7, [r4, #DDRC_RFSHCTL3]
+	ldr	r7, =0x3
+	str	r7, [r4, #DDRC_RFSHCTL3]
+
+	ldr	r7, =0x0
+	str	r7, [r4, #DDRC_PWRCTL]
+
+	ldr	r6, =0x1
+15:
+	ldr	r7, [r4, #DDRC_MRSTAT]
+	and	r7, r7, r6
+	cmp	r7, r6
+	beq	15b
+
+	ldr	r7, =0x10f0
+	str	r7, [r4, #DDRC_MRCTRL0]
+	ldr	r7, =0x0
+	str	r7, [r4, #DDRC_MRCTRL1]
+	ldr	r7, =0x800010f0
+	str	r7, [r4, #DDRC_MRCTRL0]
+
+	ldr	r6, =0x1
+16:
+	ldr	r7, [r4, #DDRC_MRSTAT]
+	and	r7, r7, r6
+	cmp	r7, r6
+	beq	16b
+
+	ldr	r7, =0xf0
+	str	r7, [r4, #DDRC_MRCTRL0]
+	ldr	r7, =0x930
+	str	r7, [r4, #DDRC_MRCTRL1]
+	ldr	r7, =0x800000f0
+	str	r7, [r4, #DDRC_MRCTRL0]
+
+	ldr	r7, =0x0
+	str	r7, [r4, #DDRC_RFSHCTL3]
+	ldr	r7, =0x2
+	str	r7, [r4, #DDRC_RFSHCTL3]
+
+	ldr	r6, =0x1
+17:
+	ldr	r7, [r4, #DDRC_MRSTAT]
+	and	r7, r7, r6
+	cmp	r7, r6
+	beq	17b
+
+	ldr	r7, =0xf0
+	str	r7, [r4, #DDRC_MRCTRL0]
+	ldr	r7, =0x930
+	str	r7, [r4, #DDRC_MRCTRL1]
+	ldr	r7, =0x800000f0
+	str	r7, [r4, #DDRC_MRCTRL0]
+
+	ldr	r6, =0x1
+18:
+	ldr	r7, [r4, #DDRC_MRSTAT]
+	and	r7, r7, r6
+	cmp	r7, r6
+	beq	18b
+
+	ldr	r7, =0x20f0
+	str	r7, [r4, #DDRC_MRCTRL0]
+	ldr	r7, =0x408
+	str	r7, [r4, #DDRC_MRCTRL1]
+	ldr	r7, =0x800020f0
+	str	r7, [r4, #DDRC_MRCTRL0]
+
+	ldr	r6, =0x1
+19:
+	ldr	r7, [r4, #DDRC_MRSTAT]
+	and	r7, r7, r6
+	cmp	r7, r6
+	beq	19b
+
+	ldr	r7, =0x10f0
+	str	r7, [r4, #DDRC_MRCTRL0]
+	ldr	r7, =0x4
+	str	r7, [r4, #DDRC_MRCTRL1]
+	ldr	r7, =0x800010f0
+	str	r7, [r4, #DDRC_MRCTRL0]
+
+	ldr	r7, =0x0
+	str	r7, [r4, #DDRC_DBG1]
+
+	/* enable auto self-refresh */
+	ldr	r7, [r4, #DDRC_PWRCTL]
+	orr	r7, r7, #(1 << 0)
+	str	r7, [r4, #DDRC_PWRCTL]
+
+	.endm
+
+ENTRY(imx7d_ddr3_freq_change)
+	push	{r2 - r9}
+
+	/*
+	 * 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, =0x0
+	mcr	p15, 0, r6, c8, c3, 0
+
+	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
+
+	ldr	r2, =IMX_IO_P2V(MX7D_CCM_BASE_ADDR)
+	ldr	r3, =IMX_IO_P2V(MX7D_IOMUXC_GPR_BASE_ADDR)
+	ldr	r4, =IMX_IO_P2V(MX7D_DDRC_BASE_ADDR)
+	ldr	r5, =IMX_IO_P2V(MX7D_DDRC_PHY_BASE_ADDR)
+	ldr	r9, =IMX_IO_P2V(MX7D_ANATOP_BASE_ADDR)
+
+	ldr	r6, =100000000
+	cmp	r0, r6
+	bgt	set_to_533m
+
+set_to_below_100m:
+	switch_to_below_100m
+	b	done
+
+set_to_533m:
+	switch_to_533m
+	b	done
+
+done:
+	/* 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
+
+	dsb
+	isb
+
+	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 - r9}
+	mov	pc, lr
+	.ltorg
+ENDPROC(imx7d_ddr3_freq_change)
diff --git a/arch/arm/mach-imx/ddrc.c b/arch/arm/mach-imx/ddrc.c
new file mode 100644
index 000000000000..9c7f627d465e
--- /dev/null
+++ b/arch/arm/mach-imx/ddrc.c
@@ -0,0 +1,86 @@
+/*
+ * Copyright 2015 Freescale Semiconductor, Inc.
+ *
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_device.h>
+
+#include "hardware.h"
+
+#define DDRC_MSTR		0x0
+#define	BM_DDRC_MSTR_DDR3	0x1
+#define	BM_DDRC_MSTR_LPDDR2	0x4
+#define	BM_DDRC_MSTR_LPDDR3	0x8
+
+static int ddr_type;
+
+static int imx_ddrc_probe(struct platform_device *pdev)
+{
+	struct device_node *np = pdev->dev.of_node;
+	void __iomem *ddrc_base, *reg;
+	u32 val;
+
+	ddrc_base = of_iomap(np, 0);
+	WARN_ON(!ddrc_base);
+
+	reg = ddrc_base + DDRC_MSTR;
+	/* Get ddr type */
+	val = readl_relaxed(reg);
+	val &= (BM_DDRC_MSTR_DDR3 | BM_DDRC_MSTR_LPDDR2
+		| BM_DDRC_MSTR_LPDDR3);
+
+	switch (val) {
+	case BM_DDRC_MSTR_DDR3:
+		pr_info("DDR type is DDR3!\n");
+		ddr_type = IMX_DDR_TYPE_DDR3;
+		break;
+	case BM_DDRC_MSTR_LPDDR2:
+		pr_info("DDR type is LPDDR2!\n");
+		ddr_type = IMX_DDR_TYPE_LPDDR2;
+		break;
+	case BM_DDRC_MSTR_LPDDR3:
+		pr_info("DDR type is LPDDR3!\n");
+		ddr_type = IMX_DDR_TYPE_LPDDR3;
+		break;
+	default:
+		break;
+	}
+
+	return 0;
+}
+
+int imx_ddrc_get_ddr_type(void)
+{
+	return ddr_type;
+}
+
+static struct of_device_id imx_ddrc_dt_ids[] = {
+	{ .compatible = "fsl,imx7-ddrc", },
+	{ /* sentinel */ }
+};
+
+static struct platform_driver imx_ddrc_driver = {
+	.driver		= {
+		.name	= "imx-ddrc",
+		.owner	= THIS_MODULE,
+		.of_match_table = imx_ddrc_dt_ids,
+	},
+	.probe		= imx_ddrc_probe,
+};
+
+static int __init imx_ddrc_init(void)
+{
+	return platform_driver_register(&imx_ddrc_driver);
+}
+postcore_initcall(imx_ddrc_init);
diff --git a/arch/arm/mach-imx/hardware.h b/arch/arm/mach-imx/hardware.h
index 92c5a9c9f94b..ce8e8a39f051 100644
--- a/arch/arm/mach-imx/hardware.h
+++ b/arch/arm/mach-imx/hardware.h
@@ -81,13 +81,16 @@
  *	CCM	0x020c4000+0x004000	->	0xf42c4000+0x004000
  *	ANATOP	0x020c8000+0x004000	->	0xf42c8000+0x004000
  *	UART4	0x021f0000+0x004000	->	0xf42f0000+0x004000
+ * mx7d:
+ *     CCM     0x30380000+0x010000     ->      0xf5380000+0x010000
+ *     ANATOP  0x30360000+0x010000     ->      0xf5360000+0x010000
+ *     UART1   0x30860000+0x010000     ->      0xf5860000+0x010000
  */
 #define IMX_IO_P2V(x)	(						\
-			(((x) & 0x80000000) >> 7) |			\
 			(0xf4000000 +					\
-			(((x) & 0x50000000) >> 6) +			\
-			(((x) & 0x0b000000) >> 4) +			\
-			(((x) & 0x000fffff))))
+			(((x) & 0x50000000) >> 4) +			\
+			(((x) & 0x0a000000) >> 4) +			\
+			(((x) & 0x00ffffff))))
 
 #define IMX_IO_ADDRESS(x)	IOMEM(IMX_IO_P2V(x))
 
@@ -99,6 +102,7 @@
 #include "mx2x.h"
 #include "mx21.h"
 #include "mx27.h"
+#include "mx7.h"
 
 #define imx_map_entry(soc, name, _type)	{				\
 	.virtual = soc ## _IO_P2V(soc ## _ ## name ## _BASE_ADDR),	\
diff --git a/arch/arm/mach-imx/lpddr3_freq_imx.S b/arch/arm/mach-imx/lpddr3_freq_imx.S
new file mode 100644
index 000000000000..80fb1184fa54
--- /dev/null
+++ b/arch/arm/mach-imx/lpddr3_freq_imx.S
@@ -0,0 +1,444 @@
+/*
+ * 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 "hardware.h"
+
+#define DDRC_MSTR		0x0
+#define DDRC_STAT		0x4
+#define DDRC_PWRCTL		0x30
+#define DDRC_RFSHTMG		0x64
+#define DDRC_DBG1		0x304
+#define DDRC_PSTAT		0x3fc
+#define DDRC_PCTRL_0		0x490
+#define DDRC_DFIMISC		0x1b0
+#define DDRC_DBGCAM		0x308
+#define DDRC_SWCTL		0x320
+#define DDRC_SWSTAT		0x324
+#define DDRPHY_LP_CON0		0x18
+#define IOMUXC_GPR8		0x20
+#define DDRPHY_PHY_CON1		0x4
+#define DDRPHY_MDLL_CON0	0xb0
+#define DDRPHY_MDLL_CON1	0xb4
+#define DDRPHY_OFFSETD_CON0	0x50
+#define DDRPHY_OFFSETR_CON0	0x20
+#define DDRPHY_OFFSETR_CON1	0x24
+#define DDRPHY_OFFSETR_CON2	0x28
+#define DDRPHY_OFFSETW_CON0	0x30
+#define DDRPHY_OFFSETW_CON1	0x34
+#define DDRPHY_OFFSETW_CON2	0x38
+#define DDRPHY_RFSHTMG		0x64
+#define DDRPHY_CA_WLDSKEW_CON0	0x6c
+#define DDRPHY_CA_DSKEW_CON0	0x7c
+#define DDRPHY_CA_DSKEW_CON1	0x80
+#define DDRPHY_CA_DSKEW_CON2	0x84
+
+#define ANADIG_DIGPROG		0x800
+
+	.align 3
+
+	.macro	ddrc_prepare
+
+	/* disable port */
+	ldr	r7, =0x0
+	str	r7, [r4, #DDRC_PCTRL_0]
+
+	/* wait port busy done */
+	ldr	r6, =0x10001
+1:
+	ldr	r7, [r4, #DDRC_PSTAT]
+	and	r7, r7, r6
+	cmp	r7, #0
+	bne	1b
+
+	ldr	r7, =0x20
+	str	r7, [r4, #DDRC_PWRCTL]
+
+	ldr	r6, =0x23
+2:
+	ldr	r7, [r4, #DDRC_STAT]
+	and	r7, r7, r6
+	cmp	r7, r6
+	bne	2b
+
+	ldr	r7, =0x1
+	str	r7, [r4, #DDRC_DBG1]
+
+	ldr	r6, =0x30000000
+3:
+	ldr	r7, [r4, #DDRC_DBGCAM]
+	and	r7, r7, r6
+	cmp	r7, r6
+	bne	3b
+
+	ldr	r7, =0x0
+	str	r7, [r4, #DDRC_SWCTL]
+
+	ldr	r7, =0x0
+	str	r7, [r4, #DDRC_DFIMISC]
+
+	ldr	r7, =0x1
+	str	r7, [r4, #DDRC_SWCTL]
+
+	ldr	r6, =0x1
+4:
+	ldr	r7, [r4, #DDRC_SWSTAT]
+	and	r7, r7, r6
+	cmp	r7, r6
+	bne	4b
+
+	.endm
+
+	.macro	ddrc_done
+
+	ldr	r7, =0x0
+	str	r7, [r4, #DDRC_PWRCTL]
+
+	ldr	r6, =0x3
+5:
+	ldr	r7, [r4, #DDRC_STAT]
+	and	r7, r7, r6
+	cmp	r7, r6
+	beq	5b
+
+	ldr	r7, =0x0
+	str	r7, [r4, #DDRC_DBG1]
+
+	ldr	r7, =0x1
+	str	r7, [r4, #DDRC_PCTRL_0]
+
+	/* enable auto self-refresh */
+	ldr	r7, [r4, #DDRC_PWRCTL]
+	orr	r7, r7, #(1 << 0)
+	str	r7, [r4, #DDRC_PWRCTL]
+
+	.endm
+
+	.macro	switch_to_below_100m
+
+	/* LPDDR2 and LPDDR3 has different setting */
+	ldr	r8, [r4, #DDRC_MSTR]
+	ands	r8, r8, #0x4
+	bne	9f
+
+	/* LPDDR3 */
+	ldr	r7, =0x00000100
+	str	r7, [r5, #DDRPHY_PHY_CON1]
+	b	10f
+9:
+	/* LPDDR2 */
+	ldr	r7, =0x10010100
+	str	r7, [r5, #DDRPHY_PHY_CON1]
+10:
+	ldr	r6, =24000000
+	cmp	r0, r6
+	beq	16f
+
+	ldr	r7, =0x0005000B
+	str	r7, [r4, #DDRC_RFSHTMG]
+	b	6f
+16:
+	ldr	r7, =0x00010003
+	str	r7, [r4, #DDRC_RFSHTMG]
+
+	/* dram alt sel set to OSC */
+	ldr	r7, =0x10000000
+	ldr	r8, =0xa080
+	str	r7, [r2, r8]
+	/* dram root set to from dram alt, div by 1 */
+	ldr	r7, =0x11000000
+	ldr	r8, =0x9880
+	str	r7, [r2, r8]
+	b	7f
+
+6:
+	/* dram alt sel set to pfd0_392m */
+	ldr	r7, =0x15000000
+	ldr	r8, =0xa080
+	str	r7, [r2, r8]
+	/* dram root set to from dram alt, div by 4 */
+	ldr	r7, =0x11000003
+	ldr	r8, =0x9880
+	str	r7, [r2, r8]
+7:
+	ldr	r7, =0x202ffd0
+	str	r7, [r5, #DDRPHY_MDLL_CON0]
+
+	ldr	r7, =0x7f
+	str	r7, [r5, #DDRPHY_OFFSETD_CON0]
+
+	ldr	r7, =0x7f7f7f7f
+	str	r7, [r5, #DDRPHY_OFFSETR_CON0]
+	str	r7, [r5, #DDRPHY_OFFSETR_CON1]
+	ldr	r7, =0x7f
+	str	r7, [r5, #DDRPHY_OFFSETR_CON2]
+
+	ldr	r7, =0x7f7f7f7f
+	str	r7, [r5, #DDRPHY_OFFSETW_CON0]
+	str	r7, [r5, #DDRPHY_OFFSETW_CON1]
+	ldr	r7, =0x7f
+	str	r7, [r5, #DDRPHY_OFFSETW_CON2]
+
+	ldr	r7, [r9, #ANADIG_DIGPROG]
+	and	r7, r7, #0x11
+	cmp	r7, #0x11
+	bne	11f
+
+	ldr	r7, =0x0
+	str	r7, [r5, #DDRPHY_CA_WLDSKEW_CON0]
+	ldr	r7, =0x60606060
+	str	r7, [r5, #DDRPHY_CA_DSKEW_CON0]
+	str	r7, [r5, #DDRPHY_CA_DSKEW_CON1]
+	ldr	r7, =0x00006060
+	str	r7, [r5, #DDRPHY_CA_DSKEW_CON2]
+	b	12f
+11:
+	ldr	r7, =0x0
+	str	r7, [r5, #DDRPHY_CA_DSKEW_CON0]
+	str	r7, [r5, #DDRPHY_CA_DSKEW_CON1]
+	str	r7, [r5, #DDRPHY_CA_DSKEW_CON2]
+12:
+	ldr	r7, =0x100007f
+	str	r7, [r5, #DDRPHY_OFFSETD_CON0]
+	ldr	r7, =0x7f
+	str	r7, [r5, #DDRPHY_OFFSETD_CON0]
+
+	.endm
+
+	.macro	switch_to_533m
+
+	ldr	r7, =0x10210100
+	str	r7, [r5, #DDRPHY_PHY_CON1]
+
+	ldr	r7, =0x00200038
+	str	r7, [r4, #DDRC_RFSHTMG]
+
+	/* dram root set to from dram main, div by 2 */
+	ldr	r7, =0x10000001
+	ldr	r8, =0x9880
+	str	r7, [r2, r8]
+
+	ldr	r7, =0x1010007e
+	str	r7, [r5, #DDRPHY_MDLL_CON0]
+
+	ldr	r7, =0x10000008
+	str	r7, [r5, #DDRPHY_OFFSETD_CON0]
+
+	ldr	r7, =0x08080808
+	str	r7, [r5, #DDRPHY_OFFSETR_CON0]
+	str	r7, [r5, #DDRPHY_OFFSETR_CON1]
+	ldr	r7, =0x8
+	str	r7, [r5, #DDRPHY_OFFSETR_CON2]
+
+	ldr	r7, =0x08080808
+	str	r7, [r5, #DDRPHY_OFFSETW_CON0]
+	str	r7, [r5, #DDRPHY_OFFSETW_CON1]
+	ldr	r7, =0x8
+	str	r7, [r5, #DDRPHY_OFFSETW_CON2]
+
+	/* LPDDR2 and LPDDR3 has different setting */
+	ldr	r8, [r4, #DDRC_MSTR]
+	ands	r8, r8, #0x4
+	beq	15f
+
+	ldr	r7, [r9, #ANADIG_DIGPROG]
+	and	r7, r7, #0x11
+	cmp	r7, #0x11
+	bne	14f
+
+	ldr	r7, =0x08080808
+	str	r7, [r5, #DDRPHY_CA_DSKEW_CON0]
+	str	r7, [r5, #DDRPHY_CA_DSKEW_CON1]
+	ldr	r7, =0x0a0a0808
+	str	r7, [r5, #DDRPHY_CA_DSKEW_CON2]
+	ldr	r7, =0x0a0a0a0a
+	str	r7, [r5, #DDRPHY_CA_WLDSKEW_CON0]
+	b	14f
+15:
+	ldr	r7, [r9, #ANADIG_DIGPROG]
+	and	r7, r7, #0x11
+	cmp	r7, #0x11
+	bne	13f
+
+	ldr	r7, =0x1c1c1c1c
+	str	r7, [r5, #DDRPHY_CA_DSKEW_CON0]
+	str	r7, [r5, #DDRPHY_CA_DSKEW_CON1]
+	ldr	r7, =0x30301c1c
+	str	r7, [r5, #DDRPHY_CA_DSKEW_CON2]
+	ldr	r7, =0x30303030
+	str	r7, [r5, #DDRPHY_CA_WLDSKEW_CON0]
+	b	14f
+13:
+	ldr	r7, =0x08080808
+	str	r7, [r5, #DDRPHY_CA_DSKEW_CON0]
+	str	r7, [r5, #DDRPHY_CA_DSKEW_CON1]
+	ldr	r7, =0x0808
+	str	r7, [r5, #DDRPHY_CA_DSKEW_CON2]
+14:
+	ldr	r7, =0x11000008
+	str	r7, [r5, #DDRPHY_OFFSETD_CON0]
+	ldr	r7, =0x10000008
+	str	r7, [r5, #DDRPHY_OFFSETD_CON0]
+
+	ldr	r6, =0x4
+8:
+	ldr	r7, [r5, #DDRPHY_MDLL_CON1]
+	and	r7, r7, r6
+	cmp	r7, r6
+	bne	8b
+
+	.endm
+
+ENTRY(imx_lpddr3_freq_change)
+	push	{r2 - r9}
+
+	/*
+	 * 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
+
+	ldr	r2, =IMX_IO_P2V(MX7D_CCM_BASE_ADDR)
+	ldr	r3, =IMX_IO_P2V(MX7D_IOMUXC_GPR_BASE_ADDR)
+	ldr	r4, =IMX_IO_P2V(MX7D_DDRC_BASE_ADDR)
+	ldr	r5, =IMX_IO_P2V(MX7D_DDRC_PHY_BASE_ADDR)
+	ldr	r9, =IMX_IO_P2V(MX7D_ANATOP_BASE_ADDR)
+
+	ddrc_prepare
+
+	ldr	r6, =100000000
+	cmp	r0, r6
+	bgt	set_to_533m
+
+set_to_below_100m:
+	switch_to_below_100m
+	b	done
+
+set_to_533m:
+	switch_to_533m
+	b	done
+
+done:
+	ddrc_done
+
+	/* 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 - r9}
+	mov	pc, lr
+ENDPROC(imx_lpddr3_freq_change)
diff --git a/arch/arm/mach-imx/mach-imx7d.c b/arch/arm/mach-imx/mach-imx7d.c
index ebb27592a9f7..6baead31ec29 100644
--- a/arch/arm/mach-imx/mach-imx7d.c
+++ b/arch/arm/mach-imx/mach-imx7d.c
@@ -101,6 +101,13 @@ static void __init imx7d_init_irq(void)
 	irqchip_init();
 }
 
+static void __init imx7d_map_io(void)
+{
+	debug_ll_io_init();
+	imx7_pm_map_io();
+	imx_busfreq_map_io();
+}
+
 static const char *const imx7d_dt_compat[] __initconst = {
 	"fsl,imx7d",
 	"fsl,imx7s",
@@ -108,6 +115,7 @@ static const char *const imx7d_dt_compat[] __initconst = {
 };
 
 DT_MACHINE_START(IMX7D, "Freescale i.MX7 Dual (Device Tree)")
+	.map_io         = imx7d_map_io,
 	.init_irq	= imx7d_init_irq,
 	.init_machine	= imx7d_init_machine,
 	.init_late      = imx7d_init_late,
diff --git a/arch/arm/mach-imx/mx7.h b/arch/arm/mach-imx/mx7.h
new file mode 100644
index 000000000000..afbeaef12d07
--- /dev/null
+++ b/arch/arm/mach-imx/mx7.h
@@ -0,0 +1,54 @@
+/*
+ * 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 version 2 as
+ *    * published by the Free Software Foundation.
+ *     */
+
+#ifndef __ASM_ARCH_MX7_IOMAP_H__
+#define __ASM_ARCH_MX7_IOMAP_H__
+
+#define MX7D_IO_P2V(x)                  IMX_IO_P2V(x)
+#define MX7D_IO_ADDRESS(x)              IOMEM(MX7D_IO_P2V(x))
+
+#define MX7D_LPSR_BASE_ADDR             0x30270000
+#define MX7D_LPSR_SIZE                  0x10000
+#define MX7D_CCM_BASE_ADDR              0x30380000
+#define MX7D_CCM_SIZE                   0x10000
+#define MX7D_IOMUXC_BASE_ADDR           0x30330000
+#define MX7D_IOMUXC_SIZE                0x10000
+#define MX7D_IOMUXC_GPR_BASE_ADDR       0x30340000
+#define MX7D_IOMUXC_GPR_SIZE            0x10000
+#define MX7D_ANATOP_BASE_ADDR           0x30360000
+#define MX7D_ANATOP_SIZE                0x10000
+#define MX7D_SNVS_BASE_ADDR		0x30370000
+#define MX7D_SNVS_SIZE			0x10000
+#define MX7D_GPC_BASE_ADDR              0x303a0000
+#define MX7D_GPC_SIZE                   0x10000
+#define MX7D_SRC_BASE_ADDR              0x30390000
+#define MX7D_SRC_SIZE                   0x10000
+#define MX7D_DDRC_BASE_ADDR             0x307a0000
+#define MX7D_DDRC_SIZE                  0x10000
+#define MX7D_DDRC_PHY_BASE_ADDR         0x30790000
+#define MX7D_DDRC_PHY_SIZE              0x10000
+#define MX7D_AIPS1_BASE_ADDR            0x30000000
+#define MX7D_AIPS1_SIZE                 0x400000
+#define MX7D_AIPS2_BASE_ADDR            0x30400000
+#define MX7D_AIPS2_SIZE                 0x400000
+#define MX7D_AIPS3_BASE_ADDR            0x30900000
+#define MX7D_AIPS3_SIZE                 0x300000
+#define MX7D_GIC_BASE_ADDR              0x31000000
+#define MX7D_GIC_SIZE                   0x100000
+
+#define TT_ATTRIB_NON_CACHEABLE_1M     0x802
+#define MX7_IRAM_TLB_SIZE              0x4000
+#define MX7_SUSPEND_OCRAM_SIZE         0x1000
+#define MX7_CPUIDLE_OCRAM_ADDR_OFFSET  0x1000
+#define MX7_CPUIDLE_OCRAM_SIZE         0x1000
+#define MX7_BUSFREQ_OCRAM_ADDR_OFFSET  0x2000
+#define MX7_BUSFREQ_OCRAM_SIZE         0x1000
+
+#endif
diff --git a/arch/arm/mach-imx/mxc.h b/arch/arm/mach-imx/mxc.h
index 2bfd2d59b4a6..33d560c6f0b1 100644
--- a/arch/arm/mach-imx/mxc.h
+++ b/arch/arm/mach-imx/mxc.h
@@ -33,7 +33,9 @@
 #define MXC_CPU_IMX7D		0x72
 #define MXC_CPU_IMX7ULP		0xff
 
+#define IMX_DDR_TYPE_DDR3		0
 #define IMX_DDR_TYPE_LPDDR2		1
+#define IMX_DDR_TYPE_LPDDR3		2
 
 #ifndef __ASSEMBLY__
 extern unsigned int __mxc_cpu_type;
diff --git a/arch/arm/mach-imx/pm-imx7.c b/arch/arm/mach-imx/pm-imx7.c
new file mode 100644
index 000000000000..a1e3084798f5
--- /dev/null
+++ b/arch/arm/mach-imx/pm-imx7.c
@@ -0,0 +1,148 @@
+/*
+ * Copyright (C) 2015 Freescale Semiconductor, Inc.
+ *
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/busfreq-imx.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/slab.h>
+#include <linux/of.h>
+#include <linux/of_fdt.h>
+#include <linux/of_platform.h>
+#include <asm/cacheflush.h>
+#include <asm/mach/map.h>
+#include <asm/proc-fns.h>
+#include <asm/tlb.h>
+
+#include "common.h"
+#include "hardware.h"
+
+#define M4_OCRAMS_RESERVED_SIZE	0xc
+
+extern unsigned long iram_tlb_base_addr;
+extern unsigned long iram_tlb_phys_addr;
+
+static struct map_desc imx7_pm_io_desc[] __initdata = {
+	imx_map_entry(MX7D, AIPS1, MT_DEVICE),
+	imx_map_entry(MX7D, AIPS2, MT_DEVICE),
+	imx_map_entry(MX7D, AIPS3, MT_DEVICE),
+};
+
+static const char * const low_power_ocram_match[] __initconst = {
+	"fsl,lpm-sram",
+	NULL
+};
+
+static struct map_desc iram_tlb_io_desc __initdata = {
+	/* .virtual and .pfn are run-time assigned */
+	.length     = SZ_1M,
+	.type       = MT_MEMORY_RWX_NONCACHED,
+};
+
+static int __init imx7_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 imx7_pm_map_io(void)
+{
+	unsigned long i, j;
+
+	iotable_init(imx7_pm_io_desc, ARRAY_SIZE(imx7_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(imx7_dt_find_lpsram, NULL));
+
+	/* Return if no IRAM space is allocated for suspend/resume code. */
+	if (!iram_tlb_base_addr) {
+		pr_warn("No valid ocram available for suspend/resume!\n");
+		return;
+	}
+
+	/* TODO: Handle M4 in TEE? */
+	/* Set all entries to 0 except first 3 words reserved for M4. */
+	memset((void *)(iram_tlb_base_addr + M4_OCRAMS_RESERVED_SIZE),
+		0, MX7_IRAM_TLB_SIZE - M4_OCRAMS_RESERVED_SIZE);
+
+	/*
+	 * Make sure the IRAM virtual address has a mapping in the IRAM
+	 * page table.
+	 *
+	 * Only use the top 12 bits [31-20] when storing the physical
+	 * address in the page table as only these bits are required
+	 * for 1M mapping.
+	 */
+	j = ((iram_tlb_base_addr >> 20) << 2) / 4;
+	*((unsigned long *)iram_tlb_base_addr + j) =
+		(iram_tlb_phys_addr & 0xFFF00000) | TT_ATTRIB_NON_CACHEABLE_1M;
+
+	/*
+	 * Make sure the AIPS1 virtual address has a mapping in the
+	 * IRAM page table.
+	 */
+	for (i = 0; i < 4; i++) {
+		j = ((IMX_IO_P2V(MX7D_AIPS1_BASE_ADDR + i * 0x100000) >> 20) << 2) / 4;
+		*((unsigned long *)iram_tlb_base_addr + j) =
+			((MX7D_AIPS1_BASE_ADDR + i * 0x100000) & 0xFFF00000) |
+			TT_ATTRIB_NON_CACHEABLE_1M;
+	}
+
+	/*
+	 * Make sure the AIPS2 virtual address has a mapping in the
+	 * IRAM page table.
+	 */
+	for (i = 0; i < 4; i++) {
+		j = ((IMX_IO_P2V(MX7D_AIPS2_BASE_ADDR + i * 0x100000) >> 20) << 2) / 4;
+		*((unsigned long *)iram_tlb_base_addr + j) =
+			((MX7D_AIPS2_BASE_ADDR + i * 0x100000) & 0xFFF00000) |
+			TT_ATTRIB_NON_CACHEABLE_1M;
+	}
+
+	/*
+	 * Make sure the AIPS3 virtual address has a mapping
+	 * in the IRAM page table.
+	 */
+	for (i = 0; i < 4; i++) {
+		j = ((IMX_IO_P2V(MX7D_AIPS3_BASE_ADDR + i * 0x100000) >> 20) << 2) / 4;
+		*((unsigned long *)iram_tlb_base_addr + j) =
+			((MX7D_AIPS3_BASE_ADDR + i * 0x100000) & 0xFFF00000) |
+			TT_ATTRIB_NON_CACHEABLE_1M;
+	}
+
+	/*
+	 * Make sure the GIC virtual address has a mapping in the
+	 * IRAM page table.
+	 */
+	j = ((IMX_IO_P2V(MX7D_GIC_BASE_ADDR) >> 20) << 2) / 4;
+	*((unsigned long *)iram_tlb_base_addr + j) =
+		(MX7D_GIC_BASE_ADDR & 0xFFF00000) | TT_ATTRIB_NON_CACHEABLE_1M;
+}
diff --git a/arch/arm/mach-imx/smp_wfe.S b/arch/arm/mach-imx/smp_wfe.S
new file mode 100644
index 000000000000..08894bb39c4d
--- /dev/null
+++ b/arch/arm/mach-imx/smp_wfe.S
@@ -0,0 +1,110 @@
+/*
+ * Copyright (C) 2015 Freescale Semiconductor, Inc. All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/linkage.h>
+#include <asm/smp_scu.h>
+#include "hardware.h"
+
+	.macro	disable_l1_dcache
+
+	/*
+	 * Flush all data from the L1 data cache before disabling
+	 * SCTLR.C bit.
+	 */
+	push	{r0 - r10, lr}
+	ldr	r7, =v7_flush_dcache_all
+	mov	lr, pc
+	mov	pc, r7
+	pop	{r0 - r10, lr}
+
+	/* disable d-cache */
+	mrc	p15, 0, r7, c1, c0, 0
+	bic	r7, r7, #(1 << 2)
+	mcr	p15, 0, r7, c1, c0, 0
+	dsb
+	isb
+
+	push	{r0 - r10, lr}
+	ldr	r7, =v7_flush_dcache_all
+	mov	lr, pc
+	mov	pc, r7
+	pop	{r0 - r10, lr}
+
+	.endm
+
+#ifdef CONFIG_SMP
+	.align 3
+
+ENTRY(imx7_smp_wfe)
+	push	{r4 - r11, lr}
+
+	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
+	/* Set flag of entering WFE. */
+	mov	r7, #0xff
+	lsl	r7, r7, r0
+	mov	r6, #SCU_PM_DORMANT
+	lsl	r6, r6, r0
+	ldr	r8, [r1, #0x4]
+	bic	r8, r8, r7
+	orr	r6, r6, r8
+	str	r6, [r1, #0x4]
+
+go_back_wfe:
+	wfe
+
+	/* Offset 0x0 stores busfeq done flag */
+	ldr	r6, [r1]
+	cmp	r6, #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
+
+	/* Set flag of exiting WFE. */
+	mov	r7, #0xff
+	lsl	r7, r7, r0
+	mov	r6, #SCU_PM_NORMAL
+	lsl	r6, r6, r0
+	ldr	r8, [r1, #0x4]
+	bic	r8, r8, r7
+	orr	r6, r6, r8
+	str	r6, [r1, #0x4]
+
+	/* Pop all saved registers. */
+	pop	{r4 - r11, lr}
+	mov	pc, lr
+	.ltorg
+ENDPROC(imx7_smp_wfe)
+#endif