From 4dfa6c6ee454d14a006033e218acbb7fa9ced8bd Mon Sep 17 00:00:00 2001
From: Kevin O'Connor <kevin@koconnor.net>
Date: Fri, 24 Mar 2017 23:17:23 -0400
Subject: [PATCH] avr: Introduce optimized timer_is_before()

Provide hand-coded assembler for timer_is_before() on AVR as that code
is used frequently in the time-critical timer dispatch loop and gcc
doesn't do a good job at compiling that comparison code.  Remove the
no longer needed waketime+1 hack from reschedule_timer().

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
---
 src/avr/timer.c | 25 +++++++++++++++++++------
 src/sched.c     |  6 +++---
 2 files changed, 22 insertions(+), 9 deletions(-)

diff --git a/src/avr/timer.c b/src/avr/timer.c
index e96b88bf4..f1fbd284c 100644
--- a/src/avr/timer.c
+++ b/src/avr/timer.c
@@ -25,13 +25,29 @@ timer_from_us(uint32_t us)
     return us * (F_CPU / 1000000);
 }
 
+union u32_u {
+    struct { uint8_t b0, b1, b2, b3; };
+    struct { uint16_t lo, hi; };
+    uint32_t val;
+};
+
 // Return true if time1 is before time2.  Always use this function to
 // compare times as regular C comparisons can fail if the counter
 // rolls over.
-uint8_t
+uint8_t __always_inline
 timer_is_before(uint32_t time1, uint32_t time2)
 {
-    return (int32_t)(time1 - time2) < 0;
+    // This asm is equivalent to:
+    //     return (int32_t)(time1 - time2) < 0;
+    // But gcc doesn't do a good job with the above, so it's hand coded.
+    union u32_u utime1 = { .val = time1 };
+    uint8_t f = utime1.b3;
+    asm("    cp  %A1, %A2\n"
+        "    cpc %B1, %B2\n"
+        "    cpc %C1, %C2\n"
+        "    sbc %0,  %D2"
+        : "+r"(f) : "r"(time1), "r"(time2));
+    return (int8_t)f < 0;
 }
 
 static inline uint16_t
@@ -99,11 +115,8 @@ static uint16_t timer_high;
 uint32_t
 timer_read_time(void)
 {
-    union u32_u16_u {
-        struct { uint16_t lo, hi; };
-        uint32_t val;
-    } calc;
     irqstatus_t flag = irq_save();
+    union u32_u calc;
     calc.val = timer_get();
     calc.hi = timer_high;
     if (!(TIFR1 & (1<<TOV1))) {
diff --git a/src/sched.c b/src/sched.c
index 7a68db861..cae2f819d 100644
--- a/src/sched.c
+++ b/src/sched.c
@@ -127,9 +127,9 @@ sched_del_timer(struct timer *del)
 static struct timer *
 reschedule_timer(struct timer *t)
 {
+    uint32_t waketime = t->waketime;
     struct timer *pos = t->next;
-    uint32_t minwaketime = t->waketime + 1;
-    if (!timer_is_before(pos->waketime, minwaketime))
+    if (timer_is_before(waketime, pos->waketime))
         // Timer is still the first - no insertion needed
         return t;
 
@@ -142,7 +142,7 @@ reschedule_timer(struct timer *t)
             // micro optimization for AVR - reduces register pressure
             asm("" : "+r"(prev) : : "memory");
         pos = pos->next;
-        if (!timer_is_before(pos->waketime, minwaketime))
+        if (timer_is_before(waketime, pos->waketime))
             break;
     }
     t->next = pos;