MLK-11338-2 ARM: imx: add busfreq support for imx7d sdb board

This patch adds busfreq support for i.MX7D SDB
board with DDR3 memory, 3 setpoints supported:

HIGH: DRAM CLK = 533MHz, AXI = 332MHz, AHB = 135MHz;
AUDIO: DRAM CLK = 100MHz; AXI = 24MHz, AHB = 24MHz;
LOW: DRAM CLK = 24MHz; AXI = 24MHz, AHB = 24MHz;

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

8 files changed, 1637 insertions, 3 deletions

diff --git a/arch/arm/mach-imx/Makefile b/arch/arm/mach-imx/Makefile
index e44085605692..00dc95708503 100644
--- a/arch/arm/mach-imx/Makefile
+++ b/arch/arm/mach-imx/Makefile
@@ -91,7 +91,9 @@ obj-$(CONFIG_SOC_IMX7D) += suspend-imx7.o
 obj-$(CONFIG_SOC_IMX53) += suspend-imx53.o
 endif
 obj-$(CONFIG_SOC_IMX6) += pm-imx6.o
-obj-$(CONFIG_SOC_IMX7D) += pm-imx7.o
+AFLAGS_smp_wfe.o :=-Wa,-march=armv7-a
+AFLAGS_ddr3_freq_imx7d.o :=-Wa,-march=armv7-a
+obj-$(CONFIG_SOC_IMX7D) += pm-imx7.o busfreq-imx.o busfreq_ddr3.o ddr3_freq_imx7d.o smp_wfe.o
 
 obj-$(CONFIG_SOC_IMX1) += mach-imx1.o
 obj-$(CONFIG_SOC_IMX50) += mach-imx50.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..bd8a97554377
--- /dev/null
+++ b/arch/arm/mach-imx/busfreq-imx.c
@@ -0,0 +1,689 @@
+/*
+ * 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 <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
+
+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 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 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;
+
+extern int init_ddrc_ddr_settings(struct platform_device *dev);
+extern int update_ddr_freq_imx_smp(int ddr_rate);
+extern int update_lpddr2_freq(int ddr_rate);
+
+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 int busfreq_notify(enum busfreq_event event)
+{
+	int ret;
+
+	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)
+{
+	if (audio_bus_count) {
+		clk_prepare_enable(pfd0_392m);
+		update_ddr_freq_imx_smp(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 {
+		update_ddr_freq_imx_smp(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;
+	}
+}
+
+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);
+
+	update_ddr_freq_imx_smp(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 void reduce_bus_freq_handler(struct work_struct *work)
+{
+	mutex_lock(&bus_freq_mutex);
+
+	reduce_bus_freq();
+
+	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.
+ */
+int set_low_bus_freq(void)
+{
+	if (busfreq_suspended)
+		return 0;
+
+	if (!bus_freq_scaling_initialized || !bus_freq_scaling_is_active)
+		return 0;
+
+	/*
+	 * 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_delayed_work_sync(&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_delayed_work_sync(&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(10000));
+
+	/*
+	 * 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())
+		err = init_ddrc_ddr_settings(pdev);
+
+	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;
+
+	printk(KERN_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..5d0ea9ec7980
--- /dev/null
+++ b/arch/arm/mach-imx/busfreq_ddr3.c
@@ -0,0 +1,278 @@
+/*
+ * Copyright (C) 2011-2015 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/smp.h>
+
+#include "hardware.h"
+#include "common.h"
+
+/* DDR settings */
+static void __iomem *gic_dist_base;
+
+static int curr_ddr_rate;
+
+#define SMP_WFE_CODE_SIZE	0x400
+void (*imx7d_change_ddr_freq)(u32 freq) = NULL;
+extern void imx7d_ddr3_freq_change(u32 freq);
+
+extern unsigned int ddr_med_rate;
+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);
+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_ddr3_freq_change(u32 cpuid, u32 *ddr_freq_change_done);
+extern void imx7_smp_wfe(u32 cpuid, u32 ocram_base);
+extern unsigned long wfe_ddr3_freq_change_start
+	asm("wfe_ddr3_freq_change_start");
+extern unsigned long wfe_ddr3_freq_change_end asm("wfe_ddr3_freq_change_end");
+#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);
+
+#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);
+
+	curr_ddr_rate = ddr_normal_rate;
+
+	return 0;
+}
diff --git a/arch/arm/mach-imx/common.h b/arch/arm/mach-imx/common.h
index a790b97925be..c3f67f14bbb8 100644
--- a/arch/arm/mach-imx/common.h
+++ b/arch/arm/mach-imx/common.h
@@ -118,6 +118,7 @@ int imx_mmdc_get_ddr_type(void);
 int imx_ddrc_get_ddr_type(void);
 void imx_cpu_die(unsigned int cpu);
 int imx_cpu_kill(unsigned int cpu);
