/*
 *   Copyright 2019-2021 The MathWorks, Inc.
 *
 *
 */

#include "MW_target_hardware_resources.h"
#include "MW_f2838x_includes.h"
#include "MW_F2838x_MemoryMap.h"

#define IPC_REG_BOOTSTS_OFFSET             0x20U
#define IPC_ADDR_OFFSET_NOCHANGE           2U
#define IPC_ADDR_OFFSET_MUL2               4U
#define IPC_ADDR_OFFSET_DIV2               1U
#define IPC_ADDR_OFFSET_CORR(addr, corr)  (((addr) * (corr)) / 2U)

const IPC_Instance_t IPC_Instance[IPC_TOTAL_NUM] = {

     /* IPC_CPU1_L_CPU2_R */
     {
      .IPC_Flag_Ctr_Reg   = (volatile IPC_Flag_Ctr_Reg_t *) IPC_CPUXTOCPUX_BASE,
      .IPC_SendCmd_Reg    = (volatile IPC_SendCmd_Reg_t *)
                            (IPC_CPUXTOCPUX_BASE + IPC_O_CPU1TOCPU2IPCSENDCOM),
      .IPC_RecvCmd_Reg    = (volatile IPC_RecvCmd_Reg_t *)
                            (IPC_CPUXTOCPUX_BASE + IPC_O_CPU2TOCPU1IPCRECVCOM),
      .IPC_Boot_Pump_Reg  = (volatile IPC_Boot_Pump_Reg_t *)
                            (IPC_CPUXTOCPUX_BASE + IPC_REG_BOOTSTS_OFFSET),
      .IPC_IntNum         = {INT_CIPC0, INT_CIPC1, INT_CIPC2, INT_CIPC3,
                             0U, 0U, 0U, 0U},
      .IPC_MsgRam_LtoR    = CPU1TOCPU2MSGRAM0_BASE,
      .IPC_MsgRam_RtoL    = CPU2TOCPU1MSGRAM0_BASE,
      .IPC_Offset_Corr    = IPC_ADDR_OFFSET_NOCHANGE
     },

     /* IPC_CPU1_L_CM_R */
     {
      .IPC_Flag_Ctr_Reg   = (volatile IPC_Flag_Ctr_Reg_t *) IPC_CPUXTOCM_BASE,
      .IPC_SendCmd_Reg    = (volatile IPC_SendCmd_Reg_t *)
                            (IPC_CPUXTOCM_BASE + IPC_O_CPU1TOCMIPCSENDCOM),
      .IPC_RecvCmd_Reg    = (volatile IPC_RecvCmd_Reg_t *)
                            (IPC_CPUXTOCM_BASE + IPC_O_CMTOCPU1IPCRECVCOM),
      .IPC_Boot_Pump_Reg  = (volatile IPC_Boot_Pump_Reg_t *)
                            (IPC_CPUXTOCM_BASE + IPC_REG_BOOTSTS_OFFSET),
      .IPC_IntNum         = {INT_CMTOCPUXIPC0, INT_CMTOCPUXIPC1,
                             INT_CMTOCPUXIPC2, INT_CMTOCPUXIPC3,
                             INT_CMTOCPUXIPC4, INT_CMTOCPUXIPC5,
                             INT_CMTOCPUXIPC6, INT_CMTOCPUXIPC7},
      .IPC_MsgRam_LtoR    = CPUXTOCMMSGRAM0_BASE,
      .IPC_MsgRam_RtoL    = CMTOCPUXMSGRAM0_BASE,
      .IPC_Offset_Corr    = IPC_ADDR_OFFSET_DIV2
     },

     /* IPC_CPU2_L_CPU1_R */
     {
      .IPC_Flag_Ctr_Reg   = (volatile IPC_Flag_Ctr_Reg_t *) IPC_CPUXTOCPUX_BASE,
      .IPC_SendCmd_Reg    = (volatile IPC_SendCmd_Reg_t *)
                            (IPC_CPUXTOCPUX_BASE + IPC_O_CPU2TOCPU1IPCSENDCOM),
      .IPC_RecvCmd_Reg    = (volatile IPC_RecvCmd_Reg_t *)
                            (IPC_CPUXTOCPUX_BASE + IPC_O_CPU1TOCPU2IPCRECVCOM),
      .IPC_Boot_Pump_Reg  = (volatile IPC_Boot_Pump_Reg_t *)
                            (IPC_CPUXTOCPUX_BASE + IPC_REG_BOOTSTS_OFFSET),
      .IPC_IntNum         = {INT_CIPC0, INT_CIPC1, INT_CIPC2, INT_CIPC3,
                             0U, 0U, 0U, 0U},
      .IPC_MsgRam_LtoR    = CPU2TOCPU1MSGRAM0_BASE,
      .IPC_MsgRam_RtoL    = CPU1TOCPU2MSGRAM0_BASE,
      .IPC_Offset_Corr    = IPC_ADDR_OFFSET_NOCHANGE
     },

     /* IPC_CPU2_L_CM_R */
     {
      .IPC_Flag_Ctr_Reg   = (volatile IPC_Flag_Ctr_Reg_t *) IPC_CPUXTOCM_BASE,
      .IPC_SendCmd_Reg    = (volatile IPC_SendCmd_Reg_t *)
                            (IPC_CPUXTOCM_BASE + IPC_O_CPU2TOCMIPCSENDCOM),
      .IPC_RecvCmd_Reg    = (volatile IPC_RecvCmd_Reg_t *)
                            (IPC_CPUXTOCM_BASE + IPC_O_CMTOCPU2IPCRECVCOM),
      .IPC_Boot_Pump_Reg  = (volatile IPC_Boot_Pump_Reg_t *)
                            (IPC_CPUXTOCM_BASE + IPC_REG_BOOTSTS_OFFSET),
      .IPC_IntNum         = {INT_CMTOCPUXIPC0, INT_CMTOCPUXIPC1,
                             INT_CMTOCPUXIPC2, INT_CMTOCPUXIPC3,
                             INT_CMTOCPUXIPC4, INT_CMTOCPUXIPC5,
                             INT_CMTOCPUXIPC6, INT_CMTOCPUXIPC7},
      .IPC_MsgRam_LtoR    = CPUXTOCMMSGRAM0_BASE,
      .IPC_MsgRam_RtoL    = CMTOCPUXMSGRAM0_BASE,
      .IPC_Offset_Corr    = IPC_ADDR_OFFSET_DIV2
     }
};

