/* --COPYRIGHT--,BSD
 * Copyright (c) 2016, Texas Instruments Incorporated
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * *  Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 * *  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.
 *
 * *  Neither the name of Texas Instruments Incorporated 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 OWNER 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.
 * --/COPYRIGHT--*/
//*****************************************************************************
//
// cs.c - Driver for the cs Module.
//
//*****************************************************************************

//*****************************************************************************
//
//! \addtogroup cs_api cs
//! @{
//
//*****************************************************************************

#include "inc/hw_memmap.h"

#ifdef __MSP430_HAS_CS__
#include "cs.h"

#include <assert.h>

//*****************************************************************************
//
// Internal very low power VLOCLK, low frequency oscillator with 10 kHz typical
// frequency
//
//*****************************************************************************
#define CS_VLOCLK_FREQUENCY                                               10000

//*****************************************************************************
//
// Internal, trimmed, low-frequency oscillator with 32768 Hz typical frequency
//
//*****************************************************************************
#define CS_REFOCLK_FREQUENCY                                              32768

//*****************************************************************************
//
// Internal DCO frequency range
//
//*****************************************************************************
#define CS_DCO_RANGE_1MHZ                                               1000000
#define CS_DCO_RANGE_2MHZ                                               2000000
#define CS_DCO_RANGE_4MHZ                                               4000000
#define CS_DCO_RANGE_8MHZ                                               8000000
#define CS_DCO_RANGE_12MHZ                                             12000000
#define CS_DCO_RANGE_16MHZ                                             16000000
#define CS_DCO_RANGE_20MHZ                                             20000000
#define CS_DCO_RANGE_24MHZ                                             24000000

//******************************************************************************
//
//The XT1 crystal frequency. Should be set with CS_externalClockSourceInit
//if XT1 is used and user intends to invoke CS_getSMCLK, CS_getMCLK or
//CS_getACLK
//
//******************************************************************************
static uint32_t privateXT1ClockFrequency = 0;

static uint32_t privateDCORange(void)
{
    uint32_t res = 0;
    switch((HWREG8(CS_BASE + OFS_CSCTL1)) & DCORSEL_7)
    {
    case DCORSEL_0:
        res = CS_DCO_RANGE_1MHZ;
        break;
    case DCORSEL_1:
        res = CS_DCO_RANGE_2MHZ;
        break;
    case DCORSEL_2:
        res = CS_DCO_RANGE_4MHZ;
        break;
    case DCORSEL_3:
        res = CS_DCO_RANGE_8MHZ;
        break;
    case DCORSEL_4:
        res = CS_DCO_RANGE_12MHZ;
        break;
    case DCORSEL_5:
        res = CS_DCO_RANGE_16MHZ;
        break;
    case DCORSEL_6:
        res = CS_DCO_RANGE_20MHZ;
        break;
    case DCORSEL_7:
        res = CS_DCO_RANGE_24MHZ;
        break;
    default:
        break;
    }

    return(res);
}

static uint32_t privateCSSourceClockFromDCO(uint16_t FLLRefCLKSource)
{
    uint16_t N_value;
    uint16_t n_value = 1;
    uint32_t Fref_value;

    N_value = (HWREG16(CS_BASE + OFS_CSCTL2)) & 0x03FF;
    uint16_t tempDivider = HWREG8(CS_BASE + OFS_CSCTL3) & FLLREFDIV_7;

    if(tempDivider > 1)
    {
        n_value = 32 << (tempDivider - 1);
    }

    switch((HWREG8(CS_BASE + OFS_CSCTL3)) & SELREF_3)
    {
    case SELREF__XT1CLK:
        Fref_value = privateXT1ClockFrequency;

        if(HWREG8(CS_BASE + OFS_CSCTL7) & XT1OFFG)
        {
            HWREG8(CS_BASE + OFS_CSCTL7) &= ~(XT1OFFG);
            //Clear OFIFG fault flag
            HWREG8(CS_BASE + OFS_SFRIFG1) &= ~OFIFG;

            if(HWREG8(CS_BASE + OFS_CSCTL7) & XT1OFFG)
            {
                if(HWREG16(CS_BASE + OFS_CSCTL6) & XTS)
                {
                    HWREG8(CS_BASE + OFS_CSCTL7) &= ~DCOFFG;
                    Fref_value = privateDCORange();
                }
                else
                {
                    Fref_value = CS_REFOCLK_FREQUENCY;
                }
            }
        }
        break;
    case SELREF__REFOCLK:
        Fref_value = CS_REFOCLK_FREQUENCY;
        break;
    default: break;
    }

    return (Fref_value * (N_value + 1) / n_value);
}