+void imx_busfreq_map_io(void);
 
 #ifdef CONFIG_SUSPEND
 void v7_cpu_resume(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..ab82047298e7
--- /dev/null
+++ b/arch/arm/mach-imx/ddr3_freq_imx7d.S
@@ -0,0 +1,532 @@
+/*
+ * 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 "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_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_DSKEW_CON0	0x7c
+#define DDRPHY_CA_DSKEW_CON1	0x80
+#define DDRPHY_CA_DSKEW_CON2	0x84
+
+	.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
+	bne	7f
+
+	/* 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, =0x0
+	str	r7, [r5, #DDRPHY_CA_DSKEW_CON0]
+	str	r7, [r5, #DDRPHY_CA_DSKEW_CON1]
+	str	r7, [r5, #DDRPHY_CA_DSKEW_CON2]
+
+	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]
+
+	.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]
+
+	/* dram root set to from dram main, div by 2 */
+	ldr	r7, =0x10000001
+	ldr	r8, =0x9880
+	str	r7, [r2, r8]
+
+	ldr	r7, =0x02020070
+	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, =0x0
+	str	r7, [r5, #DDRPHY_CA_DSKEW_CON0]
+	str	r7, [r5, #DDRPHY_CA_DSKEW_CON1]
+	str	r7, [r5, #DDRPHY_CA_DSKEW_CON2]
+
+	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]
+
+	.endm
+
+ENTRY(imx7d_ddr3_freq_change)
+	push	{r2 - r8}
+
+	/*
+	 * To ensure no page table walks occur in DDR, we
+	 * have a another page table stored in IRAM that only
+	 * contains entries pointing to IRAM, AIPS1 and AIPS2.
+	 * We need to set the TTBR1 to the new IRAM TLB.
+	 * Do the following steps:
+	 * 1. Flush the Branch Target Address Cache (BTAC)
+	 * 2. Set TTBR1 to point to IRAM page table.
+	 * 3. Disable page table walks in TTBR0 (PD0 = 1)
+	 * 4. Set TTBR0.N=1, implying 0-2G is translated by TTBR0
+	 *     and 2-4G is translated by TTBR1.
+	 */
+	ldr	r6, =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	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 - r8}
+	mov	pc, lr
+	.ltorg
+ENDPROC(imx7d_ddr3_freq_change)
diff --git a/arch/arm/mach-imx/mach-imx7d.c b/arch/arm/mach-imx/mach-imx7d.c
index c3db085df6b9..e3a33f15ad3e 100644
--- a/arch/arm/mach-imx/mach-imx7d.c
+++ b/arch/arm/mach-imx/mach-imx7d.c
@@ -115,8 +115,9 @@ static void __init imx7d_init_late(void)
 
 static void __init imx7d_map_io(void)
 {
-       debug_ll_io_init();
-       imx7_pm_map_io();
+	debug_ll_io_init();
+	imx7_pm_map_io();
+	imx_busfreq_map_io();
 }
 
 static const char *const imx7d_dt_compat[] __initconst = {
diff --git a/arch/arm/mach-imx/pm-imx6.c b/arch/arm/mach-imx/pm-imx6.c
index e287a9470bd4..e0f59360bd9e 100644
--- a/arch/arm/mach-imx/pm-imx6.c
+++ b/arch/arm/mach-imx/pm-imx6.c
@@ -255,6 +255,27 @@ struct imx6_cpu_pm_info {
 	u32 mmdc_io_val[MX6_MAX_MMDC_IO_NUM][2]; /* To save offset and value */
 } __aligned(8);
 
+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)
+	);
+}
+
 void imx6_set_int_mem_clk_lpm(bool enable)
 {
 	u32 val = readl_relaxed(ccm_base + CGPR);
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