#if MW_RUNTIME_FLASHLOAD
extern Uint16 MW_RamfuncsRunStart;
extern Uint16 MW_RamfuncsLoadSize;
extern Uint16 MW_RamfuncsLoadStart;
#endif

extern Uint16 RamfuncsLoadSize;
extern Uint16 Cla1funcsRunStart;
extern Uint16 Cla1funcsLoadStart;
extern Uint16 Cla1funcsLoadSize;
extern Uint16 Cla1mathTablesRunStart;
extern Uint16 Cla1mathTablesLoadStart;
extern Uint16 Cla1mathTablesLoadSize;
typedef uint32_t (*ipcFuncPointer)(uint32_t);
#ifdef CPU1
extern interrupt void CIPC0_INT_isr(void);
extern interrupt void CMTOCPUXIPC0_INT_isr(void);
uint32_t mwInitStart = 0;
uint32_t mwInitEnd = 0;
uint32_t cmInitStart = 0;
uint32_t cmInitEnd = 0;
__interrupt void CPU02toCPU01IPC0IntHandler(void);
__interrupt void CMtoCPU01IPC0IntHandler(void);
uint32_t MW_IpcFunctionCall(uint32_t* addr, uint32_t data);
uint32_t CM_IpcFunctionCall(uint32_t** addr, uint32_t data);
Uint32 setGPIOForCPU2(Uint32 gpioData);
Uint32 setGPIOForCM(Uint32 gpioData);
Uint32 setGPIOForEthernet(Uint32 data);
Uint32 setInXBARForCPU2(Uint32 inData);
Uint32 setOutXBARForCPU2(Uint32 inData);
Uint32 setEpwmXBARForCPU2(Uint32 inData);
volatile uint16_t ErrorFlag;
#else
void configureCPU2Peripherals(Uint32 gpioNumber, Uint32 gpGRegValA, Uint32 gpRegValA);
void configureInputXBar(Uint32 gpioNumber, Uint32 inputXbarNumber);
void configureOutputXBar(Uint32 xbar , Uint32 mux, Uint32 muxVal, Uint32 invert, Uint32 latch);
void configureEpwmXBar(Uint32 xbar , Uint32 mux, Uint32 muxVal, Uint32 invert);
#endif
void initSetGPIOIPC(void);

bool MW_IPC_sendCommand(IPC_Type_t ipcType, uint32_t flags, bool addrCorrEnable,
                     uint32_t command, uint32_t addr, uint32_t data)
{
    bool ret;

    //
    // Check whether the flags are not busy
    //
    if((IPC_Instance[ipcType].IPC_Flag_Ctr_Reg->IPC_FLG & flags) == 0U)
    {
        ret = true;

        if(addrCorrEnable)
        {
            //
            // Update the command registers. ADDR register holds the offset
            // from the base address of the MSG RAM
            //
            IPC_Instance[ipcType].IPC_SendCmd_Reg->IPC_SENDCOM  = command;
            IPC_Instance[ipcType].IPC_SendCmd_Reg->IPC_SENDDATA = data;
            IPC_Instance[ipcType].IPC_SendCmd_Reg->IPC_SENDADDR =
                            addr - IPC_Instance[ipcType].IPC_MsgRam_LtoR;
        }
        else
        {
            //
            // Update the command registers. addr param remains as is.
            //
            IPC_Instance[ipcType].IPC_SendCmd_Reg->IPC_SENDCOM  = command;
            IPC_Instance[ipcType].IPC_SendCmd_Reg->IPC_SENDDATA = data;
            IPC_Instance[ipcType].IPC_SendCmd_Reg->IPC_SENDADDR = addr;
        }

        //
        // Set the flags to indicate the remote core
        //
        IPC_Instance[ipcType].IPC_Flag_Ctr_Reg->IPC_SET = flags;
    }
    else
    {
        ret = false;
    }

    return(ret);
}

bool MW_IPC_readCommand(IPC_Type_t ipcType, uint32_t flags, bool addrCorrEnable,
                     uint32_t *command, uint32_t *addr, uint32_t *data)
{
    bool ret;
    uint32_t addrReg;

    //
    // Check whether the flags are not empty
    //
    if((IPC_Instance[ipcType].IPC_Flag_Ctr_Reg->IPC_STS & flags) != 0U)
    {
        ret = true;

        //
        // Read the command registers
        //
        *command   = IPC_Instance[ipcType].IPC_RecvCmd_Reg->IPC_RECVCOM;
        addrReg    = IPC_Instance[ipcType].IPC_RecvCmd_Reg->IPC_RECVADDR;
        *data      = IPC_Instance[ipcType].IPC_RecvCmd_Reg->IPC_RECVDATA;

        if(addrCorrEnable)
        {
            //
            // Calculate the address form the offset
            //
            /**addr = IPC_Instance[ipcType].IPC_MsgRam_RtoL +
                    IPC_ADDR_OFFSET_CORR(addrReg,
                                    IPC_Instance[ipcType].IPC_Offset_Corr);*/
			*addr = IPC_Instance[ipcType].IPC_MsgRam_LtoR +
                    IPC_ADDR_OFFSET_CORR(addrReg,
                                    IPC_Instance[ipcType].IPC_Offset_Corr);

        }
        else
        {
            *addr = addrReg;
        }

    }
    else
    {
        ret = false;
    }

    return(ret);
}