static uint32_t privateCSComputeCLKFrequency(uint16_t CLKSource,
                                             uint16_t CLKSourceDivider,
                                             uint8_t CLKDest)
{
    uint32_t CLKFrequency = 0;
    uint8_t CLKSourceFrequencyDivider = 1;

    CLKSourceFrequencyDivider = 1 << CLKSourceDivider;

    switch(CLKSource)
    {
    case SELMS__XT1CLK:
        CLKFrequency = (privateXT1ClockFrequency /
                        CLKSourceFrequencyDivider);

        if(HWREG8(CS_BASE + OFS_CSCTL7) & XT1OFFG)
        {
            HWREG8(CS_BASE + OFS_CSCTL7) &= ~(XT1OFFG);
            //Clear OFIFG fault flag
            HWREG8(SFR_BASE + OFS_SFRIFG1) &= ~OFIFG;
            //fail-safe operation
            if(HWREG8(CS_BASE + OFS_CSCTL7) & XT1OFFG)
            {
                //fail-safe for ACLK or XT1 mode is LF
                if(CLKDest == CS_ACLK ||
                   (HWREG16(CS_BASE + OFS_CSCTL6) & XTS) == 0)
                {
                    CLKFrequency = CS_REFOCLK_FREQUENCY;
                }
                //XT1 mode is HF
                else
                {
                    HWREG8(CS_BASE + OFS_CSCTL7) &= ~DCOFFG;
                    CLKFrequency = privateDCORange();
                }
            }
        }
        break;

    case SELMS__VLOCLK:
        CLKFrequency =
            (CS_VLOCLK_FREQUENCY / CLKSourceFrequencyDivider);
        break;
    case SELMS__REFOCLK:
        CLKFrequency =
            (CS_REFOCLK_FREQUENCY / CLKSourceFrequencyDivider);
        break;
    case SELMS__DCOCLKDIV:
        CLKFrequency =
            privateCSSourceClockFromDCO(CLKSource)
            / CLKSourceFrequencyDivider;
        break;
    }

    return (CLKFrequency);
}

static void privateCSComputeDCOFTrim(CS_initFLLParam *param)
{
    uint16_t oldDcoTap = 0xffff;
    uint16_t newDcoTap = 0xffff;
    uint16_t newDcoDelta = 0xffff;
    uint16_t bestDcoDelta = 0xffff;
    uint16_t csCtl0Copy = 0;
    uint16_t csCtl1Copy = 0;
    uint16_t csCtl0Read = 0;
    uint16_t csCtl1Read = 0;
    uint16_t dcoFreqTrim = 3;
    bool endLoop = false;

    do
    {
        HWREG16(CS_BASE + OFS_CSCTL0) = DCO8;   // DCO Tap = 256
        do
        {
            HWREG16(CS_BASE + OFS_CSCTL7) &= ~DCOFFG;   // Clear DCO fault flag
        }
        while(HWREG16(CS_BASE + OFS_CSCTL7) & DCOFFG);

        switch((HWREG16(CS_BASE + OFS_CSCTL1) & DCORSEL_7) >> 1)
        {
        // Wait FLL lock status (FLLUNLOCK) stable
        // Suggest to wait 24 cycles of divided FLL refclk
        case 0:     // 1 MHz
            __delay_cycles((uint16_t)3000);
            break;
        case 1:     // 2 MHz
            __delay_cycles((uint16_t)3000 * 2);
            break;
        case 2:     // 4 MHz
            __delay_cycles((uint16_t)3000 * 4);
            break;
        case 3:     // 8 MHz
            __delay_cycles((uint16_t)3000 * 8);
            break;
        case 4:     // 12 MHz
            __delay_cycles((uint16_t)3000 * 12);
            break;
        case 5:     // 16 MHz
            __delay_cycles((uint16_t)3000 * 16);
            break;
        case 6:     // 20 MHz
            __delay_cycles((uint16_t)3000 * 20);
            break;
        case 7:     // 24 MHz
            __delay_cycles((uint32_t)3000 * 24);
            break;
        default:        // reserved
            __delay_cycles((uint16_t)3000 * 16);
            break;
        }

        // Poll the FLLUNLOCK bits and DCOFFG bit until FLL is locked or DCO fault
        while((HWREG16(CS_BASE + OFS_CSCTL7) & (FLLUNLOCK0 | FLLUNLOCK1)) &&
              ((HWREG16(CS_BASE + OFS_CSCTL7) & DCOFFG) == 0))
        {
            ;
        }

        csCtl0Read = HWREG16(CS_BASE + OFS_CSCTL0); // Read CSCTL0
        csCtl1Read = HWREG16(CS_BASE + OFS_CSCTL1); // Read CSCTL1

        oldDcoTap = newDcoTap;  // Record DCOTAP value of last time
        newDcoTap = csCtl0Read & 0x01ff;    // Get DCOTAP value of this time
        dcoFreqTrim = (csCtl1Read & 0x0070) >> 4;   // Get DCOFTRIM value

        if(newDcoTap < 256) // DCOTAP < 256
        {
            newDcoDelta = 256 - newDcoTap;  // Delta value between DCOTAP and 256
            if((oldDcoTap != 0xffff) && (oldDcoTap >= 256)) // DCOTAP cross 256
            {
                endLoop = true; // Stop while loop
            }
            else
            {
                dcoFreqTrim--;
                HWREG16(CS_BASE +
                        OFS_CSCTL1) =
                    (csCtl1Read & (~0x0070)) | (dcoFreqTrim << 4);
            }
        }
        else   // DCOTAP >= 256
        {
            newDcoDelta = newDcoTap - 256;  // Delta value between DCOTAP and 256
            if(oldDcoTap < 256) // DCOTAP cross 256
            {
                endLoop = true; // Stop while loop
            }
            else
            {
                dcoFreqTrim++;
                HWREG16(CS_BASE +
                        OFS_CSCTL1) =
                    (csCtl1Read & (~0x0070)) | (dcoFreqTrim << 4);
            }
        }

        if(newDcoDelta < bestDcoDelta)  // Record DCOTAP closest to 256
        {
            csCtl0Copy = csCtl0Read;
            csCtl1Copy = csCtl1Read;
            bestDcoDelta = newDcoDelta;
        }
    }
    while(endLoop == false);    // Poll until endLoop == 1

    HWREG16(CS_BASE + OFS_CSCTL0) = csCtl0Copy; // Reload locked DCOTAP
    HWREG16(CS_BASE + OFS_CSCTL1) = csCtl1Copy; // Reload locked DCOFTRIM
    param->csCtl0 = csCtl0Copy;
    param->csCtl1 = csCtl1Copy;
    while(HWREG16(CS_BASE + OFS_CSCTL7) & (FLLUNLOCK0 | FLLUNLOCK1))
    {
        ;                                                               // Poll until FLL is locked
    }
}

