From 1cc62cde7f9717e9349f4b117900bd17750e4bf2 Mon Sep 17 00:00:00 2001
From: Andrzej Zaborowski <balrog@zabor.org>
Date: Fri, 25 Jul 2008 23:06:17 +0100
Subject: [PATCH] From 119f4e02ba81cffe4dbc88d8ff667048ad28d925 Mon Sep 17 00:00:00 2001
 Subject: [PATCH] Hacky CONFIG_NO_IDLE_HZ (dyn-tick) support for S3C24xx.

---
 arch/arm/plat-s3c24xx/time.c |  247 +++++++++++++++++++++++++++++++++++-------
 1 files changed, 209 insertions(+), 38 deletions(-)

diff --git a/arch/arm/plat-s3c24xx/time.c b/arch/arm/plat-s3c24xx/time.c
index 03b8643..8b101e0 100644
--- a/arch/arm/plat-s3c24xx/time.c
+++ b/arch/arm/plat-s3c24xx/time.c
@@ -3,6 +3,8 @@
  * Copyright (C) 2003-2005 Simtec Electronics
  *	Ben Dooks, <ben@simtec.co.uk>
  *
+ * dyn_tick support by Andrzej Zaborowski based on omap_dyn_tick_timer.
+ *
  * 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
@@ -44,6 +46,9 @@ static unsigned long timer_startval;
 static unsigned long timer_usec_ticks;
 static struct work_struct resume_work;
 
+unsigned long pclk;
+struct clk *clk;
+
 #define TIMER_USEC_SHIFT 16
 
 /* we use the shifted arithmetic to work out the ratio of timer ticks
@@ -178,11 +183,7 @@ static void s3c2410_timer_setup (void)
 		tcfg1 &= ~S3C2410_TCFG1_MUX4_MASK;
 		tcfg1 |= S3C2410_TCFG1_MUX4_TCLK1;
 	} else {
-		unsigned long pclk;
-		struct clk *clk;
-
-		/* for the h1940 (and others), we use the pclk from the core
-		 * to generate the timer values. since values around 50 to
+		/* since values around 50 to
 		 * 70MHz are not values we can directly generate the timer
 		 * value from, we need to pre-scale and divide before using it.
 		 *
@@ -190,20 +191,7 @@ static void s3c2410_timer_setup (void)
 		 * (8.45 ticks per usec)
 		 */
 
-		/* this is used as default if no other timer can be found */
-
-		clk = clk_get(NULL, "timers");
-		if (IS_ERR(clk))
-			panic("failed to get clock for system timer");
-
-		clk_enable(clk);
-
-		pclk = clk_get_rate(clk);
-
-		printk("pclk = %lu\n", pclk);
-
 		/* configure clock tick */
-
 		timer_usec_ticks = timer_mask_usec_ticks(6, pclk);
 		printk("timer_usec_ticks = %lu\n", timer_usec_ticks);
 
@@ -214,11 +202,6 @@ static void s3c2410_timer_setup (void)
 		tcfg0 |= ((6 - 1) / 2) << S3C2410_TCFG_PRESCALER1_SHIFT;
 
 		tcnt = (pclk / 6) / HZ;
-
-		/* start the timer running */
-		tcon |= S3C2410_TCON_T4START | S3C2410_TCON_T4RELOAD;
-		tcon &= ~S3C2410_TCON_T4MANUALUPD;
-		__raw_writel(tcon, S3C2410_TCON);
 	}
 
 	/* timers reload after counting zero, so reduce the count by 1 */
@@ -260,27 +243,37 @@ static void s3c2410_timer_setup (void)
 
 }
 
