atmel-samd/samd51: Refactor clock setup

Refactor the convoluted asf4 clock setup into something more readable.

enable_clock_generator() has 2 changes:
- Set 'Output enabled' to match the current clock setup
- Handle divisors above 511

Add an enable_clock_generator_sync() version which makes it possible to setup
clocks without waiting for syncing. The bootup would hang without this.

I have checked these registers:

 NVMCTRL->CTRLA = 0x00000004

 Peripheral clocks (only non-zero shown):
 PCHCTRL[1]=0x00000045
 PCHCTRL[10]=0x00000041

 Generator clocks (only non-zero shown):
 GENCTRL[0] = 0x00010907
 GENCTRL[1] = 0x00010906
-GENCTRL[2] = 0x00041104
+GENCTRL[2] = 0x00200904
 GENCTRL[4] = 0x00010907
 GENCTRL[5] = 0x00180906

 DFLL clock:
 OSCCTRL->DFLLCTRLA = 0x00000082
 OSCCTRL->DFLLCTRLB = 0x00000000
 OSCCTRL->DFLLVAL = 0x00008082
 OSCCTRL->DFLLMUL = 0x00000000

 DPLL clocks:
 OSCCTRL->Dpll[0].DPLLCTRLA=0x00000002
 OSCCTRL->Dpll[0].DPLLCTRLB=0x00000000
 OSCCTRL->Dpll[0].DPLLRATIO=0x0000003b
 OSCCTRL->Dpll[1].DPLLCTRLA=0x00000080
 OSCCTRL->Dpll[1].DPLLCTRLB=0x00000020
 OSCCTRL->Dpll[1].DPLLRATIO=0x00000000

 OSC32KCTRL clock:
 OSC32KCTRL->RTCCTRL = 0x00000000
 OSC32KCTRL->XOSC32K = 0x00002082
 OSC32KCTRL->CFDCTRL = 0x00000000
 OSC32KCTRL->EVCTRL = 0x00000000
 OSC32KCTRL->OSCULP32K = 0x00002300

Only gen2 changed which is due to samd51 having more bits in the simple
division register so DIVSEL wasn't necessary, and it didn't have OE set.

2 files changed, 62 insertions, 6 deletions

diff --git a/ports/atmel-samd/samd51_clocks.c b/ports/atmel-samd/samd51_clocks.c
index d5cb8aed0..7424636a4 100644
--- a/ports/atmel-samd/samd51_clocks.c
+++ b/ports/atmel-samd/samd51_clocks.c
@@ -52,9 +52,26 @@ void disconnect_gclk_from_peripheral(uint8_t gclk, uint8_t peripheral) {
     GCLK->PCHCTRL[peripheral].reg = 0;
 }
 
