1 files changed, 57 insertions, 0 deletions
diff --git a/src/libs/clock.c b/src/libs/clock.c
new file mode 100644
index 0000000..e5b493b
--- /dev/null
+++ b/src/libs/clock.c
@@ -0,0 +1,57 @@
+#include "clock.h"
+#include "addrmap.h"
+#include "types.h"
+#include "utils.h"
+
+// Updated at each systick interrupts, one for each core
+__attribute__((aligned(4))) u32 cores_systick[2];
+
+void wait(u32 ms){
+  u32 cpuid=REG_READ(SIO_CPUID);
+  cores_systick[cpuid]=0; // Init core counter
+  while(cores_systick[cpuid]<ms){} // Wait for correct amount of ticks
+}
+
+
+void xosc_init() {
+  // Starting xosc
+  REG_WRITE(XOSC_CTRL, 0xAA0); // Straight up from doc (to set it up to max (15MHz) => 12MHz in reality because of the xosc crystal)
+  REG_WRITE(XOSC_STARTUP, 0x00C4); // Startup delay (default value)
+  REG_WRITE_BITMAP_SET(XOSC_CTRL, 0xFAB << 12); // Enable clock
+  while(!(REG_READ(XOSC_STATUS) & 1<<31)){} // Wait for xosc to be stable
+
+  // Reset System PLL
+  REG_WRITE_BITMAP_CLEAR(RESETS_RESET, 1<<12); // Reset System PLL
+  while((REG_READ(RESETS_DONE) & 1<<12) == 0){} // Wait for System PLL reset to be done
+
+  // Setup and starts System PLL
+  // We use default config from (P232) which generate a 125MHz clock signal
+  REG_WRITE(PLL_SYS_FBDIV_INT, 125); // Multiply factor (we multply input frequency by 125 (so Fxosc * 10 = 125*12MHz = 1500MHz))
+  REG_WRITE(PLL_SYS_PRIM, 6<<16|2<<12); // Post divider divide output frequency by 6 then 2 (totally 12) thus Ffinal=1500MHz/12 = 125MHz
+  REG_WRITE_BITMAP_CLEAR(PLL_SYS_PWR, 1<<5|1<<3|1<<0); // Turn on VCO + PLL + fbdiv
+  while(!(REG_READ(PLL_SYS_CS) & 1<<31)){} // Wait for System PLL to be locked (Fin == Fout * Fact)
+
+  // By default System PLL is connected to sys_clk auxsrc
+  // So, just permute sys_clck to auxsrc (remember it is glitchless)
+  REG_WRITE(CLOCKS_SYS_CTRL, 0x1); // Now cores are using System PLL output
+
+  // Setup clock interrupt (1 every ms)
+  REG_WRITE(PPB_SYST_RVR, 125*1000); // We want 1tick per ms = 125MHz/125e3 = 1000Hz == 1 tick per ms, we can argue on accuracy here...
+  REG_WRITE(PPB_SYST_CSR, 0b111); // Clock source for Systick is core clock (PLL)/ Interrupt enable / Counter enable
+
+  // Now, setup USB PLL, reset USB PLL
+  REG_WRITE_BITMAP_CLEAR(RESETS_RESET, 1<<13); // Reset USB PLL
+  while((REG_READ(RESETS_DONE) & 1<<13) == 0){} // Wait for USB PLL reset to be done
+
+  // We use default config from (P232) which generate a 48MHz clock signal for USB
+  REG_WRITE(PLL_USB_FBDIV_INT, 100); // Multiply factor (we multply input frequency by 100 (so Fxosc * 100 = 100*12MHz = 1200MHz))
+  REG_WRITE(PLL_USB_PRIM, 5<<16|5<<12); // Post divider divide output frequency by 5 then 5 (totally 25) thus Ffinal=1200MHz/25 = 48MHz
+  REG_WRITE_BITMAP_CLEAR(PLL_USB_PWR, 1<<5|1<<3|1<<0); // Turn on VCO + PLL + fbdiv
+  while(!(REG_READ(PLL_USB_CS) & 1<<31)){} // Wait for USB PLL to be locked
+
+  // Enable USB clock generator
+  REG_WRITE(CLOCKS_USB_CTRL,1<<11);
+
+  // Last step, use XOSC for clock ref
+  REG_WRITE(CLOCKS_REF_CTRL, 2); // This is glitchless
+}