/**
  ******************************************************************************
  * @file    HAL_PMM.c
  * @author  Johan Zwarteveld
  * @version V1.0
  * @date    04-08-2015
  * @brief   Power Management Module Driver
  *          This file provides firmware functions to manage the following
  *          functionalities of the Power Managment Module (PMM):
  *           + Setting Vcore voltage
  ******************************************************************************
  */

/* Includes ------------------------------------------------------------------*/
#include "HAL_PMM.h"

/* Private typedef -----------------------------------------------------------*/
/* Private define ------------------------------------------------------------*/
#define _HAL_PMM_DISABLE_SVML_
#define _HAL_PMM_DISABLE_SVSL_
#define _HAL_PMM_DISABLE_FULL_PERFORMANCE_
/* Private macro -------------------------------------------------------------*/
#ifdef _HAL_PMM_DISABLE_SVML_
#define _HAL_PMM_SVMLE  (SVMLE)
#else
#define _HAL_PMM_SVMLE  (0)
#endif
#ifdef _HAL_PMM_DISABLE_SVSL_
#define _HAL_PMM_SVSLE  (SVSLE)
#else
#define _HAL_PMM_SVSLE  (0)
#endif
#ifdef _HAL_PMM_DISABLE_FULL_PERFORMANCE_
#define _HAL_PMM_SVSFP  (SVSLFP)
#define _HAL_PMM_SVMFP  (SVMLFP)
#else
#define _HAL_PMM_SVSFP  (0)
#define _HAL_PMM_SVMFP  (0)
#endif

/* Private variables ---------------------------------------------------------*/
uint16_t PMMIFG_Backup;

/* Private function prototypes -----------------------------------------------*/
static uint16_t HAL_PMM_SetVCoreUp(uint8_t level);
static uint16_t HAL_PMM_SetVCoreDown(uint8_t level);

/* Exported functions --------------------------------------------------------*/
uint16_t HAL_PMM_SetVCore(uint8_t level)
{
	uint16_t actlevel;
	uint16_t status = 0;
	level &= PMMCOREV_3;													// Set Mask for max. level
	actlevel = (PMMCTL0 & PMMCOREV_3);										// Get actual Vcore

	while(((level != actlevel) && (status == 0)) || (level < actlevel))		// step by step VCore adjustment
	{
		if (level > actlevel)
		{
			status = HAL_PMM_SetVCoreUp(++actlevel);						// Increase Vcore level
		}
		else
		{
			status = HAL_PMM_SetVCoreDown(--actlevel);						// Decrease Vcore level
		}
	}
	return status;
}

