/*****************************************************************************/
/**
 * Description: main.c
 *
 * Date: 2021-11-13 13:19
 * Last Edit Time: [2023-04-23 21:32]
 *
 * Revisions: Rev 0.0
 * History:
 * [2021-11-13]: Initial edition.
 * [2022-03-28]: Add bit error test mode.
 * [2023-04-23]: Bug fixed,
 *
 * Copyright(c) 2021 Beijing Institute of Technology.
 * Lab of Communication and Networking
 *
 ******************************************************************************/

/***************************** Include Files *********************************/
// Standard
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
// Xilinx
#include "xintc.h"        	// The generic interrupt controller driver component
#include "xil_exception.h"  // ARM Cortex A9 specific exception related APIs
#include "sleep.h"       	// Sleep APIs for Cortex A9
#include "xgpio.h"       	// GPIO APIs for Cortex A9
#include "xbram.h"
#include "xtmrctr.h"
#include "xsysmon.h"

// ADI Drivers
#include "dev_profile/dev_profile.h"
#include "adi_noos/drivers/axi_core/spi_engine/spi_engine.h"
#include "adi_noos/drivers/platform/gpio_extra.h"

// User
#include "common.h"
#include "error.h"
#include "config/app_config.h"
#include "c2c_bridge/c2c_bridge.h"
#include "phy_ctrl/phy_ctrl.h"
#include "circ_queue/circ_queue.h"
#include "inter_comm/inter_comm.h"

/************************** Function Prototypes ******************************/
int32_t SetupInterruptSystem(XIntc* instPtr, uint32_t devId);
int32_t XADC_Init(XSysMon* instPtr, uint16_t devId);
int32_t ScuTimerInit(XTmrCtr* instPtr, uint16_t devId);

// Utility functions
int32_t Intercom_C2cRecvData(void* buffPtr, int32_t dataLen);
int32_t Intercom_C2cSendData2Addr(void* buffPtr, int32_t dataLen, uint16_t subaddrId);

// IRQ Handlers
void C2c_RxIntrHandler(void* callBackRef);
void C2c_TxIntrHandler(void* callBackRef);
void Timer_IntrHandler(void* CallBackRef);
void PhyIntr_Handler(void *callBackRef);
/************************** Constant Definitions *****************************/
#define C2C_EXTRA_LEN	(sizeof(C2C_EXTRA) + COMMON_CRC_LEN)

C2C_CONFIG c2cConfig = {
	.capacity = C2C_BUFF_MAX,
	.dmaConfig = {
	.devId = C2C_AXI_DMA_DEVICE_ID,
	.transMax = C2C_BUFF_MAX,
	.defaultTransLen = C2C_BUFF_MAX,
	.timeoutTh = 500
	}
};

INTER_COMM_CONFIG intrCommConfig = {
	.data_recv = Intercom_C2cRecvData,
	.data_send = Intercom_C2cSendData2Addr,
	.phyInstPtr = NULL
};

PHY_CONFIG phyConfig = {
	.devId = 0,
	.baseAddr = PHY_CTRL_BASEADDR,
	.txBramId = PHY_TX_BRAM_BUFF_ID,
	.rxBramId = PHY_RX_BRAM_BUFF_ID,
	.antBramId = PHY_ANT_CONFIG_BRAM_ID

};

struct spi_engine_init_param spi_engine_param = {
	.ref_clk_hz = SPI_ENGINE_REF_CLK,
	.type = SPI_ENGINE,
	.spi_engine_baseaddr = SPI_ENGINE_BASEADDR,
	.cs_delay = SPI_ENGINE_CS_DELAY,
	.data_width = SPI_ENGINE_DATA_WIDTH
};

struct xil_gpio_init_param xil_gpio_param = {
	.type = GPIO_PS,
	.device_id = GPIO_DEVICE_ID
};

