/*
** ===================================================================
**     Method      :  Cpu_SetMCGModeFBE (component MK21FN1M0MC12)
**
**     Description :
**         This method sets the MCG to FBE mode.
**         This method is internal. It is used by Processor Expert only.
** ===================================================================
*/
static void Cpu_SetMCGModeFBE(uint8_t CLKMode)
{
  switch (CLKMode) {
    case 0U:
      /* Switch to FBE Mode */
      /* 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);
      /* 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=2,FRDIV=3,IREFS=0,IRCLKEN=0,IREFSTEN=0 */
      MCG_C1 = (MCG_C1_CLKS(0x02) | MCG_C1_FRDIV(0x03));
      /* 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=1 */
      MCG_C5 = MCG_C5_PRDIV0(0x01);
      /* MCG_C6: LOLIE0=0,PLLS=0,CME0=0,VDIV0=6 */
      MCG_C6 = MCG_C6_VDIV0(0x06);
      while((MCG_S & MCG_S_OSCINIT0_MASK) == 0x00U) { /* Check that the oscillator is running */
      }
      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 */
      }
      break;
    case 1U:
      /* Switch to FBE Mode */
      /* MCG_C2: LOCRE0=0,??=0,RANGE0=2,HGO0=0,EREFS0=1,LP=0,IRCS=1 */
      MCG_C2 = (MCG_C2_RANGE0(0x02) | MCG_C2_EREFS0_MASK | MCG_C2_IRCS_MASK);
      /* OSC_CR: ERCLKEN=1,??=0,EREFSTEN=0,??=0,SC2P=0,SC4P=0,SC8P=0,SC16P=0 */
      OSC_CR = OSC_CR_ERCLKEN_MASK;

      /* MCG_C7: OSCSEL=0 */
      MCG_C7 &= (uint8_t)~(uint8_t)(MCG_C7_OSCSEL_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=0 */
      MCG_C5 = MCG_C5_PRDIV0(0x00);
      /* MCG_C6: LOLIE0=0,PLLS=0,CME0=0,VDIV0=0 */
      MCG_C6 = MCG_C6_VDIV0(0x00);
      while((MCG_S & MCG_S_OSCINIT0_MASK) == 0x00U) { /* Check that the oscillator is running */
      }
      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 */
      }
      break;
    default:
      break;
  }
}

/**
 **===========================================================================
 **  Reset handler
 **===========================================================================
 */
void __init_hardware(void) {
	/* This is a cleaned up output of Processor Expert generated code */

	/* Set the interrupt vector table position */
	SCB_VTOR = (uint32_t)__vector_table;
#if 0
	/* Disable the WDOG module */
	SIM_COPC = SIM_COPC_COPT(0x00);                                   
#endif
	/* System clock initialization */
	/* Enable clock gate for ports to enable pin routing */
	SIM_SCGC5 |= SIM_SCGC5_PORTA_MASK;
	/* Update system prescalers */
	SIM_CLKDIV1 = SIM_CLKDIV1_OUTDIV1(0x00) | SIM_CLKDIV1_OUTDIV4(0x01);
	/* Select FLL as a clock source for various peripherals */
	SIM_SOPT2 &= ~SIM_SOPT2_PLLFLLSEL_MASK;
	/* LPO 1kHz oscillator drives 32 kHz clock for various peripherals */
	SIM_SOPT1 |= SIM_SOPT1_OSC32KSEL(0x03);
	/* Set the TPM clock */
	SIM_SOPT2 &= ~SIM_SOPT2_TPMSRC(0x01);
	SIM_SOPT2 |= SIM_SOPT2_TPMSRC(0x02);
	/* Enable XTAL IO pins */
	PORTA_PCR18 = PORT_PCR_MUX(0);
	PORTA_PCR19 = PORT_PCR_MUX(0);

	/* Switch to FBE Mode */
	MCG_C2 = MCG_C2_RANGE0(0x02) | MCG_C2_EREFS0_MASK;
	OSC0_CR = OSC_CR_ERCLKEN_MASK;
	MCG_C1 = MCG_C1_CLKS(0x02) | MCG_C1_FRDIV(0x03) | MCG_C1_IRCLKEN_MASK;
	MCG_C4 &= ~MCG_C4_DMX32_MASK | MCG_C4_DRST_DRS(0x03);
	MCG_C5 = MCG_C5_PRDIV0(0x03);
	MCG_C6 = MCG_C6_VDIV0(0x00);
	
	/* Check that the source of the FLL reference clock is the external reference clock. */
	while((MCG_S & MCG_S_IREFST_MASK) != 0x00U);
	/* Wait until external reference clock is selected as MCG output */
	while((MCG_S & 0x0CU) != 0x08U);

	/* Switch to PBE Mode */
	MCG_C6 = (MCG_C6_PLLS_MASK | MCG_C6_VDIV0(0x00));
	/* Wait until external reference clock is selected as MCG output */
	while((MCG_S & 0x0CU) != 0x08U);
	/* Wait until locked */
	while((MCG_S & MCG_S_LOCK0_MASK) == 0x00U);

	/* Switch to PEE Mode */
	MCG_C1 = MCG_C1_CLKS(0x00) | MCG_C1_FRDIV(0x03) | MCG_C1_IRCLKEN_MASK;
	/* Wait until output of the PLL is selected */
	while((MCG_S & 0x0CU) != 0x0CU);
}

