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,
 };