#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include "platform.h"

#include "xparameters.h"
#include "xil_exception.h"
#include "xstreamer.h"
#include "xil_cache.h"
#include "xllfifo.h"
#include "xstatus.h"
#include "xscugic.h"

/* base addresses */
#define FIFO_RX_BASE_ADDRESS XPAR_AXI_FIFO_0_BASEADDR
#define FIFO_TX_BASE_ADDRESS XPAR_AXI_FIFO_1_BASEADDR

/* interrupt ids */
#define FIFO_TX_IRQ XPAR_FABRIC_AXI_FIFO_MM_S_TX_INTERRUPT_INTR
#define FIFO_RX_IRQ XPAR_FABRIC_AXI_FIFO_MM_S_RX_INTERRUPT_INTR

/* device ids */
#define FIFO_TX_DEVICE_ID XPAR_AXI_FIFO_MM_S_TX_DEVICE_ID
#define FIFO_RX_DEVICE_ID XPAR_AXI_FIFO_MM_S_RX_DEVICE_ID

/* interrupt controller */
#define INTC           XScuGic
#define INTC_HANDLER   XScuGic_InterruptHandler
#define INTC_DEVICE_ID          XPAR_SCUGIC_SINGLE_DEVICE_ID

/* fifo parameters */
#define WORD_SIZE 4         /* Size of words in bytes */
#define MAX_PACKET_LEN 4
#define NO_OF_PACKETS 64
#define MAX_DATA_BUFFER_SIZE NO_OF_PACKETS*MAX_PACKET_LEN

/* Flags interrupt handlers use to notify the application context the events */
volatile int Done;
volatile int Error;

/* device instances */
XLlFifo FifoInstance_tx;
XLlFifo FifoInstance_rx;
static INTC Intc;

/* global buffers */
u32 SourceBuffer[MAX_DATA_BUFFER_SIZE * WORD_SIZE];
u32 DestinationBuffer[MAX_DATA_BUFFER_SIZE * WORD_SIZE];

/* function prototypes */
int fifo_enable(XLlFifo *InstancePtr, u16 DeviceId, u16 IRQ_ID);
int FIFOloopbackTest(XLlFifo *InstancePtr_tx, u16 DeviceId_tx);
int TxSend(XLlFifo *InstancePtr, u32 *SourceAddr);
static void FifoHandler(XLlFifo *Fifo);
static void FifoRecvHandler(XLlFifo *Fifo);
static void FifoSendHandler(XLlFifo *Fifo);
static void FifoErrorHandler(XLlFifo *InstancePtr, u32 Pending);
int SetupInterruptSystem(INTC *IntcInstancePtr, XLlFifo *InstancePtr,
                u16 FifoIntrId);
static void DisableIntrSystem(INTC *IntcInstancePtr, u16 FifoIntrId);

int main()
{
    init_platform();

    int Status;
    int i = 0;

    printf("\nTesting : ");
    printf("PS_AXI4-LITE -> PL_FIFO_TX --AXI4-STREAM--> PL_FIFO_RX -> PS_AXI4-LITE\n\r");

    /* enable tx fifo */
    Status = fifo_enable(&FifoInstance_tx, FIFO_TX_DEVICE_ID, FIFO_TX_IRQ);
    if (Status != XST_SUCCESS) {
        xil_printf("Failed to initialize TX_FIFO\n\r");
        return XST_FAILURE;
    }

    /* enable rx fifo */
    Status = fifo_enable(&FifoInstance_rx, FIFO_RX_DEVICE_ID, FIFO_RX_IRQ);
    if (Status != XST_SUCCESS) {
        xil_printf("Failed to initialize RX_FIFO\n\r");
        return XST_FAILURE;
    }

    /* transmit data from tx_fifo and wait until data is recieved from rx_fifo */
    Status = FIFOloopbackTest(&FifoInstance_tx, FIFO_TX_DEVICE_ID);
    if (Status != XST_SUCCESS) {
        xil_printf("Axi Streaming FIFO Interrupt Example Test Failed\n\r");
        return XST_FAILURE;
    }

    /* disable interrupts */
    DisableIntrSystem(&Intc, FIFO_TX_IRQ);
    DisableIntrSystem(&Intc, FIFO_RX_IRQ);

//  xil_printf("Din | Dout\n\r");
//  xil_printf("----------\n\r");
//  for(i = 0; i < MAX_DATA_BUFFER_SIZE; i++)
//      xil_printf("%03d | %03d\n\r",(int)SourceBuffer[i],(int)DestinationBuffer[i]);
    xil_printf("Axi Streaming FIFO Interrupt Example Test passed\n\r");

    cleanup_platform();
    return XST_SUCCESS;
}

