DSP to FPGA SRIO using 8 bit device IDs

/*
 * srio_control.cpp
 *
 *  Created on: Jun 3, 2014
 *      Author: bsnedake
 */
#include <xdc/std.h>
#include <string.h>
#include <c6x.h>
#include <xdc/runtime/IHeap.h>
#include <xdc/runtime/System.h>
#include <xdc/runtime/Error.h>
#include <xdc/runtime/Memory.h>
#include <ti/sysbios/BIOS.h>
#include <ti/sysbios/knl/Task.h>
#include <ti/sysbios/heaps/HeapBuf.h>
#include <ti/sysbios/heaps/HeapMem.h>
#include <ti/sysbios/hal/Hwi.h>
#include <ti/sysbios/family/c64p/EventCombiner.h>
#include <ti/sysbios/family/c66/tci66xx/CpIntc.h>

#include <xdc/cfg/global.h>

/* SRIO Driver Include File. */
#include <ti/drv/srio/srio_drv.h>
#include <ti/drv/srio/srio_osal.h>

/* CPPI/QMSS Include Files. */
#include <ti/drv/cppi/cppi_drv.h>
#include <ti/drv/qmss/qmss_drv.h>
#include <ti/drv/qmss/qmss_firmware.h>

/* CSL Chip Functional Layer */
#include <ti/csl/csl_chip.h>

/* CSL Cache Functional Layer */
#include <ti/csl/csl_cacheAux.h>

/* PSC CSL Include Files */
#include <ti/csl/csl_psc.h>
#include <ti/csl/csl_pscAux.h>

/* CSL SRIO Functional Layer */
#include <ti/csl/csl_srio.h>
#include <ti/csl/csl_srioAux.h>

/* CSL CPINTC Include Files. */
#include<ti/csl/csl_cpIntc.h>

/**********************************************************************
 ************************** LOCAL Definitions *************************
 **********************************************************************/
/* This is the Number of host descriptors which are available & configured
 * in the memory region for this example. */
#define NUM_HOST_DESC               128

/* This is the size of each descriptor. */
#define SIZE_HOST_DESC              48

/* MTU of the SRIO Driver. We are currently operating @ MTU of 256 bytes. */
#define SRIO_MAX_MTU				256

/* This is the size of the data buffer which is used for DIO Sockets. */
//#define SIZE_DIO_PACKET				128

/* Defines the core number responsible for system initialization. */
#define CORE_SYS_INIT               0

/* Defines number of transfers done by DIO sockets in this example */
//#define SRIO_DIO_LSU_ISR_NUM_TRANSFERS      3

/* ISR timeout value (in cycles) used in this example */
#define SRIO_DIO_LSU_ISR_TIMEOUT        100000


/**********************************************************************
 ************************** Global Variables **************************
 **********************************************************************/

/* Memory allocated for the descriptors. This is 16 bit aligned. */
#pragma DATA_ALIGN (host_region, 16)
Uint8   host_region[NUM_HOST_DESC * SIZE_HOST_DESC];

/* Memory used for the accumulator list. */
#pragma DATA_ALIGN (gHiPriAccumList, 16)
UInt32              gHiPriAccumList[64];

/* Global SRIO and QMSS Configuration */
Qmss_InitCfg   qmssInitConfig;

/* Global Varialble which keeps track of the core number executing the
 * application. */
UInt32          coreNum = 0xFFFF;

/* Shared Memory Variable to ensure synchronizing SRIO initialization
 * with all the other cores. */
/* Created an array to pad the cache line with SRIO_MAX_CACHE_ALIGN size */
#pragma DATA_ALIGN   (isSRIOInitialized, 128)
#pragma DATA_SECTION (isSRIOInitialized, ".srioSharedMem");
volatile Uint32     isSRIOInitialized[(SRIO_MAX_CACHE_ALIGN / sizeof(Uint32))] = { 0 };

Srio_DrvHandle  hDrvManagedSrioDrv;

CSL_SrioHandle  hSrioCSL;

/* These are the device identifiers used in the Example Application */
const uint32_t DEVICE_ID1_16BIT    = 0xFFFF;
const uint32_t DEVICE_ID1_8BIT     = 0xCD;
const uint32_t LOCAL_BUS_DEVICE_ID  = 0xAB;
const uint32_t FPGA_DDR_DEVICE_ID   = 0x1;
const uint32_t FPGA_TDW_DEVICE_ID   = 0x2;
//const uint32_t DEVICE_ID2_16BIT    = 0x4560;
//const uint32_t DEVICE_ID2_8BIT     = 0xCD;
//const uint32_t DEVICE_ID3_16BIT    = 0x1234;
//const uint32_t DEVICE_ID3_8BIT     = 0x12;
//const uint32_t DEVICE_ID4_16BIT    = 0x5678;
//const uint32_t DEVICE_ID4_8BIT     = 0x56;

/* Array containing SRIO socket handles */
Srio_SockHandle srioSocket;

/* Global debug variable to track number of ISRs raised */
volatile UInt32 srioDbgDoorBellCnt = 0;

/* Global variable to indicate completion of ISR processing */
volatile UInt32 srioLsuIsrServiced = 0;

/* Global variables to count good and bad transfers */
//volatile UInt32 srioDioLsuGoodTransfers = 0;
//volatile UInt32 srioDioLsuBadTransfers  = 0;



/**********************************************************************
 ************************ External Definitions ************************
 **********************************************************************/

extern int32_t SrioDevice_init (void);

/* QMSS device specific configuration */
extern Qmss_GlobalConfigParams  qmssGblCfgParams;

/* CPPI device specific configuration */
extern Cppi_GlobalConfigParams  cppiGblCfgParams;

/* OSAL Data Buffer Memory Initialization. */
extern int32_t Osal_dataBufferInitMemory(uint32_t dataBufferSize);


/**
 *  @b Description
 *  @n
 *      Utility function which converts a local address to global.
 *
 *  @param[in]  addr
 *      Local address to be converted
 *
 *  @retval
 *      Global Address
 */
static UInt32 l2_global_address (Uint32 addr)
{
	UInt32 corenum;

	/* Get the core number. */
	corenum = CSL_chipReadReg(CSL_CHIP_DNUM);

	/* Compute the global address. */
	return (addr + (0x10000000 + (corenum*0x1000000)));
}

UInt32 get_core_num() {
	return coreNum;
}

UInt32 get_doorbell_isr_count() {
	return srioDbgDoorBellCnt;
}

/**
 *  @b Description
 *  @n
 *      This function enables the power/clock domains for SRIO.
 *
 *  @retval
 *      Not Applicable.
 */
