/*================================================================================================*/
/**
*    @file        sys_init.c
*    @version     1.0.0
*
*    @brief       Integration Framework - System initialization code
*     
*    (c) Copyright 2006-2016 Freescale Semiconductor, Inc. 
*       Copyright 2017-2018 NXP
*    All Rights Reserved.
*
*    This file contains sample code only. It is not part of the production code deliverables.
*/       
/*=================================================================================================
*    Platform      :      PA
*    Build Version :      MPC5777CXG_MCAL_1_0_0_RTM_ASR_REL_4_3_REV_0001_20190318
*
==================================================================================================*/
/*==================================================================================================
==================================================================================================*/
#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 "sys_init.h"
#include "Mcal.h"
/*==================================================================================================
*                                        LOCAL MACROS
==================================================================================================*/
#define OP1_KEY              ((uint32)0x913756AFUL)
#define OP2_KEY              ((uint32)0x825A132BUL)
#define TRANS_LOCK_KEY       ((uint32)0x000000BCUL)
#define CORE_SET 0
/*==================================================================================================
*                                      FILE VERSION CHECKS
==================================================================================================*/

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

/*==================================================================================================
*                                       LOCAL CONSTANTS
==================================================================================================*/
#ifdef MCAL_ENABLE_USER_MODE_SUPPORT
volatile   VAR(uint8, MCU_VAR)  u8SwitchToSupervisor = 0U; 
#endif /* MCAL_ENABLE_USER_MODE_SUPPORT */
/*==================================================================================================
*                                       LOCAL VARIABLES
==================================================================================================*/

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

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

/*==================================================================================================
*                                   LOCAL FUNCTION PROTOTYPES
==================================================================================================*/

/*==================================================================================================
*                                       LOCAL FUNCTIONS
==================================================================================================*/
static void sys_enableAllInterrupts_call(void)
{
#ifdef MCAL_ENABLE_USER_MODE_SUPPORT
   Mcal_Trusted_Call(sys_enableAllInterrupts_call);
#else
    __asm(" wrteei 1");
#endif /* MCAL_ENABLE_USER_MODE_SUPPORT */
}

static void sys_disableAllInterrupts_call(void)
{
#ifdef MCAL_ENABLE_USER_MODE_SUPPORT
    Mcal_Trusted_Call(sys_disableAllInterrupts_call);
#else
    __asm(" wrteei 0");
#endif /* MCAL_ENABLE_USER_MODE_SUPPORT */
}
/*==================================================================================================
*                                       GLOBAL FUNCTIONS
==================================================================================================*/
/** 
* @brief Disable all FCCU REACTIONS
* @details Disable all FCCU REACTIONS
*/
/*================================================================================================*/
/** 
* @brief sys_enableAllInterrupts
* @details function used to enable all interrupts
*/
void sys_enableAllInterrupts(void)
{
    ResumeAllInterrupts();
}

/** 
* @brief sys_disableAllInterrupts
* @details function used to disable all interrupts
*/
void sys_disableAllInterrupts(void)
{
    SuspendAllInterrupts();
}
/** 
* @brief sys_enableIsrSource
* @details function used to enable the interrupt number id and set up the priority
*/
void sys_enableIsrSource(uint16 id, uint8 prio)
{
    INTC.PSR[id].R = (0x8000 >> CORE_SET | prio); 
}


/** 
* @brief sys_disableIsrSource
* @details function used to disable the interrupt number id
*/
void sys_disableIsrSource(uint16 id)
{
    INTC.PSR[id].R = 0x0000; 
}

