ENGR00117758 MX28: Add cpufreq support

Add cpufreq support

Signed-off-by: Nancy Chen <Nancy.Chen@freescale.com>

7 files changed, 777 insertions, 2 deletions

diff --git a/arch/arm/Kconfig b/arch/arm/Kconfig
index 0c0e3d7339e..877ca4f769b 100644
--- a/arch/arm/Kconfig
+++ b/arch/arm/Kconfig
@@ -1269,7 +1269,7 @@ endmenu
 
 menu "CPU Power Management"
 
-if (ARCH_SA1100 || ARCH_INTEGRATOR || ARCH_OMAP || ARCH_PXA || ARCH_S3C64XX || ARCH_MXC || ARCH_STMP3XXX)
+if (ARCH_SA1100 || ARCH_INTEGRATOR || ARCH_OMAP || ARCH_PXA || ARCH_S3C64XX || ARCH_MXC || ARCH_STMP3XXX || ARCH_MX28)
 
 source "drivers/cpufreq/Kconfig"
 
diff --git a/arch/arm/configs/imx28evk_defconfig b/arch/arm/configs/imx28evk_defconfig
index 8c2becc408d..3352dd10ece 100644
--- a/arch/arm/configs/imx28evk_defconfig
+++ b/arch/arm/configs/imx28evk_defconfig
@@ -301,6 +301,21 @@ CONFIG_CMDLINE=""
 #
 # CPU Power Management
 #
+CONFIG_CPU_FREQ=y
+CONFIG_CPU_FREQ_TABLE=y
+# CONFIG_CPU_FREQ_DEBUG is not set
+CONFIG_CPU_FREQ_STAT=y
+# CONFIG_CPU_FREQ_STAT_DETAILS is not set
+# CONFIG_CPU_FREQ_DEFAULT_GOV_PERFORMANCE is not set
+# CONFIG_CPU_FREQ_DEFAULT_GOV_POWERSAVE is not set
+CONFIG_CPU_FREQ_DEFAULT_GOV_USERSPACE=y
+# CONFIG_CPU_FREQ_DEFAULT_GOV_ONDEMAND is not set
+# CONFIG_CPU_FREQ_DEFAULT_GOV_CONSERVATIVE is not set
+CONFIG_CPU_FREQ_GOV_PERFORMANCE=y
+CONFIG_CPU_FREQ_GOV_POWERSAVE=y
+CONFIG_CPU_FREQ_GOV_USERSPACE=y
+# CONFIG_CPU_FREQ_GOV_ONDEMAND is not set
+# CONFIG_CPU_FREQ_GOV_CONSERVATIVE is not set
 # CONFIG_CPU_IDLE is not set
 
 #
diff --git a/arch/arm/mach-mx28/Makefile b/arch/arm/mach-mx28/Makefile
index 6124174ecf8..33a395e164e 100644
--- a/arch/arm/mach-mx28/Makefile
+++ b/arch/arm/mach-mx28/Makefile
@@ -1,7 +1,7 @@
 #
 # Makefile for the linux kernel.
 #
-obj-y += pinctrl.o clock.o device.o serial.o power.o
+obj-y += pinctrl.o clock.o device.o serial.o power.o bus_freq.o
 
 # Board select
 obj-$(CONFIG_MACH_MX28EVK) += mx28evk.o mx28evk_pins.o
