#include "system.h"

// Init system
int InitSystem()
{
    LOG_INFO("Init System Start");
    usleep(5000);

    // interupt system
    InitIntrSystem(&gic, SCUGIC_DEVICE_ID);
    LOG_INFO("Init Intr System Done");
    usleep(5000);
//
//#ifndef SIMULATION
//    // Init Uart ps
//    InitUart(&uart, &gic, UART_DEVICE_ID, UART_INTR_ID);
//    LOG_INFO("Init UartPs Done");
//    usleep(5000);
//
//#endif



    // eth
    UdpInit();
    LOG_INFO("Init Ethernet Done");
    sleep(1);
    for (int i = 0; i < 10; i++)

    {
        UdpSendData((uint8_t *)&txMacFrame, 100);
        xemacif_input(netif);
    }
    sleep(1);
    for (int i = 0; i < 10; i++)
    {
        UdpSendData((uint8_t *)&txMacFrame, 100);
        xemacif_input(netif);
    }


    return XST_SUCCESS;
}

int InitGpioPs(XGpioPs *gpioPtr, u16 deviceID)
{
    int status;
    XGpioPs_Config *gpioCfgPtr = XGpioPs_LookupConfig(deviceID);
    Xil_AssertNonvoid(gpioCfgPtr != NULL);
    status = XGpioPs_CfgInitialize(gpioPtr, gpioCfgPtr, gpioCfgPtr->BaseAddr);
    CHECK_STATUS(status, ERROR_MSG("Init GPIO fail"));
    return XST_SUCCESS;
}

int InitUart(XUartPs *uartPtr, XScuGic *gicPtr, u16 deviceID, u32 intrID)
{
    int status;
    XUartPs_Config *uartCfgPtr = XUartPs_LookupConfig(deviceID);
    status = XUartPs_CfgInitialize(uartPtr, uartCfgPtr, uartCfgPtr->BaseAddress);
    CHECK_STATUS(status, ERROR_MSG("Init Uart PS fail"));

    // set parameters
    XUartPs_SetInterruptMask(uartPtr, XUARTPS_IXR_TOUT);
    XUartPs_SetOperMode(uartPtr, XUARTPS_OPER_MODE_NORMAL);
    XUartPs_SetRecvTimeout(uartPtr, 32);

    XUartPs_Recv(&uart, uartRecvBuf, 64);
    memset(uartRecvBuf, 0, 64);


    XUartPs_WriteReg(uartPtr->Config.BaseAddress, XUARTPS_ISR_OFFSET, XUARTPS_IXR_MASK);
    return XST_SUCCESS;
}


int InitIntrSystem(XScuGic *gicPtr, u16 deviceId)
{
    int status;

    // Init gic
    XScuGic_Config *gicCfgPtr = XScuGic_LookupConfig(deviceId);
    status = XScuGic_CfgInitialize(gicPtr, gicCfgPtr, gicCfgPtr->CpuBaseAddress);
    CHECK_STATUS(status, ERROR_MSG("Init ScuGic Fails"));

    // Init Exception
    Xil_ExceptionInit();
    Xil_ExceptionRegisterHandler(XIL_EXCEPTION_ID_INT,
                                 (Xil_ExceptionHandler)XScuGic_InterruptHandler,
                                 (void *)gicPtr);
    Xil_ExceptionEnable();

    return XST_SUCCESS;
}
