ux500: add smp_twd clock

Add a smp_twd clock with an appropriate get_rate() implementation so that
localtimers scale correctly with cpufreq changes.

ST-Ericsson ID: 361450
ST-Ericsson Linux next: NA
ST-Ericsson FOSS-OUT ID: Trivial

Change-Id: I492af8efe1c8c081f6079e538f6edf296eca3a02
Signed-off-by: Rabin Vincent <rabin.vincent@stericsson.com>
Reviewed-on: http://gerrit.lud.stericsson.com/gerrit/33277
Reviewed-by: Linus WALLEIJ <linus.walleij@stericsson.com>

2 files changed, 67 insertions, 19 deletions

diff --git a/arch/arm/mach-ux500/clock.c b/arch/arm/mach-ux500/clock.c
index 06a9bdaec7d..108014ea55b 100644
--- a/arch/arm/mach-ux500/clock.c
+++ b/arch/arm/mach-ux500/clock.c
@@ -435,6 +435,29 @@ struct clkops prcc_kclk_rec_ops = {
 	.set_rate = clk_set_rate_rec,
 };
 
+#ifdef CONFIG_CPU_FREQ
+extern unsigned long dbx500_cpufreq_getfreq(void);
+
+unsigned long clk_smp_twd_get_rate(struct clk *clk)
+{
+	return dbx500_cpufreq_getfreq() / 2;
+}
+
+static struct clkops clk_smp_twd_ops = {
+	.get_rate = clk_smp_twd_get_rate,
+};
+
+static struct clk clk_smp_twd = {
+	.name	= "smp_twd",
+	.ops	= &clk_smp_twd_ops,
+};
+
+static struct clk_lookup clk_smp_twd_lookup = {
+	.clk	= &clk_smp_twd,
+	.dev_id	= "smp_twd",
+};
+#endif
+
 int __init clk_init(void)
 {
 	if (cpu_is_u8500()) {
@@ -451,5 +474,9 @@ int __init clk_init(void)
 	else if (cpu_is_u5500())
 		db5500_clk_init();
 
+#ifdef CONFIG_CPU_FREQ
+	clkdev_add(&clk_smp_twd_lookup);
+#endif
+
 	return 0;
 }
diff --git a/drivers/cpufreq/dbx500-cpufreq.c b/drivers/cpufreq/dbx500-cpufreq.c
index fcc21181097..1f9249f869a 100644
--- a/drivers/cpufreq/dbx500-cpufreq.c
+++ b/drivers/cpufreq/dbx500-cpufreq.c
@@ -136,6 +136,44 @@ static unsigned int dbx500_cpufreq_getspeed(unsigned int cpu)
 	return freq_table[i].frequency;
 }
 
+static bool initialized;
+
+static void __init dbx500_cpufreq_early_init(void)
+{
+	if (cpu_is_u5500()) {
+		freq_table = db5500_freq_table;
+		idx2opp = db5500_idx2opp;
+
+	} else if (cpu_is_u8500()) {
+		freq_table = db8500_freq_table;
+		idx2opp = db8500_idx2opp;
+
+		if (!prcmu_is_u8400()) {
+			freq_table[1].frequency = 400000;
+			freq_table[2].frequency = 800000;
+			if (prcmu_has_arm_maxopp())
+				freq_table[3].frequency = 1000000;
+		}
+
+	} else {
+		ux500_unknown_soc();
+	}
+
+	initialized = true;
+}
+
+/*
+ * This is called from localtimer initialization, via the clk_get_rate() for
+ * the smp_twd clock.  This is way before cpufreq is initialized.
+ */
+unsigned long dbx500_cpufreq_getfreq(void)
+{
+	if (!initialized)
+		dbx500_cpufreq_early_init();
+
+	return dbx500_cpufreq_getspeed(0) * 1000;
+}
+
 int dbx500_cpufreq_get_limits(int cpu, int r,
 			      unsigned int *min, unsigned int *max)
 {
@@ -252,25 +290,8 @@ static int __init dbx500_cpufreq_register(void)
 	if (cpu_is_u8500() && !cpu_is_u8500v20_or_later())
 		return -ENODEV;
 
-
-	if (cpu_is_u5500()) {
-		freq_table = db5500_freq_table;
-		idx2opp = db5500_idx2opp;
-
-	} else if (cpu_is_u8500()) {
-		freq_table = db8500_freq_table;
-		idx2opp = db8500_idx2opp;
-
-		if (!prcmu_is_u8400()) {
-			freq_table[1].frequency = 400000;
-			freq_table[2].frequency = 800000;
-			if (prcmu_has_arm_maxopp())
-				freq_table[3].frequency = 1000000;
-		}
-
-	} else {
-		ux500_unknown_soc();
-	}
+	if (!initialized)
+		dbx500_cpufreq_early_init();
 
 	pr_info("dbx500-cpufreq : Available frequencies:\n");