/*
** ===================================================================
**     Method      :  Cpu_SetMCGModeBLPI (component MK21FN1M0MC12)
**
**     Description :
**         This method sets the MCG to BLPI mode.
**         This method is internal. It is used by Processor Expert only.
** ===================================================================
*/
static void Cpu_SetMCGModeBLPI(uint8_t CLKMode)
{
  switch (CLKMode) {
    case 1U:
      /* Switch to BLPI Mode */
      /* MCG_C1: CLKS=1,FRDIV=0,IREFS=1,IRCLKEN=1,IREFSTEN=0 */
      MCG_C1 = MCG_C1_CLKS(0x01) |
               MCG_C1_FRDIV(0x00) |
               MCG_C1_IREFS_MASK |
               MCG_C1_IRCLKEN_MASK;
      /* MCG_C2: LOCRE0=0,??=0,RANGE0=2,HGO0=0,EREFS0=1,LP=1,IRCS=1 */
      MCG_C2 = MCG_C2_RANGE0(0x02) |
               MCG_C2_EREFS0_MASK |
               MCG_C2_LP_MASK |
               MCG_C2_IRCS_MASK;
      /* OSC_CR: ERCLKEN=1,??=0,EREFSTEN=0,??=0,SC2P=0,SC4P=0,SC8P=0,SC16P=0 */
      OSC_CR = OSC_CR_ERCLKEN_MASK;
      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 & MCG_S_IRCST_MASK) == 0x00U) { /* Check that the fast external reference clock is selected. */
      }
      break;
    default:
      break;
  }
}

/*-------------------------------------------------------------------------------*/
void FBE(void)
	{
	
		MCG->C6 &= ~MCG_C6_CME0_MASK; 					//External clock monitor is disabled for OSC0.

    MCG->C2 |= MCG_C2_RANGE0(3) |						// Very high frequency range selected for the crystal oscillator 
               MCG_C2_EREFS0_MASK  ; 				//Oscillator requested
		
    MCG->C4 &= ~MCG_C4_DRST_DRS_MASK;   // Reset DCO Range
    MCG->C4 &= ~MCG_C4_DMX32_MASK;			// DCO Maximum Frequency
    MCG->C4 |= MCG_C4_DRST_DRS(1);			// 31.25 * 1280 = 40000kHz
    
    MCG->C6 &= ~MCG_C6_PLLS_MASK;				// Select FLL
			
    MCG->C1 &= ~MCG_C1_CLKS_MASK;				// Reset Clock Source Select
    MCG->C1 |= MCG_C1_CLKS(2) | 				//External reference clock is selected
						 	MCG_C1_FRDIV(3)| 					// Divide Factor is 256
							MCG_C1_IRCLKEN_MASK;			//MCGIRCLK active
					// Output of FLL is selected for MCGOUTCLK
    
    while((MCG->S & MCG_S_OSCINIT0_MASK) == 0); 		// wait for osc init
    while((MCG->S & MCG_S_PLLST_MASK) != 0); 				// wait for FLL
    while((MCG->S & MCG_S_CLKST_MASK) != MCG_S_CLKST(2)); // wait for EXTAL is selected
		 SIM->CLKDIV1 = SIM_CLKDIV1_OUTDIV1(1 - 1) |  				// core/system clock = MCGOUTCLK / 1 = 8 / 1 = 8MHz
     SIM_CLKDIV1_OUTDIV4(1 - 1);   // flash/bus clock = core/system / 1 = 8MHz
	
	}

/*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. !!! ***/

  /*** ### MKL25Z128VLK4 "Cpu" init code ... ***/
  /*** PE initialization code after reset ***/
  SCB_VTOR = (uint32_t)(&__vect_table); /* Set the interrupt vector table position */
  /* Disable the WDOG module */
  /* SIM_COPC: ??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,??=0,COPT=0,COPCLKS=0,COPW=0 */
  SIM_COPC = SIM_COPC_COPT(0x00);

  /* 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: PORTD=1,PORTB=1,PORTA=1 */
  SIM_SCGC5 |= SIM_SCGC5_PORTD_MASK |
               SIM_SCGC5_PORTB_MASK |
               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=0,??=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(0x00)); /* Update system 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=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 */
  /* 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: LOCRE0=0,??=0,RANGE0=0,HGO0=0,EREFS0=0,LP=0,IRCS=0 */
  MCG_C2 = MCG_C2_RANGE0(0x00);
  /* MCG_C4: DMX32=0,DRST_DRS=0 */
  MCG_C4 &= (uint8_t)~(uint8_t)((MCG_C4_DMX32_MASK | MCG_C4_DRST_DRS(0x03)));
  /* OSC0_CR: ERCLKEN=1,??=0,EREFSTEN=0,??=0,SC2P=0,SC4P=0,SC8P=0,SC16P=0 */
  OSC0_CR = OSC_CR_ERCLKEN_MASK;
  /* MCG_C5: ??=0,PLLCLKEN0=0,PLLSTEN0=0,PRDIV0=0 */
  MCG_C5 = MCG_C5_PRDIV0(0x00);
  /* 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 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. !!! ***/

}