void CS_setExternalClockSource(uint32_t XT1CLK_frequency)
{
    privateXT1ClockFrequency = XT1CLK_frequency;
}

void CS_initClockSignal(uint8_t selectedClockSignal,
                        uint16_t clockSource,
                        uint16_t clockSourceDivider)
{
    uint16_t temp;
    switch(selectedClockSignal)
    {
    case CS_ACLK:

        HWREG16(CS_BASE + OFS_CSCTL4) &= ~(SELA);

        if(clockSource == CS_XT1CLK_SELECT)
        {
            clockSource = 0x0;
        }
        else if(clockSource == CS_REFOCLK_SELECT)
        {
            clockSource = 0x1;
        }
        else if(clockSource == CS_VLOCLK_SELECT)
        {
            clockSource = 0x2;
        }
        clockSource = clockSource << 8;

        HWREG16(CS_BASE + OFS_CSCTL4) |= (clockSource);
#ifdef DIVA0
        if(HWREG16(CS_BASE + OFS_CSCTL6) & XTS)
        {
            temp = HWREG16(CS_BASE + OFS_CSCTL6);
            if(clockSourceDivider != CS_CLOCK_DIVIDER_1)
            {
                clockSourceDivider = (clockSourceDivider - 3) << 8;
                HWREG16(CS_BASE +
                        OFS_CSCTL6) |= temp & ~(DIVA3 | DIVA2 | DIVA1 | DIVA0)
                                       | clockSourceDivider;
            }
        }
#endif
        break;
    case CS_SMCLK:

        HWREG16(CS_BASE + OFS_CSCTL4) &= ~(SELMS_7);
        HWREG16(CS_BASE + OFS_CSCTL4) |= (clockSource);

        temp = HWREG16(CS_BASE + OFS_CSCTL5);
        clockSourceDivider = clockSourceDivider << 4;
        HWREG16(CS_BASE + OFS_CSCTL5) = temp & ~(DIVS_3) | clockSourceDivider;
        break;
    case CS_MCLK:

        HWREG16(CS_BASE + OFS_CSCTL4) &= ~(SELMS_7);
        HWREG16(CS_BASE + OFS_CSCTL4) |= (clockSource);

        temp = HWREG16(CS_BASE + OFS_CSCTL5);
        HWREG16(CS_BASE + OFS_CSCTL5) = temp & ~(DIVM_7) | clockSourceDivider;
        break;
    case CS_FLLREF:

        HWREG8(CS_BASE + OFS_CSCTL3) &= ~(SELREF_3);

        if(clockSource == CS_XT1CLK_SELECT)
        {
            clockSource = 0x0;
        }
        clockSource = clockSource << 4;
        HWREG8(CS_BASE + OFS_CSCTL3) |= (clockSource);

        temp = HWREG8(CS_BASE + OFS_CSCTL3);
        //Note that dividers for FLLREF are slightly different
        //Hence handled differently from other CLK signals
        if(clockSourceDivider != CS_CLOCK_DIVIDER_1)
        {
            if(clockSourceDivider == CS_CLOCK_DIVIDER_640)
            {
                HWREG8(CS_BASE +
                       OFS_CSCTL3) = temp & ~(FLLREFDIV_7) |
                                     (clockSourceDivider - 10);
            }
            else if(clockSourceDivider != CS_CLOCK_DIVIDER_512)
            {
                HWREG8(CS_BASE +
                       OFS_CSCTL3) = temp & ~(FLLREFDIV_7) |
                                     (clockSourceDivider - 4);
            }
            else
            {
                HWREG8(CS_BASE +
                       OFS_CSCTL3) = temp & ~(FLLREFDIV_7) |
                                     (clockSourceDivider - 5);
            }
        }
        break;
    }
}

void CS_turnOnXT1LF(uint16_t xt1Drive){
    //Switch ON XT1 oscillator
    HWREG16(CS_BASE + OFS_CSCTL6) &= ~XT1AUTOOFF;

    //Highest drive setting for turnOnXT1
    HWREG16(CS_BASE + OFS_CSCTL6_L) |= XT1DRIVE1_L | XT1DRIVE0_L;

    //Enable LF mode and clear bypass
    HWREG16(CS_BASE + OFS_CSCTL6) &= ~(XTS | XT1BYPASS);

    while(HWREG8(CS_BASE + OFS_CSCTL7) & XT1OFFG)
    {
        //Clear OSC fault flag
        HWREG8(CS_BASE + OFS_CSCTL7) &= ~(XT1OFFG);

        //Clear OFIFG fault flag
        HWREG8(SFR_BASE + OFS_SFRIFG1) &= ~OFIFG;
    }

    //set requested Drive mode
    HWREG16(CS_BASE + OFS_CSCTL6) = (HWREG16(CS_BASE + OFS_CSCTL6) &
                                     ~(XT1DRIVE_3)
                                     ) |
                                    (xt1Drive);
}

