From ad0971fb25347a8cc36ace2c45449ef7eac6995e Mon Sep 17 00:00:00 2001
From: Lucian Copeland <hierophect@gmail.com>
Date: Fri, 5 Jun 2020 11:42:34 -0400
Subject: [PATCH] Override HAL_Delay and HAL_GetTick

---
 .../peripherals/stm32f4/stm32f401xe/clocks.h  |  2 +-
 .../peripherals/stm32f4/stm32f411xe/clocks.h  |  2 +-
 .../peripherals/stm32f4/stm32f412zx/clocks.h  |  2 +-
 ports/stm/peripherals/stm32h7/clocks.c        |  2 +-
 ports/stm/supervisor/port.c                   | 32 +++++++++++++++++--
 5 files changed, 33 insertions(+), 7 deletions(-)

diff --git a/ports/stm/peripherals/stm32f4/stm32f401xe/clocks.h b/ports/stm/peripherals/stm32f4/stm32f401xe/clocks.h
index be62370d88..d157d944a3 100644
--- a/ports/stm/peripherals/stm32f4/stm32f401xe/clocks.h
+++ b/ports/stm/peripherals/stm32f4/stm32f401xe/clocks.h
@@ -37,7 +37,7 @@
 #ifndef CPY_CLK_PLLN
 #define CPY_CLK_PLLN (336)
 #endif
-#ifndef (CPY_CLK_PLLP
+#ifndef CPY_CLK_PLLP
 #define CPY_CLK_PLLP (RCC_PLLP_DIV4)
 #endif
 #ifndef CPY_CLK_PLLQ
diff --git a/ports/stm/peripherals/stm32f4/stm32f411xe/clocks.h b/ports/stm/peripherals/stm32f4/stm32f411xe/clocks.h
index adb60e8a9e..a2fb7bd544 100644
--- a/ports/stm/peripherals/stm32f4/stm32f411xe/clocks.h
+++ b/ports/stm/peripherals/stm32f4/stm32f411xe/clocks.h
@@ -32,7 +32,7 @@
 
 // Note - the actual maximum frequency is 100MHz, but this requires divisors
 // which are incompatible with USB, and there is no additional PLL such as on
-// the F412. 
+// the F412.
 
 // Defaults:
 #ifndef CPY_CLK_VSCALE
diff --git a/ports/stm/peripherals/stm32f4/stm32f412zx/clocks.h b/ports/stm/peripherals/stm32f4/stm32f412zx/clocks.h
index 8f092adbac..e1355c8f34 100644
--- a/ports/stm/peripherals/stm32f4/stm32f412zx/clocks.h
+++ b/ports/stm/peripherals/stm32f4/stm32f412zx/clocks.h
@@ -30,7 +30,7 @@
 // Line Type:           Access Line
 // Speed:               200MHz (MAX)
 
-// Note - uses the I2S PLL for SUSB to enable full 100MHz operation, since USB 
+// Note - uses the I2S PLL for SUSB to enable full 100MHz operation, since USB
 // can't get the right divisors from 100MHz PLL settings.
 
 // Defaults:
diff --git a/ports/stm/peripherals/stm32h7/clocks.c b/ports/stm/peripherals/stm32h7/clocks.c
index d68137df0f..0e4e79f9f7 100644
--- a/ports/stm/peripherals/stm32h7/clocks.c
+++ b/ports/stm/peripherals/stm32h7/clocks.c
@@ -120,4 +120,4 @@ void stm32_peripherals_clocks_init(void) {
     if (lse_failure) {
         reset_into_safe_mode(HARD_CRASH); //TODO: make safe mode category CLOCK_FAULT?
     }
-}
\ No newline at end of file
+}
diff --git a/ports/stm/supervisor/port.c b/ports/stm/supervisor/port.c
index 8156b2272a..25504ddece 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) {