1 files changed, 42 insertions, 26 deletions

diff --git a/arch/arm/mach-ns9xxx/time.c b/arch/arm/mach-ns9xxx/time.c
index 3327d302618d..d29345501762 100644
--- a/arch/arm/mach-ns9xxx/time.c
+++ b/arch/arm/mach-ns9xxx/time.c
@@ -11,6 +11,9 @@
 #include <linux/jiffies.h>
 #include <linux/interrupt.h>
 #include <linux/irq.h>
+#include <linux/stringify.h>
+#include <linux/clocksource.h>
+
 #include <asm/arch-ns9xxx/regs-sys.h>
 #include <asm/arch-ns9xxx/clock.h>
 #include <asm/arch-ns9xxx/irqs.h>
@@ -18,8 +21,7 @@
 #include "generic.h"
 
 #define TIMERCLOCKSELECT 64
-
-static u32 usecs_per_tick;
+#define TIMER_CLOCKSOURCE 1
 
 static irqreturn_t
 ns9xxx_timer_interrupt(int irq, void *dev_id)
@@ -45,39 +47,30 @@ ns9xxx_timer_interrupt(int irq, void *dev_id)
 	return IRQ_HANDLED;
 }
 
-static unsigned long ns9xxx_timer_gettimeoffset(void)
-{
-	/* return the microseconds which have passed since the last interrupt
-	 * was _serviced_.  That is, if an interrupt is pending or the counter
-	 * reloads, return one period more. */
-
-	u32 counter1 = SYS_TR(0);
-	int pending = SYS_ISR & (1 << IRQ_TIMER0);
-	u32 counter2 = SYS_TR(0);
-	u32 elapsed;
-
-	if (pending || counter2 > counter1)
-		elapsed = 2 * SYS_TRC(0) - counter2;
-	else
-		elapsed = SYS_TRC(0) - counter1;
-
-	return (elapsed * usecs_per_tick) >> 16;
-
-}
-
 static struct irqaction ns9xxx_timer_irq = {
 	.name = "NS9xxx Timer Tick",
 	.flags = IRQF_DISABLED | IRQF_TIMER | IRQF_IRQPOLL,
 	.handler = ns9xxx_timer_interrupt,
 };
 
+static cycle_t ns9xxx_clocksource_read(void)
+{
+	return SYS_TR(TIMER_CLOCKSOURCE);
+}
+
+static struct clocksource ns9xxx_clocksource = {
+	.name	= "ns9xxx-timer" __stringify(TIMER_CLOCKSOURCE),
+	.rating	= 300,
+	.read	= ns9xxx_clocksource_read,
+	.mask	= CLOCKSOURCE_MASK(32),
+	.shift	= 20,
+	.flags	= CLOCK_SOURCE_IS_CONTINUOUS,
+};
+
 static void __init ns9xxx_timer_init(void)
 {
 	int tc;
 
-	usecs_per_tick =
-		SH_DIV(1000000 * TIMERCLOCKSELECT, ns9xxx_cpuclock(), 16);
-
 	/* disable timer */
 	if ((tc = SYS_TC(0)) & SYS_TCx_TEN)
 		SYS_TC(0) = tc & ~SYS_TCx_TEN;
@@ -94,9 +87,32 @@ static void __init ns9xxx_timer_init(void)
 	SYS_TC(0) = tc;
 
 	setup_irq(IRQ_TIMER0, &ns9xxx_timer_irq);
+
+	tc = SYS_TC(TIMER_CLOCKSOURCE);
+	if (REGGET(tc, SYS_TCx, TEN)) {
+		REGSET(tc, SYS_TCx, TEN, DIS);
+		SYS_TC(TIMER_CLOCKSOURCE) = tc;
+	}
+
+	SYS_TRC(TIMER_CLOCKSOURCE) = 0;
+
+	REGSET(tc, SYS_TCx, TEN, EN);
+	REGSET(tc, SYS_TCx, TDBG, STOP);
+	REGSET(tc, SYS_TCx, TLCS, CPU);
+	REGSET(tc, SYS_TCx, TM, IEE);
+	REGSET(tc, SYS_TCx, INTS, DIS);
+	REGSET(tc, SYS_TCx, UDS, UP);
+	REGSET(tc, SYS_TCx, TSZ, 32);
+	REGSET(tc, SYS_TCx, REN, EN);
+
+	SYS_TC(TIMER_CLOCKSOURCE) = tc;
+
+	ns9xxx_clocksource.mult = clocksource_hz2mult(ns9xxx_cpuclock(),
+			ns9xxx_clocksource.shift);
+
+	clocksource_register(&ns9xxx_clocksource);
 }
 
 struct sys_timer ns9xxx_timer = {
 	.init = ns9xxx_timer_init,
-	.offset = ns9xxx_timer_gettimeoffset,
 };