Clean setup of PLL at 126MHz (4x 31.5MHz = VGA pixel clock)

src/main.c

@@ -83,8 +83,33 @@ static void vga_vsync_program_init(PIO pio, uint sm, uint offset)
 	pio_sm_set_enabled(pio, sm, true);
 }
 
+void setup_clocks(void)
+{
+	// disable resuscitation clock
+	clocks_hw->resus.ctrl = 0;
+
+	// before changing PLL, switch sys and ref cleanly away from their aux sources
+	hw_clear_bits(&clocks_hw->clk[clk_sys].ctrl, CLOCKS_CLK_SYS_CTRL_SRC_BITS);
+	while (clocks_hw->clk[clk_sys].selected != 0x1)
+		tight_loop_contents();
+	hw_clear_bits(&clocks_hw->clk[clk_ref].ctrl, CLOCKS_CLK_REF_CTRL_SRC_BITS);
+	while (clocks_hw->clk[clk_ref].selected != 0x1)
+		tight_loop_contents();
+
+	// set PLL at 126 MHz, and wait for it to stabilize
+	pll_init(pll_sys, 1, 1512 * MHZ, 6, 2);
+
+	// re-configure sys_clk to use PLL
+	clock_configure(clk_sys, CLOCKS_CLK_SYS_CTRL_SRC_VALUE_CLKSRC_CLK_SYS_AUX,
+	                CLOCKS_CLK_SYS_CTRL_AUXSRC_VALUE_CLKSRC_PLL_SYS, 126 * MHZ, 126 * MHZ);
+
+	// re-configure CLK PERI = clk_sys
+	clock_configure(clk_peri, 0, CLOCKS_CLK_PERI_CTRL_AUXSRC_VALUE_CLK_SYS, 126 * MHZ, 126 * MHZ);
+}
+
 int main()
 {
+	setup_clocks();
 	stdio_init_all();
 	sleep_ms(5000);