C++ MCG_C1_CLKS函数代码示例

status_t CLOCK_SetPbeMode(mcg_pll_clk_select_t pllcs, mcg_pll_config_t const *config)
{
    /*
       This function is designed to change MCG to PBE mode from PEE/BLPE/FBE,
       but with this workflow, the source mode could be all modes except PEI/PBI.
     */
    MCG->C2 &= ~MCG_C2_LP_MASK; /* Disable lowpower. */

    /* Change to use external clock first. */
    MCG->C1 = ((MCG->C1 & ~(MCG_C1_CLKS_MASK | MCG_C1_IREFS_MASK)) | MCG_C1_CLKS(kMCG_ClkOutSrcExternal));

    /* Wait for CLKST clock status bits to show clock source is ext ref clk */
    while ((MCG->S & (MCG_S_IREFST_MASK | MCG_S_CLKST_MASK)) !=
           (MCG_S_IREFST(kMCG_FllSrcExternal) | MCG_S_CLKST(kMCG_ClkOutStatExt)))
    {
    }

    /* Disable PLL first, then configure PLL. */
    MCG->C6 &= ~MCG_C6_PLLS_MASK;
    while (MCG->S & MCG_S_PLLST_MASK)
    {
    }

    /* Configure the PLL. */
    {
        CLOCK_EnablePll0(config);
    }

    /* Change to PLL mode. */
    MCG->C6 |= MCG_C6_PLLS_MASK;
    while (!(MCG->S & MCG_S_PLLST_MASK))
    {
    }

    return kStatus_Success;
}

/*
** ===================================================================
**     Method      :  Cpu_SetMCGModePEE (component MK21FN1M0MC12)
**
**     Description :
**         This method sets the MCG to PEE mode.
**         This method is internal. It is used by Processor Expert only.
** ===================================================================
*/
static void Cpu_SetMCGModePEE(uint8_t CLKMode)
{
  switch (CLKMode) {
    case 0U:
      /* Switch to PEE Mode */
      /* OSC_CR: ERCLKEN=1,??=0,EREFSTEN=1,??=0,SC2P=0,SC4P=0,SC8P=0,SC16P=0 */
      OSC_CR = (OSC_CR_ERCLKEN_MASK | OSC_CR_EREFSTEN_MASK);
      /* MCG_C7: OSCSEL=0 */
      MCG_C7 &= (uint8_t)~(uint8_t)(MCG_C7_OSCSEL_MASK);
      /* MCG_C1: CLKS=0,FRDIV=3,IREFS=0,IRCLKEN=0,IREFSTEN=0 */
      MCG_C1 = (MCG_C1_CLKS(0x00) | MCG_C1_FRDIV(0x03));
      /* MCG_C2: LOCRE0=0,??=0,RANGE0=2,HGO0=0,EREFS0=1,LP=0,IRCS=0 */
      MCG_C2 = (MCG_C2_RANGE0(0x02) | MCG_C2_EREFS0_MASK);
      /* MCG_C5: ??=0,PLLCLKEN0=0,PLLSTEN0=0,PRDIV0=1 */
      MCG_C5 = MCG_C5_PRDIV0(0x01);
      /* MCG_C6: LOLIE0=0,PLLS=1,CME0=0,VDIV0=6 */
      MCG_C6 = (MCG_C6_PLLS_MASK | MCG_C6_VDIV0(0x06));
      while((MCG_S & 0x0CU) != 0x0CU) { /* Wait until output of the PLL is selected */
      }
      break;
    default:
      break;
  }
}

/************************************************************************************************ 
* SetPLL_Kinetis
* 系统的锁相环设定，其完成的主要工作为: 设定CoreClock、BusClock、FlexClock、FlashClock
* (设置的具体频率在KinetisConfig.h中配置)
************************************************************************************************/
static void SetPLL_Kinetis(void)
{
  K_int32u_t temp_reg;
  K_int8u_t  i;
// First move to FBE mode
// Enable external oscillator, RANGE=2, HGO=1, EREFS=1, LP=0, IRCS=0
  MCG_C2 = MCG_C2_RANGE(1) | MCG_C2_HGO_MASK | MCG_C2_EREFS_MASK;

// after initialization of oscillator release latched state of oscillator and GPIO
  SIM_SCGC4 |= SIM_SCGC4_LLWU_MASK;
  LLWU_CS |= LLWU_CS_ACKISO_MASK;
  
// Select external oscilator and Reference Divider and clear IREFS to start ext osc
// CLKS=2, FRDIV=3, IREFS=0, IRCLKEN=0, IREFSTEN=0
  MCG_C1 = MCG_C1_CLKS(2) | MCG_C1_FRDIV(3);

  /* if we aren't using an osc input we don't need to wait for the osc to init */

  while (MCG_S & MCG_S_IREFST_MASK){}; // wait for Reference clock Status bit to clear

  while (((MCG_S & MCG_S_CLKST_MASK) >> MCG_S_CLKST_SHIFT) != 0x2){}; // Wait for clock status bits to show clock source is ext ref clk

// Now in FBE

  /* 设定PLL时钟 */
#if CORE_CLK_Kinetis  <= 110
  MCG_C5 = MCG_C5_PRDIV(REF_CLK_Kinetis/2 - 1);     /* PLLCLK == 2MHz */
#else
  #if   REF_CLK_Kinetis % 3 == 0
    MCG_C5 = MCG_C5_PRDIV(REF_CLK_Kinetis/3 - 1);   /* PLLCLK == 3MHz */
  #elif REF_CLK_Kinetis % 4 == 0
    MCG_C5 = MCG_C5_PRDIV(REF_CLK_Kinetis/4 - 1);   /* PLLCLK == 4MHz */
  #elif REF_CLK_Kinetis % 5 == 0
    MCG_C5 = MCG_C5_PRDIV(REF_CLK_Kinetis*2/5 - 1); /* PLLCLK == 2.5MHz */
  #endif
#endif 
  /*
   * Ensure MCG_C6 is at the reset default of 0. LOLIE disabled,
   * PLL disabled, clk monitor disabled, PLL VCO divider is clear 
   */  
  MCG_C6 = 0x0;

  /* 设定各时钟的分频数 */
  temp_reg = FMC_PFAPR; // store present value of FMC_PFAPR
  // set M0PFD through M7PFD to 1 to disable prefetch
  FMC_PFAPR |= FMC_PFAPR_M7PFD_MASK | FMC_PFAPR_M6PFD_MASK | FMC_PFAPR_M5PFD_MASK
             | FMC_PFAPR_M4PFD_MASK | FMC_PFAPR_M3PFD_MASK | FMC_PFAPR_M2PFD_MASK
             | FMC_PFAPR_M1PFD_MASK | FMC_PFAPR_M0PFD_MASK;
  
  // set clock dividers to desired value  
  SIM_CLKDIV1 = SIM_CLKDIV1_OUTDIV1(0)       
              | SIM_CLKDIV1_OUTDIV2(DIV_BusClk_Kinetis - 1) 
              | SIM_CLKDIV1_OUTDIV3(DIV_FlexClk_Kinetis - 1) 
              | SIM_CLKDIV1_OUTDIV4(DIV_FlashClk_Kinetis - 1);
  // wait for dividers to change
  for (i = 0 ; i < DIV_FlashClk_Kinetis ; i++) {}
  FMC_PFAPR = temp_reg; // re-store original value of FMC_PFAPR  
  
  /* 设置倍频数,倍频数为VDIV+24 */
#if CORE_CLK_Kinetis  <= 110
  MCG_C6 = MCG_C6_PLLS_MASK | MCG_C6_VDIV(CORE_CLK_Kinetis/2 - 24); 
#else
  #if   REF_CLK_Kinetis % 3 == 0
    MCG_C6 = MCG_C6_PLLS_MASK | MCG_C6_VDIV(CORE_CLK_Kinetis/3 - 24); 
  #elif REF_CLK_Kinetis % 4 == 0
    MCG_C6 = MCG_C6_PLLS_MASK | MCG_C6_VDIV(CORE_CLK_Kinetis/4 - 24);
  #elif REF_CLK_Kinetis % 5 == 0
    MCG_C6 = MCG_C6_PLLS_MASK | MCG_C6_VDIV(CORE_CLK_Kinetis*2/5 - 24); 
  #endif
#endif
  
  while (!(MCG_S & MCG_S_PLLST_MASK)){}; // wait for PLL status bit to set

  while (!(MCG_S & MCG_S_LOCK_MASK)){}; // Wait for LOCK bit to set

// Now running PBE Mode

// Transition into PEE by setting CLKS to 0
// CLKS=0, FRDIV=3, IREFS=0, IRCLKEN=0, IREFSTEN=0
  MCG_C1 &= ~MCG_C1_CLKS_MASK;

// Wait for clock status bits to update
  while (((MCG_S & MCG_S_CLKST_MASK) >> MCG_S_CLKST_SHIFT) != 0x3){};
} 

