/*==================================================================================================
*   Project              : RTD AUTOSAR 4.4
*   Platform             : CORTEXM
*   Peripheral           : 
*   Dependencies         : none
*
*   Autosar Version      : 4.4.0
*   Autosar Revision     : ASR_REL_4_4_REV_0000
*   Autosar Conf.Variant :
*   SW Version           : 1.0.0
*   Build Version        : S32K3_RTD_1_0_0_HF02_D2112_ASR_REL_4_4_REV_0000_20211222
*
*   (c) Copyright 2020 - 2021 NXP Semiconductors
*   All Rights Reserved.
*
*   NXP Confidential. This software is owned or controlled by NXP and may only be
*   used strictly in accordance with the applicable license terms. By expressly
*   accepting such terms or by downloading, installing, activating and/or otherwise
*   using the software, you are agreeing that you have read, and that you agree to
*   comply with and are bound by, such license terms. If you do not agree to be
*   bound by the applicable license terms, then you may not retain, install,
*   activate or otherwise use the software.
==================================================================================================*/

/**
*   @file       Clock_Ip_ProgFreqSwitch.c
*   @version    1.0.0
*
*   @brief   CLOCK driver implementations.
*   @details CLOCK driver implementations.
*
*   @addtogroup CLOCK_DRIVER Clock Ip Driver
*   @{
*/


#ifdef __cplusplus
extern "C"{
#endif


/*==================================================================================================
*                                          INCLUDE FILES
* 1) system and project includes
* 2) needed interfaces from external units
* 3) internal and external interfaces from this unit
==================================================================================================*/

#include "Clock_Ip_Private.h"

/*==================================================================================================
                               SOURCE FILE VERSION INFORMATION
==================================================================================================*/
#define CLOCK_IP_PROGFREQSWITCH_VENDOR_ID_C                      43
#define CLOCK_IP_PROGFREQSWITCH_AR_RELEASE_MAJOR_VERSION_C       4
#define CLOCK_IP_PROGFREQSWITCH_AR_RELEASE_MINOR_VERSION_C       4
#define CLOCK_IP_PROGFREQSWITCH_AR_RELEASE_REVISION_VERSION_C    0
#define CLOCK_IP_PROGFREQSWITCH_SW_MAJOR_VERSION_C               1
#define CLOCK_IP_PROGFREQSWITCH_SW_MINOR_VERSION_C               0
#define CLOCK_IP_PROGFREQSWITCH_SW_PATCH_VERSION_C               0

/*==================================================================================================
*                                     FILE VERSION CHECKS
==================================================================================================*/
/* Check if Clock_Ip_ProgFreqSwitch.c file and Clock_Ip_Private.h file are of the same vendor */
#if (CLOCK_IP_PROGFREQSWITCH_VENDOR_ID_C != CLOCK_IP_PRIVATE_VENDOR_ID)
    #error "Clock_Ip_ProgFreqSwitch.c and Clock_Ip_Private.h have different vendor ids"
#endif

/* Check if Clock_Ip_ProgFreqSwitch.c file and Clock_Ip_Private.h file are of the same Autosar version */
#if ((CLOCK_IP_PROGFREQSWITCH_AR_RELEASE_MAJOR_VERSION_C != CLOCK_IP_PRIVATE_AR_RELEASE_MAJOR_VERSION) || \
     (CLOCK_IP_PROGFREQSWITCH_AR_RELEASE_MINOR_VERSION_C != CLOCK_IP_PRIVATE_AR_RELEASE_MINOR_VERSION) || \
     (CLOCK_IP_PROGFREQSWITCH_AR_RELEASE_REVISION_VERSION_C != CLOCK_IP_PRIVATE_AR_RELEASE_REVISION_VERSION) \
    )
    #error "AutoSar Version Numbers of Clock_Ip_ProgFreqSwitch.c and Clock_Ip_Private.h are different"
#endif