/**
 * @brief Initialize the PLL Bypassed External Mode.
 *
 * MCGOUTCLK is derived from external reference clock (oscillator).
 * PLL output is not used.
 * Clock source is the external reference clock (oscillator).
 * The PLL loop will locks to VDIV times the frequency
 * corresponding by PRDIV.
 * Previous allowed mode are FBE or BLPE.
 */
static void kinetis_mcg_set_pbe(void)
{
    /* select external reference clock and divide factor */
    MCG->C1 = (uint8_t)(MCG_C1_CLKS(2) | MCG_C1_FRDIV(KINETIS_MCG_ERC_FRDIV));

    /* Wait until ERC is selected */
    while ((MCG->S & MCG_S_CLKST_MASK) != MCG_S_CLKST(2));

    /* PLL is not disabled in bypass mode */
    MCG->C2 &= ~(uint8_t)(MCG_C2_LP_MASK);

    /* set external reference devider */
    MCG->C5 = (uint8_t)(MCG_C5_PRDIV0(KINETIS_MCG_PLL_PRDIV));

    /* set external reference devider */
    MCG->C6 = (uint8_t)(MCG_C6_VDIV0(KINETIS_MCG_PLL_VDIV0));

    /* select PLL */
    MCG->C6 |= (uint8_t)(MCG_C6_PLLS_MASK);

    /* Wait until the source of the PLLS clock is PLL */
    while ((MCG->S & MCG_S_PLLST_MASK) == 0);

    /* Wait until PLL locked */
    while ((MCG->S & MCG_S_LOCK0_MASK) == 0);

    current_mode = KINETIS_MCG_PBE;
}

void FEI_to_FBE (void)
{
	SIM->SCGC5 |= SIM_SCGC5_PORTA_MASK;  	// enable clock for port A
	
	MCG->C2 |= MCG_C2_RANGE0(0x1) ;				// Very high frequency range selected for the crystal oscillator 
  MCG->C2 &= ~MCG_C2_HGO0_MASK ; 				// Configure crystal oscillator for low-power operation.
	MCG->C2 |= MCG_C2_EREFS0_MASK ;				// Select Oscillator requested.
	
	MCG->C6 &=~MCG_C6_PLLS_MASK ; 				// FLL is selected
	MCG->C2 &=~MCG_C2_LP_MASK   ; 				// FLL or PLL is not disabled in bypass modes .
	
	MCG->C1 |= MCG_C1_CLKS(0x2);  				// External reference clock is selected.
	MCG->C1 &=~MCG_C1_IREFS_MASK; 				// Selects the reference clock source for the FLL. External reference clock is selected.
	MCG->C1 |=MCG_C1_FRDIV(0x3) ;   			// Divide Factor is 256, OSC clock = 8MHz => 8000 / 256 = 31.25kHz
	MCG->C1 |=MCG_C1_IRCLKEN_MASK ;				// Enable Internal reference clock as MCGIRCLK
	
	MCG->C4 &= ~MCG_C4_DMX32_MASK;  			// FLL Factor =1280 
  MCG->C4 |= MCG_C4_DRST_DRS(1); 				// Reference range in 31.25–39.0625 kHz => FLL output=31.25 * 1280 = 40MHz
	OSC0->CR = (uint8_t)0x80U;
	
	/*Waiting for everything is config*/
	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 , MCG_S_CLKST*/
  }
	
	
}