static void Sys_FCCU_Disable_Reactions(void) 
{
    VAR(uint32, AUTOMATIC) tempNCFS0, tempStatus;
    
   
    /* set up the key to clear the status registers*/
    FCCU.CTRL.B.OPR = 3;
    while(FCCU.CTRL.B.OPS != 3);
    tempStatus=FCCU.STAT.B.STATUS;
    
    while(tempStatus != 0)
    {
        FCCU.CTRL.B.OPR = 10;
        while(FCCU.CTRL.B.OPS != 3);
        
        tempNCFS0 = FCCU.NCF_S[0].R;
        FCCU.NCFK.R = 0xAB3498FEUL;

        FCCU.NCF_S[0].R = tempNCFS0;
        while(FCCU.CTRL.B.OPS != 3);
        
        /*read fccu state*/
        FCCU.CTRL.B.OPR=3;
        while(FCCU.CTRL.B.OPS != 3);
        tempStatus = FCCU.STAT.B.STATUS;

    }

    /* Unlock FCCU transient lock */
    FCCU.TRANS_LOCK.R = TRANS_LOCK_KEY; 
    /* Write configuration timeout - Time protection on Init*/
    FCCU.CFG_TO.R = 0x07UL;     
        
    /* Enter CONFIG State */
    FCCU.CTRLK.R = OP1_KEY; 
    FCCU.CTRL.R = (uint32)0x00000001UL; 
    /* wait for FCCU to pass to the configuration state */
    while (FCCU.CTRL.B.OPS != 3);

     /* Disable IRQ_ALARM */
    FCCU.IRQ_ALARM_EN[0].R = (uint32)0x000000000UL;
    FCCU.IRQ_ALARM_EN[1].R = (uint32)0x000000000UL;
    
    /* Enter NORMAL State */
    FCCU.CTRLK.R = OP2_KEY; 
    FCCU.CTRL.R = (uint32)0x00000002UL; 
    /* wait for FCCU to pass to the normal state */
    while (FCCU.CTRL.B.OPS != 3);
   
    FCCU.CTRLK.R = OP1_KEY; 
    FCCU.CTRL.R = (uint32)0x00000001UL; 
    /* wait for FCCU to pass to the configuration state */
    while (FCCU.CTRL.B.OPS != 3);    
    
    /* Disable NMI */
    FCCU.NMI_EN[0].R = (uint32)0x000000000UL;
    FCCU.NMI_EN[1].R = (uint32)0x000000000UL;

    /* Enter NORMAL State */
    FCCU.CTRLK.R = OP2_KEY; 
    FCCU.CTRL.R = (uint32)0x00000002UL; 

    /* wait for FCCU to pass to the normal state */
    while (FCCU.CTRL.B.OPS != 3);
    
    FCCU.CTRLK.R = OP1_KEY; 
    FCCU.CTRL.R = (uint32)0x00000001UL; 
    /* wait for FCCU to pass to the configuration state */
    while (FCCU.CTRL.B.OPS != 3);    

    /* Disable REACTIONS */
    FCCU.NCF_E[0].R = (uint32)0x000000000UL;
    FCCU.NCF_E[1].R = (uint32)0x000000000UL;
    
    /* Enter NORMAL State */
    FCCU.CTRLK.R = OP2_KEY; 
    FCCU.CTRL.R = (uint32)0x00000002UL; 

    /* wait for FCCU to pass to the normal state */
    while (FCCU.CTRL.B.OPS != 3);
   
}
#ifdef MCAL_ENABLE_USER_MODE_SUPPORT

/* make the switch to supervisor mode.
Return how many times the supervisor mode was entered */

uint8 Sys_GoToSupervisor(void)
{

    u8SwitchToSupervisor++;
    Mcal_goToSupervisor();

    return u8SwitchToSupervisor;
}

/*make the switch back to user mode, return a uint32 value passed as parameter */
uint32 Sys_GoToUser_Return(uint32 u32returnValue)
{
    /* if previously, the supervisor mode was accessed, then switch back to UserMode */
    if(1UL == u8SwitchToSupervisor) 
    { 
        Mcal_goToUser(); 
    } 
    if(0UL < u8SwitchToSupervisor)
    {
        u8SwitchToSupervisor--;
    }
    /* return the value from the driver's call */
    return u32returnValue; 
}
/*make the switch back to user mode, return 0. */
void Sys_GoToUser(void)
{
    /* if previously, the supervisor mode was accessed, then switch back to UserMode */
    if(1UL == u8SwitchToSupervisor) 
    { 
        Mcal_goToUser(); 
    } 
    if(0UL < u8SwitchToSupervisor)
    {
        u8SwitchToSupervisor--;
    }
}
#endif /* MCAL_ENABLE_USER_MODE_SUPPORT */