/*********************** Driver Instance Definitions **************************/
static XIntc intcInst; // General interrupt controller instance
static XSysMon SysMonInst;
#ifdef GLINK_REMOTE_TEST_ENABLE
static XTmrCtr axiTimeInst; // PS scu timer
static bool isSendRemotCheckAsserted = false;//true when 1s comes
#endif
static bool isPhyUploadAsserted = false;//true when 1s comes
static CIRC_QUEUE *c2cRecvQueue;
static PHY_INST* phyInstPtr = NULL;
static INTER_COMM_INST* intrCommInstPtr = NULL;
static C2C_EXTRA* c2cExtraBuffPtr = NULL;

// For AD9361
struct ad9516_dev *ad9516_inst;
struct ad9361_reg_dev *ad9361_1_reg_inst;
struct ad9361_reg_dev *ad9361_2_reg_inst;
struct ad9361_rf_phy *ad9361_phy;

/* Helper function for AD9361 IO control*/
static int XGpio_WritePin(XGpio *InstPtr, uint32_t Channel, u32 Pin, uint8_t Value){
	ASSERT(Pin <= 31 && Pin >=0 );
	ASSERT(InstPtr != NULL);
	ASSERT(InstPtr->IsReady == XIL_COMPONENT_IS_READY);
	ASSERT((Channel == 1) || ((Channel == 2) && (InstPtr->IsDual == TRUE)));
	u32 CurrValue = XGpio_DiscreteRead(InstPtr, Channel);
	CurrValue = (Value == 0)? (CurrValue & (~(0x00000001 << Pin))) : (CurrValue | (0x00000001 << Pin));
	XGpio_DiscreteWrite(InstPtr, Channel, CurrValue);
	return SUCCESS;
}// XGpio_WritePin

/****************************************************************************/
/**
 * This function dose a hardware initialization.
 *
 * @param    None
 *
 * @return  SUCCESS if initialize successful.
 *          XST_FAILURE if initialization failed.
 *
 *****************************************************************************/