diff --git a/arch/arm/mach-mx28/bus_freq.c b/arch/arm/mach-mx28/bus_freq.c
new file mode 100644
index 00000000000..5a36ce207ee
--- /dev/null
+++ b/arch/arm/mach-mx28/bus_freq.c
@@ -0,0 +1,227 @@
+/*
+ * Copyright (C) 2010 Freescale Semiconductor, Inc. All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+
+/*!
+ * @file bus_freq.c
+ *
+ * @brief A common API for the Freescale Semiconductor i.MXC CPUfreq module.
+ *
+ * @ingroup PM
+ */
+#include <linux/types.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/delay.h>
+#include <linux/init.h>
+#include <linux/err.h>
+#include <linux/clk.h>
+#include <linux/spinlock.h>
+#include <linux/platform_device.h>
+#include <linux/notifier.h>
+#include <linux/cpufreq.h>
+
+#include <mach/hardware.h>
+#include <linux/io.h>
+#include <asm/system.h>
+#include <mach/clock.h>
+#include <mach/bus_freq.h>
+
+#include "regs-clkctrl.h"
+#include "regs-digctl.h"
+
+#define CLKCTRL_BASE_ADDR IO_ADDRESS(CLKCTRL_PHYS_ADDR)
+#define DIGCTRL_BASE_ADDR IO_ADDRESS(DIGCTL_PHYS_ADDR)
+#define BP_CLKCTRL_HBUS_ASM_ENABLE 20
+#define CLKCTRL_PLL_PWD_BIT 17
+#define CLKCTRL_PLL_BYPASS 0x1ff
+#define BF(value, field) (((value) << BP_##field) & BM_##field)
+
+struct profile profiles[] = {
+	{ 454740, 151580, 196360, 0, 1550000,
+	1450000, 355000, 3300000, 1750000, 0 },
+	{ 392730, 130910, 160000, 0, 1475000,
+	1375000, 225000, 3300000, 1750000, 0 },
+	{ 360000, 120000, 120000, 0, 1375000,
+	1275000, 200000, 3300000, 1750000, 0 },
+	{ 261820, 130910, 130910, 0, 1275000,
+	1175000, 173000, 3300000, 1750000, 0 },
+	{  64000,  64000,  96000, 3, 1050000,
+	975000, 150000, 3300000, 1750000, 0 },
+	{  0,  0,  0, 0, 0,
+	0, 0, 0, 0, 0 },
+};
+
+static struct device *busfreq_dev;
+static struct clk *usb_clk0;
+static struct clk *usb_clk1;
+static struct clk *lcdif_clk;
+const char *ahb_clk_id = "h";
+u32 clkseq_setting;
+
+int low_freq_used(void)
+{
+	if ((clk_get_usecount(usb_clk0) == 0)
+	    && (clk_get_usecount(usb_clk1) == 0)
+	    && (clk_get_usecount(lcdif_clk) == 0))
+		return 1;
+	else
+		return 0;
+}
+
+void hbus_auto_slow_mode_enable(void)
+{
+	__raw_writel(BP_CLKCTRL_HBUS_ASM_ENABLE,
+			CLKCTRL_BASE_ADDR + HW_CLKCTRL_HBUS_SET);
+}
+EXPORT_SYMBOL(hbus_auto_slow_mode_enable);
+
+void hbus_auto_slow_mode_disable(void)
+{
+	__raw_writel(BP_CLKCTRL_HBUS_ASM_ENABLE,
+			  CLKCTRL_BASE_ADDR + HW_CLKCTRL_HBUS_CLR);
+}
+EXPORT_SYMBOL(hbus_auto_slow_mode_disable);
+
+int cpu_clk_set_pll_on(struct clk *clk, unsigned int freq)
+{
+	struct cpufreq_freqs freqs;
+
+	freqs.old = clk_get_rate(clk);
+	freqs.cpu = 0;
+	freqs.new = freq;
+
+	if (freqs.old == 24000 && freqs.new > 24000) {
+		/* turn pll on */
+		__raw_writel(CLKCTRL_PLL_PWD_BIT, CLKCTRL_BASE_ADDR +
+			      HW_CLKCTRL_PLL0CTRL0_SET);
+		udelay(10);
+	} else if (freqs.old > 24000 && freqs.new == 24000)
+		clkseq_setting = __raw_readl(CLKCTRL_BASE_ADDR +
+						 HW_CLKCTRL_CLKSEQ);
+	return 0;
+}
+
+int cpu_clk_set_pll_off(struct clk *clk, unsigned int freq)
+{
+	struct cpufreq_freqs freqs;
+
+	freqs.old = clk_get_rate(clk);
+	freqs.cpu = 0;
+	freqs.new = freq;
+
+	if (freqs.old > 24000 && freqs.new == 24000) {
+		/* turn pll off */
+		__raw_writel(CLKCTRL_PLL_PWD_BIT, CLKCTRL_BASE_ADDR +
+			      HW_CLKCTRL_PLL0CTRL0_CLR);
+		__raw_writel(CLKCTRL_PLL_BYPASS, CLKCTRL_BASE_ADDR +
+			      HW_CLKCTRL_CLKSEQ);
+	} else if (freqs.old == 24000 && freqs.new > 24000)
+		__raw_writel(clkseq_setting, CLKCTRL_BASE_ADDR +
+				HW_CLKCTRL_CLKSEQ);
+
+	return 0;
+}
+
+int timing_ctrl_rams(int ss)
+{
+	__raw_writel(BF(ss, DIGCTL_ARMCACHE_VALID_SS) |
+				      BF(ss, DIGCTL_ARMCACHE_DRTY_SS) |
+				      BF(ss, DIGCTL_ARMCACHE_CACHE_SS) |
+				      BF(ss, DIGCTL_ARMCACHE_DTAG_SS) |
+				      BF(ss, DIGCTL_ARMCACHE_ITAG_SS),
+				      DIGCTRL_BASE_ADDR + HW_DIGCTL_ARMCACHE);
+	return 0;
+}
+
+/*!
+ * 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 __devinit busfreq_probe(struct platform_device *pdev)
+{
+	int ret = 0;
+
+	busfreq_dev = &pdev->dev;
+
+	usb_clk0 = clk_get(NULL, "usb_clk0");
+	if (IS_ERR(usb_clk0)) {
+		ret = PTR_ERR(usb_clk0);
+		goto out_usb0;
+	}
+
+	usb_clk1 = clk_get(NULL, "usb_clk1");
+	if (IS_ERR(usb_clk1)) {
+		ret = PTR_ERR(usb_clk1);
+		goto out_usb1;
+	}
+
+	lcdif_clk = clk_get(NULL, "dis_lcdif");
+	if (IS_ERR(lcdif_clk)) {
+		ret = PTR_ERR(lcdif_clk);
+		goto out_lcd;
+	}
+	return 0;
+
+out_lcd:
+	clk_put(usb_clk1);
+out_usb1:
+	clk_put(usb_clk0);
+out_usb0:
+	return ret;
+}
+
+static struct platform_driver busfreq_driver = {
+	.driver = {
+		   .name = "busfreq",
+		},
+	.probe = busfreq_probe,
+};
+
+/*!
+ * Initialise the busfreq_driver.
+ *
+ * @return  The function always returns 0.
+ */
+
+static int __init busfreq_init(void)
+{
+	if (platform_driver_register(&busfreq_driver) != 0) {
+		printk(KERN_ERR "busfreq_driver register failed\n");
+		return -ENODEV;
+	}
+
+	printk(KERN_INFO "Bus freq driver module loaded\n");
+	return 0;
+}
+
+static void __exit busfreq_cleanup(void)
+{
+	/* Unregister the device structure */
+	platform_driver_unregister(&busfreq_driver);
+}
+
+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/plat-mxs/Makefile b/arch/arm/plat-mxs/Makefile
index 69b065ea1ab..105e410dfa5 100644
--- a/arch/arm/plat-mxs/Makefile
+++ b/arch/arm/plat-mxs/Makefile
@@ -16,6 +16,7 @@ obj-$(CONFIG_ARCH_MXS) += usb_common.o
 
 obj-$(CONFIG_MXS_LRADC) += lradc.o
 # Power Management