static Int32 enable_srio (void)
{
    /* SRIO power domain is turned OFF by default. It needs to be turned on before doing any
     * SRIO device register access. This not required for the simulator. */

    /* Set SRIO Power domain to ON */
    CSL_PSC_enablePowerDomain (CSL_PSC_PD_SRIO);

    /* Enable the clocks too for SRIO */
    CSL_PSC_setModuleNextState (CSL_PSC_LPSC_SRIO, PSC_MODSTATE_ENABLE);

    /* Start the state transition */
    CSL_PSC_startStateTransition (CSL_PSC_PD_SRIO);

    /* Wait until the state transition process is completed. */
    while (!CSL_PSC_isStateTransitionDone (CSL_PSC_PD_SRIO));

    /* Return SRIO PSC status */
    if ((CSL_PSC_getPowerDomainState(CSL_PSC_PD_SRIO) == PSC_PDSTATE_ON) &&
        (CSL_PSC_getModuleState (CSL_PSC_LPSC_SRIO) == PSC_MODSTATE_ENABLE))
    {
        /* SRIO ON. Ready for use */
        return 0;
    }
    else
    {
        /* SRIO Power on failed. Return error */
        return -1;
    }
}


/**
 *  @b Description
 *  @n
 *      System Initialization Code. This is added here only for illustrative
 *      purposes and needs to be invoked once during initialization at
 *      system startup.
 *
 *  @retval
 *      Success     -   0
 *  @retval
 *      Error       -   <0
 */
static Int32 system_init (Void)
{
    Int32               result;
    Qmss_MemRegInfo     memRegInfo;

    /* Initialize the QMSS Configuration block. */
    memset (&qmssInitConfig, 0, sizeof (Qmss_InitCfg));

    /* Initialize the Host Region. */
    memset ((void *)&host_region, 0, sizeof(host_region));

    /* Set up the linking RAM. Use the internal Linking RAM.
     * LLD will configure the internal linking RAM address and maximum internal linking RAM size if
     * a value of zero is specified. Linking RAM1 is not used */
    qmssInitConfig.linkingRAM0Base = 0;
    qmssInitConfig.linkingRAM0Size = 0;
    qmssInitConfig.linkingRAM1Base = 0;
    qmssInitConfig.maxDescNum      = NUM_HOST_DESC;

    /* PDSP Configuration: Little Endian */
    qmssInitConfig.pdspFirmware[0].pdspId   = Qmss_PdspId_PDSP1;
    qmssInitConfig.pdspFirmware[0].firmware = (void*)acc48_le;
    qmssInitConfig.pdspFirmware[0].size     = sizeof (acc48_le);

    /* Initialize Queue Manager Sub System */
    result = Qmss_init (&qmssInitConfig, &qmssGblCfgParams);
    if (result != QMSS_SOK)
    {
        System_printf ("Error initializing Queue Manager SubSystem error code : %d\n", result);
        return -1;
    }

    /* Start the QMSS. */
    if (Qmss_start() != QMSS_SOK)
    {
        System_printf ("Error: Unable to start the QMSS\n");
        return -1;
    }

    /* Memory Region 0 Configuration */
    memRegInfo.descBase         = (UInt32 *)l2_global_address((UInt32)host_region);
    memRegInfo.descSize         = SIZE_HOST_DESC;
    memRegInfo.descNum          = NUM_HOST_DESC;
    memRegInfo.manageDescFlag   = Qmss_ManageDesc_MANAGE_DESCRIPTOR;
    memRegInfo.memRegion        = Qmss_MemRegion_MEMORY_REGION_NOT_SPECIFIED;

    /* Initialize and inset the memory region. */
    result = Qmss_insertMemoryRegion (&memRegInfo);
    if (result < QMSS_SOK)
    {
        System_printf ("Error inserting memory region: %d\n", result);
        return -1;
    }

    /* Initialize CPPI CPDMA */
    result = Cppi_init (&cppiGblCfgParams);
    if (result != CPPI_SOK)
    {
        System_printf ("Error initializing Queue Manager SubSystem error code : %d\n", result);
        return -1;
    }

    /* CPPI and Queue Manager are initialized. */
    System_printf ("Debug(Core %d): Queue Manager and CPPI are initialized.\n", coreNum);
    System_printf ("Debug(Core %d): Host Region 0x%x\n", coreNum, host_region);
    return 0;
}

/**
 * Doorbell Interrupt Service Routine
 */
uint16_t doorbellStatus = 0;
Void DoorBellIsr (UArg arg)
{
	Uint16 doorbellStatus;
	Uint8  doorbellReg = coreNum % 4;
	Uint16 mask = (Uint16)arg;
	Uint16 doorbellBits;

	/* Get the doorbell bits */
    CSL_SRIO_GetDoorbellPendingInterrupt(hSrioCSL,doorbellReg,&doorbellStatus);

    /* Mask off the bits that didn't cause the interrupt */
    doorbellBits = doorbellStatus & mask;
	if(doorbellBits > 0)
	{
		/* Clear the doorbell bits */
    	CSL_SRIO_ClearDoorbellPendingInterrupt(hSrioCSL,doorbellReg, doorbellBits);

    	/* Some cores use the high bits of the registers, shift them down if this
    	 * is one of those cores */
    	if(doorbellBits > 0xFF) {
    		doorbellBits = doorbellBits >> 8;
    	}

    	/* Check which doorbell bits were set */
    	if(doorbellBits & (1 << 0)) {
    		/* This bit is used to indicate an LSU transaction completed */
    	    srioDbgDoorBellCnt++;
    	    srioLsuIsrServiced = 1;
    	}

    	/* These bits are currently not used, just count them */
    	if(doorbellBits & (1 << 1))
    		srioDbgDoorBellCnt++;
    	if(doorbellBits & (1 << 2))
    		srioDbgDoorBellCnt++;
    	if(doorbellBits & (1 << 3))
    		srioDbgDoorBellCnt++;
    	if(doorbellBits & (1 << 4))
    		srioDbgDoorBellCnt++;
    	if(doorbellBits & (1 << 5))
    		srioDbgDoorBellCnt++;
    	if(doorbellBits & (1 << 6))
    		srioDbgDoorBellCnt++;
    	if(doorbellBits & (1 << 7))
    		srioDbgDoorBellCnt++;
    }
}


/**
 * Function to create and bind SRIO socket
 */