/*==================================================================================================*/
/** 
@brief sys_init
@details function used to initiatialize platform clocks.                           
@return E_NOT_OK/E_OK
*/
/*==================================================================================================*/
Std_ReturnType sys_init(void) 
{
    volatile uint32_t timeout = 0;
    Std_ReturnType ret_val = E_OK;
	
#ifdef MCAL_ENABLE_USER_MODE_SUPPORT
    /* Enter supervisor mode */
    Sys_GoToSupervisor();
#endif /* MCAL_ENABLE_USER_MODE_SUPPORT */
   
    Sys_FCCU_Disable_Reactions(); 
    
/* select core clock from IRC clock */    
    SIU.SYSDIV.B.SYSCLKSEL=0;
    
/* Disable the PLLs */    
    PLLDIG.PLL0CR.R=0;
    PLLDIG.PLL1CR.R=0;
    
/* Wait for XOSC to LOCK  */
    while (SIU.RSR.B.XOSC==0) {}; 
 
/* Select XOSC as PLL0 source and PHI1 output of PLL0 as PLL1 source */
    SIU.SYSDIV.B.PLL0SEL=0;
    SIU.SYSDIV.B.PLL1SEL=1;
 
/* Program PLL0:
    # RFDPHI1 = 0x0C
    # RFDPHI = 0x03
    # PREDIV = 0x5 
    # MFD = 0x48
*/    
    PLLDIG.PLL0DV.R=0x60035048;
 
/* Turn on PLL0 and wait for lock */
    PLLDIG.PLL0CR.R=0x0000300;
    while (PLLDIG.PLL0SR.B.LOCK==0) {};
  
/* Program PLL1:
    # RFDPHI = 0x02
    # MFD = 0x16
*/    
    PLLDIG.PLL1DV.R=0x00020016;
 
/* Turn on PLL1 and wait for lock */
    PLLDIG.PLL1CR.R=0x0000300;
    while (PLLDIG.PLL1SR.B.LOCK==0) {};
 
/* Select clock dividers and sources
    PLL0 ref = XOSX
    PLL1 ref = PLL0 PHI1
    PERCLKSEL = PLL1
    PERDIV = Divide-by-2
    MCANSEL = XOSC
    SYSCLKSEL = PLL1
    ETPUDIV = Divide-by-1
    SYSCLKDIV = Divide-by-1
    PCS = Disabled
*/    
    SIU.SYSDIV.R=0x05002110;  
    
/* Set ENG clock */
    SIU.ECCR.B.ENGDIV = 200; 
	
#ifdef MCAL_ENABLE_USER_MODE_SUPPORT
    /* Go back to User mode */
    Sys_GoToUser();
#endif /* MCAL_ENABLE_USER_MODE_SUPPORT */   

    return ret_val;
}