+obj-$(CONFIG_CPU_FREQ) += cpufreq.o
 
 # charging/current limitation testing
 
diff --git a/arch/arm/plat-mxs/cpufreq.c b/arch/arm/plat-mxs/cpufreq.c
new file mode 100644
index 00000000000..8cb0ff58126
--- /dev/null
+++ b/arch/arm/plat-mxs/cpufreq.c
@@ -0,0 +1,487 @@
+/*
+ * Copyright (C) 2008-2010 Freescale Semiconductor, Inc.
+ * Copyright 2008 Embedded Alley Solutions, Inc All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+#include <linux/types.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/cpufreq.h>
+#include <linux/delay.h>
+#include <linux/init.h>
+#include <linux/err.h>
+#include <linux/clk.h>
+#include <linux/spinlock.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/consumer.h>
+#include <linux/notifier.h>
+
+#include <mach/hardware.h>
+#include <linux/io.h>
+#include <asm/system.h>
+#include <mach/regulator.h>
+#include <mach/power.h>
+#include <mach/clock.h>
+#include <mach/bus_freq.h>
+
+static struct regulator *cpu_regulator;
+static struct clk *cpu_clk;
+static struct clk *ahb_clk;
+static struct clk *emi_clk;
+static struct regulator *vddd;
+static struct regulator *vdddbo;
+static struct regulator *vddio;
+static struct regulator *vdda;
+static struct cpufreq_frequency_table imx_freq_table[7];
+int cpu_freq_khz_min;
+int cpu_freq_khz_max;
+int cpufreq_trig_needed;
+int cur_freq_table_size;
+int lcd_on_freq_table_size;
+int lcd_off_freq_table_size;
+
+extern char *ahb_clk_id;
+extern struct profile profiles[OPERATION_WP_SUPPORTED];
+extern int low_freq_used(void);
+
+static int set_freq_table(struct cpufreq_policy *policy, int end_index)
+{
+	int ret = 0;
+	int i;
+
+	cpu_freq_khz_min = profiles[0].cpu;
+	cpu_freq_khz_max = profiles[0].cpu;
+	for (i = 0; i < end_index; i++) {
+		imx_freq_table[end_index - 1 - i].index = end_index  - i;
+		imx_freq_table[end_index - 1 - i].frequency =
+						profiles[i].cpu;
+
+		if ((profiles[i].cpu) < cpu_freq_khz_min)
+			cpu_freq_khz_min = profiles[i].cpu;
+
+		if ((profiles[i].cpu) > cpu_freq_khz_max)
+			cpu_freq_khz_max = profiles[i].cpu;
+	}
+
+	imx_freq_table[i].index = 0;
+	imx_freq_table[i].frequency = CPUFREQ_TABLE_END;
+
+	policy->cur = clk_get_rate(cpu_clk) / 1000;
+	policy->governor = CPUFREQ_DEFAULT_GOVERNOR;
+	policy->min = policy->cpuinfo.min_freq = cpu_freq_khz_min;
+	policy->max = policy->cpuinfo.max_freq = cpu_freq_khz_max;
+
+	/* Manual states, that PLL stabilizes in two CLK32 periods */
+	policy->cpuinfo.transition_latency = 1000;
+
+	ret = cpufreq_frequency_table_cpuinfo(policy, imx_freq_table);
+
+	if (ret < 0) {
+		printk(KERN_ERR "%s: failed to register i.MXC CPUfreq\n",
+		       __func__);
+		return ret;
+	}
+
+	cpufreq_frequency_table_get_attr(imx_freq_table, policy->cpu);
+
+	return ret;
+}
+
+static int set_op(unsigned int target_freq)
+{
+	struct cpufreq_freqs freqs;
+	int ret = 0, i;
+
+	freqs.old = clk_get_rate(cpu_clk) / 1000;
+	freqs.cpu = 0;
+
+/* work around usb problem when in updater firmare  mode*/
+#ifdef CONFIG_MXS_UTP
+	return 0;
+#endif
+	for (i = cur_freq_table_size - 1; i > 0; i--) {
+		if (profiles[i].cpu <= target_freq &&
+		    target_freq < profiles[i - 1].cpu) {
+			freqs.new = profiles[i].cpu;
+			break;
+		}
+
+		if (!vddd && profiles[i].cpu > freqs.old) {
+			/* can't safely set more than now */
+			freqs.new = profiles[i + 1].cpu;
+			break;
+		}
+	}
+
+	if (i == 0)
+		freqs.new = profiles[i].cpu;
+
+	if (freqs.old == freqs.new) {
+		if (regulator_get_voltage(vddd) == profiles[i].vddd)
+			return 0;
+	}
+
+	cpu_clk_set_pll_on(cpu_clk, freqs.new);
+
+	if (cpu_regulator && (freqs.old < freqs.new)) {
+		ret = regulator_set_current_limit(cpu_regulator,
+			profiles[i].cur, profiles[i].cur);
+		if (ret)
+			return ret;
+	}
+
+	cpufreq_notify_transition(&freqs, CPUFREQ_PRECHANGE);
+	if (freqs.old > freqs.new) {
+		int ss = profiles[i].ss;
+		clk_set_rate(cpu_clk, (profiles[i].cpu) * 1000);
+		clk_set_rate(ahb_clk, (profiles[i].ahb) * 1000);
+		clk_set_rate(emi_clk, (profiles[i].emi) * 1000);
+		timing_ctrl_rams(ss);
+		if (vddd && vdddbo && vddio && vdda) {
+			ret = regulator_set_voltage(vddd,
+						    profiles[i].vddd,
+						    profiles[i].vddd);
+			if (ret)
+				ret = regulator_set_voltage(vddd,
+							    profiles[i].vddd,
+							    profiles[i].vddd);
+			regulator_set_voltage(vdddbo,
+					      profiles[i].vddd_bo,
+					      profiles[i].vddd_bo);
+
+			ret = regulator_set_voltage(vddio,
+						    profiles[i].vddio,
+						    profiles[i].vddio);
+			if (ret)
+				ret = regulator_set_voltage(vddio,
+							    profiles[i].vddio,
+							    profiles[i].vddio);
+			ret = regulator_set_voltage(vdda,
+						    profiles[i].vdda,
+						    profiles[i].vdda);
+			if (ret)
+				ret = regulator_set_voltage(vdda,
+							    profiles[i].vdda,
+							    profiles[i].vdda);
+		}
+	} else {
+		int ss = profiles[i].ss;
+		if (vddd && vdddbo && vddio && vdda) {
+			ret = regulator_set_voltage(vddd,
+						    profiles[i].vddd,
+						    profiles[i].vddd);
+			if (ret)
+				ret = regulator_set_voltage(vddd,
+							    profiles[i].vddd,
+							    profiles[i].vddd);
+			regulator_set_voltage(vdddbo,
+					      profiles[i].vddd_bo,
+					      profiles[i].vddd_bo);
+			ret = regulator_set_voltage(vddio,
+						    profiles[i].vddio,
+						    profiles[i].vddio);
+			if (ret)
+				ret = regulator_set_voltage(vddio,
+							    profiles[i].vddio,
+							    profiles[i].vddio);
+			ret = regulator_set_voltage(vdda,
+						    profiles[i].vdda,
+						    profiles[i].vdda);
+			if (ret)
+				ret = regulator_set_voltage(vdda,
+							    profiles[i].vdda,
+							    profiles[i].vdda);
+		}
+		timing_ctrl_rams(ss);
+		clk_set_rate(cpu_clk, (profiles[i].cpu) * 1000);
+		clk_set_rate(ahb_clk, (profiles[i].ahb) * 1000);
+		clk_set_rate(emi_clk, (profiles[i].emi) * 1000);
+	}
+	udelay(100);
+
+	cpu_clk_set_pll_off(cpu_clk, freqs.new);
+
+	cpufreq_notify_transition(&freqs, CPUFREQ_POSTCHANGE);
+
+	if (cpu_regulator && (freqs.old > freqs.new))   /* will not fail */
+		regulator_set_current_limit(cpu_regulator,
+					    profiles[i].cur,
+					    profiles[i].cur);
+
+	return ret;
+}
+
+static int calc_frequency_khz(int target, unsigned int relation)
+{
+	int i;
+
+	if (target * 1000 == clk_get_rate(cpu_clk))
+		return target;
+
+	if (relation == CPUFREQ_RELATION_H) {
+		for (i = cur_freq_table_size - 1; i >= 0; i--) {
+			if (imx_freq_table[i].frequency <= target)
+				return imx_freq_table[i].frequency;
+		}
+	} else if (relation == CPUFREQ_RELATION_L) {
+		for (i = 0; i < cur_freq_table_size; i++) {
+			if (imx_freq_table[i].frequency >= target)
+				return imx_freq_table[i].frequency;
+		}
+}
+
+	printk(KERN_ERR "Error: No valid cpufreq relation\n");
+	return cpu_freq_khz_max;
+}
+
+static int mxs_target(struct cpufreq_policy *policy,
+			  unsigned int target_freq,
+			  unsigned int relation)
+{
+	int freq_KHz;
+	struct cpufreq_freqs freqs;
+	int low_freq_bus_ready = 0;
+
+	if (cpufreq_trig_needed  == 1) {
+		/* Set the current working point. */
+		cpufreq_trig_needed = 0;
+		target_freq = clk_get_rate(cpu_clk) / 1000;
+		freq_KHz = calc_frequency_khz(target_freq, relation);
+
+		freqs.old = target_freq;
+		freqs.new = freq_KHz;
+		freqs.cpu = 0;
+		freqs.flags = 0;
+
+		low_freq_bus_ready = low_freq_used();
+		if (low_freq_bus_ready) {
+			cur_freq_table_size = lcd_off_freq_table_size;
+			hbus_auto_slow_mode_enable();
+		} else {
+			cur_freq_table_size = lcd_on_freq_table_size;
+			hbus_auto_slow_mode_disable();
+		}
+
+		set_freq_table(policy, cur_freq_table_size);
+
+		cpufreq_notify_transition(&freqs, CPUFREQ_POSTCHANGE);
+
+		return 0;
+}
+
+	/*
+	 * Some governors do not respects CPU and policy lower limits
+	 * which leads to bad things (division by zero etc), ensure
+	 * that such things do not happen.
+	 */
+	if (target_freq < policy->cpuinfo.min_freq)
+		target_freq = policy->cpuinfo.min_freq;
+
+	if (target_freq < policy->min)
+		target_freq = policy->min;
+
+	freq_KHz = calc_frequency_khz(target_freq, relation);
+	return set_op(freq_KHz);
+	}
+
+static unsigned int mxs_getspeed(unsigned int cpu)
+{
+	struct cpufreq_freqs freqs;
+	int freq_KHz;
+	unsigned int target_freq;
+
+	if (cpu)
+		return 0;
+
+	if (cpufreq_trig_needed  == 1) {
+		target_freq = clk_get_rate(cpu_clk) / 1000;
+		freq_KHz = calc_frequency_khz(target_freq, CPUFREQ_RELATION_L);
+
+		freqs.old = target_freq;
+		freqs.new = freq_KHz;
+		freqs.cpu = 0;
+		freqs.flags = 0;
+
+		cpufreq_notify_transition(&freqs, CPUFREQ_PRECHANGE);
+	}
+
+	return clk_get_rate(cpu_clk) / 1000;
+}
+
+
+static int mxs_verify_speed(struct cpufreq_policy *policy)
+{
+	if (policy->cpu != 0)
+		return -EINVAL;
+
+	return cpufreq_frequency_table_verify(policy, imx_freq_table);
+}
+
+static int __init mxs_cpu_init(struct cpufreq_policy *policy)
+{
+	int ret = 0;
+	int i;
+
+	cpu_clk = clk_get(NULL, "cpu");
+	if (IS_ERR(cpu_clk)) {
+		ret = PTR_ERR(cpu_clk);
+		goto out_cpu;
+	}
+
+	ahb_clk = clk_get(NULL, ahb_clk_id);
+	if (IS_ERR(ahb_clk)) {
+		ret = PTR_ERR(ahb_clk);
+		goto out_ahb;
+	}
+
+	emi_clk = clk_get(NULL, "emi");
+	if (IS_ERR(emi_clk)) {
+		ret = PTR_ERR(emi_clk);
+		goto out_emi;
+	}
+
+	if (policy->cpu != 0)
+		return -EINVAL;
+
+	cpu_regulator = regulator_get(NULL, "cpufreq-1");
+	if (IS_ERR(cpu_regulator)) {
+		printk(KERN_ERR "%s: failed to get CPU regulator\n", __func__);
+		cpu_regulator = NULL;
+		ret = PTR_ERR(cpu_regulator);
+		goto out_cur;
+	}
+
+	vddd = regulator_get(NULL, "vddd");
+	if (IS_ERR(vddd)) {
+		printk(KERN_ERR "%s: failed to get vddd regulator\n", __func__);
+		vddd = NULL;
+		ret = PTR_ERR(vddd);
+		goto out_cur;
+	}
+
+	vdddbo = regulator_get(NULL, "vddd_bo");
+	if (IS_ERR(vdddbo)) {
+		vdddbo = NULL;
+		pr_warning("unable to get vdddbo");
+		ret = PTR_ERR(vdddbo);
+		goto out_cur;
+	}
+
+	vddio = regulator_get(NULL, "vddio");
+	if (IS_ERR(vddio)) {
+		vddio = NULL;
+		pr_warning("unable to get vddio");
+		ret = PTR_ERR(vddio);
+		goto out_cur;
+	}
+
+	vdda = regulator_get(NULL, "vdda");
+	if (IS_ERR(vdda)) {
+		vdda = NULL;
+		pr_warning("unable to get vdda");
+		ret = PTR_ERR(vdda);
+		goto out_cur;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(profiles); i++) {
+		if ((profiles[i].cpu) == LCD_ON_CPU_FREQ_KHZ) {
+			lcd_on_freq_table_size = i + 1;
+			break;
+		}
+	}
+
+	if (i == ARRAY_SIZE(profiles)) {
+		pr_warning("unable to find frequency for LCD on");
+		printk(KERN_ERR "lcd_on_freq_table_size=%d\n",
+			lcd_on_freq_table_size);
+		goto out_cur;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(profiles); i++) {
+		if ((profiles[i].cpu) == 0) {
+			lcd_off_freq_table_size = i + 1;
+			break;
+		}
+	}
+
+	if (i == ARRAY_SIZE(profiles))
+		lcd_off_freq_table_size = i + 1;
+
+	/* Set the current working point. */
+	set_freq_table(policy, lcd_on_freq_table_size);
+	cpufreq_trig_needed = 0;
+	cur_freq_table_size = lcd_on_freq_table_size;
+
+	printk(KERN_INFO "%s: cpufreq init finished\n", __func__);
+	return 0;
+out_cur:
+	if (cpu_regulator)
+		regulator_put(cpu_regulator);
+	if (vddd)
+		regulator_put(vddd);
+	if (vddio)
+		regulator_put(vddio);
+	if (vdda)
+		regulator_put(vdda);
+
+	clk_put(emi_clk);
+out_emi:
+	clk_put(ahb_clk);
+out_ahb:
+	clk_put(cpu_clk);
+out_cpu:
+	return ret;
+}
+
+static int mxs_cpu_exit(struct cpufreq_policy *policy)
+{
+	cpufreq_frequency_table_put_attr(policy->cpu);
+
+	/* Reset CPU to 392MHz */
+	set_op(profiles[1].cpu);
+
+	clk_put(cpu_clk);
+	regulator_put(cpu_regulator);
+	return 0;
+}
+
+static struct cpufreq_driver mxs_driver = {
+	.flags		= CPUFREQ_STICKY,
+	.verify		= mxs_verify_speed,
+	.target		= mxs_target,
+	.get		= mxs_getspeed,
+	.init		= mxs_cpu_init,
+	.exit		= mxs_cpu_exit,
+	.name		= "mxs",
+};
+
+static int __devinit mxs_cpufreq_init(void)
+{
+	return cpufreq_register_driver(&mxs_driver);
+}
+
+static void mxs_cpufreq_exit(void)
+{
+	cpufreq_unregister_driver(&mxs_driver);
+}
+
+module_init(mxs_cpufreq_init);
+module_exit(mxs_cpufreq_exit);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("CPUfreq driver for i.MX");
+MODULE_LICENSE("GPL");
+
diff --git a/arch/arm/plat-mxs/include/mach/bus_freq.h b/arch/arm/plat-mxs/include/mach/bus_freq.h
new file mode 100644
index 00000000000..0d3406671d8
--- /dev/null
+++ b/arch/arm/plat-mxs/include/mach/bus_freq.h
@@ -0,0 +1,45 @@
+/*
+ * Copyright (C) 2010 Freescale Semiconductor, Inc. All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+
+#ifndef BUS_FREQ_H__
+#define BUS_FREQ_H__
+
+#define VERY_HI_RATE		2000000000
+#define LCD_ON_CPU_FREQ_KHZ 261820
+#define OPERATION_WP_SUPPORTED	6
+
+struct profile {
+	int cpu;
+	int ahb;
+	int emi;
+	int ss;
+	int vddd;
+	int vddd_bo;
+	int cur;
+	int vddio;
+	int vdda;
+	int pll_off;
+};
+
+void hbus_auto_slow_mode_enable(void);
+void hbus_auto_slow_mode_disable(void);
+extern int cpu_clk_set_pll_on(struct clk *clk, unsigned int freq);
+extern int cpu_clk_set_pll_off(struct clk *clk, unsigned int freq);
+extern int timing_ctrl_rams(int ss);
+
+#endif