1 files changed, 17 insertions, 59 deletions

diff --git a/src/stm32/stm32g0.c b/src/stm32/stm32g0.c
index 9374376a..3d3cca69 100644
--- a/src/stm32/stm32g0.c
+++ b/src/stm32/stm32g0.c
@@ -19,71 +19,29 @@
 #define FREQ_PERIPH 64000000
 #define FREQ_USB 48000000
 
-// Map an APB peripheral address to an enable bit
-static int
-lookup_apb_bit(uint32_t periph_base)
+// Map a peripheral address to its enable bits
+struct cline
+lookup_clock_line(uint32_t periph_base)
 {
+    if (periph_base >= IOPORT_BASE) {
+        uint32_t bit = 1 << ((periph_base - IOPORT_BASE) / 0x400);
+        return (struct cline){.en=&RCC->IOPENR, .rst=&RCC->IOPRSTR, .bit=bit};
+    } else if (periph_base >= AHBPERIPH_BASE) {
+        uint32_t bit = 1 << ((periph_base - AHBPERIPH_BASE) / 0x400);
+        return (struct cline){.en=&RCC->AHBENR, .rst=&RCC->AHBRSTR, .bit=bit};
+    }
     if (periph_base == USB_BASE)
-        return 13;
+        return (struct cline){.en=&RCC->APBENR1,.rst=&RCC->APBRSTR1,.bit=1<<13};
     if (periph_base == CRS_BASE)
-        return 16;
+        return (struct cline){.en=&RCC->APBENR1,.rst=&RCC->APBRSTR1,.bit=1<<16};
     if (periph_base == SPI1_BASE)
-        return 32 + 12;
+        return (struct cline){.en=&RCC->APBENR2,.rst=&RCC->APBRSTR2,.bit=1<<12};
     if (periph_base == USART1_BASE)
-        return 32 + 14;
+        return (struct cline){.en=&RCC->APBENR2,.rst=&RCC->APBRSTR2,.bit=1<<14};
     if (periph_base == ADC1_BASE)
-        return 32 + 20;
-    return (periph_base - APBPERIPH_BASE) / 0x400;
-}
-
-// Enable a peripheral clock
-void
-enable_pclock(uint32_t periph_base)
-{
-    if (periph_base >= IOPORT_BASE) {
-        uint32_t pos = (periph_base - IOPORT_BASE) / 0x400;
-        RCC->IOPENR |= 1 << pos;
-        RCC->IOPENR;
-        RCC->IOPRSTR |= (1<<pos);
-        RCC->IOPRSTR &= ~(1<<pos);
-    } else if (periph_base >= AHBPERIPH_BASE) {
-        uint32_t pos = (periph_base - AHBPERIPH_BASE) / 0x400;
-        RCC->AHBENR |= 1 << pos;
-        RCC->AHBENR;
-        RCC->AHBRSTR |= (1<<pos);
-        RCC->AHBRSTR &= ~(1<<pos);
-    } else {
-        uint32_t pos = lookup_apb_bit(periph_base);
-        if (pos < 32) {
-            RCC->APBENR1 |= 1 << pos;
-            RCC->APBENR1;
-            RCC->APBRSTR1 |= (1 << pos);
-            RCC->APBRSTR1 &= ~(1 << pos);
-        } else {
-            RCC->APBENR2 |= 1 << (pos - 32);
-            RCC->APBENR2;
-            RCC->APBRSTR2 |= (1 << (pos - 32));
-            RCC->APBRSTR2 &= ~(1 << (pos - 32));
-        }
-    }
-}
-
-// Check if a peripheral clock has been enabled
-int
-is_enabled_pclock(uint32_t periph_base)
-{
-    if (periph_base >= IOPORT_BASE) {
-        uint32_t pos = (periph_base - IOPORT_BASE) / 0x400;
-        return RCC->IOPENR & (1 << pos);
-    } else if (periph_base >= AHBPERIPH_BASE) {
-        uint32_t pos = (periph_base - AHBPERIPH_BASE) / 0x400;
-        return RCC->AHBENR & (1 << pos);
-    } else {
-        uint32_t pos = lookup_apb_bit(periph_base);
-        if (pos < 32)
-            return RCC->APBENR1 & (1 << pos);
-        return RCC->APBENR2 & (1 << (pos - 32));
-    }
+        return (struct cline){.en=&RCC->APBENR2,.rst=&RCC->APBRSTR2,.bit=1<<20};
+    uint32_t bit = 1 << ((periph_base - APBPERIPH_BASE) / 0x400);
+    return (struct cline){.en=&RCC->APBENR1, .rst=&RCC->APBRSTR1, .bit=bit};
 }
 
 // Return the frequency of the given peripheral clock