+static void enable_clock_generator_sync(uint8_t gclk, uint32_t source, uint16_t divisor, bool sync) {
+    uint32_t divsel = 0;
+    // The datasheet says 8 bits and max value of 512, how is that possible?
+    if (divisor > 255) { // Generator 1 has 16 bits
+        divsel = GCLK_GENCTRL_DIVSEL;
+        for (int i = 15; i > 0; i--) {
+            if (divisor & (1 << i)) {
+                divisor = i - 1;
+                break;
+            }
+        }
+    }
+
+    GCLK->GENCTRL[gclk].reg = GCLK_GENCTRL_SRC(source) | GCLK_GENCTRL_DIV(divisor) | divsel | GCLK_GENCTRL_OE | GCLK_GENCTRL_GENEN;
+    if (sync)
+        while ((GCLK->SYNCBUSY.vec.GENCTRL & (1 << gclk)) != 0) {}
+}
+
 void enable_clock_generator(uint8_t gclk, uint32_t source, uint16_t divisor) {
-    GCLK->GENCTRL[gclk].reg = GCLK_GENCTRL_SRC(source) | GCLK_GENCTRL_DIV(divisor) | GCLK_GENCTRL_GENEN;
-    while ((GCLK->SYNCBUSY.vec.GENCTRL & (1 << gclk)) != 0) {}
+    enable_clock_generator_sync(gclk, source, divisor, true);
 }
 
 void disable_clock_generator(uint8_t gclk) {
@@ -62,6 +79,47 @@ void disable_clock_generator(uint8_t gclk) {
     while ((GCLK->SYNCBUSY.vec.GENCTRL & (1 << gclk)) != 0) {}
 }
 
+static void init_clock_source_osculp32k(void) {
+    // Calibration value is loaded at startup
+    OSC32KCTRL->OSCULP32K.bit.EN1K = 0;
+    OSC32KCTRL->OSCULP32K.bit.EN32K = 0;
+}
+
+static void init_clock_source_xosc32k(void) {
+    OSC32KCTRL->XOSC32K.reg = OSC32KCTRL_XOSC32K_ONDEMAND |
+                              OSC32KCTRL_XOSC32K_ENABLE |
+                              OSC32KCTRL_XOSC32K_CGM(1);
+}
+
+static void init_clock_source_dpll0(void)
+{
+    GCLK->PCHCTRL[OSCCTRL_GCLK_ID_FDPLL0].reg = GCLK_PCHCTRL_CHEN | GCLK_PCHCTRL_GEN(5);
+    OSCCTRL->Dpll[0].DPLLRATIO.reg = OSCCTRL_DPLLRATIO_LDRFRAC(0) | OSCCTRL_DPLLRATIO_LDR(59);
+    OSCCTRL->Dpll[0].DPLLCTRLB.reg = OSCCTRL_DPLLCTRLB_REFCLK(0);
+    OSCCTRL->Dpll[0].DPLLCTRLA.reg = OSCCTRL_DPLLCTRLA_ENABLE;
+
+    while (!(OSCCTRL->Dpll[0].DPLLSTATUS.bit.LOCK || OSCCTRL->Dpll[0].DPLLSTATUS.bit.CLKRDY)) {}
+}
+
+void clock_init(void) {
+    // DFLL48M is enabled by default
+
+    init_clock_source_osculp32k();
+    init_clock_source_xosc32k();
+
+    OSC32KCTRL->RTCCTRL.bit.RTCSEL = OSC32KCTRL_RTCCTRL_RTCSEL_ULP1K_Val;
+
+    MCLK->CPUDIV.reg = MCLK_CPUDIV_DIV(1);
+
+    enable_clock_generator_sync(0, GCLK_GENCTRL_SRC_DPLL0_Val, 1, false);
+    enable_clock_generator_sync(1, GCLK_GENCTRL_SRC_DFLL_Val, 1, false);
+    enable_clock_generator_sync(2, GCLK_GENCTRL_SRC_OSCULP32K_Val, 32, false);
+    enable_clock_generator_sync(4, GCLK_GENCTRL_SRC_DPLL0_Val, 1, false);
+    enable_clock_generator_sync(5, GCLK_GENCTRL_SRC_DFLL_Val, 24, false);
+
+    init_clock_source_dpll0();
+}
+
 static bool clk_enabled(uint8_t clk) {
     return GCLK->PCHCTRL[clk].bit.CHEN;
 }
diff --git a/ports/atmel-samd/supervisor/port.c b/ports/atmel-samd/supervisor/port.c
index a92ab2913..55304e8ba 100644
--- a/ports/atmel-samd/supervisor/port.c
+++ b/ports/atmel-samd/supervisor/port.c
@@ -181,11 +181,9 @@ safe_mode_t port_init(void) {
 #ifdef SAMD21
     hri_nvmctrl_set_CTRLB_RWS_bf(NVMCTRL, 2);
     _pm_init();
-    clock_init();
-#endif
-#ifdef SAMD51
-    init_mcu();
 #endif
+    clock_init();
+
     board_init();
 
     // Configure millisecond timer initialization.