static Int32 create_srio_sockets() {
    UInt8                   isAllocated;
    Srio_DrvConfig          drvCfg;
    Srio_SockBindAddrInfo   bindInfo;
	Uint16                  mask;

    /* Initialize the SRIO Driver Configuration. */
    memset ((Void *)&drvCfg, 0, sizeof(Srio_DrvConfig));

    /* Initialize the OSAL */
    if (Osal_dataBufferInitMemory(SRIO_MAX_MTU) < 0)
    {
	    System_printf ("Error: Unable to initialize the OSAL. \n");
	    return -1;
    }

    /********************************************************************************
     * The SRIO Driver Instance is going to be created with the following properties:
     * - Driver Managed
     * - Interrupt Support (Pass the Rx Completion Queue as NULL)
     ********************************************************************************/

    /* Setup the SRIO Driver Managed Configuration. */
    drvCfg.bAppManagedConfig = FALSE;

    /* Driver Managed: Receive Configuration */
    drvCfg.u.drvManagedCfg.bIsRxCfgValid             = 1;
    drvCfg.u.drvManagedCfg.rxCfg.rxMemRegion         = Qmss_MemRegion_MEMORY_REGION0;
    drvCfg.u.drvManagedCfg.rxCfg.numRxBuffers        = 4;
    drvCfg.u.drvManagedCfg.rxCfg.rxMTU               = SRIO_MAX_MTU;

    /* Accumulator Configuration. */
    {
	    int32_t coreToQueueSelector[8];

	    /* This is the table which maps the core to a specific receive queue. */
	    coreToQueueSelector[0] = 704;
	    coreToQueueSelector[1] = 705;
	    coreToQueueSelector[2] = 706;
	    coreToQueueSelector[3] = 707;
	    coreToQueueSelector[4] = 708;
	    coreToQueueSelector[5] = 709;
	    coreToQueueSelector[6] = 710;
	    coreToQueueSelector[7] = 711;

	    /* Since we are programming the accumulator we want this queue to be a HIGH PRIORITY Queue */
	    drvCfg.u.drvManagedCfg.rxCfg.rxCompletionQueue = Qmss_queueOpen (Qmss_QueueType_HIGH_PRIORITY_QUEUE,
	    															     coreToQueueSelector[coreNum], &isAllocated);
		if (drvCfg.u.drvManagedCfg.rxCfg.rxCompletionQueue < 0)
		{
			System_printf ("Error: Unable to open the SRIO Receive Completion Queue\n");
			return -1;
		}

		/* Accumulator Configuration is VALID. */
		drvCfg.u.drvManagedCfg.rxCfg.bIsAccumlatorCfgValid = 1;

		/* Accumulator Configuration. */
	    drvCfg.u.drvManagedCfg.rxCfg.accCfg.channel             = coreNum;
	    drvCfg.u.drvManagedCfg.rxCfg.accCfg.command             = Qmss_AccCmd_ENABLE_CHANNEL;
	    drvCfg.u.drvManagedCfg.rxCfg.accCfg.queueEnMask         = 0;
	    drvCfg.u.drvManagedCfg.rxCfg.accCfg.queMgrIndex         = coreToQueueSelector[coreNum];
	    drvCfg.u.drvManagedCfg.rxCfg.accCfg.maxPageEntries      = 2;
	    drvCfg.u.drvManagedCfg.rxCfg.accCfg.timerLoadCount      = 0;
	    drvCfg.u.drvManagedCfg.rxCfg.accCfg.interruptPacingMode = Qmss_AccPacingMode_LAST_INTERRUPT;
	    drvCfg.u.drvManagedCfg.rxCfg.accCfg.listEntrySize       = Qmss_AccEntrySize_REG_D;
	    drvCfg.u.drvManagedCfg.rxCfg.accCfg.listCountMode       = Qmss_AccCountMode_ENTRY_COUNT;
	    drvCfg.u.drvManagedCfg.rxCfg.accCfg.multiQueueMode      = Qmss_AccQueueMode_SINGLE_QUEUE;

        /* Initialize the accumulator list memory */
        memset ((Void *)&gHiPriAccumList[0], 0, sizeof(gHiPriAccumList));
        drvCfg.u.drvManagedCfg.rxCfg.accCfg.listAddress = l2_global_address((UInt32)&gHiPriAccumList[0]);
    }

    /* Driver Managed: Transmit Configuration */
    drvCfg.u.drvManagedCfg.bIsTxCfgValid             = 1;
    drvCfg.u.drvManagedCfg.txCfg.txMemRegion         = Qmss_MemRegion_MEMORY_REGION0;
    drvCfg.u.drvManagedCfg.txCfg.numTxBuffers        = 4;
    drvCfg.u.drvManagedCfg.txCfg.txMTU               = SRIO_MAX_MTU;

    /* Start the Driver Managed SRIO Driver. */
    hDrvManagedSrioDrv = Srio_start(&drvCfg);
    if (hDrvManagedSrioDrv == NULL)
    {
        System_printf ("Error(Core %d): SRIO Driver failed to start\n", coreNum);
        return -1;
    }

    System_printf ("*************************************************************\n");
    System_printf ("******* DIO Socket Example with Interrupts (Core %d) ********\n", coreNum);
    System_printf ("*************************************************************\n");

    /* Get the CSL SRIO Handle. */
    hSrioCSL = CSL_SRIO_Open (0);
    if (hSrioCSL == NULL)
    	return -1;

    /**************************************************************
     *                   INTERRUPT SETUP                          *
     **************************************************************/

    /***************** Doorbell Interrupts ************************/

    /*
     * Doorbell Interrupt Destinations 0 - 7 are routed to System Events INTDST 16 - 23
     * respectively. This routing is handled in srio_device.  The C66x CorePac routes
     * System Events INTDST(coreNum + 16) to Event Number 20, so no special CorePac
     * routing is needed to expose the doorbell interrupts.
     */
    Hwi_Params hwiParams;
    Error_Block eb;

    Hwi_Params_init(&hwiParams);
    Error_init(&eb);

    /*
     * Core numbers 0 - 3 get the lower 7 bits of Doorbell registers 0 - 3 respectively.
     * Core numbers 4 - 7 get the upper 7 bits of Doorbell registers 0 - 3 respectively.
     * Make the arg be the mask of the bits for the doorbell
     */
    if(coreNum < 4) {
    	mask = 0x00FF;
    } else {
    	mask = 0xFF00;
    }

    hwiParams.arg = mask;

    /* Set the event id */
    hwiParams.eventId = 20;

    /* don't allow this interrupt to nest itself */
    hwiParams.maskSetting = Hwi_MaskingOption_SELF;

    /*
     * Configure interrupt 4 to invoke the interrupt handler for the doorbells.
     * This will automatically enable the interrupt
     */
    Hwi_create(4, DoorBellIsr, &hwiParams, &eb);

    /*
     * Check the error code to make sure the interrupt handler was created
     */
    if (Error_check(&eb)) {
        System_printf("Error creating Doorbell ISR\n");
    }

    /****************** LSU Interrupts *****************************/
    /* LSU Interrupts are routed based on the srcID in the socket binding.  Each core
     * is going to be creating two sockets, and the interrupts from both sockets
     * will be routed to a common interrupt destination (INTDST).  Routing is
     * done through the Interrupt Condition Routing (ICR) register, and will
     * be set up according to this table:
     *
     *   Core  SysID  ICR  INTDST  Interrupts Expected
     *   ----  -----  ---  ------  -------------------
     *    0      0     0      0           N
     *    0      1     1      0           Y
     *    1      2     2      1           N
     *    1      3     3      1           Y
     *    2      4     4      2           N
     *    2      5     5      2           Y
     *    3      6     6      3           N
     *    3      7     7      3           Y
     *    4      8     8      4           N
     *    4      9     9      4           Y
     *    5     10    10      5           N
     *    5     11    11      5           Y
     *    6     12    12      6           N
     *    6     13    13      6           Y
     *    7     14    14      7           N
     *    7     15    15      7           Y
     */

//    /* Disable Interrupt Pacing for the INTDST */
//    CSL_SRIO_DisableInterruptPacing (hSrioCSL, coreNum);
//
//    /* Route LSU ICR to INTDST. This is the NO_INTERRUPT socket, so nothing is expected from this */
//    CSL_SRIO_RouteLSUInterrupts (hSrioCSL, coreNum*2, coreNum);
//
//    /* Route LSU ICR to INTDST.  This is the INTERRUPT socket, so interrupts are expected from this */
//    CSL_SRIO_RouteLSUInterrupts (hSrioCSL, coreNum*2+1, coreNum);
//
//    /* The INTDST 0-7 lines do not route directly to the CorePac, instead we need to go
//     * through the Chip Interrupt Controller (Sometimes called CIC, sometimes called INTC).
//     * There are two CIC's, CIC0 routes to cores 0-3, CIC1 routes to cores 4-7.  Figure out
//     * which CIC and INTDST combo is needed based on the core number */
//    switch(coreNum) {
//    case 0:
//    	cic = 0;
//    	intcIntdst = CSL_INTC0_INTDST0;
//    	break;
//    case 1:
//    	cic = 0;
//    	intcIntdst = CSL_INTC0_INTDST1;
//    	break;
//    case 2:
//    	cic = 0;
//    	intcIntdst = CSL_INTC0_INTDST2;
//    	break;
//    case 3:
//    	cic = 0;
//    	intcIntdst = CSL_INTC0_INTDST3;
//    	break;
//    case 4:
//    	cic = 1;
//    	intcIntdst = CSL_INTC1_INTDST4;
//    	break;
//    case 5:
//    	cic = 1;
//    	intcIntdst = CSL_INTC1_INTDST5;
//    	break;
//    case 6:
//    	cic = 1;
//    	intcIntdst = CSL_INTC1_INTDST6;
//    	break;
//    case 7:
//    	cic = 1;
//    	intcIntdst = CSL_INTC1_INTDST7;
//    	break;
//    default:
//    	// This should never happen
//    	System_printf("Unknown core number %d\n", coreNum);
//    	intcIntdst = CSL_INTC0_INTDST0;
//    }
//
//    /* The CIC's are configured through the CorePac Interrupt Controller (CpIntc).  Plug the
//     * CIC INTDST System Interrupt with our Interrupt Service Routine */
//    CpIntc_dispatchPlug(intcIntdst, (CpIntc_FuncPtr)myDioTxCompletionIsr, (UArg)hDrvManagedSrioDrv, TRUE);
//
//    /* The INTDST's are System Interrupts, these need to be mapped to Host Interrupts, which are
//     * linked to CorePac Event ID's.  CorePac Event ID 92 is mapped according to this formula:
//     *    CIC0_OUT(CoreNum * 8 + 4) for core's 0 - 3
//     *    CIC1_OUT((CoreNum-4) * 8 + 4) for core's 4 - 7
//     *
//     *  Compute the Host Interrupt number based on the coreNum */
//    if(coreNum < 4) {
//    	hostInt = coreNum * 8 + 4;
//    } else {
//    	hostInt = (coreNum - 4) * 8 + 4;
//    }
//
//    /* Map the INTDST System Interrupt to the Host Interrupt */
//    CpIntc_mapSysIntToHostInt(cic, intcIntdst, hostInt);
//
//    /* Enable Host Interrupt */
//    CpIntc_enableHostInt(cic, hostInt);
//
//    /* Enable System Interrupt */
//    CpIntc_enableSysInt(cic, intcIntdst);
//
//    /* Get the CorePac event number for the Host Interrupt.  This should always return 92 */
//    eventId = CpIntc_getEventId(hostInt);
//
//    /* Setup the HW Interrupt Parameters */
//    Hwi_Params_init(&hwiParams);
//    hwiParams.arg       = hostInt; /* Interrupts don't seem to work without arg set to hostInt */
//    hwiParams.eventId   = eventId;
//    hwiParams.enableInt = TRUE;
//
//    /* Configure interrupt 5 to invoke the interrupt.  Because we had to map
//     * the interrupt through the CpIntc, we need to route this interrupt through
//     * the CpIntc dispatcher, which will map it to our ISR plug.  This call
//     * will automatically enable the interrupt */
//    Hwi_create(5, &CpIntc_dispatch, &hwiParams, &eb);
//
//    /*
//     * Check the error code to make sure the interrupt handler was created
//     */
//    if (Error_check(&eb)) {
//        System_printf("Error creating Doorbell ISR\n");
//    }

    /*****************************************************************
     *                     Create the Sockets                        *
     *****************************************************************/

    /* Open DIO SRIO Non-Blocking Socket */
    srioSocket = Srio_sockOpen (hDrvManagedSrioDrv, Srio_SocketType_DIO, FALSE);
    if (srioSocket == NULL)
    {
    	System_printf ("Error: Unable to open the DIO socket\n");
    	return -1;
    }

    /* DIO Binding Information: Use 8 bit identifiers and we are bound to the first source id.
     * and we are using 8 bit device identifiers. */
    bindInfo.dio.doorbellValid  = 1;
    bindInfo.dio.intrRequest    = 0;
    bindInfo.dio.supInt         = 0;
    bindInfo.dio.xambs          = 0;
    bindInfo.dio.priority       = 0;
    bindInfo.dio.outPortID      = 0;
    bindInfo.dio.idSize         = 1;
    bindInfo.dio.srcIDMap       = 0;
    bindInfo.dio.hopCount       = 0;
    bindInfo.dio.doorbellReg    = coreNum % 4;
    bindInfo.dio.doorbellBit    = 8 * (coreNum / 4);

    /* Bind the SRIO socket: DIO sockets do not need any binding information. */
    if (Srio_sockBind_DIO (srioSocket, &bindInfo) < 0)
    {
    	System_printf ("Error: Binding the SIO socket failed.\n");
    	return -1;
    }

    return 0;
}

