/*
 * Copyright (c) 2013 - 2014, Freescale Semiconductor, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *
 * o Redistributions of source code must retain the above copyright notice, this list
 *   of conditions and the following disclaimer.
 *
 * o Redistributions in binary form must reproduce the above copyright notice, this
 *   list of conditions and the following disclaimer in the documentation and/or
 *   other materials provided with the distribution.
 *
 * o Neither the name of Freescale Semiconductor, Inc. nor the names of its
 *   contributors may be used to endorse or promote products derived from this
 *   software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
 * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include <stdarg.h>
#include "device/fsl_device_registers.h"
#include "uart/fsl_uart_driver.h"
#include "fsl_uart_hal.h"
#include "smc/fsl_smc_manager.h"
#include "fsl_smc_features.h"
#include "fsl_smc_hal.h"
#include "gpio/fsl_gpio_driver.h"
#include "interrupt/fsl_interrupt_manager.h"
#include "clock/fsl_clock_manager.h"
#include "fsl_port_hal.h"
#include "fsl_sim_hal.h"
#include "fsl_mcg_hal.h"
#include "fsl_osc_hal.h"
#include "board.h"
#include "low_power_demo.h"
#include "fsl_pmc_hal.h"

/*******************************************************************************
 * Prototypes
 ******************************************************************************/
void llwu_isr(void);
extern void cpu_identify(void);
void de_init_pins(void);
/*******************************************************************************
 * Variables
 ******************************************************************************/
uint8_t s_uartInstance; /*!< uart instance for the target */
uint8_t uartEnabled = 1;


static uart_state_t uartState; /*!< uart state information */
static uart_user_config_t uartConfig;

int32_t fastIrcFreq = 4000000;  /*!<default fast irc frequency */
int32_t slowIrcFreq = 32768; /*!<default slow irc frequency */
int32_t mcgClkHz; /*!< MCG out clock frequecy in Hz */
int32_t mcgClkKhz; /*!<MCG out clock frequency in KHz */
int32_t coreClkKhz; /*!<Core clock frequency in KHz */
int32_t periphClkKhz; /*!<Peripheral clock frequency in KHz */
int32_t pllClkKhz; /*!<PLL clock frequency in KHz */
int32_t uart0ClkKhz; /*!<UART0 clock frequency in KHz */
uint32_t uart0ClkHz; /*!<UART0 clock frequency in Hz */
uint32_t uartClkHz; /*!<Other uart clock frequcy in Hz */


/*******************************************************************************
 * Code
 ******************************************************************************/

/*!
 * @brief main function.
 */
void main(void)
{
    smc_power_mode_protection_config_t pmodes = {
        .vlpProt  = true,
        .llsProt  = true,
        .vllsProt = true
    };

    /* initialize the hardware */
    hardware_init();

    dbg_uart_init();

	/* Initialize the UART module */
    uartConfig.baudRate = 19200;
    uartConfig.bitCountPerChar = kUart8BitsPerChar;
    uartConfig.parityMode = kUartParityDisabled;
    uartConfig.stopBitCount = kUartOneStopBit;
    uart_init(BOARD_DEBUG_UART_INSTANCE, &uartState, &uartConfig);


#if (defined(TWR_K22F120M)||defined(TWR_K64F120M)||defined(FRDM_K22F120M))
    s_uartInstance = 1;
#elif (defined(FRDM_KL25Z48M)||defined(FRDM_K64F120M))
    s_uartInstance = 0;
#endif

    /* check and print the reson of reset */
    out_srs();

    /* identify the cpu */
    cpu_identify();

    /* Configure the allowable power modes ONCE before continuing the application */
    smc_hal_config_power_mode_protection(&pmodes);

    /* initialize the low-leakage wakeup unit */
    llwu_init();

    /* configure tower hardware port pins for the application */
    port_init();

    /* configure the LED pins in the hardware */
    led_init();

    /* start test */
    low_power_modes_test();
}

/*!
 * @brief configure tower/freedom hardware port pins for the application
 */
void port_init(void){
    uint8_t debug = 'n';
    volatile uint32_t buttonRead = 1;

    /* De-initialize all unused pins */
    de_init_pins();

    /* Configure both pushbuttons for falling edge interrupts */
    switchPins[0].config.interrupt = kPortIntFallingEdge;
    switchPins[1].config.interrupt = kPortIntFallingEdge;

    /* Initialize the pins used for switches and LEDs only */
    gpio_init(switchPins, ledPins);

    uint32_t i;

#if (defined(TWR_K22F120M)||defined(FRDM_K22F120M))
    /* For TWR_K22F120M: configure SW1/PTC6 for wakeup interrupt source from LLS mode etc */
    /* For FRDM_K22F120M: configure SW2/PTC1 for wakeup interrupt source from LLS mode etc */
    /* Enable the IRQs */
    interrupt_enable(PORTC_IRQn);
    interrupt_register_handler(PORTC_IRQn, &port_isr);

#elif (defined(FRDM_K64F120M) || defined(TWR_K64F120M))
    /* Configure the interrupt vector for the interrupt handler */
    /* configure SW2(FRDM K64)/SW1(TWR K64) of PTC6 for wakeup interrupt source from LLS/VLLSx mode etc */
    /* Enable the IRQs */
    interrupt_register_handler(PORTA_IRQn, &port_isr);
    interrupt_enable(PORTA_IRQn);
#endif



    /* Give a little delay here */
    for(i=0; i<1000; i++)
    {}

     /* read SW1 level */
    buttonRead = gpio_read_pin_input(kGpioSW1);

    if (buttonRead)
    {
        debug = 'n';
    }
    else
    {
      debug = 'y';
    }

    if((debug == 'n') || (debug == 'N'))
    {
      /* disable debug */
      SWD_CLK_DISABLE;
      SWD_DIO_DISABLE;
      printf("\n\r*--------------D E B U G    D I S A B L E D------------------*");
#if (defined(TWR_K22F120M)||defined(TWR_K64F120M))
      printf("\n\r*-------Press SW1 then press Reset to re-enable debug---------*");
#elif (defined(FRDM_K64F120M)||defined(FRDM_K22F120M))
      printf("\n\r*-------Press SW2 then press Reset to re-enable debug---------*");
#endif
    } else
    {
      printf("\n\r*--------------D E B U G    E N A B L E D--------------------*");
    }
}

/*!
 * @brief configure LED pins for the tower/freedom hardware
 */
void led_init(void)
{
   LED0_ON;
   LED1_ON;
   LED2_ON;
#if defined(TWR_K22F120M)
   LED3_ON;
#endif
}

/*!
 * @brief returns the Frequency Locked Loop frequency
 *
 * @param fllRef - reference FLL frequency
 */
int32_t fll_freq(int32_t fllRef)
{
  int32_t fllFreqHz = 0;

  /* Check that only allowed ranges have been selected */
  if (clock_get_drst_drs() > 0x1)
  {
    return 0x3B; /* return error code if DRS range 2 or 3 selected */
  }

  /* if DMX32 set */
  if (clock_get_dmx32())
  {
    /* determine multiplier based on DRS */
    switch (clock_get_drst_drs())
    {
    case 0:
      fllFreqHz = (fllRef * 732);
      if (fllFreqHz < 20000000) {return 0x33;}
      else if (fllFreqHz > 25000000) {return 0x34;}
      break;
    case 1:
      fllFreqHz = (fllRef * 1464);
      if (fllFreqHz < 40000000) {return 0x35;}
      else if (fllFreqHz > 50000000) {return 0x36;}
      break;
    case 2:
      fllFreqHz = (fllRef * 2197);
      if (fllFreqHz < 60000000) {return 0x37;}
      else if (fllFreqHz > 75000000) {return 0x38;}
      break;
    case 3:
      fllFreqHz = (fllRef * 2929);
      if (fllFreqHz < 80000000) {return 0x39;}
      else if (fllFreqHz > 100000000) {return 0x3A;}
      break;
    default:
      break;
    }
  }
  /* if DMX32 = 0 */
  else
  {
    /* determine multiplier based on DRS */
	switch (clock_get_drst_drs())
    {
    case 0:
      fllFreqHz = (fllRef * 640);
      if (fllFreqHz < 20000000) {return 0x33;}
      else if (fllFreqHz > 25000000) {return 0x34;}
      break;
    case 1:
      fllFreqHz = (fllRef * 1280);
      if (fllFreqHz < 40000000) {return 0x35;}
      else if (fllFreqHz > 50000000) {return 0x36;}
      break;
    case 2:
      fllFreqHz = (fllRef * 1920);
      if (fllFreqHz < 60000000) {return 0x37;}
      else if (fllFreqHz > 75000000) {return 0x38;}
      break;
    case 3:
      fllFreqHz = (fllRef * 2560);
      if (fllFreqHz < 80000000) {return 0x39;}
      else if (fllFreqHz > 100000000) {return 0x3A;}
      break;
    default:
      break;
    }
  }
  return fllFreqHz;
} /* fll_freq */

/*!
 * @brief returns the the current MCG mode
 */
uint8_t what_mcg_mode(void)
{
    /* check if in FEI mode */
	if (((clock_get_clkst() == kMcgClkstFll) && /* check CLKS mux has selcted FLL output */
    //if (((clock_get_clks() == kMcgClockSelectOut) && /* check CLKS mux has selcted FLL output */
          (clock_get_irefst() == kMcgIrefstInt) &&  /* check FLL ref is internal ref clk */
          (clock_get_pllst() != kMcgPllstPllcs)))  /* check PLLS mux has selected FLL */
    {
        /* return FEI code */
        return FEI;
    }
    /* Check MCG is in PEE mode */
	else if (((clock_get_clkst() == kMcgClkstPll) && /* check CLKS mux has selcted PLL output */
    //else if (((clock_get_clks() == kMcgClockSelectOut) && /* check CLKS mux has selcted PLL output */
          (clock_get_irefst() != kMcgIrefstInt) &&	 /* check FLL ref is external ref clk */
          (clock_get_pllst() == kMcgPllstPllcs)))	 /* check PLLS mux has selected PLL */
    {
        /* return PEE code */
        return PEE;
    }
    /* Check MCG is in PBE mode */
	else if (((clock_get_clkst() == kMcgClkstEref) && /* check CLKS mux has selcted external reference */
          (clock_get_irefst() != kMcgIrefstInt) &&	  /* check FLL ref is external ref clk */
          (clock_get_pllst() == kMcgPllstPllcs) &&	  /* check PLLS mux has selected PLL */
          (clock_get_lp() != kMcgLpSelectLowPower)))	  /* check MCG_C2[LP] bit is not set */
    {
        /* return PBE code */
        return PBE;
    }
    /* Check MCG is in FBE mode */
    else if (((clock_get_clkst() == kMcgClkstEref) &&   /* check CLKS mux has selcted external reference */
          (clock_get_irefst() != kMcgIrefstInt) &&	/* check FLL ref is external ref clk */
          (clock_get_pllst() != kMcgPllstPllcs) &&	/* check PLLS mux has selected FLL */
          (clock_get_lp() != kMcgLpSelectLowPower)))	/* check MCG_C2[LP] bit is not set */
    {
        /* return FBE code */
        return FBE;
    }
    /* Check MCG is in BLPE mode */
    else if (((clock_get_clkst() == kMcgClkstEref) && 	/* check CLKS mux has selcted external reference */
          (clock_get_irefst() != kMcgIrefstInt) &&	/* check FLL ref is external ref clk */
          (clock_get_lp() == kMcgLpSelectLowPower)))	/* check MCG_C2[LP] bit is set */
    {
        /* return BLPE code */
        return BLPE;
    }
    /* check if in BLPI mode */
    else if (((clock_get_clkst() == kMcgClkstIref) && 	/* check CLKS mux has selcted int ref clk */
          (clock_get_irefst() == kMcgIrefstInt) &&	/* check FLL ref is internal ref clk */
          (clock_get_pllst() != kMcgPllstPllcs) &&	/* check PLLS mux has selected FLL */
          (clock_get_lp() == kMcgLpSelectLowPower)))	/* check LP bit is set */
    {
        return BLPI;                                    /* return BLPI code */
    }
    /* check if in FBI mode */
    else if (((clock_get_clkst() == kMcgClkstIref) && 	/* check CLKS mux has selcted int ref clk */
          (clock_get_irefst() == kMcgIrefstInt) &&	/* check FLL ref is internal ref clk */
          (clock_get_pllst() != kMcgPllstPllcs) &&	/* check PLLS mux has selected FLL */
          (clock_get_lp() != kMcgLpSelectLowPower)))	/* check LP bit is clear */
    {
        /* return FBI code */
        return FBI;
    }
    /* Check MCG is in FEE mode */
    else if (((clock_get_clkst() == kMcgClkstFll) &&    /* check CLKS mux has selcted FLL */
          (clock_get_irefst() != kMcgIrefstInt) &&	/* check FLL ref is external ref clk */
          (clock_get_pllst() != kMcgPllstPllcs)))	/* check PLLS mux has selected FLL */
    {
        /* return FEE code */
        return FEE;
    }
    else
    {
        return 0;                                                            /* error condition */
    }
} /* what_mcg_mode */

/*!
 * @brief switch from PEE mode to BLPE mode
 */
int32_t pee_to_blpe(void)
{
    int32_t mcgClockHz;

    mcgClockHz = atc(FAST_IRC, fastIrcFreq, MCG_OUT_FREQ);

    mcgClockHz = pee_pbe(CLK0_FREQ_HZ);
    mcgClockHz = pbe_blpe(CLK0_FREQ_HZ);

    return mcgClockHz;
}

/*!
 * @brief switch from BLPE mode to PEE mode
 */
int32_t blpe_to_pee(void)
{
    int32_t mcgClockHz;

    clock_hal_set_clock_out_dividers(0,0,0,1);

    /* After wake up back to the original clock frequency */
    mcgClockHz = blpe_pbe(CLK0_FREQ_HZ, PLL0_PRDIV,PLL0_VDIV);
    mcgClockHz = pbe_pee(CLK0_FREQ_HZ);

    return mcgClockHz;
}


/*!
 * @brief Settings done before swithing in low power mode
 * @param outOfLp - indicate whether to do the setting for going in/coming out of low power mode
 *
 *              = 0 for going into low power mode
 *              = 1 for exiting low power mode
 */
void set_for_lp(uint8_t outOfLp)
{
    if(outOfLp == 1)
    {
        clock_manager_set_gate(kClockModuleUART, s_uartInstance, true);
        /* Enable the pins for the selected SCI */
        configure_uart_pin_mux(s_uartInstance);

        /* Set variable to let other functions know that the UART is now enabled */
        uartEnabled = 1;

        /* Enable Debug */
        SWD_CLK_ENABLE;
        SWD_DIO_ENABLE;

#if defined(TWR_K22F120M)
        /* configure SW3/PTC7 */
        port_hal_pull_select(HW_PORTC, 7, kPortPullUp);
        port_hal_configure_pull(HW_PORTC, 7, true);
        port_hal_configure_passive_filter(HW_PORTC, 7, true);
        port_hal_configure_pin_interrupt(HW_PORTC, 7, kPortIntFallingEdge); /* IRQ Falling edge */
        port_hal_mux_control(HW_PORTC, 7, kPortMuxAsGpio);

        /* Enable the IRQs */
        interrupt_enable(PORTC_IRQn);

        /* configure SW1/PTC6 */
        port_hal_pull_select(HW_PORTC, 6, kPortPullUp);
        port_hal_configure_pull(HW_PORTC, 6, true);
        port_hal_configure_passive_filter(HW_PORTC, 6, true);
        port_hal_mux_control(HW_PORTC, 6, kPortMuxAsGpio);

#elif (defined(FRDM_K64F120M)|| defined(TWR_K64F120M))

	/* configure SW3/PTA4 for debug enable sense during after reset */
	port_hal_pull_select(HW_PORTA, 4, kPortPullUp);
	port_hal_configure_pull(HW_PORTA, 4, true);
	port_hal_configure_passive_filter(HW_PORTA, 4, true);
	port_hal_configure_pin_interrupt(HW_PORTA, 4, kPortIntFallingEdge); /* IRQ Falling edge */
	port_hal_mux_control(HW_PORTA, 4, kPortMuxAsGpio);

	/* Enable the IRQs */
	interrupt_enable(PORTC_IRQn);
	interrupt_enable(PORTA_IRQn);

	/* configure SW2(FRDM K64)/SW1(TWR K64) of PTC6 for wakeup interrupt source from LLS/VLLSx mode etc */
	port_hal_pull_select(HW_PORTC, 6, kPortPullUp);
	port_hal_configure_pull(HW_PORTC, 6, true);
	port_hal_configure_passive_filter(HW_PORTC, 6, true);
	port_hal_mux_control(HW_PORTC, 6, kPortMuxAsGpio);

#elif defined(FRDM_K22F120M)    
        /* configure SW3/PTB17 */
        port_hal_pull_select(HW_PORTB, 17, kPortPullUp);
        port_hal_configure_pull(HW_PORTB, 17, true);
        port_hal_configure_passive_filter(HW_PORTB, 17, true);
        port_hal_configure_pin_interrupt(HW_PORTB, 17, kPortIntFallingEdge); /* IRQ Falling edge */
        port_hal_mux_control(HW_PORTB, 17, kPortMuxAsGpio);

        /* Enable the IRQs */
        interrupt_enable(PORTC_IRQn);
        interrupt_enable(PORTB_IRQn);

        /* configure SW2/PTC1 */
        port_hal_pull_select(HW_PORTC, 1, kPortPullUp);
        port_hal_configure_pull(HW_PORTC, 1, true);
        port_hal_configure_passive_filter(HW_PORTC, 1, true);
        port_hal_mux_control(HW_PORTC, 1, kPortMuxAsGpio);
#endif
    }
    else  if(outOfLp == 2)/* outOfLp = 0 for going into low power mode -  turn things off */
    {
        /* clock gate off the UART */
        clock_manager_set_gate(kClockModuleUART, s_uartInstance, false);

        /* Disable UART pins */
#if (defined(FRDM_KL25Z48M)||defined(TWR_K64F120M))
        port_hal_mux_control(HW_PORTC, 4, kPortPinDisabled);
        port_hal_mux_control(HW_PORTC, 3, kPortPinDisabled);
#elif (defined(TWR_K22F120M)||defined(FRDM_K22F120M))
        port_hal_mux_control(HW_PORTE, 0, kPortPinDisabled);
        port_hal_mux_control(HW_PORTE, 1, kPortPinDisabled);
#endif
        /* Set variable so other functions know the UART is disabled */
        uartEnabled = 0;

        /* Disable Debug */
        SWD_CLK_DISABLE;
        SWD_DIO_DISABLE;
   }
	else  /* outOfLp = 0 for going into low power mode -  turn things off */
    {
#if defined(TWR_K22F120M)
        /* SW3 is disabled */
        port_hal_configure_pin_interrupt(HW_PORTC, 7, kPortIntDisabled);
        port_hal_mux_control(HW_PORTC, 7, kPortPinDisabled);
#elif (defined(FRDM_K64F120M)||defined(TWR_K64F120M))
        /* Disable SW3 interrupt before disabling the pin */
        port_hal_configure_pin_interrupt(HW_PORTA, 4, kPortIntDisabled);
        /* Disable SW3 */
        port_hal_mux_control(HW_PORTA, 4, kPortPinDisabled);
#elif defined(FRDM_K22F120M)        
        /* SW3 is disabled */
        port_hal_configure_pin_interrupt(HW_PORTC, 1, kPortIntDisabled);
        port_hal_mux_control(HW_PORTC, 1, kPortPinDisabled);
#endif

        /* clock gate off the UART */
        clock_manager_set_gate(kClockModuleUART, s_uartInstance, false);

        /* Disable UART pins */
#if (defined(FRDM_KL25Z48M)||defined(TWR_K64F120M))
        port_hal_mux_control(HW_PORTC, 4, kPortPinDisabled);
        port_hal_mux_control(HW_PORTC, 3, kPortPinDisabled);
#elif (defined(TWR_K22F120M)||defined(FRDM_K22F120M))
        port_hal_mux_control(HW_PORTE, 0, kPortPinDisabled);
        port_hal_mux_control(HW_PORTE, 1, kPortPinDisabled);
#endif
        /* Set variable so other functions know the UART is disabled */
        uartEnabled = 0;

        /* Disable Debug */
        SWD_CLK_DISABLE;
        SWD_DIO_DISABLE;
   }
}

