1 files changed, 50 insertions, 36 deletions

diff --git a/arch/arm/mach-imx/time.c b/arch/arm/mach-imx/time.c
index 8ae4a2c5066f..40039b2a90b3 100644
--- a/arch/arm/mach-imx/time.c
+++ b/arch/arm/mach-imx/time.c
@@ -14,6 +14,7 @@
 #include <linux/interrupt.h>
 #include <linux/irq.h>
 #include <linux/time.h>
+#include <linux/clocksource.h>
 
 #include <asm/hardware.h>
 #include <asm/io.h>
@@ -24,33 +25,7 @@
 /* Use timer 1 as system timer */
 #define TIMER_BASE IMX_TIM1_BASE
 
-/*
- * Returns number of us since last clock interrupt.  Note that interrupts
- * will have been disabled by do_gettimeoffset()
- */
-static unsigned long imx_gettimeoffset(void)
-{
-	unsigned long ticks;
-
-	/*
-	 * 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.
-	 */
-	ticks = IMX_TCN(TIMER_BASE);
-
-	/*
-	 * Interrupt pending?  If so, we've reloaded once already.
-	 */
-	if (IMX_TSTAT(TIMER_BASE) & TSTAT_COMP)
-		ticks += LATCH;
-
-	/*
-	 * Convert the ticks to usecs
-	 */
-	return (1000000 / CLK32) * ticks;
-}
+static unsigned long evt_diff;
 
 /*
  * IRQ handler for the timer
@@ -58,14 +33,23 @@ static unsigned long imx_gettimeoffset(void)
 static irqreturn_t
 imx_timer_interrupt(int irq, void *dev_id)
 {
-	write_seqlock(&xtime_lock);
+	uint32_t tstat;
 
 	/* clear the interrupt */
-	if (IMX_TSTAT(TIMER_BASE))
-		IMX_TSTAT(TIMER_BASE) = 0;
+	tstat = IMX_TSTAT(TIMER_BASE);
+	IMX_TSTAT(TIMER_BASE) = 0;
+
+	if (tstat & TSTAT_COMP) {
+		do {
+
+			write_seqlock(&xtime_lock);
+			timer_tick();
+			write_sequnlock(&xtime_lock);
+			IMX_TCMP(TIMER_BASE) += evt_diff;
 
-	timer_tick();
-	write_sequnlock(&xtime_lock);
+		} while (unlikely((int32_t)(IMX_TCMP(TIMER_BASE)
+					- IMX_TCN(TIMER_BASE)) < 0));
+	}
 
 	return IRQ_HANDLED;
 }
@@ -77,9 +61,9 @@ static struct irqaction imx_timer_irq = {
 };
 
 /*
- * Set up timer interrupt, and return the current time in seconds.
+ * Set up timer hardware into expected mode and state.
  */
-static void __init imx_timer_init(void)
+static void __init imx_timer_hardware_init(void)
 {
 	/*
 	 * Initialise to a known state (all timers off, and timing reset)
@@ -87,7 +71,38 @@ static void __init imx_timer_init(void)
 	IMX_TCTL(TIMER_BASE) = 0;
 	IMX_TPRER(TIMER_BASE) = 0;
 	IMX_TCMP(TIMER_BASE) = LATCH - 1;
-	IMX_TCTL(TIMER_BASE) = TCTL_CLK_32 | TCTL_IRQEN | TCTL_TEN;
+
+	IMX_TCTL(TIMER_BASE) = TCTL_FRR | TCTL_CLK_PCLK1 | TCTL_IRQEN | TCTL_TEN;
+	evt_diff = LATCH;
+}
+
+cycle_t imx_get_cycles(void)
+{
+	return IMX_TCN(TIMER_BASE);
+}
+
+static struct clocksource clocksource_imx = {
+	.name 		= "imx_timer1",
+	.rating		= 200,
+	.read		= imx_get_cycles,
+	.mask		= 0xFFFFFFFF,
+	.shift 		= 20,
+	.is_continuous 	= 1,
+};
+
+static int __init imx_clocksource_init(void)
+{
+	clocksource_imx.mult =
+		clocksource_hz2mult(imx_get_perclk1(), clocksource_imx.shift);
+	clocksource_register(&clocksource_imx);
+
+	return 0;
+}
+
+static void __init imx_timer_init(void)
+{
+	imx_timer_hardware_init();
+	imx_clocksource_init();
 
 	/*
 	 * Make irqs happen for the system timer
@@ -97,5 +112,4 @@ static void __init imx_timer_init(void)
 
 struct sys_timer imx_timer = {
 	.init		= imx_timer_init,
-	.offset		= imx_gettimeoffset,
 };