/**
 * SRIO Doorbell Send Function
 */
Int32 srio_send_doorbell(uint32_t doorbellRegister, uint32_t doorbellBit) {
	Srio_SockAddrInfo       to;
	uint32_t                doorbellInfo;

	to.dio.rapidIOMSB = 0x0;
	to.dio.rapidIOLSB = 0x0;
	to.dio.dstID      = LOCAL_BUS_DEVICE_ID;
	to.dio.ttype      = Srio_Ttype_Doorbell_DEFAULT;
	to.dio.ftype      = Srio_Ftype_DOORBELL;

	doorbellInfo = SRIO_SET_DBELL_INFO(doorbellRegister, doorbellBit);

	if (Srio_sockSend(srioSocket, (Srio_DrvBuffer)doorbellInfo, 0, &to) < 0) {
		System_printf ("Debug(Core %d): Doorbell Send Failed\n", coreNum);
		return -1;
	}

	return 0;
}

/**
 * SRIO Transfer Function
 */
Int32 srio_transfer(uint32_t destination, uint8_t dioFtype, uint8_t dioTtype, uint32_t hwRegAddress, uint32_t dspRegAddress, uint32_t numRegisters, uint32_t waitForCompletion) {
	Srio_SockAddrInfo       to;
	int32_t startTime;

    /* Populate the DIO Address Information where the data is to be sent. */
    to.dio.rapidIOMSB    = 0x0;
    to.dio.rapidIOLSB    = hwRegAddress;
    to.dio.dstID         = destination;
    to.dio.ttype         = dioTtype;
    to.dio.ftype         = dioFtype;

    if (waitForCompletion) {
    	srioLsuIsrServiced = 0;
    }

    /* Send the DIO Information. */
    if (Srio_sockSend_DIO (srioSocket, (Srio_DrvBuffer)dspRegAddress, numRegisters*4, (Srio_SockAddrInfo*)&to) < 0)
    {
        System_printf ("Debug(Core %d): Socket Send Failed\n", coreNum);
        return -1;
    }

    if(waitForCompletion) {
    	/* Wait for the interrupt to occur without touching the peripheral. */
    	/* Other useful work could be done here such as by invoking a scheduler */
    	startTime = TSCL;
    	while((! srioLsuIsrServiced) && ((TSCL - startTime) < SRIO_DIO_LSU_ISR_TIMEOUT));

    	if (! srioLsuIsrServiced) {
    		System_printf ("ISR didn't happen within set time - %d cycles. Example failed !!!\n", SRIO_DIO_LSU_ISR_TIMEOUT);
    		return -1;
    	}
    }

    return 0;

}