void c2000_flash_init(void)
{
#if MW_RUNTIME_FLASHLOAD
    /* Copy InitFlash function code and Flash setup code to RAM */
    memcpy(&RamfuncsRunStart,&RamfuncsLoadStart, (Uint32)(&RamfuncsLoadSize));
    memcpy(&MW_RamfuncsRunStart, &MW_RamfuncsLoadStart, (Uint32)&MW_RamfuncsLoadSize);
#ifdef CLA_BLOCK_INCLUDED
    memcpy(&Cla1funcsRunStart, &Cla1funcsLoadStart, (Uint32)&Cla1funcsLoadSize);
    /*memcpy(&Cla1mathTablesRunStart, &Cla1mathTablesLoadStart, (Uint32)&Cla1mathTablesLoadSize); */
#endif
    /* Call Flash Initialization to setup flash waitstates
   This function must reside in RAM */
    InitFlash();
#endif
}

void initSetGPIOIPC(void)
{
    
    uint32_t *pulMsgRam;
    volatile uint32_t ipcCountVar = 0;
#ifdef CPU1
    uint32_t i = 0;
    uint32_t *cmMsgRam;
    uint32_t MemCfgRegVar = 0;
    uint32_t mode = 0;
    EALLOW; // This is needed to write to EALLOW protected registers
    PieVectTable.CIPC0_INT = &CPU02toCPU01IPC0IntHandler;
    PieVectTable.CMTOCPUXIPC0_INT = &CMtoCPU01IPC0IntHandler;
    /*Each global shared RAM section size is 4096 (2^12)*/
	/*Global shared RAM starts at 0xD000*/
	
	// CPU2 
    for (i=((uint32_t)CPU2_RAMGS_DATA_START) >> 12; i < (((uint32_t)CPU2_RAMGS_DATA_LENGTH + (uint32_t)CPU2_RAMGS_DATA_START) >> 12); i++)
    {

        MemCfgRegVar = MemCfgRegVar | (1UL<<(i-13));
    }
    for (i=((uint32_t)CPU2_RAMGS_PROG_START) >> 12; i < (((uint32_t)CPU2_RAMGS_PROG_LENGTH + (uint32_t)CPU2_RAMGS_PROG_START) >> 12); i++)
    {
        MemCfgRegVar = MemCfgRegVar | (1UL<<(i-13));
    }
	
	//CPU1
	for (i=((uint32_t)CPU1_RAMGS_DATA_START) >> 12; i < (((uint32_t)CPU1_RAMGS_DATA_LENGTH + (uint32_t)CPU1_RAMGS_DATA_START) >> 12); i++)
    {
        MemCfgRegVar = MemCfgRegVar & (~(1UL<<(i-13)));
    }
    for (i=((uint32_t)CPU1_RAMGS_PROG_START) >> 12; i < (((uint32_t)CPU1_RAMGS_PROG_LENGTH + (uint32_t)CPU1_RAMGS_PROG_START) >> 12); i++)
    {
        MemCfgRegVar = MemCfgRegVar & (~(1UL<<(i-13)));
    }
	
    // IPC related configurations will be added in IPC source file MW_c28xIPC.c
	
    MemCfgRegs.GSxMSEL.all = MemCfgRegs.GSxMSEL.all | MemCfgRegVar;
    EDIS;    // This is needed to disable write to EALLOW protected registers
    pulMsgRam = (void *)CPU01_TO_CPU02_PASSMSG;
    pulMsgRam[0] = (uint32_t)&setGPIOForCPU2;
    pulMsgRam[1] = (uint32_t)&mwInitStart;
    pulMsgRam[2] = (uint32_t)&mwInitEnd;
    pulMsgRam[3] = (uint32_t)&setInXBARForCPU2;
    pulMsgRam[4] = (uint32_t)&setOutXBARForCPU2;
    pulMsgRam[5] = (uint32_t)&setEpwmXBARForCPU2;
    
    cmMsgRam = (void *)CPU01_TO_CM_PASSMSG;
    cmMsgRam[0] = (uint32_t)&setGPIOForCM;
    cmMsgRam[1] = (uint32_t)&cmInitStart;
    cmMsgRam[2] = (uint32_t)&cmInitEnd;
	cmMsgRam[3] = (uint32_t)&setGPIOForEthernet;
    
    PieCtrlRegs.PIEIER1.bit.INTx13 = 1;     // IPC INT0
    PieCtrlRegs.PIEIER11.bit.INTx9 = 1;     // CM_CPUx_IPC0_INT
    IER |= M_INT1;
    IER |= M_INT11;
    EINT;  // Enable Global interrupt INTM
    ERTM;  // Enable Global realtime interrupt DBGM
    
    //#if MW_RUNTIME_FLASHLOAD
    
    SysCtl_controlCPU2Reset(SYSCTL_CORE_DEACTIVE);
    //while(SysCtl_isCPU2Reset() == 0x1U);
    //Wait for CPU2 to get ready to accept boot command from CPU1
    while (((IPC_getBootStatus(IPC_CPU1_L_CPU2_R) & 0x0000000F) != 0x7) && (700 > ipcCountVar))
    {
        ipcCountVar++;
    }
    //CPU01 to CPU02 IPC Boot Mode Register
    mode |= (uint32_t)((uint32_t)0x5A << 24);
    mode |= (uint32_t)((uint32_t)MW_CLOCKING_CLOSESTCPUCLOCK << 8);
    mode |= 0x03; // Flash boot option 0
    // CPU01 To CPU02 IPC Command Register
    IPC_setBootMode(IPC_CPU1_L_CPU2_R, mode);
    // CPU01 to CPU02 IPC flag register
    IPC_setFlagLtoR(IPC_CPU1_L_CPU2_R, (IPC_FLAG0 | IPC_FLAG31));
    ipcCountVar = 0;
    // Give ~35 usec delay to start CPU2 init
    while (((IPC_getBootStatus(IPC_CPU1_L_CPU2_R) & 0x0000000F) != 0x2) && (500 > ipcCountVar)) {
        ipcCountVar++;
    }
    //CM init
    ipcCountVar = 0;
    mode = 0;
    
    
    ClkCfgRegs.CLKSRCCTL2.bit.MCANABITCLKSEL = 0; //set CM Clk as source for MCAN
    SysCtl_setMCANClk(SYSCTL_MCANCLK_DIV_4); //set Clk divider for MCAN Bit CLK
    
    SysCtl_controlCMReset(SYSCTL_CORE_DEACTIVE);
    while(SysCtl_isCMReset() == 0x1U);
    //Wait for CM to get ready to accept boot command from CPU1
    while (((IPC_getBootStatus(IPC_CPU1_L_CM_R) & 0x0000000F) != 0x8) && (700 > ipcCountVar))
    {
        ipcCountVar++;
    }
    
    ipcCountVar = 0;
    
    //CPU01 to CM IPC Boot Mode Register
    mode |= (uint32_t)((uint32_t)0x5A << 24);
    mode |= (uint32_t)((uint32_t)MW_CLOCKING_CMCLKVAL << 8);
    mode |= 0x03; // Flash boot option 0
    // CPU01 To CM IPC Command Register
    IPC_setBootMode(IPC_CPU1_L_CM_R, mode);
    // CPU01 to CM IPC flag register
    IPC_setFlagLtoR(IPC_CPU1_L_CM_R, (IPC_FLAG0 | IPC_FLAG31));
    
    // Give ~35 usec delay to start CM init
    while (((IPC_getBootStatus(IPC_CPU1_L_CM_R) & 0x0000000F) != 0x2) && (500 > ipcCountVar)) {
        ipcCountVar++;
    }
    
#else
    pulMsgRam = (void *)CPU01_TO_CPU02_PASSMSG;
    MW_IPC_sendCommand(IPC_CPU2_L_CPU1_R, (IPC_FLAG0 | IPC_FLAG31), IPC_ADDR_CORRECTION_DISABLE, IPC_DATA_WRITE_32, pulMsgRam[1], 0x425);
#endif
}