/*==================================================================================================*/
/** 
@brief sys_init_ext
@details function used to initiatialize platform clocks based on given system clock value and clock mode
@param[in] sys_mode - System Clock Mode
@return E_NOT_OK/E_OK
*/
/*==================================================================================================*/
Std_ReturnType sys_init_ext(system_clock_values_t sys_pll_clk_value, system_clock_values_t sec_pll_clk_value) 
{
    volatile uint32_t timeout = 0;
    Std_ReturnType ret_val = E_OK;
	
#ifdef MCAL_ENABLE_USER_MODE_SUPPORT
    /* Enter supervisor mode */
    Sys_GoToSupervisor();
#endif /* MCAL_ENABLE_USER_MODE_SUPPORT */
 
    Sys_FCCU_Disable_Reactions(); 
    
/* select core clock from IRC clock */    
    SIU.SYSDIV.B.SYSCLKSEL=0;
    
/* Disable the PLLs */    
    PLLDIG.PLL0CR.R=0;
    PLLDIG.PLL1CR.R=0;
    
/* Wait for XOSC to LOCK  */
    while (SIU.RSR.B.XOSC==0) {}; 
 
/* Select XOSC as PLL0 source and PHI1 output of PLL0 as PLL1 source */
    SIU.SYSDIV.B.PLL0SEL=0;
    SIU.SYSDIV.B.PLL1SEL=1;
 
/* Program PLL0:
    # RFDPHI1 = 0x0C
    # RFDPHI = 0x03
    # PREDIV = 0x5 
    # MFD = 0x48
*/    
    PLLDIG.PLL0DV.R=0x60035048;
 
/* Turn on PLL0 and wait for lock */
    PLLDIG.PLL0CR.R=0x0000300;
    while (PLLDIG.PLL0SR.B.LOCK==0) {};
  
/* Program PLL1:
    # RFDPHI = 0x04
    # MFD = 0x16
    the PLL1 clock will be 132 MHz
*/    
    PLLDIG.PLL1DV.R=0x00040016;
 
/* Turn on PLL1 and wait for lock */
    PLLDIG.PLL1CR.R=0x0000300;
    while (PLLDIG.PLL1SR.B.LOCK==0) {};
 
/* Select clock dividers and sources
    PLL0 ref = XOSX
    PLL1 ref = PLL0 PHI1
    PERCLKSEL = PLL1
    PERDIV = Divide-by-2
    MCANSEL = XOSC
    SYSCLKSEL = PLL1
    ETPUDIV = Divide-by-1
    SYSCLKDIV = Divide-by-1
    PCS = Disabled
*/    
    SIU.SYSDIV.R=0x05002110;  
    
/* Set ENG clock */
    SIU.ECCR.B.ENGDIV = 200;   
    
	#ifdef MCAL_ENABLE_USER_MODE_SUPPORT
    /* Go back to User mode */
    Sys_GoToUser();
#endif /* MCAL_ENABLE_USER_MODE_SUPPORT */   
    return ret_val;
}

/*==================================================================================================*/
/** 
@brief sys_set_output_clock
@details function used to set the output clock ENGCLK to the specified source and value
         formula used: ENGCLK freq = selected clock freq /(ENGDIV × 2)

@param[in] source  - output clock source (SYS_CLK:2 or XOSC_CLK)
@param[in] divider - divider for output clock: 
@return E_OK
*/
/*==================================================================================================*/
Std_ReturnType  sys_set_output_clock(output_clock_selector_t source, uint8_t divider)
{
#ifdef MCAL_ENABLE_USER_MODE_SUPPORT
    /* Enter supervisor mode */
    Sys_GoToSupervisor();
#endif /* MCAL_ENABLE_USER_MODE_SUPPORT */

    SIU.ECCR.B.ECSS = source;
    SIU.ECCR.B.ENGDIV = divider;
	
#ifdef MCAL_ENABLE_USER_MODE_SUPPORT
    /* Go back to User mode */
    Sys_GoToUser();
#endif /* MCAL_ENABLE_USER_MODE_SUPPORT */  

    return E_OK;
}


/*==================================================================================================*/
/** 
@brief sys_halt
@details function used to enter halt mode
*/
/*==================================================================================================*/
void sys_halt(void)
{
    /* not yet implemented */
    /* just put the core in wait state */
    EXECUTE_WAIT();
}

/*==================================================================================================*/
/** 
@brief sys_stop
@details function used to enter stop mode
*/
/*==================================================================================================*/
void sys_stop(void)
{
     /* not yet implemented */
     /* just put the core in wait state */
     EXECUTE_WAIT();
}