/* Check if Clock_Ip_ProgFreqSwitch.c file and Clock_Ip_Private.h file are of the same Software version */
#if ((CLOCK_IP_PROGFREQSWITCH_SW_MAJOR_VERSION_C != CLOCK_IP_PRIVATE_SW_MAJOR_VERSION) || \
     (CLOCK_IP_PROGFREQSWITCH_SW_MINOR_VERSION_C != CLOCK_IP_PRIVATE_SW_MINOR_VERSION) || \
     (CLOCK_IP_PROGFREQSWITCH_SW_PATCH_VERSION_C != CLOCK_IP_PRIVATE_SW_PATCH_VERSION) \
    )
    #error "Software Version Numbers of Clock_Ip_ProgFreqSwitch.c and Clock_Ip_Private.h are different"
#endif

/*==================================================================================================
*                           LOCAL TYPEDEFS (STRUCTURES, UNIONS, ENUMS)
==================================================================================================*/

/*==================================================================================================
*                                          LOCAL MACROS
==================================================================================================*/
#ifdef CLOCK_IP_CGM_X_PCFS_SDUR_DIVC_DIVE_DIVS
/* Pcfs settings that are dependent on device */
#define CLOCK_IP_A_MAX_SIZE 6U

/* microA per MHz */
#define CLOCK_IP_DYNAMIC_IDD_CHANGE 2360U

#define CLOCK_IP_DIVIDE_BY_1000000                              1000000U
#define CLOCK_IP_DIVIDE_BY_100000                               100000U
#define CLOCK_IP_CONSTANT_2048000                               (1024U * 2000U)
#endif

/*==================================================================================================
*                                         LOCAL CONSTANTS
==================================================================================================*/

/*==================================================================================================
*                                         LOCAL VARIABLES
==================================================================================================*/

/*==================================================================================================
*                                        GLOBAL VARIABLES
==================================================================================================*/

/*==================================================================================================
*                                    LOCAL FUNCTION PROTOTYPES
==================================================================================================*/
/* Clock start section code */
#define MCU_START_SEC_CODE

#include "Mcu_MemMap.h"


static void Clock_Ip_ProgressiveFrequencyClockSwitchEmpty(Clock_Ip_PcfsConfigType const* Config);
#ifdef CLOCK_IP_CGM_X_PCFS_SDUR_SDUR
static void Clock_Ip_CgmXPcfsSdurSdur(Clock_Ip_PcfsConfigType const *Config);
#endif
#ifdef CLOCK_IP_CGM_X_PCFS_SDUR_DIVC_DIVE_DIVS
static void Clock_Ip_CgmXPcfsSdurDivcDiveDivs(Clock_Ip_PcfsConfigType const *Config);
#endif

/* Clock stop section code */
#define MCU_STOP_SEC_CODE

#include "Mcu_MemMap.h"
/*==================================================================================================
*                                         LOCAL FUNCTIONS
==================================================================================================*/


/* Clock start section code */
#define MCU_START_SEC_CODE

#include "Mcu_MemMap.h"

static void Clock_Ip_ProgressiveFrequencyClockSwitchEmpty(Clock_Ip_PcfsConfigType const* Config)
{
    (void)Config;
    /* No implementation */
}


#ifdef CLOCK_IP_CGM_X_PCFS_SDUR_SDUR
static void Clock_Ip_CgmXPcfsSdurSdur(Clock_Ip_PcfsConfigType const *Config)
{
    uint32 Instance      = Clock_Ip_au8ClockFeatures[Config->Name][CLOCK_IP_MODULE_INSTANCE];
    uint32 HwIndex       = Clock_Ip_au8ClockFeatures[Config->Name][PCFS_HW_INDEX];
    uint32 SwIndex       = Clock_Ip_au8ClockFeatures[Config->Name][PCFS_SW_INDEX];
    pcfsEntry *Pcfs      = GetPcfs(SwIndex);

    Clock_Ip_apxCgmPcfs[Instance]->PCFS_SDUR = MC_CGM_PCFS_SDUR_SDUR(Pcfs->Sdur);
    (void)HwIndex;    /* hw index is not used because divc_init, divc_rate, divc_rate, div_startValue, div_endValue registers are not implemented */

    /* No implementation */
}
#endif


