5 files changed, 380 insertions, 104 deletions

diff --git a/arch/arm/mach-ns9xxx/Makefile b/arch/arm/mach-ns9xxx/Makefile
index 4476411b8140..6fb82b855a55 100644
--- a/arch/arm/mach-ns9xxx/Makefile
+++ b/arch/arm/mach-ns9xxx/Makefile
@@ -1,4 +1,4 @@
-obj-y := irq.o time.o generic.o
+obj-y := irq.o time.o generic.o gpio.o
 
 obj-$(CONFIG_MACH_CC9P9360DEV) += mach-cc9p9360dev.o
 obj-$(CONFIG_MACH_CC9P9360JS) += mach-cc9p9360js.o
diff --git a/arch/arm/mach-ns9xxx/board-a9m9750dev.c b/arch/arm/mach-ns9xxx/board-a9m9750dev.c
index 925048e7adfe..0f65177f9e5f 100644
--- a/arch/arm/mach-ns9xxx/board-a9m9750dev.c
+++ b/arch/arm/mach-ns9xxx/board-a9m9750dev.c
@@ -13,6 +13,7 @@
 #include <linux/irq.h>
 
 #include <asm/mach/map.h>
+#include <asm/gpio.h>
 
 #include <asm/arch-ns9xxx/board.h>
 #include <asm/arch-ns9xxx/regs-sys.h>
@@ -44,7 +45,13 @@ static void a9m9750dev_fpga_ack_irq(unsigned int irq)
 
 static void a9m9750dev_fpga_mask_irq(unsigned int irq)
 {
-        FPGA_IER &= ~(1 << (irq - FPGA_IRQ(0)));
+        u8 ier;
+
+        ier = __raw_readb(FPGA_IER);
+
+        ier &= ~(1 << (irq - FPGA_IRQ(0)));
+
+        __raw_writeb(ier, FPGA_IER);
 }
 
 static void a9m9750dev_fpga_maskack_irq(unsigned int irq)
