Override HAL_Delay and HAL_GetTick

1 files changed, 29 insertions, 3 deletions

diff --git a/ports/stm/supervisor/port.c b/ports/stm/supervisor/port.c
index 8156b2272..25504ddec 100644
--- a/ports/stm/supervisor/port.c
+++ b/ports/stm/supervisor/port.c
@@ -147,9 +147,10 @@ __attribute__((used, naked)) void Reset_Handler(void) {
     __enable_irq();
     main();
 }
-
 #endif //end H7 specific code
 
+// Low power clock variables
+static volatile uint32_t systick_ms;
 static RTC_HandleTypeDef _hrtc;
 
 #if BOARD_HAS_LOW_SPEED_CRYSTAL
@@ -159,7 +160,7 @@ static uint32_t rtc_clock_frequency = LSI_VALUE;
 #endif
 
 safe_mode_t port_init(void) {
-    HAL_Init();
+    HAL_Init(); // Turns on SysTick
     __HAL_RCC_SYSCFG_CLK_ENABLE();
 
     #if (CPY_STM32F4)
@@ -182,13 +183,38 @@ safe_mode_t port_init(void) {
     HAL_RTC_Init(&_hrtc);
     HAL_NVIC_EnableIRQ(RTC_Alarm_IRQn);
 
+    // Turn off SysTick
+    SysTick->CTRL = 0;
+
     return NO_SAFE_MODE;
 }
 
+void HAL_Delay(uint32_t delay_ms) {
+    if (SysTick->CTRL != 0) {
+        // SysTick is on, so use it
+        uint32_t tickstart = systick_ms;
+        while (systick_ms - tickstart < delay_ms) {
+        }
+    } else {
+        mp_hal_delay_ms(delay_ms);
+    }
+}
+
+uint32_t HAL_GetTick() {
+    if (SysTick->CTRL != 0) {
+        return systick_ms;
+    } else {
+        uint8_t subticks;
+        uint32_t result = (uint32_t)port_get_raw_ticks(&subticks);
+        return result;
+    }
+}
+
+
 void SysTick_Handler(void) {
+    systick_ms += 1;
     // Read the CTRL register to clear the SysTick interrupt.
     SysTick->CTRL;
-    HAL_IncTick();
 }
 
 void reset_port(void) {