/*!
 * @brief VLLS0 mode entry routine. Puts the processor into VLLS0 mode from
 * normal run mode or VLPR.
 *
 * Mode transitions:
 *
 *      RUN -> VLLS0, VLPR -> VLLS0
 *
 * NOTE: VLLSx modes will always exit to RUN mode even if you were
 * in VLPR mode before entering VLLSx.
 *
 * Wakeup from VLLSx mode is controlled by the LLWU module. Most
 * modules cannot issue a wakeup interrupt in VLLSx mode, so make
 * sure to setup the desired wakeup sources in the LLWU before
 * calling this function.
 *
 * @param PORPOValue - POR detect circuit is enabled/disabled
 *              - 0 POR detect circuit is enabled in VLLS0
 *              - 1 POR detect circuit is disabled in VLLS0
 */
void enter_vlls0(uint8_t PORPOValue)
{
    smc_power_mode_config_t smcConfig;

    /* set power mode to specific Run mode */
    smcConfig.lpwuiOption = false;
    smcConfig.porOption = true;
    smcConfig.porOptionValue = (smc_por_option_t)PORPOValue;
    smcConfig.powerModeName = kPowerModeVlls;
    smcConfig.stopSubMode = kSmcStopSub0;
    smc_set_power_mode(&smcConfig);
}

/*!
 * @brief VLLS1 mode entry routine. Puts the processor into
 * VLLS1 mode from normal run mode or VLPR.
 *
 * Mode transitions:
 * RUN -> VLLS1
 * VLPR -> VLLS1
 *
 * NOTE: VLLSx modes will always exit to RUN mode even if you were
 * in VLPR mode before entering VLLSx.
 *
 * Wakeup from VLLSx mode is controlled by the LLWU module. Most
 * modules cannot issue a wakeup interrupt in VLLSx mode, so make
 * sure to setup the desired wakeup sources in the LLWU before
 * calling this function.
 */
 /********************************************************************/

void enter_vlls1(void)
{
    smc_power_mode_config_t smcConfig;

    /* set power mode to specific Run mode */
    smcConfig.lpwuiOption = false;
    smcConfig.porOption = false;
    smcConfig.powerModeName = kPowerModeVlls;
    smcConfig.stopSubMode = kSmcStopSub1;
    smc_set_power_mode(&smcConfig);
}

/*!
 * @brief VLLS2 mode entry routine. Puts the processor into
 * VLLS2 mode from normal run mode or VLPR.
 *
 * Mode transitions:
 * RUN -> VLLS2
 * VLPR -> VLLS2
 *
 * NOTE: VLLSx modes will always exit to RUN mode even if you were
 * in VLPR mode before entering VLLSx.
 *
 * Wakeup from VLLSx mode is controlled by the LLWU module. Most
 * modules cannot issue a wakeup interrupt in VLLSx mode, so make
 * sure to setup the desired wakeup sources in the LLWU before
 * calling this function.
 */
 /********************************************************************/

void enter_vlls2(void)
{
    smc_power_mode_config_t smcConfig;

    /* set power mode to specific Run mode */
    smcConfig.lpwuiOption = false;
    smcConfig.porOption = false;
    smcConfig.powerModeName = kPowerModeVlls;
    smcConfig.stopSubMode = kSmcStopSub2;
    smc_set_power_mode(&smcConfig);
}

/*!
 * @brief enter in VLPS mode
 */
void enter_vlps(void)
{
    smc_power_mode_config_t smcConfig;

    /* set power mode to specific Run mode */
    smcConfig.lpwuiOption = false;
    smcConfig.porOption = false;
    smcConfig.powerModeName = kPowerModeVlps;
    smc_set_power_mode(&smcConfig);
}

/*!
 * @brief enter in VLLS3 mode
 */
void enter_vlls3(void)
{
    smc_power_mode_config_t smcConfig;

    /* set power mode to specific Run mode */
    smcConfig.lpwuiOption = false;
    smcConfig.porOption = false;
    smcConfig.powerModeName = kPowerModeVlls;
    smcConfig.stopSubMode = kSmcStopSub3;
    smc_set_power_mode(&smcConfig);
}

/*!
 * @brief LLS mode entry routine. Puts the processor into LLS mode from
 * normal run mode or VLPR.
 *
 * Mode transitions:
 * RUN -> LLS
 * VLPR -> LLS
 *
 * NOTE: LLS mode will always exit to RUN mode even if you were
 * in VLPR mode before entering LLS.
 *
 * Wakeup from LLS mode is controlled by the LLWU module. Most
 * modules cannot issue a wakeup interrupt in LLS mode, so make
 * sure to setup the desired wakeup sources in the LLWU before
 * calling this function.
 */
void enter_lls(void)
{
    smc_power_mode_config_t smcConfig;

    /* set power mode to specific Run mode */
    smcConfig.lpwuiOption = false;
    smcConfig.porOption = false;
    smcConfig.powerModeName = kPowerModeLls;
    smcConfig.stopSubMode = kSmcStopSub3;
    smc_set_power_mode(&smcConfig);
}

/*!
 * @brief VLPR mode exit routine. Puts the processor into normal run mode
 * from VLPR mode. You can transition from VLPR to normal run using
 * this function.
 *
 * Mode transitions:
 * VLPR -> RUN
 */
void exit_vlpr(void)
{
    int32_t i;
    smc_power_mode_config_t smcConfig;

    /* set power mode to specific Run mode */
    smcConfig.lpwuiOption = false;
    smcConfig.porOption = false;
    smcConfig.powerModeName = kPowerModeRun;
    smc_set_power_mode(&smcConfig);

    /* Wait for normal RUN regulation mode to be confirmed */
       /* 1 PMSTAT MCU is in RUN  mode */
       /* 4 PMSTAT MCU is in VLPR mode */
       for (i=0;i<0xff;i++)
       {
           if (smc_hal_get_power_mode_stat() == SMC_PMSTAT_PMSTAT_MASK)
           {

			if(pmc_hal_get_regulator_status()==PMC_REGSC_REGONS_MASK)
			{
    	        break;
            }
           }
       }
}

/*!
 * @brief VLPR mode entry routine.Puts the processor into very low power
 * run mode. In this mode all clocks are enabled, but the core clock limited.
 * The flash clock is limited to 1MHz or less.
 *
 * Mode transitions:
 * RUN -> VLPR
 *
 * exit_vlpr() function can be used
 * to switch from VLPR back to RUN.
 *
 * while in VLPR,VLPW or VLPS the exit to VLPR is not possible
 *
 *
 * @return PMSTAT - value or error code
 *
 *                PMSTAT =  000_0001 Current power mode is RUN
 *                       =  000_0100 Current power mode is VLPR
 *
 *                ERROR Code =  0x14 - already in VLPR mode
 *                           =  0x24 - REGONS never clear indicating stop regulation
 */
int32_t enter_vlpr(void)
{
    smc_power_mode_config_t smcConfig;
    int32_t i;
    uint32_t returnValue = 0;  /*default return value = indicates error */
    if (smc_hal_get_power_mode_stat() == kStatVlpr){
        returnValue = 0x14;
    }


    /* Set the (for MC1)LPLLSM or (for MC2)STOPM field
     * to 0b010 for VLPS mode -
     * and RUNM bits to 0b010 for VLPR mode
     */
    smcConfig.lpwuiOption = false;
    smcConfig.porOption = false;
    smcConfig.powerModeName = kPowerModeVlpr;
    smc_set_power_mode(&smcConfig);

    /* Wait for VLPS regulator mode to be confirmed */
    for (i = 0 ; i < 10000 ; i++)
    {
      /*
       * check that the value of REGONS bit is not 0
       * once it is a zero we can stop checking
       */

	  if(pmc_hal_get_regulator_status()==PMC_REGSC_REGONS_MASK){
      /*
       * 0 Regulator is in stop regulation or in transition to/from it
       * 1 MCU is in Run regulation mode
       */
      }
      else
      {
        break;
      }
    }

	if(pmc_hal_get_regulator_status()==PMC_REGSC_REGONS_MASK)
    {
      returnValue = 0x24;
    }
    /* SMC_PMSTAT register only exist in Mode Controller 2 MCU versions */
    if (smc_hal_get_power_mode_stat() == kStatVlpr)
    {
      returnValue = smc_hal_get_power_mode_stat();
    }
    return (returnValue);
}

/*!
 * @brief WAIT mode entry routine. Puts the processor into wait mode.
 * In this mode the core clock is disabled (no code executing), but
 * bus clocks are enabled (peripheral modules are operational).
 *
 * Mode transitions:
 * RUN -> WAIT
 * VLPR -> VLPW
 *
 * This function can be used to enter normal wait mode or VLPW
 * mode. If you are executing in normal run mode when calling this
 * function, then you will enter normal wait mode. If you are in VLPR
 * mode when calling this function, then you will enter VLPW mode instead.
 *
 * NOTE: Some modules include a programmable option to disable them in
 * wait mode. If those modules are programmed to disable in wait mode,
 * they will not be able to generate interrupts to wake up the core.
 *
 * WAIT mode is exited using any enabled interrupt or RESET, so no
 * exit_wait routine is needed.
 *
 * @param pMode - specify the specific wait modes
 *
 *              = kPowerModeVlpw to enter in VLPW mode
 *              = kPowerModeWait to enter in WAIT mode
 */
void enter_wait(power_modes_t pMode)
{
    smc_power_mode_config_t smcConfig;

    smcConfig.lpwuiOption = false;
    smcConfig.porOption = false;
    smcConfig.powerModeName = pMode;
    smc_set_power_mode(&smcConfig);
}

/*!
 * @brief STOP mode entry routine. Puts the processor into normal stop mode.
 * In this mode core, bus and peripheral clocks are disabled.
 *
 * Mode transitions:
 * RUN -> STOP
 *
 * This function can be used to enter normal stop mode.
 * If you are executing in normal run mode when calling this
 * function and AVLP = 0, then you will enter normal stop mode.
 * If AVLP = 1 with previous write to PMPROT
 * then you will enter VLPS mode instead.
 *
 * STOP mode is exited using any enabled interrupt or RESET, so no
 * exit_stop routine is needed.
 *
 * @param partialStopOpt - Partial Stop Option:
 *  0x00 = STOP - Normal Stop Mode
 *  0x40 = PSTOP1 - Partial Stop with both system and bus clocks disabled
 *  0x80 = PSTOP2 - Partial Stop with system clock disabled and bus clock enabled
 *  0xC0 = Reserved
 */
void enter_stop(smc_stop_submode_t partialStopOpt)
{
    smc_power_mode_config_t smcConfig;

    smcConfig.lpwuiOption = false;
    smcConfig.porOption = false;
    smcConfig.powerModeName = kPowerModeStop;
    smcConfig.stopSubMode = partialStopOpt;
    smc_set_power_mode(&smcConfig);
}

/*!
 * @brief clock configuration for very low power
 * @param nextMode = BLPI or BLPE
 */
void vlp_clock_config(int8_t nextMode)
{
    int32_t currentMcgMode, uartClkFreqHz;
    uint8_t returnCode;
    uint32_t settings;

    currentMcgMode = what_mcg_mode();
    if (smc_hal_get_power_mode_stat() == kStatVlpr){
         printf("\n\rIn VLPR Mode ! Must go to RUN to change Clock Modes --> ");
         exit_vlpr();
    }
    if (smc_hal_get_power_mode_stat() == kStatRun)
    {
        printf("In RUN Mode !\n\r");
    }

    switch(currentMcgMode)
    {
      /*current_mcg_mode ==BLPI */
      case 1:
          printf("\n\r in BLPI mode with fast IRC \n\r");
          if(nextMode == FEI){
              mcgClkHz = blpi_fbi(slowIrcFreq, 0);
              mcgClkHz = fbi_fei(slowIrcFreq);
              MCG_C4 |= (MCG_C4_DRST_DRS(1) | MCG_C4_DMX32_MASK);/* 32.768Khz * 1464 = 48Mhz */
              mcgClkHz = 48000000; /*FEI mode */

              clock_hal_set_clock_out_dividers(0,0,0,1);
              /* the uart0 clock frequency will equal the FLL frequency */
              uartClkFreqHz = mcgClkHz;

  #if defined(CPU_MKL25Z128VLK4)
              /*Configure UART for the new clock frequency and IR clk*/
              clock_hal_set_clock_source(kSimClockUart0Src, 1);
  #endif
              uart_hal_set_baud_rate(s_uartInstance, uartClkFreqHz, uartConfig.baudRate);
              /* clear PLLFLLSEL to select the FLL for this clock source */
              clock_hal_set_clock_source(kSimClockPllfllSel, 0);
          }
          break;
      /* current_mcg_mode ==FBI */
      case 2:
          printf("\n\r in FBI mode now \n\r");
          break;

      /* current_mcg_mode ==FEI */
      case 3:
          printf("\n\r in FEI mode ");
          if (nextMode == BLPE)
          {
              mcgClkHz =  fei_fbe( CLK0_FREQ_HZ,  kMcgHgoSelectHigh, kMcgErefClockSelectOsc);
              mcgClkHz = fbe_blpe(CLK0_FREQ_HZ);
              osc_hal_enable_external_reference_clock(kOsc0);
              uartClkFreqHz = CLK0_FREQ_HZ; /* UART0 clock frequency will equal ERCLK */
       #if defined(CPU_MKL25Z128VLK4)
          clock_hal_set_clock_source(kSimClockUart0Src, 3);
       #endif
          uart_hal_set_baud_rate(s_uartInstance, uartClkFreqHz, uartConfig.baudRate);

          printf("\n\r moved to BLPE mode \n\r");

		  clock_hal_set_clock_out_dividers(0,0,0,7);

          }
		  else if (nextMode == BLPI)
          {
            /* next_mode is BLPI */
            clock_set_fcrdiv(0);	/*set to div by 1 */
            mcgClkHz = fei_fbi(fastIrcFreq,FAST_IRC);
            mcgClkHz = fbi_blpi(fastIrcFreq,FAST_IRC);
            /* Internal Fast IRC is 4 MHz so div by 1 for sysclk and div 4 for flash clock */
            /* div 5 for flash clk margin */
            clock_hal_set_clock_out_dividers(0,0,0,4);
            /* the uart0 clock frequency will equal the FAST IRC frequency */
            uartClkFreqHz = mcgClkHz;

            clock_set_irclken(true);
            clock_set_ircs(kMcgIrefClockSelectFast);

		#if defined(CPU_MKL25Z128VLK4)
          /* Configure UART for the oscerclk frequency */
          clock_hal_set_clock_source(kSimClockUart0Src, 3);
        #endif
          uart_hal_set_baud_rate(s_uartInstance, uartClkFreqHz, uartConfig.baudRate);
          printf("\n\r moved to BLPI mode \n\r");
		  }

          break;

      /* current_mcg_mode ==FEE) */
      case 4:
          printf("\n\r in FEE mode \n\r");
          if (nextMode == BLPI){
              clock_set_ircs(kMcgIrefClockSelectSlow);
              clock_set_irclken(true);
              clock_set_fcrdiv(0);	/*set to div by 1 */
              mcgClkHz = fee_fbi(fastIrcFreq, FAST_IRC);
              mcgClkHz = fbi_blpi(fastIrcFreq,FAST_IRC);
              /* Internal Fast IRC is 4 MHz so div by 1 for sysclk and div 4 for flash clock */
              /* div 5 for flash clk margin */
              clock_hal_set_clock_out_dividers(0,0,0,4);
              /* the uart0 clock frequency will equal the FAST IRC frequency */
              uartClkFreqHz = mcgClkHz;
  #if defined(CPU_MKL25Z128VLK4)
              clock_hal_set_clock_source(kSimClockUart0Src, 3);
  #endif
              uart_hal_set_baud_rate(s_uartInstance, uartClkFreqHz, uartConfig.baudRate);
              printf("\n moved to BLPI mode \n");
          }
          else if (nextMode == BLPE){
              mcgClkHz =  fee_fbe( CLK0_FREQ_HZ);
              mcgClkHz = fbe_blpe(CLK0_FREQ_HZ);
              osc_hal_enable_external_reference_clock(kOsc0);
              uartClkFreqHz = CLK0_FREQ_HZ; /* UART0 clock frequency will equal ERCLK   */
  #if defined(CPU_MKL25Z128VLK4)
              clock_hal_set_clock_source(kSimClockUart0Src, 3);
  #endif
              uart_hal_set_baud_rate(s_uartInstance, uartClkFreqHz, uartConfig.baudRate);
              printf("\n moved to BLPE mode \n");
          }

          break;

      /* current_mcg_mode ==FBE) */
      case 5:
          printf("\n\r in FBE mode \n\r");
          break;

      /* current_mcg_mode ==BLPE) */
      case 6:
          printf("\n\r in BLPE mode \n\r");
          if (nextMode == BLPI){
              clock_set_ircs(kMcgIrefClockSelectSlow);
              clock_set_irclken(true);
              clock_set_fcrdiv(0);	/*set to div by 1 */
              mcgClkHz = blpe_fbe(CLK0_FREQ_HZ);
              mcgClkHz = fbe_fbi(fastIrcFreq, FAST_IRC);
              mcgClkHz = fbi_blpi(fastIrcFreq,FAST_IRC);
              /* Internal Fast IRC is 4 MHz so div by 1 for sysclk and div 4 for flash clock */
              /* div 5 for flash clk margin */
              clock_hal_set_clock_out_dividers(0,0,0,4);
              /* the uart0 clock frequency will equal the FAST IRC frequency */
              uartClkFreqHz = mcgClkHz;
  #if defined(CPU_MKL25Z128VLK4)
              clock_hal_set_clock_source(kSimClockUart0Src, 3);
  #endif
              uart_hal_set_baud_rate(s_uartInstance, uartClkFreqHz, uartConfig.baudRate);
              printf("\n\r moved to BLPI mode \n\r");
          }else if (nextMode == PEE){
             clock_hal_set_clock_out_dividers(0,0,0,1);
             /*After wake up back to the original clock frequency */
             mcgClkHz = blpe_pbe(CLK0_FREQ_HZ, PLL0_PRDIV,PLL0_VDIV);
             mcgClkHz = pbe_pee(CLK0_FREQ_HZ);
             uartClkFreqHz = mcgClkHz;
  #if defined(CPU_MKL25Z128VLK4)
             /*Configure UART for the oscerclk frequency */
             uart0ClkHz = (mcgClkHz / 2);
             clock_hal_set_clock_source(kSimClockUart0Src, 1);
  #endif
             uart_hal_set_baud_rate(s_uartInstance, uartClkFreqHz, uartConfig.baudRate);

             printf("moved to PEE clock mode \n\r");
          }
          break;

        /* current_mcg_mode == PBE */
      case 7:
          printf("\n\r in PBE mode \n\r");
          if (nextMode == PEE){

              clock_hal_set_clock_out_dividers(0,0,0,1);
             /* After wake up back to the original clock mode */
             mcgClkHz = pbe_pee(CLK0_FREQ_HZ);
  #if defined(CPU_MKL25Z128VLK4)
             /*Configure UART for the oscerclk frequency*/
             clock_hal_set_clock_source(kSimClockUart0Src, 1);
  #endif
             uart_hal_set_baud_rate(s_uartInstance, mcgClkHz, uartConfig.baudRate);
          }
          printf("moved to PEE clock mode \n\r");

          break;

      /* current_mcg_mode ==8) PEE */
      case 8:
          if (nextMode == BLPI)
          {
          #if (defined(TWR_K22F120M)||defined(FRDM_K22F120M))  /* Use 8Mhz crystal*/
              /* run ATC test to determine irc trim */
              returnCode = atc(FAST_IRC, fastIrcFreq, mcgClkHz);
              if (returnCode != 0){
                printf("\n\rAutotrim Failed\n\r");
              }

              mcgClkHz =  pee_pbe(CRYSTAL);
              mcgClkHz = pbe_fbe(CRYSTAL);
		  #elif (defined(FRDM_K64F120M)||defined(TWR_K64F120M))  /* Use 50Mhz external oscillator*/
			  mcgClkHz =  pee_pbe(CANNED_OSC);
              mcgClkHz = pbe_fbe(CANNED_OSC);
		  #endif
              clock_set_fcrdiv(0);	/*set to div by 1 */
              mcgClkHz = fbe_fbi(fastIrcFreq, FAST_IRC);
              mcgClkHz = fbi_blpi(fastIrcFreq, FAST_IRC);
              uartClkFreqHz = mcgClkHz; /* UART0 clock frequency will equal ERCLK */
              /* div 5 for flash clk margin */
              clock_hal_set_clock_out_dividers(0,0,0,4);
              clock_set_irclken(true);
              clock_set_ircs(kMcgIrefClockSelectFast);
              clock_set_cme0(false);
  #if defined(CPU_MKL25Z128VLK4)
              clock_hal_set_clock_source(kSimClockUart0Src, 3);
  #endif
              uart_hal_set_baud_rate(s_uartInstance, uartClkFreqHz, uartConfig.baudRate);
              printf("\n\r Now moved to BLPI clock mode \n\r");
          }
		  else /*nextMode == blpe*/
          {
              printf("\n\r in PEE clock mode moving to BLPE clock mode \n\r");
              returnCode = atc(FAST_IRC, fastIrcFreq, mcgClkHz);
              if (returnCode != 0){
                    printf("\n\rAutotrim Failed\n\r");
              }
              mcgClkHz = pee_to_blpe();
              osc_hal_enable_external_reference_clock(kOsc0);
#if defined(CPU_MKL25Z128VLK4)
              /*
               * External Reference is 8 MHz so div by 2 for sysclk and further
			   * div by 4 for flash clock
               */
              clock_hal_set_clock_out_dividers(1,0,0,3);
#elif defined(CPU_MK22FN512VDC12)
              /*
               * External Reference is 8 MHz so div by 2 for sysclk, bus clock,
               * flexbus clock and div 8 for flash clock
               */
              clock_hal_set_clock_out_dividers(1,1,1,7);
#endif
              clock_hal_get_clock_divider(kSimClockDividerOutdiv1, &settings);
              mcgClkHz =  mcgClkHz / (settings + 1);
#if defined(CPU_MKL25Z128VLK4)
              /*Configure UART for the oscerclk frequency*/
              uartClkFreqHz = fastIrcFreq; /* UART0 clock frequency will equal fast irc */
#elif defined(CPU_MK22FN512VDC12)
              uartClkFreqHz = mcgClkHz; /* UART clock frequency will equal system clock */
#endif
              clock_set_irclken(true);	/* enable irc */
              clock_set_ircs(kMcgIrefClockSelectFast);	/* select fast irc */
              clock_set_fcrdiv(0);	/*set to div by 1 */
  #if defined(CPU_MKL25Z128VLK4)
              clock_hal_set_clock_source(kSimClockUart0Src, 3);
  #endif
              uart_hal_set_baud_rate(s_uartInstance, uartClkFreqHz, uartConfig.baudRate);
              printf("\n\r Now moved to BLPE clock mode \n\r");
          }
          break;

      default:
          break;
      /* end of case statement*/
    }
}