static int Hardware_Initialize(void){
	int Status;
	XGpio Gpio;
	Status = XGpio_Initialize(&Gpio, GPIO_DEVICE_ID);
	if (Status != SUCCESS)
	{
		return FAILURE;
	}
	XGpio_SetDataDirection(&Gpio, GPIO_BANK_OUT, 0x00000000); // Bank 1 output
	XGpio_SetDataDirection(&Gpio, GPIO_BANK_IN, 0xFFFFFFFF); // Bank 0 inputT_FAILURE;

	/**********************************************************/
	/****************** AD9516 Configuration ******************/
	/**********************************************************/
	Status = ad9516_init(&ad9516_inst, &ad9516_default_init_param);
	XGpio_WritePin(&Gpio, GPIO_BANK_OUT, AD9516_POWEDOWN_B, 1);
	XGpio_WritePin(&Gpio, GPIO_BANK_OUT, AD9516_SYNC_B, 1);
	XGpio_WritePin(&Gpio, GPIO_BANK_OUT, AD9516_RESET_B, 0);
	mdelay(200);
	XGpio_WritePin(&Gpio, GPIO_BANK_OUT, AD9516_RESET_B, 1);
	for(int i = 0; i < ad9516_default_config_len; i++){
		Status = ad9516_write(ad9516_inst, ad9516_default_config[i].addr, ad9516_default_config[i].value);
		if (ad9516_default_config[i].addr == 0x0000){
			mdelay(500);
		}
	}

	uint8_t rd_back;
	Status = ad9516_read(ad9516_inst, 0x0003, &rd_back);
	do
	{
		Status = ad9516_read(ad9516_inst, 0x001F, &rd_back);
	}
	while((rd_back & 0x01) != 0x01);

	/**********************************************************/
	/***************** AD9361-1 Configuration *****************/
	/**********************************************************/
	XGpio_WritePin(&Gpio, GPIO_BANK_OUT, AD9361_RX_FIFO_RST, 1);
	XGpio_WritePin(&Gpio, GPIO_BANK_OUT, AD9361_SYNC, 0);

	ad9361_reg_init(&ad9361_1_reg_inst, &ad9361_1_reg_default_init_param);

	XGpio_WritePin(&Gpio, GPIO_BANK_OUT, AD9361_1_RESET_B, 0);
	mdelay(200);
	XGpio_WritePin(&Gpio, GPIO_BANK_OUT, AD9361_1_RESET_B, 1);

	XGpio_WritePin(&Gpio, GPIO_BANK_OUT, AD9361_1_ENABLE, 0);
	ad9361_reg_config(ad9361_1_reg_inst);
	XGpio_WritePin(&Gpio, GPIO_BANK_OUT, AD9361_1_ENABLE, 1);

	/**********************************************************/
	/***************** AD9361-2 Configuration *****************/
	/**********************************************************/
	ad9361_reg_init(&ad9361_2_reg_inst, &ad9361_2_reg_default_init_param);

	XGpio_WritePin(&Gpio, GPIO_BANK_OUT, AD9361_2_RESET_B, 0);
	mdelay(200);
	XGpio_WritePin(&Gpio, GPIO_BANK_OUT, AD9361_2_RESET_B, 1);

	XGpio_WritePin(&Gpio, GPIO_BANK_OUT, AD9361_2_ENABLE, 0);
	ad9361_reg_config(ad9361_2_reg_inst);
	XGpio_WritePin(&Gpio, GPIO_BANK_OUT, AD9361_2_ENABLE, 1);

	// sync clock output
	XGpio_WritePin(&Gpio, GPIO_BANK_OUT, AD9516_SYNC_B, 0);
	mdelay(500);
	XGpio_WritePin(&Gpio, GPIO_BANK_OUT, AD9516_SYNC_B, 1);

	// SYNC BBPLL and DIGITAL SAMPLING CLOCKS
	ad9361_reg_init(&ad9361_1_reg_inst, &ad9361_1_reg_default_init_param);
	ad9361_reg_init(&ad9361_2_reg_inst, &ad9361_2_reg_default_init_param);

	ad9361_reg_write(ad9361_1_reg_inst, 0x0001, 0x0D);    // MCS RF Enable; MCS BBPLL Enable; MCS BB Enable
	ad9361_reg_write(ad9361_2_reg_inst, 0x0001, 0x0D);
	ad9361_reg_write(ad9361_1_reg_inst, 0x0047, 0x80);    // MCS refclk Scale Enable
	ad9361_reg_write(ad9361_2_reg_inst, 0x0047, 0x80);
	XGpio_WritePin(&Gpio, GPIO_BANK_OUT, AD9361_SYNC, 1); // Input a rising-edge pulse to the SYNC_IN pin
	mdelay(500);
	XGpio_WritePin(&Gpio, GPIO_BANK_OUT, AD9361_SYNC, 0);
	mdelay(2000);
	ad9361_reg_write(ad9361_1_reg_inst, 0x0001, 0x0B);    // MCS RF Enable; MCS Digital CLK Enable ; MCS BB Enable
	ad9361_reg_write(ad9361_2_reg_inst, 0x0001, 0x0B);
	XGpio_WritePin(&Gpio, GPIO_BANK_OUT, AD9361_SYNC, 1); // Input another rising-edge pulse to the SYNC_IN pin
	mdelay(500);
	XGpio_WritePin(&Gpio, GPIO_BANK_OUT, AD9361_SYNC, 0);
	ad9361_reg_write(ad9361_1_reg_inst, 0x0001, 0x08);    // clear MCS bits
	ad9361_reg_write(ad9361_2_reg_inst, 0x0001, 0x08);

	XGpio_WritePin(&Gpio, GPIO_BANK_OUT, AD9361_RX_FIFO_RST, 0);

	return SUCCESS;
}// Hardware_Initialize

/*****************************************************************************/
/**
 * @brief   Initialize ScuTimer instance.
 *
 * @param   InstPtr         A pointer to QSPI instance
 *
 * @param   DevId           ScuTimer instance device ID
 *
 * @return  SUCCESS
 *          XST_FAILURE
 *
 ******************************************************************************/