/*================================================================================================*/
/**
* @brief sys_disable_dcache
* @details function used to invalidate and disable the DCACHE
* @param[in] void
* @return void
*/
/*================================================================================================*/
void sys_disable_dcache(void)
{
#ifdef MCAL_ENABLE_USER_MODE_SUPPORT
    /* Enter supervisor mode */
    Sys_GoToSupervisor();
#endif /* MCAL_ENABLE_USER_MODE_SUPPORT */

    ASM_KEYWORD("DCACHE_CFG1: ");
    ASM_KEYWORD(" e_li r5, 0x2 ");
    ASM_KEYWORD(" mtspr l1csr0,r5 ");
    
    ASM_KEYWORD(" e_li r7, 0x4 ");
    ASM_KEYWORD(" e_li r8, 0x2 ");
    ASM_KEYWORD(" e_lis   r11, 0xFFFF ");
    ASM_KEYWORD(" e_or2i  r11, 0xFFFB ");
    
    ASM_KEYWORD("DCACHE_INV1: ");
    ASM_KEYWORD(" mfspr r9, l1csr0 ");
    ASM_KEYWORD(" and.  r10, r7, r9 ");
    ASM_KEYWORD(" e_beq   DCACHE_NO_ABORT1 ");
    ASM_KEYWORD(" and.  r10, r11, r9 ");
    ASM_KEYWORD(" mtspr l1csr0, r10 ");
    ASM_KEYWORD(" e_b  DCACHE_CFG1 ");
    
    ASM_KEYWORD("DCACHE_NO_ABORT1: ");
    ASM_KEYWORD(" and.  r10, r8, r9 ");
    ASM_KEYWORD(" e_bne DCACHE_INV1 ");
    
    ASM_KEYWORD(" mfspr r5, l1csr0 ");
    ASM_KEYWORD(" e_ori   r5, r5, 0x0000 ");
    ASM_KEYWORD(" se_isync ");
    ASM_KEYWORD(" msync ");
    ASM_KEYWORD(" mtspr l1csr0, r5 ");

#ifdef MCAL_ENABLE_USER_MODE_SUPPORT
    /* Go back to User mode */
    Sys_GoToUser();
#endif /* MCAL_ENABLE_USER_MODE_SUPPORT */  

}

/*================================================================================================*/
/**
* @brief sys_enaable_dcache
* @details function used to invalidate and enable the DCACHE
* @param[in] void
* @return void
*/
/*================================================================================================*/
void sys_enable_dcache(void)
{
#ifdef MCAL_ENABLE_USER_MODE_SUPPORT
    /* Enter supervisor mode */
    Sys_GoToSupervisor();
#endif /* MCAL_ENABLE_USER_MODE_SUPPORT */
    ASM_KEYWORD("DCACHE_CFG2: ");
    ASM_KEYWORD(" e_li r5, 0x2 ");
    ASM_KEYWORD(" mtspr l1csr0,r5 ");
    
    ASM_KEYWORD(" e_li r7, 0x4 ");
    ASM_KEYWORD(" e_li r8, 0x2 ");
    ASM_KEYWORD(" e_lis   r11, 0xFFFF ");
    ASM_KEYWORD(" e_or2i  r11, 0xFFFB ");
    
    ASM_KEYWORD("DCACHE_INV2: ");
    ASM_KEYWORD(" mfspr r9, l1csr0 ");
    ASM_KEYWORD(" and.  r10, r7, r9 ");
    ASM_KEYWORD(" e_beq   DCACHE_NO_ABORT2 ");
    ASM_KEYWORD(" and.  r10, r11, r9 ");
    ASM_KEYWORD(" mtspr l1csr0, r10 ");
    ASM_KEYWORD(" e_b  DCACHE_CFG2 ");
    
    ASM_KEYWORD("DCACHE_NO_ABORT2: ");
    ASM_KEYWORD(" and.  r10, r8, r9 ");
    ASM_KEYWORD(" e_bne DCACHE_INV2 ");
    
    ASM_KEYWORD(" mfspr r5, l1csr0 ");
    ASM_KEYWORD(" e_ori   r5, r5, 0x0001 ");
    ASM_KEYWORD(" se_isync ");
    ASM_KEYWORD(" msync ");
    ASM_KEYWORD(" mtspr l1csr0, r5 ");
#ifdef MCAL_ENABLE_USER_MODE_SUPPORT
    /* Go back to User mode */
    Sys_GoToUser();
#endif /* MCAL_ENABLE_USER_MODE_SUPPORT */  
}



#ifdef __cplusplus
}
#endif