int FIFOloopbackTest(XLlFifo *InstancePtrTx, u16 DeviceId_tx)
{
    /* BEGIN STREAMING EXAMPLE */
    int Status;
    int i = 0;
    int err = 0;

    /* Transmit the Data Stream */
    Status = TxSend(InstancePtrTx, SourceBuffer);
    if (Status != XST_SUCCESS){
        xil_printf("Transmission of Data failed\n\r");
        return XST_FAILURE;
    }
    while(!Done);

    /* Check for errors */
    if(Error) {
        xil_printf("Errors in the FIFO\n\r");
        return XST_FAILURE;
    }

    err = 0;

    /* Compare the data sent with the data received */
    xil_printf("Comparing data...\n\r");
    for( i=0 ; i<MAX_DATA_BUFFER_SIZE ; i++ ){
        if ( *(SourceBuffer + i) != *(DestinationBuffer + i) ){
            err++;
        }
    }

    if (err != 0){
        return XST_FAILURE;
    }

    printf("%d transmission errors occured\n\r",err);
    return Status;
}

int TxSend(XLlFifo *InstancePtr, u32  *SourceAddr)
{
    int i;
    int j;

    if(InstancePtr->BaseAddress == FIFO_RX_BASE_ADDRESS)
        xil_printf("Transmitting Data from RX_FIFO\n\r");
    else if(InstancePtr->BaseAddress == FIFO_TX_BASE_ADDRESS)
        xil_printf("Transmitting Data from TX_FIFO\n\r");

    XLlFifo_TxReset(InstancePtr);

    /* Filling the buffer with data */
    for (i=0; i < MAX_DATA_BUFFER_SIZE; i++)
        *(SourceAddr + i) = i;

    xil_printf("tx fifo vacancy (pre-send): %d\n\r",(int)XLlFifo_iTxVacancy(InstancePtr));

    for(i=0; i < NO_OF_PACKETS; i++){
        /* Writing into the FIFO Transmit Port Buffer */
        for (j=0; j < MAX_PACKET_LEN; j++){
            if( XLlFifo_iTxVacancy(InstancePtr) ){
                XLlFifo_TxPutWord(InstancePtr,
                    *(SourceAddr+(i*MAX_PACKET_LEN)+j));
            }
        }
    }

    xil_printf("tx fifo vacancy (post-send): %d\n\r",(int)XLlFifo_iTxVacancy(InstancePtr));

    /* Start Transmission by writing transmission length into the TLR */
    XLlFifo_iTxSetLen(InstancePtr, (MAX_DATA_BUFFER_SIZE * WORD_SIZE));


    /* Transmission Complete */
    return XST_SUCCESS;
}

int fifo_enable(XLlFifo *InstancePtr, u16 DeviceId, u16 IRQ_ID)
{
    XLlFifo_Config *Config;
    int Status;
    Status = XST_SUCCESS;

    /* Initialize the Device Configuration Interface driver */
    Config = XLlFfio_LookupConfig(DeviceId);
    if (!Config) {
        xil_printf("No config found for %d\r\n", DeviceId);
        return XST_FAILURE;
    }

    /*
     * This is where the virtual address would be used, this example
     * uses physical address.
     */
    Status = XLlFifo_CfgInitialize(InstancePtr, Config, Config->BaseAddress);
    if (Status != XST_SUCCESS) {
        xil_printf("fifo initialization failed\n\r");
        return Status;
    }


    /* Check for the Reset value */
    Status = XLlFifo_Status(InstancePtr);
    XLlFifo_IntClear(InstancePtr,0xffffffff);
    Status = XLlFifo_Status(InstancePtr);
    if(Status != 0x0) {
        xil_printf("\n ERROR : Reset value of ISR : 0x%x\t"
                "Expected : 0x0\n\r",
                XLlFifo_Status(InstancePtr));
        return XST_FAILURE;
    }

    /*
     * Connect the Axi Streaming FIFO to the interrupt subsystem such
     * that interrupts can occur. This function is application specific.
     */
    Status = SetupInterruptSystem(&Intc, InstancePtr, IRQ_ID);
    if (Status != XST_SUCCESS) {
        xil_printf("Failed intr setup\r\n");
        return XST_FAILURE;
    }

    XLlFifo_IntEnable(InstancePtr, XLLF_INT_ALL_MASK);

    return XST_SUCCESS;
}