/*!
 *  @brief      PLL超频
 *  @param      PLL_e       频率设置参数
 *  @return     超频频率（MHz）
 *  @since      v5.0
 *  @warning    此函数只能在 复位后没进行任何频率设置情况下调用，即MCG在FEI模式下才可调用
 *  Sample usage:       uint8 clk = pll_init(PLL100);        //超频
 */
uint8 pll_init(PLL_e pll)
{

    mcg_div_count( pll);

    SIM_SCGC5 |= SIM_SCGC5_PORTA_MASK;      //PTA18 和 PTA19 用于 晶振

    // set clock dividers to desired value
    SIM_CLKDIV1 = SIM_CLKDIV1_OUTDIV1(mcg_div.core_div) |  SIM_CLKDIV1_OUTDIV4(mcg_div.bus_div);


    //上电复位后，单片机会自动进入 FEI 模式，使用 内部参考时钟

    //FEI -> FBE
    OSC0_CR  =  ( 0
                  | OSC_CR_ERCLKEN_MASK     //使能 外部参考时钟
                  //| OSC_CR_SC2P_MASK      //配置电容
                  //| OSC_CR_SC4P_MASK      //配置电容
                  //| OSC_CR_SC8P_MASK      //配置电容
                  | OSC_CR_SC16P_MASK       //配置电容
                 );

    MCG_C2 =    ( 0
                | MCG_C2_RANGE0(2)
                | MCG_C2_EREFS0_MASK
                );

    MCG_C1 = (0
              | MCG_C1_CLKS(2)
              | MCG_C1_FRDIV(7)
              | MCG_C1_IRCLKEN_MASK
              );

    while (MCG_S & MCG_S_IREFST_MASK) {};                       //等待FLL参考时钟 为 外部参考时钟（S[IREFST]=0，表示使用外部参考时钟，）

    while ((MCG_S & MCG_S_CLKST_MASK)  != MCG_S_CLKST(0x2)) {}; //等待选择外部参考时钟

    //现在已经进入了 FBE模式

    //FBE -> PBE


    MCG_C5 = MCG_C5_PRDIV0(mcg_cfg[pll].prdiv);                      //分频， EXTAL_IN_MHz/( PRDIV+1)
    MCG_C6 = MCG_C6_PLLS_MASK | MCG_C6_VDIV0(mcg_cfg[pll].vdiv) ;    //倍频， EXTAL_IN_MHz/( PRDIV+1)  * (VDIV+24)

    while (!(MCG_S & MCG_S_PLLST_MASK)) {};                         //等待时钟源选择PLL

    while (!(MCG_S & MCG_S_LOCK0_MASK)) {};                          //等待 PLL锁了（锁相环）

    // 现在已经进入了 PBE 模式

    // PBE -> PEE
    //MCG_C1 &= ~MCG_C1_CLKS_MASK;
    MCG_C1 = MCG_C1_IRCLKEN_MASK;

    while (((MCG_S & MCG_S_CLKST_MASK) ) != MCG_S_CLKST(0x3)) {};//等待选择输出PLL

    // 现在已经进入了 PEE 模式
    
    SIM_SOPT2 |= (0         //选择 PLL时钟
              | SIM_SOPT2_PLLFLLSEL_MASK
             );

    return mcg_cfg[pll].clk;
} //pll_init

/**
 * @brief   K20x clock initialization.
 * @note    All the involved constants come from the file @p board.h.
 * @note    This function is meant to be invoked early during the system
 *          initialization, it is usually invoked from the file
 *          @p board.c.
 * @todo    This function needs to be more generic.
 *
 * @special
 */