#ifdef CPU1
Uint32 setGPIOForCPU2(Uint32 gpioData)
{
    if (((gpioData & 0xFF000000)>>24) == GPIO_MUX_CPU2CLA) /* from CLA inside CPU2, GPIO_MUX_CPU2CLA defined to value 3*/
    {
        gpioData = gpioData & 0x00FFFFFF; /* remove added encryption GPIO_MUX_CPU2CLA, MSB is the flag and we use only flag 0*/
        GPIO_SetupPinMux(gpioData & 0x000000FF, GPIO_MUX_CPU2CLA, (gpioData & 0x0000FF00)>>8);
        GPIO_SetupPinOptions(gpioData & 0x000000FF, (gpioData & 0x00FF0000)>>16, (gpioData & 0xFF000000)>>24);
    }
    else
    {
        GPIO_SetupPinMux(gpioData & 0x000000FF, GPIO_MUX_CPU2, (gpioData & 0x0000FF00)>>8);/* From CPU2*/
        GPIO_SetupPinOptions(gpioData & 0x000000FF, (gpioData & 0x00FF0000)>>16, (gpioData & 0xFF000000)>>24);
    }
    return 1;
}

Uint32 setGPIOForCM(Uint32 gpioData)
{
    GPIO_SetupPinMux(gpioData & 0x000000FF, GPIO_MUX_CM, (gpioData & 0x0000FF00)>>8);
    GPIO_SetupPinOptions(gpioData & 0x000000FF, (gpioData & 0x00FF0000)>>16, (gpioData & 0xFF000000)>>24);
    
    return 1;
}

