Merge branch 'topic/lt-3.2-imx6-cpufreq' into lt-3.2-imx6

* topic/lt-3.2-imx6-cpufreq:
  arm/imx6q: select ARCH_HAS_CPUFREQ
  arm/imx6q: register arm_clk as cpu to clkdev
  dts/imx6q: add cpufreq property
  cpufreq: add clk-reg cpufreq driver
  arm/imx: cpufreq: remove loops_per_jiffy recalculate for smp
  ARM: add cpufreq transiton notifier to adjust loops_per_jiffy for smp
  arm/imx: add cpu_voltage to cpu_op
  arm/imx: cpufreq: add multi-core support

10 files changed, 428 insertions, 33 deletions

diff --git a/Documentation/devicetree/bindings/cpufreq/clk-reg-cpufreq b/Documentation/devicetree/bindings/cpufreq/clk-reg-cpufreq
new file mode 100644
index 00000000000..bf07c1b87a0
--- /dev/null
+++ b/Documentation/devicetree/bindings/cpufreq/clk-reg-cpufreq
@@ -0,0 +1,7 @@
+Generic cpufreq driver based on clk and regulator APIs
+
+Required properties in /cpus/cpu@0:
+- cpu-freqs : cpu frequency points it support, in unit of Hz.
+- cpu-volts : cpu voltages required by the frequency point at the same index,
+	      in unit of uV.
+- trans-latency :  transition_latency, in unit of ns.
diff --git a/arch/arm/boot/dts/imx6q.dtsi b/arch/arm/boot/dts/imx6q.dtsi
index 2b255ae12b8..7609c95329d 100644
--- a/arch/arm/boot/dts/imx6q.dtsi
+++ b/arch/arm/boot/dts/imx6q.dtsi
@@ -29,6 +29,13 @@
 			compatible = "arm,cortex-a9";
 			reg = <0>;
 			next-level-cache = <&L2>;