int32_t ScuTimerInit(XTmrCtr* instPtr, uint16_t devId) {
	int Status = SUCCESS;
	XTmrCtr_Config *configPtr;
	/*
	 * Initialize the Scu Private Timer driver.
	 */
	configPtr = XTmrCtr_LookupConfig(devId);
	/*
	 * This is where the virtual address would be used, this example
	 * uses physical address.
	 */
	Status = XTmrCtr_Initialize(instPtr, configPtr->DeviceId);
	if (Status != SUCCESS) {
		return Status;
	}
	/*
	 * Perform a self-test to ensure that the hardware was built correctly.
	 */
	Status = XTmrCtr_SelfTest(instPtr, XTC_TIMER_0);

	return Status;
}// ScuTimerInit

/*****************************************************************************/
/**
 * @brief   Initialize XADC instance.
 *
 * @param   InstPtr         A pointer to QSPI instance
 *
 * @param   DevId           ScuTimer instance device ID
 *
 * @return  SUCCESS
 *          XST_FAILURE
 *
 ******************************************************************************/
int32_t XADC_Init(XSysMon* instPtr, uint16_t devId) {
	int Status = SUCCESS;
	XSysMon_Config *configPtr;

	/*
	 * Initialize the Scu Private Timer driver.
	 */
	configPtr = XSysMon_LookupConfig(devId);
	if (configPtr == NULL) {
		return XST_FAILURE;
	}

	XSysMon_CfgInitialize(instPtr, configPtr, configPtr->BaseAddress);

	//safe mode
	XSysMon_SetSequencerMode(instPtr, XSM_SEQ_MODE_SAFE);

	//enable channels
	XSysMon_SetSeqChEnables(instPtr, XSM_SEQ_CH_TEMP|XSM_SEQ_CH_VCCINT);

	/* Set the ADCCLK frequency equal to 1/32 of System clock for the System
	 * Monitor/ADC in the Configuration Register 2.
	 */
	XSysMon_SetAdcClkDivisor(instPtr, 32);
	//mode
	XSysMon_SetSequencerMode(instPtr, XSM_SEQ_MODE_CONTINPASS);

	/*
	 * Wait till the End of Sequence occurs
	 */
	XSysMon_GetStatus(instPtr); /* Clear the old status */
	while ((XSysMon_GetStatus(instPtr) & XSM_SR_EOS_MASK) !=
			XSM_SR_EOS_MASK);

	return Status;
}// XADC_Init

/****************************************************************************/
/**
 * This function is responsible for initializing the SCUGIC instance, looking
 * for the device configuration based on the unique device ID, initializing a
 * SCUGIC instance, and initialize the interrupt at IRQ_F2P[0:0]
 *
 * @param    deviceID is the XPAR_<GPIO_instance>_DEVICE_ID value from
 *           "xparameters.h".
 * @param    XIntc is a reference to the XScuGic driver Instance.
 *
 * @return  SUCCESS if initialize successful.
 *          XST_FAILURE if initialization failed.
 *
 *****************************************************************************/
int32_t SetupInterruptSystem(XIntc* instPtr, uint32_t devId){
	int Status = SUCCESS;
	// Initialize the GIC instance.
	Status = XIntc_Initialize(instPtr, devId);
	if (Status != SUCCESS) {
		return Status;
	}

	// Perform a self-test to ensure that the hardware was built correctly.
	Status = XIntc_SelfTest(instPtr);
	if (Status != SUCCESS) {
		return Status;
	}

	/* Hook up interrupt service routine */
	Status = XIntc_Connect(instPtr, PHY_INTR_ID,
			(XInterruptHandler) PhyIntr_Handler, (void *) instPtr);
	if (Status != SUCCESS) {
		return Status;
	}
	/* Enable the interrupt vector at the interrupt controller */
	XIntc_Enable(instPtr, PHY_INTR_ID);

	/* Hook up interrupt service routine */
	Status = XIntc_Connect(instPtr, C2C_TX_INTR_ID,
			(XInterruptHandler) C2c_TxIntrHandler, (void *) instPtr);
	if (Status != SUCCESS) {
		return Status;
	}
	Status = XIntc_Connect(instPtr, C2C_RX_INTR_ID,
			(XInterruptHandler) C2c_RxIntrHandler, (void *) instPtr);
	if (Status != SUCCESS) {
		return Status;
	}
	/* Enable the interrupt vector at the interrupt controller */
	XIntc_Enable(instPtr, C2C_TX_INTR_ID);
	XIntc_Enable(instPtr, C2C_RX_INTR_ID);

#ifdef GLINK_REMOTE_TEST_ENABLE
	// ScuTimer interrupt
	Status = XIntc_Connect(instPtr, SCUTIMER_INTR_ID,
			(Xil_ExceptionHandler) Timer_IntrHandler,
			(void *) &axiTimeInst);
	if (Status != SUCCESS) {
		return Status;
	}
	XIntc_Enable(instPtr, SCUTIMER_INTR_ID);
#endif

	/* Start the interrupt controller such that interrupts are recognized
	 * and handled by the processor
	 */
	Status = XIntc_Start(instPtr, XIN_REAL_MODE);
	if (Status != SUCCESS) {
		return Status;
	}

	/* Initialize the exception table and register the interrupt controller
	 * handler with the exception table
	 */
	Xil_ExceptionInit();

	Xil_ExceptionRegisterHandler(XIL_EXCEPTION_ID_INT,
			(Xil_ExceptionHandler) XIntc_InterruptHandler,
			(void *) instPtr);

	// Enable non-critical exceptions
	Xil_ExceptionEnable();

	return SUCCESS;
} // SetupInterruptSystem