Uint32 setGPIOForEthernet(Uint32 data)
{
	// MDIO Signals
    
	GPIO_SetupPinMux(0x00000E69U & 0x000000FF, GPIO_MUX_CPU1, (0x00000E69U & 0x0000FF00)>>8);
	GPIO_SetupPinMux(0x00000E6AU & 0x000000FF, GPIO_MUX_CPU1, (0x00000E6AU & 0x0000FF00)>>8);
	
	//MII Signals
	GPIO_SetupPinMux(0x00000E6DU & 0x000000FF, GPIO_MUX_CPU1, (0x00000E6DU & 0x0000FF00)>>8);
	GPIO_SetupPinMux(0x00000E6EU & 0x000000FF, GPIO_MUX_CPU1, (0x00000E6EU & 0x0000FF00)>>8);
	
	GPIO_SetupPinMux(0x00000B4BU & 0x000000FF, GPIO_MUX_CPU1, (0x00000B4BU & 0x0000FF00)>>8);
	GPIO_SetupPinMux(0x00000E7AU & 0x000000FF, GPIO_MUX_CPU1, (0x00000E7AU & 0x0000FF00)>>8);
	GPIO_SetupPinMux(0x00000E7BU & 0x000000FF, GPIO_MUX_CPU1, (0x00000E7BU & 0x0000FF00)>>8);
	GPIO_SetupPinMux(0x00000E7CU & 0x000000FF, GPIO_MUX_CPU1, (0x00000E7CU & 0x0000FF00)>>8);
	
	GPIO_SetupPinMux(0x00000E76U & 0x000000FF, GPIO_MUX_CPU1, (0x00000E76U & 0x0000FF00)>>8);
	GPIO_SetupPinMux(0x00000E72U & 0x000000FF, GPIO_MUX_CPU1, (0x00000E72U & 0x0000FF00)>>8);
	GPIO_SetupPinMux(0x00000E73U & 0x000000FF, GPIO_MUX_CPU1, (0x00000E73U & 0x0000FF00)>>8);
	GPIO_SetupPinMux(0x00000E74U & 0x000000FF, GPIO_MUX_CPU1, (0x00000E74U & 0x0000FF00)>>8);
	GPIO_SetupPinMux(0x00000E75U & 0x000000FF, GPIO_MUX_CPU1, (0x00000E75U & 0x0000FF00)>>8);
	GPIO_SetupPinMux(0x00000E71U & 0x000000FF, GPIO_MUX_CPU1, (0x00000E71U & 0x0000FF00)>>8);
	GPIO_SetupPinMux(0x00000E70U & 0x000000FF, GPIO_MUX_CPU1, (0x00000E70U & 0x0000FF00)>>8);

	GPIO_SetupPinMux(0x00000B2CU & 0x000000FF, GPIO_MUX_CPU1, (0x00000B2CU & 0x0000FF00)>>8);
	GPIO_SetupPinMux(0x00000E6FU & 0x000000FF, GPIO_MUX_CPU1, (0x00000E6FU & 0x0000FF00)>>8);
	
	//Power down pin to bring the external PHY out of Power down
	
	volatile uint32_t *gpioBaseAddr;
	volatile uint32_t *gpioDataReg;
    uint32_t pinMask;
	gpioBaseAddr = (uint32_t *)GPIOCTRL_BASE +
                   ((108 / 32U) * GPIO_CTRL_REGS_STEP);
    pinMask = (uint32_t)1U << (108 % 32U);
	EALLOW;
	gpioBaseAddr[GPIO_GPxDIR_INDEX] |= pinMask;
	gpioBaseAddr[GPIO_GPxPUD_INDEX] &= ~pinMask;
	EDIS;
	gpioDataReg = (uint32_t *)GPIODATA_BASE +
                  ((108 / 32U) * GPIO_DATA_REGS_STEP);
	gpioDataReg[GPIO_GPxSET_INDEX] = pinMask;
	
	//PHY Reset Pin to be driven High to bring external PHY out of Reset
	
	volatile uint32_t *gpioBaseAddr1;
	volatile uint32_t *gpioDataReg1;
    uint32_t pinMask1;
	gpioBaseAddr1 = (uint32_t *)GPIOCTRL_BASE +
                   ((119 / 32U) * GPIO_CTRL_REGS_STEP);
    pinMask1 = (uint32_t)1U << (119 % 32U);
	EALLOW;
	gpioBaseAddr1[GPIO_GPxDIR_INDEX] |= pinMask1;
	gpioBaseAddr1[GPIO_GPxPUD_INDEX] &= ~pinMask1;
	EDIS;
	gpioDataReg1 = (uint32_t *)GPIODATA_BASE +
                  ((119 / 32U) * GPIO_DATA_REGS_STEP);
	gpioDataReg1[GPIO_GPxSET_INDEX] = pinMask1;
	
    return 1;
}

Uint32 setInXBARForCPU2(Uint32 inData)
{
    Uint16 pin = (Uint16)(inData & 0x000000FF);
    EALLOW;
    switch((inData & 0x0000FF00) >> 8)
    {
        case 1:
            InputXbarRegs.INPUT1SELECT = pin;
            break;
        case 2:
            InputXbarRegs.INPUT2SELECT = pin;
            break;
        case 3:
            InputXbarRegs.INPUT3SELECT = pin;
            break;
        case 4:
            InputXbarRegs.INPUT4SELECT = pin;
            break;
        case 5:
            InputXbarRegs.INPUT5SELECT = pin;
            break;
        case 6:
            InputXbarRegs.INPUT6SELECT = pin;
            break;
        case 7:
            InputXbarRegs.INPUT7SELECT = pin;
            break;
        case 8:
            InputXbarRegs.INPUT8SELECT = pin;
            break;
        case 9:
            InputXbarRegs.INPUT9SELECT = pin;
            break;
        case 10:
            InputXbarRegs.INPUT10SELECT = pin;
            break;
        case 11:
            InputXbarRegs.INPUT11SELECT = pin;
            break;
        case 12:
            InputXbarRegs.INPUT12SELECT = pin;
            break;
        case 13:
            InputXbarRegs.INPUT13SELECT = pin;
            break;
        case 14:
            InputXbarRegs.INPUT14SELECT = pin;
            break;
        case 15:
            InputXbarRegs.INPUT15SELECT = pin;
            break;
        case 16:
            InputXbarRegs.INPUT16SELECT = pin;
            break;    
    }
    EDIS;
    return 1;
}

Uint32 setOutXBARForCPU2(Uint32 inData)
{
    volatile Uint32 *muxEnableRegAddr;
    volatile Uint32 *muxRegAddr;
    volatile Uint32 *muxLatchRegAddr;
    volatile Uint32 *muxInvertRegAddr;
    Uint16 latch = (Uint16)((inData & 0xF0000000) >> 28);
    Uint16 invert = (Uint16)((inData & 0x0F000000) >> 24);
    Uint16 muxVal = (Uint16)((inData & 0x00FF0000) >> 16);
    Uint16 mux = (Uint16)((inData & 0x0000FF00) >> 8);
    Uint16 xbar = (Uint16)(inData & 0x000000FF);
    Uint16 muxPosition = mux%16;
    
    muxRegAddr          = (Uint32 *)&OutputXbarRegs + ((2*xbar)-2) + (mux/16);
    muxEnableRegAddr    = (Uint32 *)&OutputXbarRegs + 0x10 + (xbar-1);
    muxLatchRegAddr    = (Uint32 *)&OutputXbarRegs + 0x1B;
    muxInvertRegAddr    = (Uint32 *)&OutputXbarRegs + 0x1C;
    EALLOW;
    *muxRegAddr        |= (Uint32)(muxVal & 0x3) << (2*muxPosition);
    *muxEnableRegAddr  |= (Uint32)1 << mux;
    *muxLatchRegAddr   |= (Uint32)(latch & 0x1) << (xbar-1);
    *muxInvertRegAddr  |= (Uint32)(invert & 0x1) << (xbar-1);
    EDIS;
    return 1;
}

