ARM: plat-nomadik:timer: Add delay timer support

Change-Id: If518e847cf391d1d9dbc4a0d0bca8336d66de88f
Signed-off-by: Jonas Aaberg <jonas.aberg@stericsson.com>
Reviewed-on: http://gerrit.lud.stericsson.com/gerrit/32049

1 files changed, 27 insertions, 0 deletions

diff --git a/arch/arm/plat-nomadik/timer.c b/arch/arm/plat-nomadik/timer.c
index 9222e5522a4..a40620ce675 100644
--- a/arch/arm/plat-nomadik/timer.c
+++ b/arch/arm/plat-nomadik/timer.c
@@ -17,6 +17,7 @@
 #include <linux/clk.h>
 #include <linux/jiffies.h>
 #include <linux/err.h>
+#include <linux/delay.h>
 #include <asm/mach/time.h>
 #include <asm/sched_clock.h>
 
@@ -142,6 +143,28 @@ static struct clock_event_device nmdk_clkevt = {
 	.set_next_event	= nmdk_clkevt_next,
 };
 
+#ifdef ARCH_HAS_READ_CURRENT_TIMER
+static void nmdk_timer_delay_loop(unsigned long loops)
+{
+	unsigned long bclock, now;
+
+	bclock = ~readl(mtu_base + MTU_VAL(0));
+	do {
+		now = ~readl(mtu_base + MTU_VAL(0));
+		/* If timer have been cleared (suspend) or wrapped we exit */
+		if (unlikely(now < bclock))
+			return;
+	} while ((now - bclock) < loops);
+}
+
+/* Used to calibrate the delay */
+int read_current_timer(unsigned long *timer_val)
+{
+	*timer_val = ~readl(mtu_base + MTU_VAL(0));
+	return 0;
+}
+#endif
+
 /*
  * IRQ Handler for timer 1 of the MTU block.
  */
@@ -220,4 +243,8 @@ void __init nmdk_timer_init(void __iomem *base)
 	setup_irq(IRQ_MTU0, &nmdk_timer_irq);
 	nmdk_clkevt.cpumask = cpumask_of(0);
 	clockevents_config_and_register(&nmdk_clkevt, rate, 2, 0xffffffffU);
+#ifdef ARCH_HAS_READ_CURRENT_TIMER
+	set_delay_fn(nmdk_timer_delay_loop);
+#endif
+
 }