void CS_bypassXT1(void)
{
    //Enable HF/LF mode
    HWREG16(CS_BASE + OFS_CSCTL6) &= ~XTS;

    //Switch OFF XT1 oscillator and enable BYPASS mode
    HWREG16(CS_BASE + OFS_CSCTL6) |= (XT1BYPASS | XT1AUTOOFF);

    while(HWREG8(CS_BASE + OFS_CSCTL7) & (XT1OFFG))
    {
        //Clear OSC fault flags
        HWREG8(CS_BASE + OFS_CSCTL7) &= ~(XT1OFFG);

        // Clear the global fault flag. In case the XT1 caused the global fault
        // flag to get set this will clear the global error condition. If any
        // error condition persists, global flag will get again.
        HWREG8(SFR_BASE + OFS_SFRIFG1) &= ~OFIFG;
    }
}

bool CS_turnOnXT1LFWithTimeout(uint16_t xt1Drive,
                               uint16_t timeout)
{
    //Switch ON XT1 oscillator
    HWREG16(CS_BASE + OFS_CSCTL6) &= ~XT1AUTOOFF;

    //Highest drive setting for turnOnXT1
    HWREG16(CS_BASE + OFS_CSCTL6_L) |= XT1DRIVE1_L | XT1DRIVE0_L;

    //Enable LF mode and clear bypass
    HWREG16(CS_BASE + OFS_CSCTL6) &= ~(XTS | XT1BYPASS);

    do
    {
        HWREG8(CS_BASE + OFS_CSCTL7) &= ~(XT1OFFG);

        //Clear OFIFG fault flag
        HWREG8(SFR_BASE + OFS_SFRIFG1) &= ~OFIFG;
    }
    while((HWREG8(CS_BASE + OFS_CSCTL7) & XT1OFFG) && --timeout);

    if(timeout)
    {
        //set requested Drive mode
        HWREG16(CS_BASE + OFS_CSCTL6) = (HWREG16(CS_BASE + OFS_CSCTL6) &
                                         ~(XT1DRIVE_3)
                                         ) |
                                        (xt1Drive);

        return (STATUS_SUCCESS);
    }
    else
    {
        return (STATUS_FAIL);
    }
}

bool CS_bypassXT1WithTimeout(uint16_t timeout)
{
    //Enable HF/LF mode
    HWREG16(CS_BASE + OFS_CSCTL6) &= ~XTS;

    //Switch OFF XT1 oscillator  and enable bypass
    HWREG16(CS_BASE + OFS_CSCTL6) |= (XT1BYPASS | XT1AUTOOFF);

    do
    {
        //Clear OSC fault flags
        HWREG8(CS_BASE + OFS_CSCTL7) &= ~(XT1OFFG);

        // Clear the global fault flag. In case the XT1 caused the global fault
        // flag to get set this will clear the global error condition. If any
        // error condition persists, global flag will get again.
        HWREG8(SFR_BASE + OFS_SFRIFG1) &= ~OFIFG;
    }
    while((HWREG8(CS_BASE + OFS_CSCTL7) & (XT1OFFG)) && --timeout);

    if(timeout)
    {
        return (STATUS_SUCCESS);
    }
    else
    {
        return (STATUS_FAIL);
    }
}

void CS_turnOffXT1(void)
{
    //Switch off XT1 oscillator
    HWREG16(CS_BASE + OFS_CSCTL6) |= XT1AUTOOFF;
}

void CS_turnOnXT1HF(uint16_t xt1Drive,
                    uint16_t xt1HFFreq){
#ifdef XT1HFFREQ_3
    //Switch ON XT1 oscillator
    HWREG16(CS_BASE + OFS_CSCTL6) &= ~XT1AUTOOFF;

    //Enable HF and highest drive setting for XT1
    HWREG16(CS_BASE + OFS_CSCTL6_L) |= XTS | XT1DRIVE1_L | XT1DRIVE0_L;

    //Clear bypass
    HWREG16(CS_BASE + OFS_CSCTL6) &= ~XT1BYPASS;

    while(HWREG8(CS_BASE + OFS_CSCTL7) & XT1OFFG)
    {
        //Clear OSC fault flag
        HWREG8(CS_BASE + OFS_CSCTL7) &= ~(XT1OFFG);

        //Clear OFIFG fault flag
        HWREG8(SFR_BASE + OFS_SFRIFG1) &= ~OFIFG;
    }

    //set requested Drive mode
    HWREG16(CS_BASE + OFS_CSCTL6) = (HWREG16(CS_BASE + OFS_CSCTL6) &
                                     ~(XT1DRIVE_3 | XT1HFFREQ_3)) |
                                    xt1Drive | xt1HFFreq;
#endif
}

