3 files changed, 44 insertions, 61 deletions

diff --git a/wirish/time.c b/wirish/time.c
index 69e962c..c0fd03e 100644
--- a/wirish/time.c
+++ b/wirish/time.c
@@ -23,8 +23,6 @@
  * ****************************************************************************/
 
 /**
- *  @file time.c
- *
  *  @brief 
  */
 
@@ -32,46 +30,23 @@
 #include "systick.h"
 #include "time.h"
 
-#define CYCLES_PER_MICROSECOND  72
-#define FUDGE                   42
-
-extern volatile uint32 systick_timer_millis;
-
-uint32 millis() {
-   unsigned long m;
-   m = systick_timer_millis;
-   return m;
-}
-
 void delay(unsigned long ms)
 {
-   unsigned long start = millis();
-
-   while (millis() - start <= ms)
-      ;
+   uint32 i;
+   for (i = 0; i < ms; i++) {
+      delayMicroseconds(1000);
+   }
 }
 
-
-
-#if 1
 void delayMicroseconds(uint32 us) {
-    uint32 target;
-    uint32 last, cur, count;
-    /* fudge factor hacky hack hack for function overhead  */
-    target = us * CYCLES_PER_MICROSECOND - FUDGE;
-
-    /* Get current count */
-    last = systick_get_count();
-    cur = systick_get_count();
-    count = last;
-    while ((count-cur) <= target) {
-        cur = systick_get_count();
-
-        /* check for overflow  */
-        if (cur > last) {
-            count += MAPLE_RELOAD_VAL;
-        }
-        last = cur;
-    }
+    us *= 12;
+
+    /* fudge for function call overhead  */
+    us--;
+    asm volatile("   mov r0, %[us]          \n\t"
+                 "1: subs r0, #1            \n\t"
+                 "   bhi 1b                 \n\t"
+                 :
+                 : [us] "r" (us)
+                 : "r0");
 }
-#endif
diff --git a/wirish/time.h b/wirish/time.h
index 9e5536b..45e82dc 100644
--- a/wirish/time.h
+++ b/wirish/time.h
@@ -23,8 +23,6 @@
  * ****************************************************************************/
 
 /**
- *  @file time.h
- *
  *  @brief 
  */
 
@@ -34,32 +32,42 @@
 #ifdef __cplusplus
 extern "C"{
 #endif
-/* Returns time since boot in milliseconds  */
-uint32 millis(void);
+
+#include "nvic.h"
+#include "systick.h"
+
+#define CYCLES_PER_MICROSECOND  72
+#define US_PER_MS               1000
+#define MAPLE_RELOAD_VAL        72000
+
+extern volatile uint32 systick_timer_millis;
+
+/* time in milliseconds since boot  */
+static inline uint32 millis(void) {
+   return systick_timer_millis;
+}
 
 /* Time in microseconds since boot  */
-uint32 micros(void);
+static inline uint32 micros(void) {
+   uint32 ms;
+   uint32 cycle_cnt;
+   uint32 res;
 
-/* Delay for ms milliseconds  */
-void delay(unsigned long ms);
+   nvic_globalirq_disable();
 
-/* Delay for us microseconds  */
-void delayMicroseconds(uint32 us);
+   cycle_cnt = systick_get_count();
+   ms = millis();
+
+   nvic_globalirq_enable();
 
-#if 0
-static inline void delay_us(uint32 us) {
-    us *= 12;
-    asm volatile("mov  r0, %[us]        \n\t"
-                 "subs r0, #2 \n\t"
-"1:                                    \n\t"
-                  "subs r0, r0, #1           \n\t"
-                  "bne 1b"
-                 :
-                 : [us] "r" (us)
-                 : "r0", "cc");
+   res = (ms * US_PER_MS) + (MAPLE_RELOAD_VAL - cycle_cnt)/CYCLES_PER_MICROSECOND;
 
+   return res;
 }
-#endif
+
+void delay(unsigned long ms);
+void delayMicroseconds(uint32 us);
+
 #ifdef __cplusplus
 } // extern "C"
 #endif
diff --git a/wirish/wirish.c b/wirish/wirish.c
index 1938ec9..4281875 100644
--- a/wirish/wirish.c
+++ b/wirish/wirish.c
@@ -53,7 +53,7 @@ void init(void) {
    rcc_set_prescaler(RCC_PRESCALER_APB2, RCC_APB2_HCLK_DIV_1);
 
    nvic_init();
-   systick_init();
+   systick_init(MAPLE_RELOAD_VAL);
    gpio_init();
    adc_init();
    timer_init(1, 1);