Uint32 setEpwmXBARForCPU2(Uint32 inData)
{
    volatile Uint32 *muxEnableRegAddr;
    volatile Uint32 *muxRegAddr;
    volatile Uint32 *muxInvertRegAddr;
    Uint16 invert = (Uint16)((inData & 0x0F000000) >> 24);
    Uint16 muxVal = (Uint16)((inData & 0x00FF0000) >> 16);
    Uint16 mux = (Uint16)((inData & 0x0000FF00) >> 8);
    Uint16 xbar = (Uint16)(inData & 0x000000FF);
    Uint16 muxPosition = mux%16;
    
    muxRegAddr          = (Uint32 *)&EPwmXbarRegs + ((2*xbar)-2) + (mux/16);
    muxEnableRegAddr    = (Uint32 *)&EPwmXbarRegs + 0x10 + (xbar-1);
    muxInvertRegAddr    = (Uint32 *)&EPwmXbarRegs + 0x1C;
    EALLOW;
    *muxRegAddr        |= (Uint32)(muxVal & 0x3) << (2*muxPosition);
    *muxEnableRegAddr  |= (Uint32)1 << mux;
    *muxInvertRegAddr  |= (Uint32)(invert & 0x1) << (xbar-1);
    EDIS;
    return 1;
}

__interrupt void CPU02toCPU01IPC0IntHandler (void)
{
    // Continue processing messages
    uint32_t command;
    uint32_t address = 0;
    uint32_t data;
    bool status = false;
    
    IPC_waitForFlag(IPC_CPU1_L_CPU2_R, (IPC_FLAG0 | IPC_FLAG31));
    status = MW_IPC_readCommand(IPC_CPU1_L_CPU2_R, (IPC_FLAG0 | IPC_FLAG31), IPC_ADDR_CORRECTION_DISABLE, &command, &address, &data);
    if (status == true)
        switch (command)
        {
            case IPC_DATA_WRITE_16:
                *((uint16_t*)address) = (uint16_t)data;
                IPC_sendResponse(IPC_CPU1_L_CPU2_R, (uint32_t)(*((uint16_t*)address)));
                IPC_ackFlagRtoL(IPC_CPU1_L_CPU2_R, (IPC_FLAG0 | IPC_FLAG31));
                break;
            case IPC_DATA_READ_16:
                IPC_sendResponse(IPC_CPU1_L_CPU2_R, (uint32_t)(*((uint16_t*)address)));
                IPC_ackFlagRtoL(IPC_CPU1_L_CPU2_R, (IPC_FLAG0 | IPC_FLAG31));
                break;
            case IPC_DATA_WRITE_32:
                *((uint32_t*)address) = (uint32_t)data;
                IPC_sendResponse(IPC_CPU1_L_CPU2_R, (*((uint32_t*)address)));
                IPC_ackFlagRtoL(IPC_CPU1_L_CPU2_R, (IPC_FLAG0 | IPC_FLAG31));
                break;
            case IPC_DATA_READ_32:
                IPC_sendResponse(IPC_CPU1_L_CPU2_R, (*((uint32_t*)address)));
                IPC_ackFlagRtoL(IPC_CPU1_L_CPU2_R, (IPC_FLAG0 | IPC_FLAG31));
                break;
            case IPC_FUNC_CALL:
                IPC_sendResponse(IPC_CPU1_L_CPU2_R, MW_IpcFunctionCall(&address, data));
                IPC_ackFlagRtoL(IPC_CPU1_L_CPU2_R, (IPC_FLAG0 | IPC_FLAG31));
                break;
            default:
                break;
        }
    else
    {
        ErrorFlag = 1;
    }
    
    PieCtrlRegs.PIEACK.all = PIEACK_GROUP1;   // Acknowledge interrupt to PIE
    // try re-mapping the IPC INT0.
    if (data == 0x36E) // mwInitEnd
    {
        IPC_ackFlagRtoL(IPC_CPU1_L_CPU2_R, (IPC_FLAG0 | IPC_FLAG31));
        IPC_clearFlagLtoR(IPC_CPU1_L_CPU2_R, (IPC_FLAG0 | IPC_FLAG31));
#ifdef REMAPIPCINT0
        EALLOW;
        PieVectTable.CIPC0_INT = &CIPC0_INT_isr;/* Hook interrupt to the ISR*/
        EDIS;
#else
        asm(" SETC INTM");
        PieCtrlRegs.PIEIER1.bit.INTx13 = 0;     // IPC INT0 disable
        asm(" RPT #5 || NOP");
        IFR &= 0xFFFE;
        PieCtrlRegs.PIEACK.all = 0x1;
        asm(" CLRC INTM"); /* Enable Interrupt Global Enable Bit - set it to 0 */
#endif
    }
}