+			cpu-freqs = <996000000 792000000 396000000 198000000>;
+			cpu-volts = <	/* min		max */
+					1225000		1450000	/* 996M */
+					1100000		1450000	/* 792M */
+					950000		1450000	/* 396M */
+					850000		1450000>; /* 198M */
+			trans-latency = <61036>; /* two CLK32 periods */
 		};
 
 		cpu@1 {
diff --git a/arch/arm/kernel/smp.c b/arch/arm/kernel/smp.c
index ef5640b9e21..ac9cadc3850 100644
--- a/arch/arm/kernel/smp.c
+++ b/arch/arm/kernel/smp.c
@@ -25,6 +25,7 @@
 #include <linux/percpu.h>
 #include <linux/clockchips.h>
 #include <linux/completion.h>
+#include <linux/cpufreq.h>
 
 #include <linux/atomic.h>
 #include <asm/cacheflush.h>
@@ -631,3 +632,56 @@ int setup_profiling_timer(unsigned int multiplier)
 {
 	return -EINVAL;
 }
+
+#ifdef CONFIG_CPU_FREQ
+
+static DEFINE_PER_CPU(unsigned long, l_p_j_ref);
+static DEFINE_PER_CPU(unsigned long, l_p_j_ref_freq);
+static unsigned long global_l_p_j_ref;
+static unsigned long global_l_p_j_ref_freq;
+
+static int cpufreq_callback(struct notifier_block *nb,
+					unsigned long val, void *data)
+{
+	struct cpufreq_freqs *freq = data;
+	int cpu = freq->cpu;
+
+	if (freq->flags & CPUFREQ_CONST_LOOPS)
+		return NOTIFY_OK;
+
+	if (!per_cpu(l_p_j_ref, cpu)) {
+		per_cpu(l_p_j_ref, cpu) =
+			per_cpu(cpu_data, cpu).loops_per_jiffy;
+		per_cpu(l_p_j_ref_freq, cpu) = freq->old;
+		if (!global_l_p_j_ref) {
+			global_l_p_j_ref = loops_per_jiffy;
+			global_l_p_j_ref_freq = freq->old;
+		}
+	}
+
+	if ((val == CPUFREQ_PRECHANGE  && freq->old < freq->new) ||
+	    (val == CPUFREQ_POSTCHANGE && freq->old > freq->new) ||
+	    (val == CPUFREQ_RESUMECHANGE || val == CPUFREQ_SUSPENDCHANGE)) {
+		loops_per_jiffy = cpufreq_scale(global_l_p_j_ref,
+						global_l_p_j_ref_freq,
+						freq->new);
+		per_cpu(cpu_data, cpu).loops_per_jiffy =
+			cpufreq_scale(per_cpu(l_p_j_ref, cpu),
+					per_cpu(l_p_j_ref_freq, cpu),
+					freq->new);
+	}
+	return NOTIFY_OK;
+}
+
+static struct notifier_block cpufreq_notifier = {
+	.notifier_call  = cpufreq_callback,
+};
+
+static int __init register_cpufreq_notifier(void)
+{
+	return cpufreq_register_notifier(&cpufreq_notifier,
+						CPUFREQ_TRANSITION_NOTIFIER);
+}
+core_initcall(register_cpufreq_notifier);
+
+#endif
diff --git a/arch/arm/mach-imx/Kconfig b/arch/arm/mach-imx/Kconfig
index 67556bf68e8..9132c04b7e0 100644
--- a/arch/arm/mach-imx/Kconfig
+++ b/arch/arm/mach-imx/Kconfig
@@ -595,6 +595,7 @@ comment "i.MX6 family:"
 
 config SOC_IMX6Q
 	bool "i.MX6 Quad support"
+	select ARCH_HAS_CPUFREQ
 	select ARM_GIC
 	select CACHE_L2X0
 	select CPU_V7
diff --git a/arch/arm/mach-imx/clock-imx6q.c b/arch/arm/mach-imx/clock-imx6q.c
index c246712948c..f86afe81abb 100644
--- a/arch/arm/mach-imx/clock-imx6q.c
+++ b/arch/arm/mach-imx/clock-imx6q.c
@@ -1914,6 +1914,7 @@ static struct clk_lookup lookups[] = {
 	_REGISTER_CLOCK(NULL, "gpmi_io_clk", gpmi_io_clk),
 	_REGISTER_CLOCK(NULL, "usboh3_clk", usboh3_clk),
 	_REGISTER_CLOCK(NULL, "sata_clk", sata_clk),
+	_REGISTER_CLOCK(NULL, "cpu", arm_clk),
 };
 
 int imx6q_set_lpm(enum mxc_cpu_pwr_mode mode)
diff --git a/arch/arm/plat-mxc/cpufreq.c b/arch/arm/plat-mxc/cpufreq.c
index 73db34bf588..058b1e6aad8 100644
--- a/arch/arm/plat-mxc/cpufreq.c
+++ b/arch/arm/plat-mxc/cpufreq.c
@@ -22,6 +22,7 @@
 #include <linux/clk.h>
 #include <linux/err.h>
 #include <linux/slab.h>
+#include <asm/cpu.h>
 #include <mach/hardware.h>
 #include <mach/clock.h>
 
@@ -34,6 +35,7 @@ static int cpu_freq_khz_min;
 static int cpu_freq_khz_max;
 
 static struct clk *cpu_clk;
+static DEFINE_MUTEX(cpu_lock);
 static struct cpufreq_frequency_table *imx_freq_table;
 
 static int cpu_op_nr;
@@ -59,17 +61,11 @@ static int set_cpu_freq(int freq)
 
 static int mxc_verify_speed(struct cpufreq_policy *policy)
 {
-	if (policy->cpu != 0)
-		return -EINVAL;
-
 	return cpufreq_frequency_table_verify(policy, imx_freq_table);
 }
 
 static unsigned int mxc_get_speed(unsigned int cpu)
 {
-	if (cpu)
-		return 0;
-
 	return clk_get_rate(cpu_clk) / 1000;
 }
 
@@ -77,23 +73,39 @@ static int mxc_set_target(struct cpufreq_policy *policy,
 			  unsigned int target_freq, unsigned int relation)
 {
 	struct cpufreq_freqs freqs;
-	int freq_Hz;
+	int freq_Hz, cpu;
 	int ret = 0;
 	unsigned int index;
 
+	mutex_lock(&cpu_lock);
+
 	cpufreq_frequency_table_target(policy, imx_freq_table,
 			target_freq, relation, &index);
 	freq_Hz = imx_freq_table[index].frequency * 1000;
 
 	freqs.old = clk_get_rate(cpu_clk) / 1000;
-	freqs.new = freq_Hz / 1000;
-	freqs.cpu = 0;
+	freqs.new = clk_round_rate(cpu_clk, freq_Hz);
+	freqs.new = (freqs.new ? freqs.new : freq_Hz) / 1000;
 	freqs.flags = 0;
-	cpufreq_notify_transition(&freqs, CPUFREQ_PRECHANGE);
+
+	if (freqs.old == freqs.new) {
+		mutex_unlock(&cpu_lock);
+		return 0;
+	}
+
+	for_each_possible_cpu(cpu) {
+		freqs.cpu = cpu;
+		cpufreq_notify_transition(&freqs, CPUFREQ_PRECHANGE);
+	}
 
 	ret = set_cpu_freq(freq_Hz);
 
-	cpufreq_notify_transition(&freqs, CPUFREQ_POSTCHANGE);
+	for_each_possible_cpu(cpu) {
+		freqs.cpu = cpu;
+		cpufreq_notify_transition(&freqs, CPUFREQ_POSTCHANGE);
+	}
+
+	mutex_unlock(&cpu_lock);
 
 	return ret;
 }
@@ -105,7 +117,7 @@ static int mxc_cpufreq_init(struct cpufreq_policy *policy)
 
 	printk(KERN_INFO "i.MXC CPU frequency driver\n");
 
-	if (policy->cpu != 0)
+	if (policy->cpu >= num_possible_cpus())
 		return -EINVAL;
 
 	if (!get_cpu_op)
@@ -117,37 +129,45 @@ static int mxc_cpufreq_init(struct cpufreq_policy *policy)
 		return PTR_ERR(cpu_clk);
 	}
 
