From a1c61563a0312c9c9f7c050e4a7588537585e49a Mon Sep 17 00:00:00 2001
From: Kevin O'Connor <kevin@koconnor.net>
Date: Fri, 31 Mar 2017 14:26:29 -0400
Subject: [PATCH] avr: Fix bug causing timer_read_time() to not be in sync with
 scheduler

Commit 16e3dbb1 changed the avr implementation of timer_read_time().
Unfortunately, it raised the possibility for timer_read_time() to be
out of sync with the scheduler's understanding of the current time.
In particular, it was common for the timer irq to overflow the 16bit
hardware counter once at startup, and this would lead to
timer_read_time() always returning a time ~4ms ahead of the scheduler
on 16Mhz chips.  This resulted in "Move queue empty" errors.

To resolve this issue, only increment timer_high from timer_periodic()
and make sure the timer irqs start immediately after timer_init().

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
---
 src/avr/timer.c | 29 +++++++++++++++++++----------
 1 file changed, 19 insertions(+), 10 deletions(-)

diff --git a/src/avr/timer.c b/src/avr/timer.c
index ac7509ed..c0a04624 100644
--- a/src/avr/timer.c
+++ b/src/avr/timer.c
@@ -70,6 +70,17 @@ timer_repeat_set(uint16_t next)
     TIFR1 = 1<<OCF1B;
 }
 
+// Reset the timer - clear settings and dispatch next timer immediately
+static void
+timer_reset(void)
+{
+    uint16_t now = timer_get();
+    timer_set(now + 50);
+    TIFR1 = 1<<OCF1A;
+    timer_repeat_set(now + 50);
+}
+DECL_SHUTDOWN(timer_reset);
+
 static void
 timer_init(void)
 {
@@ -85,8 +96,13 @@ timer_init(void)
     TCCR1A = 0;
     // Normal Mode
     TCCR1B = 1<<CS10;
+    // Setup for first irq
+    irqstatus_t flag = irq_save();
+    timer_reset();
+    TIFR1 = 1<<TOV1;
     // enable interrupt
     TIMSK1 = 1<<OCIE1A;
+    irq_restore(flag);
 }
 DECL_INIT(timer_init);
 
@@ -105,16 +121,9 @@ timer_read_time(void)
     union u32_u calc;
     calc.val = timer_get();
     calc.hi = timer_high;
-    if (likely(!(TIFR1 & (1<<TOV1)))) {
-        irq_restore(flag);
-        return calc.val;
-    }
-    // Hardware timer has overflowed - update overflow counter
-    TIFR1 = 1<<TOV1;
-    timer_high = calc.hi + 1;
-    irq_restore(flag);
-    if (calc.lo < 0x8000)
+    if (TIFR1 & (1<<TOV1) && calc.lo < 0x8000)
         calc.hi++;
+    irq_restore(flag);
     return calc.val;
 }
 
@@ -122,7 +131,7 @@ timer_read_time(void)
 void
 timer_periodic(void)
 {
-    if (unlikely(TIFR1 & (1<<TOV1))) {
+    if (TIFR1 & (1<<TOV1)) {
         // Hardware timer has overflowed - update overflow counter
         TIFR1 = 1<<TOV1;
         timer_high++;