From 34ec77c7fdb29afaca981624153fa0f16350c632 Mon Sep 17 00:00:00 2001
From: Joey Castillo <jose.castillo@gmail.com>
Date: Sun, 29 Aug 2021 15:50:46 -0400
Subject: handle changing CPU speed

---
 watch-library/hpl/core/hpl_core_m0plus_base.c |  9 +++++++--
 watch-library/watch/watch.c                   | 19 +++++++++++++++++++
 watch-library/watch/watch.h                   |  3 +++
 watch-library/watch/watch_uart.c              | 19 +------------------
 4 files changed, 30 insertions(+), 20 deletions(-)

diff --git a/watch-library/hpl/core/hpl_core_m0plus_base.c b/watch-library/hpl/core/hpl_core_m0plus_base.c
index 1d32300a..4a79ac3e 100644
--- a/watch-library/hpl/core/hpl_core_m0plus_base.c
+++ b/watch-library/hpl/core/hpl_core_m0plus_base.c
@@ -41,6 +41,7 @@
 #endif
 #include <utils_assert.h>
 #include <peripheral_clk_config.h>
+#include "watch.h"
 
 #ifndef CONF_CPU_FREQUENCY
 #define CONF_CPU_FREQUENCY 1000000
@@ -167,7 +168,9 @@ static inline uint32_t _get_cycles_for_us_internal(const uint16_t us, const uint
  */
 uint32_t _get_cycles_for_us(const uint16_t us)
 {
-	return _get_cycles_for_us_internal(us, CONF_CPU_FREQUENCY, CPU_FREQ_POWER);
+    uint32_t freq = watch_get_cpu_speed();
+    if (freq > 10000000) return _get_cycles_for_us_internal(us, freq, 8);
+    else return _get_cycles_for_us_internal(us, freq, 7);
 }
 
 /**
@@ -196,5 +199,7 @@ static inline uint32_t _get_cycles_for_ms_internal(const uint16_t ms, const uint
  */
 uint32_t _get_cycles_for_ms(const uint16_t ms)
 {
-	return _get_cycles_for_ms_internal(ms, CONF_CPU_FREQUENCY, CPU_FREQ_POWER);
+    uint32_t freq = watch_get_cpu_speed();
+    if (freq > 10000000) return _get_cycles_for_ms_internal(ms, freq, 8);
+    else return _get_cycles_for_ms_internal(ms, freq, 7);
 }
diff --git a/watch-library/watch/watch.c b/watch-library/watch/watch.c
index 059c2a93..e3a6c4d5 100644
--- a/watch-library/watch/watch.c
+++ b/watch-library/watch/watch.c
@@ -35,3 +35,22 @@
 #include "watch_uart.c"
 #include "watch_deepsleep.c"
 #include "watch_private.c"
+
+uint32_t watch_get_cpu_speed() {
+    uint8_t fsel = hri_oscctrl_get_OSC16MCTRL_FSEL_bf(OSCCTRL, OSCCTRL_OSC16MCTRL_MASK);
+    switch (fsel) {
+        case OSCCTRL_OSC16MCTRL_FSEL_4_Val:
+            return 4000000;
+            break;
+        case OSCCTRL_OSC16MCTRL_FSEL_8_Val:
+            return 8000000;
+            break;
+        case OSCCTRL_OSC16MCTRL_FSEL_12_Val:
+            return 12000000;
+            break;
+        case OSCCTRL_OSC16MCTRL_FSEL_16_Val:
+            return 16000000;
+            break;
+    }
+    return 0;
+}
diff --git a/watch-library/watch/watch.h b/watch-library/watch/watch.h
index 8c207ddb..0b27c515 100644
--- a/watch-library/watch/watch.h
+++ b/watch-library/watch/watch.h
@@ -65,4 +65,7 @@
 
 #include "watch_private.h"
 
+/// @brief gets the current CPU speed
+uint32_t watch_get_cpu_speed();
+
 #endif /* WATCH_H_ */
\ No newline at end of file
diff --git a/watch-library/watch/watch_uart.c b/watch-library/watch/watch_uart.c
index afebff1b..1ab888bd 100644
--- a/watch-library/watch/watch_uart.c
+++ b/watch-library/watch/watch_uart.c
@@ -53,24 +53,7 @@
 #include "peripheral_clk_config.h"
 
 void watch_enable_debug_uart(uint32_t baud) {
-    uint8_t fsel = hri_oscctrl_get_OSC16MCTRL_FSEL_bf(OSCCTRL, OSCCTRL_OSC16MCTRL_MASK);
-    uint32_t freq = 0;
-    switch (fsel) {
-        case OSCCTRL_OSC16MCTRL_FSEL_4_Val:
-            freq = 4000000;
-            break;
-        case OSCCTRL_OSC16MCTRL_FSEL_8_Val:
-            freq = 8000000;
-            break;
-        case OSCCTRL_OSC16MCTRL_FSEL_12_Val:
-            freq = 12000000;
-            break;
-        case OSCCTRL_OSC16MCTRL_FSEL_16_Val:
-            freq = 16000000;
-            break;
-        default:
-            return;
-    }
+    uint32_t freq = watch_get_cpu_speed();
     uint64_t br = (uint64_t)65536 * ((freq * 4) - 16 * baud) / (freq * 4);
 
     gpio_set_pin_direction(D1, GPIO_DIRECTION_IN);
-- 
cgit v1.2.3