#ifdef CLOCK_IP_CGM_X_PCFS_SDUR_DIVC_DIVE_DIVS
static void Clock_Ip_CgmXPcfsSdurDivcDiveDivs(Clock_Ip_PcfsConfigType const *Config)
{
    volatile Clock_Ip_CgmPcfsType* CgmPcfsBase  = Clock_Ip_apxCgmPcfs[Clock_Ip_au8ClockFeatures[Config->SelectorName][CLOCK_IP_MODULE_INSTANCE]];
    uint32 HwIndex                       = Clock_Ip_au8ClockFeatures[Config->Name][CLOCK_IP_PCFS_INDEX];
    static const uint32 AMax[CLOCK_IP_A_MAX_SIZE] = {0U,5U,10U,15U,20U,100U};
    static const uint32 PcfsRate[CLOCK_IP_A_MAX_SIZE] = {0U,12U,48U,112U,184U,1000U};
    static uint32 Hash[CLOCK_IP_CMU_INFO_SIZE];

    uint32 Finput = 0U;
    uint32 Fsafe = 0U;
    uint32 AmaxBrut;
    uint32 Rate = 0U;
    uint32 Index;
    uint32 Var1;
    uint32 Var2;
    uint32 Var3;
    uint32 K;

    uint32 Sdur;
    uint32 DivcInit;
    uint32 DivcRate;
    uint32 DivStartValue;
    uint32 DivEndValue;

#if (defined(CLOCK_IP_DEV_ERROR_DETECT) && (CLOCK_IP_DEV_ERROR_DETECT == STD_ON))
        CLOCK_IP_DEV_ASSERT(Config->SelectorName != RESERVED_CLK);
#endif

    if (Hash[Clock_Ip_au8ClockFeatures[Config->Name][CLOCK_IP_PCFS_INDEX]] != ((((uint32)Config->ClockSourceFrequency) ^ ((uint32)Config->MaxAllowableIDDchange) ^ ((uint32)Config->Name)  ^ ((uint32)Config->SelectorName) ^ ((uint32)Config->StepDuration))))
    {
        Hash[Clock_Ip_au8ClockFeatures[Config->Name][CLOCK_IP_PCFS_INDEX]] = ((((uint32)Config->ClockSourceFrequency) ^ ((uint32)Config->MaxAllowableIDDchange) ^ ((uint32)Config->Name)  ^ ((uint32)Config->SelectorName) ^ ((uint32)Config->StepDuration)));

        Finput = Config->ClockSourceFrequency / CLOCK_IP_DIVIDE_BY_1000000;
    #if defined(CLOCK_IP_HAS_FIRC_CLK)
        Fsafe = Clock_Ip_GetConfiguredFrequencyValue(FIRC_CLK) / CLOCK_IP_DIVIDE_BY_1000000;
    #endif

#if (defined(CLOCK_IP_DEV_ERROR_DETECT) && (CLOCK_IP_DEV_ERROR_DETECT == STD_ON))
        CLOCK_IP_DEV_ASSERT(Finput != 0U);
        CLOCK_IP_DEV_ASSERT(Fsafe != 0U);
        CLOCK_IP_DEV_ASSERT(Config->MaxAllowableIDDchange != 0U);
        CLOCK_IP_DEV_ASSERT(Config->StepDuration != 0U);
#endif

        /* Calculate amax=fchg/Finput */
        AmaxBrut = (Config->MaxAllowableIDDchange * Config->StepDuration * CLOCK_IP_DIVIDE_BY_100000 / (Finput * CLOCK_IP_DYNAMIC_IDD_CHANGE));
        Rate = AmaxBrut;

        /* Round pcfs rate by rounding amax */
        if (AmaxBrut < AMax[0U])
        {
            Rate = PcfsRate[0U];
        }
        else if (AmaxBrut > AMax[CLOCK_IP_A_MAX_SIZE-1U])
        {
            Rate = PcfsRate[CLOCK_IP_A_MAX_SIZE-1U];
        }
        else
        {
            for (Index = 1U; Index < (uint8)CLOCK_IP_A_MAX_SIZE; Index++)
            {
                if (AMax[Index-1U] < AmaxBrut)
                {
                    Rate = PcfsRate[Index];
                }
            }
        }

        /* Calculate K by using formula k = ceil(0.5 + sqrt(0.25 - (2000 * (1 -(Finput/fsafe)) / Rate))) */

        Var1 = 256U + ((CLOCK_IP_CONSTANT_2048000 * Finput) / (Fsafe * Rate)) - (CLOCK_IP_CONSTANT_2048000 / Rate);
        Var2 = 1UL << 30U;   /* The second-to-top bit is set: use 1u << 14 for uint16 type; use 1uL<<30 for uint32 type  */
        Var3 = 0U;

        /* Implement sqrt from K formula by using a square-root computing in embedded C */

        /* "one" starts at the highest power of four <= than the argument */
        while (Var2 > Var1)
        {
            Var2 = Var2 >> 2;
        }
        /* Implement sqrt from K formula by using a square-root computing in embedded C */

        while (Var2 != 0U)
        {
            if (Var1 >= (Var3 + Var2))
            {
                Var1 = Var1 - (Var3 + Var2);
                Var3 = Var3 + (Var2 << 1U);
            }

            Var3 = Var3 >> 1U;
            Var2 = Var2 >> 2U;
        }

        K = (64U + 127U + (Var3 << 2U)) >> 7U;   /* Calculated K from k = ceil(0.5 + sqrt(0.25 - (2000 * (1 -(Fi/Fsafe)) / Rate))) */

        Sdur = Config->StepDuration * Fsafe;
        DivcInit = Rate * K;
        DivcRate = Rate;
        DivStartValue = 999U + ((Rate * K * (K+1U)) >> 1U);
        DivEndValue = (Finput * 1000U / Fsafe) - 1U;

        /* Configure pcfs registers */
        CgmPcfsBase->PCFS_SDUR = MC_CGM_PCFS_SDUR_SDUR(Sdur);
        CgmPcfsBase->PCFS[HwIndex].DIVC = MC_CGM_PCFS_DIVC_RATE(DivcRate) | MC_CGM_PCFS_DIVC_INIT(DivcInit);
        CgmPcfsBase->PCFS[HwIndex].DIVE = MC_CGM_PCFS_DIVE_DIVE(DivEndValue);
        CgmPcfsBase->PCFS[HwIndex].DIVS = MC_CGM_PCFS_DIVS_DIVS(DivStartValue);
    }
}
#endif