-	cpu_op_tbl = get_cpu_op(&cpu_op_nr);
+	mutex_lock(&cpu_lock);
+	if (!imx_freq_table) {
+		cpu_op_tbl = get_cpu_op(&cpu_op_nr);
 
-	cpu_freq_khz_min = cpu_op_tbl[0].cpu_rate / 1000;
-	cpu_freq_khz_max = cpu_op_tbl[0].cpu_rate / 1000;
+		cpu_freq_khz_min = cpu_op_tbl[0].cpu_rate / 1000;
+		cpu_freq_khz_max = cpu_op_tbl[0].cpu_rate / 1000;
 
-	imx_freq_table = kmalloc(
-		sizeof(struct cpufreq_frequency_table) * (cpu_op_nr + 1),
-			GFP_KERNEL);
-	if (!imx_freq_table) {
-		ret = -ENOMEM;
-		goto err1;
-	}
+		imx_freq_table = kmalloc(sizeof(struct cpufreq_frequency_table)
+					* (cpu_op_nr + 1), GFP_KERNEL);
+		if (!imx_freq_table) {
+			ret = -ENOMEM;
+			mutex_unlock(&cpu_lock);
+			goto err1;
+		}
 
-	for (i = 0; i < cpu_op_nr; i++) {
-		imx_freq_table[i].index = i;
-		imx_freq_table[i].frequency = cpu_op_tbl[i].cpu_rate / 1000;
+		for (i = 0; i < cpu_op_nr; i++) {
+			imx_freq_table[i].index = i;
+			imx_freq_table[i].frequency =
+					cpu_op_tbl[i].cpu_rate / 1000;
 
-		if ((cpu_op_tbl[i].cpu_rate / 1000) < cpu_freq_khz_min)
-			cpu_freq_khz_min = cpu_op_tbl[i].cpu_rate / 1000;
+			if ((cpu_op_tbl[i].cpu_rate / 1000) < cpu_freq_khz_min)
+				cpu_freq_khz_min =
+					cpu_op_tbl[i].cpu_rate / 1000;
 
-		if ((cpu_op_tbl[i].cpu_rate / 1000) > cpu_freq_khz_max)
-			cpu_freq_khz_max = cpu_op_tbl[i].cpu_rate / 1000;
-	}
+			if ((cpu_op_tbl[i].cpu_rate / 1000) > cpu_freq_khz_max)
+				cpu_freq_khz_max =
+					cpu_op_tbl[i].cpu_rate / 1000;
+		}
 
-	imx_freq_table[i].index = i;
-	imx_freq_table[i].frequency = CPUFREQ_TABLE_END;
+		imx_freq_table[i].index = i;
+		imx_freq_table[i].frequency = CPUFREQ_TABLE_END;
+	}
+	mutex_unlock(&cpu_lock);
 
 	policy->cur = clk_get_rate(cpu_clk) / 1000;
 	policy->min = policy->cpuinfo.min_freq = cpu_freq_khz_min;
 	policy->max = policy->cpuinfo.max_freq = cpu_freq_khz_max;
