1 files changed, 41 insertions, 8 deletions

diff --git a/cpu/nios2/interrupts.c b/cpu/nios2/interrupts.c
index 1c3566e..b552db4 100644
--- a/cpu/nios2/interrupts.c
+++ b/cpu/nios2/interrupts.c
@@ -56,7 +56,40 @@ volatile ulong timestamp = 0;
 
 void reset_timer (void)
 {
+	nios_timer_t *tmr =(nios_timer_t *)CONFIG_SYS_NIOS_TMRBASE;
+
+	/* From Embedded Peripherals Handbook:
+	 *
+	 * "When the hardware is configured with Writeable period
+	 * disabled, writing to one of the period_n registers causes
+	 * the counter to reset to the fixed Timeout Period specified
+	 * at system generation time."
+	 *
+	 * Here we force a reload to prevent early timeouts from
+	 * get_timer() when the interrupt period is greater than
+	 * than 1 msec.
+	 *
+	 * Simply write to periodl with its own value to force an
+	 * internal counter reload, THEN reset the timestamp.
+	 */
+	writel (readl (&tmr->periodl), &tmr->periodl);
 	timestamp = 0;
+
+	/* From Embedded Peripherals Handbook:
+	 *
+	 * "Writing to one of the period_n registers stops the internal
+	 * counter, except when the hardware is configured with Start/Stop
+	 * control bits off. If Start/Stop control bits is off, writing
+	 * either register does not stop the counter."
+	 *
+	 * In order to accomodate either configuration, the control
+	 * register is re-written. If the counter is stopped, it will
+	 * be restarted. If it is running, the write is essentially
+	 * a nop.
+	 */
+	writel (NIOS_TIMER_ITO | NIOS_TIMER_CONT | NIOS_TIMER_START,
+			&tmr->control);
+
 }
 
 ulong get_timer (ulong base)
@@ -81,7 +114,7 @@ void tmr_isr (void *arg)
 	/* Interrupt is cleared by writing anything to the
 	 * status register.
 	 */
-	writel (&tmr->status, 0);
+	writel (0, &tmr->status);
 	timestamp += CONFIG_SYS_NIOS_TMRMS;
 #ifdef CONFIG_STATUS_LED
 	status_led_tick(timestamp);
@@ -92,16 +125,16 @@ static void tmr_init (void)
 {
 	nios_timer_t *tmr =(nios_timer_t *)CONFIG_SYS_NIOS_TMRBASE;
 
-	writel (&tmr->status, 0);
-	writel (&tmr->control, 0);
-	writel (&tmr->control, NIOS_TIMER_STOP);
+	writel (0, &tmr->status);
+	writel (0, &tmr->control);
+	writel (NIOS_TIMER_STOP, &tmr->control);
 
 #if defined(CONFIG_SYS_NIOS_TMRCNT)
-	writel (&tmr->periodl, CONFIG_SYS_NIOS_TMRCNT & 0xffff);
-	writel (&tmr->periodh, (CONFIG_SYS_NIOS_TMRCNT >> 16) & 0xffff);
+	writel (CONFIG_SYS_NIOS_TMRCNT & 0xffff, &tmr->periodl);
+	writel ((CONFIG_SYS_NIOS_TMRCNT >> 16) & 0xffff, &tmr->periodh);
 #endif
-	writel (&tmr->control, NIOS_TIMER_ITO | NIOS_TIMER_CONT |
-			  NIOS_TIMER_START );
+	writel (NIOS_TIMER_ITO | NIOS_TIMER_CONT | NIOS_TIMER_START,
+			&tmr->control);
 	irq_install_handler (CONFIG_SYS_NIOS_TMRIRQ, tmr_isr, (void *)tmr);
 }