bool CS_turnOnXT1HFWithTimeout(uint16_t xt1Drive,
                               uint16_t xt1HFFreq,
                               uint16_t timeout)
{
#ifdef XT1HFFREQ_3
    //Switch ON XT1 oscillator
    HWREG16(CS_BASE + OFS_CSCTL6) &= ~XT1AUTOOFF;

    //Enable HF and highest drive setting for XT1
    HWREG16(CS_BASE + OFS_CSCTL6_L) |= XTS | XT1DRIVE1_L | XT1DRIVE0_L;

    //Clear bypass
    HWREG16(CS_BASE + OFS_CSCTL6) &= ~XT1BYPASS;

    do
    {
        HWREG8(CS_BASE + OFS_CSCTL7) &= ~(XT1OFFG);

        //Clear OFIFG fault flag
        HWREG8(SFR_BASE + OFS_SFRIFG1) &= ~OFIFG;
    }
    while((HWREG8(CS_BASE + OFS_CSCTL7) & XT1OFFG) && --timeout);

    if(timeout)
    {
        //set requested Drive mode
        HWREG16(CS_BASE + OFS_CSCTL6) = (HWREG16(CS_BASE + OFS_CSCTL6) &
                                         ~(XT1DRIVE_3 | XT1HFFREQ_3)) |
                                        xt1Drive | xt1HFFreq;

        return (STATUS_SUCCESS);
    }
    else
    {
        return (STATUS_FAIL);
    }
#else
    return (STATUS_FAIL);
#endif
}

void CS_turnOnSMCLK(void)
{
    //Turn on SMCLK
    HWREG16(CS_BASE + OFS_CSCTL5) &= ~SMCLKOFF;
}

void CS_turnOffSMCLK(void)
{
    //Turn off SMCLK
    HWREG16(CS_BASE + OFS_CSCTL5) |= SMCLKOFF;
}

void CS_enableVLOAutoOff(void)
{
    //Enable VLO Auto Off
    HWREG16(CS_BASE + OFS_CSCTL5) |= VLOAUTOOFF;
}

void CS_disableVLOAutoOff(void)
{
    //Disable VLO Auto Off
    HWREG16(CS_BASE + OFS_CSCTL5) &= ~VLOAUTOOFF;
}

bool CS_initFLLSettle(uint16_t fsystem,
                      uint16_t ratio)
{
    volatile uint16_t x = ratio * 32;

    bool status = CS_initFLL(fsystem, ratio);

    while(x--)
    {
        __delay_cycles(30);
    }

    return(status);
}

bool CS_initFLL(uint16_t fsystem,
                uint16_t ratio)
{
    uint16_t dco_FLLN, dco_FLLD = FLLD__1;
    bool status = true;

    //Save actual state of FLL loop control, then disable it. This is needed to
    //prevent the FLL from acting as we are making fundamental modifications to
    //the clock setup.
    uint16_t srRegisterState = __get_SR_register() & SCG0;

    //Have at least a divider of 2
    dco_FLLN = ratio;

    // Disable FLL
    __bis_SR_register(SCG0);

    //Set DCO to lowest Tap
    HWREG16(CS_BASE + OFS_CSCTL0) &= ~(DCO8 |
                                       DCO7 |
                                       DCO6 |
                                       DCO5 |
                                       DCO4 |
                                       DCO3 |
                                       DCO2 |
                                       DCO1 |
                                       DCO0
                                       );

    //Reset FLLN bits
    HWREG16(CS_BASE + OFS_CSCTL2) &= ~(FLLN9 |
                                       FLLN8 |
                                       FLLN7 |
                                       FLLN6 |
                                       FLLN5 |
                                       FLLN4 |
                                       FLLN3 |
                                       FLLN2 |
                                       FLLN1 |
                                       FLLN0
                                       );
    HWREG16(CS_BASE + OFS_CSCTL2) = dco_FLLD | (dco_FLLN - 1);

    HWREG8(CS_BASE + OFS_CSCTL1) &= ~DCORSEL_7;
    if(fsystem <= 1000)             //fsystem <= 1MHz
    {
        HWREG8(CS_BASE + OFS_CSCTL1) |= DCORSEL_0;
    }
    else if(fsystem <= 2000)         //1MHz < fsystem <= 2MHz
    {
        HWREG8(CS_BASE + OFS_CSCTL1) |= DCORSEL_1;
    }
    else if(fsystem <= 4000)         //2MHz < fsystem <=  4MHz
    {
        HWREG8(CS_BASE + OFS_CSCTL1) |= DCORSEL_2;
    }
    else if(fsystem <= 8000)         //4MHz  < fsystem <=  8MHz
    {
        HWREG8(CS_BASE + OFS_CSCTL1) |= DCORSEL_3;
    }
    else if(fsystem <= 12000)        //8MHz    < fsystem <= 12MHz
    {
        HWREG8(CS_BASE + OFS_CSCTL1) |= DCORSEL_4;
    }
    else if(fsystem <= 16000)        //12MHz   < fsystem <=  16MHz
    {
        HWREG8(CS_BASE + OFS_CSCTL1) |= DCORSEL_5;
    }
    else if(fsystem <= 20000)       //16MHz    < fsystem <=  20MHz
    {
        HWREG8(CS_BASE + OFS_CSCTL1) |= DCORSEL_6;
    }
    else if(fsystem <= 24000)       //20MHz    < fsystem <=  24MHz
    {
        HWREG8(CS_BASE + OFS_CSCTL1) |= DCORSEL_7;
    }
    else
    {
        //exceeds 24MHz, not supported
        status = false;
    }

    // Re-enable FLL
    __bic_SR_register(SCG0);

    while((HWREG16(CS_BASE + OFS_CSCTL7) & (FLLUNLOCK0 | FLLUNLOCK1)) ||
          (HWREG8(CS_BASE + OFS_CSCTL7_L) & DCOFFG))
    {
        //Clear OSC fault flags
        HWREG8(CS_BASE + OFS_CSCTL7_L) &= ~(DCOFFG);

        //Clear OFIFG fault flag
        HWREG8(SFR_BASE + OFS_SFRIFG1) &= ~OFIFG;
    }

    // Restore previous SCG0
    __bis_SR_register(srRegisterState);

    return(status);
}