/**
 * Function to read data over SRIO
 */
Int32 srio_read_regs(uint32_t* hw_reg_address, uint32_t* dest_reg_address, uint32_t numRegisters) {
	Uint32 global_hw_reg_address   = l2_global_address((Uint32)hw_reg_address);
	//Uint32 global_hw_reg_address   = (Uint32)hw_reg_address;
	Uint32 global_dest_reg_address = l2_global_address((Uint32)dest_reg_address);

	/* Transfer the data over the socket that generates an interrupt */
	return srio_transfer(
			FPGA_DDR_DEVICE_ID,
			Srio_Ftype_REQUEST,
			Srio_Ttype_Request_NREAD,
			global_hw_reg_address,
			global_dest_reg_address,
			numRegisters,
			1);
}

/**
 * Function to write data over SRIO
 */
Int32 srio_write_regs(uint32_t* hw_reg_address, uint32_t* source_reg_address, uint32_t numRegisters) {
	Uint32 global_hw_reg_address     = l2_global_address((Uint32)hw_reg_address);
	//Uint32 global_hw_reg_address   = (Uint32)hw_reg_address;
	Uint32 global_source_reg_address = l2_global_address((Uint32)source_reg_address);

	/* Transfer the data over the socket that does not generate an interrupt */
	return srio_transfer(
			FPGA_DDR_DEVICE_ID,
			Srio_Ftype_WRITE,
			Srio_Ttype_Write_NWRITE_R,
			global_hw_reg_address,
			global_source_reg_address,
			numRegisters,
			0);
}

/**
 * Function to initialize SRIO
 */
void srio_setup() {

    /* Get the core number. */
	coreNum = CSL_chipReadReg (CSL_CHIP_DNUM);

    System_printf ("Initializing SRIO DEVICE\n");

    /* Initialize the system only if the core was configured to do so. */
    if (coreNum == CORE_SYS_INIT)
    {
        System_printf ("Debug(Core %d): System Initialization for CPPI & QMSS\n", coreNum);

        /* System Initialization */
        if (system_init() < 0)
            return;

        /* Power on SRIO peripheral before using it */
        if (enable_srio () < 0)
        {
            System_printf ("Error: SRIO PSC Initialization Failed\n");
            return;
        }

	    /* Device Specific SRIO Initializations: This should always be called before
         initializing the SRIO Driver.*/
    	if (SrioDevice_init() < 0)
        	return;


        /* Initialize the SRIO Driver */
        if (Srio_init () < 0)
        {
            System_printf ("Error: SRIO Driver Initialization Failed\n");
            return;
        }

        /* SRIO Driver is operational at this time. */
        System_printf ("Debug(Core %d): SRIO Driver has been initialized\n", coreNum);

        /* Write to the SHARED memory location at this point in time. The other cores cannot execute
         * till the SRIO Driver is up and running. */
        isSRIOInitialized[0] = 1;

        /* The SRIO IP block has been initialized. We need to writeback the cache here because it will
         * ensure that the rest of the cores which are waiting for SRIO to be initialized would now be
         * woken up. */
        CACHE_wbL1d ((Uint32 *) &isSRIOInitialized[0], 128, CACHE_WAIT);
    }
    else
    {
        /* All other cores need to wait for the SRIO to be initialized before they proceed. */
        System_printf ("Debug(Core %d): Waiting for SRIO to be initialized.\n", coreNum);

        /* All other cores loop around forever till the SRIO is up and running.
         * We need to invalidate the cache so that we always read this from the memory. */
        while (isSRIOInitialized[0] == 0 )
            CACHE_invL1d ((Uint32 *) &isSRIOInitialized[0], 128, CACHE_WAIT);

        /* Start the QMSS. */
        if (Qmss_start() != QMSS_SOK)
        {
            System_printf ("Error: Unable to start the QMSS\n");
            return;
        }

        System_printf ("Debug(Core %d): SRIO can now be used.\n", coreNum);
    }

    create_srio_sockets();
}

/**
 *   @file  device_srio.c
 *
 *   @brief
 *      The 6608 SRIO Device specific code. The SRIO driver calls out
 *      this code to initialize the SRIO IP block. The file is provided as
 *      a sample configuration and should be modified by customers for
 *      their own platforms and configurations.
 *
 *  \par
 *  NOTE:
 *      (C) Copyright 2010 Texas Instruments, Inc.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *    Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 *    Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the
 *    distribution.
 *
 *    Neither the name of Texas Instruments Incorporated nor the names of
 *    its contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 *  A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 *  OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 *  SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 *  LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 *  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 *  THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 *  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 *  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *  \par
*/

