UCS problem of MSP430F5151

//******************************************************************************
//   MSP430F51x2 Demo - Software Toggle P1.0 with 25MHz DCO
//
//   Description: Toggle P1.0 by xor'ing P1.0 inside of a software loop.
//   MCLK, SMCLK and ACLK are brought out on pins P3.0 to P3.2 respectively.
//   ACLK = REFO = 32kHz, MCLK = SMCLK = 25MHz
//
//                 MSP430F51x2
//             -----------------
//         /|\|                 |
//          | |             P3.0|--> MCLK
//          --|RST          P3.1|--> SMCLK
//            |             P3.2|--> ACLK
//            |                 |
//            |             P1.0|-->Port Pin
//
//   Bhargavi Nisarga
//   Texas Instruments Inc.
//   Dec 2009
//   Built with CCS v4 and IAR Embedded Workbench Version: 4.21
//******************************************************************************
#include  "msp430f5172.h"

// Function Definitions
void Port_Mapping(void);
void SetVcoreUp (unsigned int level);

//#define DCOCLK
void main(void)
{
  volatile unsigned int i;

  WDTCTL = WDTPW+WDTHOLD;                   // Stop WDT
  Port_Mapping();                           // Port Map port3

  // Setup Port Pins
  P2DIR |= BIT2;                            // P1.0 output
  P3DIR |= 0x07;                            // P3.0 - P3.2 output
  P3SEL |= 0x07;                            // Port map P3.0 - P3.2

  // Increase Vcore setting to level3 to support fsystem=25MHz
  // NOTE: Change core voltage one level at a time..
  SetVcoreUp (0x01);
  SetVcoreUp (0x02);
  SetVcoreUp (0x03);

  UCSCTL3 = SELREF_2;                       // Set DCO FLL reference = REFO
#ifdef DCOCLK
  UCSCTL4 |= SELA_2+SELM__DCOCLK;                        // Set ACLK = REFO
#else
  UCSCTL4 |= SELA_2+SELM__DCOCLKDIV;                        // Set ACLK = REFO
#endif

  __bis_SR_register(SCG0);                  // Disable the FLL control loop
  UCSCTL0 = 0x0000;                         // Set lowest possible DCOx, MODx
  UCSCTL1 = DCORSEL_5;                      // Select DCO range 50MHz operation
  UCSCTL2 = FLLD_1 + 243;        //243~8M           // Set DCO Multiplier for 25MHz
                                            // (N + 1) * FLLRef = Fdco
                                            // (243 + 1) * 32768 = 8MHz
                                            // Set FLL Div = fDCOCLK/2
  __bic_SR_register(SCG0);                  // re-enable the FLL control loop

  // Worst-case settling time for the DCO when the DCO range bits have been
  // changed is n x 32 x 32 x f_MCLK / f_FLL_reference. See UCS chapter in 5xx
  // UG for optimization.
  // 32 x 32 x 25 MHz / 32,768 Hz ~ 780k MCLK cycles for DCO to settle

  //32*32*8M/32768=250000;
  __delay_cycles(250000);
//  __delay_cycles(782000);

  // Loop until XT1 & DCO stabilizes - In this case only DCO has to stabilize
  do
  {
    UCSCTL7 &= ~(XT1LFOFFG + XT1HFOFFG + DCOFFG);
                                            // Clear XT1,DCO fault flags
    SFRIFG1 &= ~OFIFG;                      // Clear fault flags
  }while (SFRIFG1&OFIFG);                   // Test oscillator fault flag

  while(1)
  {
    P2OUT ^= BIT2;                          // Toggle P1.0
    __delay_cycles(8000);                 // Delay
  }
}

void SetVcoreUp (unsigned int level)
{
  // Open PMM registers for write
  PMMCTL0_H = PMMPW_H;
  // Set SVS/SVM high side new level
  SVSMHCTL = SVSHE + SVSHRVL0 * level + SVMHE + SVSMHRRL0 * level;
  // Set SVM low side to new level
  SVSMLCTL = SVSLE + SVMLE + SVSMLRRL0 * level;
  // Wait till SVM is settled
  while ((PMMIFG & SVSMLDLYIFG) == 0);
  // Clear already set flags
  PMMIFG &= ~(SVMLVLRIFG + SVMLIFG);
  // Set VCore to new level
  PMMCTL0_L = PMMCOREV0 * level;
  // Wait till new level reached
  if ((PMMIFG & SVMLIFG))
    while ((PMMIFG & SVMLVLRIFG) == 0);
  // Set SVS/SVM low side to new level
  SVSMLCTL = SVSLE + SVSLRVL0 * level + SVMLE + SVSMLRRL0 * level;
  // Lock PMM registers for write access
  PMMCTL0_H = 0x00;
}

void Port_Mapping(void)
{
  __disable_interrupt();                    // Disable Interrupts before altering Port Mapping registers
  PMAPPWD = 0x02D52;                        // Enable Write-access to modify port mapping registers

  #ifdef PORT_MAP_RECFG
  PMAPCTL = PMAPRECFG;                      // Allow reconfiguration during runtime
  #endif

  P3MAP0 = PM_TD0CLKMCLK;
  P3MAP1 = PM_TD0_0SMCLK;
  P3MAP2 = PM_TD1OUTH;

  PMAPPWD = 0;                              // Disable Write-Access to modify port mapping registers
  #ifdef PORT_MAP_EINT
  __enable_interrupt();                     // Re-enable all interrupts
  #endif
}