bool CS_initFLLCalculateTrim(uint16_t fsystem,
                             uint16_t ratio,
                             CS_initFLLParam *param)
{
    uint16_t dco_FLLN, dco_FLLD = FLLD__1;
    bool status = true;
    volatile uint16_t x = ratio * 32;

    //Save actual state of FLL loop control, then disable it. This is needed to
    //prevent the FLL from acting as we are making fundamental modifications to
    //the clock setup.
    uint16_t srRegisterState = __get_SR_register() & SCG0;

    //Have at least a divider of 2
    dco_FLLN = ratio;

    // Disable FLL
    __bis_SR_register(SCG0);

    //Set DCO to lowest Tap
    HWREG16(CS_BASE + OFS_CSCTL0) &= ~(DCO8 |
                                       DCO7 |
                                       DCO6 |
                                       DCO5 |
                                       DCO4 |
                                       DCO3 |
                                       DCO2 |
                                       DCO1 |
                                       DCO0
                                       );
    //Reset FLLN bits
    HWREG16(CS_BASE + OFS_CSCTL2) &= ~(FLLN9 |
                                       FLLN8 |
                                       FLLN7 |
                                       FLLN6 |
                                       FLLN5 |
                                       FLLN4 |
                                       FLLN3 |
                                       FLLN2 |
                                       FLLN1 |
                                       FLLN0
                                       );
    HWREG16(CS_BASE + OFS_CSCTL2) = dco_FLLD | (dco_FLLN - 1);

    HWREG8(CS_BASE + OFS_CSCTL1) &= ~DCORSEL_7;
    if(fsystem <= 1000)             //fsystem <= 1MHz
    {
        HWREG8(CS_BASE + OFS_CSCTL1) |= DCORSEL_0;
    }
    else if(fsystem <= 2000)         //1MHz < fsystem <= 2MHz
    {
        HWREG8(CS_BASE + OFS_CSCTL1) |= DCORSEL_1;
    }
    else if(fsystem <= 4000)         //2MHz < fsystem <=  4MHz
    {
        HWREG8(CS_BASE + OFS_CSCTL1) |= DCORSEL_2;
    }
    else if(fsystem <= 8000)         //4MHz  < fsystem <=  8MHz
    {
        HWREG8(CS_BASE + OFS_CSCTL1) |= DCORSEL_3;
    }
    else if(fsystem <= 12000)        //8MHz    < fsystem <= 12MHz
    {
        HWREG8(CS_BASE + OFS_CSCTL1) |= DCORSEL_4;
    }
    else if(fsystem <= 16000)        //12MHz   < fsystem <=  16MHz
    {
        HWREG8(CS_BASE + OFS_CSCTL1) |= DCORSEL_5;
    }
    else if(fsystem <= 20000)        //16MHz   < fsystem <= 20MHz
    {
        HWREG8(CS_BASE + OFS_CSCTL1) |= DCORSEL_6;
    }
    else if(fsystem <= 24000)        //20MHz   < fsystem <=  24MHz
    {
        HWREG8(CS_BASE + OFS_CSCTL1) |= DCORSEL_7;
    }
    else
    {
        //exceeds 24MHz, not supported
        status = false;
    }

    // Re-enable FLL
    __bic_SR_register(SCG0);

    // Enable DCO frequency trim
    HWREG16(CS_BASE + OFS_CSCTL1) |= DCOFTRIMEN;

    // Calculates DCO frequency trim values and stores them in struct pointer
    param->fsystem = fsystem;
    privateCSComputeDCOFTrim(param);

    while((HWREG16(CS_BASE + OFS_CSCTL7) & (FLLUNLOCK0 | FLLUNLOCK1)) ||
          (HWREG8(CS_BASE + OFS_CSCTL7_L) & DCOFFG))
    {
        //Clear OSC fault flags
        HWREG8(CS_BASE + OFS_CSCTL7_L) &= ~(DCOFFG);

        //Clear OFIFG fault flag
        HWREG8(SFR_BASE + OFS_SFRIFG1) &= ~OFIFG;
    }

    // Restore previous SCG0
    __bis_SR_register(srRegisterState);

    while(x--)
    {
        __delay_cycles(30);
    }

    return(status);
}