__interrupt void CMtoCPU01IPC0IntHandler (void)
{
    // Continue processing messages
    uint32_t command;
    uint32_t* address = 0;
    uint32_t data;
    bool status = false;
    
    IPC_waitForFlag(IPC_CPU1_L_CM_R, (IPC_FLAG0 | IPC_FLAG31));
    status = MW_IPC_readCommand(IPC_CPU1_L_CM_R, (IPC_FLAG0 | IPC_FLAG31), IPC_ADDR_CORRECTION_ENABLE, &command, &address, &data);
    if (status == true)
        switch (command)
        {
            case IPC_DATA_WRITE_16:
                *((uint16_t*)(*address)) = (uint16_t)data;
                IPC_sendResponse(IPC_CPU1_L_CM_R, (uint32_t)(*((uint16_t*)(*address))));
                IPC_ackFlagRtoL(IPC_CPU1_L_CM_R, (IPC_FLAG0 | IPC_FLAG31));
                break;
            case IPC_DATA_READ_16:
                IPC_sendResponse(IPC_CPU1_L_CM_R, (uint32_t)(*((uint16_t*)(*address))));
                IPC_ackFlagRtoL(IPC_CPU1_L_CM_R, (IPC_FLAG0 | IPC_FLAG31));
                break;
            case IPC_DATA_WRITE_32:
                *((uint32_t*)(*address)) = (uint32_t)data;
                IPC_sendResponse(IPC_CPU1_L_CM_R, (*((uint32_t*)(*address))));
                IPC_ackFlagRtoL(IPC_CPU1_L_CM_R, (IPC_FLAG0 | IPC_FLAG31));
                break;
            case IPC_DATA_READ_32:
                IPC_sendResponse(IPC_CPU1_L_CM_R, (*((uint32_t*)(*address))));
                IPC_ackFlagRtoL(IPC_CPU1_L_CM_R, (IPC_FLAG0 | IPC_FLAG31));
                break;
            case IPC_FUNC_CALL:
                IPC_sendResponse(IPC_CPU1_L_CM_R, CM_IpcFunctionCall(&address, data));
                IPC_ackFlagRtoL(IPC_CPU1_L_CM_R, (IPC_FLAG0 | IPC_FLAG31));
                break;
            default:
                break;
        }
    else
    {
        ErrorFlag = 1;
    }
    
    PieCtrlRegs.PIEACK.all = PIEACK_GROUP11;   // Acknowledge interrupt to PIE
    // try re-mapping the CMTOCPUXIPC0_INTs.
    if (data == 0x36E) // mwInitEnd
    {
        IPC_ackFlagRtoL(IPC_CPU1_L_CM_R, (IPC_FLAG0 | IPC_FLAG31));
        IPC_clearFlagLtoR(IPC_CPU1_L_CM_R, (IPC_FLAG0 | IPC_FLAG31));
#ifdef REMAPCMIPCINT0
        EALLOW;
        PieVectTable.CMTOCPUXIPC0_INT = &CMTOCPUXIPC0_INT_isr;/* Hook interrupt to the ISR*/
        EDIS;
#else
        asm(" SETC INTM");
        PieCtrlRegs.PIEIER11.bit.INTx9 = 0;     // IPC INT0 disable
        asm(" RPT #5 || NOP");
        IFR &= 0xFFFE;
        PieCtrlRegs.PIEACK.all = 0x1;
        asm(" CLRC INTM"); /* Enable Interrupt Global Enable Bit - set it to 0 */
#endif
    }
}

uint32_t MW_IpcFunctionCall(uint32_t* addr, uint32_t data)
{
    ipcFuncPointer func_call = (ipcFuncPointer)*addr;
    return func_call(data);
}

uint32_t CM_IpcFunctionCall(uint32_t** addr, uint32_t data)

{
    uint32_t* func_addr;
    func_addr = *addr;
    ipcFuncPointer func_call = (ipcFuncPointer)*func_addr;
    return func_call(data);
}
#else
void configureCPU2Peripherals(Uint32 gpioNumber, Uint32 gpGRegValA, Uint32 gpRegValA)
{
    Uint32 *pulMsgRam = (void *)CPU01_TO_CPU02_PASSMSG;
    Uint32 gpioData = 0x00010000;
    gpioData = gpioData|gpioNumber;
    gpioData = gpioData|(gpRegValA << 8);
    gpioData = gpioData|(gpGRegValA << 10);
    MW_IPC_sendCommand(IPC_CPU2_L_CPU1_R, (IPC_FLAG0 | IPC_FLAG31), IPC_ADDR_CORRECTION_DISABLE, IPC_FUNC_CALL, pulMsgRam[0], gpioData);
    IPC_waitForAck(IPC_CPU2_L_CPU1_R, IPC_FLAG31);
    IPC_getResponse(IPC_CPU2_L_CPU1_R);
}
void configureInputXBar(Uint32 gpioNumber, Uint32 inputXbarNumber)
{
    Uint32 *pulMsgRam = (void *)CPU01_TO_CPU02_PASSMSG;
    Uint32 gpioData = 0x00000000;
    gpioData = 0x00000000;
    gpioData = gpioData|gpioNumber;
    gpioData = gpioData|(inputXbarNumber << 8);
    MW_IPC_sendCommand(IPC_CPU2_L_CPU1_R, (IPC_FLAG0 | IPC_FLAG31), IPC_ADDR_CORRECTION_DISABLE, IPC_FUNC_CALL, pulMsgRam[3], gpioData);
    IPC_waitForAck(IPC_CPU2_L_CPU1_R, IPC_FLAG31);
    IPC_getResponse(IPC_CPU2_L_CPU1_R);
}
void configureOutputXBar(Uint32 xbar , Uint32 mux, Uint32 muxVal, Uint32 invert, Uint32 latch)
{
    Uint32 *pulMsgRam = (void *)CPU01_TO_CPU02_PASSMSG;
    Uint32 gpioData = 0x00000000;
    gpioData = 0x00000000;
    gpioData = gpioData|xbar;
    gpioData = gpioData|(mux << 8);
    gpioData = gpioData|(muxVal << 16);
    gpioData = gpioData|(invert << 24);
    gpioData = gpioData|(latch << 28);
    MW_IPC_sendCommand(IPC_CPU2_L_CPU1_R, (IPC_FLAG0 | IPC_FLAG31), IPC_ADDR_CORRECTION_DISABLE, IPC_FUNC_CALL, pulMsgRam[4], gpioData);
	IPC_waitForAck(IPC_CPU2_L_CPU1_R, IPC_FLAG31);
    IPC_getResponse(IPC_CPU2_L_CPU1_R);
}
void configureEpwmXBar(Uint32 xbar , Uint32 mux, Uint32 muxVal, Uint32 invert)
{
    Uint32 *pulMsgRam = (void *)CPU01_TO_CPU02_PASSMSG;
    Uint32 gpioData = 0x00000000;
    gpioData = 0x00000000;
    gpioData = gpioData|xbar;
    gpioData = gpioData|(mux << 8);
    gpioData = gpioData|(muxVal << 16);
    gpioData = gpioData|(invert << 24);
    MW_IPC_sendCommand(IPC_CPU2_L_CPU1_R, (IPC_FLAG0 | IPC_FLAG31), IPC_ADDR_CORRECTION_DISABLE, IPC_FUNC_CALL, pulMsgRam[5], gpioData);
	IPC_waitForAck(IPC_CPU2_L_CPU1_R, IPC_FLAG31);
    IPC_getResponse(IPC_CPU2_L_CPU1_R);
}
#endif

