#include "archinc.h"
#include "pll.h"

#ifndef CONFIG_INPUT_CLOCK_HZ
#error "No input clock frequency defined!"
#endif

static unsigned int cpuFreqHz = CONFIG_INPUT_CLOCK_HZ;
static unsigned int vpbDivider = 4;
static unsigned int pllMultiplier = 1;

static unsigned int pllStatus;

#ifdef CONFIG_TOOLCHAIN_IAR
#pragma inline
#endif
static INLINE_KEYWORD void pllPassword(void)
{
  IRQ_DISABLE();
  PLLFEED = (unsigned long)0xAA;
  PLLFEED = (unsigned long)0x55;
  IRQ_ENABLE();
}

#ifdef CONFIG_TOOLCHAIN_IAR
#pragma inline
#endif
static INLINE_KEYWORD void pllWaitLock(void)
{
  while (!(PLLSTAT & 0x0400));
}

int pllInit(void) 
{
#ifdef CONFIG_INCLUDE_LPC210X_PLL

#if defined(CONFIG_BOARD_MEDUSA) || defined(CONFIG_BOARD_MEDUSA_REV2)
  PLLCFG = (unsigned long)0x02; // multiply by 3, divide by 1
  pllMultiplier = 3;
#endif

#ifdef CONFIG_BOARD_OLIMEX_P213X
  PLLCFG = (unsigned long)0x03; // multiply by 4, divide by 1
  pllMultiplier = 4;
#endif

  // enable the PLL and wait for lock
  PLLCON = (unsigned long)0x01;
  pllPassword();
  pllWaitLock();

  // wonder twin powers, activate!
  PLLCON = (unsigned long)0x03;
  pllPassword();

#endif

  // update cpu frequency so pllGetPeriphFreq() is correct
  cpuFreqHz *= pllMultiplier;

  pllStatus = PLLSTAT;

  return 0;
}

int pllSetPeriphDivider(unsigned int divider)
{
  int retVal = -1;

  switch (divider) {
    case 1:
      VPBDIV = (unsigned long)0x01;
      vpbDivider = 1;
      retVal = 0;
      break;
    case 2:
      VPBDIV = (unsigned long)0x02;
      vpbDivider = 2;
      retVal = 0;
      break;
    case 4:
      VPBDIV = (unsigned long)0x00;
      vpbDivider = 4;
      retVal = 0;
      break;
    default:
      break;
    }

  return retVal;
}
// returns current clock frequency in khz
unsigned int pllGetCpuFreq(void)
{
  // TODO: get these values from registers
  return cpuFreqHz;
}

unsigned int pllGetPeriphFreq(void)
{
  // TODO: get these values from registers
  return (cpuFreqHz/vpbDivider);
}

unsigned int pllGetPeriphDivider(void)
{
  return vpbDivider;
}