/* SRIO Driver Includes. */
#include <ti/drv/srio/srio_types.h>
#include <ti/drv/srio/include/listlib.h>
#include <ti/drv/srio/srio_drv.h>

/* CSL SRIO Functional Layer */
#include <ti/csl/csl_srio.h>
#include <ti/csl/csl_srioAux.h>
#include <ti/csl/csl_srioAuxPhyLayer.h>

/* CSL BootCfg Module */
#include <ti/csl/csl_bootcfg.h>
#include <ti/csl/csl_bootcfgAux.h>

/* CSL Chip Functional Layer */
#include <ti/csl/csl_chip.h>

/* CSL PSC Module */
#include <ti/csl/csl_pscAux.h>

/* QMSS Include */
#include <ti/drv/qmss/qmss_drv.h>

/**********************************************************************
 ************************* LOCAL Definitions **************************
 **********************************************************************/

/* These are the GARBAGE queues which are used by the TXU to dump the 
 * descriptor if there is an error instead of recycling the descriptor
 * to the free queue. */
#define GARBAGE_LEN_QUEUE		    905
#define GARBAGE_TOUT_QUEUE		    906
#define GARBAGE_RETRY_QUEUE		    907
#define GARBAGE_TRANS_ERR_QUEUE	    908
#define GARBAGE_PROG_QUEUE		    909
#define GARBAGE_SSIZE_QUEUE		    910

/* SRIO Device Information
 * - 16 bit Device Identifier.
 * - 8 bit Device Identifier.
 * - Vendor Identifier. 
 * - Device Revision. */
#define DEVICE_VENDOR_ID            0x30
#define DEVICE_REVISION             0x0

/* SRIO Assembly Information
 * - Assembly Identifier
 * - Assembly Vendor Identifier. 
 * - Assembly Device Revision. 
 * - Assembly Extension Features */
#define DEVICE_ASSEMBLY_ID          0x0
#define DEVICE_ASSEMBLY_VENDOR_ID   0x30
#define DEVICE_ASSEMBLY_REVISION    0x0
#define DEVICE_ASSEMBLY_INFO        0x0100
/**********************************************************************
 ************************* Extern Definitions *************************
 **********************************************************************/

extern const uint32_t DEVICE_ID1_16BIT;
extern const uint32_t DEVICE_ID1_8BIT;
extern const uint32_t DEVICE_ID2_16BIT;
extern const uint32_t DEVICE_ID2_8BIT;
extern const uint32_t DEVICE_ID3_16BIT;
extern const uint32_t DEVICE_ID3_8BIT_ID;
extern const uint32_t DEVICE_ID4_16BIT;
extern const uint32_t DEVICE_ID4_8BIT_ID;

/**********************************************************************
 *********************** DEVICE SRIO FUNCTIONS ***********************
 **********************************************************************/

/** @addtogroup SRIO_DEVICE_API
 @{ */

/**
 *  @b Description
 *  @n
 *      The function provides the initialization sequence for the SRIO IP
 *      block. This can be modified by customers for their application and
 *      configuration.
 *
 *  @retval
 *      Success     -   0
 *  @retval
 *      Error       -   <0
 */