/****************************************************************************/
/**
 * This is a utility function to implement a C2C data transmission.
 *
 * @param    buffPtr a pointer to data buffer, which need to be 2 byte aligned.
 * @param    dataLen length of data needs to be sent.
 *
 * @return   None.
 *
 * @note     None.
 *
 *****************************************************************************/
int32_t Intercom_C2cRecvData(void* buffPtr, int32_t buffSize){
	int Status;
	uint8_t* c2cRecvDataPtr;

	if(isCircQueueEmpty(c2cRecvQueue)){
		return EIGNORE;
	}
	else{
		Status = CircQueuePop(c2cRecvQueue, (void**)(&c2cRecvDataPtr));
		if(Status == false || c2cRecvDataPtr == NULL){
			return FAILURE;
		}
	}
	
	uint32_t recvLen = IO_In32((uintptr_t)(c2cRecvDataPtr + C2C_DLEN_OFFSET)) - C2C_HEADER_LEN;
	memcpy(buffPtr, c2cRecvDataPtr + C2C_PLOAD_OFFSET, recvLen);

	// Check if buffer is big enough.
	if(buffSize < recvLen - C2C_EXTRA_LEN){
		DBG_ERROR("Buffer too small.\r\n",);
		return FAILURE;
	}

	// Check CRC
	if(CrcCheck(buffPtr + recvLen - C2C_EXTRA_LEN, C2C_EXTRA_LEN) == true){
		// Store received data
		memcpy(c2cExtraBuffPtr, buffPtr + recvLen - C2C_EXTRA_LEN, sizeof(C2C_EXTRA));
	}
	else{
		DBG_ERROR("C2C extra crc check failed.\r\n");
		return ECRC;
	}
	return SUCCESS;
} //Intercom_C2cRecvData

/****************************************************************************/
/**
 * This is a utility function to implement a C2C data transmission.
 *
 * @param    buffPtr a pointer to data buffer, which need to be 2 byte aligned.
 * @param    dataLen length of data needs to be sent.
 * @param    subaddrId taget subAddress
 *
 * @return   None.
 *
 * @note     None.
 *
 *****************************************************************************/