/*!
 * @brief LLWU ISR function
 */
void llwu_isr(void)
{
    NVIC->ICPR[0] |= 1 << (LLW_IRQn%32);

    /* Print LLWU acknowledgement only if UART is enabled */
    if(uartEnabled)
    {
        printf("\n\n\r-- Entered LLWU interrupt ISR! --\n\n\r");
    }

   if (LLWU_F1 & LLWU_F1_WUF0_MASK) {
        LLWU_F1 |= LLWU_F1_WUF0_MASK;   /* write one to clear the flag */
   }
   if (LLWU_F1 & LLWU_F1_WUF1_MASK) {
        LLWU_F1 |= LLWU_F1_WUF1_MASK;   /* write one to clear the flag */
   }
   if (LLWU_F1 & LLWU_F1_WUF2_MASK) {
        LLWU_F1 |= LLWU_F1_WUF2_MASK;   /* write one to clear the flag */
   }
   if (LLWU_F1 & LLWU_F1_WUF3_MASK) {
        LLWU_F1 |= LLWU_F1_WUF3_MASK;   /* write one to clear the flag */
   }
   if (LLWU_F1 & LLWU_F1_WUF4_MASK) {
        LLWU_F1 |= LLWU_F1_WUF4_MASK;   /* write one to clear the flag */
   }
   if (LLWU_F1 & LLWU_F1_WUF5_MASK) {
        LLWU_F1 |= LLWU_F1_WUF5_MASK;   /* write one to clear the flag */
   }
   if (LLWU_F1 & LLWU_F1_WUF6_MASK) {
        LLWU_F1 |= LLWU_F1_WUF6_MASK;   /* write one to clear the flag */
    }
   if (LLWU_F1 & LLWU_F1_WUF7_MASK) {
        LLWU_F1 |= LLWU_F1_WUF7_MASK;   /* write one to clear the flag */
   }
   if (LLWU_F2 & LLWU_F2_WUF8_MASK) {
        LLWU_F2 |= LLWU_F2_WUF8_MASK;   /* write one to clear the flag */
   }
   if (LLWU_F2 & LLWU_F2_WUF9_MASK) {
        LLWU_F2 |= LLWU_F2_WUF9_MASK;   /* write one to clear the flag */
   }
   if (LLWU_F2 & LLWU_F2_WUF10_MASK) {
        LLWU_F2 |= LLWU_F2_WUF10_MASK;   /* write one to clear the flag */
   }
   if (LLWU_F2 & LLWU_F2_WUF11_MASK) {
        LLWU_F2 |= LLWU_F2_WUF11_MASK;   /* write one to clear the flag */
   }
   if (LLWU_F2 & LLWU_F2_WUF12_MASK) {
        LLWU_F2 |= LLWU_F2_WUF12_MASK;   /* write one to clear the flag */
   }
   if (LLWU_F2 & LLWU_F2_WUF13_MASK) {
        LLWU_F2 |= LLWU_F2_WUF13_MASK;   /* write one to clear the flag */
   }
   if (LLWU_F2 & LLWU_F2_WUF14_MASK) {
        LLWU_F2 |= LLWU_F2_WUF14_MASK;   /* write one to clear the flag */
   }
   if (LLWU_F2 & LLWU_F2_WUF15_MASK) {
        LLWU_F2 |= LLWU_F2_WUF15_MASK;   /* write one to clear the flag */
   }

   /*
    * Note: This ISR does not write to the LLWU_F3 register because these
    * are peripheral module wakeups.  The flags contained in the LLWU_F3
    * register should be cleared through the associated module interrupt
    * and not through the LLWU_F3 per the Kinetis L Family Reference
    * Manual (LLWU Chapter)
    */
    if (LLWU_F3 & LLWU_F3_MWUF0_MASK) {
        clock_manager_set_gate(kClockModuleLPTIMER, 0, true);
        LPTMR0_CSR |=  LPTMR_CSR_TCF_MASK;   /* write 1 to TCF to clear the LPT timer compare flag */
        LPTMR0_CSR = ( LPTMR_CSR_TEN_MASK | LPTMR_CSR_TIE_MASK | LPTMR_CSR_TCF_MASK  );
    }
    if(LLWU_FILT1 & LLWU_FILT1_FILTF_MASK){
	LLWU_FILT1 |= LLWU_FILT1_FILTF_MASK;
    }
    if(LLWU_FILT2 & LLWU_FILT2_FILTF_MASK){
	LLWU_FILT2 |= LLWU_FILT2_FILTF_MASK;
    }
}

/*!
 * @brief ISR Routine for Low Power Timer
 */
void demo_lptmr_isr(void)
{
    volatile uint32_t dummyRead;
    clock_manager_set_gate(kClockModuleLPTIMER, 0, 1);

    if(uartEnabled)
    {
        printf(" \r[demo_lptmr_isr] \n\r  > ");
    }

    LPTMR0_CSR |=  LPTMR_CSR_TCF_MASK;   /* write 1 to TCF to clear the LPT timer compare flag */
    LPTMR0_CSR = ( LPTMR_CSR_TEN_MASK | LPTMR_CSR_TIE_MASK | LPTMR_CSR_TCF_MASK  );
    /* enable timer */
    /* enable interrupts */
    /* clear the flag */
    /*wait for write to complete to  before returning */
    dummyRead = LPTMR0_CSR;

    return;
}

/*!
 * @brief Low Power Timer initialization routine
 * @param count - initialize timer for 'count' miliseconds
 * @param clockSource - clock source
 */
void lptmr_init(uint32_t count, uint32_t clockSource)
{
    clock_manager_set_gate(kClockModuleLPTIMER, 0, true);

    LPTMR0_PSR = ( LPTMR_PSR_PRESCALE(0) /* 0000 is div 2 */
                 | LPTMR_PSR_PBYP_MASK  /* LPO feeds directly to LPT */
                 | LPTMR_PSR_PCS(clockSource)) ; /* use the choice of clock */
    if (clockSource== 0)
    {
        printf("\n\r LPTMR Clock source is the MCGIRCLK \n\r");
    }
    if (clockSource== 1)
    {
        printf("\n\r LPTMR Clock source is the LPOCLK \n\r");
    }
    if (clockSource== 2)
    {
        printf("\n\r LPTMR Clock source is the ERCLK32 \n\r");
    }
    if (clockSource== 3)
    {
        printf("\n\r LPTMR Clock source is the OSCERCLK \n\r");
    }

    LPTMR0_CMR = LPTMR_CMR_COMPARE(count);  /*Set compare value */

    LPTMR0_CSR =(  LPTMR_CSR_TCF_MASK   /* Clear any pending interrupt */
                 | LPTMR_CSR_TIE_MASK   /* LPT interrupt enabled */
                 | LPTMR_CSR_TPS(0)     /*TMR pin select */
                 |!LPTMR_CSR_TPP_MASK   /*TMR Pin polarity */
                 |!LPTMR_CSR_TFC_MASK   /* Timer Free running counter is reset whenever TMR counter equals compare */
                 |!LPTMR_CSR_TMS_MASK   /*LPTMR0 as Timer */
                );

    /* Enable the IRQs */
    interrupt_enable(LPTimer_IRQn);

    LPTMR0_CSR |= LPTMR_CSR_TEN_MASK;   /*Turn on LPT and start counting */
}

/*!
 * @brief initializes LLWU module
 */
void llwu_init(void)
{
    /* Configure the interrupt vector for the interrupt handler */
    interrupt_register_handler(LLW_IRQn, &llwu_isr);

    /* Enable the IRQs */
    interrupt_enable(LLW_IRQn);

    /* Configure the interrupt vector for the interrupt handler */
    interrupt_register_handler(LPTimer_IRQn, &demo_lptmr_isr);

    /* 
     * TWR_K22F120M and TWR_K64F120M: SW1,PTC6 LLWU_P10
     * FRDM_K64F120M: SW2,PTC6 LLWU_P10
     * FRDM_K22F120M: SW2,PTC1 LLWU_P6
     */
#if defined(FRDM_K22F120M)    
    llwu_configure(0x0040, LLWU_PIN_RISING, 0x1);
#else
    llwu_configure(0x0400, LLWU_PIN_RISING, 0x1);
#endif
}

/*!
 * @brief main low power mode test routine
 */
