S3C64XX: Initial support for CPU frequency scaling

This patch provides initial support for CPU frequency scaling on the
Samsung S3C64XX series processors. Currently only S3C6410 processors
are supported, though addition of another data table with supported
clock rates should be sufficient to enable support for further CPUs.

Signed-off-by: Mark Brown <broonie@opensource.wolfsonmicro.com>

3 files changed, 160 insertions, 1 deletions

diff --git a/arch/arm/Kconfig b/arch/arm/Kconfig
index 27d4dffc327..2dfd1c28ae7 100644
--- a/arch/arm/Kconfig
+++ b/arch/arm/Kconfig
@@ -1086,7 +1086,8 @@ endmenu
 
 menu "CPU Power Management"
 
-if (ARCH_SA1100 || ARCH_INTEGRATOR || ARCH_OMAP || ARCH_IMX || ARCH_PXA)
+if (ARCH_SA1100 || ARCH_INTEGRATOR || ARCH_OMAP || ARCH_IMX || ARCH_PXA || \
+    ARCH_S3C64XX)
 
 source "drivers/cpufreq/Kconfig"
 
@@ -1126,6 +1127,10 @@ config CPU_FREQ_PXA
 	default y
 	select CPU_FREQ_DEFAULT_GOV_USERSPACE
 
+config CPU_FREQ_S3C64XX
+	bool "CPUfreq support for S3C64xx CPUs"
+	depends on CPU_FREQ && CPU_S3C6410
+
 endif
 
 source "drivers/cpuidle/Kconfig"
diff --git a/arch/arm/plat-s3c64xx/Makefile b/arch/arm/plat-s3c64xx/Makefile
index 4ecefb68252..0d5a9cdf3bb 100644
--- a/arch/arm/plat-s3c64xx/Makefile
+++ b/arch/arm/plat-s3c64xx/Makefile
@@ -34,6 +34,8 @@ obj-$(CONFIG_PM)		+= pm.o
 obj-$(CONFIG_PM)		+= sleep.o
 obj-$(CONFIG_PM)		+= irq-pm.o
 
+obj-$(CONFIG_CPU_FREQ_S3C64XX)	+= cpufreq.o
+
 # Device setup
 
 obj-$(CONFIG_S3C64XX_SETUP_I2C0) += setup-i2c0.o
diff --git a/arch/arm/plat-s3c64xx/cpufreq.c b/arch/arm/plat-s3c64xx/cpufreq.c
new file mode 100644
index 00000000000..4179cab3dfb
--- /dev/null
+++ b/arch/arm/plat-s3c64xx/cpufreq.c
@@ -0,0 +1,152 @@
+/* linux/arch/arm/plat-s3c64xx/cpufreq.c
+ *
+ * Copyright 2009 Wolfson Microelectronics plc
+ *
+ * S3C64XX CPUfreq Support
+ *
+ * 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.
+ */
+
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/init.h>
+#include <linux/cpufreq.h>
+#include <linux/clk.h>
+#include <linux/err.h>
+
+#include <mach/cpu.h>
+
+static struct clk *armclk;
+
+static struct cpufreq_frequency_table s3c6410_freq_table[] = {
+	{ 0,  66000 },
+	{ 1, 133000 },
+	{ 2, 222000 },
+	{ 3, 266000 },
+	{ 4, 333000 },
+	{ 5, 400000 },
+	{ 6, 532000 },
+	{ 7, 533000 },
+	{ 8, 667000 },
+	{ 0, CPUFREQ_TABLE_END },
+};
+
+/* Data tables for current CPU and maximum index into it */
+static struct cpufreq_frequency_table *s3c64xx_freq_table;
+
+static int s3c64xx_cpufreq_verify_speed(struct cpufreq_policy *policy)
+{
+	if (policy->cpu != 0)
+		return -EINVAL;
+
+	return cpufreq_frequency_table_verify(policy, s3c64xx_freq_table);
+}
+
+static unsigned int s3c64xx_cpufreq_get_speed(unsigned int cpu)
+{
+	if (cpu != 0)
+		return 0;
+
+	return clk_get_rate(armclk) / 1000;
+}
+
+static int s3c64xx_cpufreq_set_target(struct cpufreq_policy *policy,
+				      unsigned int target_freq,
+				      unsigned int relation)
+{
+	int ret;
+	unsigned int index;
+	struct cpufreq_freqs freqs;
+
+	ret = cpufreq_frequency_table_target(policy, s3c64xx_freq_table,
+					     target_freq, relation, &index);
+	if (ret != 0)
+		return ret;
+
+	freqs.cpu = 0;
+	freqs.old = clk_get_rate(armclk) / 1000;
+	freqs.new = s3c64xx_freq_table[index].frequency;
+	freqs.flags = 0;
+
+	if (freqs.old == freqs.new)
+		return 0;
+
+	pr_debug("cpufreq: Transition %d-%dkHz\n", freqs.old, freqs.new);
+
+	cpufreq_notify_transition(&freqs, CPUFREQ_PRECHANGE);
+
+	ret = clk_set_rate(armclk, freqs.new * 1000);
+
+	cpufreq_notify_transition(&freqs, CPUFREQ_POSTCHANGE);
+
+	if (ret < 0) {
+		pr_err("cpufreq: Failed to set rate %dkHz: %d\n",
+		       freqs.new, ret);
+		return ret;
+	}
+
+	pr_debug("cpufreq: Set actual frequency %lukHz\n",
+		 clk_get_rate(armclk) / 1000);
+
+	return 0;
+}
+
+static int __init s3c64xx_cpufreq_driver_init(struct cpufreq_policy *policy)
+{
+	struct cpufreq_frequency_table *freq;;
+
+	if (policy->cpu != 0)
+		return -EINVAL;
+
+	if (cpu_is_s3c6410())
+		s3c64xx_freq_table = s3c6410_freq_table;
+
+	if (s3c64xx_freq_table == NULL) {
+		pr_err("cpufreq: No frequency information for this CPU\n");
+		return -ENODEV;
+	}
+
+	armclk = clk_get(NULL, "armclk");
+	if (IS_ERR(armclk)) {
+		pr_err("cpufreq: Unable to obtain ARMCLK: %ld\n",
+		       PTR_ERR(armclk));
+		return PTR_ERR(armclk);
+	}
+
+	/* Check for frequencies we can generate */
+	freq = s3c64xx_freq_table;
+	while (freq->frequency != CPUFREQ_TABLE_END) {
+		unsigned long r;
+
+		r = clk_round_rate(armclk, freq->frequency * 1000);
+		r /= 1000;
+
+		if (r != freq->frequency)
+			freq->frequency = CPUFREQ_ENTRY_INVALID;
+
+		freq++;
+	}
+
+	policy->cur = clk_get_rate(armclk) / 1000;
+	policy->cpuinfo.transition_latency = CPUFREQ_ETERNAL;
+
+	return cpufreq_frequency_table_cpuinfo(policy, s3c64xx_freq_table);
+}
+
+static struct cpufreq_driver s3c64xx_cpufreq_driver = {
+	.owner		= THIS_MODULE,
+	.flags          = 0,
+	.verify		= s3c64xx_cpufreq_verify_speed,
+	.target		= s3c64xx_cpufreq_set_target,
+	.get		= s3c64xx_cpufreq_get_speed,
+	.init		= s3c64xx_cpufreq_driver_init,
+	.name		= "s3c64xx",
+};
+
+static int __init s3c64xx_cpufreq_init(void)
+{
+	return cpufreq_register_driver(&s3c64xx_cpufreq_driver);
+}
+module_init(s3c64xx_cpufreq_init);