bool CS_initFLLLoadTrim(uint16_t fsystem,
                        uint16_t ratio,
                        CS_initFLLParam *param)
{
    if(param->fsystem != fsystem)
    {
        // Protection against wrong clock frequency and trim combination
        return(false);
    }

    uint16_t dco_FLLN, dco_FLLD = FLLD__1;
    bool status = true;
    volatile uint16_t x = ratio * 32;

    //Save actual state of FLL loop control, then disable it. This is needed to
    //prevent the FLL from acting as we are making fundamental modifications to
    //the clock setup.
    uint16_t srRegisterState = __get_SR_register() & SCG0;

    //Have at least a divider of 2
    dco_FLLN = ratio;

    // Disable FLL
    __bis_SR_register(SCG0);

    //Set DCO to proper tap
    HWREG16(CS_BASE + OFS_CSCTL0) &= ~(DCO8 |
                                       DCO7 |
                                       DCO6 |
                                       DCO5 |
                                       DCO4 |
                                       DCO3 |
                                       DCO2 |
                                       DCO1 |
                                       DCO0
                                       );
    HWREG16(CS_BASE + OFS_CSCTL0) |= (param->csCtl0 &
                                      (DCO8 |
                                       DCO7 |
                                       DCO6 |
                                       DCO5 |
                                       DCO4 |
                                       DCO3 |
                                       DCO2 |
                                       DCO1 |
                                       DCO0
                                      ));

    //Reset FLLN bits
    HWREG16(CS_BASE + OFS_CSCTL2) &= ~(FLLN9 |
                                       FLLN8 |
                                       FLLN7 |
                                       FLLN6 |
                                       FLLN5 |
                                       FLLN4 |
                                       FLLN3 |
                                       FLLN2 |
                                       FLLN1 |
                                       FLLN0
                                       );
    HWREG16(CS_BASE + OFS_CSCTL2) = dco_FLLD | (dco_FLLN - 1);

    // Set proper DCORSEL value
    HWREG8(CS_BASE + OFS_CSCTL1) &= ~DCORSEL_7;
    if(fsystem <= 1000)             //fsystem <= 1MHz
    {
        HWREG8(CS_BASE + OFS_CSCTL1) |= DCORSEL_0;
    }
    else if(fsystem <= 2000)         //1MHz < fsystem <= 2MHz
    {
        HWREG8(CS_BASE + OFS_CSCTL1) |= DCORSEL_1;
    }
    else if(fsystem <= 4000)         //2MHz < fsystem <=  4MHz
    {
        HWREG8(CS_BASE + OFS_CSCTL1) |= DCORSEL_2;
    }
    else if(fsystem <= 8000)         //4MHz  < fsystem <=  8MHz
    {
        HWREG8(CS_BASE + OFS_CSCTL1) |= DCORSEL_3;
    }
    else if(fsystem <= 12000)        //8MHz    < fsystem <= 12MHz
    {
        HWREG8(CS_BASE + OFS_CSCTL1) |= DCORSEL_4;
    }
    else if(fsystem <= 16000)        //12MHz   < fsystem <=  16MHz
    {
        HWREG8(CS_BASE + OFS_CSCTL1) |= DCORSEL_5;
    }
    else if(fsystem <= 20000)        //16MHz   < fsystem <= 20MHz
    {
        HWREG8(CS_BASE + OFS_CSCTL1) |= DCORSEL_6;
    }
    else if(fsystem <= 24000)        //20MHz   < fsystem <=  24MHz
    {
        HWREG8(CS_BASE + OFS_CSCTL1) |= DCORSEL_7;
    }
    else
    {
        //exceeds 24MHz, not supported
        status = false;
    }

    // Enable DCO frequency trim
    HWREG16(CS_BASE + OFS_CSCTL1) |= DCOFTRIMEN;

    // Set proper DCOFTRIM value
    HWREG16(CS_BASE + OFS_CSCTL1) &= ~(DCOFTRIM0 | DCOFTRIM1 | DCOFTRIM2);
    HWREG16(CS_BASE +
            OFS_CSCTL1) |=
        (param->csCtl1 & (DCOFTRIM0 | DCOFTRIM1 | DCOFTRIM2));

    // Re-enable FLL
    __bic_SR_register(SCG0);

    while((HWREG16(CS_BASE + OFS_CSCTL7) & (FLLUNLOCK0 | FLLUNLOCK1)) ||
          (HWREG8(CS_BASE + OFS_CSCTL7_L) & DCOFFG))
    {
        //Clear OSC fault flags
        HWREG8(CS_BASE + OFS_CSCTL7_L) &= ~(DCOFFG);

        //Clear OFIFG fault flag
        HWREG8(SFR_BASE + OFS_SFRIFG1) &= ~OFIFG;
    }

    // Restore previous SCG0
    __bis_SR_register(srRegisterState);

    while(x--)
    {
        __delay_cycles(30);
    }

    return(status);
}

void CS_enableClockRequest(uint8_t selectClock)
{
    HWREG8(CS_BASE + OFS_CSCTL8) |= selectClock;
}

void CS_disableClockRequest(uint8_t selectClock)
{
    HWREG8(CS_BASE + OFS_CSCTL8) &= ~selectClock;
}

uint8_t CS_getFaultFlagStatus(uint8_t mask)
{
    return (HWREG8(CS_BASE + OFS_CSCTL7) & mask);
}

void CS_clearFaultFlag(uint8_t mask)
{
    HWREG8(CS_BASE + OFS_CSCTL7) &= ~mask;
}