void low_power_modes_test(void)
{
    int32_t i;
    uint8_t opMode, fcrDivVal;
    int8_t testNum = UNDEF_VALUE;
    uint8_t testVal;
    uint32_t settings;

    printf("\n\r*------------------------------------------------------------*");
#if defined(CPU_MK22FN512VDC12)
    printf("\n\r*                    K22 Low Power DEMO                       *");
#elif defined(CPU_MK64FN1M0VMD12)
    printf("\n\r*                    K64 Low Power DEMO                      *");
#else
    printf("\n\r*                    KL Low Power DEMO                       *");
#endif
    printf("\n\r*                    %s %s                    *", __DATE__, __TIME__);
    printf("\n\r*------------------------------------------------------------*\n\r");

    /* Read and flush any unread garbage data in the receive uart DATA port */

    while(1)
    {
        while (testNum > 22 | testNum < 0){
            LED0_ON;
            LED1_ON;
            LED2_ON;
#if defined(TWR_K22F120M)
            LED3_ON;
#endif
            if (smc_hal_get_power_mode_stat() == kStatVlpr){
                printf("  in VLPR Mode !  ");
            } else if (smc_hal_get_power_mode_stat() == kStatRun){
                printf("  in Run Mode  !  ");
            }
            opMode = what_mcg_mode();
            mcgClkHz = clock_hal_get_outclk();

            clock_hal_get_clock_divider(kSimClockDividerOutdiv1, &settings);
            mcgClkHz =  mcgClkHz / (settings + 1);

            if (opMode ==1)
            {
                printf("in BLPI mode now at %d Hz\r\n",mcgClkHz );
            }
            if (opMode ==2)
            {
                printf(" in FBI mode now at %d Hz\r\n",mcgClkHz );
            }
            if (opMode ==3)
            {
                printf(" in FEI mode now at %d Hz\r\n",mcgClkHz );
            }
            if (opMode ==4)
            {
                printf(" in FEE mode now at %d Hz\r\n",mcgClkHz );
            }
            if (opMode ==5)
            {
                printf(" in FBE mode now at %d Hz\r\n",mcgClkHz );
            }
            if (opMode ==6)
            {
                printf(" in BLPE mode now at %d Hz\r\n",mcgClkHz );
            }
            if (opMode ==7)
            {
                printf(" in PBE mode now at %d Hz\r\n",mcgClkHz );
            }
            if (opMode ==8)
            {
                printf(" in PEE mode now at %d Hz\r\n",mcgClkHz );
            }
            printf("\n\rSelect the desired operation \n\r");
            printf("0 for CASE 0: Enter VLLS0 with POR disabled (Very Low Leakage STOP 0) NO POR\n\r");
            printf("1 for CASE 1: Enter VLLS0 with POR enabled (Very Low Leakage STOP 0) with POR\n\r");
            printf("2 for CASE 2: Enter VLLS1 (Very Low Leakage STOP 1)\n\r");
#if (defined(CPU_MK22FN512VDC12) || defined(CPU_MK64FN1M0VMD12))
            printf("3 for CASE 3: Enter VLLS2 (Very Low Leakage STOP 2)\n\r");
#endif
            printf("4 for CASE 4: Enter VLLS3 (Very Low Leakage STOP 3)\n\r");
            printf("5 for CASE 5: Enter LLS(Low Leakage Stop)\n\r");
            printf("6 for CASE 6: Enter VLPS(Very Low Power Stop)\n\r");
#if (defined(TWR_K22F120M)||defined(FRDM_K22F120M))
            printf("7 for CASE 7: Enter VLPR(Very Low Power RUN) in BLPE (8 MHz Crystal) \n\r");
#elif (defined(FRDM_K64F120M)||defined(TWR_K64F120M))
	    printf("7 for CASE 7: BLPE demo not supported on this platform! \n\r");
#endif
            printf("8 for CASE 8: Exit VLPR(Very Low Power RUN)\n\r");
            printf("9 for CASE 9: Enter VLPW(Very Low Power WAIT)\n\r");
            printf("A for CASE 10: Enter WAIT from RUN \n\r");
            printf("B for CASE 11: Enter Normal STOP from RUN or VLPS from VLPR\n\r");
#if defined(CPU_MK22FN512VDC12)
            printf("C for CASE 12: Enter PARTIAL STOP 1 with both system and bus clocks disabled\n\r");
            printf("D for CASE 13: Enter PARTIAL STOP 2 with system clock disabled and bus clock enabled\n\r");
#endif
            printf("F for CASE 15: Enter LLS with LPTMR 1 second wakeup loop (Low Leakage Stop)\n\r");
            printf("G for CASE 16: Enable LPTMR to wakeup every 5 seconds from any mode except VLLS0\n\r");
            printf("H for CASE 17: Disable LPTMR wakeup\n\r");
            printf("I for CASE 18: Enter VLPR in BLPI at Core Frequency of 4 MHz\n\r");
            printf("J for CASE 19: Enter VLPR in BLPI at Core Frequency of 2 MHz\n\r");
            printf("L for CASE 21: To enable DEBUG");

            printf("\n\r > ");

            uart_receive_data(&uartState, &testVal, 1, kSyncWaitForever);

			uart_send_data(&uartState, &testVal, 1, kSyncWaitForever);

            printf("\n\r");

            if((testVal>=0x30) && (testVal<=0x39))
            {
                testNum = testVal - 0x30;
            }
            if((testVal>=0x41) && (testVal<=0x4C))
            {
                testNum = testVal - 0x37;
            }
            if((testVal>=0x61) && (testVal<=0x6C))
            {
                testNum = testVal - 0x57;
            }
        }
        LED0_OFF;
        LED1_OFF;
        LED2_OFF;
#if defined(TWR_K22F120M)
        LED3_OFF;
#endif

        switch(testNum){
            case 0:/*VLLS0 no POR */
                printf("Press any key to enter VLLS0 with POR disable\n\r ");
                uart_receive_data(&uartState, &testVal, 1, kSyncWaitForever);
#if defined(FRDM_KL25Z48M)
                printf("Press RESET to wake up from VLLS0\n\r ");
#elif (defined(TWR_K22F120M)||defined(TWR_K64F120M))
                printf("Press SW1 to wake up from VLLS0\n\r ");
#elif (defined(FRDM_K64F120M)||defined(FRDM_K22F120M))
                printf("Press SW2 to wake up from VLLS0\n\r ");
#endif
                /* Disable clock monitor before entering low power mode */
                clock_set_cme0(false);

                set_for_lp(0);
                enter_vlls0(1);
                break;

            case 1:/*VLLS0 */
                printf("Press any key to enter VLLS0 with POR enabled\n\r ");
                uart_receive_data(&uartState, &testVal, 1, kSyncWaitForever);
#if defined(FRDM_KL25Z48M)
                printf("\n\rPress RESET to wake up from VLLS0\n\r ");
#elif (defined(TWR_K22F120M)||defined(TWR_K64F120M))
                printf("Press SW1 to wake up from VLLS0\n\r ");
#elif (defined(FRDM_K64F120M)||defined(FRDM_K22F120M))
                printf("Press SW2 to wake up from VLLS0\n\r ");
#endif
                /* Disable clock monitor before entering low power mode */
                clock_set_cme0(false);
                set_for_lp(0);
                enter_vlls0(0);
                break;

            case 2:/*VLLS1 */
                printf("Press any key to enter VLLS1\n\r ");
                uart_receive_data(&uartState, &testVal, 1, kSyncWaitForever);
#if defined(FRDM_KL25Z48M)
                printf("Press RESET to wake up from VLLS1\n\r ");
#elif (defined(TWR_K22F120M)||defined(TWR_K64F120M))
                printf("Press SW1 to wake up from VLLS1\n\r ");
#elif (defined(FRDM_K64F120M)||defined(FRDM_K22F120M))
                printf("Press SW2 to wake up from VLLS1\n\r ");
#endif
                /* Disable clock monitor before entering low power mode */
                clock_set_cme0(false);
                set_for_lp(0);
                enter_vlls1();
                break;

#if (defined(CPU_MK22FN512VDC12) || defined(CPU_MK64FN1M0VMD12))
            case 3:/*VLLS2 */
                printf("Press any key to enter VLLS2\n\r ");
                uart_receive_data(&uartState, &testVal, 1, kSyncWaitForever);
#if (defined(TWR_K22F120M)||defined(TWR_K64F120M))
                printf("Press SW1 to wake up from VLLS2\n\r ");
#elif (defined(FRDM_K64F120M)||defined(FRDM_K22F120M))
                printf("Press SW2 to wake up from VLLS2\n\r ");
#endif
                /* Disable clock monitor before entering low power mode */
                clock_set_cme0(false);
                set_for_lp(0);
                enter_vlls2();
                break;
#endif
            case 4:/*VLLS3 */
                printf("Press any key to enter VLLS3\n\r ");
                uart_receive_data(&uartState, &testVal, 1, kSyncWaitForever);
#if defined(FRDM_KL25Z48M)
                printf("Press RESET to wake up from VLLS3\n\r ");
#elif (defined(TWR_K22F120M)||defined(TWR_K64F120M))
                printf("Press SW1 to wake up from VLLS3\n\r ");
#elif (defined(FRDM_K64F120M)||defined(FRDM_K22F120M))
                printf("Press SW2 to wake up from VLLS3\n\r ");
#endif
                /* Disable clock monitor before entering low power mode */
                clock_set_cme0(false);
                set_for_lp(0);
                enter_vlls3();
                break;

            case 5:/*LLS */
                printf("Press any key to enter LLS\n\r ");
                uart_receive_data(&uartState, &testVal, 1, kSyncWaitForever);
#if defined(FRDM_KL25Z48M)
                printf("Press RESET to wake up from LLS\n\r ");
#elif (defined(TWR_K22F120M)||defined(TWR_K64F120M))
                printf("Press SW1 to wake up from LLS\n\r ");
#elif (defined(FRDM_K64F120M)||defined(FRDM_K22F120M))
                printf("Press SW2 to wake up from LLS\n\r ");
#endif
                /* Disable clock monitor before entering low power mode */
                clock_set_cme0(false);
                set_for_lp(0);
                enter_lls();
                set_for_lp(1);
           /*
                   * After LLS wake up that was enter from PEE the exit will be on PBE
                   * for this reason after wake up make sure to get back to desired
                   * clock mode
                   */
                opMode = what_mcg_mode();
                if(opMode==PBE)
                {
                    mcgClkHz = pbe_pee(CLK0_FREQ_HZ);
#if defined(CPU_MKL25Z128VLK4)
                    uart0ClkHz = (mcgClkHz / 2); /* UART0 clock frequency will equal half the PLL frequency */
                    uart_hal_set_baud_rate(s_uartInstance, uart0ClkHz, uartConfig.baudRate);
#elif (defined(CPU_MK22FN512VDC12)||defined(CPU_MK64FN1M0VMD12))
                    uartClkHz = mcgClkHz; /* UART clock frequency will equal system frequency */
                    uart_hal_set_baud_rate(s_uartInstance, uartClkHz, uartConfig.baudRate);
#endif
                }

                printf(" LLS Back to RUN mode \n\r");

                break;

            case 6:/*VLPS */
                printf("Press any key to enter VLPS\n\r ");
                uart_receive_data(&uartState, &testVal, 1, kSyncWaitForever);
#if defined(FRDM_KL25Z48M)
                printf("Press RESET to wake up from VLPS\n\r ");
#elif (defined(TWR_K22F120M)||defined(FRDM_K64F120M)||defined(TWR_K64F120M)||defined(FRDM_K22F120M))
                printf("Press SW3 to wake up from VLPS\n\r ");
#endif
                clock_set_cme0(false);
                // Setup for LP mode
                set_for_lp(2);
                /*Go to VLPS*/
                enter_vlps();  /*1 Means: Any interrupt could wake up from this mode */
                
                // Re-enable pins
                set_for_lp(1);
                
                if (smc_hal_get_power_mode_stat() == kStatVlpr){
                    printf("  in VLPR Mode !\n\r ");
                } else if (smc_hal_get_power_mode_stat() == kStatRun){
                    opMode = what_mcg_mode(); /* check if in PBE mode and if so, switch to PEE */
                    if(opMode==PBE)
                    {
                        mcgClkHz = pbe_pee(CLK0_FREQ_HZ);
#if defined(CPU_MKL25Z128VLK4)
                        uart0ClkHz = (mcgClkHz / 2); /* UART0 clock frequency will equal half the PLL frequency */
                        uart_hal_set_baud_rate(s_uartInstance, uart0ClkHz, uartConfig.baudRate);
#elif (defined(CPU_MK22FN512VDC12)||defined(CPU_MK64FN1M0VMD12))
                        uartClkHz = mcgClkHz; /* UART clock frequency will equal system frequency */
                        uart_hal_set_baud_rate(s_uartInstance, uartClkHz, uartConfig.baudRate);
#endif
                        printf("  VLPS back to Run Mode  !\n\r ");
                    }
                    clock_set_cme0(true);
                }
                break;

            case 7:/*VLPR */
#if defined(FRDM_K64F120M) || defined(TWR_K64F120M)
              printf("Mode not supported on this platform! \n\n\r");
#else
                if (smc_hal_get_power_mode_stat() == kStatVlpr)
                {
                    exit_vlpr();
                }
                /*Maximum clock frequency for this mode is core 4MHz and Flash 1Mhz*/
                printf("Press any key to enter VLPR\n\r ");
                uart_receive_data(&uartState, &testVal, 1, kSyncWaitForever);
                printf("Configure clock frequency to 4MHz core clock and 1MHz flash clock\n\r ");
                opMode = what_mcg_mode();
                if(opMode==PEE)
                {
                  vlp_clock_config(BLPE);

                }
                else if (opMode==FEI)
                {
                   vlp_clock_config(BLPI);
                }
                clock_set_cme0(false);
                /*Go to VLPR*/
                enter_vlpr();   /* get out of VLPR back to RUN */
                if (smc_hal_get_power_mode_stat() == kStatVlpr){
                    printf("  in VLPR Mode !\n\r ");
                } else if (smc_hal_get_power_mode_stat() == kStatRun){
                    printf("  in Run Mode  !\n\r ");
                    clock_set_cme0(true);
                }
#endif
                break;

            case 8:/* Exit VLPR */
                printf("Press any key to exit VLPR\n\r ");
                uart_receive_data(&uartState, &testVal, 1, kSyncWaitForever);
                /*Exit VLPR*/
                exit_vlpr();
                opMode = what_mcg_mode();
                if(opMode==BLPE)
                {
                    vlp_clock_config(PEE);
                    clock_set_cme0(true);
                } else if (opMode==BLPI)
                {
                    vlp_clock_config(FEI);
                }
                if (smc_hal_get_power_mode_stat() == kStatVlpr){
                    printf("  in VLPR Mode !\n\r ");
                } else if (smc_hal_get_power_mode_stat() == kStatRun){
                  printf("  in Run Mode  !\n\r ");

                }
                break;

            case 9:/* VLPW */
                 if (smc_hal_get_power_mode_stat() == kStatRun){
                    printf("\n\r ERROR - cannot enter VLPW from Run Mode  !\n\r ");
                 } else if (smc_hal_get_power_mode_stat() == kStatVlpr){
                    printf("Press any key to enter VLPW\n\r ");
                    uart_receive_data(&uartState, &testVal, 1, kSyncWaitForever);
#if defined(FRDM_KL25Z48M)
                    printf("Press RESET to wake up from VLPW\n\r ");
#elif (defined(TWR_K22F120M)||defined(FRDM_K64F120M)||defined(TWR_K64F120M)||defined(FRDM_K22F120M))
                    printf("Press SW3 to wake up from VLPW\n\r ");
#endif
                    printf("  in VLPR Mode moving to VLPW!\n\r ");
                    clock_set_cme0(false);
                    /*Enter to VLPW*/
                    enter_wait(kPowerModeVlpw);

                    if (smc_hal_get_power_mode_stat() == kStatVlpr){
                       printf("  VLPW back to VLPR Mode !\n\r ");
                    } else if (smc_hal_get_power_mode_stat() == kStatRun){
                       printf("  VLPW back to Run Mode  !\n\r ");
                       clock_set_cme0(true);
                   }
                }
                 break;
            case 0xa:/*WAIT*/
                printf("Press any key to enter Wait\n\r ");
                uart_receive_data(&uartState, &testVal, 1, kSyncWaitForever);
#if defined(FRDM_KL25Z48M)
                printf("Press RESET to wake up from Wait\n\r ");
#elif (defined(TWR_K22F120M)||defined(FRDM_K64F120M)||defined(TWR_K64F120M)||defined(FRDM_K22F120M))
                printf("Press SW3 to wake up from Wait\n\r ");
#endif
                clock_set_cme0(false);
                /*Enter wait mode*/
                enter_wait(kPowerModeWait);
                if (smc_hal_get_power_mode_stat() == kStatVlpr){
                    printf("  in VLPR Mode !\n\r ");
                } else if (smc_hal_get_power_mode_stat() == kStatRun){
                    printf("  in Run Mode  !\n\r ");
                    clock_set_cme0(true);
                }

                break;

            case 0xb:/*STOP*/
                printf("Press any key to enter Normal Stop\n\r ");
                uart_receive_data(&uartState, &testVal, 1, kSyncWaitForever);
#if defined(FRDM_KL25Z48M)
                printf("Press RESET to wake up from Normal Stop\n\r ");
#elif (defined(TWR_K22F120M)||defined(FRDM_K64F120M)||defined(TWR_K64F120M)||defined(FRDM_K22F120M))
                printf("Press SW3 to wake up from Normal Stop\n\r ");
#endif
                clock_set_cme0(false);
                
                // Setup for LP mode
                set_for_lp(2);
                
                /*Enter stop mode*/
                enter_stop(kSmcStopSub0);
                
                // Re-enable pins
                set_for_lp(1);
            /*
                   * After LLS wake up that was enter from PEE the exit will be on PBE
                   * for this reason after wake up make sure to get back to desired
                   * clock mode
                   */
                LED1_OFF;
                LED0_ON;
                opMode = what_mcg_mode();
                if(opMode==PBE)
                {
                    mcgClkHz = pbe_pee(CLK0_FREQ_HZ);
#if defined(CPU_MKL25Z128VLK4)
                    uart0ClkHz = (mcgClkHz / 2); /* UART0 clock frequency will equal half the PLL frequency */
                    clock_hal_set_clock_source(kSimClockUart0Src, 1);
                    uart_hal_set_baud_rate(s_uartInstance, uart0ClkHz, uartConfig.baudRate);
#elif (defined(CPU_MK22FN512VDC12)||defined(CPU_MK64FN1M0VMD12))
                    uartClkHz = mcgClkHz; /* UART clock frequency will equal system frequency */
                    uart_hal_set_baud_rate(s_uartInstance, uartClkHz, uartConfig.baudRate);
#endif
                }
                if (smc_hal_get_power_mode_stat() == kStatVlpr){
                    printf("  Stop back to VLPR Mode !\n\r ");
                } else if (smc_hal_get_power_mode_stat() == kStatRun){
                    printf("  Stop back to Run Mode  !\n\r ");
                    clock_set_cme0(true);
                }

                break;

            case 0xc:/*PSTOP1 */
                printf("Press any key to enter Stop PSTOP1\n\r ");
                uart_receive_data(&uartState, &testVal, 1, kSyncWaitForever);
#if defined(FRDM_KL25Z48M)
                printf("Press RESET to wake up from Stop PSTOP1 \n\r ");
#elif (defined(TWR_K22F120M)||defined(FRDM_K22F120M))
                printf("Press SW3 to wake up from Stop PSTOP1 \n\r ");
#endif
                clock_set_cme0(false);
                /*
                 * Enter stop mode
                 * Partial Stop Option:
                 *  0x00 = STOP - Normal Stop Mode
                 *  0x40 = PSTOP1 - Partial Stop with both system and bus clocks disabled
                 *  0x80 = PSTOP2 - Partial Stop with system clock disabled and
                 *                  bus clock enabled
                 */
                /* The next line is to resolve an errata where the LOCK bit is cleared in CPO */
                clock_set_pllclken0(true); /* set PLLSTEN to keep PLL LOCK bit from clearing */
                enter_stop(kSmcStopSub1);
                /* The next line is to clear PLLSTEN associated with PLL LOCK bit errata */
                clock_set_pllclken0(false); /* clear PLLSTEN now that CPO mode is finished with */
                /*
                 * After LLS wake up that was enter from PEE the exit will be on PBE
                 * for this reason after wake up make sure to get back to desired clock mode
                 */
                opMode = what_mcg_mode();
                if(opMode==PBE)
                {
                    mcgClkHz = pbe_pee(CLK0_FREQ_HZ);
#if defined(CPU_MKL25Z128VLK4)
                    uart0ClkHz = (mcgClkHz / 2); /* UART0 clock frequency will equal half the PLL frequency */
                    clock_hal_set_clock_source(kSimClockUart0Src, 1);
                    uart_hal_set_baud_rate(s_uartInstance, uart0ClkHz, uartConfig.baudRate);
#elif defined(CPU_MK22FN512VDC12)
                    uartClkHz = mcgClkHz; /* UART0 clock frequency will equal system frequency */
                    uart_hal_set_baud_rate(s_uartInstance, uartClkHz, uartConfig.baudRate);
#endif
                }
                if (smc_hal_get_power_mode_stat() == kStatVlpr){
                    printf("  in VLPR Mode !\n\r ");
                } else if (smc_hal_get_power_mode_stat() == kStatRun){
                    printf("  in Run Mode  !\n\r ");
                    clock_set_cme0(true);
                }
                break;

            case 0xd:/*STOP PSTOP2*/
                printf("Press any key to enter Stop PSTOP2\n\r ");
                uart_receive_data(&uartState, &testVal, 1, kSyncWaitForever);
#if defined(FRDM_KL25Z48M)
                printf("Press RESET to wake up from Stop PSTOP2\n\r ");
#elif (defined(TWR_K22F120M)||defined(FRDM_K22F120M))
                printf("Press SW3 to wake up from Stop PSTOP2\n\r ");
#endif
                clock_set_cme0(false);
                /*
                 * Enter stop mode
                 * Partial Stop Option:
                 *  0x00 = STOP - Normal Stop Mode
                 *  0x40 = PSTOP1 - Partial Stop with both system and bus clocks disabled
                 *  0x80 = PSTOP2 - Partial Stop with system clock disabled and bus clock enabled
                 */
                  enter_stop(kSmcStopSub2);
                  /*
                   * After LLS wake up that was enter from PEE the exit will be on PBE
                   * for this reason after wake up make sure to get back to desired
                   * clock mode
                   */
                  opMode = what_mcg_mode();
                  if(opMode==PBE)
                  {
                      mcgClkHz = pbe_pee(CLK0_FREQ_HZ);
#if defined(CPU_MKL25Z128VLK4)
                      uart0ClkHz = (mcgClkHz / 2); /* UART0 clock frequency will equal half the PLL frequency */
                      clock_hal_set_clock_source(kSimClockUart0Src, 1);
                      uart_hal_set_baud_rate(s_uartInstance, uart0ClkHz, uartConfig.baudRate);
#elif defined(CPU_MK22FN512VDC12)
                      uartClkHz = mcgClkHz; /* UART clock frequency will equal system frequency */
                      uart_hal_set_baud_rate(s_uartInstance, uartClkHz, uartConfig.baudRate);
#endif
                  }
                  if (smc_hal_get_power_mode_stat() == kStatVlpr){
                      printf("  in VLPR Mode !\n\r ");
                  } else if (smc_hal_get_power_mode_stat() == kStatRun){
                      printf("  in Run Mode  !\n\r ");
                      clock_set_cme0(true);
                  }
                  break;

              case 0xf:/*LLS loop wake up every 1000 ms */
                  printf("Press any key to enter LLS with LPTMR 1 Second wakeup loop\n\r ");
                  uart_receive_data(&uartState, &testVal, 1, kSyncWaitForever);

                  llwu_configure(0x0000, LLWU_PIN_FALLING, 0x1);
                  lptmr_init(1000,LPTMR_USE_LPOCLK);

                printf("Entering LLS mode with LPTMR enabled for 1 second interrupts\n\r ");
                  for (i=0;i<10;i++)
                  {
                      printf("Entering LLS mode\n\r");
                      clock_set_cme0(false);
                      printf("Set for LP ... ");
                      set_for_lp(0);
                      enter_lls();
                      set_for_lp(1);
                      clock_set_cme0(true);

              /*
                       * After LLS wake up that was enter from PEE the exit will be on PBE
                       * for this reason after wake up make sure to get back to desired
                       * clock mode
                       */
                      opMode = what_mcg_mode();
                      if(opMode==PBE)
                      {
                          mcgClkHz = pbe_pee(CLK0_FREQ_HZ);
  #if defined(CPU_MKL25Z128VLK4)
                          uart0ClkHz = (mcgClkHz / 2); /* UART0 clock frequency will equal half the PLL frequency */
                          uart_hal_set_baud_rate(s_uartInstance, uart0ClkHz, uartConfig.baudRate);
  #elif (defined(CPU_MK22FN512VDC12)||defined(CPU_MK64FN1M0VMD12))
                          uartClkHz = mcgClkHz; /* UART clock frequency will equal system frequency */
                          uart_hal_set_baud_rate(s_uartInstance, uartClkHz, uartConfig.baudRate);
  #endif
                      }
                      printf("\n Loop Count 0x%02X ",i) ;
                      printf("  Back in RUN mode \n\r");
                  }
                  LPTMR0_CSR &= ~LPTMR_CSR_TEN_MASK;
                  break;

              case 0x10:  /*set up LPTMR alarm to wakeup every 5 secondes */
                  /*enable LPTMR to cause LLWU wakeup */
                  llwu_configure(0x0000, LLWU_PIN_FALLING, 0x1);
                  lptmr_init(5000,LPTMR_USE_LPOCLK);

                  break;

              case 0x11:  /*set up LPTMR alarm to wakeup every 5 secondes */
                  /*disable LPTMR to cause LLWU wakeup */
                  clock_manager_set_gate(kClockModuleLPTIMER, 0, 1);
                  LPTMR0_CSR &= ~LPTMR_CSR_TEN_MASK;
                  printf("\n\r  Disabled LPTMR  \n\r");
                  break;

              case 0x12:/*VLPR in BLPI 4 MHZ */
                  /*Maximum clock frequency for this mode is core 4MHz and Flash 1Mhz*/
                  printf("Press any key to enter VLPR in BLPI mode \n\r ");
                  uart_receive_data(&uartState, &testVal, 1, kSyncWaitForever);
#if defined(CPU_MKL25Z128VLK4)
				  /*Maximum clock frequency for this mode is core 4MHz and Flash 1Mhz*/
                  printf("Configure clock frequency to 4MHz core clock and 1MHz flash clock\n\r ");
#elif (defined(CPU_MK22FN512VDC12)||defined(CPU_MK64FN1M0VMD12))
				  /*Maximum clock frequency for this mode is core 4MHz and Flash 800Khz*/
                  printf("Configure clock frequency to 4MHz core clock and 800KHz flash clock\n\r ");
#endif
                  /*Get out of VLPR to change clock dividers*/
                  exit_vlpr();  /* get out of VLPR back to RUN */
                  vlp_clock_config(BLPI);
                  /* default clock divider is set to divide by 1 of Fast IRC and Div2 for Bus */
                  clock_hal_set_clock_out_dividers(0,0,0,4);

                  fcrDivVal = (1 << clock_get_fcrdiv());	/* calculate the fast IRC divder factor */
                  mcgClkHz =  (fastIrcFreq / fcrDivVal); /* MCGOUT frequency equals fast IRC frequency divided by 2 */

                  clock_hal_get_clock_divider(kSimClockDividerOutdiv1, &settings);
                  mcgClkHz =  mcgClkHz / (settings + 1);

                  uartClkHz = mcgClkHz; /* UART clock frequency will equal system frequency */
                  uart_hal_set_baud_rate(s_uartInstance, uartClkHz, uartConfig.baudRate);
                  /*Go to VLPR*/
                  enter_vlpr();   /* get into VLPR */
                  if (smc_hal_get_power_mode_stat() == kStatVlpr){
                      printf("  in VLPR Mode !\n\r ");
                  } else if (smc_hal_get_power_mode_stat() == kStatRun){
                      printf("  in Run Mode  !\n\r ");
                      clock_set_cme0(true);
                  }

                  break;

              case 0x13:/*VLPR in BLPI 2 MHZ */
                  printf("Press any key to enter VLPR\n\r ");
                  uart_receive_data(&uartState, &testVal, 1, kSyncWaitForever);
#if defined(CPU_MKL25Z128VLK4)
                  /*Maximum clock frequency for this mode is core 4MHz and Flash 1Mhz*/
                  printf("Configure clock frequency to 2MHz core clock and 1MHz flash clock\n\r ");
#elif (defined(CPU_MK22FN512VDC12)||defined(CPU_MK64FN1M0VMD12))
                  /*Maximum clock frequency for this mode is core 4MHz and Flash 1Mhz*/
                  printf("Configure clock frequency to 2MHz core clock and 800KHz flash clock\n\r ");
#endif
                  /*Get out of VLPR to change clock dividers*/
                  exit_vlpr();  /* get out of VLPR back to RUN */
                  vlp_clock_config(BLPI);
#if defined(CPU_MKL25Z128VLK4)
                  /*
		   * default clock divider is set to divide by 1 of Fast IRC
		   * and Div by 2 for Bus/Flash
		   */
                  clock_hal_set_clock_out_dividers(1,0,0,2);
#elif (defined(CPU_MK22FN512VDC12)||defined(CPU_MK64FN1M0VMD12))
                  /*
				   * default clock divider is set to divide by 1 of Fast IRC
				   * and Div by 4 for Bus/Flash
				   */
                  clock_hal_set_clock_out_dividers(1,1,1,3);
#endif
                  fcrDivVal = (1 << clock_get_fcrdiv());	/* calculate the fast IRC divder factor */
                  mcgClkHz =  (fastIrcFreq / fcrDivVal); /* MCGOUT frequency equals fast IRC frequency divided by 2 */

                  clock_hal_get_clock_divider(kSimClockDividerOutdiv1, &settings);
                  mcgClkHz =  mcgClkHz / (settings + 1);

                  uartClkHz = mcgClkHz; /* UART clock frequency will equal system frequency */
                  uart_hal_set_baud_rate(s_uartInstance, uartClkHz, uartConfig.baudRate);
                  /*Go to VLPR*/
                  enter_vlpr();   /* get into VLPR  */
                  if (smc_hal_get_power_mode_stat() == kStatVlpr){
                      printf("  in VLPR Mode !\n\r ");
                  } else if (smc_hal_get_power_mode_stat() == kStatRun){
                      printf("  in Run Mode  !\n\r ");
                      clock_set_cme0(true);
                  }
                  break;

              case 0x15:  /* enable debug mode */
                  printf("\n\r*--------------D E B U G    E N A B L E D--------------------*\n\r ");
                  SWD_CLK_ENABLE;
                  SWD_DIO_ENABLE;
                  break;

              default:
                  break;
            }

            testNum = UNDEF_VALUE ;

	}
}