/* Clock stop section code */
#define MCU_STOP_SEC_CODE

#include "Mcu_MemMap.h"

/*==================================================================================================
*                                        GLOBAL FUNCTIONS
==================================================================================================*/

/*==================================================================================================
*                                        GLOBAL CONSTANTS
==================================================================================================*/

/* Clock start constant section data */
#define MCU_START_SEC_CONST_UNSPECIFIED

#include "Mcu_MemMap.h"

const Clock_Ip_PcfsCallbackType Clock_Ip_axPcfsCallbacks[CLOCK_IP_PCFS_CALLBACKS_COUNT] =
{
    {
        Clock_Ip_ProgressiveFrequencyClockSwitchEmpty,     /* Set */

    },
#ifdef CLOCK_IP_CGM_X_PCFS_SDUR_SDUR
    {
        Clock_Ip_CgmXPcfsSdurSdur,                          /* Set */
    },
#endif
#ifdef CLOCK_IP_CGM_X_PCFS_SDUR_DIVC_DIVE_DIVS
    {
        Clock_Ip_CgmXPcfsSdurDivcDiveDivs,              /* Set */
    },
#endif
};

/* Clock stop constant section data */
#define MCU_STOP_SEC_CONST_UNSPECIFIED

#include "Mcu_MemMap.h"


#ifdef __cplusplus
}
#endif

/** @} */