int32_t Intercom_C2cSendData2Addr(void* buffPtr, int32_t dataLen, uint16_t subaddrId){
	int Status;
	if(dataLen > (C2C_BUFF_MAX - (sizeof(C2C_EXTRA)+COMMON_CRC_LEN))){
		return FAILURE;
	}
	c2cExtraBuffPtr->glinkSubAddr = subaddrId;
	memcpy((uint8_t *)(C2C_TXBUFF_ADDR_BASE + C2C_PLOAD_OFFSET), buffPtr, dataLen);
	memcpy((uint8_t *)(C2C_TXBUFF_ADDR_BASE + C2C_PLOAD_OFFSET + dataLen), c2cExtraBuffPtr, sizeof(C2C_EXTRA));
	Status = AppendCrc((uint8_t *)(C2C_TXBUFF_ADDR_BASE + C2C_PLOAD_OFFSET + dataLen), C2C_EXTRA_LEN);
	if(Status != SUCCESS){
		DBG_ERROR("C2C extra data crc append error, with error code: %d.\r\n", Status);
		return Status;
	}
	Status = C2c_BlockingSendData(c2cInstPtr, (uint8_t *)C2C_TXBUFF_ADDR_BASE, dataLen + C2C_EXTRA_LEN);
	// FIXME
	usleep(1);
	return Status;
}// Intercom_C2cSendData2Addr

/****************************************************************************/
/**
 * This function is a interrupt service routine for IRQ_F2P[1:1], IRQ_F2P[1:1]
 * tells CPU that the data in OCM is ready!
 *
 * @param    callBackRef is the SCUGIC instance pointer.
 *
 * @return   None.
 *
 * @note     None.
 *
 *****************************************************************************/
uint32_t c2cRecvCnt = 0;
void C2c_RxIntrHandler(void *callBackRef) {
	int32_t Status;
	Status = C2c_RecvIrqHandler(c2cInstPtr);
	if(Status != SUCCESS){
		_nop_();
	}
	CircQueuePush(c2cRecvQueue, (void *)c2cInstPtr->lastRxBuff);
	uint8_t* nextRxBuff = (c2cInstPtr->lastRxBuff >= C2C_RXBUFF_ADDR_BASE + (C2C_BUFF_NUM-1)*C2C_BUFF_SIZE) ?
						(uint8_t*)C2C_RXBUFF_ADDR_BASE : (uint8_t*)(c2cInstPtr->lastRxBuff + C2C_BUFF_SIZE);
	C2c_RecvData(c2cInstPtr, nextRxBuff, C2C_BUFF_SIZE);
	c2cRecvCnt ++;
	return;
}  // C2cRxIntr_Handler

/****************************************************************************/
/**
 * This function is a interrupt service routine for IRQ_F2P[1:1], IRQ_F2P[1:1]
 * tells CPU that the data in OCM is ready!
 *
 * @param    callBackRef is the SCUGIC instance pointer.
 *
 * @return   None.
 *
 * @note     None.
 *
 *****************************************************************************/
uint32_t c2cSendvCnt = 0;
void C2c_TxIntrHandler(void *callBackRef) {
	int32_t Status;
	Status = C2c_SendIrqHandler(c2cInstPtr);
	if(Status != SUCCESS){
		_nop_();
	}
	c2cSendvCnt ++;
	return;
}  // C2cTxIntr_Handler

/****************************************************************************/
/**
 * This function is a interrupt service routine for IRQ_F2P[1:1], IRQ_F2P[1:1]
 * tells CPU that the data in OCM is ready!
 *
 * @param    callBackRef is the SCUGIC instance pointer.
 *
 * @return   None.
 *
 * @note     None.
 *
 *****************************************************************************/
uint32_t phyIntrCnt = 0;
void PhyIntr_Handler(void *callBackRef) {
	phyIntrCnt++;
	Phy_UpdateState(phyInstPtr);
	// Check last frame.
	if (Phy_IsFrameValid(phyInstPtr) == true) {
		Phy_RetrieveRecvData(phyInstPtr, intrCommInstPtr->phyRecvBuff);
		isPhyUploadAsserted = true;
	}
	return;
}  // PhyIntr_Handler

/*****************************************************************************/
/**
 * @brief   Handler of ScuTimer interrupt.
 *
 * @param   CallBackRef A pointer to corresponding ScuTimer instance.
 *
 * @return  None.
 *
 ******************************************************************************/
#ifdef GLINK_REMOTE_TEST_ENABLE
void Timer_IntrHandler(void* CallBackRef) {
	ASSERT(CallBackRef != NULL);
	XTmrCtr *TimerInstPtr = (XTmrCtr *) CallBackRef;
	XTmrCtr_InterruptHandler(TimerInstPtr);
	isSendRemotCheckAsserted = true;
	return;
} //Timer_IntrHandler
#endif

