stm32: Unify enable_pclock() code

Unify the handling of the enable_pclock() and is_enabled_pclock() code
across all stm32 chips.  All chips will now perform a peripheral reset
on enable_pclock() (this is a change for stm32f0 and stm32h7).  The
enable_pclock() code will now also disable irqs during the enable.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>

1 files changed, 27 insertions, 68 deletions

diff --git a/src/stm32/stm32h7.c b/src/stm32/stm32h7.c
index 0e4094c1..ebb71cfb 100644
--- a/src/stm32/stm32h7.c
+++ b/src/stm32/stm32h7.c
@@ -7,7 +7,7 @@
 #include "autoconf.h" // CONFIG_CLOCK_REF_FREQ
 #include "board/armcm_boot.h" // VectorTable
 #include "command.h" // DECL_CONSTANT_STR
-#include "internal.h" // enable_pclock
+#include "internal.h" // get_pclock_frequency
 #include "sched.h" // sched_main
 
 
@@ -17,75 +17,34 @@
 
 #define FREQ_PERIPH (CONFIG_CLOCK_FREQ / 4)
 
-// Enable a peripheral clock
-void
-enable_pclock(uint32_t periph_base)
-{
-    // periph_base determines in which bitfield at wich position to set a bit
-    // E.g. D2_AHB1PERIPH_BASE is the adress offset of the given bitfield
-    if (periph_base < D2_APB2PERIPH_BASE) {
-        uint32_t pos = (periph_base - D2_APB1PERIPH_BASE) / 0x400;
-        RCC->APB1LENR |= (1<<pos); // we assume it is not in APB1HENR
-        RCC->APB1LENR;
-    } else if (periph_base < D2_AHB1PERIPH_BASE) {
-        uint32_t pos = (periph_base - D2_APB2PERIPH_BASE) / 0x400;
-        RCC->APB2ENR |= (1<<pos);
-        RCC->APB2ENR;
-    } else if (periph_base < D2_AHB2PERIPH_BASE) {
-        uint32_t pos = (periph_base - D2_AHB1PERIPH_BASE) / 0x400;
-        RCC->AHB1ENR |= (1<<pos);
-        RCC->AHB1ENR;
-    } else if (periph_base < D1_APB1PERIPH_BASE) {
-        uint32_t pos = (periph_base - D2_AHB2PERIPH_BASE) / 0x400;
-        RCC->AHB2ENR |= (1<<pos);
-        RCC->AHB2ENR;
-    } else if (periph_base < D1_AHB1PERIPH_BASE) {
-        uint32_t pos = (periph_base - D1_APB1PERIPH_BASE) / 0x400;
-        RCC->APB3ENR |= (1<<pos);
-        RCC->APB3ENR;
-    } else if (periph_base < D3_APB1PERIPH_BASE) {
-        uint32_t pos = (periph_base - D1_AHB1PERIPH_BASE) / 0x400;
-        RCC->AHB3ENR |= (1<<pos);
-        RCC->AHB3ENR;
-    } else if (periph_base < D3_AHB1PERIPH_BASE) {
-        uint32_t pos = (periph_base - D3_APB1PERIPH_BASE) / 0x400;
-        RCC->APB4ENR |= (1<<pos);
-        RCC->APB4ENR;
-    } else {
-        uint32_t pos = (periph_base - D3_AHB1PERIPH_BASE) / 0x400;
-        RCC->AHB4ENR |= (1<<pos);
-        RCC->AHB4ENR;
-    }
-}
-
-// Check if a peripheral clock has been enabled
-int
-is_enabled_pclock(uint32_t periph_base)
+// Map a peripheral address to its enable bits
+struct cline
+lookup_clock_line(uint32_t periph_base)
 {
-    if (periph_base < D2_APB2PERIPH_BASE) {
-        uint32_t pos = (periph_base - D2_APB1PERIPH_BASE) / 0x400;
-        return RCC->APB1LENR & (1<<pos); // we assume it is not in APB1HENR
-    } else if (periph_base < D2_AHB1PERIPH_BASE) {
-        uint32_t pos = (periph_base - D2_APB2PERIPH_BASE) / 0x400;
-        return RCC->APB2ENR & (1<<pos);
-    } else if (periph_base < D2_AHB2PERIPH_BASE) {
-        uint32_t pos = (periph_base - D2_AHB1PERIPH_BASE) / 0x400;
-        return RCC->AHB1ENR & (1<<pos);
-    } else if (periph_base < D1_APB1PERIPH_BASE) {
-        uint32_t pos = (periph_base - D2_AHB2PERIPH_BASE) / 0x400;
-        return RCC->AHB2ENR & (1<<pos);
-    } else if (periph_base < D1_AHB1PERIPH_BASE) {
-        uint32_t pos = (periph_base - D1_APB1PERIPH_BASE) / 0x400;
-        return RCC->APB3ENR & (1<<pos);
-    } else if (periph_base < D3_APB1PERIPH_BASE) {
-        uint32_t pos = (periph_base - D1_AHB1PERIPH_BASE) / 0x400;
-        return RCC->AHB3ENR & (1<<pos);
-    } else if (periph_base < D3_AHB1PERIPH_BASE) {
-        uint32_t pos = (periph_base - D3_APB1PERIPH_BASE) / 0x400;
-        return RCC->APB4ENR & (1<<pos);
+    if (periph_base >= D3_AHB1PERIPH_BASE) {
+        uint32_t bit = 1 << ((periph_base - D3_AHB1PERIPH_BASE) / 0x400);
+        return (struct cline){.en=&RCC->AHB4ENR, .rst=&RCC->AHB4RSTR, .bit=bit};
+    } else if (periph_base >= D3_APB1PERIPH_BASE) {
+        uint32_t bit = 1 << ((periph_base - D3_APB1PERIPH_BASE) / 0x400);
+        return (struct cline){.en=&RCC->APB4ENR, .rst=&RCC->APB4RSTR, .bit=bit};
+    } else if (periph_base >= D1_AHB1PERIPH_BASE) {
+        uint32_t bit = 1 << ((periph_base - D1_AHB1PERIPH_BASE) / 0x400);
+        return (struct cline){.en=&RCC->AHB3ENR, .rst=&RCC->AHB3RSTR, .bit=bit};
+    } else if (periph_base >= D1_APB1PERIPH_BASE) {
+        uint32_t bit = 1 << ((periph_base - D1_APB1PERIPH_BASE) / 0x400);
+        return (struct cline){.en=&RCC->APB3ENR, .rst=&RCC->APB3RSTR, .bit=bit};
+    } else if (periph_base >= D2_AHB2PERIPH_BASE) {
+        uint32_t bit = 1 << ((periph_base - D2_AHB2PERIPH_BASE) / 0x400);
+        return (struct cline){.en=&RCC->AHB2ENR, .rst=&RCC->AHB2RSTR, .bit=bit};
+    } else if (periph_base >= D2_AHB1PERIPH_BASE) {
+        uint32_t bit = 1 << ((periph_base - D2_AHB1PERIPH_BASE) / 0x400);
+        return (struct cline){.en=&RCC->AHB1ENR, .rst=&RCC->AHB1RSTR, .bit=bit};
+    } else if (periph_base >= D2_APB2PERIPH_BASE) {
+        uint32_t bit = 1 << ((periph_base - D2_APB2PERIPH_BASE) / 0x400);
+        return (struct cline){.en=&RCC->APB2ENR, .rst=&RCC->APB2RSTR, .bit=bit};
     } else {
-        uint32_t pos = (periph_base - D3_AHB1PERIPH_BASE) / 0x400;
-        return RCC->AHB4ENR & (1<<pos);
+        uint32_t bit = 1 << ((periph_base - D2_APB1PERIPH_BASE) / 0x400);
+        return (struct cline){.en=&RCC->APB1LENR,.rst=&RCC->APB1LRSTR,.bit=bit};
     }
 }