i.MX31: Add reset_timer() and modify get_timer_masked().

This patch adds the reset_timer() function (needed by nand_base.c) and
modifies the get_timer_masked() to work in the same way as the omap24xx
function.

Signed-off-by: Magnus Lilja <lilja.magnus@gmail.com>

1 files changed, 20 insertions, 4 deletions

diff --git a/cpu/arm1136/mx31/interrupts.c b/cpu/arm1136/mx31/interrupts.c
index 21b77a544a..6e08c71a10 100644
--- a/cpu/arm1136/mx31/interrupts.c
+++ b/cpu/arm1136/mx31/interrupts.c
@@ -38,6 +38,9 @@
 #define GPTCR_CLKSOURCE_32 (4<<6)  /* Clock source */
 #define GPTCR_TEN       (1)     /* Timer enable */
 
+static ulong timestamp;
+static ulong lastinc;
+
 /* nothing really to do with interrupts, just starts up a counter. */
 int interrupt_init (void)
 {
@@ -54,14 +57,27 @@ int interrupt_init (void)
 
 void reset_timer_masked (void)
 {
-	GPTCR = 0;
-	GPTCR = GPTCR_CLKSOURCE_32 | GPTCR_TEN; /* Freerun Mode, PERCLK1 input */
+	/* reset time */
+	lastinc = GPTCNT; /* capture current incrementer value time */
+	timestamp = 0; /* start "advancing" time stamp from 0 */
+}
+
+void reset_timer(void)
+{
+	reset_timer_masked();
 }
 
 ulong get_timer_masked (void)
 {
-	ulong val = GPTCNT;
-	return val;
+	ulong now = GPTCNT; /* current tick value */
+
+	if (now >= lastinc)	/* normal mode (non roll) */
+		/* move stamp forward with absolut diff ticks */
+		timestamp += (now - lastinc);
+	else			/* we have rollover of incrementer */
+		timestamp += (0xFFFFFFFF - lastinc) + now;
+	lastinc = now;
+	return timestamp;
 }
 
 ulong get_timer (ulong base)