void MW_InitSysPll(Uint16 clock_source, Uint16 imult, Uint32 refdiv, Uint32 odiv,
        Uint16 divsel, Uint32 dccbase)
{
    Uint32 timeout,temp_syspllmult, pllLockStatus;
    bool status;
    
    if(((clock_source & 0x3) == ClkCfgRegs.CLKSRCCTL1.bit.OSCCLKSRCSEL)    &&
            (((clock_source & 0x4) >> 2) == ClkCfgRegs.XTALCR.bit.SE)           &&
            (imult  == ClkCfgRegs.SYSPLLMULT.bit.IMULT)           &&
            (refdiv  == ClkCfgRegs.SYSPLLMULT.bit.REFDIV)          &&
            (odiv == ClkCfgRegs.SYSPLLMULT.bit.ODIV)               &&
            (divsel == ClkCfgRegs.SYSCLKDIVSEL.bit.PLLSYSCLKDIV))
    {
        //
        // Everything is set as required, so just return
        //
        return;
    }
    
    EALLOW;
    
    //
    // First modify the PLL multipliers if the multipliers need an update or PLL needs
    // to be powered on / enabled
    //
    if((imult != ClkCfgRegs.SYSPLLMULT.bit.IMULT) ||
            (refdiv != ClkCfgRegs.SYSPLLMULT.bit.REFDIV)||
            (odiv != ClkCfgRegs.SYSPLLMULT.bit.ODIV) ||
            (1U != ClkCfgRegs.SYSPLLCTL1.bit.PLLEN))
    {
        //
        // Bypass PLL and set dividers to /1
        //
        ClkCfgRegs.SYSPLLCTL1.bit.PLLCLKEN = 0;
        
        //
        // Delay of at least 120 OSCCLK cycles required post PLL bypass
        //
        asm(" RPT #120 || NOP");
        
        //
        // Evaluate PLL multipliers and dividers
        //
        temp_syspllmult = ((refdiv << 24U) | (odiv << 16U)| imult);
        
        //
        // Turnoff the PLL
        //
        ClkCfgRegs.SYSPLLCTL1.bit.PLLEN = 0;
        EDIS;
        
        //
        // Delay of at least 66 OSCCLK cycles
        //
        asm(" RPT #66 || NOP");
        
        if(((clock_source & 0x3) != ClkCfgRegs.CLKSRCCTL1.bit.OSCCLKSRCSEL) ||
                (((clock_source & 0x4) >> 2) != ClkCfgRegs.XTALCR.bit.SE))
        {
            switch (clock_source)
            {
                case INT_OSC1:
                    SysIntOsc1Sel();
                    break;
                    
                case INT_OSC2:
                    SysIntOsc2Sel();
                    break;
                    
                case XTAL_OSC:
                    SysXtalOscSel();
                    break;
                    
                case XTAL_OSC_SE:
                    SysXtalOscSESel();
                    break;
            }
        }
        
        //
        // Delay of at least 60 OSCCLK cycles
        //
        asm(" RPT #60 || NOP");
        
        EALLOW;
        
        //
        // Set dividers to /1 to ensure the fastest PLL configuration
        //
        ClkCfgRegs.SYSCLKDIVSEL.bit.PLLSYSCLKDIV = 0;
        
        //
        // Program PLL multipliers
        //
        ClkCfgRegs.SYSPLLMULT.all = temp_syspllmult;
        
        //
        // Enable SYSPLL
        //
        ClkCfgRegs.SYSPLLCTL1.bit.PLLEN = 1;
        
        //
        // Lock time is 1024 OSCCLK * (REFDIV+1)
        //
        timeout = (1024U * (refdiv + 1U));
        pllLockStatus = ClkCfgRegs.SYSPLLSTS.bit.LOCKS;
        
        //
        // Wait for the SYSPLL lock
        //
        while((pllLockStatus != 1) && (timeout != 0U))
        {
            pllLockStatus = ClkCfgRegs.SYSPLLSTS.bit.LOCKS;
            timeout--;
        }
        
        EDIS;
        
        //
        // Check PLL Frequency using DCC
        //
        status = IsPLLValid(dccbase, clock_source, INT_PLL_SYSPLL,
                imult, odiv , refdiv);
        
    }
    else
    {
        //
        // Re-Lock of PLL not needed since the multipliers
        // are not updated
        //
        status = true;
    }
    
    if(status)
    {
        EALLOW;
        //
        // Set divider to produce slower output frequency to limit current increase
        //
        if(divsel != 63) //PLLCLK_BY_126
        {
            ClkCfgRegs.SYSCLKDIVSEL.bit.PLLSYSCLKDIV = divsel + 1;
        }
        else
        {
            ClkCfgRegs.SYSCLKDIVSEL.bit.PLLSYSCLKDIV = divsel;
        }
        
        //
        // Enable PLLSYSCLK is fed from system PLL clock
        //
        ClkCfgRegs.SYSPLLCTL1.bit.PLLCLKEN = 1;
        
        //
        // Small 100 cycle delay
        //
        asm(" RPT #100 || NOP");
        
        //
        // Set the divider to user value
        //
        ClkCfgRegs.SYSCLKDIVSEL.bit.PLLSYSCLKDIV = divsel;
        EDIS;
    }
    else
        ESTOP0; // If the frequency is out of range, stop here.
    
}
