#include <same70.h>

static void sys_init(void)
{
  // Disable watchdog
  WDT->WDT_MR = WDT_MR_WDDIS;

  // Set flash wait states to maximum for 150 MHz operation
  EFC->EEFC_FMR = EEFC_FMR_FWS(5) | EEFC_FMR_CLOE;

  // Enable 12 MHz Xtal
  PMC->CKGR_MOR = CKGR_MOR_KEY_PASSWD | CKGR_MOR_MOSCXTST(8) |
      CKGR_MOR_MOSCRCEN | CKGR_MOR_MOSCXTEN;
  while (!(PMC->PMC_SR & PMC_SR_MOSCXTS));
    
  PMC->CKGR_MOR = CKGR_MOR_KEY_PASSWD | CKGR_MOR_MOSCXTST(8) |
      CKGR_MOR_MOSCRCEN | CKGR_MOR_MOSCXTEN | CKGR_MOR_MOSCSEL;
  while (!(PMC->PMC_SR & PMC_SR_MOSCSELS));

  // Setup PLL (12 MHz * 25 = 300 MHz)
  PMC->CKGR_PLLAR = CKGR_PLLAR_ONE | CKGR_PLLAR_MULA(25-1) |
      CKGR_PLLAR_PLLACOUNT(0x3f) | CKGR_PLLAR_DIVA(1);
  while (!(PMC->PMC_SR & PMC_SR_LOCKA));

  // Switch main clock to PLL (two step process)
  PMC->PMC_MCKR = PMC_MCKR_CSS_MAIN_CLK | PMC_MCKR_MDIV_PCK_DIV2;
  while (!(PMC->PMC_SR & PMC_SR_MCKRDY));

  PMC->PMC_MCKR = PMC_MCKR_CSS_PLLA_CLK | PMC_MCKR_MDIV_PCK_DIV2;
  while (!(PMC->PMC_SR & PMC_SR_MCKRDY));

  // Enable PIOC
  PMC->PMC_PCER0 = PMC_PCER0_PID12; 

  // Disable altenate functions on some pins
  MATRIX->CCFG_SYSIO |= CCFG_SYSIO_SYSIO4;
}

void GPIOInit(void)
{
	/* Disable watchdog (for now) */
	WDT->WDT_MR=WDT_MR_WDDIS;
	
	/*Configure PORTC.Pin8 as output (LED)*/
	PIOC->PIO_OER=PIO_OER_P8;
	
	/* Need to set A11 as input */
	PIOA->PIO_ODR=PIO_ODR_P11;

	/* Enable Pullup */
	PIOA->PIO_PUER=PIO_PUER_P11;

}

void wait_unk() {
  volatile uint32_t j = 0;
  for(uint32_t i = 0; i < 30000000; i++) {
  }
}

void main() {

  sys_init();
  GPIOInit();

  while(1) {
     wait_unk();
     if((PIOC->PIO_PDSR & PIO_PC8) != 0) {
       PIOC->PIO_CODR = PIO_CODR_P8;
     } else {
       PIOC->PIO_SODR = PIO_SODR_P8;
     }
   }
}