void FBE_Mode(void)
{		
	/* MCG->C1: CLKS=2,FRDIV=3,IREFS=0,IRCLKEN=1,IREFSTEN=0 */
	MCG->C1 &= ~MCG_C1_IREFS_MASK;	// External reference clock is selected.
	MCG->C1 |= MCG_C1_CLKS(2)|      
						 MCG_C1_FRDIV(3); 		// Output of FLL is selected for MCGOUTCLK, Divide Factor is 256
	
	/* MCG_C2: LOCRE0=0,RANGE0=3,HGO0=0,EREFS0=1,LP=0,IRCS=0 */
	MCG->C2 |= MCG_C2_RANGE0(3) | 	// Very high frequency range selected for the crystal oscillator 
             MCG_C2_EREFS0_MASK;  // Oscillator requested
	
	/* OSC0->CR: ERCLKEN=1,??=0,EREFSTEN=0,??=0,SC2P=1,SC4P=0,SC8P=0,SC16P=0 */
  OSC0->CR = (uint8_t)0x80U;
	
	/* MCG->C4: DMX32=0,DRST_DRS=1 */
	MCG->C4 &= ~MCG_C4_DMX32_MASK;
	MCG->C4 |= MCG_C4_DRST_DRS_MASK; // MCGFLLCLK: 31.25kHz * 1280 = 40MHz

	/* MCG->C6: LOLIE0=0,PLLS=0,CME0=0,VDIV0=0 */
	MCG->C6 &= ~MCG_C6_CME0_MASK;		 // External clock monitor is disabled for OSC0
	MCG->C6 &= ~MCG_C6_PLLS_MASK;		 // FLL is selected
	
  while((MCG->S & MCG_S_IREFST_MASK) != 0); 						// wait for External clock is selected
  while((MCG->S & MCG_S_CLKST_MASK) != MCG_S_CLKST(2)); // wait for EXTAL is selected

	/* SIM->CLKDIV1: OUTDIV1=0,OUTDIV4=0,*/
	/* OUTDIV1 = 0 (Divide-by-1), OUTDIV4 = 0 (Divide-by-1) */
	SIM->CLKDIV1 &= ~SIM_CLKDIV1_OUTDIV1_MASK;  // MCGOUTCLK = 8MHz; Core clock = 8/1 = 8 MHz
	SIM->CLKDIV1 &= ~SIM_CLKDIV1_OUTDIV4_MASK;   // Bus clock = 8/1 = 8MHz
}

void PEE(void)
{
	  MCG->C6 &= ~MCG_C6_CME0_MASK;

    MCG->C2 &= ~MCG_C2_LP_MASK;
    MCG->C2 |= MCG_C2_RANGE0(3) |// Very high frequency range selected for the crystal oscillator 
               MCG_C2_EREFS0_MASK ;

    MCG->C5 &= ~MCG_C5_PRDIV0_MASK;
    MCG->C5 |= MCG_C5_PRDIV0(2 - 1); // External clock div 4
    
    MCG->C6 &= ~MCG_C6_VDIV0_MASK;
    MCG->C6 |= MCG_C6_VDIV0(24 - 24) | // Mul 24. 8 / 4 * 24 = 48MHz
               MCG_C6_CME0_MASK |
               MCG_C6_PLLS_MASK;
		
		MCG->C1 &= ~MCG_C1_CLKS_MASK;
    MCG->C1 |= ~MCG_C1_CLKS_MASK | // Output of PLL is selected for MCGOUTCLK
								MCG_C1_FRDIV(3) |
								MCG_C1_IRCLKEN_MASK;
	
    while((MCG->S & MCG_S_OSCINIT0_MASK) == 0); // wait for osc init.
    while((MCG->S & MCG_S_PLLST_MASK) != MCG_S_PLLST_MASK); // wait for PLL
    while((MCG->S & MCG_S_CLKST_MASK) != MCG_S_CLKST(3)); // wait for PLL is selected
		
		SIM->CLKDIV1 = SIM_CLKDIV1_OUTDIV1(2 - 1) |  // core/system clock = MCGOUTCLK / 2 = 96 / 2 = 48MHz
									 SIM_CLKDIV1_OUTDIV4(2 - 1);   // flash/bus clock = core/system / 2 = 24MHz
}

/*
** ===================================================================
**     Method      :  Cpu_SetMCGModePBE (component MK22FN512VDC12)
**
**     Description :
**         This method sets the MCG to PBE mode.
**         This method is internal. It is used by Processor Expert only.
** ===================================================================
*/
static void Cpu_SetMCGModePBE(uint8_t CLKMode)
{
  switch (CLKMode) {
    case 0U:
      /* Switch to PBE 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=2,FRDIV=3,IREFS=0,IRCLKEN=0,IREFSTEN=0 */
      MCG_C1 = (MCG_C1_CLKS(0x02) | MCG_C1_FRDIV(0x03));
      /* MCG_C2: LOCRE0=0,??=0,RANGE0=2,HGO0=0,EREFS0=1,LP=0,IRCS=0 */
      MCG_C2 = (MCG_C2_RANGE(0x02) | MCG_C2_EREFS_MASK);
      /* MCG_C5: ??=0,PLLCLKEN0=0,PLLSTEN0=0,PRDIV0=b (devider is 12) */
      MCG_C5 = MCG_C5_PRDIV0(0x0b);
      /* MCG_C6: LOLIE0=0,PLLS=1,CME0=0,VDIV0=0x10 (multiply is 40)*/
      MCG_C6 = (MCG_C6_PLLS_MASK | MCG_C6_VDIV0(0x10));
      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 */
      }
      break;
    case 1U:
      /* Switch to PBE Mode */
      /* OSC_CR: ERCLKEN=1,??=0,EREFSTEN=0,??=0,SC2P=0,SC4P=0,SC8P=0,SC16P=0 */
      OSC_CR = OSC_CR_ERCLKEN_MASK;
      /* MCG_C7: OSCSEL=0 */
      MCG_C7 &= (uint8_t)~(uint8_t)(MCG_C7_OSCSEL_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_C2: LOCRE0=0,??=0,RANGE0=2,HGO0=0,EREFS0=1,LP=0,IRCS=1 */
      MCG_C2 = (MCG_C2_RANGE(0x02) | MCG_C2_EREFS_MASK | MCG_C2_IRCS_MASK);
      /* MCG_C5: ??=0,PLLCLKEN0=0,PLLSTEN0=0,PRDIV0=b(devider is 12) */
      MCG_C5 = MCG_C5_PRDIV0(0x0b);
      /* MCG_C6: LOLIE0=0,PLLS=1,CME0=0,VDIV0=0x10 (multiply is 40)*/
      MCG_C6 = (MCG_C6_PLLS_MASK | MCG_C6_VDIV0(0x10));
      /* FCTRIM = 13*/
      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 */
      }
      break;
    default:
      break;
  }
}