#pragma CODE_SECTION(SrioDevice_init, ".text:SrioDevice_init");
int32_t SrioDevice_init (void)
{
    CSL_SrioHandle      hSrio;
	int32_t             i;
    SRIO_PE_FEATURES    peFeatures;
    SRIO_OP_CAR         opCar;
    Qmss_QueueHnd       queueHnd;
    uint8_t             isAllocated;
    uint32_t            gargbageQueue[] = { GARBAGE_LEN_QUEUE,  GARBAGE_TOUT_QUEUE,
                                            GARBAGE_RETRY_QUEUE,GARBAGE_TRANS_ERR_QUEUE,
                                            GARBAGE_PROG_QUEUE, GARBAGE_SSIZE_QUEUE };

    /* Get the CSL SRIO Handle. */
    hSrio = CSL_SRIO_Open (0);
    if (hSrio == NULL)
        return -1;

    /* Code to disable SRIO reset isolation */
    if (CSL_PSC_isModuleResetIsolationEnabled(CSL_PSC_LPSC_SRIO))
        CSL_PSC_disableModuleResetIsolation(CSL_PSC_LPSC_SRIO);

    /* Disable the SRIO Global block */
   	CSL_SRIO_GlobalDisable (hSrio);

   	/* Disable each of the individual SRIO blocks. */
   	for(i = 0; i <= 9; i++)
   		CSL_SRIO_DisableBlock(hSrio, i);

    /* Set boot complete to be 0; we are not done with the initialization. */
	CSL_SRIO_SetBootComplete(hSrio, 0);

    /* Now enable the SRIO block and all the individual blocks also. */
    CSL_SRIO_GlobalEnable (hSrio);
    for(i = 0; i <= 9; i++)
        CSL_SRIO_EnableBlock(hSrio,i);

    /* Configure SRIO ports to operate in loopback mode. */
    //CSL_SRIO_SetLoopbackMode(hSrio, 0);
    //CSL_SRIO_SetLoopbackMode(hSrio, 1);
    //CSL_SRIO_SetLoopbackMode(hSrio, 2);
    //CSL_SRIO_SetLoopbackMode(hSrio, 3);

	/* Enable Automatic Priority Promotion of response packets. */
	CSL_SRIO_EnableAutomaticPriorityPromotion(hSrio);

	/* Set the SRIO Prescalar select to operate in the range of 44.7 to 89.5 */
	CSL_SRIO_SetPrescalarSelect (hSrio, 0);

    /* Unlock the Boot Configuration Kicker */
    CSL_BootCfgUnlockKicker ();

    /* Assuming the link rate is 312.5; program the PLL accordingly. */
    CSL_BootCfgSetSRIOSERDESConfigPLL (0x241);

	/* Configure the SRIO SERDES Receive Configuration. */
	//CSL_BootCfgSetSRIOSERDESRxConfig (0, 0x00440495);
	//CSL_BootCfgSetSRIOSERDESRxConfig (1, 0x00440495);
	//CSL_BootCfgSetSRIOSERDESRxConfig (2, 0x00440495);
	//CSL_BootCfgSetSRIOSERDESRxConfig (3, 0x00440495);


	/* Configure the SRIO SERDES Transmit Configuration. */
	//CSL_BootCfgSetSRIOSERDESTxConfig (0, 0x00180795);
	//CSL_BootCfgSetSRIOSERDESTxConfig (1, 0x00180795);
	//CSL_BootCfgSetSRIOSERDESTxConfig (2, 0x00180795);
	//CSL_BootCfgSetSRIOSERDESTxConfig (3, 0x00180795);

	/*1 lane at 1/4 speed*/
	/* Configure the SRIO SERDES Receive Configuration. */
	CSL_BootCfgSetSRIOSERDESRxConfig (0, 0x004404B5);

	/* Configure the SRIO SERDES Transmit Configuration. */
	CSL_BootCfgSetSRIOSERDESTxConfig (0, 0x001807B5);


#ifndef SIMULATOR_SUPPORT
    /* Loop around till the SERDES PLL is not locked. */
    while (1)
    {
        uint32_t    status;

        /* Get the SRIO SERDES Status */
        CSL_BootCfgGetSRIOSERDESStatus(&status);
        if (status & 0x1)
            break;
    }
#endif

    /* Clear the LSU pending interrupts. */
    CSL_SRIO_ClearLSUPendingInterrupt (hSrio, 0xFFFFFFFF, 0xFFFFFFFF);

    /* Clear the doorbell pending interrupts */
    for(i = 0; i < 4; i++)
    	CSL_SRIO_ClearDoorbellPendingInterrupt(hSrio, i, 0xFFFF);

    /* Set the Device Information */
    CSL_SRIO_SetDeviceInfo (hSrio, DEVICE_ID1_16BIT, DEVICE_VENDOR_ID, DEVICE_REVISION);

    /* Set the Assembly Information */
    CSL_SRIO_SetAssemblyInfo(hSrio, DEVICE_ASSEMBLY_ID, DEVICE_ASSEMBLY_VENDOR_ID,
                             DEVICE_ASSEMBLY_REVISION, DEVICE_ASSEMBLY_INFO);

    /* TODO: Configure the processing element features
     *  The SRIO RL file is missing the Re-transmit Suppression Support (Bit6) field definition */
    peFeatures.isBridge                          = 0;
    peFeatures.isEndpoint                        = 0;
    peFeatures.isProcessor                       = 1;
    peFeatures.isSwitch                          = 0;
    peFeatures.isMultiport                       = 0;
    peFeatures.isFlowArbiterationSupported       = 0;
    peFeatures.isMulticastSupported              = 0;
    peFeatures.isExtendedRouteConfigSupported    = 0;
    peFeatures.isStandardRouteConfigSupported    = 1;
    peFeatures.isFlowControlSupported            = 1;
    peFeatures.isCRFSupported                    = 0;
    peFeatures.isCTLSSupported                   = 1;
    peFeatures.isExtendedFeaturePtrValid         = 1;
    peFeatures.numAddressBitSupported            = 1;
    CSL_SRIO_SetProcessingElementFeatures (hSrio, &peFeatures);

    /* Configure the source operation CAR */
    memset ((void *) &opCar, 0, sizeof (opCar));
    opCar.portWriteOperationSupport = 1;
    opCar.atomicClearSupport        = 1;
    opCar.atomicSetSupport          = 1;
    opCar.atomicDecSupport          = 1;
    opCar.atomicIncSupport          = 1;
    opCar.atomicTestSwapSupport     = 1;
    opCar.doorbellSupport           = 1;
    opCar.dataMessageSupport        = 1;
    opCar.writeResponseSupport      = 1;
    opCar.streamWriteSupport        = 1;
    opCar.writeSupport              = 1;
    opCar.readSupport               = 1;
    opCar.dataStreamingSupport      = 1;
    CSL_SRIO_SetSourceOperationCAR (hSrio, &opCar);

    /* Configure the destination operation CAR */
	memset ((void *) &opCar, 0, sizeof (opCar));
    opCar.portWriteOperationSupport  = 1;
    opCar.doorbellSupport            = 1;
    opCar.dataMessageSupport         = 1;
    opCar.writeResponseSupport       = 1;
    opCar.streamWriteSupport         = 1;
    opCar.writeSupport               = 1;
    opCar.readSupport                = 1;
    CSL_SRIO_SetDestOperationCAR (hSrio, &opCar);

    /* Set the 16 bit and 8 bit identifier for the SRIO Device. */
    CSL_SRIO_SetDeviceIDCSR (hSrio, DEVICE_ID1_8BIT, DEVICE_ID1_16BIT);

    /* Enable TLM Base Routing Information for Maintainance Requests & ensure that
     * the BRR's can be used by all the ports. */
    /* Note: Port 0, BRR 0 is enabled by default, so it doesn't need to
     * be set up specifically like all the other Port/BRR combinations */
    CSL_SRIO_SetTLMPortBaseRoutingInfo(hSrio, 0, 1, 1, 1, 0);
    CSL_SRIO_SetTLMPortBaseRoutingInfo(hSrio, 0, 2, 1, 1, 0);
    CSL_SRIO_SetTLMPortBaseRoutingInfo(hSrio, 0, 3, 1, 1, 0);
    CSL_SRIO_SetTLMPortBaseRoutingInfo(hSrio, 1, 0, 1, 1, 0);
    CSL_SRIO_SetTLMPortBaseRoutingInfo(hSrio, 1, 1, 1, 1, 0);
    CSL_SRIO_SetTLMPortBaseRoutingInfo(hSrio, 1, 2, 1, 1, 0);
    CSL_SRIO_SetTLMPortBaseRoutingInfo(hSrio, 1, 3, 1, 1, 0);
    CSL_SRIO_SetTLMPortBaseRoutingInfo(hSrio, 2, 0, 1, 1, 0);
    CSL_SRIO_SetTLMPortBaseRoutingInfo(hSrio, 2, 1, 1, 1, 0);
    CSL_SRIO_SetTLMPortBaseRoutingInfo(hSrio, 2, 2, 1, 1, 0);
    CSL_SRIO_SetTLMPortBaseRoutingInfo(hSrio, 2, 3, 1, 1, 0);
    CSL_SRIO_SetTLMPortBaseRoutingInfo(hSrio, 3, 0, 1, 1, 0);
    CSL_SRIO_SetTLMPortBaseRoutingInfo(hSrio, 3, 1, 1, 1, 0);
    CSL_SRIO_SetTLMPortBaseRoutingInfo(hSrio, 3, 2, 1, 1, 0);
    CSL_SRIO_SetTLMPortBaseRoutingInfo(hSrio, 3, 3, 1, 1, 0);

    /* Configure the Base Routing Register to ensure that all packets matching the 
     * Device Identifier & the Secondary Device Id are admitted. */
    CSL_SRIO_SetTLMPortBaseRoutingPatternMatch(hSrio, 0, 1, DEVICE_ID1_16BIT, 0xFFFF);
    //CSL_SRIO_SetTLMPortBaseRoutingPatternMatch(hSrio, 0, 2, DEVICE_ID3_16BIT, 0xFFFF);
    //CSL_SRIO_SetTLMPortBaseRoutingPatternMatch(hSrio, 0, 3, DEVICE_ID4_16BIT, 0xFFFF);
    //CSL_SRIO_SetTLMPortBaseRoutingPatternMatch(hSrio, 1, 0, DEVICE_ID1_8BIT,  0xFF);

    /* We need to open the Garbage collection queues in the QMSS. This is done to ensure that 
     * these queues are not opened by another system entity. */
    for (i = 0; i < 6; i++)
    {
        /* Open the Garabage queues */
        queueHnd = Qmss_queueOpen (Qmss_QueueType_GENERAL_PURPOSE_QUEUE, gargbageQueue[i], &isAllocated);
        if (queueHnd < 0)
            return -1;

        /* Make sure the queue has not been opened already; we dont the queues to be shared by some other
         * entity in the system. */
        if (isAllocated > 1)
            return -1;
    }

    /* Set the Transmit Garbage Collection Information. */
    CSL_SRIO_SetTxGarbageCollectionInfo (hSrio, GARBAGE_LEN_QUEUE, GARBAGE_TOUT_QUEUE, 
                                         GARBAGE_RETRY_QUEUE, GARBAGE_TRANS_ERR_QUEUE, 
                                         GARBAGE_PROG_QUEUE, GARBAGE_SSIZE_QUEUE);

    /* Set the Host Device Identifier. */
    CSL_SRIO_SetHostDeviceID (hSrio, DEVICE_ID1_16BIT);

    /* Configure the component tag CSR */
    CSL_SRIO_SetCompTagCSR (hSrio, 0x00000000);

    /* Configure the PLM for all the ports. */
	for (i = 0; i < 4; i++)
	{
	    /* Set the PLM Port Silence Timer. */
        CSL_SRIO_SetPLMPortSilenceTimer (hSrio, i, 0x2);

        /* TODO: We need to ensure that the Port 0 is configured to support both
         * the 2x and 4x modes. The Port Width field is read only. So here we simply
         * ensure that the Input and Output ports are enabled. */
        CSL_SRIO_EnableInputPort (hSrio, i);
        CSL_SRIO_EnableOutputPort (hSrio, i);

        /* Set the PLM Port Discovery Timer. */
        CSL_SRIO_SetPLMPortDiscoveryTimer (hSrio, i, 0x2);

        /* Reset the Port Write Reception capture. */
        CSL_SRIO_SetPortWriteReceptionCapture(hSrio, i, 0x0);
    }

    /* Set the Port link timeout CSR */
    CSL_SRIO_SetPortLinkTimeoutCSR (hSrio, 0x000FFF);

    /* Set the Port General CSR: Only executing as Master Enable */
    CSL_SRIO_SetPortGeneralCSR (hSrio, 0, 1, 0);

    /* Clear the sticky register bits. */
    CSL_SRIO_SetLLMResetControl (hSrio, 1);

    /* Set the device id to be 0 for the Maintenance Port-Write operation
     * to report errors to a system host. */
    CSL_SRIO_SetPortWriteDeviceId (hSrio, 0x0, 0x0, 0x0);

    /* Set the Data Streaming MTU */
    CSL_SRIO_SetDataStreamingMTU (hSrio, 64);

    /* Configure the path mode for the ports. */
    for(i = 0; i < 4; i++)
        CSL_SRIO_SetPLMPortPathControlMode (hSrio, i, 0);

    /* Set the LLM Port IP Prescalar. */
    CSL_SRIO_SetLLMPortIPPrescalar (hSrio, 0x21);

    /* Enable the peripheral. */
    CSL_SRIO_EnablePeripheral(hSrio);

    /* Configuration has been completed. */
    CSL_SRIO_SetBootComplete(hSrio, 1);

#ifndef SIMULATOR_SUPPORT
    /* This code checks if the ports are operational or not. The functionality is not supported
     * on the simulator. */

	//for(i = 0; i < 4; i++)
    //    while (CSL_SRIO_IsPortOk (hSrio, i) != TRUE);

    // Check to see if Port 0 has a port ok (Testing with 1 Lane)
     while (CSL_SRIO_IsPortOk (hSrio, 0) != TRUE);
#endif

    /* Set all the queues 0 to operate at the same priority level and to send packets onto Port 0 */
    for (i =0 ; i < 16; i++)
        CSL_SRIO_SetTxQueueSchedInfo(hSrio, i, 0, 0);

    /* Set the Doorbell route to determine which routing table is to be used
     * This configuration implies that the Interrupt Routing Table is configured as
     * follows:-
     *  Interrupt Destination 0 - INTDST 16
     *  Interrupt Destination 1 - INTDST 17
     *  Interrupt Destination 2 - INTDST 18
     *  Interrupt Destination 3 - INTDST 19
     *  Interrupt Destination 4 - INTDST 20
     *  Interrupt Destination 5 - INTDST 21
     *  Interrupt Destination 6 - INTDST 22
     *  Interrupt Destination 7 - INTDST 23
     */
    CSL_SRIO_SetDoorbellRoute(hSrio, 0);

    /* Route the Doorbell interrupts.
     *  Doorbell Register 0 - Doorbits 0 -  7 are routed to Interrupt Destination 0
     *                        Doorbits 8 - 15 are routed to Interrupt Destination 4
     *  Doorbell Register 1 - Doorbits 0 -  7 are routed to Interrupt Destination 1
     *                        Doorbits 8 - 15 are routed to Interrupt Destination 5
     *  Doorbell Register 2 - Doorbits 0 -  7 are routed to Interrupt Destination 2
     *                        Doorbits 8 - 15 are routed to Interrupt Destination 6
     *  Doorbell Register 3 - Doorbits 0 -  7 are routed to Interrupt Destination 3
     *                        Doorbits 8 - 15 are routed to Interrupt Destination 7 */
    for (i = 0; i < 8; i++)
    {
    	/*                           SRIO Handle, Doorbell, Doorbit, Interrupt Destination*/
        CSL_SRIO_RouteDoorbellInterrupts( hSrio,     0,       i,              0);
        CSL_SRIO_RouteDoorbellInterrupts( hSrio,     0,      i+8,             4);
        CSL_SRIO_RouteDoorbellInterrupts( hSrio,     1,       i,              1);
        CSL_SRIO_RouteDoorbellInterrupts( hSrio,     1,      i+8,             5);
        CSL_SRIO_RouteDoorbellInterrupts( hSrio,     2,       i,              2);
        CSL_SRIO_RouteDoorbellInterrupts( hSrio,     2,      i+8,             6);
        CSL_SRIO_RouteDoorbellInterrupts( hSrio,     3,       i,              3);
        CSL_SRIO_RouteDoorbellInterrupts( hSrio,     3,      i+8,             7);
    }

    /* Initialization has been completed. */
    return 0;
}

/**
@}
*/