/*
 * Clock Initialization Code MSP430F5438A
 * Initialize Clock Source XT1 @ 32768 Hz
 * Initialize Clock Source XT2 @ 20MHz
 *
 * Sunseeker Telemetry 2021
 *
 * Last modified October 2015 by Scott Haver
 * Last modified May 2021 by B. Bazuin
 *
 * Main CLK     :  MCLK  = XT2     = 20 MHz
 * Sub-Main CLK :  SMCLK = XT2/2   = 10  MHz
 * Aux CLK      :  ACLK  = XT1     = 32.768 kHz
 *
 */
#include <msp430x54xa.h>


void Port_Init(void);
void Clock_XT1_Init(void);
void Clock_XT2_Init(void);
void SetVCoreUp(unsigned int level);


void clock_init(void)
{
	WDTCTL = WDTPW + WDTHOLD;   //Stop watchdog timer

	Port_Init();	            //ensure clock pins are configured
	
	SetVCoreUp(1);				//Configure MCU core voltage for HF clock
    SetVCoreUp(2);				//
    SetVCoreUp(3);				//

	Clock_XT1_Init();		    //LF clock source init
	Clock_XT2_Init();			//HF clock source init

}

void Port_Init(void)
{
	//Clock Source TEST PINS ACLK/MCLK/SMCLK
	P11DIR |= (1 << 0) | (1 << 1) | (1 << 2); //set P11.0:P11.2 as output ACLK/MCLK/SMCLK
    P11SEL |= (1 << 0) | (1 << 1) | (1 << 2); //set P11.0:P11.2 as ACLK/MCLK/SMCLK function

	//XT1 ALTERNATE PIN CONFIG
	P7SEL |= (1 << 0) | (1 << 1);			  //set P7.0 & P7.1 as XT1IN/XT1OUT periph
	P7DIR |= (1 << 0) | (1 << 1);

	//XT2 ALTERNATE PIN CONFIG
    P5SEL |= (1 << 2) | (1 << 3);             //set P5.2 & P5.3 as XT2IN/XT2OUT periph
    P5DIR |= (1 << 2) | (1 << 3);
}

void Clock_XT1_Init(void)
{
	char i;

	//XT1 CLOCK CONFIG
	UCSCTL6 &= ~(XT1OFF);           		  //Enable XT1
	UCSCTL6 &= ~(XT1DRIVE1 | XT1DRIVE0);	  //lowest drive current LF 32KHz oscillator

	do
	{
		UCSCTL7 &= ~(XT2OFFG + XT1LFOFFG + XT1HFOFFG + DCOFFG);//Clear XT2,XT1,DCO fault flags
		SFRIFG1 &= ~OFIFG;                    //Clear fault flags
		for(i=255;i>0;i--);                   //Delay for Osc to stabilize
	}
	while ((SFRIFG1 & OFIFG) != 0);           //Test oscillator fault flag

    UCSCTL4 |= SELA__XT1CLK; 				  //Clock Source ACLK = XT1 = 32kHz
    UCSCTL5 |= DIVA_0;               		  //Divide ACLK/1 = 32kHz

}

void Clock_XT2_Init(void)
{
	char i;

	//XT2 CLOCK CONFIG
	UCSCTL6 &= ~(XT2OFF);           		  //Enable XT2
	UCSCTL6 |= XT2DRIVE_3; 					  //Drive current 16-24 MHz Clock
	UCSCTL6 &= ~XT2BYPASS; 				      //XT2 Sourced Externally from pin - 20MHz

    do
	{
    	UCSCTL7 &= ~(XT2OFFG + XT1LFOFFG + XT1HFOFFG + DCOFFG);//Clear XT2,XT1,DCO fault flags
    	SFRIFG1 &= ~OFIFG;                    //Clear fault flags
    	for(i=255;i>0;i--);                   //Delay for Osc to stabilize
	 }
     while ((SFRIFG1 & OFIFG) != 0);          //Test oscillator fault flag

	 UCSCTL4 |= (SELS__XT2CLK | SELM__XT2CLK);//Clock Source SMCLK=MCLK = XT2 = 20MHz
	 UCSCTL5 |= DIVM_0 | DIVS_1;              //MCLK:XT2/1 = 20MHz SMCLK:XT2/2 = 10MHz

}

/*************************************************************
/ Name: SetVCoreUp
/ IN: int Level
/ OUT:  void
/ DESC:  This function is used to set the voltage of the VCORE to
/        The level specified in input
/ Reference: Users Guide page 74
************************************************************/
void SetVCoreUp (unsigned int level)
{
  // Open PMM registers for write access
  PMMCTL0_H = 0xA5;
  // Set SVS/SVM high side new level
  SVSMHCTL = SVSHE + SVSHRVL0 * level + SVMHE + SVSMHRRL0 * level;
  // Set SVM low side to new level
  SVSMLCTL = SVSLE + SVMLE + SVSMLRRL0 * level;
  // Wait till SVM is settled
  while ((PMMIFG & SVSMLDLYIFG) == 0);
  // Clear already set flags
  PMMIFG &= ~(SVMLVLRIFG + SVMLIFG);
  // Set VCore to new level
  PMMCTL0_L = PMMCOREV0 * level;
  // Wait till new level reached
  if ((PMMIFG & SVMLIFG))
    while ((PMMIFG & SVMLVLRIFG) == 0);
  // Set SVS/SVM low side to new level
  SVSMLCTL = SVSLE + SVSLRVL0 * level + SVMLE + SVSMLRRL0 * level;
  // Wait till SVM is settled
  while ((PMMIFG & SVSMLDLYIFG) == 0);
  // Lock PMM registers for write access
  PMMCTL0_H = 0x00;
}

