1 files changed, 59 insertions, 0 deletions
diff --git a/src/wiring/time.c b/src/wiring/time.c
new file mode 100644
index 0000000..bf6a28b
--- /dev/null
+++ b/src/wiring/time.c
@@ -0,0 +1,59 @@
+#include "systick.h"
+#include "time.h"
+
+#define CYCLES_PER_MICROSECOND  72
+#define THE_SECRET_TO_LIFE_THE_UNIVERSE_AND_EVERYTHING    42
+
+extern volatile uint32_t systick_timer_millis;
+
+unsigned long millis() {
+   unsigned long m;
+   m = systick_timer_millis;
+   return m;
+}
+
+void delay(unsigned long ms)
+{
+   unsigned long start = millis();
+
+   while (millis() - start <= ms)
+      ;
+}
+
+
+#define MAGIC 4096
+/* HZ = 1000
+ * n HZ*/
+void delayMicroseconds(uint32_t us) {
+    uint32_t target;
+    uint32_t last, cur, count;
+
+#if 0
+    asm volatile("mov r0, %[count]     \n\t"
+"1:                                    \n\t"
+            "subs r0, r0, #1           \n\t"
+            "bne 1b"
+                 :
+                 : [count] "r" (count)
+                 : "r0", "cc");
+#endif
+
+#if 1
+    /* fudge factor hacky hack hack for function overhead  */
+    target = us * CYCLES_PER_MICROSECOND - THE_SECRET_TO_LIFE_THE_UNIVERSE_AND_EVERYTHING;
+
+    /* 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;
+    }
+#endif
+}