void InitClock()
{
// If the internal load capacitors are being used, they should be selected 
// before enabling the oscillator. Application specific. 16pF and 8pF selected
// in this example
  OSC_CR = OSC_CR_SC16P_MASK | OSC_CR_SC8P_MASK;
// Enabling the oscillator for 8 MHz crystal
// RANGE=1, should be set to match the frequency of the crystal being used
// HGO=1, high gain is selected, provides better noise immunity but does draw
// higher current
// EREFS=1, enable the external oscillator
// LP=0, low power mode not selected (not actually part of osc setup)
// IRCS=0, slow internal ref clock selected (not actually part of osc setup)
  MCG_C2 = MCG_C2_RANGE(1) | MCG_C2_HGO_MASK | MCG_C2_EREFS_MASK;
// Select ext oscillator, reference divider and clear IREFS to start ext osc
// CLKS=2, select the external clock source 
// FRDIV=3, set the FLL ref divider to keep the ref clock in range 
//         (even if FLL is not being used) 8 MHz / 256 = 31.25 kHz         
// IREFS=0, select the external clock 
// IRCLKEN=0, disable IRCLK (can enable it if desired)
// IREFSTEN=0, disable IRC in stop mode (can keep it enabled in stop if desired)
  MCG_C1 = MCG_C1_CLKS(2) | MCG_C1_FRDIV(3);
// wait for oscillator to initialize
  while (!(MCG_S & MCG_S_OSCINIT_MASK)){}  
// wait for Reference clock to switch to external reference 
  while (MCG_S & MCG_S_IREFST_MASK){} 
// Wait for MCGOUT to switch over to the external reference clock 
  while (((MCG_S & MCG_S_CLKST_MASK) >> MCG_S_CLKST_SHIFT) != 0x2){}
// Now configure the PLL and move to PBE mode
// set the PRDIV field to generate a 4MHz reference clock (8MHz /2)
  MCG_C5 = MCG_C5_PRDIV(1); // PRDIV=1 selects a divide by 2
// set the VDIV field to 0, which is x24, giving 4 x 24  = 96 MHz
// the PLLS bit is set to enable the PLL
// the clock monitor is enabled, CME=1 to cause a reset if crystal fails  
// LOLIE can be optionally set to enable the loss of lock interrupt
  
  MCG_C6 = MCG_C6_CME_MASK | MCG_C6_PLLS_MASK;
// wait until the source of the PLLS clock has switched to the PLL  
  while (!(MCG_S & MCG_S_PLLST_MASK)){}
// wait until the PLL has achieved lock
  while (!(MCG_S & MCG_S_LOCK_MASK)){}
// set up the SIM clock dividers BEFORE switching to the PLL to ensure the
// system clock speeds are in spec.
// core = PLL (96MHz), bus = PLL/2 (48MHz), flexbus = PLL/2 (48MHz), flash = PLL/4 (24MHz)  
  SIM_CLKDIV1 = SIM_CLKDIV1_OUTDIV1(0) | SIM_CLKDIV1_OUTDIV2(1) 
              | SIM_CLKDIV1_OUTDIV3(1) | SIM_CLKDIV1_OUTDIV4(3);
  
// Transition into PEE by setting CLKS to 0
// previous MCG_C1 settings remain the same, just need to set CLKS to 0
  MCG_C1 &= ~MCG_C1_CLKS_MASK;
// Wait for MCGOUT to switch over to the PLL
  while (((MCG_S & MCG_S_CLKST_MASK) >> MCG_S_CLKST_SHIFT) != 0x3){}
// The USB clock divider in the System Clock Divider Register 2 (SIM_CLKDIV2) 
// should be configured to generate the 48 MHz USB clock before configuring 
// the USB module.
    SIM_CLKDIV2 |= SIM_CLKDIV2_USBDIV(1); // sets USB divider to /2 assuming reset
										// state of the SIM_CLKDIV2 register
}

/*****************************************************************************
 * @name     pll_init
 *
 * @brief:   Initialization of the MCU.
 *
 * @param  : None
 *
 * @return : None
 *****************************************************************************
 * It will configure the MCU to disable STOP and COP Modules.
 * It also set the MCG configuration and bus clock frequency.
 ****************************************************************************/