/*!
 * @brief switches from PEE to PBE mode
 * @param crystalVal - crystal value
 */
int32_t pee_pbe(int32_t crystalVal)
{
    int16_t i;

    /* Check MCG is in PEE mode */
    if (!((clock_get_clkst() == kMcgClkstPll) &&        /* check CLKS mux has selcted PLL output */
          (clock_get_irefst() != kMcgIrefstInt) &&	/* check FLL ref is external ref clk */
          (clock_get_pllst() == kMcgPllstPllcs)))	/* check PLLS mux has selected PLL */
    {
      return 0x8;                                                       /* return error code */
    }

    /*
     * As we are running from the PLL by default the PLL and external clock settings are valid
     * To move to PBE from PEE simply requires the switching of the CLKS mux to select the ext clock
      */
    /* As CLKS is already 0 the CLKS value can simply be OR'ed into the register  */
    clock_set_clks(kMcgClockSelectExt);	/* switch CLKS mux to select external reference clock as MCG_OUT */


    /* Wait for clock status bits to update */
    for (i = 0 ; i < 2000 ; i++)
    {
        if (clock_get_clkst() == kMcgClkstEref)
        {
            break; /* jump out early if CLKST shows EXT CLK slected before loop finishes */
        }
    }
    if (clock_get_clkst() != kMcgClkstEref)
    {
        return 0x1A; /* check EXT CLK is really selected and return with error if not */
    }

    /* Now in PBE mode */
    return crystalVal; /* MCGOUT frequency equals external clock frequency */
} /* pee_pbe */

/*!
 * @brief switches from PBE to PEE mode
 * @param crystalVal - crystal value
 */
int32_t pbe_pee(int32_t crystalVal)
{
    uint8_t prDiv, vDiv;
    int16_t i;

    /* Check MCG is in PBE mode */
    if (!((clock_get_clkst() == kMcgClkstEref) &&       /* check CLKS mux has selcted external reference */
          (clock_get_irefst() != kMcgIrefstInt) &&	/* check FLL ref is external ref clk */
          (clock_get_pllst() == kMcgPllstPllcs) &&	/* check PLLS mux has selected PLL */
          (clock_get_lp() != kMcgLpSelectLowPower)))	/* check MCG_C2[LP] bit is not set */
    {
      return 0x7;                                                       /* return error code */
    }

    /* As the PLL settings have already been checked when PBE mode was enterred they are not checked here */

    /* Check the PLL state before transitioning to PEE mode */

    /* Check LOCK bit is set before transitioning MCG to PLL output (already checked in fbe_pbe but good practice */
    /* to re-check before switch to use PLL) */
    for (i = 0 ; i < 2000 ; i++)
    {
        if (clock_get_lock0())
        {
            break; /* jump out early if LOCK sets before loop finishes */
        }
    }
    if (!clock_get_lock0())
    {
      return 0x44; /* check bit is really set and return with error if not set */
    }
    /* Use actual PLL settings to calculate PLL frequency */
    prDiv = ((MCG_C5 & MCG_C5_PRDIV0_MASK) + 1);
    vDiv = ((MCG_C6 & MCG_C6_VDIV0_MASK) + 24);

    clock_set_clks(kMcgClockSelectOut);	/* clear CLKS to switch CLKS mux to select PLL as MCG_OUT */

    /* Wait for clock status bits to update */
    for (i = 0 ; i < 2000 ; i++)
    {
        if (clock_get_clkst() == kMcgClkstPll)
        {
            break; /* jump out early if CLKST = 3 before loop finishes */
        }
    }
    if (clock_get_clkst() != kMcgClkstPll)
    {
        return 0x1B; /* check CLKST is set correctly and return with error if not */
    }

    /* Now in PEE */
    return ((crystalVal / prDiv) * vDiv); /*MCGOUT equals PLL output frequency */

}  /* pbe_pee */


/*!
 * @brief switches from PBE to FBE mode
 * @param crystalVal - crystal value
 */
int32_t pbe_fbe(int32_t crystalVal)
{
    int16_t i;

    /* Check MCG is in PBE mode */
    if (!((clock_get_clkst() == kMcgClkstEref) &&       /* check CLKS mux has selcted external reference */
          (clock_get_irefst() != kMcgIrefstInt) &&	/* check FLL ref is external ref clk */
          (clock_get_pllst() == kMcgPllstPllcs) &&	/* check PLLS mux has selected PLL */
          (clock_get_lp() != kMcgLpSelectLowPower)))	/* check MCG_C2[LP] bit is not set */
    {
        return 0x7;                                                       /* return error code */
    }

    /*
     * As we are running from the ext clock, by default the external clock settings are valid
     * To move to FBE from PBE simply requires the switching of the PLLS mux to disable the PLL
     */

    clock_set_plls(kMcgPllSelectFll);	/* clear PLLS to disable PLL, still clocked from ext ref clk */

    /* wait for PLLST status bit to set */
    for (i = 0 ; i < 2000 ; i++)
    {
        if (!clock_get_pllst())
        {
            break; /* jump out early if PLLST clears before loop finishes */
        }
    }
    if (clock_get_pllst())
    {
        return 0x15; /* check bit is really clear and return with error if not clear */
    }

    /* Now in FBE mode */
    return crystalVal; /* MCGOUT frequency equals external clock frequency */
} /* pbe_fbe */

/*!
 * @brief switches from FBE to PBE mode
 * This function transitions the MCG from FBE mode to PBE mode.
 * This function presently only supports OSC0 and PLL0. Support for OSC1 and PLL1 will be added soon
 * The function requires the desired OSC and PLL be passed in to it for compatibility with the
 * future support of OSC/PLL selection
 *
 * @param crystalVal - external clock frequency in Hz
 * @param prDivVal - value to divide the external clock source by to create the desired
 *                           PLL reference clock frequency
 * @param vDivVal - value to multiply the PLL reference clock frequency by
 * Return value : MCGCLKOUT frequency (Hz) or error code
 */
int32_t fbe_pbe(int32_t crystalVal, int8_t prDivVal, int8_t vDivVal)
{
    int16_t i;
    int32_t pllFreq;

    /* Check MCG is in FBE mode */
    if (!((clock_get_clkst() == kMcgClkstEref) &&       /* check CLKS mux has selcted external reference */
          (clock_get_irefst() != kMcgIrefstInt) &&	/* check FLL ref is external ref clk */
          (clock_get_pllst() != kMcgPllstPllcs) &&	/* check PLLS mux has selected FLL */
          (clock_get_lp() != kMcgLpSelectLowPower)))	/* check MCG_C2[LP] bit is not set */
    {
        return 0x4;                                                       /* return error code */
    }

    /* As the external frequency has already been checked when FBE mode was enterred it is not checked here */

    /* Check PLL divider settings are within spec. */
    if ((prDivVal < 1) || (prDivVal > 25)) {return 0x41;}
    if ((vDivVal < 24) || (vDivVal > 50)) {return 0x42;}

    /* Check PLL reference clock frequency is within spec. */
    if (((crystalVal / prDivVal) < 2000000) || ((crystalVal / prDivVal) > 4000000)) {return 0x43;}

    /* Check PLL output frequency is within spec. */
    pllFreq = (crystalVal / prDivVal) * vDivVal;
    if ((pllFreq < 48000000) || (pllFreq > 100000000)) {return 0x45;}

    /* Configure MCG_C5 */
    /* If the PLL is to run in STOP mode then the PLLSTEN bit needs to be OR'ed in here or in user code. */
    clock_set_prdiv0(prDivVal - 1);	/*set PLL ref divider */

    /* Configure MCG_C6 */
    /*
     * The PLLS bit is set to enable the PLL, MCGOUT still sourced from ext ref clk
     * The clock monitor is not enabled here as it has likely been enabled previously and so the value of CME
     * is not altered here.
     *
     * The loss of lock interrupt can be enabled by seperately OR'ing in the LOLIE bit in MCG_C6
     */
	/* write new VDIV and enable PLL */
    clock_set_vdiv0(vDivVal - 24);
    clock_set_plls(kMcgPllSelectPllcs);

    /* wait for PLLST status bit to set */
    for (i = 0 ; i < 2000 ; i++)
    {
      if (clock_get_pllst())
      {
        break; /* jump out early if PLLST sets before loop finishes */
      }
    }
    if (!clock_get_pllst())
    {
      return 0x16; /* check bit is really set and return with error if not set */
    }

    /* Wait for LOCK bit to set */
    for (i = 0 ; i < 2000 ; i++)
    {
        if (clock_get_lock0())
        {
            break; /* jump out early if LOCK sets before loop finishes */
        }
    }
    if (!clock_get_lock0())
    {
        return 0x44; /* check bit is really set and return with error if not set */
    }

    /* now in PBE */
    return crystalVal; /* MCGOUT frequency equals external clock frequency */
} /* fbe_pbe */

/*!
 * @brief switches from PBE to BLPE mode
 * @param crystalVal - crystal value
 */
int32_t pbe_blpe(int32_t crystalVal)
{
    /* Check MCG is in PBE mode */
    if (!((clock_get_clkst() == kMcgClkstEref) &&       /* check CLKS mux has selcted external reference */
          (clock_get_irefst() != kMcgIrefstInt) &&	/* check FLL ref is external ref clk */
          (clock_get_pllst() == kMcgPllstPllcs) &&	/* check PLLS mux has selected PLL */
          (clock_get_lp() != kMcgLpSelectLowPower)))	/* check MCG_C2[LP] bit is not set */
    {
        return 0x7;                                                       /* return error code */
    }

    /* To enter BLPE mode the LP bit must be set, disabling the PLL */
    clock_set_lp(kMcgLpSelectLowPower);


    /* Now in BLPE mode */
    return crystalVal;
} /* pbe_blpe */

/*!
 * @brief switches from BLPE to PBE mode
 * Since PBE mode can be enterred via FBE -> BLPE modes, it cannot be assumed that the PLL has been
 * previously configured correctly. That is why this general purpose driver has the PLL settings as
 * passed parameters.
 * @param crystalVal - external clock frequency in Hz
 * @param prDivVal - value to divide the external clock source by to create the desired
 *                           PLL reference clock frequency
 * @param vDivVal - value to multiply the PLL reference clock frequency by
 * Return value : MCGCLKOUT frequency (Hz) or error code
 */
int32_t blpe_pbe(int32_t crystalVal, int8_t prDivVal, int8_t vDivVal)
{
    int16_t i;

    /* Check MCG is in BLPE mode */
    if (!((clock_get_clkst() == kMcgClkstEref) && 	/* check CLKS mux has selcted external reference */
          (clock_get_irefst() != kMcgIrefstInt) &&	/* check FLL ref is external ref clk */
          (clock_get_lp() == kMcgLpSelectLowPower)))	/* check MCG_C2[LP] bit is set */
    {
        return 0x6;                                                       /* return error code */
    }

    /* As the external frequency has already been checked when FBE mode was enterred it is not checked here */

    /* Check PLL divider settings are within spec. */
    if ((prDivVal < 1) || (prDivVal > 25)) {return 0x41;}
    if ((vDivVal < 24) || (vDivVal > 50)) {return 0x42;}

    /* Check PLL reference clock frequency is within spec. */
    if (((crystalVal / prDivVal) < 2000000) || ((crystalVal / prDivVal) > 4000000)) {return 0x43;}

    /* If PRDIV, VDIV and the PLL ref clock are in spec. then the PLL frequency is within spec. */

    /* Configure MCG_C5 */
    /* If the PLL is to run in STOP mode then the PLLSTEN bit needs to be OR'ed in here or in user code. */
    clock_set_prdiv0(prDivVal - 1);	/*set PLL ref divider */

    /* Configure MCG_C6 */
    /*
     * The PLLS bit is set to enable the PLL, MCGOUT still sourced from ext ref clk
     * The clock monitor is not enabled here as it has likely been enabled previously and so the value of CME
     * is not altered here.
     */
    /* The loss of lock interrupt can be enabled by seperately OR'ing in the LOLIE bit in MCG_C6 */
	/* write new VDIV and enable PLL */
	clock_set_vdiv0(vDivVal - 24);
    clock_set_plls(kMcgPllSelectPllcs);

    /* Now that PLL is configured, LP is cleared to enable the PLL */
    clock_set_lp(kMcgLpSelectNormal);

    /* wait for PLLST status bit to set */
    for (i = 0 ; i < 2000 ; i++)
    {
        if (clock_get_pllst())
        {
            break; /* jump out early if PLLST sets before loop finishes */
        }
    }
    if (!clock_get_pllst())
    {
        return 0x16; /* check bit is really set and return with error if not set */
    }

    /* Wait for LOCK bit to set */
    for (i = 0 ; i < 2000 ; i++)
    {
        if (clock_get_lock0())
        {
            break; /* jump out early if LOCK sets before loop finishes */
        }
    }
    if (!clock_get_lock0())
      {
          return 0x44; /* check bit is really set and return with error if not set */
      }

    /* now in PBE */
    return crystalVal; /* MCGOUT frequency equals external clock frequency */
} /* blpe_pbe */

/*!
 * @brief switches from BLPE to FBE mode
 * @param crystalVal - external clock frequency in Hz
 */
int32_t blpe_fbe(int32_t crystalVal)
{
    int16_t i;

    /* Check MCG is in BLPE mode */
    if (!((clock_get_clkst() == kMcgClkstEref) &&       /* check CLKS mux has selcted external reference */
          (clock_get_irefst() != kMcgIrefstInt) &&	/* check FLL ref is external ref clk */
          (clock_get_lp() == kMcgLpSelectLowPower)))	/* check MCG_C2[LP] bit is set */
    {
        return 0x6;                                                       /* return error code */
    }

    /* To move from BLPE to FBE the PLLS mux be set to select the FLL output and the LP bit must be cleared */
    clock_set_plls(kMcgPllSelectFll);	/* clear PLLS to select the FLL */
    clock_set_lp(kMcgLpSelectNormal);	/* clear LP bit */

    /* wait for PLLST status bit to clear */
    for (i = 0 ; i < 2000 ; i++)
    {
        if (!clock_get_pllst())
        {
            break; /* jump out early if PLLST clears before loop finishes */
        }
    }
    if (clock_get_pllst())
    {
        return 0x15; /* check bit is really clear and return with error if not clear  */
    }

    /* now in FBE mode */
    return crystalVal; /* MCGOUT frequency equals external clock frequency */
} /* blpe_fbe */

/*!
 * @brief switches from FBE to PLPE mode
 * @param crystalVal - external clock frequency in Hz
 */
int32_t fbe_blpe(int32_t crystalVal)
{
    /* Check MCG is in FBE mode */
    if (!((clock_get_clkst() == kMcgClkstEref) && 	/* check CLKS mux has selcted external reference */
          (clock_get_irefst() != kMcgIrefstInt) &&	/* check FLL ref is external ref clk */
          (clock_get_pllst() != kMcgPllstPllcs) &&	/* check PLLS mux has selected FLL */
          (clock_get_lp() != kMcgLpSelectLowPower)))	/* check MCG_C2[LP] bit is not set */
    {
        return 0x4;                                                       /* return error code */
    }

    /* To move from FBE to BLPE the LP bit must be set */
    clock_set_lp(kMcgLpSelectLowPower);	/* set LP bit */

    /* now in FBE mode */
    return crystalVal; /* MCGOUT frequency equals external clock frequency */
} /* fbe_blpe */

