1 files changed, 62 insertions, 27 deletions

diff --git a/arch/arm/mach-clps711x/common.c b/arch/arm/mach-clps711x/common.c
index 20ff50f3ccf0..f6d1746366d4 100644
--- a/arch/arm/mach-clps711x/common.c
+++ b/arch/arm/mach-clps711x/common.c
@@ -27,12 +27,14 @@
 #include <linux/clk.h>
 #include <linux/clkdev.h>
 #include <linux/clockchips.h>
+#include <linux/clocksource.h>
 #include <linux/clk-provider.h>
 
 #include <asm/exception.h>
 #include <asm/mach/irq.h>
 #include <asm/mach/map.h>
 #include <asm/mach/time.h>
+#include <asm/sched_clock.h>
 #include <asm/system_misc.h>
 
 #include <mach/hardware.h>
@@ -213,7 +215,7 @@ void __init clps711x_init_irq(void)
         }
 }
 
-inline u32 fls16(u32 x)
+static inline u32 fls16(u32 x)
 {
         u32 r = 15;
 
@@ -237,27 +239,52 @@ inline u32 fls16(u32 x)
 
 asmlinkage void __exception_irq_entry clps711x_handle_irq(struct pt_regs *regs)
 {
-        u32 irqstat;
-        void __iomem *base = CLPS711X_VIRT_BASE;
-
-        irqstat = readl_relaxed(base + INTSR1) & readl_relaxed(base + INTMR1);
-        if (irqstat) {
-                handle_IRQ(fls16(irqstat), regs);
-                return;
-        }
+        do {
+                u32 irqstat;
+                void __iomem *base = CLPS711X_VIRT_BASE;
+
+                irqstat = readw_relaxed(base + INTSR1) &
+                          readw_relaxed(base + INTMR1);
+                if (irqstat)
+                        handle_IRQ(fls16(irqstat), regs);
+
+                irqstat = readw_relaxed(base + INTSR2) &
+                          readw_relaxed(base + INTMR2);
+                if (irqstat) {
+                        handle_IRQ(fls16(irqstat) + 16, regs);
+                        continue;
+                }
+
+                break;
+        } while (1);
+}
 
-        irqstat = readl_relaxed(base + INTSR2) & readl_relaxed(base + INTMR2);
-        if (likely(irqstat))
-                handle_IRQ(fls16(irqstat) + 16, regs);
+static u32 notrace clps711x_sched_clock_read(void)
+{
+        return ~readw_relaxed(CLPS711X_VIRT_BASE + TC1D);
 }
 
 static void clps711x_clockevent_set_mode(enum clock_event_mode mode,
                                          struct clock_event_device *evt)
 {
+        disable_irq(IRQ_TC2OI);
+
+        switch (mode) {
+        case CLOCK_EVT_MODE_PERIODIC:
+                enable_irq(IRQ_TC2OI);
+                break;
+        case CLOCK_EVT_MODE_ONESHOT:
+                /* Not supported */
+        case CLOCK_EVT_MODE_SHUTDOWN:
+        case CLOCK_EVT_MODE_UNUSED:
+        case CLOCK_EVT_MODE_RESUME:
+                /* Left event sources disabled, no more interrupts appear */
+                break;
+        }
 }
 
 static struct clock_event_device clockevent_clps711x = {
-        .name           = "CLPS711x Clockevents",
+        .name           = "clps711x-clockevent",
         .rating         = 300,
         .features       = CLOCK_EVT_FEAT_PERIODIC,
         .set_mode       = clps711x_clockevent_set_mode,
@@ -271,8 +298,8 @@ static irqreturn_t clps711x_timer_interrupt(int irq, void *dev_id)
 }
 
 static struct irqaction clps711x_timer_irq = {
-        .name           = "CLPS711x Timer Tick",
-        .flags          = IRQF_DISABLED | IRQF_TIMER | IRQF_IRQPOLL,
+        .name           = "clps711x-timer",
+        .flags          = IRQF_TIMER | IRQF_IRQPOLL,
         .handler        = clps711x_timer_interrupt,
 };
 
@@ -301,6 +328,7 @@ void __init clps711x_timer_init(void)
                 cpu = ext;
                 bus = cpu;
                 spi = 135400;
+                pll = 0;
         } else {
                 cpu = pll;
                 if (cpu >= 36864000)
@@ -319,9 +347,9 @@ void __init clps711x_timer_init(void)
                 else
                         timh = 541440;
         } else
-                timh = cpu / 144;
+                timh = DIV_ROUND_CLOSEST(cpu, 144);
 
-        timl = timh / 256;
+        timl = DIV_ROUND_CLOSEST(timh, 256);
 
         /* All clocks are fixed */
         add_fixed_clk(clk_pll, "pll", pll);
@@ -334,13 +362,24 @@ void __init clps711x_timer_init(void)
 
         pr_info("CPU frequency set at %i Hz.\n", cpu);
 
+        /* Start Timer1 in free running mode (Low frequency) */
+        tmp = clps_readl(SYSCON1) & ~(SYSCON1_TC1S | SYSCON1_TC1M);
+        clps_writel(tmp, SYSCON1);
+
+        setup_sched_clock(clps711x_sched_clock_read, 16, timl);
+
+        clocksource_mmio_init(CLPS711X_VIRT_BASE + TC1D,
+                              "clps711x_clocksource", timl, 300, 16,
+                              clocksource_mmio_readw_down);
+
+        /* Set Timer2 prescaler */
         clps_writew(DIV_ROUND_CLOSEST(timh, HZ), TC2D);
 
-        tmp = clps_readl(SYSCON1);
-        tmp |= SYSCON1_TC2S | SYSCON1_TC2M;
+        /* Start Timer2 in prescale mode (High frequency)*/
+        tmp = clps_readl(SYSCON1) | SYSCON1_TC2M | SYSCON1_TC2S;
         clps_writel(tmp, SYSCON1);
 
-        clockevents_config_and_register(&clockevent_clps711x, timh, 1, 0xffff);
+        clockevents_config_and_register(&clockevent_clps711x, timh, 0, 0);
 
         setup_irq(IRQ_TC2OI, &clps711x_timer_irq);
 }
@@ -353,15 +392,11 @@ void clps711x_restart(char mode, const char *cmd)
 static void clps711x_idle(void)
 {
         clps_writel(1, HALT);
-        __asm__ __volatile__(
-        "mov    r0, r0\n\
-        mov     r0, r0");
+        asm("mov r0, r0");
+        asm("mov r0, r0");
 }
 
-static int __init clps711x_idle_init(void)
+void __init clps711x_init_early(void)
 {
         arm_pm_idle = clps711x_idle;
-        return 0;
 }
-
-arch_initcall(clps711x_idle_init);