int SetupInterruptSystem(INTC *IntcInstancePtr, XLlFifo *InstancePtr,
                u16 FifoIntrId)
{

    int Status;
    XScuGic_Config *IntcConfig;

    /*
     * Initialize the interrupt controller driver so that it is ready to
     * use.
     */
    IntcConfig = XScuGic_LookupConfig(INTC_DEVICE_ID);
    if (NULL == IntcConfig) {
        return XST_FAILURE;
    }

    Status = XScuGic_CfgInitialize(IntcInstancePtr, IntcConfig,
                IntcConfig->CpuBaseAddress);
    if (Status != XST_SUCCESS) {
        return XST_FAILURE;
    }

    XScuGic_SetPriorityTriggerType(IntcInstancePtr, FifoIntrId, 0xA0, 0x3);

    /*
     * Connect the device driver handler that will be called when an
     * interrupt for the device occurs, the handler defined above performs
     * the specific interrupt processing for the device.
     */
    Status = XScuGic_Connect(IntcInstancePtr, FifoIntrId,
                (Xil_InterruptHandler)FifoHandler,
                InstancePtr);
    if (Status != XST_SUCCESS) {
        return Status;
    }

    XScuGic_Enable(IntcInstancePtr, FifoIntrId);

    /*
     * Initialize the exception table.
     */
    Xil_ExceptionInit();

    /*
     * Register the interrupt controller handler with the exception table.
     */
    Xil_ExceptionRegisterHandler(XIL_EXCEPTION_ID_INT,
        (Xil_ExceptionHandler)INTC_HANDLER,
        (void *)IntcInstancePtr);;

    /*
     * Enable exceptions.
     */
    Xil_ExceptionEnable();

    return XST_SUCCESS;
}

static void DisableIntrSystem(INTC *IntcInstancePtr, u16 FifoIntrId)
{
    XScuGic_Disconnect(IntcInstancePtr, FifoIntrId);
}

static void FifoErrorHandler(XLlFifo *InstancePtr, u32 Pending)
{
    if (Pending & XLLF_INT_RPURE_MASK) {
        xil_printf("error : receive under read\n\r");
        XLlFifo_RxReset(InstancePtr);
    } else if (Pending & XLLF_INT_RPORE_MASK) {
        xil_printf("error : receive over read\n\r");
        XLlFifo_RxReset(InstancePtr);
    } else if(Pending & XLLF_INT_RPUE_MASK) {
        xil_printf("error : receive under run (empty)\n\r");
        XLlFifo_RxReset(InstancePtr);
    } else if (Pending & XLLF_INT_TPOE_MASK) {
        xil_printf("error : transmit over read\n\r");
        XLlFifo_TxReset(InstancePtr);
    } else if (Pending & XLLF_INT_TSE_MASK) {
        xil_printf("error : transmit length mismatch\n\r");
    }
    Error++;
}

static void FifoSendHandler(XLlFifo *InstancePtr)
{
    XLlFifo_IntClear(InstancePtr, XLLF_INT_TC_MASK);

    Done = 1;
}

static void FifoRecvHandler(XLlFifo *InstancePtr)
{
    int i;
    u32 RxWord;
    static u32 ReceiveLength;

    if(InstancePtr->BaseAddress == FIFO_RX_BASE_ADDRESS)
        xil_printf("Receiving Data from RX_FIFO\n\r");
    else if(InstancePtr->BaseAddress == FIFO_TX_BASE_ADDRESS)
        xil_printf("Receiving Data from TX_FIFO\n\r");

    /* Read Recieve Length */
    ReceiveLength = (XLlFifo_iRxGetLen(InstancePtr))/WORD_SIZE;
    xil_printf("rx receive length : %d\n\r", (int)ReceiveLength);
    xil_printf("rd fifo occupancy : %d\n\r",(int)XLlFifo_iRxOccupancy(InstancePtr));
//  while(XLlFifo_iRxOccupancy(InstancePtr)) {
        for (i=0; i < ReceiveLength; i++) {
                RxWord = XLlFifo_RxGetWord(InstancePtr);
                *(DestinationBuffer+i) = RxWord;
        }
//  }
}

static void FifoHandler(XLlFifo *InstancePtr)
{
    u32 Pending;

    Pending = XLlFifo_IntPending(InstancePtr);
    while (Pending) {
        if (Pending & XLLF_INT_RC_MASK) {
            FifoRecvHandler(InstancePtr);
            XLlFifo_IntClear(InstancePtr, XLLF_INT_RC_MASK);
        }
        else if (Pending & XLLF_INT_TC_MASK) {
            FifoSendHandler(InstancePtr);
        }
        else if (Pending & XLLF_INT_ERROR_MASK){
            FifoErrorHandler(InstancePtr, Pending);
            XLlFifo_IntClear(InstancePtr, XLLF_INT_ERROR_MASK);
        } else {
            XLlFifo_IntClear(InstancePtr, Pending);
        }
        Pending = XLlFifo_IntPending(InstancePtr);
    }
}