void k20x_clock_init(void) {
#if !KINETIS_NO_INIT

  /* Disable the watchdog */
  WDOG->UNLOCK = 0xC520;
  WDOG->UNLOCK = 0xD928;
  WDOG->STCTRLH &= ~WDOG_STCTRLH_WDOGEN;

  SIM->SCGC5 |= SIM_SCGC5_PORTA |
                SIM_SCGC5_PORTB |
                SIM_SCGC5_PORTC |
                SIM_SCGC5_PORTD |
                SIM_SCGC5_PORTE;

#if KINETIS_MCG_MODE == KINETIS_MCG_MODE_FEI
  /* This is the default mode at reset. */

  /* Configure FEI mode */
  MCG->C4 = MCG_C4_DRST_DRS(KINETIS_MCG_FLL_DRS) |
            (KINETIS_MCG_FLL_DMX32 ? MCG_C4_DMX32 : 0);

  /* Set clock dividers */
  SIM->CLKDIV1 = SIM_CLKDIV1_OUTDIV1(KINETIS_CLKDIV1_OUTDIV1-1) |
                 SIM_CLKDIV1_OUTDIV2(KINETIS_CLKDIV1_OUTDIV2-1) |
                 SIM_CLKDIV1_OUTDIV4(KINETIS_CLKDIV1_OUTDIV4-1);
  SIM->CLKDIV2 = SIM_CLKDIV2_USBDIV(0); /* not strictly necessary since usb_lld will set this */

#elif KINETIS_MCG_MODE == KINETIS_MCG_MODE_PEE

  uint32_t ratio, frdiv;
  uint32_t ratios[] = { 32, 64, 128, 256, 512, 1024, 1280, 1536 };
  uint8_t ratio_quantity = sizeof(ratios) / sizeof(ratios[0]);
  uint8_t i;

  /* EXTAL0 and XTAL0 */
  PORTA->PCR[18] = 0;
  PORTA->PCR[19] = 0;

  /*
   * Start in FEI mode
   */

  /* Internal capacitors for crystal */
#if defined(KINETIS_BOARD_OSCILLATOR_SETTING)
  OSC0->CR = KINETIS_BOARD_OSCILLATOR_SETTING;
#else /* KINETIS_BOARD_OSCILLATOR_SETTING */
  /* Disable the internal capacitors */
  OSC0->CR = 0;
#endif /* KINETIS_BOARD_OSCILLATOR_SETTING */

  /* TODO: need to add more flexible calculation, specially regarding
   *       divisors which may not be available depending on the XTAL
   *       frequency, which would required other registers to be modified.
   */
  /* Enable OSC, low power mode */
  MCG->C2 = MCG_C2_LOCRE0 | MCG_C2_EREFS0;
  if (KINETIS_XTAL_FREQUENCY > 8000000UL)
    MCG->C2 |= MCG_C2_RANGE0(2);
  else
    MCG->C2 |= MCG_C2_RANGE0(1);

  frdiv = 7;
  ratio = KINETIS_XTAL_FREQUENCY / 31250UL;
  for (i = 0; i < ratio_quantity; ++i) {
    if (ratio == ratios[i]) {
      frdiv = i;
      break;
    }
  }

  /* Switch to crystal as clock source, FLL input of 31.25 KHz */
  MCG->C1 = MCG_C1_CLKS(2) | MCG_C1_FRDIV(frdiv);

  /* Wait for crystal oscillator to begin */
  while (!(MCG->S & MCG_S_OSCINIT0));

  /* Wait for the FLL to use the oscillator */
  while (MCG->S & MCG_S_IREFST);

  /* Wait for the MCGOUTCLK to use the oscillator */
  while ((MCG->S & MCG_S_CLKST_MASK) != MCG_S_CLKST(2));

  /*
   * Now in FBE mode
   */
  #define KINETIS_PLLIN_FREQUENCY 2000000UL
  /*
   * Config PLL input for 2 MHz
   * TODO: Make sure KINETIS_XTAL_FREQUENCY >= 2Mhz && <= 50Mhz
   */
//.........这里部分代码省略.........

status_t CLOCK_SetMcgConfig(const mcg_config_t *config)
{
    mcg_mode_t next_mode;
    status_t status = kStatus_Success;

    mcg_pll_clk_select_t pllcs = kMCG_PllClkSelPll0;

    /* If need to change external clock, MCG_C7[OSCSEL]. */
    if (MCG_C7_OSCSEL_VAL != config->oscsel)
    {
        /* If external clock is in use, change to FEI first. */
        if (!(MCG->S & MCG_S_IRCST_MASK))
        {
            CLOCK_ExternalModeToFbeModeQuick();
            CLOCK_SetFeiMode(config->dmx32, config->drs, (void (*)(void))0);
        }

        CLOCK_SetExternalRefClkConfig(config->oscsel);
    }

    /* Re-configure MCGIRCLK, if MCGIRCLK is used as system clock source, then change to FEI/PEI first. */
    if (MCG_S_CLKST_VAL == kMCG_ClkOutStatInt)
    {
        MCG->C2 &= ~MCG_C2_LP_MASK; /* Disable lowpower. */

        {
            CLOCK_SetFeiMode(config->dmx32, config->drs, CLOCK_FllStableDelay);
        }
    }

    /* Configure MCGIRCLK. */
    CLOCK_SetInternalRefClkConfig(config->irclkEnableMode, config->ircs, config->fcrdiv);

    next_mode = CLOCK_GetMode();

    do
    {
        next_mode = mcgModeMatrix[next_mode][config->mcgMode];

        switch (next_mode)
        {
            case kMCG_ModeFEI:
                status = CLOCK_SetFeiMode(config->dmx32, config->drs, CLOCK_FllStableDelay);
                break;
            case kMCG_ModeFEE:
                status = CLOCK_SetFeeMode(config->frdiv, config->dmx32, config->drs, CLOCK_FllStableDelay);
                break;
            case kMCG_ModeFBI:
                status = CLOCK_SetFbiMode(config->dmx32, config->drs, (void (*)(void))0);
                break;
            case kMCG_ModeFBE:
                status = CLOCK_SetFbeMode(config->frdiv, config->dmx32, config->drs, (void (*)(void))0);
                break;
            case kMCG_ModeBLPI:
                status = CLOCK_SetBlpiMode();
                break;
            case kMCG_ModeBLPE:
                status = CLOCK_SetBlpeMode();
                break;
            case kMCG_ModePBE:
                /* If target mode is not PBE or PEE, then only need to set CLKS = EXT here. */
                if ((kMCG_ModePEE == config->mcgMode) || (kMCG_ModePBE == config->mcgMode))
                {
                    {
                        status = CLOCK_SetPbeMode(pllcs, &config->pll0Config);
                    }
                }
                else
                {
                    MCG->C1 = ((MCG->C1 & ~MCG_C1_CLKS_MASK) | MCG_C1_CLKS(kMCG_ClkOutSrcExternal));
                    while (MCG_S_CLKST_VAL != kMCG_ClkOutStatExt)
                    {
                    }
                }
                break;
            case kMCG_ModePEE:
                status = CLOCK_SetPeeMode();
                break;
            default:
                break;
        }
        if (kStatus_Success != status)
        {
            return status;
        }
    } while (next_mode != config->mcgMode);

    if (config->pll0Config.enableMode & kMCG_PllEnableIndependent)
    {
        CLOCK_EnablePll0(&config->pll0Config);
    }
    else
    {
        MCG->C5 &= ~(uint32_t)kMCG_PllEnableIndependent;
    }
    return kStatus_Success;
}

/**
 * @brief   MK20D5 clock initialization.
 * @note    All the involved constants come from the file @p board.h.
 * @note    This function is meant to be invoked early during the system
 *          initialization, it is usually invoked from the file
 *          @p board.c.
 * @todo    This function needs to be more generic.
 *
 * @special
 */
void mk20d50_clock_init(void) {

  uint32_t ratio, frdiv;
  uint32_t ratios[] = { 32, 64, 128, 256, 512, 1024, 1280, 1536 };
  int ratio_quantity = sizeof(ratios) / sizeof(ratios[0]);
  int i;

  /* Disable the watchdog */
  WDOG->UNLOCK = 0xC520;
  WDOG->UNLOCK = 0xD928;
  WDOG->STCTRLH &= ~WDOG_STCTRLH_WDOGEN;

  SIM->SCGC5 |= SIM_SCGC5_PORTA |
                SIM_SCGC5_PORTB |
                SIM_SCGC5_PORTC |
                SIM_SCGC5_PORTD |
                SIM_SCGC5_PORTE;

  /* EXTAL0 and XTAL0 */
  PORTA->PCR[18] = 0;
  PORTA->PCR[19] = 0;

  /*
   * Start in FEI mode
   */

  /* Disable capacitors for crystal */
  OSC->CR = 0;

  /* TODO: need to add more flexible calculation, specially regarding
   *       divisors which may not be available depending on the XTAL
   *       frequency, which would required other registers to be modified.
   */

  /* Enable OSC, low power mode */
  MCG->C2 = MCG_C2_LOCRE0 | MCG_C2_EREFS0;
  if (KINETIS_XTAL_FREQUENCY > 8000000)
    MCG->C2 |= MCG_C2_RANGE0(2);
  else
    MCG->C2 |= MCG_C2_RANGE0(1);

  frdiv = 7;
  ratio = KINETIS_XTAL_FREQUENCY / 31250;
  for (i = 0; i < ratio_quantity; ++i) {
    if (ratio == ratios[i]) {
      frdiv = i;
      break;
    }
  }

  /* Switch to crystal as clock source, FLL input of 31.25 KHz */
  MCG->C1 = MCG_C1_CLKS(2) | MCG_C1_FRDIV(frdiv);

  /* Wait for crystal oscillator to begin */
  while (!(MCG->S & MCG_S_OSCINIT0));

  /* Wait for the FLL to use the oscillator */
  while (MCG->S & MCG_S_IREFST);

  /* Wait for the MCGOUTCLK to use the oscillator */
  while ((MCG->S & MCG_S_CLKST_MASK) != MCG_S_CLKST(2));

  /*
   * Now in FBE mode
   */

  /* Config PLL input for 2 MHz */
  MCG->C5 = MCG_C5_PRDIV0((KINETIS_XTAL_FREQUENCY / 2000000) - 1);

  /* Config PLL for 96 MHz output */
  MCG->C6 = MCG_C6_PLLS | MCG_C6_VDIV0(0);

  /* Wait for PLL to start using crystal as its input */
  while (!(MCG->S & MCG_S_PLLST));

  /* Wait for PLL to lock */
  while (!(MCG->S & MCG_S_LOCK0));

  /*
   * Now in PBE mode
   */

  /* Switch to PLL as clock source */
  MCG->C1 = MCG_C1_CLKS(0);

  /* Wait for PLL clock to be used */
  while ((MCG->S & MCG_S_CLKST_MASK) != MCG_S_CLKST_PLL);

  /*
   * Now in PEE mode
//.........这里部分代码省略.........

/**
 * Initialize the system
 *
 * @param  none
 * @return none
 *
 * @brief  Setup the microcontroller system.
 *         Initialize the System.
 */
void SystemInit (void) {
    // system dividers
    SIM_CLKDIV1 = SIM_CLKDIV1_OUTDIV1(0) | SIM_CLKDIV1_OUTDIV2(1) | SIM_CLKDIV1_OUTDIV3(2) | SIM_CLKDIV1_OUTDIV4(5);
    
    
    // after reset, we are in FEI mode
    
    // enable external clock source - OSC0
#if __SYS_OSC_CLK <= 8000000
    MCG_C2 = MCG_C2_LOCRE0_MASK | MCG_C2_RANGE(RANGE0_VAL) | (/*hgo_val*/0 << MCG_C2_HGO_SHIFT) | (/*erefs_val*/0 << MCG_C2_EREFS_SHIFT);
#else
    // On rev. 1.0 of silicon there is an issue where the the input bufferd are enabled when JTAG is connected.
    // This has the affect of sometimes preventing the oscillator from running. To keep the oscillator amplitude
    // low, RANGE = 2 should not be used. This should be removed when fixed silicon is available.
    MCG_C2 = MCG_C2_LOCRE_MASK | MCG_C2_RANGE(2) | (/*hgo_val*/0 << MCG_C2_HGO_SHIFT) | (/*erefs_val*/0 << MCG_C2_EREFS_SHIFT);
//    MCG_C2 = MCG_C2_LOCRE_MASK | MCG_C2_RANGE(1) | (/*hgo_val*/0 << MCG_C2_HGO_SHIFT) | (/*erefs_val*/0 << MCG_C2_EREFS_SHIFT);
#endif
    
    // select clock mode, we want FBE mode
    // CLKS = 2, FRDIV = frdiv_val, IREFS = 0, IRCLKEN = 0, IREFSTEN = 0
    MCG_C1 = MCG_C1_CLKS(2) | MCG_C1_FRDIV(FRDIV_VAL);
    
    /* wait until the MCG has moved into the proper mode */
    // if the external oscillator is used need to wait for OSCINIT to set
//      for (i = 0 ; i < 10000 ; i++)
//      {
//        if (MCG_S & MCG_S_OSCINIT_MASK) break; // jump out early if OSCINIT sets before loop finishes
//      }
//      if (!(MCG_S & MCG_S_OSCINIT_MASK)) return 0x23; // check bit is really set and return with error if not set
    
    // wait for reference clock status bit is cleared and clock source is ext ref clk
    while ((MCG_S & MCG_S_IREFST_MASK) || MCG_S_CLKST(2) != (MCG_S & MCG_S_CLKST_MASK));

    // ... FBE mode
    
    // enable clock monitor for osc0
    MCG_C6 = MCG_C6_CME_MASK;

    // PLL0
    MCG_C5 = MCG_C5_PRDIV(PRDIV_VAL - 1);       // set PLL0 ref divider, osc0 is reference

    MCG_C6 = MCG_C6_PLLS_MASK | MCG_C6_VDIV(VDIV_VAL - 16);     // set VDIV and enable PLL
        
    // wait to lock...
    while (!(MCG_S & MCG_S_PLLST_MASK));
    while (!(MCG_S & MCG_S_LOCK_MASK));
    
//    // Use actual PLL settings to calculate PLL frequency
//    prdiv = ((MCG_C5 & MCG_C5_PRDIV_MASK) + 1);
//    vdiv = ((MCG_C6 & MCG_C6_VDIV_MASK) + 16);

    // ... PBE mode
    MCG_C1 &= ~MCG_C1_CLKS_MASK;        // CLKS = 0, select PLL as MCG_OUT
    
    while (MCG_S_CLKST(3) != (MCG_S & MCG_S_CLKST_MASK));

    // ... PEE mode
    
/* ToDo: add code to initialize the system
         do not use global variables because this function is called before
         reaching pre-main. RW section maybe overwritten afterwards.          */
  SystemCoreClock = __SYSTEM_CLOCK;
}

status_t CLOCK_SetFbiMode(mcg_dmx32_t dmx32, mcg_drs_t drs, void (*fllStableDelay)(void))
{
    uint8_t mcg_c4;
    bool change_drs = false;

#if (defined(MCG_CONFIG_CHECK_PARAM) && MCG_CONFIG_CHECK_PARAM)
    mcg_mode_t mode = CLOCK_GetMode();

    if (!((kMCG_ModeFEE == mode) || (kMCG_ModeFBI == mode) || (kMCG_ModeFBE == mode) || (kMCG_ModeFEI == mode) ||
          (kMCG_ModeBLPI == mode)))

    {
        return kStatus_MCG_ModeUnreachable;
    }
#endif

    mcg_c4 = MCG->C4;

    MCG->C2 &= ~MCG_C2_LP_MASK; /* Disable lowpower. */

    /*
       Errata: ERR007993
       Workaround: Invert MCG_C4[DMX32] or change MCG_C4[DRST_DRS] before
       reference clock source changes, then reset to previous value after
       reference clock changes.
     */
    if (kMCG_FllSrcExternal == MCG_S_IREFST_VAL)
    {
        change_drs = true;
        /* Change the LSB of DRST_DRS. */
        MCG->C4 ^= (1U << MCG_C4_DRST_DRS_SHIFT);
    }

    /* Set CLKS and IREFS. */
    MCG->C1 =
        ((MCG->C1 & ~(MCG_C1_CLKS_MASK | MCG_C1_IREFS_MASK)) | (MCG_C1_CLKS(kMCG_ClkOutSrcInternal)    /* CLKS = 1 */
                                                                | MCG_C1_IREFS(kMCG_FllSrcInternal))); /* IREFS = 1 */

    /* Wait and check status. */
    while (kMCG_FllSrcInternal != MCG_S_IREFST_VAL)
    {
    }

    /* Errata: ERR007993 */
    if (change_drs)
    {
        MCG->C4 = mcg_c4;
    }

    while (kMCG_ClkOutStatInt != MCG_S_CLKST_VAL)
    {
    }

    MCG->C4 = (mcg_c4 & ~(MCG_C4_DMX32_MASK | MCG_C4_DRST_DRS_MASK)) | (MCG_C4_DMX32(dmx32) | MCG_C4_DRST_DRS(drs));

    /* Wait for FLL stable time. */
    if (fllStableDelay)
    {
        fllStableDelay();
    }

    return kStatus_Success;
}

/*
** ===================================================================
**     Method      :  Cpu_SetOperationMode (component MK22FN512VDC12)
**
**     Description :
**         This method requests to change the component's operation
**         mode (RUN, WAIT, SLEEP, STOP). The target operation mode
**         will be entered immediately.
**         See <Operation mode settings> for further details of the
**         operation modes mapping to low power modes of the cpu.
**     Parameters  :
**         NAME            - DESCRIPTION
**         OperationMode   - Requested driver
**                           operation mode
**         ModeChangeCallback - Callback to
**                           notify the upper layer once a mode has been
**                           changed. Parameter is ignored, only for
**                           compatibility of API with other components.
**       * ModeChangeCallbackParamPtr
**                           - Pointer to callback parameter to notify
**                           the upper layer once a mode has been
**                           changed. Parameter is ignored, only for
**                           compatibility of API with other components.
**     Returns     :
**         ---             - Error code
**                           ERR_OK - OK
**                           ERR_PARAM_MODE - Invalid operation mode
** ===================================================================
*/
LDD_TError Cpu_SetOperationMode(LDD_TDriverOperationMode OperationMode, LDD_TCallback ModeChangeCallback, LDD_TCallbackParam *ModeChangeCallbackParamPtr)
{
  (void) ModeChangeCallback;           /* Parameter is not used, suppress unused argument warning */
  (void) ModeChangeCallbackParamPtr;   /* Parameter is not used, suppress unused argument warning */
  switch (OperationMode) {
      case DOM_HSRUN:
      SMC_PMPROT = SMC_PMPROT_AHSRUN_MASK;
      SMC_PMCTRL |= SMC_PMCTRL_RUNM(3); /*HS RUN */
      while((SMC_PMSTAT & SMC_PMSTAT_PMSTAT_MASK) != SMC_PMSTAT_PMSTAT(0x80)) {  /* HS RUN status */
      };
       /* SCB_SCR: SLEEPDEEP=0,SLEEPONEXIT=0 */
      SCB_SCR &= (uint32_t)~(uint32_t)(
                  SCB_SCR_SLEEPDEEP_MASK |
                  SCB_SCR_SLEEPONEXIT_MASK
                 );
      if  (ClockConfigurationID != 2U) {
        if ((MCG_S & MCG_S_CLKST_MASK) != MCG_S_CLKST(3)) { /* If in PBE mode, switch to PEE. PEE to PBE transition was caused by wakeup from low power mode. */
          /* MCG_C1: CLKS=0,IREFS=0 */
	  MCG_C1 &= (uint8_t)~(uint8_t)((MCG_C1_CLKS(0x03) | MCG_C1_IREFS_MASK));
          while( (MCG_S & MCG_S_LOCK0_MASK) == 0x00U) { /* Wait for PLL lock */
          }
        }
      }
      break;
    case DOM_RUN:
      SMC_PMCTRL &= ~(SMC_PMCTRL_RUNM(3)); /*Normal RUN */
      while((SMC_PMSTAT & SMC_PMSTAT_PMSTAT_MASK) != SMC_PMSTAT_PMSTAT(0x1)) {  /* Normal RUN status */
      };
      /* SCB_SCR: SLEEPDEEP=0,SLEEPONEXIT=0 */
      SCB_SCR &= (uint32_t)~(uint32_t)(
                  SCB_SCR_SLEEPDEEP_MASK |
                  SCB_SCR_SLEEPONEXIT_MASK
                 );
      if  (ClockConfigurationID != 2U) {
        if ((MCG_S & MCG_S_CLKST_MASK) != MCG_S_CLKST(3)) { /* If in PBE mode, switch to PEE. PEE to PBE transition was caused by wakeup from low power mode. */
          /* MCG_C1: CLKS=0,IREFS=0 */
	  MCG_C1 &= (uint8_t)~(uint8_t)((MCG_C1_CLKS(0x03) | MCG_C1_IREFS_MASK));
          while( (MCG_S & MCG_S_LOCK0_MASK) == 0x00U) { /* Wait for PLL lock */
          }
        }
      }
      break;
    case DOM_WAIT:
      /* SCB_SCR: SLEEPDEEP=0 */
      SCB_SCR &= (uint32_t)~(uint32_t)(SCB_SCR_SLEEPDEEP_MASK);
      /* SCB_SCR: SLEEPONEXIT=0 */
      SCB_SCR &= (uint32_t)~(uint32_t)(SCB_SCR_SLEEPONEXIT_MASK);
      PE_WFI();
      break;
    case DOM_SLEEP:
      /* SCB_SCR: SLEEPDEEP=1 */
      SCB_SCR |= SCB_SCR_SLEEPDEEP_MASK;
      /* SMC_PMCTRL: STOPM=0 */
      SMC_PMCTRL &= (uint8_t)~(uint8_t)(SMC_PMCTRL_STOPM(0x07));
      (void)(SMC_PMCTRL == 0U);        /* Dummy read of SMC_PMCTRL to ensure the register is written before enterring low power mode */
      /* SCB_SCR: SLEEPONEXIT=1 */
      SCB_SCR |= SCB_SCR_SLEEPONEXIT_MASK;
      PE_WFI();
      break;
    case DOM_STOP:
      break;
    default:
      return ERR_PARAM_MODE;
  }
  return ERR_OK;
}

/*! @brief Sets up the clock out of RESET
 *
 */
void clock_initialise(void) {

#if (CLOCK_MODE == CLOCK_MODE_NONE)
   // No clock setup
#else
   // XTAL/EXTAL Pins
   SIM->SCGC5  |= SIM_SCGC5_PORTA_MASK;
   PORTA->PCR[3]  = PORT_PCR_MUX(0);
   PORTA->PCR[4]  = PORT_PCR_MUX(0);

   // Configure the Crystal Oscillator
   RTC->CR = RTC_CR_WPE_M|RTC_CR_SUP_M|RTC_CR_UM_M|RTC_CR_OSCE_M|RTC_CR_CLKO_M|RTC_CR_SCP_M;

   // Fast Internal Clock divider
   MCG->SC = MCG_SC_FCRDIV_M;

   // Out of reset MCG is in FEI mode
   // =============================================================

   SIM->CLKDIV1 = SIM_CLKDIV1_OUTDIV1(3) | SIM_CLKDIV1_OUTDIV2(7) | SIM_CLKDIV1_OUTDIV3(3) | SIM_CLKDIV1_OUTDIV4(7);

   // Switch from FEI -> FEI/FBI/FEE/FBE
   // =============================================================

   // Set up crystal or external clock source
   MCG->C2 =
            MCG_C2_LOCRE0_M     | // LOCRE0 = 0,1   -> Loss of clock reset enable
            MCG_C2_RANGE0_M     | // RANGE0 = 0,1,2 -> Oscillator low/high/very high clock range
            MCG_C2_HGO0_M       | // HGO0   = 0,1   -> Oscillator low power/high gain
            MCG_C2_EREFS0_M     | // EREFS0 = 0,1   -> Select external clock/crystal oscillator
            MCG_C2_IRCS_M;        // IRCS   = 0,1   -> Select slow/fast internal clock for internal reference

#if ((CLOCK_MODE == CLOCK_MODE_FEI) || (CLOCK_MODE == CLOCK_MODE_FBI) || (CLOCK_MODE == CLOCK_MODE_BLPI) )
   // Transition via FBI
   //=====================================
#define BYPASS (1) // CLKS value used while FLL locks
   MCG->C1 =  MCG_C1_CLKS(BYPASS)     | // CLKS     = X     -> External reference source while PLL locks
              MCG_C1_FRDIV_M          | // FRDIV    = N     -> XTAL/2^n ~ 31.25 kHz
              MCG_C1_IREFS_M          | // IREFS    = 0,1   -> External/Slow IRC for FLL source
              MCG_C1_IRCLKEN_M        | // IRCLKEN  = 0,1   -> IRCLK disable/enable
              MCG_C1_IREFSTEN_M;        // IREFSTEN = 0,1   -> Internal reference enabled in STOP mode

   // Wait for S_IREFST to indicate FLL Reference has switched
   do {
      __asm__("nop");
   } while ((MCG->S & MCG_S_IREFST_MASK) != (MCG_C1_IREFS_V<<MCG_S_IREFST_SHIFT));

   // Wait for S_CLKST to indicating that OUTCLK has switched to bypass PLL/FLL
   do {
      __asm__("nop");
   } while ((MCG->S & MCG_S_CLKST_MASK) != MCG_S_CLKST(BYPASS));

   // Set FLL Parameters
   MCG->C4 = (MCG->C4&~(MCG_C4_DMX32_MASK|MCG_C4_DRST_DRS_MASK))|MCG_C4_DMX32_M|MCG_C4_DRST_DRS_M;
#endif

#if ((CLOCK_MODE == CLOCK_MODE_FBE) || (CLOCK_MODE == CLOCK_MODE_FEE) || (CLOCK_MODE == CLOCK_MODE_PLBE) || (CLOCK_MODE == CLOCK_MODE_PBE) || (CLOCK_MODE == CLOCK_MODE_PEE))

   // Transition via FBE
   //=====================================
#define BYPASS (2) // CLKS value used while PLL locks
   MCG->C1 =  MCG_C1_CLKS(BYPASS)     | // CLKS     = 2     -> External reference source while PLL locks
              MCG_C1_FRDIV_M          | // FRDIV    = N     -> XTAL/2^n ~ 31.25 kHz
              MCG_C1_IREFS_M          | // IREFS    = 0,1   -> External/Slow IRC for FLL source
              MCG_C1_IRCLKEN_M        | // IRCLKEN  = 0,1   -> IRCLK disable/enable
              MCG_C1_IREFSTEN_M;        // IREFSTEN = 0,1   -> Internal reference enabled in STOP mode

#if (MCG_C2_EREFS_V != 0)
   // Wait for oscillator stable (if used)
   do {
      __asm__("nop");
   } while ((MCG->S & MCG_S_OSCINIT0_MASK) == 0);
#endif

   // Wait for S_IREFST to indicate FLL Reference has switched
   do {
      __asm__("nop");
   } while ((MCG->S & MCG_S_IREFST_MASK) != (MCG_C1_IREFS_V<<MCG_S_IREFST_SHIFT));

   // Wait for S_CLKST to indicating that OUTCLK has switched to bypass PLL/FLL
   do {
      __asm__("nop");
   } while ((MCG->S & MCG_S_CLKST_MASK) != MCG_S_CLKST(BYPASS));

   // Set FLL Parameters
   MCG->C4 = (MCG->C4&~(MCG_C4_DMX32_MASK|MCG_C4_DRST_DRS_MASK))|MCG_C4_DMX32_M|MCG_C4_DRST_DRS_M;
#endif

   // Select FEI/FBI/FEE/FBE clock mode
   MCG->C1 =  MCG_C1_CLKS_M       | // CLKS     = 0,1,2 -> Select FLL/IRCSCLK/ERCLK
              MCG_C1_FRDIV_M      | // FRDIV    = N     -> XTAL/2^n ~ 31.25 kHz
              MCG_C1_IREFS_M      | // IREFS    = 0,1   -> External/Slow IRC for FLL source
              MCG_C1_IRCLKEN_M    | // IRCLKEN  = 0,1   -> IRCLK disable/enable
              MCG_C1_IREFSTEN_M;    // IREFSTEN = 0,1   -> Internal reference enabled in STOP mode

   // Wait for mode change
   do {
//.........这里部分代码省略.........

void Boot_Init_Clock(void){
	
	  /* System clock initialization */
	  /* SIM_CLKDIV1: OUTDIV1=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,OUTDIV4=3,??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0 */
	  SIM_CLKDIV1 = (SIM_CLKDIV1_OUTDIV1(0x00) | SIM_CLKDIV1_OUTDIV4(0x03)); /* Set the system prescalers to safe value */
	  /* SIM_SCGC5: PORTA=1 */
	  SIM_SCGC5 |= SIM_SCGC5_PORTA_MASK;   /* Enable clock gate for ports to enable pin routing */
	  if ((PMC_REGSC & PMC_REGSC_ACKISO_MASK) != 0x0U) {
	    /* PMC_REGSC: ACKISO=1 */
	    PMC_REGSC |= PMC_REGSC_ACKISO_MASK; /* Release IO pads after wakeup from VLLS mode. */
	  }
	  /* SIM_CLKDIV1: OUTDIV1=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,OUTDIV4=1,??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0 */
	  SIM_CLKDIV1 = (SIM_CLKDIV1_OUTDIV1(0x00) | SIM_CLKDIV1_OUTDIV4(0x01)); /* Update system prescalers */
	  /* SIM_SOPT2: PLLFLLSEL=1 */
	  SIM_SOPT2 |= SIM_SOPT2_PLLFLLSEL_MASK; /* Select PLL as a clock source for various peripherals */
	  /* SIM_SOPT1: OSC32KSEL=3 */
	  SIM_SOPT1 |= SIM_SOPT1_OSC32KSEL(0x03); /* LPO 1kHz oscillator drives 32 kHz clock for various peripherals */
	  /* SIM_SOPT2: TPMSRC=1 */
	  SIM_SOPT2 = (uint32_t)((SIM_SOPT2 & (uint32_t)~(uint32_t)(
	               SIM_SOPT2_TPMSRC(0x02)
	              )) | (uint32_t)(
	               SIM_SOPT2_TPMSRC(0x01)
	              ));                      /* Set the TPM clock */
	  /* PORTA_PCR18: ISF=0,MUX=0 */
	  PORTA_PCR18 &= (uint32_t)~(uint32_t)((PORT_PCR_ISF_MASK | PORT_PCR_MUX(0x07)));
	  /* PORTA_PCR19: ISF=0,MUX=0 */
	  PORTA_PCR19 &= (uint32_t)~(uint32_t)((PORT_PCR_ISF_MASK | PORT_PCR_MUX(0x07)));
	  /* Switch to FBE Mode */
	  /* MCG_C2: LOCRE0=0,RANGE0=2,HGO0=0,EREFS0=1,LP=0,IRCS=0 */
	  MCG_C2 = (uint8_t)((MCG_C2 & (uint8_t)~(uint8_t)(
	            MCG_C2_LOCRE0_MASK |
	            MCG_C2_RANGE0(0x01) |
	            MCG_C2_HGO0_MASK |
	            MCG_C2_LP_MASK |
	            MCG_C2_IRCS_MASK
	           )) | (uint8_t)(
	            MCG_C2_RANGE0(0x02) |
	            MCG_C2_EREFS0_MASK
	           ));
	  /* OSC0_CR: ERCLKEN=1,??=0,EREFSTEN=0,??=0,SC2P=0,SC4P=0,SC8P=0,SC16P=0 */
	  OSC0_CR = OSC_CR_ERCLKEN_MASK;
	  /* MCG_C1: CLKS=2,FRDIV=3,IREFS=0,IRCLKEN=1,IREFSTEN=0 */
	  MCG_C1 = (MCG_C1_CLKS(0x02) | MCG_C1_FRDIV(0x03) | MCG_C1_IRCLKEN_MASK);
	  /* MCG_C4: DMX32=0,DRST_DRS=0 */
	  MCG_C4 &= (uint8_t)~(uint8_t)((MCG_C4_DMX32_MASK | MCG_C4_DRST_DRS(0x03)));
	  /* MCG_C5: ??=0,PLLCLKEN0=0,PLLSTEN0=0,PRDIV0=3 */
	  MCG_C5 = MCG_C5_PRDIV0(0x03);
	  /* MCG_C6: LOLIE0=0,PLLS=0,CME0=0,VDIV0=0 */
	  MCG_C6 = MCG_C6_VDIV0(0x00);
	  while((MCG_S & MCG_S_IREFST_MASK) != 0x00U) { /* Check that the source of the FLL reference clock is the external reference clock. */
	  }
	  while((MCG_S & 0x0CU) != 0x08U) {    /* Wait until external reference clock is selected as MCG output */
	  }
	  /* Switch to PBE Mode */
	  /* MCG_C6: LOLIE0=0,PLLS=1,CME0=0,VDIV0=0 */
	  MCG_C6 = (MCG_C6_PLLS_MASK | MCG_C6_VDIV0(0x00));
	  while((MCG_S & 0x0CU) != 0x08U) {    /* Wait until external reference clock is selected as MCG output */
	  }
	  while((MCG_S & MCG_S_LOCK0_MASK) == 0x00U) { /* Wait until locked */
	  }
	  /* Switch to PEE Mode */
	  /* MCG_C1: CLKS=0,FRDIV=3,IREFS=0,IRCLKEN=1,IREFSTEN=0 */
	  MCG_C1 = (MCG_C1_CLKS(0x00) | MCG_C1_FRDIV(0x03) | MCG_C1_IRCLKEN_MASK);
	  while((MCG_S & 0x0CU) != 0x0CU) {    /* Wait until output of the PLL is selected */
	  }
	  /* MCG_C6: CME0=1 */
	  MCG_C6 |= MCG_C6_CME0_MASK;          /* Enable the clock monitor */
	  /*** End of PE initialization code after reset ***/
}

void __startup(void) {
  // The CPU has a watchdog feature which is on by default,
  // so we have to configure it to not have nasty reset-surprises
  // later on.
  startup_watchdog_hook();

  // If the system was in VLLS mode, some peripherials and 
  // the I/O pins are in latched mode. We need to restore
  // config and can then acknowledge the isolation to get back
  // to normal. For now, we'll just ack TODO: properly do this
  if (PMC_REGSC & PMC_REGSC_ACKISO_MASK) PMC_REGSC |= PMC_REGSC_ACKISO_MASK;

  // There is a write-once-after-reset register that allows to
  // set which power states are available. Let's set it here.
  SMC_PMPROT = ENABLED_POWER_MODES;

  // For the sake of simplicity, enable all GPIO port clocks
  SIM_SCGC5 |= (  SIM_SCGC5_PORTA_MASK
                | SIM_SCGC5_PORTB_MASK
                | SIM_SCGC5_PORTC_MASK
	        | SIM_SCGC5_PORTD_MASK
	        | SIM_SCGC5_PORTE_MASK);

  // ----------------------------------------------------------------------------------
  // Setup clocks
  // ----------------------------------------------------------------------------------
  // See section 5 in the Freescale K20 manual for how clock distribution works
  // The limits are outlined in section 5.5:
  //   Core and System clocks: max 72 MHz
  //   Bus/peripherial clock:  max 50 MHz (integer divide of core)
  //   Flash clock:            max 25 MHz
  //
  // The teensy 3.x has a 16 MHz external oscillator
  // So we'll enable the external clock for the OSC module. Since
  // we're in high-frequency mode, also enable capacitors
  OSC_CR = OSC_CR_SC8P_MASK | OSC_CR_SC2P_MASK; // TODO This does not actually seem enable the ext crystal

  // Set MCG to very high frequency crystal and request oscillator. We have
  // to do this first so that the divisor will be correct (512 and not 16)
  MCG_C2 = MCG_C2_RANGE0(2) | MCG_C2_EREFS0_MASK;

  // Select the external reference clock for MCGOUTCLK
  // The divider for the FLL has to be chosen that we get something in 31.25 to 39.0625 kHz
  // 16MHz / 512 = 31.25 kHz -> set FRDIV to 4
  MCG_C1 =  MCG_C1_CLKS(2) | MCG_C1_FRDIV(4);

  // Wait for OSC to become ready
  while ((MCG_S & MCG_S_OSCINIT0_MASK) == 0) ;

  // Wait for the FLL to synchronize to external reference
  while ((MCG_S & MCG_S_IREFST_MASK) != 0) ;

  // Wait for the clock mode to synchronize to external
  while ((MCG_S & MCG_S_CLKST_MASK) != MCG_S_CLKST(2)) ;

  // The clock is now in FBE mode

#if F_CPU <= 16000000
  // For 16 MHz and below, the crystal is fast enough
  // -> enable BLPE mode which will disable both FLL and PLL
  MCG_C2 = MCG_C2_RANGE0(2) | MCG_C2_EREFS_MASK | MCG_C2_LP_MASK;
#else
  // We need PLL to go above 16 MHz
  #if   F_CPU == 96000000
    MCG_C5 = MCG_C5_PRDIV0(3); // 16MHz / 4 = 4MHz (this needs to be 2-4MHz)
    MCG_C6 = MCG_C6_PLLS_MASK | MCG_C6_VDIV0(0); // Enable PLL*24 = 96 MHz
  #elif F_CPU == 72000000
    MCG_C5 = MCG_C5_PRDIV0(5); // 16 MHz / 6 = 2.66 MHz (this needs to be 2-4MHz)
    MCG_C6 = MCG_C6_PLLS_MASK | MCG_C6_VDIV0(3); // Enable PLL*27 = 71.82 MHz
  #elif F_CPU == 48000000
    MCG_C5 = MCG_C5_PRDIV0(7); // 16 MHz / 8 = 2 MHz (this needs to be 2-4MHz)
    MCG_C6 = MCG_C6_PLLS_MASK | MCG_C6_VDIV0(0); // Enable PLL*24 = 48 MHz
  #elif F_CPU == 24000000
    // For 24 MHz, we'll use a 48 MHz PLL and divide in the SIM
    MCG_C5 = MCG_C5_PRDIV0(7); // 16 MHz / 8 = 2 MHz (this needs to be 2-4MHz)
    MCG_C6 = MCG_C6_PLLS_MASK | MCG_C6_VDIV0(0); // Enable PLL*24 = 48 MHz
  #else
    #error "Unknown F_CPU value"
  #endif

  // Now that we setup and enabled the PLL, wait for it to become active
  while (!(MCG_S & MCG_S_PLLST_MASK)) ;
  // and locked
  while (!(MCG_S & MCG_S_LOCK0_MASK)) ;
#endif

  // Next up: Setup clock dividers for MCU, peripherials, flash and USB
  // This is done by the SIM (System Integration Module)
  // There are two registers:
  //  SIM_CLKDIV1:
  //   OUTDIV1: Core/system clock divider
  //   OUTDIV2: Peripherial/Bus clock
  //   OUTDIV4: Flash clock
  //  SIM_CLKDIV2:
  //   USBDIV: Divisor
  //   USBFRAC: Fraction
  //   Output is input_clock*(USBFRAC+1)/(USBDIV+1)
  //
  // USB needs a 48MHz clock, so the divider should be setup accordingly. Also,
  // for the USB FS OTG controller to work, the system clock needs to be >= 20 MHz
//.........这里部分代码省略.........

/*lint -esym(765,Cpu_Interrupt) Disable MISRA rule (8.10) checking for symbols (Cpu_Interrupt). */
void __init_hardware(void)
{

  /*** !!! Here you can place your own code before PE initialization using property "User code before PE initialization" on the build options tab. !!! ***/

  /*** ### MK60DN512ZVLQ10 "Cpu" init code ... ***/
  /*** PE initialization code after reset ***/
  SCB_VTOR = (uint32_t)(&__vect_table); /* Set the interrupt vector table position */
  /* Disable the WDOG module */
  /* WDOG_UNLOCK: WDOGUNLOCK=0xC520 */
  WDOG_UNLOCK = WDOG_UNLOCK_WDOGUNLOCK(0xC520); /* Key 1 */
  /* WDOG_UNLOCK: WDOGUNLOCK=0xD928 */
  WDOG_UNLOCK = WDOG_UNLOCK_WDOGUNLOCK(0xD928); /* Key 2 */
  /* WDOG_STCTRLH: ??=0,DISTESTWDOG=0,BYTESEL=0,TESTSEL=0,TESTWDOG=0,??=0,STNDBYEN=1,WAITEN=1,STOPEN=1,DBGEN=0,ALLOWUPDATE=1,WINEN=0,IRQRSTEN=0,CLKSRC=1,WDOGEN=0 */
  WDOG_STCTRLH = WDOG_STCTRLH_BYTESEL(0x00) |
                 WDOG_STCTRLH_STNDBYEN_MASK |
                 WDOG_STCTRLH_WAITEN_MASK |
                 WDOG_STCTRLH_STOPEN_MASK |
                 WDOG_STCTRLH_ALLOWUPDATE_MASK |
                 WDOG_STCTRLH_CLKSRC_MASK;

  /* System clock initialization */
  /* SIM_CLKDIV1: OUTDIV1=0,OUTDIV2=1,OUTDIV3=3,OUTDIV4=3,??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0 */
  SIM_CLKDIV1 = SIM_CLKDIV1_OUTDIV1(0x00) |
                SIM_CLKDIV1_OUTDIV2(0x01) |
                SIM_CLKDIV1_OUTDIV3(0x03) |
                SIM_CLKDIV1_OUTDIV4(0x03); /* Set the system prescalers to safe value */
  /* SIM_SCGC5: PORTC=1,PORTA=1 */
  SIM_SCGC5 |= (SIM_SCGC5_PORTC_MASK | SIM_SCGC5_PORTA_MASK); /* Enable clock gate for ports to enable pin routing */
  /* SIM_CLKDIV1: OUTDIV1=0,OUTDIV2=0,OUTDIV3=1,OUTDIV4=1,??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0 */
  SIM_CLKDIV1 = SIM_CLKDIV1_OUTDIV1(0x00) |
                SIM_CLKDIV1_OUTDIV2(0x00) |
                SIM_CLKDIV1_OUTDIV3(0x01) |
                SIM_CLKDIV1_OUTDIV4(0x01); /* Update system prescalers */
  /* SIM_CLKDIV2: USBDIV=0,USBFRAC=1 */
  SIM_CLKDIV2 = (uint32_t)((SIM_CLKDIV2 & (uint32_t)~(uint32_t)(
                 SIM_CLKDIV2_USBDIV(0x07)
                )) | (uint32_t)(
                 SIM_CLKDIV2_USBFRAC_MASK
                ));                    /* Update USB clock prescalers */
  /* SIM_SOPT2: PLLFLLSEL=0 */
  SIM_SOPT2 &= (uint32_t)~(uint32_t)(SIM_SOPT2_PLLFLLSEL_MASK); /* Select FLL as a clock source for various peripherals */
  /* SIM_SOPT1: OSC32KSEL=0 */
  SIM_SOPT1 &= (uint32_t)~(uint32_t)(SIM_SOPT1_OSC32KSEL_MASK); /* System oscillator drives 32 kHz clock for various peripherals */
  /* Switch to FEI Mode */
  /* MCG_C1: CLKS=0,FRDIV=0,IREFS=1,IRCLKEN=1,IREFSTEN=0 */
  MCG_C1 = MCG_C1_CLKS(0x00) |
           MCG_C1_FRDIV(0x00) |
           MCG_C1_IREFS_MASK |
           MCG_C1_IRCLKEN_MASK;
  /* MCG_C2: ??=0,??=0,RANGE=0,HGO=0,EREFS=0,LP=0,IRCS=0 */
  MCG_C2 = MCG_C2_RANGE(0x00);
  /* MCG_C4: DMX32=0,DRST_DRS=0 */
  MCG_C4 &= (uint8_t)~(uint8_t)((MCG_C4_DMX32_MASK | MCG_C4_DRST_DRS(0x03)));
  /* OSC_CR: ERCLKEN=1,??=0,EREFSTEN=0,??=0,SC2P=0,SC4P=0,SC8P=0,SC16P=0 */
  OSC_CR = OSC_CR_ERCLKEN_MASK;
  /* SIM_SOPT2: MCGCLKSEL=0 */
  SIM_SOPT2 &= (uint32_t)~(uint32_t)(SIM_SOPT2_MCGCLKSEL_MASK);
  /* MCG_C5: ??=0,PLLCLKEN=0,PLLSTEN=0,PRDIV=0 */
  MCG_C5 = MCG_C5_PRDIV(0x00);
  /* MCG_C6: LOLIE=0,PLLS=0,CME=0,VDIV=0 */
  MCG_C6 = MCG_C6_VDIV(0x00);
  while((MCG_S & MCG_S_IREFST_MASK) == 0x00U) { /* Check that the source of the FLL reference clock is the internal reference clock. */
  }
  while((MCG_S & 0x0CU) != 0x00U) {    /* Wait until output of the FLL is selected */
  }
  /*** End of PE initialization code after reset ***/

  /*** !!! Here you can place your own code after PE initialization using property "User code after PE initialization" on the build options tab. !!! ***/

}

status_t CLOCK_SetFeeMode(uint8_t frdiv, mcg_dmx32_t dmx32, mcg_drs_t drs, void (*fllStableDelay)(void))
{
    uint8_t mcg_c4;
    bool change_drs = false;

#if (defined(MCG_CONFIG_CHECK_PARAM) && MCG_CONFIG_CHECK_PARAM)
    mcg_mode_t mode = CLOCK_GetMode();
    if (!((kMCG_ModeFEE == mode) || (kMCG_ModeFBI == mode) || (kMCG_ModeFBE == mode) || (kMCG_ModeFEI == mode)))
    {
        return kStatus_MCG_ModeUnreachable;
    }
#endif
    mcg_c4 = MCG->C4;

    /*
       Errata: ERR007993
       Workaround: Invert MCG_C4[DMX32] or change MCG_C4[DRST_DRS] before
       reference clock source changes, then reset to previous value after
       reference clock changes.
     */
    if (kMCG_FllSrcInternal == MCG_S_IREFST_VAL)
    {
        change_drs = true;
        /* Change the LSB of DRST_DRS. */
        MCG->C4 ^= (1U << MCG_C4_DRST_DRS_SHIFT);
    }

    /* Set CLKS and IREFS. */
    MCG->C1 = ((MCG->C1 & ~(MCG_C1_CLKS_MASK | MCG_C1_FRDIV_MASK | MCG_C1_IREFS_MASK)) |
               (MCG_C1_CLKS(kMCG_ClkOutSrcOut)         /* CLKS = 0 */
                | MCG_C1_FRDIV(frdiv)                  /* FRDIV */
                | MCG_C1_IREFS(kMCG_FllSrcExternal))); /* IREFS = 0 */

    /* If use external crystal as clock source, wait for it stable. */
    {
        if (MCG->C2 & MCG_C2_EREFS_MASK)
        {
            while (!(MCG->S & MCG_S_OSCINIT0_MASK))
            {
            }
        }
    }

    /* Wait and check status. */
    while (kMCG_FllSrcExternal != MCG_S_IREFST_VAL)
    {
    }

    /* Errata: ERR007993 */
    if (change_drs)
    {
        MCG->C4 = mcg_c4;
    }

    /* Set DRS and DMX32. */
    mcg_c4 = ((mcg_c4 & ~(MCG_C4_DMX32_MASK | MCG_C4_DRST_DRS_MASK)) | (MCG_C4_DMX32(dmx32) | MCG_C4_DRST_DRS(drs)));
    MCG->C4 = mcg_c4;

    /* Wait for DRST_DRS update. */
    while (MCG->C4 != mcg_c4)
    {
    }

    /* Check MCG_S[CLKST] */
    while (kMCG_ClkOutStatFll != MCG_S_CLKST_VAL)
    {
    }

    /* Wait for FLL stable time. */
    if (fllStableDelay)
    {
        fllStableDelay();
    }

    return kStatus_Success;
}