rp2040: set clock to 200Mhz

Signed-off-by: Timofey Titovets <nefelim4ag@gmail.com>
Klipper3d · Feb 27, 2025 · 2f6d240 · 2f6d240
1 parent edc3d34
commit 2f6d240
Showing 1 changed file with 18 additions and 2 deletions.
diff --git a/src/rp2040/main.c b/src/rp2040/main.c
@@ -12,6 +12,7 @@
 #include "hardware/structs/resets.h" // sio_hw
 #include "hardware/structs/watchdog.h" // watchdog_hw
 #include "hardware/structs/xosc.h" // xosc_hw
+#include "hardware/structs/vreg_and_chip_reset.h" // vreg_and_chip_reset_hw
 #include "internal.h" // enable_pclock
 #include "sched.h" // sched_main
 
@@ -58,9 +59,23 @@ bootloader_request(void)
  ****************************************************************/
 
 #define FREQ_XOSC 12000000
-#define FREQ_SYS (CONFIG_MACH_RP2040 ? 125000000 : CONFIG_CLOCK_FREQ)
+#define FREQ_SYS (CONFIG_MACH_RP2040 ? 200000000 : CONFIG_CLOCK_FREQ)
+#define FBDIV (FREQ_SYS == 200000000 ? 100 : 125)
 #define FREQ_USB 48000000
 
+#if CONFIG_MACH_RP2040
+void set_vsel(void)
+{
+    uint32_t cval = vreg_and_chip_reset_hw->vreg;
+    uint32_t vref = VREG_AND_CHIP_RESET_VREG_VSEL_RESET + 1;
+    cval &= ~VREG_AND_CHIP_RESET_VREG_VSEL_BITS;
+    cval |= vref << VREG_AND_CHIP_RESET_VREG_VSEL_LSB;
+    vreg_and_chip_reset_hw->vreg = cval;
+}
+#else
+void set_vsel(void) {}
+#endif
+
 void
 enable_pclock(uint32_t reset_bit)
 {
@@ -141,7 +156,8 @@ clock_setup(void)
     // Setup xosc, pll_sys, and switch clk_sys
     xosc_setup();
     enable_pclock(RESETS_RESET_PLL_SYS_BITS);
-    pll_setup(pll_sys_hw, 125, 125*FREQ_XOSC/FREQ_SYS);
+    set_vsel();
+    pll_setup(pll_sys_hw, FBDIV, FBDIV * FREQ_XOSC / FREQ_SYS);
     csys->ctrl = 0;
     csys->div = 1<<CLOCKS_CLK_SYS_DIV_INT_LSB;
     csys->ctrl = CLOCKS_CLK_SYS_CTRL_SRC_VALUE_CLKSRC_CLK_SYS_AUX;