[PATCH 4/9] Experimental S3C2410A cpufreq driver (timer)

[Cesar Eduardo Barros]

This is the cpufreq notifier for the S3C2410 timer code.

Signed-off-by: Cesar Eduardo Barros <cesarb at cesarb.net>
---
 arch/arm/plat-s3c24xx/time.c |  130 ++++++++++++++++++++++++++++++++++++++++++
 1 files changed, 130 insertions(+), 0 deletions(-)

diff --git a/arch/arm/plat-s3c24xx/time.c b/arch/arm/plat-s3c24xx/time.c
index 2ec1daa..2d8e575 100644
--- a/arch/arm/plat-s3c24xx/time.c
+++ b/arch/arm/plat-s3c24xx/time.c
@@ -25,6 +25,7 @@
 #include <linux/irq.h>
 #include <linux/err.h>
 #include <linux/clk.h>
+#include <linux/cpufreq.h>
 
 #include <asm/system.h>
 #include <asm/leds.h>
@@ -148,6 +149,132 @@ static struct irqaction s3c2410_timer_irq = {
 	machine_is_anubis()	|| \
 	machine_is_osiris() )
 
+#ifdef CONFIG_CPU_FREQ
+
+static unsigned long old_pclk;
+
+static inline void s3c2410_timer_cpufreq_setup(unsigned long pclk)
+{
+	old_pclk = pclk;
+}
+
+static int s3c2410_timer_cpufreq_notifier(struct notifier_block *nb,
+		unsigned long val, void *data __maybe_unused)
+{
+	struct clk *clk;
+	unsigned long pclk, tcnt, tval;
+	unsigned long tcon;
+	unsigned long flags;
+	bool waszero;
+
+	BUG_ON(!old_pclk);
+
+	if (!(val == CPUFREQ_POSTCHANGE
+			|| val == CPUFREQ_SUSPENDCHANGE
+			|| val == CPUFREQ_RESUMECHANGE))
+		return 0;
+
+	clk = clk_get(NULL, "timers");
+	BUG_ON(!clk);
+	pclk = clk_get_rate(clk);
+	clk_put(clk);
+
+	/* So nobody else plays with S3C2410_TCON. */
+	local_irq_save(flags);
+	tcon = __raw_readl(S3C2410_TCON);
+
+	/* Pause the timer so it will not wrap under us. */
+	tcon &= ~S3C2410_TCON_T4START;
+	__raw_writel(tcon, S3C2410_TCON);
+
+	/* Adjust the remaining ticks. */
+	tval = __raw_readl(S3C2410_TCNTO(4));
+	waszero = tval == 0;
+	tval = cpufreq_scale(tval, old_pclk, pclk);
+	/* Do not miss the interrupt on the 1->0 transition. */
+	if (!tval && !waszero)
+		tval = 1;
+
+	if (unlikely(tval > 0xffff)) {
+		/* Shouldn't happen. */
+		printk(KERN_CRIT
+			"s3c2410_timer: timer current counter overflow\n");
+		tval = 0xffff;
+	}
+
+	tcon |= S3C2410_TCON_T4RELOAD | S3C2410_TCON_T4MANUALUPD;
+	__raw_writel(tval, S3C2410_TCNTB(4));
+	__raw_writel(tcon, S3C2410_TCON);
+
+	/* Adjust the reload value. */
+	tcnt = (pclk / 6) / HZ;
+	tcnt--;
+
+	if (unlikely(tcnt > 0xffff)) {
+		/* Shouldn't happen. */
+		printk(KERN_CRIT
+			"s3c2410_timer: timer reload counter overflow\n");
+		tcnt = 0xffff;
+	}
+	timer_startval = tcnt;
+
+	tcon &= ~S3C2410_TCON_T4MANUALUPD;
+	__raw_writel(tcon, S3C2410_TCON);
+	__raw_writel(tcnt, S3C2410_TCNTB(4));
+	__raw_writel(tcnt, S3C2410_TCMPB(4));
+
+	/* Restart the timer. */
+	tcon |= S3C2410_TCON_T4START;
+	__raw_writel(tcon, S3C2410_TCON);
+
+	/* Save the current pclk for later use. */
+	old_pclk = pclk;
+
+	local_irq_restore(flags);
+
+	return 0;
+}
+
+static struct notifier_block s3c2410_timer_cpufreq_notifier_block = {
+	.notifier_call	= s3c2410_timer_cpufreq_notifier,
+	.priority	= 10,	/* slight boost so it runs before the rest */
+};
+
+static bool s3c2410_timer_enabled __initdata = false;
+
+static inline void __init s3c2410_timer_cpufreq_notifier_setup(void)
+{
+	if (use_tclk1_12())
+		return;
+
+	s3c2410_timer_enabled = true;
+}
+
+/* Must be called late in initialization (later than pure_initcall). */
+static int __init s3c2410_timer_cpufreq_notifier_initcall(void)
+{
+	if (!s3c2410_timer_enabled)
+		return 0;
+
+	return cpufreq_register_notifier(&s3c2410_timer_cpufreq_notifier_block,
+			CPUFREQ_TRANSITION_NOTIFIER);
+}
+device_initcall(s3c2410_timer_cpufreq_notifier_initcall);
+
+#else
+
+static inline void s3c2410_timer_cpufreq_setup(
+		unsigned long pclk __maybe_unused)
+{
+}
+
+static inline int __init s3c2410_timer_cpufreq_notifier_setup()
+{
+	return 0;
+}
+
+#endif
+
 /*
  * Set up timer interrupt, and return the current time in seconds.
  *
@@ -212,6 +339,8 @@ static void s3c2410_timer_setup (void)
 		tcfg0 |= ((6 - 1) / 2) << S3C2410_TCFG_PRESCALER1_SHIFT;
 
 		tcnt = (pclk / 6) / HZ;
+
+		s3c2410_timer_cpufreq_setup(pclk);
 	}
 
 	/* timers reload after counting zero, so reduce the count by 1 */
@@ -252,6 +381,7 @@ static void s3c2410_timer_setup (void)
 static void __init s3c2410_timer_init (void)
 {
 	s3c2410_timer_setup();
+	s3c2410_timer_cpufreq_notifier_setup();
 	setup_irq(IRQ_TIMER4, &s3c2410_timer_irq);
 }
 
-- 
1.5.4