samd/clock_config: Support changing machine.freq() for SAMD21.

The range is 1MHz - 48 MHz.  Note that below 8 MHz there is no USB support.
The frequency will be set to an integer fraction of 48 MHz.  And after
changing the frequency, the peripherals like PWM, UART, I2C, SPI have to be
reconfigured.

Current consumption e.g. of the Seeed Xiao board at 1 MHz is about 1.5 mA,
mostly caused by the on-board LED (green LED with 1k resistor at 3.3V).

5 files changed, 36 insertions, 15 deletions

diff --git a/ports/samd/mcu/samd21/clock_config.c b/ports/samd/mcu/samd21/clock_config.c
index 2402ed2e3..204a5294f 100644
--- a/ports/samd/mcu/samd21/clock_config.c
+++ b/ports/samd/mcu/samd21/clock_config.c
@@ -51,7 +51,36 @@ uint32_t get_peripheral_freq(void) {
 }
 
 void set_cpu_freq(uint32_t cpu_freq_arg) {
-    cpu_freq = cpu_freq_arg;
+
+    // Set 1 waitstate to be safe
+    NVMCTRL->CTRLB.reg = NVMCTRL_CTRLB_MANW | NVMCTRL_CTRLB_RWS(1);
+
+    int div = DFLL48M_FREQ / cpu_freq_arg;
+    peripheral_freq = cpu_freq = DFLL48M_FREQ / div;
+
+    // Enable GCLK output: 48M on both CCLK0 and GCLK2
+    GCLK->GENDIV.reg = GCLK_GENDIV_ID(0) | GCLK_GENDIV_DIV(div);
+    GCLK->GENCTRL.reg = GCLK_GENCTRL_GENEN | GCLK_GENCTRL_SRC_DFLL48M | GCLK_GENCTRL_ID(0);
+    while (GCLK->STATUS.bit.SYNCBUSY) {
+    }
+    GCLK->GENDIV.reg = GCLK_GENDIV_ID(2) | GCLK_GENDIV_DIV(div);
+    GCLK->GENCTRL.reg = GCLK_GENCTRL_GENEN | GCLK_GENCTRL_SRC_DFLL48M | GCLK_GENCTRL_ID(2);
+    while (GCLK->STATUS.bit.SYNCBUSY) {
+    }
+    if (cpu_freq >= 8000000) {
+        // Enable GCLK output: 48MHz on GCLK5 for USB
+        GCLK->GENDIV.reg = GCLK_GENDIV_ID(5) | GCLK_GENDIV_DIV(1);
+        GCLK->GENCTRL.reg = GCLK_GENCTRL_GENEN | GCLK_GENCTRL_SRC_DFLL48M | GCLK_GENCTRL_ID(5);
+        while (GCLK->STATUS.bit.SYNCBUSY) {
+        }
+    } else {
+        // Disable GCLK output on GCLK5 for USB, since USB is not reliable below 8 Mhz.
+        GCLK->GENCTRL.reg = GCLK_GENCTRL_ID(5);
+        while (GCLK->STATUS.bit.SYNCBUSY) {
+        }
+    }
+    // Set 0 waitstates for slower CPU clock
+    NVMCTRL->CTRLB.reg = NVMCTRL_CTRLB_MANW | NVMCTRL_CTRLB_RWS(cpu_freq > 24000000 ? 1 : 0);
 }
 
 void check_usb_recovery_mode(void) {
@@ -76,6 +105,7 @@ void init_clocks(uint32_t cpu_freq) {
     // GCLK2: 48MHz from DFLL for Peripherals
     // GCLK3: 1Mhz for the us-counter (TC4/TC5)
     // GCLK4: 32kHz from crystal, if present
+    // GCLK5: 48MHz from DFLL for USB
     // GCLK8: 1kHz clock for WDT
 
     NVMCTRL->CTRLB.bit.MANW = 1; // errata "Spurious Writes"
@@ -180,15 +210,7 @@ void init_clocks(uint32_t cpu_freq) {
 
     #endif // MICROPY_HW_XOSC32K
 
-    // Enable GCLK output: 48M on both CCLK0 and GCLK2
-    GCLK->GENDIV.reg = GCLK_GENDIV_ID(0) | GCLK_GENDIV_DIV(1);
-    GCLK->GENCTRL.reg = GCLK_GENCTRL_GENEN | GCLK_GENCTRL_SRC_DFLL48M | GCLK_GENCTRL_ID(0);
-    while (GCLK->STATUS.bit.SYNCBUSY) {
-    }
-    GCLK->GENDIV.reg = GCLK_GENDIV_ID(2) | GCLK_GENDIV_DIV(1);
-    GCLK->GENCTRL.reg = GCLK_GENCTRL_GENEN | GCLK_GENCTRL_SRC_DFLL48M | GCLK_GENCTRL_ID(2);
-    while (GCLK->STATUS.bit.SYNCBUSY) {
-    }
+    set_cpu_freq(cpu_freq);
 
     // Enable GCLK output: 1MHz on GCLK3 for TC4
     GCLK->GENDIV.reg = GCLK_GENDIV_ID(3) | GCLK_GENDIV_DIV(48);
@@ -200,7 +222,6 @@ void init_clocks(uint32_t cpu_freq) {
     GCLK->GENCTRL.reg = GCLK_GENCTRL_GENEN | GCLK_GENCTRL_SRC_OSCULP32K | GCLK_GENCTRL_ID(8);
     while (GCLK->STATUS.bit.SYNCBUSY) {
     }
-
 }
 
 void enable_sercom_clock(int id) {
diff --git a/ports/samd/mcu/samd21/mpconfigmcu.h b/ports/samd/mcu/samd21/mpconfigmcu.h
index a84b31276..e0af60552 100644
--- a/ports/samd/mcu/samd21/mpconfigmcu.h
+++ b/ports/samd/mcu/samd21/mpconfigmcu.h
@@ -19,6 +19,7 @@
 
 #define CPU_FREQ                        (48000000)
 #define DFLL48M_FREQ                    (48000000)
+#define MAX_CPU_FREQ                    (48000000)
 
 #define IRQ_PRI_PENDSV                  ((1 << __NVIC_PRIO_BITS) - 1)
 
diff --git a/ports/samd/mcu/samd51/mpconfigmcu.h b/ports/samd/mcu/samd51/mpconfigmcu.h
index 19193992f..819bc1bb1 100644
--- a/ports/samd/mcu/samd51/mpconfigmcu.h
+++ b/ports/samd/mcu/samd51/mpconfigmcu.h
@@ -26,6 +26,7 @@ unsigned long trng_random_u32(void);
 
 #define CPU_FREQ                        (120000000)
 #define DFLL48M_FREQ                    (48000000)
+#define MAX_CPU_FREQ                    (200000000)
 #define DPLLx_REF_FREQ                  (32768)
 
 #define NVIC_PRIORITYGROUP_4            ((uint32_t)0x00000003)
diff --git a/ports/samd/modmachine.c b/ports/samd/modmachine.c
index d16b66064..21d700ac3 100644
--- a/ports/samd/modmachine.c
+++ b/ports/samd/modmachine.c
@@ -67,13 +67,11 @@ STATIC mp_obj_t machine_freq(size_t n_args, const mp_obj_t *args) {
     if (n_args == 0) {
         return MP_OBJ_NEW_SMALL_INT(get_cpu_freq());
     } else {
-        #if defined(MCU_SAMD51)
         uint32_t freq = mp_obj_get_int(args[0]);
-        if (freq >= 1000000 && freq <= 200000000) {
+        if (freq >= 1000000 && freq <= MAX_CPU_FREQ) {
             set_cpu_freq(freq);
             SysTick_Config(get_cpu_freq() / 1000);
         }
-        #endif
         return mp_const_none;
     }
 }
diff --git a/ports/samd/samd_soc.c b/ports/samd/samd_soc.c
index 113529aee..6d8348ebc 100644
--- a/ports/samd/samd_soc.c
+++ b/ports/samd/samd_soc.c
@@ -41,7 +41,7 @@
 static void usb_init(void) {
     // Init USB clock
     #if defined(MCU_SAMD21)
-    GCLK->CLKCTRL.reg = GCLK_CLKCTRL_CLKEN | GCLK_CLKCTRL_GEN_GCLK0 | GCLK_CLKCTRL_ID_USB;
+    GCLK->CLKCTRL.reg = GCLK_CLKCTRL_CLKEN | GCLK_CLKCTRL_GEN_GCLK5 | GCLK_CLKCTRL_ID_USB;
     PM->AHBMASK.bit.USB_ = 1;
     PM->APBBMASK.bit.USB_ = 1;
     uint8_t alt = 6; // alt G, USB