/*****************************************************************************/
/**
 * @brief	Main function, initialize all the device needed, initialize
 * 			interrupt system, halt in while(1) wait for event interrupt.
 *
 * @param   None.
 *
 * @return
 *
 ******************************************************************************/
int main(void) {
	int Status;

	/*** Initialize Hardware AD9361 AD9516 ***/
	Status = Hardware_Initialize();
	if (Status == SUCCESS) {
		DBG_PRINT("Hardware Init SUCCESS! \r\n");
	}
	else {
		DBG_PRINT("Hardware Init FAILED! \r\n");
		return Status;
	}

#ifdef GLINK_REMOTE_TEST_ENABLE
	/*** Initialize Remote Test Updater Module ***/
	//-- ScuTimer Initialization
	Status = ScuTimerInit(&axiTimeInst, SCUTIMER_DEVICE_ID);
	if (Status == SUCCESS) {
		DBG_INFO("ScuTimer Init SUCCESS! \r\n");
	}
	else {
		DBG_ERROR("ScuTimer Init FAILED, ErrorCode: 0x%08X\r\n, Status");
		return Status;
	}
	XTmrCtr_InitHw(&axiTimeInst);

	//-- XADC Initialization
	Status = XADC_Init(&SysMonInst, SYSMON_DEVICE_ID);
	if (Status == SUCCESS) {
		DBG_INFO("XADC Init SUCCESS! \r\n");
	}
	else {
		DBG_ERROR("XADC Init FAILED, ErrorCode: 0x%08X\r\n, Status");
		return Status;
	}
#endif

	/*** Initialize C2C bridge ***/
	//-- Buffer Initialization
	Status = CircQueueCreate(&c2cRecvQueue, C2C_BUFF_NUM);
	if (Status == true) {
		DBG_PRINT("CircQueue Creat SUCCESS! \r\n");
	}
	else {
		DBG_PRINT("CircQueue Creat FAILED! \r\n");
		return EUNEXP;
	}

	c2cExtraBuffPtr = (C2C_EXTRA*) malloc(sizeof(C2C_EXTRA));
	if ((c2cExtraBuffPtr) == NULL) {
		DBG_PRINT("c2cExtraBuffPtr allocate FAILED! \r\n");
		return EALLOC;
	}

	//-- C2C Hardware Initialization
	Status = C2c_Init(&c2cInstPtr, &c2cConfig);
	if (Status == SUCCESS) {
		DBG_INFO("C2C bridge Init SUCCESS! \r\n");
	}
	else {
		DBG_ERROR("C2C bridge Init FAILED, ErrorCode: %d\r\n, Status");
		return Status;
	}

	//-- C2C Intercommunication Module Initialization
	intrCommConfig.phyInstPtr = phyInstPtr;
	Status = InterComm_Init(&intrCommInstPtr, &intrCommConfig);
	if (Status == SUCCESS) {
		DBG_INFO("nterComm Init SUCCESS! \r\n");
	}
	else {
		DBG_ERROR("nterComm Init FAILED, ErrorCode: %d\r\n, Status");
		return Status;
	}

	//-- Start C2C Receiving Process
	Status = C2c_RecvData(c2cInstPtr, (uint8_t*)C2C_RXBUFF_ADDR_BASE, C2C_BUFF_SIZE);
	if (Status == SUCCESS) {
		DBG_PRINT("C2C start listening SUCCESS! \r\n");
	}
	else {
		DBG_PRINT("C2C start listening FAILED! \r\n");
		return Status;
	}

	//-- Enable C2C Interrupts
	Status = C2c_IrqEnable(c2cInstPtr);
	if (Status == SUCCESS) {
		DBG_INFO("Glink interrupts enable SUCCESS!\r\n");
	}
	else {
		DBG_ERROR("Glink interrupts enable FAILED, with error code: %d.\r\n", Status);
		return Status;
	}

	/*** Initialize Physical Layer Control Module ***/
	//-- Physical Layer Hardware Initialization
	Status = Phy_InstInit(&phyInstPtr, &phyConfig);
	if (Status == SUCCESS) {
		DBG_PRINT("PhyCtrl Init SUCCESS! \r\n");
	}
	else {
		DBG_PRINT("PhyCtrl Init FAILED! \r\n");
		return Status;
	}
	//-- Set Physical Layer Mode
	Status = Phy_SetMode(phyInstPtr, PHY_MODE_PULL);
	if (Status == SUCCESS) {
		DBG_PRINT("Physical Mode %u Setup SUCCESS! \r\n", PHY_MODE_LISTEN);
	}
	else {
		DBG_PRINT("Physical Mode %u Setup FAILED! \r\n", PHY_MODE_LISTEN);
		return Status;
	}

	/*** Initialize GIC ***/
	Status = SetupInterruptSystem(&intcInst, INTC_DEVICE_ID);
	if (Status == SUCCESS) {
		DBG_INFO("Intc Init SUCCESS! \r\n");
	}
	else {
		DBG_ERROR("Intc Init FAILED, ErrorCode: %d\r\n, Status");
		return Status;
	}


#ifdef GLINK_REMOTE_TEST_ENABLE
	// Start Remote Control Timer
	XTmrCtr_SetLoadReg(axiTimeInst.BaseAddress, XTC_TIMER_0, TIMER_COUNT_MAX);
	XTmrCtr_SetControlStatusReg(axiTimeInst.BaseAddress, XTC_TIMER_0,
	         XTC_CSR_ENABLE_TMR_MASK | XTC_CSR_ENABLE_INT_MASK |
			 XTC_CSR_AUTO_RELOAD_MASK | XTC_CSR_DOWN_COUNT_MASK);
	XTmrCtr_Start(&axiTimeInst,XTC_TIMER_0);
	XTmrCtr_Start(&axiTimeInst,XTC_TIMER_0);
#endif

	DBG_PRINT("Starting Physical Layer...");
	Phy_Start(phyInstPtr);

	/*** Main Routine - Handling C2C Handshake ***/
	while(true){
		// Upload physical frame data if new frame received.
		if(isPhyUploadAsserted && !C2c_IsSendBusy(c2cInstPtr)){
			isPhyUploadAsserted = false;
			Status = InterComm_PhyDataUpload(intrCommInstPtr);
			if(Status != SUCCESS){
				DBG_PRINT("####\tInterComm L1 data upload finished with code: %d", Status);
				continue;
			}
		}

		// Handle received intercom frame if received.
		else if(isCircQueueEmpty(c2cRecvQueue) == false){
			Status = InterComm_RecvMsg(intrCommInstPtr);
			if(Status != SUCCESS){
				DBG_PRINT("####\tInterComm L1 data upload finished with code: %d.\r\n", Status);
				continue;
			}
			else{
				Status = InterComm_MsgHandler(intrCommInstPtr);
				if(Status != SUCCESS){
					DBG_PRINT("####\tInterComm message handler finished with code: %d\n\r", Status);
					continue;
				}
			}
		}
#ifdef GLINK_REMOTE_TEST_ENABLE
		// Update remote monitoring info if needed.
		else if(isSendRemotCheckAsserted && !C2c_IsSendBusy(c2cInstPtr)){
			isSendRemotCheckAsserted = false;
			Status = InterComm_SendRemotCheck(intrCommInstPtr, &SysMonInst, c2cExtraBuffPtr);
			if(Status != SUCCESS){
				DBG_PRINT("####\tInterComm L1 data upload finished with code: %d", Status);
			}
		}
#endif
	}// Waiting for interrupt

//	uint8_t * sendBuffPtr = (uint8_t *)C2C_TXBUFF_ADDR_BASE;
//	uint32_t cnt = 0;
//	while(cnt < 10240){
//		if(!C2c_IsSendBusy(c2cInstPtr)){
//			for(int i = 0; i < 512; i++){
//				sendBuffPtr[i] = i + cnt;
//			}
//			C2c_BlockingSendData(c2cInstPtr, sendBuffPtr, 512);
//			cnt++;
//		}
//	}
//	while(true);
} // main