+struct sys_timer s3c24xx_timer;
 static void timer_resume_work(struct work_struct *work)
 {
-	s3c2410_timer_setup();
-}
-
-/* ooh a nasty situation arises if we try to call s3c2410_timer_setup() from
- * the resume handler.  It is called in atomic context but the clock APIs
- * try to lock a mutex which may sleep.  We are in a bit of an unusual
- * situation because we don't have a tick source right now, but it should be
- * okay to try to schedule a work item... hopefully
- */
-
-static void s3c2410_timer_resume_atomic(void)
-{
-	int ret = schedule_work(&resume_work);
-	if (!ret)
-		printk(KERN_INFO"Failed to schedule_work tick ctr (%d)\n", ret);
+	clk_enable(clk);
+
+#ifdef CONFIG_NO_IDLE_HZ
+	if (s3c24xx_timer.dyn_tick->state & DYN_TICK_ENABLED)
+		s3c24xx_timer.dyn_tick->enable();
+	else
+#endif
+		s3c2410_timer_setup();
 }
 
 static void __init s3c2410_timer_init (void)
 {
+	if (!use_tclk1_12()) {
+		/* for the h1940 (and others), we use the pclk from the core
+		 * to generate the timer values.
+		 */
+
+		/* this is used as default if no other timer can be found */
+		clk = clk_get(NULL, "timers");
+		if (IS_ERR(clk))
+			panic("failed to get clock for system timer");
+
+		clk_enable(clk);
+
+		pclk = clk_get_rate(clk);
+		printk("pclk = %lu\n", pclk);
+	}
+
 	INIT_WORK(&resume_work, timer_resume_work);
 	s3c2410_timer_setup();
 	setup_irq(IRQ_TIMER4, &s3c2410_timer_irq);
@@ -302,8 +295,186 @@ static void s3c2410_timer_resume(void)
 		    "s3c2410_timer_resume_work already queued ???\n");
 }
 
