#include "device.h"

//====================================================================
/**
  * Initializes FLL+ and load capacitances for the 32kHz oscillator
  *
  * \param cl       required load capacitance in pF
  * \param fsystem  required system frequency (MCLK) in kHz
  * \param fcyrstal cyrstal frequency in kHz
  */
void init_fll(int cl, int fsystem, int fcrystal)
{
  unsigned int cint, d, dco_div_bits;
  int nplus1;
  volatile int i, j;
  //  /\  Prevend variables from being "optimized".
  
  // Choose internal load capacitors Cint for LFXT1 depending on the 
  // load capacitor CL ("cl") required by the crystal

  if (cl <= 3)      // 0pF < CL =< 3pF?
    cint= XCAP0PF;  // Use Cint = 0pF
  else if (cl <= 7) // 3pF < CL =< 7pF?
    cint= XCAP10PF; // Use Cint = 10pF
  else if (cl <= 9) // 7pF < CL =< 9pF?
    cint= XCAP14PF; // Use Cint = 14pF
  else              //  CL > 9pF!
    cint= XCAP18PF; // Use Cint = 18pF and external Cs

  // Choose the system frequency divider
  if ((fsystem/fcrystal) < 121)
    d= 1; // D = 0 (fDCOCLK/1)
  else if ((fsystem/fcrystal) < 240)
    d= 2; // D = 1 (fDCOCLK/2)
  else if ((fsystem/fcrystal) < 480)
    d= 4; // D = 2 (fDCOCLK/4)
  else
    d= 8; // D = 3 (fDCOCLK/8)
    
  nplus1= (fsystem/fcrystal)/d;
  
  switch (d)
  {
    case  1: dco_div_bits= FLLD_1; break;  // fDCOCLK/1
    case  2: dco_div_bits= FLLD_2; break;  // fDCOCLK/2
    case  4: dco_div_bits= FLLD_4; break;  // fDCOCLK/4
    default: dco_div_bits= FLLD_8;         // fDCOCLK/8
  }

  if (fsystem < 3000) // fsystem < 3MHz
    SCFI0=    0 | dco_div_bits;
  else if (fsystem <  6000) // 3MHz <= fsystem <  6MHz
    SCFI0= FN_2 | dco_div_bits;
  else if (fsystem <  9000) // 6MHz <= fsystem <  9MHz
    SCFI0= FN_3 | dco_div_bits;
  else if (fsystem < 13000) // 9MHz <= fsystem < 13MHz
    SCFI0= FN_4 | dco_div_bits;
  else
    SCFI0= FN_8 | dco_div_bits;

  FLL_CTL0= DCOPLUS | cint;
  SCFQCTL = nplus1 - 1;

  // Allow FLL+ to settle at the correct DCO tap
  for (i= 32*d*nplus1; i > 0; i--)
    for (j= 8; j > 0; j--);

} // End of fll_init()