/*!
 * @brief switches from FEI to FBE mode
 *
 * Mode transition: FEI to FBE mode
 *
 * This function transitions the MCG from FEI mode to FBE mode. This is
 * achieved by setting the MCG_C2[LP] bit. There is no status bit to
 * check so 0 is always returned if the function was called with the MCG
 * in FBI mode. The MCGCLKOUT frequency does not change
 *
 * @param crystalVal - external clock frequency in Hz
 * @param hgoVal - selects whether low power or high gain mode is selected
 *                           for the crystal oscillator. This has no meaning if an
 *                           external clock is used.
 * @param erefsVal - selects external clock (=kMcgErefClockSelectExt) or
 *                                    crystal osc (=kMcgErefClockSelectOsc)
 * Return value : MCGCLKOUT frequency (Hz) or error code
 */
int32_t fei_fbe(int32_t crystalVal, mcg_hgo_select_t hgoVal, mcg_eref_clock_select_t erefsVal)
{
    uint8_t frDivVal;
    int16_t i;

    /* check if in FEI mode */
    if (!((clock_get_clkst() == kMcgClkstFll) &&        /* check CLKS mux has selcted FLL output */
          (clock_get_irefst() == kMcgIrefstInt) &&	/* check FLL ref is internal ref clk */
          (clock_get_pllst() != kMcgPllstPllcs)))	/* check PLLS mux has selected FLL */
    {
        return 0x1;                                                     /* return error code */
    }

    /* check external frequency is less than the maximum frequency */
    if  (crystalVal > 50000000) {return 0x21;}

    /* check crystal frequency is within spec. if crystal osc is being used */
    if (erefsVal)
    {
        if ((crystalVal < 30000) ||
            ((crystalVal > 40000) && (crystalVal < 3000000)) ||
            (crystalVal > 32000000)) {return 0x22;} /* return error if one of the available crystal options is not available */
    }

    /* make sure HGO will never be greater than 1. Could return an error instead if desired. */
    if (hgoVal > 0)
    {
        hgoVal = kMcgHgoSelectHigh; /* force hgo_val to 1 if > 0 */
    }

    /* configure the MCG_C2 register */
    /* the RANGE value is determined by the external frequency. Since the RANGE parameter affects the FRDIV divide value */
    /* it still needs to be set correctly even if the oscillator is not being used */
    if (crystalVal <= 40000)
    {
        clock_set_range0(kMcgFreqRangeSelectLow);
        clock_set_hgo0(hgoVal);
        clock_set_erefs0(erefsVal);
    }
    else if (crystalVal <= 8000000)
    {
        clock_set_range0(kMcgFreqRangeSelectHigh);
        clock_set_hgo0(hgoVal);
        clock_set_erefs0(erefsVal);
    }
    else
    {
        clock_set_range0(kMcgFreqRangeSelectVeryHigh);
        clock_set_hgo0(hgoVal);
        clock_set_erefs0(erefsVal);
    }


    /* determine FRDIV based on reference clock frequency */
    /* since the external frequency has already been checked only the maximum frequency for each FRDIV value needs to be compared here. */
    if (crystalVal <= 1250000) {frDivVal = 0;}
    else if (crystalVal <= 2500000) {frDivVal = 1;}
    else if (crystalVal <= 5000000) {frDivVal = 2;}
    else if (crystalVal <= 10000000) {frDivVal = 3;}
    else if (crystalVal <= 20000000) {frDivVal = 4;}
    else {frDivVal = 5;}

    /*
     * Select external oscilator and Reference Divider and clear IREFS to start ext osc
     * If IRCLK is required it must be enabled outside of this driver, existing state will be maintained
     * CLKS=2, FRDIV=frdiv_val, IREFS=0, IRCLKEN=0, IREFSTEN=0
     */
	/* Set the required CLKS and FRDIV values */
    clock_set_clks_frdiv_irefs(kMcgClockSelectExt, frDivVal, kMcgIrefClockSourceExt);

    /* if the external oscillator is used need to wait for OSCINIT to set */
    if (erefsVal)
    {
        for (i = 0 ; i < 10000 ; i++)
        {
            if (clock_get_oscinit0())
            {
                break; /* jump out early if OSCINIT sets before loop finishes */
            }
        }
        if (!clock_get_oscinit0())
        {
            return 0x23; /* check bit is really set and return with error if not set */
        }
    }

    /* wait for Reference clock Status bit to clear */
    for (i = 0 ; i < 2000 ; i++)
    {
        if (!clock_get_irefst())
        {
            break; /* jump out early if IREFST clears before loop finishes */
        }
    }
    if (clock_get_irefst())
    {
        return 0x11; /* check bit is really clear and return with error if not set */
    }

    /* Wait for clock status bits to show clock source is ext ref clk */
    for (i = 0 ; i < 2000 ; i++)
    {
        if (clock_get_clkst() == kMcgClkstEref)
        {
            break; /* jump out early if CLKST shows EXT CLK slected before loop finishes */
        }
    }
    if (clock_get_clkst() != kMcgClkstEref)
    {
        return 0x1A; /* check EXT CLK is really selected and return with error if not */
    }

    /*
     * Now in FBE
     * It is recommended that the clock monitor is enabled when using an external clock as the clock source/reference.
     * It is enabled here but can be removed if this is not required.
     */
    clock_set_cme0(true);


    return crystalVal; /* MCGOUT frequency equals external clock frequency */
} /* fei_fbe */

/*!
 * @brief switches from FEE to FBE mode
 * @param crystalVal - external clock frequency in Hz
 */
int32_t fee_fbe(int32_t crystalVal)
{
    int16_t i;

    /* Check MCG is in FEE mode */
    if (!((clock_get_clkst() == kMcgClkstFll) &&        /* check CLKS mux has selcted FLL */
          (clock_get_irefst() != kMcgIrefstInt) &&	/* check FLL ref is external ref clk */
          (clock_get_pllst() != kMcgPllstPllcs)))	/* check PLLS mux has selected FLL */
    {
        return 0x2;                                                       /* return error code */
    }

    /* Set CLKS field to 2 to switch CLKS mux to select ext ref clock */
    /* MCG is current in FEE mode so CLKS field = 0 so can just OR in new value */
    clock_set_clks(kMcgClockSelectExt); /* set CLKS to select ext ref clock */

    /* Wait for clock status bits to show clock source is ext ref clk */
    for (i = 0 ; i < 2000 ; i++)
    {
        if (clock_get_clkst() == kMcgClkstEref)
        {
            break; /* jump out early if CLKST shows EXT CLK slected before loop finishes */
        }
    }
    if (clock_get_clkst() != kMcgClkstEref)
    {
        return 0x1A; /* check EXT CLK is really selected and return with error if not */
    }

    /* Now in FBE mode */
    return crystalVal;
} /* fee_fbe */

/*!
 * @brief switches from FBE to PLPE mode
 * @param ircFreq - IRC clock frequency
 * @param ircSelect - 0 if slow irc, 1 if fast irc
 */
int32_t fbe_fbi(int32_t ircFreq, uint8_t ircSelect)
{
    uint8_t fcrDivVal;
    int16_t i;

    /* Check MCG is in FBE mode */
    if (!((clock_get_clkst() == kMcgClkstEref) && 	/* check CLKS mux has selcted external reference */
          (clock_get_irefst() != kMcgIrefstInt) &&	/* check FLL ref is external ref clk */
          (clock_get_pllst() != kMcgPllstPllcs) &&	/* check PLLS mux has selected FLL */
          (clock_get_lp() != kMcgLpSelectLowPower)))/* check MCG_C2[LP] bit is not set */
    {
        return 0x4;                                                       /* return error code */
    }

    /* Check that the irc frequency matches the selected IRC */
    if (!(ircSelect))
    {
        if ((ircFreq < 31250) || (ircFreq > 39063)) {return 0x31;}
    }
    else
    {
        if ((ircFreq < 3000000) || (ircFreq > 5000000)) {return 0x32;} /* Fast IRC freq */
    }

    /* Select the required IRC */
    if (ircSelect)
    {
        clock_set_ircs(kMcgIrefClockSelectFast); /* select fast IRC by setting IRCS */
    }
    else
    {
        clock_set_ircs(kMcgIrefClockSelectSlow); /* select slow IRC by clearing IRCS */
    }

    /* Make sure the clock monitor is disabled before switching modes otherwise it will trigger */
    clock_set_cme0(false);

    /* Select the IRC as the CLKS mux selection */
	/* select IRC as MCGOUT and enable IREFS */
    clock_set_clks_frdiv_irefs(kMcgClockSelectIn, clock_get_frdiv(), kMcgIrefClockSourceSlow);

    /* wait until internal reference switches to requested irc. */
    if (!(ircSelect))
    {
        for (i = 0 ; i < 2000 ; i++)
        {
          if (!(MCG_S & MCG_S_IRCST_MASK))
          {
              break; /* jump out early if IRCST clears before loop finishes */
          }
      }
      if (MCG_S & MCG_S_IRCST_MASK)
      {
          return 0x13; /* check bit is really clear and return with error if set */
      }
    }
    else
    {
        for (i = 0 ; i < 2000 ; i++)
        {
            if (MCG_S & MCG_S_IRCST_MASK)
            {
                break; /* jump out early if IRCST sets before loop finishes */
            }
        }
        if (!(MCG_S & MCG_S_IRCST_MASK))
        {
            return 0x14; /* check bit is really set and return with error if not set */
        }
    }

    /* Wait for clock status bits to update */
    for (i = 0 ; i < 2000 ; i++)
    {
        if (clock_get_clkst() == kMcgClkstIref)
        {
            break; /* jump out early if CLKST shows IRC slected before loop finishes */
        }
    }
    if (clock_get_clkst() != kMcgClkstIref)
    {
        return 0x19; /* check IRC is really selected and return with error if not */
    }

    /* wait for Reference clock Status bit to set */
    for (i = 0 ; i < 2000 ; i++)
    {
        if (clock_get_irefst())
        {
            break; /* jump out early if IREFST sets before loop finishes */
        }
    }
    if (!clock_get_irefst())
    {
        return 0x12; /* check bit is really set and return with error if not set */
    }

    /* Now in FBI mode */

    if (ircSelect)
    {
        fcrDivVal = (1 << clock_get_fcrdiv()); /* calculate the fast IRC divder factor */
        return (ircFreq / fcrDivVal); /* MCGOUT frequency equals fast IRC frequency divided by FCRDIV factor */
    }
    else
    {
        return ircFreq; /* MCGOUT frequency equals slow IRC frequency */
    }
} /*fbe_fbi*/

/*!
 * @brief switches from FBI to FBE mode
 *
 * @param crystalVal - external clock frequency in Hz
 * @param hgoVal - selects whether low power or high gain mode is selected
 *                           for the crystal oscillator. This has no meaning if an
 *                           external clock is used.
 * @param erefsVal - selects external clock (=kMcgErefClockSelectExt) or
 *                                    crystal osc (=kMcgErefClockSelectOsc)
 * Return value : MCGCLKOUT frequency (Hz) or error code
 */
int32_t fbi_fbe(int32_t crystalVal, mcg_hgo_select_t hgoVal, mcg_eref_clock_select_t erefsVal)
{
    uint8_t frDivVal;
    int16_t i;

    /* check if in FBI mode */
    if (!((clock_get_clkst() == kMcgClkstIref) && 	/* check CLKS mux has selcted int ref clk */
          (clock_get_irefst() == kMcgIrefstInt) &&	/* check FLL ref is internal ref clk */
          (clock_get_pllst() != kMcgPllstPllcs) &&	/* check PLLS mux has selected FLL */
          (clock_get_lp() != kMcgLpSelectLowPower)))/* check LP bit is clear */
    {
        return 0x3;                                                       /* MCG not in correct mode return fail code */
    }

    /* check external frequency is less than the maximum frequency */
    if  (crystalVal > 50000000) {return 0x21;}

    /* check crystal frequency is within spec. if crystal osc is being used */
    if (erefsVal)
    {
        if ((crystalVal < 30000) ||
          ((crystalVal > 40000) && (crystalVal < 3000000)) ||
          (crystalVal > 32000000)) {return 0x22;} /* return error if one of the available crystal options is not available */
    }

    /* make sure HGO will never be greater than 1. Could return an error instead if desired. */
    if (hgoVal > 0)
    {
        hgoVal = kMcgHgoSelectHigh; /* force hgo_val to 1 if > 0 */
    }

    /* configure the MCG_C2 register */
    /* the RANGE value is determined by the external frequency. Since the RANGE parameter affects the FRDIV divide value */
    /* it still needs to be set correctly even if the oscillator is not being used */
    if (crystalVal <= 40000)
    {
        clock_set_range0(kMcgFreqRangeSelectLow);
        clock_set_hgo0(hgoVal);
        clock_set_erefs0(erefsVal);
    }
    else if (crystalVal <= 8000000)
    {
        clock_set_range0(kMcgFreqRangeSelectHigh);
        clock_set_hgo0(hgoVal);
        clock_set_erefs0(erefsVal);
    }
    else
    {
        clock_set_range0(kMcgFreqRangeSelectVeryHigh);
        clock_set_hgo0(hgoVal);
        clock_set_erefs0(erefsVal);
    }

    /* determine FRDIV based on reference clock frequency */
    /* since the external frequency has already been checked only the maximum frequency for each FRDIV value needs to be compared here. */
    if (crystalVal <= 1250000) {frDivVal = 0;}
    else if (crystalVal <= 2500000) {frDivVal = 1;}
    else if (crystalVal <= 5000000) {frDivVal = 2;}
    else if (crystalVal <= 10000000) {frDivVal = 3;}
    else if (crystalVal <= 20000000) {frDivVal = 4;}
    else {frDivVal = 5;}

    /*
     * Select external oscilator and Reference Divider and clear IREFS to start ext osc
     * If IRCLK is required it must be enabled outside of this driver, existing state will be maintained
     * CLKS=2, FRDIV=frdiv_val, IREFS=0, IRCLKEN=0, IREFSTEN=0
     */
	/* Set the required CLKS and FRDIV values */
    clock_set_clks_frdiv_irefs(kMcgClockSelectExt, frDivVal, kMcgIrefClockSourceExt);

    /* if the external oscillator is used need to wait for OSCINIT to set */
    if (erefsVal)
    {
        for (i = 0 ; i < 10000 ; i++)
        {
            if (clock_get_oscinit0())
            {
                break; /* jump out early if OSCINIT sets before loop finishes */
            }
        }
        if (!clock_get_oscinit0())
        {
             return 0x23; /* check bit is really set and return with error if not set */
        }
    }

    /* wait for Reference clock Status bit to clear */
    for (i = 0 ; i < 2000 ; i++)
    {
        if (!clock_get_irefst())
        {
            break; /* jump out early if IREFST clears before loop finishes */
        }
    }
    if (clock_get_irefst())
    {
        return 0x11; /* check bit is really clear and return with error if not set */
    }

    /* Wait for clock status bits to show clock source is ext ref clk */
    for (i = 0 ; i < 2000 ; i++)
    {
        if (clock_get_clkst() == kMcgClkstEref)
        {
            break; /* jump out early if CLKST shows EXT CLK slected before loop finishes */
        }
    }
    if (clock_get_clkst() != kMcgClkstEref)
    {
        return 0x1A; /* check EXT CLK is really selected and return with error if not */
    }

    /*
     * Now in FBE
     * It is recommended that the clock monitor is enabled when using an external clock as the clock source/reference.
     *
     * It is enabled here but can be removed if this is not required.
     */
    clock_set_cme0(true);

    return crystalVal; /* MCGOUT frequency equals external clock frequency */
} /* fbi_fbe*/

/*!
 * @brief switches from FBI to BLPI mode
 * This function transitions the MCG from FBI mode to BLPI mode. This is
 * achieved by setting the MCG_C2[LP] bit. There is no status bit to
 * check so 0 is always returned if the function was called with the MCG
 * in FBI mode.
 *
 * @param ircFreq - IRC clock frequency
 * @param ircSelect - 0 if slow irc, 1 if fast irc
 * Return value : MCGOUT frequency or error code
 */
int32_t fbi_blpi(int32_t ircFreq, uint8_t ircSelect)
{
    uint8_t fcrDivVal;

    /* check if in FBI mode */
    if (!((clock_get_clkst() == kMcgClkstIref) && 	/* check CLKS mux has selcted int ref clk */
          (clock_get_irefst() == kMcgIrefstInt) &&	/* check FLL ref is internal ref clk */
          (clock_get_pllst() != kMcgPllstPllcs) &&	/* check PLLS mux has selected FLL */
          (clock_get_lp() != kMcgLpSelectLowPower)))/* check LP bit is clear */
    {
        return 0x3;                                                       /* MCG not in correct mode return fail code */
    }

    /* Set LP bit to disable the FLL and enter BLPI */
    clock_set_lp(kMcgLpSelectLowPower);

    /* Now in BLPI */
    if (ircSelect)
    {
        fcrDivVal = (1 << clock_get_fcrdiv()); /* calculate the fast IRC divder factor */
        return (ircFreq / fcrDivVal); /* MCGOUT frequency equals fast IRC frequency divided by 2 */
    }
    else
    {
        return ircFreq; /* MCGOUT frequency equals slow IRC frequency */
    }
} /* fbi_blpi*/



/*!
 * @brief switches from BLPI to FBI mode
 *
 * This function transitions the MCG from BLPI mode to FBI mode. This is
 * achieved by clearing the MCG_C2[LP] bit. There is no status bit to
 * check so 0 is always returned if the function was called with the MCG
 * in BLPI mode.
 *
 * @param ircFreq - IRC clock frequency
 * @param ircSelect - 0 if slow irc, 1 if fast irc
 * Return value : MCGOUT frequency or error code
 */
int32_t blpi_fbi(int32_t ircFreq, uint8_t ircSelect)
{
    uint8_t fcrDivVal;
    /* check if in BLPI mode */
    if (!((clock_get_clkst() == kMcgClkstIref) && 	/* check CLKS mux has selcted int ref clk */
          (clock_get_irefst() == kMcgIrefstInt) &&	/* check FLL ref is internal ref clk */
          (clock_get_pllst() != kMcgPllstPllcs) &&	/* check PLLS mux has selected FLL */
          (clock_get_lp() == kMcgLpSelectLowPower)))/* check LP bit is set */
    {
        return 0x5;                                                       /* MCG not in correct mode return fail code */
    }

    /* Clear LP bit to enable the FLL and enter FBI mode */
    clock_set_lp(kMcgLpSelectNormal);

    /* Now in FBI mode */
    if (ircSelect)
    {
        fcrDivVal = (1 << clock_get_fcrdiv()); /* calculate the fast IRC divder factor */
        return (ircFreq / fcrDivVal); /* MCGOUT frequency equals fast IRC frequency divided by 2 */
    }
    else
    {
        return ircFreq; /* MCGOUT frequency equals slow IRC frequency */
    }
} /* blpi_fbi*/

/*!
 * @brief switches from FEE to FBI mode
 * @param ircFreq - IRC clock frequency
 * @param ircSelect - 0 if slow irc, 1 if fast irc
 * Return value : MCGOUT frequency or error code
 */