/* Private functions  --------------------------------------------------------*/
/* @note	do not use Lower power mode beyond 1. low power modes beyond 1 can cause code hang ups and unexpected resets. */
static uint16_t HAL_PMM_SetVCoreUp(uint8_t level)
{
	uint16_t PMMRIE_Backup;
	uint16_t SVSMHCTL_Backup;

	// Open PMM registers for write access
	PMMCTL0_H = 0xA5;

	// Backup registers
	PMMRIE_Backup = PMMRIE;
	SVSMHCTL_Backup = SVSMHCTL;

	// Disable and clear all interrupts
	PMMRIE &= ~(SVSMHDLYIE | SVSMLDLYIE | SVMLVLRIE | SVMHVLRIE | SVMHVLRPE);
	PMMIFG &= ~(SVMHVLRIFG | SVMHIFG | SVSMHDLYIFG | SVMLVLRIFG | SVMLIFG | SVSMLDLYIFG);
	PMMIFG_Backup = 0;

	// Enable SVM highside interrupt
	PMMRIE |= (SVSMHDLYIE);

	// Set SVM highside to new level and check if a VCore increase is possible
	SVSMHCTL = SVMHE | SVSHE | (SVSMHRRL0 * level);

	// Wait until SVM highside is settled
	__bis_SR_register( LPM1_bits + GIE );

	// Disable SVM highside interrupt
	PMMRIE &= ~(SVSMHDLYIE);
	// Check if a VCore increase is possible
	if (PMMIFG_Backup & SVMHIFG)       										//-> Vcc is to low for a Vcore increase
	{
		// Enable SVM highside interrupt
		PMMRIE |= (SVSMHDLYIE);

		// recover the previous settings
		SVSMHCTL = SVSMHCTL_Backup;

		// Wait until SVM highside is settled
		__bis_SR_register( LPM1_bits + GIE );

		// Disable SVM highside interrupt
		PMMRIE &= ~(SVSMHDLYIE);

	  	// Clear all Flags
		PMMIFG &= ~(SVMHVLRIFG | SVMHIFG | SVSMHDLYIFG | SVMLVLRIFG | SVMLIFG | SVSMLDLYIFG);
		// backup PMM-Interrupt-Register
		PMMRIE = PMMRIE_Backup;

		// Lock PMM registers for write access
		PMMCTL0_H = 0x00;
		return PMM_STATUS_ERROR;            								// return: voltage not set
	}
	// Set also SVS highside to new level	    							//-> Vcc is high enough for a Vcore increase
	SVSMHCTL |= (SVSHRVL0 * level);

	// Enable SVM low side interrupt
	PMMRIE |= (SVSMLDLYIE);

	// Set SVM low side to new level
	SVSMLCTL = SVMLE | (SVSMLRRL0 * level);

	// Wait until SVM low side is settled
	__bis_SR_register( LPM1_bits + GIE );

	// Disable SVM highside interrupt
	PMMRIE &= ~(SVSMLDLYIE);

	// Set VCore to new level
	PMMCTL0_L = PMMCOREV0 * level;
	// Wait until new level reached
	if (PMMIFG & SVMLIFG)
	{
		// Enable SVM low side interrupt
		PMMRIE |= (SVMLVLRIE);

		// Wait until SVM low side is settled
		__bis_SR_register( LPM1_bits + GIE );

		// Disable SVM low side interrupt
		PMMRIE &= ~(SVMLVLRIE);
	}
	// Enable SVM low side interrupt
	PMMRIE |= (SVSMLDLYIE);

	// Set also SVS/SVM low side to new level
	SVSMLCTL |= SVSLE | (SVSLRVL0 * level);

	// Wait until SVM low side is settled
	__bis_SR_register( LPM1_bits + GIE );

	// Disable SVM highside interrupt
	PMMRIE &= ~(SVSMLDLYIE);

	// Disable SVS/SVM Low
	// Disable full-performance mode to save energy
	SVSMLCTL &= ~(_HAL_PMM_SVSLE + _HAL_PMM_SVMLE + _HAL_PMM_SVSFP + _HAL_PMM_SVMFP);
	SVSMHCTL &= ~(_HAL_PMM_SVSFP + _HAL_PMM_SVMFP);

	// Clear all Flags
	PMMIFG &= ~(SVMHVLRIFG | SVMHIFG | SVSMHDLYIFG | SVMLVLRIFG | SVMLIFG | SVSMLDLYIFG);
	// backup PMM-Interrupt-Register
	PMMRIE = PMMRIE_Backup;

	// Lock PMM registers for write access
	PMMCTL0_H = 0x00;
	return PMM_STATUS_OK;                               					// return: OK
}