+#ifdef CONFIG_NO_IDLE_HZ
+/*
+ * We'll set a constant prescaler so we don't have to bother setting it
+ * when reprogramming and so that we avoid costly divisions.
+ *
+ * (2 * HZ) << INPUT_FREQ_SHIFT is the desired frequency after prescaler.
+ * At HZ == 200, HZ * 1024 should work for PCLKs of up to ~53.5 MHz.
+ */
+#define INPUT_FREQ_SHIFT	9
+
+static int ticks_last;
+static int ticks_left;
+static uint32_t tcnto_last;
+
+static inline int s3c24xx_timer_read(void)
+{
+	uint32_t tcnto = __raw_readl(S3C2410_TCNTO(4));
+
+	/*
+	 * WARNING: sometimes we get called before TCNTB has been
+	 * loaded into the counter and TCNTO then returns its previous
+	 * value and kill us, so don't do anything before counter is
+	 * reloaded.
+	 */
+	if (unlikely(tcnto == tcnto_last))
+		return ticks_last;
+
+	tcnto_last = -1;
+	return tcnto <<
+		((__raw_readl(S3C2410_TCFG1) >> S3C2410_TCFG1_MUX4_SHIFT) & 3);
+}
+
+static inline void s3c24xx_timer_program(int ticks)
+{
+	uint32_t tcon = __raw_readl(S3C2410_TCON) & ~(7 << 20);
+	uint32_t tcfg1 = __raw_readl(S3C2410_TCFG1) & ~S3C2410_TCFG1_MUX4_MASK;
+
+	/* Just make sure the timer is stopped.  */
+	__raw_writel(tcon, S3C2410_TCON);
+
+	/* TODO: add likely()ies / unlikely()ies */
+	if (ticks >> 18) {
+		ticks_last = min(ticks, 0xffff << 3);
+		ticks_left = ticks - ticks_last;
+		__raw_writel(tcfg1 | S3C2410_TCFG1_MUX4_DIV16, S3C2410_TCFG1);
+		__raw_writel(ticks_last >> 3, S3C2410_TCNTB(4));
+	} else if (ticks >> 17) {
+		ticks_last = ticks;
+		ticks_left = 0;
+		__raw_writel(tcfg1 | S3C2410_TCFG1_MUX4_DIV8, S3C2410_TCFG1);
+		__raw_writel(ticks_last >> 2, S3C2410_TCNTB(4));
+	} else if (ticks >> 16) {
+		ticks_last = ticks;
+		ticks_left = 0;
+		__raw_writel(tcfg1 | S3C2410_TCFG1_MUX4_DIV4, S3C2410_TCFG1);
+		__raw_writel(ticks_last >> 1, S3C2410_TCNTB(4));
+	} else {
+		ticks_last = ticks;
+		ticks_left = 0;
+		__raw_writel(tcfg1 | S3C2410_TCFG1_MUX4_DIV2, S3C2410_TCFG1);
+		__raw_writel(ticks_last >> 0, S3C2410_TCNTB(4));
+	}
+
+	tcnto_last = __raw_readl(S3C2410_TCNTO(4));
+	__raw_writel(tcon | S3C2410_TCON_T4MANUALUPD,
+			S3C2410_TCON);
+	__raw_writel(tcon | S3C2410_TCON_T4START,
+			S3C2410_TCON);
+}
+
+/*
+ * If we have already waited all the time we were supposed to wait,
+ * kick the timer, setting the longest allowed timeout value just
+ * for time-keeping.
+ */
+static inline void s3c24xx_timer_program_idle(void)
+{
+	s3c24xx_timer_program(0xffff << 3);
+}
+
+static inline void s3c24xx_timer_update(int restart)
+{
+	int ticks_cur = s3c24xx_timer_read();
+	int jiffies_elapsed = (ticks_last - ticks_cur) >> INPUT_FREQ_SHIFT;
+	int subjiffy = ticks_last - (jiffies_elapsed << INPUT_FREQ_SHIFT);
+
+	if (restart) {
+		if (ticks_left >= (1 << INPUT_FREQ_SHIFT))
+			s3c24xx_timer_program(ticks_left);
+		else
+			s3c24xx_timer_program_idle();
+		ticks_last += subjiffy;
+	} else
+		ticks_last = subjiffy;
+
+	while (jiffies_elapsed --)
+		timer_tick();
+}
+
+/* Called when the timer expires.  */
+static irqreturn_t s3c24xx_timer_handler(int irq, void *dev_id)
+{
+	tcnto_last = -1;
+	s3c24xx_timer_update(1);
+
+	return IRQ_HANDLED;
+}
+
+/* Called to update jiffies with time elapsed.  */
+static irqreturn_t s3c24xx_timer_handler_dyn_tick(int irq, void *dev_id)
+{
+	s3c24xx_timer_update(0);
+
+	return IRQ_HANDLED;
+}
+
+/*
+ * Programs the next timer interrupt needed.  Called when dynamic tick is
+ * enabled, and to reprogram the ticks to skip from pm_idle.  The CPU goes
+ * to sleep directly after this.
+ */
+static void s3c24xx_timer_reprogram_dyn_tick(unsigned long next_jiffies)
+{
+	int subjiffy_left = ticks_last - s3c24xx_timer_read();
+
+	s3c24xx_timer_program(max((int) next_jiffies, 1) << INPUT_FREQ_SHIFT);
+	ticks_last += subjiffy_left;
+}
+
+static unsigned long s3c24xx_timer_offset_dyn_tick(void)
+{
+	/* TODO */
+	return 0;
+}
+
+static int s3c24xx_timer_enable_dyn_tick(void)
+{
+	/* Set our constant prescaler.  */
+	uint32_t tcfg0 = __raw_readl(S3C2410_TCFG0);
+	int prescaler =
+		max(min(256, (int) pclk / (HZ << (INPUT_FREQ_SHIFT + 1))), 1);
+
+	tcfg0 &= ~S3C2410_TCFG_PRESCALER1_MASK;
+	tcfg0 |= (prescaler - 1) << S3C2410_TCFG_PRESCALER1_SHIFT;
+	__raw_writel(tcfg0, S3C2410_TCFG0);
+
+	/* Override handlers.  */
+	s3c2410_timer_irq.handler = s3c24xx_timer_handler;
+	s3c24xx_timer.offset = s3c24xx_timer_offset_dyn_tick;
+
+	printk(KERN_INFO "dyn_tick enabled on s3c24xx timer 4, "
+			"%li Hz pclk with prescaler %i\n", pclk, prescaler);
+
+	s3c24xx_timer_program_idle();
+
+	return 0;
+}
+
+static int s3c24xx_timer_disable_dyn_tick(void)
+{
+	s3c2410_timer_irq.handler = s3c2410_timer_interrupt;
+	s3c24xx_timer.offset = s3c2410_gettimeoffset;
+	s3c2410_timer_setup();
+
+	return 0;
+}
+
+static struct dyn_tick_timer s3c24xx_dyn_tick_timer = {
+	.enable		= s3c24xx_timer_enable_dyn_tick,
+	.disable	= s3c24xx_timer_disable_dyn_tick,
+	.reprogram	= s3c24xx_timer_reprogram_dyn_tick,
+	.handler	= s3c24xx_timer_handler_dyn_tick,
+};
+#endif	/* CONFIG_NO_IDLE_HZ */
+
 struct sys_timer s3c24xx_timer = {
 	.init		= s3c2410_timer_init,
 	.offset		= s3c2410_gettimeoffset,
 	.resume		= s3c2410_timer_resume,
+#ifdef CONFIG_NO_IDLE_HZ
+	.dyn_tick	= &s3c24xx_dyn_tick_timer,
+#endif
 };
-- 
1.5.6.3