int32_t fee_fbi(int32_t ircFreq, uint8_t ircSelect)
{
    uint8_t fcrDivVal;
    int16_t i;

    /* Check MCG is in FEE mode */
    if (!((clock_get_clkst() == kMcgClkstFll) && 	/* check CLKS mux has selcted FLL output */
          (clock_get_irefst() != kMcgIrefstInt) &&	/* check FLL ref is external ref clk */
          (clock_get_pllst() != kMcgPllstPllcs)))	/* check PLLS mux has selected FLL */
    {
        return 0x2;                                                     /* return error code */
    }

    /* Check that the irc frequency matches the selected IRC */
    if (!(ircSelect))
    {
        if ((ircFreq < 31250) || (ircFreq > 39063)) {return 0x31;}
    }
    else
    {
        if ((ircFreq < 3000000) || (ircFreq > 5000000)) {return 0x32;} /* Fast IRC freq */
    }

    /* Select the required IRC */
    if (ircSelect)
    {
        clock_set_ircs(kMcgIrefClockSelectFast); /* select fast IRC by setting IRCS */
    }
    else
    {
        clock_set_ircs(kMcgIrefClockSelectSlow); /* select slow IRC by clearing IRCS */
    }

    /* Make sure the clock monitor is disabled before switching modes otherwise it will trigger */
    clock_set_cme0(false);

    /* Select the IRC as the CLKS mux selection */
	/* set IREFS and select IRC as MCGOUT */
    clock_set_clks_frdiv_irefs(kMcgClockSelectIn, clock_get_frdiv(),kMcgIrefClockSourceSlow);

    /* wait until internal reference switches to requested irc. */
    if (!(ircSelect))
    {
        for (i = 0 ; i < 2000 ; i++)
        {
            if (!(MCG_S & MCG_S_IRCST_MASK))
            {
                break; /* jump out early if IRCST clears before loop finishes */
            }
        }
      if (MCG_S & MCG_S_IRCST_MASK)
      {
          return 0x13; /* check bit is really clear and return with error if set */
      }
    }
    else
    {
        for (i = 0 ; i < 2000 ; i++)
        {
            if (MCG_S & MCG_S_IRCST_MASK)
            {
                break; /* jump out early if IRCST sets before loop finishes */
            }
        }
        if (!(MCG_S & MCG_S_IRCST_MASK))
        {
            return 0x14; /* check bit is really set and return with error if not set */
        }
    }

    /* Wait for clock status bits to update */
    for (i = 0 ; i < 2000 ; i++)
    {
        if (clock_get_clkst() == kMcgClkstIref)
        {
            break; /* jump out early if CLKST shows IRC slected before loop finishes */
        }
    }
    if (clock_get_clkst() != kMcgClkstIref)
    {
        return 0x19; /* check IRC is really selected and return with error if not */
    }

    /* wait for Reference clock Status bit to set */
    for (i = 0 ; i < 2000 ; i++)
    {
        if (clock_get_irefst())
        {
            break; /* jump out early if IREFST sets before loop finishes */
        }
    }
    if (!clock_get_irefst())
    {
        return 0x12; /* check bit is really set and return with error if not set */
    }

    /* Now in FBI mode */
    if (ircSelect)
    {
        fcrDivVal = (1 << clock_get_fcrdiv()); /* calculate the fast IRC divder factor */
        return (ircFreq / fcrDivVal); /* MCGOUT frequency equals fast IRC frequency divided by 2 */
    }
    else
    {
        return ircFreq; /* MCGOUT frequency equals slow IRC frequency */
    }
} /* fee_fbi */

/*!
 * @brief switches from FBI to FEI mode
 * @param slowIrcFreqInner - slow IRD frequency
 */
int32_t fbi_fei(int32_t slowIrcFreqInner)
{
    int16_t i;
    int32_t mcgOut;

    /* check if in FBI mode */
    if (!((clock_get_clkst() == kMcgClkstIref) && 	/* check CLKS mux has selcted int ref clk */
          (clock_get_irefst() == kMcgIrefstInt) &&	/* check FLL ref is internal ref clk */
          (clock_get_pllst() != kMcgPllstPllcs) &&	/* check PLLS mux has selected FLL */
          (clock_get_lp() != kMcgLpSelectLowPower)))/* check LP bit is clear */
    {
        return 0x3;                                                       /* MCG not in correct mode return fail code */
    }

    /* Check IRC frequency is within spec. */
    if ((slowIrcFreqInner < 31250) || (slowIrcFreqInner > 39063))
    {
        return 0x31;
    }

    /* Check resulting FLL frequency */
    mcgOut = fll_freq(slowIrcFreqInner);
    if (mcgOut < 0x3C) {return mcgOut;} /* If error code returned, return the code to calling function */

    /* Change the CLKS mux to select the FLL output as MCGOUT */

	/* select FLL as MCGOUT */
    /* make sure IRC is FLL reference */
    clock_set_clks_frdiv_irefs(kMcgClockSelectOut, clock_get_frdiv(), kMcgIrefClockSourceSlow);

    /* wait for Reference clock Status bit to clear */
    for (i = 0 ; i < 2000 ; i++)
    {
        if (clock_get_irefst())
        {
            break; /* jump out early if IREFST clears before loop finishes */
        }
    }
    if (!clock_get_irefst())
    {
        return 0x12; /* check bit is really set and return with error if not set */
    }

    /* Wait for clock status bits to show clock source is ext ref clk */
    for (i = 0 ; i < 2000 ; i++)
    {
        if (clock_get_clkst() == kMcgClkstFll)
        {
            break; /* jump out early if CLKST shows FLL slected before loop finishes */
        }
    }
    if (clock_get_clkst() != kMcgClkstFll)
    {
        return 0x18; /* check FLL is really selected and return with error if not */
    }

    /* Now in FEI mode */
    return mcgOut;
} /* fbi_fei*/

/*!
 * @brief switches from FEI to FBI mode
 * @param ircFreq - IRC clock frequency
 * @param ircSelect - 0 if slow irc, 1 if fast irc
 */
int32_t fei_fbi(int32_t ircFreq, uint8_t ircSelect)
{
    uint8_t fcrDivVal;
    int16_t i;

    /* Check MCG is in FEI mode */
    if (!((clock_get_clkst() == kMcgClkstFll) && 	/* check CLKS mux has selcted FLL output */
          (clock_get_irefst() == kMcgIrefstInt) &&	/* check FLL ref is internal ref clk */
          (clock_get_pllst() != kMcgPllstPllcs)))	/* check PLLS mux has selected FLL */
    {
        return 0x1;                                                       /* return error code */
    }

    /* Check that the irc frequency matches the selected IRC  */
    if (!(ircSelect))
    {
        if ((ircFreq < 31250) || (ircFreq > 39063)) {return 0x31;}
    }
    else
    {
        if ((ircFreq < 3000000) || (ircFreq > 5000000)) {return 0x32;} /* Fast IRC freq */
    }

    /* Select the desired IRC */
    if (ircSelect)
    {
        clock_set_ircs(kMcgIrefClockSelectFast); /* select fast IRCS */
    }
    else
    {
        clock_set_ircs(kMcgIrefClockSelectSlow); /* select slow IRCS */
    }

    /* Change the CLKS mux to select the IRC as the MCGOUT */
    clock_set_clks(kMcgClockSelectIn); /* select IRC as the MCG clock sourse */

    /* wait until internal reference switches to requested irc. */
    if (!(ircSelect))
    {
        for (i = 0 ; i < 2000 ; i++)
        {
            if (!(MCG_S & MCG_S_IRCST_MASK))
            {
                break; /* jump out early if IRCST clears before loop finishes */
            }
        }
        if (MCG_S & MCG_S_IRCST_MASK)
        {
            return 0x13; /* check bit is really clear and return with error if set */
        }
    }
    else
    {
        for (i = 0 ; i < 2000 ; i++)
        {
            if (MCG_S & MCG_S_IRCST_MASK)
            {
                break; /* jump out early if IRCST sets before loop finishes */
            }
        }
        if (!(MCG_S & MCG_S_IRCST_MASK))
        {
            return 0x14; /* check bit is really set and return with error if not set */
        }
    }

    /* Wait for clock status bits to update */
    for (i = 0 ; i < 2000 ; i++)
    {
        if (clock_get_clkst() == kMcgClkstIref)
        {
            break; /* jump out early if CLKST shows IRC slected before loop finishes */
        }
    }
    if (clock_get_clkst() != kMcgClkstIref)
    {
        return 0x19; /* check IRC is really selected and return with error if not */
    }

    /* Now in FBI mode */
    if (ircSelect)
    {
        fcrDivVal = (1 << clock_get_fcrdiv()); /* calculate the fast IRC divder factor */
        return (ircFreq / fcrDivVal); /* MCGOUT frequency equals fast IRC frequency divided by 2 */
    }
    else
    {
        return ircFreq; /* MCGOUT frequency equals slow IRC frequency */
    }
} /* fei_fbi*/

/*!
 * @brief switches from FEI to FEE mode
 *
 * This function transitions the MCG from FEI mode to FEE mode. This is
 * achieved by setting the MCG_C2[LP] bit. There is no status bit to
 * check so 0 is always returned if the function was called with the MCG
 * in FBI mode. The MCGCLKOUT frequency does not change
 *
 * @param crystalVal - external clock frequency in Hz
 * @param hgoVal - selects whether low power or high gain mode is selected
 *                           for the crystal oscillator. This has no meaning if an
 *                           external clock is used.
 * @param erefsVal - selects external clock (=kMcgErefClockSelectExt) or
 *                                    crystal osc (=kMcgErefClockSelectOsc)
 * Return value : MCGCLKOUT frequency (Hz) or error code
 */
int32_t fei_fee(int32_t crystalVal, mcg_hgo_select_t hgoVal, mcg_eref_clock_select_t erefsVal)
{
    uint8_t frDivVal;
    int32_t mcgOut, fllRefFreq, i;

    /* check if in FEI mode */
    if (!((clock_get_clkst() == kMcgClkstFll) && 	/* check CLKS mux has selcted FLL output */
          (clock_get_irefst() == kMcgIrefstInt) &&	/* check FLL ref is internal ref clk */
          (clock_get_pllst() != kMcgPllstPllcs)))	/* check PLLS mux has selected FLL */
    {
        return 0x1;                                                     /* return error code */
    }

    /* check external frequency is less than the maximum frequency */
    if  (crystalVal > 50000000) {return 0x21;}

    /* check crystal frequency is within spec. if crystal osc is being used */
    if (erefsVal)
    {
        if ((crystalVal < 30000) ||
            ((crystalVal > 40000) && (crystalVal < 3000000)) ||
            (crystalVal > 32000000)) {return 0x22;} /* return error if one of the available crystal options is not available */
    }

    /* make sure HGO will never be greater than 1. Could return an error instead if desired. */
    if (hgoVal > 0)
    {
        hgoVal = kMcgHgoSelectHigh; /* force hgo_val to 1 if > 0 */
    }

    /*
     * configure the MCG_C2 register
     * the RANGE value is determined by the external frequency. Since the RANGE parameter affects the FRDIV divide value
     * it still needs to be set correctly even if the oscillator is not being used
     */
    if (crystalVal <= 40000)
    {
        clock_set_range0(kMcgFreqRangeSelectLow);
        clock_set_hgo0(hgoVal);
        clock_set_erefs0(erefsVal);
    }
    else if (crystalVal <= 8000000)
    {
        clock_set_range0(kMcgFreqRangeSelectHigh);
        clock_set_hgo0(hgoVal);
        clock_set_erefs0(erefsVal);
    }
    else
    {
        clock_set_range0(kMcgFreqRangeSelectVeryHigh);
        clock_set_hgo0(hgoVal);
        clock_set_erefs0(erefsVal);
    }

    /* determine FRDIV based on reference clock frequency */
    /* since the external frequency has already been checked only the maximum frequency for each FRDIV value needs to be compared here. */
    if (crystalVal <= 1250000) {frDivVal = 0;}
    else if (crystalVal <= 2500000) {frDivVal = 1;}
    else if (crystalVal <= 5000000) {frDivVal = 2;}
    else if (crystalVal <= 10000000) {frDivVal = 3;}
    else if (crystalVal <= 20000000) {frDivVal = 4;}
    else {frDivVal = 5;}

    /* The FLL ref clk divide value depends on FRDIV and the RANGE value */
    if (((MCG_C2 & MCG_C2_RANGE0_MASK) >> MCG_C2_RANGE0_SHIFT) > 0)
    {
        fllRefFreq = ((crystalVal) / (32 << frDivVal));
    }
    else
    {
        fllRefFreq = ((crystalVal) / (1 << frDivVal));
    }

    /* Check resulting FLL frequency  */
    mcgOut = fll_freq(fllRefFreq); /* FLL reference frequency calculated from ext ref freq and FRDIV */
    if (mcgOut < 0x3C) {return mcgOut;} /* If error code returned, return the code to calling function */

    /*
     * Select external oscilator and Reference Divider and clear IREFS to start ext osc
     * If IRCLK is required it must be enabled outside of this driver, existing state will be maintained
     * CLKS=0, FRDIV=frdiv_val, IREFS=0, IRCLKEN=0, IREFSTEN=0
     */
	/* Set the required CLKS and FRDIV values */
    clock_set_clks_frdiv_irefs(kMcgClockSelectOut, frDivVal, kMcgIrefClockSourceExt);

    /* if the external oscillator is used need to wait for OSCINIT to set */
    if (erefsVal)
    {
        for (i = 0 ; i < 20000000 ; i++)
        {
            if (clock_get_oscinit0())
            {
                break; /* jump out early if OSCINIT sets before loop finishes */
            }
      }
      if (!clock_get_oscinit0())
      {
          return 0x23; /* check bit is really set and return with error if not set */
      }
    }

    /* wait for Reference clock Status bit to clear */
    for (i = 0 ; i < 2000 ; i++)
    {
        if (!clock_get_irefst())
        {
          break; /* jump out early if IREFST clears before loop finishes */
        }
    }
    if (clock_get_irefst())
    {
        return 0x11; /* check bit is really clear and return with error if not set */
    }

    /*
     * Now in FBE
     * It is recommended that the clock monitor is enabled when using an external clock as the clock source/reference.
     * It is enabled here but can be removed if this is not required.
     */
    clock_set_cme0(true);

    return mcgOut; /* MCGOUT frequency equals FLL frequency */
} /* fei_fee*/

/*!
 * @brief switches from
 * @param slowIrcFreqInner - slow IRC frequency
 */
int32_t fee_fei(int32_t slowIrcFreqInner)
{
    int16_t i;
    uint32_t mcgOut;
    clock_manager_error_code_t clockFreqError;

    /* Check MCG is in FEE mode */
    if (!((clock_get_clkst() == kMcgClkstFll) && 	/* check CLKS mux has selcted FLL */
          (clock_get_irefst() != kMcgIrefstInt) &&	/* check FLL ref is external ref clk */
          (clock_get_pllst() != kMcgPllstPllcs)))	/* check PLLS mux has selected FLL */
    {
        return 0x2;                                                     /* return error code */
    }

    /* Check IRC frequency is within spec. */
    if ((slowIrcFreqInner < 31250) || (slowIrcFreqInner > 39063))
    {
        return 0x31;
    }

    /* Check resulting FLL frequency  */
    clockFreqError = clock_manager_get_frequency(kMcgFllClock, &mcgOut);
	/* If error code returned, return the code to calling function */
    if(clockFreqError != kClockManagerSuccess)
    {
        return mcgOut;
    }

    /* Ensure clock monitor is disabled before switching to FEI otherwise a loss of clock will trigger */
    clock_set_cme0(false);

    /* Change FLL reference clock from external to internal by setting IREFS bit */
    clock_set_irefs(kMcgIrefClockSourceSlow); /* select internal reference */

    /* wait for Reference clock to switch to internal reference */
    for (i = 0 ; i < 2000 ; i++)
    {
        if (clock_get_irefst())
        {
            break; /* jump out early if IREFST sets before loop finishes */
        }
    }
    if (!clock_get_irefst())
    {
        return 0x12; /* check bit is really set and return with error if not set */
    }

    /* Now in FEI mode */
    return mcgOut;
} /* fee_fei*/

/*!
 * @brief configure for MCG auto trimming
 * @param ircSelect - 0 if slow irc, 1 if fast irc
 * @param ircFreq - IRC frequency
 * @param mcgOutFreq - MCG output frequency
 */
uint8_t atc(uint8_t ircSelect, int32_t ircFreq, int32_t mcgOutFreq)
{
    uint8_t mcgMode;
    uint16_t atcv;
    uint32_t busClockFreq;
    uint32_t outdiv1, outdiv2, outdiv3, outdiv4;
#if (defined(CPU_MKL25Z128VLK4) || defined(CPU_MK22FN512VDC12))
    int32_t  busClkDivVal;

    busClkDivVal = mcgOutFreq / 16000000; /* calculate bus clock divider to generate fastest allowed bus clock for autotrim */
#endif

    if (ircSelect > 0) /* force irc to 1 if greater than 0 */
    {
        ircSelect = 1;
    }

    mcgMode = what_mcg_mode(); /* get present MCG mode */
    if ((mcgMode != PEE) && (mcgMode != PBE) && (mcgMode != FBE))
    {
        return 1; /* return error code if not in PEE, PBE or FBE modes */
    }

    /*store present clock divider values */
    clock_hal_get_clock_divider(kSimClockDividerOutdiv1, &outdiv1);
    clock_hal_get_clock_divider(kSimClockDividerOutdiv2, &outdiv2);
    clock_hal_get_clock_divider(kSimClockDividerOutdiv3, &outdiv3);
    clock_hal_get_clock_divider(kSimClockDividerOutdiv4, &outdiv4);

#if defined(CPU_MKL25Z128VLK4)
    /* set all bus and flash dividers to same value to ensure clocking restrictions are met */
    clock_hal_set_clock_divider(kSimClockDividerOutdiv4, busClkDivVal);
#elif defined(CPU_MK22FN512VDC12)
    /* set all bus and flash dividers to same value to ensure clocking restrictions are met */
    clock_hal_set_clock_divider(kSimClockDividerOutdiv2, busClkDivVal);
    clock_hal_set_clock_divider(kSimClockDividerOutdiv3, busClkDivVal);
    clock_hal_set_clock_divider(kSimClockDividerOutdiv4, busClkDivVal);
#endif

    clock_manager_get_frequency(kBusClock, &busClockFreq);

    if ((busClockFreq < 8000000) || (busClockFreq > 16000000))
    {
        clock_hal_set_clock_out_dividers(outdiv1, outdiv2, outdiv3, outdiv4);   /* set SIM_CLKDIV1 back to original value */
        return 3; /* error, bus clock frequency is not within 8MHz to 16MHz */
    }

    if(!ircSelect) /*determine if slow or fast IRC to be trimmed */
    {
        if (ircFreq < 31250) /* check frequency is above min spec. */
        {
            clock_hal_set_clock_out_dividers(outdiv1, outdiv2, outdiv3, outdiv4);   /* set SIM_CLKDIV1 back to original value */
            return 4;
        }
        if (ircFreq > 39062) /* check frequency is below max spec. */
        {
            clock_hal_set_clock_out_dividers(outdiv1, outdiv2, outdiv3, outdiv4);   /* set SIM_CLKDIV1 back to original value */
            return 5;
        }
    }
    else
    {
        if (ircFreq < 3000000) /* check frequency is above min spec. */
        {
            clock_hal_set_clock_out_dividers(outdiv1, outdiv2, outdiv3, outdiv4);   /* set SIM_CLKDIV1 back to original value */
            return 6;
        }
        if (ircFreq > 5000000) /* check frequency is below max spec. */
        {
            clock_hal_set_clock_out_dividers(outdiv1, outdiv2, outdiv3, outdiv4);   /* set SIM_CLKDIV1 back to original value */
            return 7;
        }
    } /* if */

    /* Set up autocal registers, must use floating point calculation */
    if (ircSelect)
    {
        atcv = (uint16_t)(128.0f * (21.0f * (busClockFreq / (float)ircFreq)));
    }
    else
    {
        atcv = (uint16_t)(21.0f * (busClockFreq / (float)ircFreq));
    }

    clock_set_atcvl(atcv & 0xFF); /*Set ATCVL to lower 8 bits of count value */
    clock_set_atcvh((uint16_t)(atcv & 0xFF00) >> 8); /* Set ATCVH to upper 8 bits of count value */

    /* Enable autocal */
    /*Select IRC to trim and enable trim machine */
    clock_set_atms((mcg_atms_select_t)ircSelect);
    clock_set_atme(true);

    while (clock_get_atme())
    {} /*poll for ATME bit to clear */

    if (clock_get_atmf())	/* check if error flag set */
    {
        clock_hal_set_clock_out_dividers(outdiv1, outdiv2, outdiv3, outdiv4);   /* set SIM_CLKDIV1 back to original value */
        return 8;
    }
    else
    {
        if (!ircSelect)
        {
            /* Might want to establish a MAX and MIN definition here  */
            if ((clock_get_sctrim() == 0xFF) || (clock_get_sctrim() == 0))
            {
                clock_hal_set_clock_out_dividers(outdiv1, outdiv2, outdiv3, outdiv4);   /* set SIM_CLKDIV1 back to original value */
                return 9;
            }
        }
        else
        {
#if defined(CPU_MKL25Z128VLK4)
            if ((clock_get_fcftrim() == 0xF) || (clock_get_fcftrim() == 0))
            {
                clock_hal_set_clock_out_dividers(outdiv1, outdiv2, outdiv3, outdiv4);   /* set SIM_CLKDIV1 back to original value */
                return 10;
            }
#endif
        }
    }
    clock_hal_set_clock_out_dividers(outdiv1, outdiv2, outdiv3, outdiv4);   /* set SIM_CLKDIV1 back to original value */
    return 0;
}/* atc*/

