1 files changed, 54 insertions, 0 deletions
diff --git a/stmhal/systick.c b/stmhal/systick.c
new file mode 100644
index 0000000000..55c22dab01
--- /dev/null
+++ b/stmhal/systick.c
@@ -0,0 +1,54 @@
+#include <stm32f4xx_hal.h>
+#include "misc.h"
+#include "systick.h"
+
+void sys_tick_init(void) {
+    // SysTick_Config is now called from HAL_RCC_ClockConfig, which is called
+    // from SystemClock_Config
+    HAL_NVIC_SetPriority(SysTick_IRQn, 0, 0); // make it highest priority
+}
+
+// called on SysTick interrupt
+void SysTick_Handler(void) {
+    HAL_IncTick();
+    HAL_SYSTICK_IRQHandler();
+    // hack!
+    //void audio_drain(void);
+    //audio_drain();
+}
+
+void sys_tick_delay_ms(uint32_t delay_ms) {
+    sys_tick_wait_at_least(HAL_GetTick(), delay_ms);
+}
+
+// waits until at least delay_ms milliseconds have passed from the sampling of stc
+// handles overflow properl
+// assumes stc was taken from HAL_GetTick() some time before calling this function
+// eg stc <= HAL_GetTick() for the case of no wrap around of HAL_GetTick()
+void sys_tick_wait_at_least(uint32_t stc, uint32_t delay_ms) {
+    // stc_wait is the value of HAL_GetTick() that we wait for
+    uint32_t stc_wait = stc + delay_ms;
+    if (stc_wait < stc) {
+        // stc_wait wrapped around
+        while (stc <= HAL_GetTick() || HAL_GetTick() < stc_wait) {
+            __WFI(); // enter sleep mode, waiting for interrupt
+        }
+    } else {
+        // stc_wait did not wrap around
+        while (stc <= HAL_GetTick() && HAL_GetTick() < stc_wait) {
+            __WFI(); // enter sleep mode, waiting for interrupt
+        }
+    }
+}
+
+bool sys_tick_has_passed(uint32_t stc, uint32_t delay_ms) {
+    // stc_wait is the value of HAL_GetTick() that we wait for
+    uint32_t stc_wait = stc + delay_ms;
+    if (stc_wait < stc) {
+        // stc_wait wrapped around
+        return !(stc <= HAL_GetTick() || HAL_GetTick() < stc_wait);
+    } else {
+        // stc_wait did not wrap around
+        return !(stc <= HAL_GetTick() && HAL_GetTick() < stc_wait);
+    }
+}