diff --git a/memory.x b/memory.x
index 0596611..23918e1 100644
--- a/memory.x
+++ b/memory.x
@@ -10,4 +10,4 @@ SECTIONS {
     {
         KEEP(*(.boot2));
     } > BOOT2
-} INSERT BEFORE .text;
\ No newline at end of file
+} INSERT BEFORE .text;
diff --git a/src/main.rs b/src/main.rs
index e500874..953cae9 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -84,6 +84,11 @@ fn init(
 
     pll::PLL::new(pll_sys).configure(1, 1500_000_000, 6, 2);
     pll::PLL::new(pll_usb).configure(1, 480_000_000, 5, 2);
+
+    // Switch clk_sys to pll_sys
+    clocks.clk_sys_ctrl.modify(|_, w| w.auxsrc().clksrc_pll_sys() );
+    clocks.clk_sys_ctrl.modify(|_, w| w.src().clksrc_clk_sys_aux() );
+    while clocks.clk_sys_selected.read().bits() != 2 {}
 }
 
 #[entry]
@@ -95,6 +100,7 @@ fn main() -> ! {
     init(p.RESETS, p.WATCHDOG, p.CLOCKS, p.XOSC, p.PLL_SYS, p.PLL_USB);
 
     let led_pin = 25;
+    let half_cycle = 125000000/2;
 
     loop {
         info!("on!");
@@ -104,7 +110,7 @@ fn main() -> ! {
             w
         });
 
-        cortex_m::asm::delay(1_000_000);
+        cortex_m::asm::delay(half_cycle);
 
         info!("off!");
         p.IO_BANK0.gpio[led_pin].gpio_ctrl.write(|w| {
@@ -113,6 +119,6 @@ fn main() -> ! {
             w
         });
 
-        cortex_m::asm::delay(1_000_000);
+        cortex_m::asm::delay(half_cycle);
     }
 }
diff --git a/src/pll.rs b/src/pll.rs
index 2faf91b..0b929df 100644
--- a/src/pll.rs
+++ b/src/pll.rs
@@ -16,15 +16,13 @@ impl<T: Instance> PLL<T> {
 
     pub fn configure(&mut self, refdiv: u32, vco_freq: u32, post_div1: u8, post_div2: u8) {
         let p = &self.inner;
+        let ref_mhz = XOSC_MHZ / refdiv;
+        let fbdiv = vco_freq / (ref_mhz * 1_000_000);
 
         // Power off in case it's already running
         p.pwr.reset();
         p.fbdiv_int.reset();
 
-        let ref_mhz = XOSC_MHZ / refdiv;
-        p.cs.write(|w| unsafe { w.bits(ref_mhz as _) });
-
-        let fbdiv = vco_freq / (ref_mhz * 1_000_000);
         assert!(fbdiv >= 16 && fbdiv <= 520);
         assert!((post_div1 >= 1 && post_div1 <= 7) && (post_div2 >= 1 && post_div2 <= 7));
         assert!(post_div2 <= post_div1);