/*!
 * @brief Option to move from FEI to PEE mode or to just initialize the PLL
 *
 * @param crystalVal - external clock frequency in Hz either from a crystal or square
 *                           wave clock source
 * @param hgoVal     - selects whether low power or high gain mode is selected
 *                           for the crystal oscillator. This has no meaning if an
 *                           external clock is used.
 * @param erefsVal   - selects external clock (=0) or crystal osc (=1)
 * @param prdivVal   - value to divide the external clock source by to create the desired
 *                           PLL reference clock frequency
 * @param vdivVal    - value to multiply the PLL reference clock frequency by
 * @param mcgOutSelect  - 0 if the PLL is just to be enabled, non-zero if the PLL is used
 *                              to provide the MCGOUT clock for the system.
 *
 * Return value : PLL frequency (Hz) or error code
 */
int32_t pll_init(int32_t crystalVal, mcg_hgo_select_t hgoVal, mcg_eref_clock_select_t erefsVal, int8_t prDivVal, int8_t vDivVal, uint8_t mcgOutSelect)
{
    uint8_t frDivVal;
    uint8_t prDiv, vDiv;
    int16_t i;
    int32_t refFreq;
    int32_t pllFreq;

    /* check if in FEI mode */
    if (!((clock_get_clkst() == kMcgClkstFll) && 	/* check CLKS mux has selcted FLL output */
          (clock_get_irefst() == kMcgIrefstInt) &&	/* check FLL ref is internal ref clk */
          (clock_get_pllst() != kMcgPllstPllcs)))	/* check PLLS mux has selected FLL */
    {
        return 0x1;                                                     /* return error code */
    }

    /* check external frequency is less than the maximum frequency */
    if  (crystalVal > 50000000) {return 0x21;}

    /* check crystal frequency is within spec. if crystal osc is being used as PLL ref */
    if (crystalVal)
    {
        if ((crystalVal < 3000000) || (crystalVal > 32000000)) {return 0x22;} /* return 1 if one of the available crystal options is not available */
    }

    /* make sure HGO will never be greater than 1. Could return an error instead if desired. */
    if (hgoVal > 0)
    {
        hgoVal = kMcgHgoSelectHigh; /* force hgo_val to 1 if > 0 */
    }

    /* Check PLL divider settings are within spec. */
    if ((prDivVal < 1) || (prDivVal > 25)) {return 0x41;}
    if ((vDivVal < 24) || (vDivVal > 50)) {return 0x42;}

    /* Check PLL reference clock frequency is within spec. */
    refFreq = crystalVal / prDivVal;
    if ((refFreq < 2000000) || (refFreq > 4000000)) {return 0x43;}

    /* Check PLL output frequency is within spec. */
    pllFreq = (crystalVal / prDivVal) * vDivVal;
    if ((pllFreq < 48000000) || (pllFreq > 100000000)) {return 0x45;}

    /*
     * configure the MCG_C2 register
     * the RANGE value is determined by the external frequency. Since the RANGE parameter affects the FRDIV divide value
     */
    /* it still needs to be set correctly even if the oscillator is not being used */


    if (crystalVal <= 8000000)
    {
        clock_set_range0(kMcgFreqRangeSelectHigh);
        clock_set_hgo0(hgoVal);
        clock_set_erefs0(erefsVal);
    }
    else
    {
        clock_set_range0(kMcgFreqRangeSelectVeryHigh);
        clock_set_hgo0(hgoVal);
        clock_set_erefs0(erefsVal);
    }

    /* determine FRDIV based on reference clock frequency */
    /* since the external frequency has already been checked only the maximum frequency for each FRDIV value needs to be compared here. */
    if (crystalVal <= 1250000) {frDivVal = 0;}
    else if (crystalVal <= 2500000) {frDivVal = 1;}
    else if (crystalVal <= 5000000) {frDivVal = 2;}
    else if (crystalVal <= 10000000) {frDivVal = 3;}
    else if (crystalVal <= 20000000) {frDivVal = 4;}
    else {frDivVal = 5;}

    /*
     * Select external oscillator and Reference Divider and clear IREFS to start ext osc
     * If IRCLK is required it must be enabled outside of this driver, existing state will be maintained
     * CLKS=2, FRDIV=frdiv_val, IREFS=0, IRCLKEN=0, IREFSTEN=0
     */
	/* Set the required CLKS and FRDIV values */
    clock_set_clks_frdiv_irefs(kMcgClockSelectExt, frDivVal, kMcgIrefClockSourceExt);



    /* if the external oscillator is used need to wait for OSCINIT to set */
    if (erefsVal)
    {
        for (i = 0 ; i < 20000 ; i++)
        {
            if (clock_get_oscinit0())
            {
                break; /* jump out early if OSCINIT sets before loop finishes */
            }
        }
        if (!(clock_get_oscinit0()))
        {
            return 0x23; /* check bit is really set and return with error if not set */
        }
    }

    /* wait for Reference clock Status bit to clear */
    for (i = 0 ; i < 2000 ; i++)
    {
        if (!(clock_get_irefst()))
        {
            break; /* jump out early if IREFST clears before loop finishes */
        }
    }
    if (clock_get_irefst())
    {
        return 0x11; /* check bit is really clear and return with error if not set */
    }

    /* Wait for clock status bits to show clock source is ext ref clk */
    for (i = 0 ; i < 2000 ; i++)
    {
        if (clock_get_clkst() == kMcgClkstEref)
        {
            break; /* jump out early if CLKST shows EXT CLK slected before loop finishes */
        }
    }
    if (clock_get_clkst() != kMcgClkstEref)
    {
        return 0x1A; /* check EXT CLK is really selected and return with error if not */
    }

    /*
     * Now in FBE
     * It is recommended that the clock monitor is enabled when using an external clock as the clock source/reference.
     * It is enabled here but can be removed if this is not required.
    */
    clock_set_cme0(true);

    /*
     * Configure PLL
     * Configure MCG_C5
     * If the PLL is to run in STOP mode then the PLLSTEN bit needs to be OR'ed in here or in user code.
     */
    clock_set_prdiv0(prDivVal - 1); /*set PLL ref divider */

    /*
     * Configure MCG_C6
     * The PLLS bit is set to enable the PLL, MCGOUT still sourced from ext ref clk
     * The loss of lock interrupt can be enabled by seperately OR'ing in the LOLIE bit in MCG_C6
     */
	/* write new VDIV and enable PLL */
    clock_set_vdiv0(vDivVal - 24);
    clock_set_plls(kMcgPllSelectPllcs);

    /* wait for PLLST status bit to set */
    for (i = 0 ; i < 2000 ; i++)
    {
        if (clock_get_pllst())
        {
            break; /* jump out early if PLLST sets before loop finishes */
        }
    }
    if (!(clock_get_pllst()))
    {
        return 0x16; /* check bit is really set and return with error if not set */
    }

    /* Wait for LOCK bit to set */
    for (i = 0 ; i < 4000 ; i++)
    {
        if (clock_get_lock0())
        {
            break; /* jump out early if LOCK sets before loop finishes */
        }
    }
    if (!(clock_get_lock0()))
    {
        return 0x44; /* check bit is really set and return with error if not set */
    }

    /* Use actual PLL settings to calculate PLL frequency */

    prDiv = clock_get_prdiv0();
    vDiv = clock_get_vdiv0();

    /* now in PBE */

    clock_set_clks(kMcgClockSelectOut); /* clear CLKS to switch CLKS mux to select PLL as MCG_OUT */

    /* Wait for clock status bits to update */
    for (i = 0 ; i < 2000 ; i++)
    {

        if (clock_get_clkst() == kMcgClkstPll)
        {
            break; /* jump out early if CLKST = 3 before loop finishes */
        }
    }

    if (clock_get_clkst() != kMcgClkstPll)
    {
        return 0x1B; /* check CLKST is set correctly and return with error if not */
    }

    /* Now in PEE */

    return ((crystalVal / prDiv) * vDiv); /*MCGOUT equals PLL output frequency */
} /* pll_init*/

/*!
 * @brief checks the value in the SRS registers and sends
 * messages to the terminal announcing the status at the start of the
 * code.
 */
void out_srs(void)
{
    if (RCM_SRS1 & RCM_SRS1_SACKERR_MASK)
    {
        printf("\n\rStop Mode Acknowledge Error Reset");
    }
    if (RCM_SRS1 & RCM_SRS1_MDM_AP_MASK)
    {
	printf("\n\rMDM-AP Reset");
    }
    if (RCM_SRS1 & RCM_SRS1_SW_MASK)
    {
	printf("\n\rSoftware Reset");
    }
    if (RCM_SRS1 & RCM_SRS1_LOCKUP_MASK)
    {
	printf("\n\rCore Lockup Event Reset");
    }

    if (RCM_SRS0 & RCM_SRS0_POR_MASK)
    {
	printf("\n\rPower-on Reset");
    }
    if (RCM_SRS0 & RCM_SRS0_PIN_MASK)
    {
	printf("\n\rExternal Pin Reset");
    }
    if (RCM_SRS0 & RCM_SRS0_WDOG_MASK)
    {
	printf("\n\rWatchdog(COP) Reset");
    }
    if (RCM_SRS0 & RCM_SRS0_LOC_MASK)
    {
	printf("\n\rLoss of External Clock Reset");
    }
    if (RCM_SRS0 & RCM_SRS0_LOL_MASK)
    {
	printf("\n\rLoss of Lock in PLL Reset");
    }
    if (RCM_SRS0 & RCM_SRS0_LVD_MASK)
    {
	printf("\n\rLow-voltage Detect Reset");
    }
    if (RCM_SRS0 & RCM_SRS0_WAKEUP_MASK)
    {
        printf("\n\r[outSRS]Wakeup bit set from low power mode ");
        if (smc_hal_power_mode_get_stop_config() == kSmcLls)
        {
            printf("LLS exit ") ;
        }
        if ((smc_hal_power_mode_get_stop_config()== kSmcVlls) && (smc_hal_power_mode_get_stop_submode_config()== kSmcStopSub0))
        {
            printf("VLLS0 exit ") ;
        }
        if ((smc_hal_power_mode_get_stop_config()== kSmcVlls) && (smc_hal_power_mode_get_stop_submode_config()== kSmcStopSub1))
        {
            printf("VLLS1 exit ") ;
        }
        if ((smc_hal_power_mode_get_stop_config()== kSmcVlls) && (smc_hal_power_mode_get_stop_submode_config()== kSmcStopSub2))
        {
            printf("VLLS2 exit") ;
        }
        if ((smc_hal_power_mode_get_stop_config()== kSmcVlls) && (smc_hal_power_mode_get_stop_submode_config()== kSmcStopSub3))
        {
            printf("VLLS3 exit ") ;
        }
    }

    printf("\n\r");

    if ((RCM_SRS0 == 0) && (RCM_SRS1 == 0))
    {
	printf("[outSRS]RCM_SRS0 is ZERO   = %#02X \r\n\r", (RCM_SRS0))  ;
	printf("[outSRS]RCM_SRS1 is ZERO   = %#02X \r\n\r", (RCM_SRS1))  ;
    }
}
/*!
 * @brief Port ISR routine for the on board switch
 */
void port_isr(void)
{
    /* Clear the interrupt flag for the UART pin*/
    /* gpio_clear_pin_interrupt_flag(switchPins[0].pinName);*/

    if (uartEnabled)
    {
        printf("\n\n\r-- Entered PORT interrupt ISR! --\n\n\r");
    }

#if defined(TWR_K22F120M)
    if(port_hal_read_port_interrupt_flag(HW_GPIOC) == (1<<7))
    {
        port_hal_clear_port_interrupt_flag(HW_GPIOC);
    }

    if(port_hal_read_port_interrupt_flag(HW_GPIOC) == (1<<6))
    {
        port_hal_clear_port_interrupt_flag(HW_GPIOC);
    }

#elif (defined(FRDM_K64F120M) || defined(TWR_K64F120M) )
    if(port_hal_read_port_interrupt_flag(HW_GPIOA) == (1<<4))
    {
	port_hal_clear_port_interrupt_flag(HW_GPIOA);
    }
    if(port_hal_read_port_interrupt_flag(HW_GPIOC) == (1<<6))
    {
	port_hal_clear_port_interrupt_flag(HW_GPIOC);
    }

#elif defined(FRDM_K22F120M)
    if(port_hal_read_port_interrupt_flag(HW_GPIOB) == (1<<17))
    {
        port_hal_clear_port_interrupt_flag(HW_GPIOB);
    }

    if(port_hal_read_port_interrupt_flag(HW_GPIOC) == (1<<1))
    {
        port_hal_clear_port_interrupt_flag(HW_GPIOC);
    }
    
#endif

    return;
}

void HardFault_Handler(void)
{
#ifdef TWR_K22F120M
    PORTD_PCR5 = 0;
    PORTD_PCR6 = 0;
    PORTD_PCR7 = 0;

    PORTD_PCR4 = PORT_PCR_MUX(1);

    GPIOD_PDDR = (1 << 4);
    GPIOD_PCOR = (1 << 4);
#else
    PORTE_PCR7 = 0;
    PORTE_PCR8 = 0;
    PORTE_PCR9 = 0;

    PORTE_PCR6 = PORT_PCR_MUX(1);

    GPIOE_PDDR = (1 << 6);
    GPIOE_PCOR = (1 << 6);
#endif
    while(1)
    {}

}

void de_init_pins(void)
{

    /* I2C Pins */
    /* PORTD_PCR8 */
    port_hal_mux_control(HW_PORTD,8u,kPortPinDisabled);
    /* PORTD_PCR9 */
    port_hal_mux_control(HW_PORTD,9u,kPortPinDisabled);
    /* PORTC_PCR10 */
    port_hal_mux_control(HW_PORTC,10u,kPortPinDisabled);
    /* PORTC_PCR11 */
    port_hal_mux_control(HW_PORTC,11u,kPortPinDisabled);

#if defined(FRDM_K64F120M)
    /* SDHC Pins */
    /* PORTE_PCR3 */
    port_hal_mux_control(HW_PORTE,3u,kPortPinDisabled);
    /* PORTE_PCR1 */
    port_hal_mux_control(HW_PORTE,1u,kPortPinDisabled);
    /* PORTE_PCR0 */
    port_hal_mux_control(HW_PORTE,0u,kPortPinDisabled);
    /* PORTE_PCR5 */
    port_hal_mux_control(HW_PORTE,5u,kPortPinDisabled);
    /* PORTE_PCR4 */
    port_hal_mux_control(HW_PORTE,4u,kPortPinDisabled);
    /* PORTE_PCR2 */
    port_hal_mux_control(HW_PORTE,2u,kPortPinDisabled);
#endif

    /* SPI Pins */
    /* PORTD_PCR0 */
    port_hal_mux_control(HW_PORTD,0u,kPortPinDisabled);
    /* PORTD_PCR3 */
    port_hal_mux_control(HW_PORTD,3u,kPortPinDisabled);
    /* PORTD_PCR1 */
    port_hal_mux_control(HW_PORTD,1u,kPortPinDisabled);
    /* PORTD_PCR2 */
    port_hal_mux_control(HW_PORTD,2u,kPortPinDisabled);
    /* PORTB_PCR10 */
    port_hal_mux_control(HW_PORTB,10u,kPortPinDisabled);
    /* PORTB_PCR11 */
    port_hal_mux_control(HW_PORTB,11u,kPortPinDisabled);
#if !defined(FRDM_K64F120M)
    /* PORTB_PCR17 */
    port_hal_mux_control(HW_PORTB,17u,kPortPinDisabled);
    /* PORTB_PCR16 */
    port_hal_mux_control(HW_PORTB,16u,kPortPinDisabled);
#endif

    /* CAN Pins */
    /* PORTB_PCR19 */
    port_hal_mux_control(HW_PORTB,19u,kPortPinDisabled);
    /* PORTB_PCR18 */
    port_hal_mux_control(HW_PORTB,18u,kPortPinDisabled);

    /* Ethernet Pins */
    /* PORTC_PCR16 */
    port_hal_mux_control(HW_PORTC,16u,kPortPinDisabled);
    /* PORTC_PCR17 */
    port_hal_mux_control(HW_PORTC,17u,kPortPinDisabled);
    /* PORTC_PCR18 */
    port_hal_mux_control(HW_PORTC,18u,kPortPinDisabled);
    /* PORTC_PCR19 */
    port_hal_mux_control(HW_PORTC,19u,kPortPinDisabled);
    /* PORTA_PCR14 */
    port_hal_mux_control(HW_PORTA,14u,kPortPinDisabled);
    /* PORTB_PCR1 */
    port_hal_mux_control(HW_PORTB,1u,kPortPinDisabled);
    /* PORTB_PCR0 */
    port_hal_mux_control(HW_PORTB,0u,kPortPinDisabled);
    /* PORTA_PCR13 */
    port_hal_mux_control(HW_PORTA,13u,kPortPinDisabled);
    /* PORTA_PCR12 */
    port_hal_mux_control(HW_PORTA,12u,kPortPinDisabled);
    /* PORTA_PCR5 */
    port_hal_mux_control(HW_PORTA,5u,kPortPinDisabled);
    /* PORTA_PCR16 */
    port_hal_mux_control(HW_PORTA,16u,kPortPinDisabled);
    /* PORTA_PCR17 */
    port_hal_mux_control(HW_PORTA,17u,kPortPinDisabled);
    /* PORTA_PCR15 */
    port_hal_mux_control(HW_PORTA,15u,kPortPinDisabled);
}

/*******************************************************************************
 * EOF
 ******************************************************************************/