-
+	policy->shared_type = CPUFREQ_SHARED_TYPE_ANY;
+	cpumask_setall(policy->cpus);
 	/* Manual states, that PLL stabilizes in two CLK32 periods */
 	policy->cpuinfo.transition_latency = 2 * NANOSECOND / CLK32_FREQ;
 
@@ -174,7 +194,10 @@ static int mxc_cpufreq_exit(struct cpufreq_policy *policy)
 
 	set_cpu_freq(cpu_freq_khz_max * 1000);
 	clk_put(cpu_clk);
+	mutex_lock(&cpu_lock);
 	kfree(imx_freq_table);
+	imx_freq_table = NULL;
+	mutex_unlock(&cpu_lock);
 	return 0;
 }
 
diff --git a/arch/arm/plat-mxc/include/mach/mxc.h b/arch/arm/plat-mxc/include/mach/mxc.h
index a4d36d601d5..1ff7fe7e659 100644
--- a/arch/arm/plat-mxc/include/mach/mxc.h
+++ b/arch/arm/plat-mxc/include/mach/mxc.h
@@ -166,6 +166,7 @@ extern unsigned int __mxc_cpu_type;
 
 struct cpu_op {
 	u32 cpu_rate;
+	int cpu_voltage;
 };
 
 int tzic_enable_wake(int is_idle);
diff --git a/drivers/cpufreq/Kconfig b/drivers/cpufreq/Kconfig
index e24a2a1b666..95470f15e13 100644
--- a/drivers/cpufreq/Kconfig
+++ b/drivers/cpufreq/Kconfig
@@ -179,6 +179,16 @@ config CPU_FREQ_GOV_CONSERVATIVE
 
 	  If in doubt, say N.
 
+config CLK_REG_CPUFREQ_DRIVER
+	tristate "Generic cpufreq driver using clk and regulator APIs"
+	depends on HAVE_CLK && OF && REGULATOR
+	select CPU_FREQ_TABLE
+	help
+	  This adds generic CPUFreq driver based on clk and regulator APIs.
+	  It assumes all cores of the CPU share the same clock and voltage.
+
+	  If in doubt, say N.
+
 menu "x86 CPU frequency scaling drivers"
 depends on X86
 source "drivers/cpufreq/Kconfig.x86"
diff --git a/drivers/cpufreq/Makefile b/drivers/cpufreq/Makefile
index a48bc02cd76..eca670910d1 100644
--- a/drivers/cpufreq/Makefile
+++ b/drivers/cpufreq/Makefile
@@ -13,6 +13,8 @@ obj-$(CONFIG_CPU_FREQ_GOV_CONSERVATIVE)	+= cpufreq_conservative.o
 # CPUfreq cross-arch helpers
 obj-$(CONFIG_CPU_FREQ_TABLE)		+= freq_table.o
 
+obj-$(CONFIG_CLK_REG_CPUFREQ_DRIVER)	+= clk-reg-cpufreq.o
+
 ##################################################################################
 # x86 drivers.
 # Link order matters. K8 is preferred to ACPI because of firmware bugs in early