uint32_t CS_getACLK(void)
{
    //Find ACLK source
    uint16_t ACLKSource = (HWREG16(CS_BASE + OFS_CSCTL4) & SELA);

    ACLKSource = ACLKSource >> 8;

    if(ACLKSource == 0x0)
    {
        ACLKSource = SELMS__XT1CLK;
    }
    else if(ACLKSource == 0x1)
    {
        ACLKSource = SELMS__REFOCLK;
    }
    else
    {
        ACLKSource = SELMS__VLOCLK;
    }

    uint16_t ACLKSourceDivider = 0;
#ifdef DIVA0
    if(HWREG16(CS_BASE + OFS_CSCTL6) & XTS)
    {
        uint16_t div = (HWREG16(CS_BASE + OFS_CSCTL6) &
                        (DIVA0 | DIVA1 | DIVA2 | DIVA3)) >> 8;
        switch(div)
        {
        case 1:
        case 2:
        case 3:
        case 4:
        case 5:
            ACLKSourceDivider = 8 * (1 << div);
            break;
        case 6:
            ACLKSourceDivider = 384;
            break;
        case 7:
            ACLKSourceDivider = 512;
            break;
        case 8:
            ACLKSourceDivider = 768;
            break;
        case 9:
            ACLKSourceDivider = 1024;
            break;
        case 10:
            ACLKSourceDivider = 108;
            break;
        case 11:
            ACLKSourceDivider = 338;
            break;
        case 12:
            ACLKSourceDivider = 414;
            break;
        case 13:
            ACLKSourceDivider = 640;
            break;
        default:
            break;
        }
    }
#endif
    return (privateCSComputeCLKFrequency(
                ACLKSource,
                ACLKSourceDivider,
                CS_ACLK)
            );
}

uint32_t CS_getSMCLK(void)
{
    uint16_t SMCLKSource = HWREG8(CS_BASE + OFS_CSCTL4_L) & SELMS_7;

    uint16_t SMCLKSourceDivider =
        HWREG16(CS_BASE + OFS_CSCTL5) & DIVS_3;
    SMCLKSourceDivider = SMCLKSourceDivider >> 4;

    return (privateCSComputeCLKFrequency(
                SMCLKSource,
                SMCLKSourceDivider,
                CS_SMCLK)
            );
}

uint32_t CS_getMCLK(void)
{
    //Find AMCLK source
    uint16_t MCLKSource = (HWREG16(CS_BASE + OFS_CSCTL4) & SELMS_7);

    uint16_t MCLKSourceDivider = HWREG16(CS_BASE + OFS_CSCTL5) & DIVM_7;

    return (privateCSComputeCLKFrequency(
                MCLKSource,
                MCLKSourceDivider,
                CS_MCLK)
            );
}

uint16_t CS_clearAllOscFlagsWithTimeout(uint16_t timeout){
    do
    {
        // Clear all osc fault flags
        HWREG8(CS_BASE + OFS_CSCTL7) &= ~(DCOFFG | XT1OFFG);

        // Clear the global osc fault flag.
        HWREG8(SFR_BASE + OFS_SFRIFG1) &= ~OFIFG;

        // Check XT1 fault flags
    }
    while((HWREG8(SFR_BASE + OFS_SFRIFG1) & OFIFG) && --timeout);

    return (HWREG8(CS_BASE + OFS_CSCTL7) & (DCOFFG | XT1OFFG));
}

void CS_enableXT1AutomaticGainControl(void)
{
    HWREG16(CS_BASE + OFS_CSCTL6) &= ~XT1AGCOFF;
}

void CS_disableXT1AutomaticGainControl(void)
{
    HWREG16(CS_BASE + OFS_CSCTL6) |= XT1AGCOFF;
}

void CS_enableFLLUnlock(void)
{
    HWREG16(CS_BASE + OFS_CSCTL7) |= FLLULPUC;
}

void CS_disableFLLUnlock(void)
{
    HWREG16(CS_BASE + OFS_CSCTL7) &= ~FLLULPUC;
}

void CS_enableREFOLP(void)
{
#ifdef REFOLP
    HWREG16(CS_BASE + OFS_CSCTL3) |= REFOLP;
#endif
}

void CS_disableREFOLP(void)
{
#ifdef REFOLP
    HWREG16(CS_BASE + OFS_CSCTL3) &= ~REFOLP;
#endif
}

bool CS_getREFOLP(void)
{
#ifdef REFOLP
    return ((HWREG16(CS_BASE + OFS_CSCTL3) & REFOLP) ? true : false);
#else
    return(false);
#endif
}

void CS_enableXT1FaultOff(void)
{
#ifdef XT1FAULTOFF
    HWREG16(CS_BASE + OFS_CSCTL6) |= XT1FAULTOFF;
#endif
}

void CS_disableXT1FaultOff(void)
{
#ifdef XT1FAULTOFF
    HWREG16(CS_BASE + OFS_CSCTL6) &= ~XT1FAULTOFF;
#endif
}

bool CS_getXT1FaultOff(void)
{
#ifdef XT1FAULTOFF
    return ((HWREG16(CS_BASE + OFS_CSCTL6) & XT1FAULTOFF) ? true : false);
#else
    return(false);
#endif
}

bool CS_getREFOReady(void)
{
#ifdef REFOREADY
    return ((HWREG16(CS_BASE + OFS_CSCTL7) & REFOREADY) ? true : false);
#else
    return(false);
#endif
}

#endif
//*****************************************************************************
//
//! Close the doxygen group for cs_api
//! @}
//
//*****************************************************************************