static unsigned char pll_init()
{
	/* First move to FBE mode */
	/* Enable external oscillator, RANGE=1, HGO=1, EREFS=1, LP=0, IRCS=0 */
	MCG_C2 = MCG_C2_RANGE0(2) | MCG_C2_HGO0_MASK | MCG_C2_EREFS0_MASK | MCG_C2_IRCS_MASK;
	
    /* Select external oscillator 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);

	/* Wait for oscillator to initialize */
   while (!(MCG_S & MCG_S_OSCINIT0_MASK)){};

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

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

    MCG_C5 = MCG_C5_PRDIV0(BSP_REF_CLOCK_DIV - 1) | MCG_C5_PLLCLKEN0_MASK;

    /* Ensure MCG_C6 is at the reset default of 0. LOLIE disabled,
     PLL enabled, clk monitor disabled, PLL VCO divider is clear */
    MCG_C6 = 0;

    /* Set system options dividers */
	#if (defined MCU_MK20D5) || (defined MCU_MK40D7)
		SIM_CLKDIV1 =   SIM_CLKDIV1_OUTDIV1(BSP_CORE_DIV - 1) | 	/* core/system clock */
						SIM_CLKDIV1_OUTDIV2(BSP_BUS_DIV - 1)  | 	/* peripheral clock; */
						SIM_CLKDIV1_OUTDIV4(BSP_FLASH_DIV - 1);     /* flash clock */
	#else  
		SIM_CLKDIV1 =   SIM_CLKDIV1_OUTDIV1(BSP_CORE_DIV - 1) 	| 	/* Core/system clock */
						SIM_CLKDIV1_OUTDIV2(BSP_BUS_DIV - 1)  	| 	/* Peripheral clock; */
						SIM_CLKDIV1_OUTDIV3(BSP_FLEXBUS_DIV - 1)|  	/* FlexBus clock driven to the external pin (FB_CLK)*/
						SIM_CLKDIV1_OUTDIV4(BSP_FLASH_DIV - 1);     /* Flash clock */
	#endif
		
    /* Set the VCO divider and enable the PLL, LOLIE = 0, PLLS = 1, CME = 0, VDIV = */
    MCG_C6 = MCG_C6_PLLS_MASK | MCG_C6_VDIV0(BSP_CLOCK_MUL - 24); /* 2MHz * BSP_CLOCK_MUL */

    while (!(MCG_S & MCG_S_PLLST_MASK)){}; 	/* Wait for PLL status bit to set */
    while (!(MCG_S & MCG_S_LOCK0_MASK)){}; 	/* Wait for LOCK bit to set */

    /* 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){};
     
    /* Enable the ER clock of oscillators */
    OSC_CR = OSC_CR_ERCLKEN_MASK | OSC_CR_EREFSTEN_MASK;
    
    /* Now running in PEE Mode */
    SIM_SOPT1 |= SIM_SOPT1_USBREGEN_MASK;
    
    return 0;
} //pll_init

/*
** ===================================================================
**     Method      :  Cpu_SetMCGClockInModePEE (component MK22FN512VDC12)
**
**     Description :
**         Calling of this method will cause the clock frequency
**         change in mode PEE, typically from 120M to 80M, vice versa.
**     Parameters  :
**         NAME            - DESCRIPTION
**         ModeID          - Clock configuration identifier
**     Returns     :
**         ---             - ERR_OK - OK.
**                           ERR_RANGE - Mode parameter out of range
** ===================================================================
*/
static LDD_TError Cpu_SetMCGClockInModePEE(LDD_TClockConfiguration ModeID)
{
  if (ModeID > 0x03U)
      return ERR_RANGE;

  switch (ModeID) {
    case CPU_CLOCK_CONFIG_3:
      /* 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 |= SIM_SOPT2_PLLFLLSEL(0x01);
      /* MCG_C1: CLKS=0,FRDIV=3,IREFS=0,IRCLKEN=1,IREFSTEN=0 */
      MCG_C1 = (MCG_C1_FRDIV(0x03) | MCG_C1_IRCLKEN_MASK);
      /* MCG_C2: ??=0,??=0,RANGE=2,HGO=0,EREFS=1,LP=0,IRCS=0 */
      MCG_C2 = (MCG_C2_RANGE(0x02) | MCG_C2_EREFS_MASK);
      /* MCG_C5: ??=0,PLLCLKEN=0,PLLSTEN=0,PRDIV=5 (devider is 6) */
      MCG_C5 = MCG_C5_PRDIV0(0x05);
      /* MCG_C6: LOLIE=0,PLLS=1,CME=0,VDIV=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;
    case CPU_CLOCK_CONFIG_0:
      /* OSC_CR: ERCLKEN=1,??=0,EREFSTEN=0,??=0,SC2P=0,SC4P=0,SC8P=0,SC16P=0 */
      OSC_CR = (uint8_t)0x80U;
      /* SIM_SOPT2: MCGCLKSEL=0 */
      SIM_SOPT2 |= SIM_SOPT2_PLLFLLSEL(0x01);
      /* MCG_C1: CLKS=0,FRDIV=3,IREFS=0,IRCLKEN=1,IREFSTEN=0 */
      MCG_C1 = (MCG_C1_FRDIV(0x03) | MCG_C1_IRCLKEN_MASK);
      /* MCG_C2: ??=0,??=0,RANGE=2,HGO=0,EREFS=1,LP=0,IRCS=0 */
      MCG_C2 = (MCG_C2_RANGE(0x02) | MCG_C2_EREFS_MASK);
      /* MCG_C5: ??=0,PLLCLKEN=0,PLLSTEN=0,PRDIV=0xb (devider is 12) */
      MCG_C5 = MCG_C5_PRDIV0(0x0b);
      /* MCG_C6: LOLIE=0,PLLS=1,CME=0,VDIV=16 (multiply is 40)*/
      MCG_C6 = (MCG_C6_PLLS_MASK | MCG_C6_VDIV0(0x10));
      while((MCG_S & 0x0CU) != 0x0CU) { /* Wait until output of the PLL is selected */
      }
      break;
    default:
      break;
  }
  return ERR_OK;
}

