delay() with micros()

Author: nerdralph

src/ArduinoShrink.h

@@ -12,18 +12,6 @@ inline void delayMicroseconds(unsigned int us)
     _delay_us(us);
 }
 
-extern "C" uint32_t micros_raw();
-inline uint32_t micros()
-{
-    /*
-    register uint32_t micros_ asm("r22");
-    asm ("call micros_raw" : "=r" (micros_) );
-    return micros_;
-    */
-    // asm volatile ("call micros_raw");
-    return micros_raw() * 4;
-}
-
 extern "C" uint32_t millis_raw();
 inline uint32_t millis()
 {

src/millis.c

@@ -8,6 +8,14 @@ struct {
     uint8_t ovfl_pad;
 } t0_millis;
 
+extern uint32_t micros_raw();
+uint32_t micros()
+{
+    register uint32_t u asm("r22");
+    asm ("%~call %x1" : "=r" (u) : "i"(micros_raw) : "r30", "r31" );
+    return u * 4;
+}
+
 // portability macros for mega8
 #ifndef TCCR0B
 #define TCCR0B TCCR0

src/wiring.c

@@ -4,13 +4,29 @@
 #include <Arduino.h>
 #include <util/delay.h>
 
+/*
 __attribute ((noinline))
 void delay(unsigned long ms)
 {
     do {
         _delay_us(999);                 // leave 1us for loop overhead
     } while (--ms);
 }
+*/
+
+void delay(unsigned long ms)
+{
+    // lower 16-bits of micros is enough considering wraparound
+    uint16_t start = micros();
+
+    while (ms > 0) {
+        yield();
+        while (ms > 0 && ((uint16_t)micros() - start) >= 1000) {
+            ms --;
+            start += 1000;
+        }
+    }
+}
 
 // avr-libc math.h does not declare gcc log2 builtin
 double log2(double);