diff --git a/drivers/cpufreq/clk-reg-cpufreq.c b/drivers/cpufreq/clk-reg-cpufreq.c
new file mode 100644
index 00000000000..c30d2c5993e
--- /dev/null
+++ b/drivers/cpufreq/clk-reg-cpufreq.c
@@ -0,0 +1,289 @@
+/*
+ * Copyright (C) 2011 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
+ */
+
+#define pr_fmt(fmt)	KBUILD_MODNAME ": " fmt
+
+#include <linux/module.h>
+#include <linux/cpufreq.h>
+#include <linux/clk.h>
+#include <linux/regulator/consumer.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+#include <linux/of.h>
+
+static u32 *cpu_freqs; /* Hz */
+static u32 *cpu_volts; /* uV */
+static u32 trans_latency; /* ns */
+static int cpu_op_nr;
+static unsigned int cur_index;
+
+static struct clk *cpu_clk;
+static struct regulator *cpu_reg;
+static struct cpufreq_frequency_table *freq_table;
+
+static int set_cpu_freq(unsigned long freq, int index, int higher)
+{
+	int ret = 0;
+
+	if (higher && cpu_reg) {
+		ret = regulator_set_voltage(cpu_reg,
+				cpu_volts[index * 2], cpu_volts[index * 2 + 1]);
+		if (ret) {
+			pr_err("set cpu voltage failed!\n");
+			return ret;
+		}
+	}
+
+	ret = clk_set_rate(cpu_clk, freq);
+	if (ret) {
+		if (cpu_reg)
+			regulator_set_voltage(cpu_reg, cpu_volts[cur_index * 2],
+						cpu_volts[cur_index * 2 + 1]);
+		pr_err("cannot set CPU clock rate\n");
+		return ret;
+	}
+
+	if (!higher && cpu_reg) {
+		ret = regulator_set_voltage(cpu_reg,
+				cpu_volts[index * 2], cpu_volts[index * 2 + 1]);
+		if (ret)
+			pr_warn("set cpu voltage failed, might run on"
+				" higher voltage!\n");
+		ret = 0;
+	}
+
+	return ret;
+}
+
+static int clk_reg_verify_speed(struct cpufreq_policy *policy)
+{
+	return cpufreq_frequency_table_verify(policy, freq_table);
+}
+
+static unsigned int clk_reg_get_speed(unsigned int cpu)
+{
+	return clk_get_rate(cpu_clk) / 1000;
+}
+
+static int clk_reg_set_target(struct cpufreq_policy *policy,
+			  unsigned int target_freq, unsigned int relation)
+{
+	struct cpufreq_freqs freqs;
+	unsigned long freq_Hz;
+	int cpu;
+	int ret = 0;
+	unsigned int index;
+
+	cpufreq_frequency_table_target(policy, freq_table,
+			target_freq, relation, &index);
+	freq_Hz = clk_round_rate(cpu_clk, cpu_freqs[index]);
+	freq_Hz = freq_Hz ? freq_Hz : cpu_freqs[index];
+	freqs.old = clk_get_rate(cpu_clk) / 1000;
+	freqs.new = freq_Hz / 1000;
+	freqs.flags = 0;
+
+	if (freqs.old == freqs.new)
+		return 0;
+
+	for_each_possible_cpu(cpu) {
+		freqs.cpu = cpu;
+		cpufreq_notify_transition(&freqs, CPUFREQ_PRECHANGE);
+	}
+
+	ret = set_cpu_freq(freq_Hz, index, (freqs.new > freqs.old));
+	if (ret)
+		freqs.new = clk_get_rate(cpu_clk) / 1000;
+	else
+		cur_index = index;
+
+	for_each_possible_cpu(cpu) {
+		freqs.cpu = cpu;
+		cpufreq_notify_transition(&freqs, CPUFREQ_POSTCHANGE);
+	}
+
+	return ret;
+}
+
+static int clk_reg_cpufreq_init(struct cpufreq_policy *policy)
+{
+	int ret;
+
+	if (policy->cpu >= num_possible_cpus())
+		return -EINVAL;
+
+	policy->cur = clk_get_rate(cpu_clk) / 1000;
+	policy->shared_type = CPUFREQ_SHARED_TYPE_ANY;
+	cpumask_setall(policy->cpus);
+	policy->cpuinfo.transition_latency = trans_latency;
+
+	ret = cpufreq_frequency_table_cpuinfo(policy, freq_table);
+
+	if (ret < 0) {
+		pr_err("invalid frequency table for cpu %d\n",
+			policy->cpu);
+		return ret;
+	}
+
+	cpufreq_frequency_table_get_attr(freq_table, policy->cpu);
+	cpufreq_frequency_table_target(policy, freq_table, policy->cur,
+					CPUFREQ_RELATION_H, &cur_index);
+	return 0;
+}
+
+static int clk_reg_cpufreq_exit(struct cpufreq_policy *policy)
+{
+	cpufreq_frequency_table_put_attr(policy->cpu);
+	return 0;
+}
+
+static struct cpufreq_driver clk_reg_cpufreq_driver = {
+	.flags = CPUFREQ_STICKY,
+	.verify = clk_reg_verify_speed,
+	.target = clk_reg_set_target,
+	.get = clk_reg_get_speed,
+	.init = clk_reg_cpufreq_init,
+	.exit = clk_reg_cpufreq_exit,
+	.name = "clk-reg",
+};
+
+
+static u32 max_freq = UINT_MAX / 1000; /* kHz */
+module_param(max_freq, uint, 0);
+MODULE_PARM_DESC(max_freq, "max cpu frequency in unit of kHz");
+
+static int __devinit clk_reg_cpufreq_driver_init(void)
+{
+	struct device_node *cpu0;
+	const struct property *pp;
+	int i, ret;
+
+	cpu0 = of_find_node_by_path("/cpus/cpu@0");
+	if (!cpu0)
+		return -ENODEV;
+
+	pp = of_find_property(cpu0, "cpu-freqs", NULL);
+	if (!pp) {
+		ret = -ENODEV;
+		goto put_node;
+	}
+	cpu_op_nr = pp->length / sizeof(u32);
+	if (!cpu_op_nr) {
+		ret = -ENODEV;
+		goto put_node;
+	}
+	ret = -ENOMEM;
+	cpu_freqs = kzalloc(sizeof(*cpu_freqs) * cpu_op_nr, GFP_KERNEL);
+	if (!cpu_freqs)
+		goto put_node;
+	of_property_read_u32_array(cpu0, "cpu-freqs", cpu_freqs, cpu_op_nr);
+
+	pp = of_find_property(cpu0, "cpu-volts", NULL);
+	if (pp) {
+		if (cpu_op_nr * 2 == pp->length / sizeof(u32)) {
+			cpu_volts = kzalloc(sizeof(*cpu_volts) * cpu_op_nr * 2,
+						GFP_KERNEL);
+			if (!cpu_volts)
+				goto free_cpu_freqs;
+			of_property_read_u32_array(cpu0, "cpu-volts",
+						cpu_volts, cpu_op_nr * 2);
+		} else
+			pr_warn("invalid cpu_volts!\n");
+	}
+
+	if (of_property_read_u32(cpu0, "trans-latency", &trans_latency))
+		trans_latency = CPUFREQ_ETERNAL;
+
+	cpu_clk = clk_get(NULL, "cpu");
+	if (IS_ERR(cpu_clk)) {
+		pr_err("failed to get cpu clock\n");
+		ret = PTR_ERR(cpu_clk);
+		goto free_cpu_volts;
+	}
+
+	if (cpu_volts) {
+		cpu_reg = regulator_get(NULL, "cpu");
+		if (IS_ERR(cpu_reg)) {
+			pr_warn("regulator cpu get failed.\n");
+			cpu_reg = NULL;
+		}
+	}
+
+	freq_table = kmalloc(sizeof(struct cpufreq_frequency_table)
+				* (cpu_op_nr + 1), GFP_KERNEL);
+	if (!freq_table) {
+		ret = -ENOMEM;
+		goto reg_put;
+	}
+
+	for (i = 0; i < cpu_op_nr; i++) {
+		freq_table[i].index = i;
+		if (cpu_freqs[i] > max_freq * 1000) {
+			freq_table[i].frequency = CPUFREQ_ENTRY_INVALID;
+			continue;
+		}
+
+		if (cpu_reg) {
+			ret = regulator_is_supported_voltage(cpu_reg,
+					cpu_volts[i * 2], cpu_volts[i * 2 + 1]);
+			if (ret <= 0) {
+				freq_table[i].frequency = CPUFREQ_ENTRY_INVALID;
+				continue;
+			}
+		}
+		freq_table[i].frequency = cpu_freqs[i] / 1000;
+	}
+
+	freq_table[i].index = i;
+	freq_table[i].frequency = CPUFREQ_TABLE_END;
+
+	ret = cpufreq_register_driver(&clk_reg_cpufreq_driver);
+	if (ret)
+		goto free_freq_table;
+
+	of_node_put(cpu0);
+
+	return 0;
+
+free_freq_table:
+	kfree(freq_table);
+reg_put:
+	if (cpu_reg)
+		regulator_put(cpu_reg);
+	clk_put(cpu_clk);
+free_cpu_volts:
+	kfree(cpu_volts);
+free_cpu_freqs:
+	kfree(cpu_freqs);
+put_node:
+	of_node_put(cpu0);
+
+	return ret;
+}
+
+static void clk_reg_cpufreq_driver_exit(void)
+{
+	cpufreq_unregister_driver(&clk_reg_cpufreq_driver);
+	kfree(cpu_freqs);
+	kfree(cpu_volts);
+	clk_put(cpu_clk);
+	if (cpu_reg)
+		regulator_put(cpu_reg);
+	kfree(freq_table);
+}
+
+module_init(clk_reg_cpufreq_driver_init);
+module_exit(clk_reg_cpufreq_driver_exit);
+
+MODULE_AUTHOR("Freescale Semiconductor Inc. Richard Zhao <richard.zhao@freescale.com>");
+MODULE_DESCRIPTION("Generic CPUFreq driver based on clk and regulator APIs");
+MODULE_LICENSE("GPL");