#include <MSP430F5510.h>
#include <HAL_PMM.h>
#include <HAL_UCS.h>


void main(void)
{
  WDTCTL = WDTPW+WDTHOLD;                   // Stop WDT
  
  P5SEL = BIT4 + BIT5;                                 //Setup XT1
  P1DIR |= BIT2;                                          // P1.2 output

  SetVCore(0);                                             //Set Vcore to accomodate for max. allowed system speed
  LFXT_Start(XT1DRIVE_0);                           //Use 32.768kHz XTAL as reference
  Init_FLL_Settle(8000, 244);                         //Set system clock (8MHz)

  while(1)
  {
    P1OUT ^= BIT2;                                      // Toggle P1.2

  }
}

// Библиотечные функции:


uint16_t SetVCore(uint8_t level)
{
  uint16_t actlevel;
  uint16_t status = 0;
  
  level &= PMMCOREV_3;                       // Set Mask for Max. level
  actlevel = (PMMCTL0 & PMMCOREV_3);         // Get actual VCore
                                             // step by step increase or decrease
  while (((level != actlevel) && (status == 0)) || (level < actlevel)) {
    if (level > actlevel) {
      status = SetVCoreUp(++actlevel);
    }
    else {
      status = SetVCoreDown(--actlevel);
    }
  }
  
  return status;
}

void XT1_Start(uint16_t xtdrive)
{
  // Check if drive value is the expected one
  if ((UCSCTL6 & XT1DRIVE_3) != xtdrive) {
    UCSCTL6 &= ~XT1DRIVE_3;                 // Clear XT1drive field
    UCSCTL6 |= xtdrive;                     // Set requested value
  }
  
  UCSCTL6 &= ~XT1OFF;                       // Enable XT1
  UCSCTL6 |= XTS;                           // Enable HF mode

  while (SFRIFG1 & OFIFG) {   // Check OFIFG fault flag
    UCSCTL7 &= ~(DCOFFG+XT1LFOFFG+XT1HFOFFG+XT2OFFG); // Clear OSC flaut Flags
    SFRIFG1 &= ~OFIFG;        // Clear OFIFG fault flag
  }
}


void Init_FLL_Settle(uint16_t fsystem, uint16_t ratio)
{
  volatile uint16_t x = ratio * 32;      

  Init_FLL(fsystem, ratio);
  
  while (x--) {
   __delay_cycles(30);
  }
}

void Init_FLL(uint16_t fsystem, uint16_t ratio)
{
  uint16_t d, dco_div_bits;
  uint16_t mode = 0;

  // Save actual state of FLL loop control, then disable it. This is needed to
  // prevent the FLL from acting as we are making fundamental modifications to
  // the clock setup.
  uint16_t srRegisterState = __get_SR_register() & SCG0;
  __bic_SR_register(SCG0);  
  
  d = ratio;
  dco_div_bits = FLLD__2;        // Have at least a divider of 2
  
  if (fsystem > 16000) {
    d >>= 1;
    mode = 1;
  }
  else {
    fsystem <<= 1;               // fsystem = fsystem * 2
  }

  while (d > 512) {
    dco_div_bits = dco_div_bits + FLLD0;  // Set next higher div level
    d >>= 1;
  }

  UCSCTL0 = 0x0000;              // Set DCO to lowest Tap

  UCSCTL2 &= ~(0x03FF);          // Reset FN bits
  UCSCTL2 = dco_div_bits | (d - 1);

  if (fsystem <= 630)            //           fsystem < 0.63MHz
    UCSCTL1 = DCORSEL_0;
  else if (fsystem <  1250)      // 0.63MHz < fsystem < 1.25MHz
    UCSCTL1 = DCORSEL_1;
  else if (fsystem <  2500)      // 1.25MHz < fsystem <  2.5MHz
    UCSCTL1 = DCORSEL_2;
  else if (fsystem <  5000)      // 2.5MHz  < fsystem <    5MHz
    UCSCTL1 = DCORSEL_3;
  else if (fsystem <  10000)     // 5MHz    < fsystem <   10MHz
    UCSCTL1 = DCORSEL_4;
  else if (fsystem <  20000)     // 10MHz   < fsystem <   20MHz
    UCSCTL1 = DCORSEL_5;
  else if (fsystem <  40000)     // 20MHz   < fsystem <   40MHz
    UCSCTL1 = DCORSEL_6;
  else
    UCSCTL1 = DCORSEL_7;

  while (SFRIFG1 & OFIFG) {                               // Check OFIFG fault flag
    UCSCTL7 &= ~(DCOFFG+XT1LFOFFG+XT1HFOFFG+XT2OFFG);     // Clear OSC flaut Flags
    SFRIFG1 &= ~OFIFG;                                    // Clear OFIFG fault flag
  }

  if (mode == 1) {                                        // fsystem > 16000
    SELECT_MCLK_SMCLK(SELM__DCOCLK + SELS__DCOCLK);       // Select DCOCLK
  }
  else {
    SELECT_MCLK_SMCLK(SELM__DCOCLKDIV + SELS__DCOCLKDIV); // Select DCODIVCLK
  }
  
  __bis_SR_register(srRegisterState);                      // Restore previous SCG0
}