/**
 * Update SystemCoreClock variable
 *
 * @brief  Updates the SystemCoreClock with current core Clock
 *         retrieved from CPU registers.
 */
void SystemCoreClockUpdate(void) {
   uint32_t oscerclk = OSCCLK_CLOCK;
   switch (MCG_S&MCG_S_CLKST_MASK) {
      case MCG_S_CLKST(0) : // FLL
         if ((MCG_C1&MCG_C1_IREFS_MASK) == 0) {
            SystemCoreClock = oscerclk/(1<<((MCG_C1&MCG_C1_FRDIV_MASK)>>MCG_C1_FRDIV_SHIFT));
            if ((MCG_C2&MCG_C2_RANGE0_MASK) != 0) {
               if ((MCG_C1&MCG_C1_FRDIV_M) == MCG_C1_FRDIV(6)) {
                  SystemCoreClock /= 20;
               }
               else if ((MCG_C1&MCG_C1_FRDIV_M) == MCG_C1_FRDIV(7)) {
                  SystemCoreClock /= 12;
               }
               else {
                  SystemCoreClock /= 32;
               }
            }
         }
         else {

/*!
 * @brief Update SystemCoreClock variable
 *
 * Updates the SystemCoreClock variable with current core Clock retrieved from CPU registers.
 */
void SystemCoreClockUpdate(void) {
   uint32_t oscerclk = (MCG->C7&MCG_C7_OSCSEL_MASK)?RTCCLK_CLOCK:OSCCLK0_CLOCK;
   switch (MCG->S&MCG_S_CLKST_MASK) {
      case MCG_S_CLKST(0) : // FLL
         SystemCoreClock = (MCG->C4&MCG_C4_DMX32_MASK)?732:640;
         if ((MCG->C1&MCG_C1_IREFS_MASK) == 0) {
            SystemCoreClock *= oscerclk/(1<<((MCG->C1&MCG_C1_FRDIV_MASK)>>MCG_C1_FRDIV_SHIFT));
            if (((MCG->C2&MCG_C2_RANGE0_MASK) != 0) && ((MCG->C7&MCG_C7_OSCSEL_MASK) !=  1)) {
               if ((MCG->C1&MCG_C1_FRDIV_MASK) == MCG_C1_FRDIV(6)) {
                  SystemCoreClock /= 20;
               }
               else if ((MCG->C1&MCG_C1_FRDIV_MASK) == MCG_C1_FRDIV(7)) {
                  SystemCoreClock /= 12;
               }
               else {
                  SystemCoreClock /= 32;
               }
            }
         }
         else {

void BoardConfig_vfnInit(void)
{
	/* 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 */

	/* Switch to FBE Mode */
	/* 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);
	/* 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 */
	}
	
	SIM_SCGC5 |= SIM_SCGC5_PORTB_MASK | SIM_SCGC5_PORTC_MASK | SIM_SCGC5_PORTD_MASK;
	
	GPIO_vfnPinMux(GPIO_PORT_C,10,GPIO_MUX_ALT_1);
	
	GPIO_vfnPinMux(GPIO_PORT_C,11,GPIO_MUX_ALT_1);
	
	GPIO_vfnPinMux(GPIO_PORT_C,12,GPIO_MUX_ALT_1);
	
	GPIO_vfnPinMux(GPIO_PORT_C,13,GPIO_MUX_ALT_1);
	
	GPIO_vfnPinMux(GPIO_PORT_D,7,GPIO_MUX_ALT_1);
	
	GPIO_vfnPinMux(GPIO_PORT_D,6,GPIO_MUX_ALT_1);
	
	GPIO_vfnPinMux(GPIO_PORT_B,19,GPIO_MUX_ALT_1);
	
	GPIOC_PDDR |= (1<<10) | (1<<11) | (1<<12) | (1<<13);
	
	GPIOD_PDDR |= (1<<7) | (1<<6);
	
	GPIOB_PDDR |= (1<<19);
}

void Clock_init(void)
{
// Init system clock
  /* System clock initialization */
  /* SIM_SCGC5: PORTA=1 */
  SIM_SCGC5 |= SIM_SCGC5_PORTA_MASK;   /* Enable clock gate for ports to enable pin routing */
  /* 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=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(0x03)); /* System 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)));
  /* MCG_SC: FCRDIV=1 */
  MCG_SC = (uint8_t)((MCG_SC & (uint8_t)~(uint8_t)(
			MCG_SC_FCRDIV(0x06)
		   )) | (uint8_t)(
			MCG_SC_FCRDIV(0x01)
		   ));
  /* Switch to FEE Mode */
  /* MCG_C2: LOCRE0=0,??=0,RANGE0=2,HGO0=0,EREFS0=1,LP=0,IRCS=1 */
  MCG_C2 = (MCG_C2_RANGE0(0x02) | MCG_C2_EREFS0_MASK | MCG_C2_IRCS_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=0,FRDIV=3,IREFS=0,IRCLKEN=1,IREFSTEN=0 */
  MCG_C1 = (MCG_C1_CLKS(0x00) | MCG_C1_FRDIV(0x03) | MCG_C1_IRCLKEN_MASK);
  /* MCG_C4: DMX32=0,DRST_DRS=1 */
  MCG_C4 = (uint8_t)((MCG_C4 & (uint8_t)~(uint8_t)(
			MCG_C4_DMX32_MASK |
			MCG_C4_DRST_DRS(0x02)
		   )) | (uint8_t)(
			MCG_C4_DRST_DRS(0x01)
		   ));
  /* MCG_C5: ??=0,PLLCLKEN0=0,PLLSTEN0=0,PRDIV0=0 */
  MCG_C5 = MCG_C5_PRDIV0(0x00);
  /* 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) != 0x00U) {    /* Wait until output of the FLL is selected */
  }
  /*** End of PE initialization code after reset ***/

}

/**
 * @brief Initialize the FLL Engaged External Mode.
 *
 * MCGOUTCLK is derived from the FLL clock.
 * Clock source is the external reference clock (IRC or oscillator).
 * The FLL loop will lock the DCO frequency to the FLL-Factor.
 */
static void kinetis_mcg_set_fee(void)
{
    kinetis_mcg_enable_osc();
    kinetis_mcg_set_fll_factor(KINETIS_MCG_FLL_FACTOR_FEE);

    /* select external reference clock and divide factor */
    MCG->C1 = (uint8_t)(MCG_C1_CLKS(0) | MCG_C1_FRDIV(KINETIS_MCG_ERC_FRDIV));

    /* Wait until output of FLL is selected */
    while ((MCG->S & MCG_S_CLKST_MASK) != MCG_S_CLKST(0));

    kinetis_mcg_disable_pll();
    current_mode = KINETIS_MCG_FEE;
}

/**
 * @brief Initialize the FLL Bypassed External Mode.
 *
 * MCGOUTCLK is derived from external reference clock (oscillator).
 * FLL output is not used.
 * Clock source is the external reference clock (oscillator).
 * The FLL loop will lock the DCO frequency to the FLL-Factor.
 */
static void kinetis_mcg_set_fbe(void)
{
    kinetis_mcg_enable_osc();
    kinetis_mcg_set_fll_factor(KINETIS_MCG_FLL_FACTOR_FEE);

    /* FLL is not disabled in bypass mode */
    MCG->C2 &= ~(uint8_t)(MCG_C2_LP_MASK);

    /* select external reference clock and divide factor */
    MCG->C1 = (uint8_t)(MCG_C1_CLKS(2) | MCG_C1_FRDIV(KINETIS_MCG_ERC_FRDIV));

    /* Wait until ERC is selected */
    while ((MCG->S & MCG_S_CLKST_MASK) != MCG_S_CLKST(2));

    kinetis_mcg_disable_pll();
    current_mode = KINETIS_MCG_FBE;
}

/** PLL initialization.
 */
static void pll_init(void) {
    // First move to FBE mode
    // Enable external oscillator, RANGE=0, HGO=, EREFS=, LP=, IRCS=
    MCG_C2 = 0;
    
    // 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);
    
    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
    
    // ... FBE mode
    // Configure PLL Ref Divider, PLLCLKEN=0, PLLSTEN=0, PRDIV=0x18
    // The crystal frequency is used to select the PRDIV value. Only even frequency crystals are supported
    // that will produce a 2MHz reference clock to the PLL.
    MCG_C5 = MCG_C5_PRDIV(REF_CLOCK_DIV - 1);

    // Ensure MCG_C6 is at the reset default of 0. LOLIE disabled, PLL disabled, clk monitor disabled, PLL VCO divider is clear
    MCG_C6 = 0;

    // Set system options dividers
    SIM_CLKDIV1 = SIM_CLKDIV1_OUTDIV1(CORE_DIV - 1) |           // core/system clock
                  SIM_CLKDIV1_OUTDIV2(BUS_DIV - 1) |