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