/* @note	do not use Lower power mode beyond 1. low power modes beyond 1 can cause code hang ups and unexpected resets. */
uint16_t HAL_PMM_SetVCoreDown(uint8_t level)
{
	uint16_t PMMRIE_Backup;

	// Open PMM registers for write access
	PMMCTL0_H = 0xA5;

	// Backup registers
	PMMRIE_Backup = PMMRIE;

	// Disable and clear all interrupts
	PMMRIE &= ~(SVSMHDLYIE | SVSMLDLYIE | SVMLVLRIE | SVMHVLRIE | SVMHVLRPE);
	PMMIFG &= ~(SVMHVLRIFG | SVMHIFG | SVSMHDLYIFG | SVMLVLRIFG | SVMLIFG | SVSMLDLYIFG);
	PMMIFG_Backup = 0;

	// Enable SVM highside interrupt
	PMMRIE |= (SVSMHDLYIE);

	// Set SVM highside to new level
	SVSMHCTL = SVMHE | (SVSMHRRL0 * level);

	// Wait until SVM highside is settled
	__bis_SR_register( LPM1_bits + GIE );

	// Disable SVM highside interrupt
	PMMRIE &= ~(SVSMHDLYIE);

	// Set SVM low side to new level
	SVSMLCTL = SVMLE | (SVSMLRRL0 * level);

	// Enable SVM low side interrupt
	PMMRIE |= (SVSMLDLYIE);

	// Wait until SVM low side is settled
	__bis_SR_register( LPM1_bits + GIE );

	// Disable SVM highside interrupt
	PMMRIE &= ~(SVSMLDLYIE);

	// Set VCore to new level
	PMMCTL0_L = PMMCOREV0 * level;

	// Enable SVS highside interrupt
	PMMRIE |= (SVSMHDLYIE);

	// Set SVS highside to new level
	SVSMHCTL |= SVSHE | (SVSHRVL0 * level);

	// Wait until SVS highside is settled
	__bis_SR_register( LPM1_bits + GIE );

	// Disable SVS highside interrupt
	PMMRIE &= ~(SVSMHDLYIE);

	// Enable SVS low side interrupt
	PMMRIE |= (SVSMLDLYIE);

	// Set SVS low side to new level
	SVSMLCTL |= SVSLE | (SVSLRVL0 * level);

	// Wait until SVS low side is settled
	__bis_SR_register( LPM1_bits + GIE );

	// Disable SVS highside interrupt
	PMMRIE &= ~(SVSMLDLYIE);

	// Disable full-performance mode to save energy
	SVSMHCTL &= ~_HAL_PMM_SVSFP;
	// Disable SVS/SVM Low
	// Disable full-performance mode to save energy
	SVSMLCTL &= ~(_HAL_PMM_SVSLE + _HAL_PMM_SVMLE + _HAL_PMM_SVSFP + _HAL_PMM_SVMFP);
	SVSMHCTL &= ~(_HAL_PMM_SVSFP + _HAL_PMM_SVMFP);

	// Clear all Flags
	PMMIFG &= ~(SVMHVLRIFG | SVMHIFG | SVSMHDLYIFG | SVMLVLRIFG | SVMLIFG | SVSMLDLYIFG);
	// backup PMM-Interrupt-Register
	PMMRIE = PMMRIE_Backup;
	// Lock PMM registers for write access
	PMMCTL0_H = 0x00;

	if (PMMIFG_Backup & SVMHIFG)
		return PMM_STATUS_ERROR;					 	// Highside is still to low for the adjusted VCore Level
	else
		return PMM_STATUS_OK;							// Return: OK

}

/* Interrupt Service Routine -------------------------------------------------*/
#if defined(__TI_COMPILER_VERSION__) || defined(__IAR_SYSTEMS_ICC__)
#pragma vector=SYSNMI_VECTOR
__interrupt void SYSNMI_ISR(void)
#elif defined(__GNUC__)
void __attribute__ ((interrupt(SYSNMI_VECTOR))) SYSNMI_ISR (void)
#else
#error Compiler not supported!
#endif
{
	// Backup Power Management interrupt flags
	PMMIFG_Backup = PMMIFG;

	switch(__even_in_range(SYSSNIV,20))
	{
		case 0: break;							// No interrupt pending
		case 2: break;							// SVMLIFG interrupt pending
		case 4: break;							// SVMHIFG interrupt pending
		case 6:									// SVSMLDLYIFG interrupt pending
			LPM4_EXIT;
			break;
		case 8:									// SVSMHDLYIFG interrupt pending
			LPM4_EXIT;
			break;
		case 10: break;							// VMAIFG interrupt pending
		case 12: break;							// JMBINIFG interrupt pending
		case 14: break;							// JMBOUTIFG interrupt pending
		case 16:								// SVMLVLRIFG interrupt pending
			LPM4_EXIT;
			break;
		case 18:								// SVMHVLRIFG interrupt pending
			LPM4_EXIT;
			break;
		case 20: break;							// Reserved

	}
}
