#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
}