/******************************************************************************
*
* Copyright 2006-2015 Freescale Semiconductor, Inc.
* Copyright 2016-2017 NXP
*
******************************************************************************/

/****************************************************************************//*!
*
* @file     scg.c
*
* @brief    SCG routines
*
*******************************************************************************/

/*******************************************************************************
* Includes
*******************************************************************************/
#include "device_registers.h"
#include "scg.h"
#include "main.h"
#include "power_mode.h"

/*****************************************************************************
*
* Function: void SCG_Init(uint8_t clkMode)
*
* Description: Init SCG
*
*****************************************************************************/
void SCG_Init(uint8_t clkMode)
{
	// Configure SCG based on selected clock mode
	switch(clkMode)
	{
    	// RUN mode after reset
    	// System oscillator 8MHz, core clock 80MHz, system clock 80MHz, bus clock 40MHz
		case RUN_PLL:
		{
			/* Disable monitor, disable clock and clear error. */
			SCG->SOSCCSR = SCG_SOSCCSR_SOSCERR_MASK;

			/* Now start to set up OSC clock. */
			/* Step 1. Setup dividers. */
			SCG->SOSCDIV =  SCG_SOSCDIV_SOSCDIV1(1) |
							SCG_SOSCDIV_SOSCDIV2(1);

			/* Step 2. Set OSC configuration. */
			SCG->SOSCCFG =  SCG_SOSCCFG_RANGE(2) |
                    		SCG_SOSCCFG_HGO(0) |
							SCG_SOSCCFG_EREFS(1);
			/* Step 3. Enable clock. */
			//SCG->SOSCCSR =  SCG_SOSCCSR_SOSCEN(1) |
			//				SCG_SOSCCSR_SOSCERCLKEN(0);
			/* Step 4. Enable monitor. */
			SCG->SOSCCSR = (SCG->SOSCCSR & ~(SCG_SOSCCSR_SOSCCM_MASK   |
                                     	 	 SCG_SOSCCSR_SOSCCMRE_MASK |
											 SCG_SOSCCSR_SOSCERR_MASK));
			break;
		}
		case(RUN_FIRC):
		{
	    	/* FIRC mode after reset */
	    	/* FIRC 48MHz, core clock 48MHz, system clock 48MHz, bus clock 48MHz, Flash clock 24MHz */
			/* FIRC Configuration 48MHz */
			SCG->FIRCDIV = SCG_FIRCDIV_FIRCDIV1(0b01)	   /*FIRC DIV1=1*/
						  |SCG_FIRCDIV_FIRCDIV2(0b01);	   /*FIRC DIV2=1*/

			SCG->FIRCCFG =SCG_FIRCCFG_RANGE(0b00);	/* Fast IRC trimmed 48MHz*/

			while(SCG->FIRCCSR & SCG_FIRCCSR_LK_MASK); /*Is PLL control and status register locked?*/

			SCG->FIRCCSR = SCG_FIRCCSR_FIRCEN_MASK;  /*Enable FIRC*/

			while(!(SCG->FIRCCSR & SCG_FIRCCSR_FIRCVLD_MASK)); /*Check that FIRC clock is valid*/

			/* RUN Clock Configuration */
			SCG->RCCR=SCG_RCCR_SCS(0b0011) /* FIRC as clock source*/
					  |SCG_RCCR_DIVCORE(0b00) /* DIVCORE=1, Core clock = 48 MHz*/
					  |SCG_RCCR_DIVBUS(0b00)  /* DIVBUS=1, Bus clock = 48 MHz*/
					  |SCG_RCCR_DIVSLOW(0b01);/* DIVSLOW=2, Flash clock= 24 MHz*/


			break;
		}
		case VLPR:
		{
			/* VLPR mode after reset */
			/* SIRC Configuration 8MHz */
			SCG->SIRCDIV = SCG_SIRCDIV_SIRCDIV1(0b01)	   /*SIRC DIV1=1*/
	   				      |SCG_SIRCDIV_SIRCDIV2(0b01);	   /*SIRC DIV2=1*/

			SCG->SIRCCFG =SCG_SIRCCFG_RANGE(0b01);	/* Slow IRC trimmed 8MHz*/

			while(SCG->SIRCCSR & SCG_SIRCCSR_LK_MASK); /*Is SIRC control and status register locked?*/

			SCG->SIRCCSR |= SCG_SIRCCSR_SIRCEN_MASK;  /*Enable SIRC*/

			while(!(SCG->SIRCCSR & SCG_SIRCCSR_SIRCVLD_MASK)); /*Check that SIRC clock is valid*/

			/* VLPR Clock Configuration */
			SCG->VCCR=SCG_VCCR_SCS(0b10)		/* SIRC selected for VLP modes*/
					 |SCG_VCCR_DIVCORE(0b01)		/* DIVCORE=2, Core clock= 4MHz*/
					 |SCG_VCCR_DIVBUS(0b01)		/* DIVBUS=2, BUS= 2MHz*/
					 |SCG_VCCR_DIVSLOW(0b11);	/* DIVSLOW=4,Flash clock=1MHz*/

			SCG->SOSCCSR&=~SCG_SOSCCSR_SOSCCM_MASK; /* Disable system oscillator clock monitor*/


			break;
		}
	}

	// Enable very low power modes
	SMC->PMPROT = SMC_PMPROT_AVLP_MASK;
}