@@ -55,7 +62,13 @@ static void a9m9750dev_fpga_maskack_irq(unsigned int irq)
 
 static void a9m9750dev_fpga_unmask_irq(unsigned int irq)
 {
-        FPGA_IER |= 1 << (irq - FPGA_IRQ(0));
+        u8 ier;
+
+        ier = __raw_readb(FPGA_IER);
+
+        ier |= 1 << (irq - FPGA_IRQ(0));
+
+        __raw_writeb(ier, FPGA_IER);
 }
 
 static struct irq_chip a9m9750dev_fpga_chip = {
@@ -68,30 +81,34 @@ static struct irq_chip a9m9750dev_fpga_chip = {
 static void a9m9750dev_fpga_demux_handler(unsigned int irq,
                 struct irq_desc *desc)
 {
-        int stat = FPGA_ISR;
+        u8 stat = __raw_readb(FPGA_ISR);
+
+        desc->chip->mask_ack(irq);
 
         while (stat != 0) {
                 int irqno = fls(stat) - 1;
+                struct irq_desc *fpgadesc;
 
                 stat &= ~(1 << irqno);
 
-                desc = irq_desc + FPGA_IRQ(irqno);
+                fpgadesc = irq_desc + FPGA_IRQ(irqno);
 
-                desc_handle_irq(FPGA_IRQ(irqno), desc);
+                desc_handle_irq(FPGA_IRQ(irqno), fpgadesc);
         }
+
+        desc->chip->unmask(irq);
 }
 
 void __init board_a9m9750dev_init_irq(void)
 {
-        u32 reg;
+        u32 eic;
         int i;
 
-        /*
-         * configure gpio for IRQ_EXT2
-         * use GPIO 11, because GPIO 32 is used for the LCD
-         */
-        /* XXX: proper GPIO handling */
-        BBU_GCONFb1(1) &= ~0x2000;
+        if (gpio_request(11, "board a9m9750dev extirq2") == 0)
+                ns9xxx_gpio_configure(11, 0, 1);
+        else
+                printk(KERN_ERR "%s: cannot get gpio 11 for IRQ_EXT2\n",
+                                __func__);
 
         for (i = FPGA_IRQ(0); i <= FPGA_IRQ(7); ++i) {
                 set_irq_chip(i, &a9m9750dev_fpga_chip);
@@ -100,10 +117,10 @@ void __init board_a9m9750dev_init_irq(void)
         }
 
         /* IRQ_EXT2: level sensitive + active low */
-        reg = SYS_EIC(2);
-        REGSET(reg, SYS_EIC, PLTY, AL);
-        REGSET(reg, SYS_EIC, LVEDG, LEVEL);
-        SYS_EIC(2) = reg;
+        eic = __raw_readl(SYS_EIC(2));
+        REGSET(eic, SYS_EIC, PLTY, AL);
+        REGSET(eic, SYS_EIC, LVEDG, LEVEL);
+        __raw_writel(eic, SYS_EIC(2));
 
         set_irq_chained_handler(IRQ_EXT2,
                         a9m9750dev_fpga_demux_handler);
@@ -167,17 +184,18 @@ void __init board_a9m9750dev_init_machine(void)
         u32 reg;
 
         /* setup static CS0: memory base ... */
-        REGSETIM(SYS_SMCSSMB(0), SYS_SMCSSMB, CSxB,
-                        NS9XXX_CSxSTAT_PHYS(0) >> 12);
+        reg = __raw_readl(SYS_SMCSSMB(0));
+        REGSETIM(reg, SYS_SMCSSMB, CSxB, NS9XXX_CSxSTAT_PHYS(0) >> 12);
+        __raw_writel(reg, SYS_SMCSSMB(0));
 
         /* ... and mask */
-        reg = SYS_SMCSSMM(0);
+        reg = __raw_readl(SYS_SMCSSMM(0));
         REGSETIM(reg, SYS_SMCSSMM, CSxM, 0xfffff);
         REGSET(reg, SYS_SMCSSMM, CSEx, EN);
-        SYS_SMCSSMM(0) = reg;
+        __raw_writel(reg, SYS_SMCSSMM(0));
 
         /* setup static CS0: memory configuration */
-        reg = MEM_SMC(0);
+        reg = __raw_readl(MEM_SMC(0));
         REGSET(reg, MEM_SMC, PSMC, OFF);
         REGSET(reg, MEM_SMC, BSMC, OFF);
         REGSET(reg, MEM_SMC, EW, OFF);
@@ -185,13 +203,13 @@ void __init board_a9m9750dev_init_machine(void)
         REGSET(reg, MEM_SMC, PC, AL);
         REGSET(reg, MEM_SMC, PM, DIS);
         REGSET(reg, MEM_SMC, MW, 8);
-        MEM_SMC(0) = reg;
+        __raw_writel(reg, MEM_SMC(0));
 
         /* setup static CS0: timing */
-        MEM_SMWED(0) = 0x2;
-        MEM_SMOED(0) = 0x2;
-        MEM_SMRD(0) = 0x6;
-        MEM_SMWD(0) = 0x6;
+        __raw_writel(0x2, MEM_SMWED(0));
+        __raw_writel(0x2, MEM_SMOED(0));
+        __raw_writel(0x6, MEM_SMRD(0));
+        __raw_writel(0x6, MEM_SMWD(0));
 
         platform_add_devices(board_a9m9750dev_devices,
                         ARRAY_SIZE(board_a9m9750dev_devices));
diff --git a/arch/arm/mach-ns9xxx/gpio.c b/arch/arm/mach-ns9xxx/gpio.c
new file mode 100644
index 000000000000..b2230213b983
--- /dev/null
+++ b/arch/arm/mach-ns9xxx/gpio.c
@@ -0,0 +1,190 @@
+/*
+ * arch/arm/mach-ns9xxx/gpio.c
+ *
+ * Copyright (C) 2006 by Digi International Inc.
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ */
+#include <linux/compiler.h>
+#include <linux/init.h>
+#include <linux/spinlock.h>
+#include <linux/module.h>
+
+#include <asm/arch-ns9xxx/gpio.h>
+#include <asm/arch-ns9xxx/processor.h>
+#include <asm/arch-ns9xxx/regs-bbu.h>
+#include <asm/io.h>
+#include <asm/bug.h>
+#include <asm/types.h>
+#include <asm/bitops.h>
+
+#if defined(CONFIG_PROCESSOR_NS9360)
+#define GPIO_MAX 72
+#elif defined(CONFIG_PROCESSOR_NS9750)
+#define GPIO_MAX 49
+#endif
+
+/* protects BBU_GCONFx and BBU_GCTRLx */
+static spinlock_t gpio_lock = __SPIN_LOCK_UNLOCKED(gpio_lock);
+
+/* only access gpiores with atomic ops */
+static DECLARE_BITMAP(gpiores, GPIO_MAX);
+
+static inline int ns9xxx_valid_gpio(unsigned gpio)
+{
+#if defined(CONFIG_PROCESSOR_NS9360)
+        if (processor_is_ns9360())
+                return gpio <= 72;
+        else
+#endif
+#if defined(CONFIG_PROCESSOR_NS9750)
+        if (processor_is_ns9750())
+                return gpio <= 49;
+        else
+#endif
+                BUG();
+}
+
+static inline void __iomem *ns9xxx_gpio_get_gconfaddr(unsigned gpio)
+{
+        if (gpio < 56)
+                return BBU_GCONFb1(gpio / 8);
+        else
+                /*
+                 * this could be optimised away on
+                 * ns9750 only builds, but it isn't ...
+                 */
+                return BBU_GCONFb2((gpio - 56) / 8);
+}
+
+static inline void __iomem *ns9xxx_gpio_get_gctrladdr(unsigned gpio)
+{
+        if (gpio < 32)
+                return BBU_GCTRL1;
+        else if (gpio < 64)
+                return BBU_GCTRL2;
+        else
+                /* this could be optimised away on ns9750 only builds */
+                return BBU_GCTRL3;
+}
+
+static inline void __iomem *ns9xxx_gpio_get_gstataddr(unsigned gpio)
+{
+        if (gpio < 32)
+                return BBU_GSTAT1;
+        else if (gpio < 64)
+                return BBU_GSTAT2;
+        else
+                /* this could be optimised away on ns9750 only builds */
+                return BBU_GSTAT3;
+}
+
+int gpio_request(unsigned gpio, const char *label)
+{
+        if (likely(ns9xxx_valid_gpio(gpio)))
+                return test_and_set_bit(gpio, gpiores) ? -EBUSY : 0;
+        else
+                return -EINVAL;
+}
+EXPORT_SYMBOL(gpio_request);
+
+void gpio_free(unsigned gpio)
+{
+        clear_bit(gpio, gpiores);
+        return;
+}
+EXPORT_SYMBOL(gpio_free);
+
+/*
+ * each gpio can serve for 4 different purposes [0..3].  These are called
+ * "functions" and passed in the parameter func.  Functions 0-2 are always some
+ * special things, function 3 is GPIO.  If func == 3 dir specifies input or
+ * output, and with inv you can enable an inverter (independent of func).
+ */
+static int __ns9xxx_gpio_configure(unsigned gpio, int dir, int inv, int func)
+{
+        void __iomem *conf = ns9xxx_gpio_get_gconfaddr(gpio);
+        u32 confval;
+        unsigned long flags;
+
+        spin_lock_irqsave(&gpio_lock, flags);
+
+        confval = __raw_readl(conf);
+        REGSETIM_IDX(confval, BBU_GCONFx, DIR, gpio & 7, dir);
+        REGSETIM_IDX(confval, BBU_GCONFx, INV, gpio & 7, inv);
+        REGSETIM_IDX(confval, BBU_GCONFx, FUNC, gpio & 7, func);
+        __raw_writel(confval, conf);
+
+        spin_unlock_irqrestore(&gpio_lock, flags);
+
+        return 0;
+}
+
+int ns9xxx_gpio_configure(unsigned gpio, int inv, int func)
+{
+        if (likely(ns9xxx_valid_gpio(gpio))) {
+                if (func == 3) {
+                        printk(KERN_WARNING "use gpio_direction_input "
+                                        "or gpio_direction_output\n");
+                        return -EINVAL;
+                } else
+                        return __ns9xxx_gpio_configure(gpio, 0, inv, func);
+        } else
+                return -EINVAL;
+}
+EXPORT_SYMBOL(ns9xxx_gpio_configure);
+
+int gpio_direction_input(unsigned gpio)
+{
+        if (likely(ns9xxx_valid_gpio(gpio))) {
+                return __ns9xxx_gpio_configure(gpio, 0, 0, 3);
+        } else
+                return -EINVAL;
+}
+EXPORT_SYMBOL(gpio_direction_input);
+
+int gpio_direction_output(unsigned gpio, int value)
+{
+        if (likely(ns9xxx_valid_gpio(gpio))) {
+                gpio_set_value(gpio, value);
+
+                return __ns9xxx_gpio_configure(gpio, 1, 0, 3);
+        } else
+                return -EINVAL;
+}
+EXPORT_SYMBOL(gpio_direction_output);
+
+int gpio_get_value(unsigned gpio)
+{
+        void __iomem *stat = ns9xxx_gpio_get_gstataddr(gpio);
+        int ret;
+
+        ret = 1 & (__raw_readl(stat) >> (gpio & 31));
+
+        return ret;
+}
+EXPORT_SYMBOL(gpio_get_value);
+
+void gpio_set_value(unsigned gpio, int value)
+{
+        void __iomem *ctrl = ns9xxx_gpio_get_gctrladdr(gpio);
+        u32 ctrlval;
+        unsigned long flags;
+
+        spin_lock_irqsave(&gpio_lock, flags);
+
+        ctrlval = __raw_readl(ctrl);
+
+        if (value)
+                ctrlval |= 1 << (gpio & 31);
+        else
+                ctrlval &= ~(1 << (gpio & 31));
+
+        __raw_writel(ctrlval, ctrl);
+
+        spin_unlock_irqrestore(&gpio_lock, flags);
+}
+EXPORT_SYMBOL(gpio_set_value);
diff --git a/arch/arm/mach-ns9xxx/irq.c b/arch/arm/mach-ns9xxx/irq.c
index b8c7b00522e6..00001b874e97 100644
--- a/arch/arm/mach-ns9xxx/irq.c
+++ b/arch/arm/mach-ns9xxx/irq.c
@@ -9,6 +9,7 @@
  * the Free Software Foundation.
  */
 #include <linux/interrupt.h>
+#include <asm/io.h>
 #include <asm/mach/irq.h>
 #include <asm/mach-types.h>
 #include <asm/arch-ns9xxx/regs-sys.h>
@@ -17,48 +18,17 @@
 
 #include "generic.h"
 
-static void ns9xxx_ack_irq_timer(unsigned int irq)
-{
-        u32 tc = SYS_TC(irq - IRQ_TIMER0);
-
-        /*
-         * If the timer is programmed to halt on terminal count, the
-         * timer must be disabled before clearing the interrupt.
-         */
-        if (REGGET(tc, SYS_TCx, REN) == 0) {
-                REGSET(tc, SYS_TCx, TEN, DIS);
-                SYS_TC(irq - IRQ_TIMER0) = tc;
-        }
-
-        REGSET(tc, SYS_TCx, INTC, SET);
-        SYS_TC(irq - IRQ_TIMER0) = tc;
-
-        REGSET(tc, SYS_TCx, INTC, UNSET);
-        SYS_TC(irq - IRQ_TIMER0) = tc;
-}
-
-static void (*ns9xxx_ack_irq_functions[NR_IRQS])(unsigned int) = {
-        [IRQ_TIMER0] = ns9xxx_ack_irq_timer,
-        [IRQ_TIMER1] = ns9xxx_ack_irq_timer,
-        [IRQ_TIMER2] = ns9xxx_ack_irq_timer,
-        [IRQ_TIMER3] = ns9xxx_ack_irq_timer,
-};
-
 static void ns9xxx_mask_irq(unsigned int irq)
 {
         /* XXX: better use cpp symbols */
-        SYS_IC(irq / 4) &= ~(1 << (7 + 8 * (3 - (irq & 3))));
+        u32 ic = __raw_readl(SYS_IC(irq / 4));
+        ic &= ~(1 << (7 + 8 * (3 - (irq & 3))));
+        __raw_writel(ic, SYS_IC(irq / 4));
 }
 
 static void ns9xxx_ack_irq(unsigned int irq)
 {
-        if (!ns9xxx_ack_irq_functions[irq]) {
-                printk(KERN_ERR "no ack function for irq %u\n", irq);
-                BUG();
-        }
-
-        ns9xxx_ack_irq_functions[irq](irq);
-        SYS_ISRADDR = 0;
+        __raw_writel(0, SYS_ISRADDR);
 }
 
 static void ns9xxx_maskack_irq(unsigned int irq)
@@ -70,7 +40,9 @@ static void ns9xxx_maskack_irq(unsigned int irq)
 static void ns9xxx_unmask_irq(unsigned int irq)
 {
         /* XXX: better use cpp symbols */
-        SYS_IC(irq / 4) |= 1 << (7 + 8 * (3 - (irq & 3)));
+        u32 ic = __raw_readl(SYS_IC(irq / 4));
+        ic |= 1 << (7 + 8 * (3 - (irq & 3)));
+        __raw_writel(ic, SYS_IC(irq / 4));
 }
 
 static struct irq_chip ns9xxx_chip = {
@@ -86,14 +58,14 @@ void __init ns9xxx_init_irq(void)
 
         /* disable all IRQs */
         for (i = 0; i < 8; ++i)
-                SYS_IC(i) = (4 * i) << 24 | (4 * i + 1) << 16 |
-                        (4 * i + 2) << 8 | (4 * i + 3);
+                __raw_writel((4 * i) << 24 | (4 * i + 1) << 16 |
+                                (4 * i + 2) << 8 | (4 * i + 3), SYS_IC(i));
 
         /* simple interrupt prio table:
          * prio(x) < prio(y) <=> x < y
          */
         for (i = 0; i < 32; ++i)
-                SYS_IVA(i) = i;
+                __raw_writel(i, SYS_IVA(i));
 
         for (i = IRQ_WATCHDOG; i <= IRQ_EXT3; ++i) {
                 set_irq_chip(i, &ns9xxx_chip);
diff --git a/arch/arm/mach-ns9xxx/time.c b/arch/arm/mach-ns9xxx/time.c
index b97d0c54a388..c3dd1f4acb99 100644
--- a/arch/arm/mach-ns9xxx/time.c
+++ b/arch/arm/mach-ns9xxx/time.c
@@ -11,78 +11,174 @@
 #include <linux/jiffies.h>
 #include <linux/interrupt.h>
 #include <linux/irq.h>
+#include <linux/stringify.h>
+#include <linux/clocksource.h>
+#include <linux/clockchips.h>
+
 #include <asm/arch-ns9xxx/regs-sys.h>
 #include <asm/arch-ns9xxx/clock.h>
 #include <asm/arch-ns9xxx/irqs.h>
 #include <asm/arch/system.h>
 #include "generic.h"
 
-#define TIMERCLOCKSELECT 64
+#define TIMER_CLOCKSOURCE 0
+#define TIMER_CLOCKEVENT 1
+static u32 latch;
+
+static cycle_t ns9xxx_clocksource_read(void)
+{
+        return __raw_readl(SYS_TR(TIMER_CLOCKSOURCE));
+}
 
-static u32 usecs_per_tick;
+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 irqreturn_t
-ns9xxx_timer_interrupt(int irq, void *dev_id)
+static void ns9xxx_clockevent_setmode(enum clock_event_mode mode,
+                struct clock_event_device *clk)
 {
-        write_seqlock(&xtime_lock);
-        timer_tick();
-        write_sequnlock(&xtime_lock);
+        u32 tc = __raw_readl(SYS_TC(TIMER_CLOCKEVENT));
+
+        switch(mode) {
+        case CLOCK_EVT_MODE_PERIODIC:
+                __raw_writel(latch, SYS_TRC(TIMER_CLOCKEVENT));
+                REGSET(tc, SYS_TCx, REN, EN);
+                REGSET(tc, SYS_TCx, INTS, EN);
+                REGSET(tc, SYS_TCx, TEN, EN);
+                break;
+
+        case CLOCK_EVT_MODE_ONESHOT:
+                REGSET(tc, SYS_TCx, REN, DIS);
+                REGSET(tc, SYS_TCx, INTS, EN);
+
+                /* fall through */
+
+        case CLOCK_EVT_MODE_UNUSED:
+        case CLOCK_EVT_MODE_SHUTDOWN:
+        case CLOCK_EVT_MODE_RESUME:
+        default:
+                REGSET(tc, SYS_TCx, TEN, DIS);
+                break;
+        }
+
+        __raw_writel(tc, SYS_TC(TIMER_CLOCKEVENT));
+}
 
-        return IRQ_HANDLED;
+static int ns9xxx_clockevent_setnextevent(unsigned long evt,
+                struct clock_event_device *clk)
+{
+        u32 tc = __raw_readl(SYS_TC(TIMER_CLOCKEVENT));
+
+        if (REGGET(tc, SYS_TCx, TEN)) {
+                REGSET(tc, SYS_TCx, TEN, DIS);
+                __raw_writel(tc, SYS_TC(TIMER_CLOCKEVENT));
+        }
+
+        REGSET(tc, SYS_TCx, TEN, EN);
+
+        __raw_writel(evt, SYS_TRC(TIMER_CLOCKEVENT));
+
+        __raw_writel(tc, SYS_TC(TIMER_CLOCKEVENT));
+
+        return 0;
 }
 
-static unsigned long ns9xxx_timer_gettimeoffset(void)
+static struct clock_event_device ns9xxx_clockevent_device = {
+        .name           = "ns9xxx-timer" __stringify(TIMER_CLOCKEVENT),
+        .shift          = 20,
+        .features       = CLOCK_EVT_FEAT_PERIODIC | CLOCK_EVT_FEAT_ONESHOT,
+        .set_mode       = ns9xxx_clockevent_setmode,
+        .set_next_event = ns9xxx_clockevent_setnextevent,
+};
+
+static irqreturn_t ns9xxx_clockevent_handler(int irq, void *dev_id)
 {
-        /* 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. */
+        int timerno = irq - IRQ_TIMER0;
+        u32 tc;
 
-        u32 counter1 = SYS_TR(0);
-        int pending = SYS_ISR & (1 << IRQ_TIMER0);
-        u32 counter2 = SYS_TR(0);
-        u32 elapsed;
+        struct clock_event_device *evt = &ns9xxx_clockevent_device;
 
-        if (pending || counter2 > counter1)
-                elapsed = 2 * SYS_TRC(0) - counter2;
-        else
-                elapsed = SYS_TRC(0) - counter1;
+        /* clear irq */
+        tc = __raw_readl(SYS_TC(timerno));
+        if (REGGET(tc, SYS_TCx, REN) == SYS_TCx_REN_DIS) {
+                REGSET(tc, SYS_TCx, TEN, DIS);
+                __raw_writel(tc, SYS_TC(timerno));
+        }
+        REGSET(tc, SYS_TCx, INTC, SET);
+        __raw_writel(tc, SYS_TC(timerno));
+        REGSET(tc, SYS_TCx, INTC, UNSET);
+        __raw_writel(tc, SYS_TC(timerno));
 
-        return (elapsed * usecs_per_tick) >> 16;
+        evt->event_handler(evt);
 
+        return IRQ_HANDLED;
 }
 
-static struct irqaction ns9xxx_timer_irq = {
-        .name = "NS9xxx Timer Tick",
-        .flags = IRQF_DISABLED | IRQF_TIMER | IRQF_IRQPOLL,
-        .handler = ns9xxx_timer_interrupt,
+static struct irqaction ns9xxx_clockevent_action = {
+        .name           = "ns9xxx-timer" __stringify(TIMER_CLOCKEVENT),
+        .flags          = IRQF_DISABLED | IRQF_TIMER | IRQF_IRQPOLL,
+        .handler        = ns9xxx_clockevent_handler,
 };
 
 static void __init ns9xxx_timer_init(void)
 {
         int tc;
 
-        usecs_per_tick =
-                SH_DIV(1000000 * TIMERCLOCKSELECT, ns9xxx_cpuclock(), 16);
+        tc = __raw_readl(SYS_TC(TIMER_CLOCKSOURCE));
+        if (REGGET(tc, SYS_TCx, TEN)) {
+                REGSET(tc, SYS_TCx, TEN, DIS);
+                __raw_writel(tc, SYS_TC(TIMER_CLOCKSOURCE));
+        }
 
-        /* disable timer */
-        if ((tc = SYS_TC(0)) & SYS_TCx_TEN)
-                SYS_TC(0) = tc & ~SYS_TCx_TEN;
-
-        SYS_TRC(0) = SH_DIV(ns9xxx_cpuclock(), (TIMERCLOCKSELECT * HZ), 0);
+        __raw_writel(0, SYS_TRC(TIMER_CLOCKSOURCE));
 
         REGSET(tc, SYS_TCx, TEN, EN);
-        REGSET(tc, SYS_TCx, TLCS, DIV64); /* This must match TIMERCLOCKSELECT */
-        REGSET(tc, SYS_TCx, INTS, EN);
-        REGSET(tc, SYS_TCx, UDS, DOWN);
         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(0) = tc;
 
-        setup_irq(IRQ_TIMER0, &ns9xxx_timer_irq);
+        __raw_writel(tc, SYS_TC(TIMER_CLOCKSOURCE));
+
+        ns9xxx_clocksource.mult = clocksource_hz2mult(ns9xxx_cpuclock(),
+                        ns9xxx_clocksource.shift);
+
+        clocksource_register(&ns9xxx_clocksource);
+
+        latch = SH_DIV(ns9xxx_cpuclock(), HZ, 0);
+
+        tc = __raw_readl(SYS_TC(TIMER_CLOCKEVENT));
+        REGSET(tc, SYS_TCx, TEN, DIS);
+        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, DOWN);
+        REGSET(tc, SYS_TCx, TSZ, 32);
+        REGSET(tc, SYS_TCx, REN, EN);
+        __raw_writel(tc, SYS_TC(TIMER_CLOCKEVENT));
+
+        ns9xxx_clockevent_device.mult = div_sc(ns9xxx_cpuclock(),
+                        NSEC_PER_SEC, ns9xxx_clockevent_device.shift);
+        ns9xxx_clockevent_device.max_delta_ns =
+                clockevent_delta2ns(-1, &ns9xxx_clockevent_device);
+        ns9xxx_clockevent_device.min_delta_ns =
+                clockevent_delta2ns(1, &ns9xxx_clockevent_device);
+
+        ns9xxx_clockevent_device.cpumask = cpumask_of_cpu(0);
+        clockevents_register_device(&ns9xxx_clockevent_device);
+
+        setup_irq(IRQ_TIMER0 + TIMER_CLOCKEVENT, &ns9xxx_clockevent_action);
 }
 
 struct sys_timer ns9xxx_timer = {
         .init = ns9xxx_timer_init,
-        .offset = ns9xxx_timer_gettimeoffset,
 };