1 files changed, 31 insertions, 42 deletions

diff --git a/arch/arm/mach-realview/core.c b/arch/arm/mach-realview/core.c
index 61d70218f1e..f805840f6f4 100644
--- a/arch/arm/mach-realview/core.c
+++ b/arch/arm/mach-realview/core.c
@@ -25,6 +25,7 @@
 #include <linux/interrupt.h>
 #include <linux/amba/bus.h>
 #include <linux/amba/clcd.h>
+#include <linux/clocksource.h>
 
 #include <asm/system.h>
 #include <asm/hardware.h>
@@ -485,47 +486,6 @@ void realview_leds_event(led_event_t ledevt)
 #endif
 
 /*
- * Returns number of ms since last clock interrupt.  Note that interrupts
- * will have been disabled by do_gettimeoffset()
- */
-static unsigned long realview_gettimeoffset(void)
-{
-	unsigned long ticks1, ticks2, status;
-
-	/*
-	 * Get the current number of ticks.  Note that there is a race
-	 * condition between us reading the timer and checking for
-	 * an interrupt.  We get around this by ensuring that the
-	 * counter has not reloaded between our two reads.
-	 */
-	ticks2 = readl(TIMER0_VA_BASE + TIMER_VALUE) & 0xffff;
-	do {
-		ticks1 = ticks2;
-		status = __raw_readl(__io_address(REALVIEW_GIC_DIST_BASE + GIC_DIST_PENDING_SET)
-				     + ((IRQ_TIMERINT0_1 >> 5) << 2));
-		ticks2 = readl(TIMER0_VA_BASE + TIMER_VALUE) & 0xffff;
-	} while (ticks2 > ticks1);
-
-	/*
-	 * Number of ticks since last interrupt.
-	 */
-	ticks1 = TIMER_RELOAD - ticks2;
-
-	/*
-	 * Interrupt pending?  If so, we've reloaded once already.
-	 *
-	 * FIXME: Need to check this is effectively timer 0 that expires
-	 */
-	if (status & IRQMASK_TIMERINT0_1)
-		ticks1 += TIMER_RELOAD;
-
-	/*
-	 * Convert the ticks to usecs
-	 */
-	return TICKS2USECS(ticks1);
-}
-
-/*
  * IRQ handler for the timer
  */
 static irqreturn_t realview_timer_interrupt(int irq, void *dev_id)
@@ -549,6 +509,34 @@ static struct irqaction realview_timer_irq = {
 	.handler	= realview_timer_interrupt,
 };
 
+static cycle_t realview_get_cycles(void)
+{
+	return ~readl(TIMER3_VA_BASE + TIMER_VALUE);
+}
+
+static struct clocksource clocksource_realview = {
+	.name	= "timer3",
+	.rating	= 200,
+	.read	= realview_get_cycles,
+	.mask	= CLOCKSOURCE_MASK(32),
+	.shift	= 20,
+	.flags	= CLOCK_SOURCE_IS_CONTINUOUS,
+};
+
+static void __init realview_clocksource_init(void)
+{
+	/* setup timer 0 as free-running clocksource */
+	writel(0, TIMER3_VA_BASE + TIMER_CTRL);
+	writel(0xffffffff, TIMER3_VA_BASE + TIMER_LOAD);
+	writel(0xffffffff, TIMER3_VA_BASE + TIMER_VALUE);
+	writel(TIMER_CTRL_32BIT | TIMER_CTRL_ENABLE | TIMER_CTRL_PERIODIC,
+		TIMER3_VA_BASE + TIMER_CTRL);
+
+	clocksource_realview.mult =
+		clocksource_khz2mult(1000, clocksource_realview.shift);
+	clocksource_register(&clocksource_realview);
+}
+
 /*
  * Set up timer interrupt, and return the current time in seconds.
  */
@@ -585,9 +573,10 @@ static void __init realview_timer_init(void)
 	 * Make irqs happen for the system timer
 	 */
 	setup_irq(IRQ_TIMERINT0_1, &realview_timer_irq);
+
+	realview_clocksource_init();
 }
 
 struct sys_timer realview_timer = {
 	.init		= realview_timer_init,
-	.offset		= realview_gettimeoffset,
 };