/******************************************************************************
 * Copyright (c) 2010 Texas Instruments Incorporated - http://www.ti.com
 * 
 *  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.
 * 
 ****************************************************************************
 *
 * Project     EVM
 * @file       Srio_util.c
 * 
 * @brief      EVM srio test utility functions
 *  
 * @ingroup    EVM_SRIO_TEST
 *
 ****************************************************************************
 *                                                                                          
 * "$Revision: 1.2 $"
 * "$Date: 2011/02/24 16:01:14 $"
 *
 ***************************************************************************/


/****************************************************************************
 *                                 Includes                                 *
 ****************************************************************************/

// !! M.T. temporary remove of csl warnings
#pragma diag_suppress 880
#include "csl_srioAux.h"
#include "csl_srioAuxPhyLayer.h"
#pragma diag_default 880

#include "csl_chip.h"
#include "cslr_device.h"
#include "Srio_util.h"
#include "c6x.h"
#include "stdio.h"
#define TOUT1       0x5000000

#define SRIO_REGS         ((CSL_SrioRegs *) CSL_SRIO_CONFIG_REGS)


#define CHIP_CFG_REG CSL_BOOT_CFG_REGS
#define KICK0 0x38
#define KICK1 0x3C
#define SRIO_SERDES_CFGPLL 0x360
#define SRIO_SERDES_CFGRX0 0x364
#define SRIO_SERDES_CFGTX0 0x368
#define SRIO_SERDES_CFGRX1 0x36C
#define SRIO_SERDES_CFGTX1 0x370
#define SRIO_SERDES_CFGRX2 0x374
#define SRIO_SERDES_CFGTX2 0x378
#define SRIO_SERDES_CFGRX3 0x37C
#define SRIO_SERDES_CFGTX3 0x380
#define SRIO_SERDES_STS    0x154


// Define the ICRR values
#define LSU_ICRR 0x00000000
#define DOORBELL_ICRR 0x00000000
#define AMU_ICRR 0x00000000
#define ERR_RST_ICRR 0x00000000

// Address ranges are inclusives
#define L2LOCAL_START   0x00800000
#define L2LOCAL_END     0x008FFFFF
#define L1PLOCAL_START  0x00E00000
#define L1PLOCAL_END    0x00E07FFF
#define L1DLOCAL_START  0x00F00000
#define L1DLOCAL_END    0x00F07FFF

static CSL_SrioHandle hSrio;
static Int32 i32MaintenanceSrcBuf;
static Int32 i32MaintenanceDstBuf;

static Uint32 srcgblMaintR;
static Uint32 srcgblMaintW;


#define _SRIO_REG_WR_32(addr,write_data) (*(volatile Uint32 *) (addr)) = (write_data)
#define _SRIO_REG_RD_32(addr) (*(volatile Uint32 *) (addr))


// This macro wait for lsu ready to be used and return the related context transaction info
// will wait for a maximum number of timeout loops (must be volatile var)
// At end of macro, error if timeout < 0, else result is ok
#define _SRIO_WAITLSURDY(lsuno,ltid,lcb,timeout) \
{                                                                                    \
    Uint32 temp;                                                                     \
    /* Wait until not busy and not full */                                           \
    while (((temp=hSrio->LSU_CMD[(lsuno)].RIO_LSU_REG6) & (CSL_SRIO_RIO_LSU_REG6_FULL_MASK | \
           CSL_SRIO_RIO_LSU_REG6_BUSY_MASK))!= 0 && (--timeout >= 0));               \
    /* If we got no error */                                                         \
    if (timeout>=0)                                                                  \
    {                                                                                \
        /* Populate the context and transaction identifier. */                       \
        (lcb) = CSL_FEXT (temp, SRIO_RIO_LSU_REG6_LCB);                              \
        (ltid) = CSL_FEXT (temp, SRIO_RIO_LSU_REG6_LTID);                            \
    }                                                                                \
}

// This macro converts an address that could be a local core address to a global
// physical address. addressin is the address that is to be converted, addressout
// is the converted address.
#define _SRIO_GLOBALADDR(addressin,addressout) \
{                                                                                         \
    /* Verify if address is between a local range that can be globalized? */              \
    if ((addressin)>=L2LOCAL_START && (addressin)<=L2LOCAL_END ||                         \
        (addressin)>=L1DLOCAL_START && (addressin)<=L1DLOCAL_END ||                       \
        (addressin)>=L1PLOCAL_START && (addressin)<=L1PLOCAL_END)                         \
    {                                                                                     \
        (addressout) = 0x10000000 | ((CSL_chipReadReg(CSL_CHIP_DNUM))<<24) | (addressin); \
    }                                                                                     \
    else                                                                                  \
    {                                                                                     \
        /* By default will return current address if cannot be converted */               \
        (addressout) = (addressin);                                                       \
    }                                                                                     \
}



/****************************************************************************
 *   NAME : Srio_Init
 ************************************************************************//**
 *
 *   This function initializes the specified SRIO port of the DSP and gives it an ID.
 *
 *   WARNING: Parameters must match hardware srio evm powerup configuration that
 *   is related to the port to be used. These evm parameters cannot be changed by
 *   this function, but are needed to configure the dsp correctly for the evm and
 *   the onboard srio switch. If user want to change srio powerup evm configuration,
 *   the configuration must be changed by DSP1 only using BSL functions
 *
 *   @param [in] config
 *      SRIO lane configuration (default value=4 on evm)
 *   
 *   @param [in] mode
 *      SRIO lane configuration mode (for config=4, 0: all =1X mode, 2: p0,p1=1x, p2=2x, 4: p0=4X)
 *   
 *   @param [in] datarate
 *      Port operation speed (the speed must match switch configuration)
 *   
 *   @param [in] LEN
 *      Endianness configuration

 *   @param [in] loopback
 *      Dsp internal srio loopback mode (1: Yes,  0: No (0 for normal operation))
 *   
 *   @param [in] ID
 *      16 bits srio identification (8 bits ID is compute from LSB of this value)
 *   
 *   @return
 *      error code: 0 if OK, !=0 in case of problem
 *   
 *   @ingroup EVM_SRIO_UTIL
 *
 ***************************************************************************/        
Int32 Srio_Init(Int32 config, Int32 mode, enum SRIO_DR datarate, Uint32 LEND, Uint32 loopback, Uint16 ID)
{
   Int32 i;
   Uint32 rate,sertxconf,serrxconf;
   Uint32 wdata,rdata,mask,mdata;
   SRIO_TLM_CONTROL TLMCtl;

   // Open the SRIO handler
   hSrio = CSL_SRIO_Open(CSL_SRIO);
   if (hSrio == NULL)
       return(1);
 
   // Convert to a global address if needed internal maintenance buffers
   _SRIO_GLOBALADDR((Uint32)(&i32MaintenanceSrcBuf),srcgblMaintW);
   _SRIO_GLOBALADDR((Uint32)(&i32MaintenanceDstBuf),srcgblMaintR);

   //Disable the Global and Blocks 
   CSL_SRIO_GlobalDisable (hSrio);
   for(i=0;i<=9;i++)
   {
       CSL_SRIO_DisableBlock(hSrio,i);
   }
   
   // bringing SRIO from reset isolation
   SRIO_REGS->RIO_PCR = 0x11; 
     
   // Write 0 to Boot complete bit
	CSL_SRIO_SetBootComplete(hSrio,0); 
	
   //Enable the Global and Blocks 
   CSL_SRIO_GlobalEnable (hSrio);
   for(i=0;i<=9;i++)
   {
   		CSL_SRIO_EnableBlock(hSrio,i);
   }
  
	// Enable the Promote_Dis
 	CSL_SRIO_EnableAutomaticPriorityPromotion(hSrio);

	
    // CSL_SRIO_SetUDIBufferingMode(hSrio, 1);
    
	if(loopback)
	{
	    SRIO_REGS->RIO_PER_SET_CNTL1 = 0xF0;
	}
	else
	{
	    SRIO_REGS->RIO_PER_SET_CNTL1 = 0x00;
	}  
	//Set the SRIO Prescalar select to operate in the range of 44.7 to 89.5
	CSL_SRIO_SetPrescalarSelect (hSrio, 4);

	// programming the chip level KICK registers
    _SRIO_REG_WR_32(CHIP_CFG_REG + KICK0,0x83e70b13);
    _SRIO_REG_WR_32(CHIP_CFG_REG + KICK1,0x95a4f1e0);

// For boards revision rev b2 and before
#if defined(EVMBOARD_VARIANT00) || defined(EVMBOARD_VARIANT01) || defined(EVMBOARD_VARIANT02) 
    // Considering present Ref clk frequency is 312.5Mhz and desired line rate of 3.125Ghz
	if (datarate == SRIO_DR_3125)
	{
		_SRIO_REG_WR_32(CHIP_CFG_REG + SRIO_SERDES_CFGPLL, 0x229); // Half
    	rate = 0;
	}
	// Considering present Ref clk frequency is 312.5Mhz and desired line rate of 5Ghz
	else if (datarate == SRIO_DR_5000)
	{
    	_SRIO_REG_WR_32(CHIP_CFG_REG + SRIO_SERDES_CFGPLL, 0x241); // ratescale = 1/2, MPY = 8
    	rate = 0;
	}
	// Considering present Ref clk frequency is 312.5Mhz and desired line rate of 2.5Ghz
	else if (datarate == SRIO_DR_2500)
	{
    	_SRIO_REG_WR_32(CHIP_CFG_REG + SRIO_SERDES_CFGPLL, 0x241); // Half
    	rate = 1;
	}
	// Considering present Ref clk frequency is 312.5Mhz and desired line rate of 1.25Ghz
	else if (datarate == SRIO_DR_1250)
	{
    	_SRIO_REG_WR_32(CHIP_CFG_REG + SRIO_SERDES_CFGPLL, 0x241); // Quarter
    	rate = 2;
	}
#else
    // Board rev b3 and later
    // Considering present Ref clk frequency is 156.25Mhz and desired line rate of 3.125Ghz
	if (datarate == SRIO_DR_3125)
	{
		_SRIO_REG_WR_32(CHIP_CFG_REG + SRIO_SERDES_CFGPLL, 0x251); // Half
    	rate = 0;
	}
	// Considering present Ref clk frequency is 156.25Mhz and desired line rate of 5Ghz
	else if (datarate == SRIO_DR_5000)
	{
    	_SRIO_REG_WR_32(CHIP_CFG_REG + SRIO_SERDES_CFGPLL, 0x281); // ratescale = 1/2, MPY = 8
    	rate = 0;
	}
	// Considering present Ref clk frequency is 156.25Mhz and desired line rate of 2.5Ghz
	else if (datarate == SRIO_DR_2500)
	{
    	_SRIO_REG_WR_32(CHIP_CFG_REG + SRIO_SERDES_CFGPLL, 0x281); // Half
    	rate = 1;
	}
	// Considering present Ref clk frequency is 156.25Mhz and desired line rate of 1.25Ghz
	else if (datarate == SRIO_DR_1250)
	{
    	_SRIO_REG_WR_32(CHIP_CFG_REG + SRIO_SERDES_CFGPLL, 0x281); // Quarter
    	rate = 2;
	}
#endif

	serrxconf = 0x00440485 | (rate << 4);
    sertxconf = 0x00180785 | (rate << 4);

	// Program SerDes TX and Rx configuration registers
	if(loopback)
	{
		_SRIO_REG_WR_32(CHIP_CFG_REG + SRIO_SERDES_CFGRX0,0x01C40495);
		_SRIO_REG_WR_32(CHIP_CFG_REG + SRIO_SERDES_CFGTX0,0x00780795);
		_SRIO_REG_WR_32(CHIP_CFG_REG + SRIO_SERDES_CFGRX1,0x01C40495);
		_SRIO_REG_WR_32(CHIP_CFG_REG + SRIO_SERDES_CFGTX1,0x00780795);
		_SRIO_REG_WR_32(CHIP_CFG_REG + SRIO_SERDES_CFGRX2,0x01C40495);
		_SRIO_REG_WR_32(CHIP_CFG_REG + SRIO_SERDES_CFGTX2,0x00780795);
		_SRIO_REG_WR_32(CHIP_CFG_REG + SRIO_SERDES_CFGRX3,0x01C40495);
		_SRIO_REG_WR_32(CHIP_CFG_REG + SRIO_SERDES_CFGTX3,0x00780795);
	}
	else
	{
	    _SRIO_REG_WR_32(CHIP_CFG_REG + SRIO_SERDES_CFGRX0,serrxconf);
	    _SRIO_REG_WR_32(CHIP_CFG_REG + SRIO_SERDES_CFGTX0,sertxconf);
	    _SRIO_REG_WR_32(CHIP_CFG_REG + SRIO_SERDES_CFGRX1,serrxconf);
	    _SRIO_REG_WR_32(CHIP_CFG_REG + SRIO_SERDES_CFGTX1,sertxconf);
	    _SRIO_REG_WR_32(CHIP_CFG_REG + SRIO_SERDES_CFGRX2,serrxconf);
	    _SRIO_REG_WR_32(CHIP_CFG_REG + SRIO_SERDES_CFGTX2,sertxconf);
	    _SRIO_REG_WR_32(CHIP_CFG_REG + SRIO_SERDES_CFGRX3,serrxconf);
	    _SRIO_REG_WR_32(CHIP_CFG_REG + SRIO_SERDES_CFGTX3,sertxconf); 
	    /* _SRIO_REG_WR_32(CHIP_CFG_REG + SRIO_SERDES_CFGRX0,0x00440495);
	    _SRIO_REG_WR_32(CHIP_CFG_REG + SRIO_SERDES_CFGTX0,0x00180795);
	    _SRIO_REG_WR_32(CHIP_CFG_REG + SRIO_SERDES_CFGRX1,0x00440495);
	    _SRIO_REG_WR_32(CHIP_CFG_REG + SRIO_SERDES_CFGTX1,0x00180795);
	    _SRIO_REG_WR_32(CHIP_CFG_REG + SRIO_SERDES_CFGRX2,0x00440495);
	    _SRIO_REG_WR_32(CHIP_CFG_REG + SRIO_SERDES_CFGTX2,0x00180795);
	    _SRIO_REG_WR_32(CHIP_CFG_REG + SRIO_SERDES_CFGRX3,0x00440495);
	    _SRIO_REG_WR_32(CHIP_CFG_REG + SRIO_SERDES_CFGTX3,0x00180795);*/
	}				
	
    // !!! M.T. add timeout here?
    do
    {	
        rdata = _SRIO_REG_RD_32(CHIP_CFG_REG + SRIO_SERDES_STS);
    } while ((rdata & 0x00000001) != 0x00000001);
	
	// Clear the pending interrupts if any

	SRIO_REGS->LSU_ICSR_ICCR[0].RIO_LSU_ICCR	= 0xFFFFFFFF ;  // clear all pending intr
  	SRIO_REGS->LSU_ICSR_ICCR[1].RIO_LSU_ICCR	= 0xFFFFFFFF ;  // clear all pending intr
  	
 
	// Program the Routing registers
    SRIO_REGS->RIO_LSU0_MODULE_ICRR[0] 	= LSU_ICRR ;  
    SRIO_REGS->RIO_LSU0_MODULE_ICRR[1]	= LSU_ICRR ;  
    SRIO_REGS->RIO_LSU0_MODULE_ICRR[2]	= LSU_ICRR ;  
    SRIO_REGS->RIO_LSU0_MODULE_ICRR[3]	= LSU_ICRR ;  
    SRIO_REGS->RIO_LSU1_MODULE_ICRR 	= LSU_ICRR ;
	
	// Route the interrupt on the General purpose interrupts
	SRIO_REGS->RIO_INTERRUPT_CTL = 0;//1;1 gen, 0 dedi
   
	// By default all doorbells routed to core 0
	/*SRIO_REGS->DOORBELL_ICRR[0].RIO_DOORBELL_ICRR1 = DOORBELL_ICRR;
	SRIO_REGS->DOORBELL_ICRR[0].RIO_DOORBELL_ICRR2 = DOORBELL_ICRR;
	SRIO_REGS->DOORBELL_ICRR[1].RIO_DOORBELL_ICRR1 = DOORBELL_ICRR;
	SRIO_REGS->DOORBELL_ICRR[1].RIO_DOORBELL_ICRR2 = DOORBELL_ICRR;
	SRIO_REGS->DOORBELL_ICRR[2].RIO_DOORBELL_ICRR1 = DOORBELL_ICRR;
	SRIO_REGS->DOORBELL_ICRR[2].RIO_DOORBELL_ICRR2 = DOORBELL_ICRR;
	SRIO_REGS->DOORBELL_ICRR[3].RIO_DOORBELL_ICRR1 = DOORBELL_ICRR;
	SRIO_REGS->DOORBELL_ICRR[3].RIO_DOORBELL_ICRR2 = DOORBELL_ICRR;*/ 
	
	_SRIO_REG_WR_32(0x2900200,DOORBELL_ICRR+DNUM); //DOORBELL0_ICRR
	_SRIO_REG_WR_32(0x2900204,DOORBELL_ICRR);
	_SRIO_REG_WR_32(0x290020C,DOORBELL_ICRR);
	_SRIO_REG_WR_32(0x2900210,DOORBELL_ICRR);
	_SRIO_REG_WR_32(0x2900218,DOORBELL_ICRR);
	_SRIO_REG_WR_32(0x290021C,DOORBELL_ICRR);
	_SRIO_REG_WR_32(0x2900220,DOORBELL_ICRR); // !!! M.T. it seems there are address problem here against doc
	_SRIO_REG_WR_32(0x2900224,DOORBELL_ICRR);
	
if(ID == 0x0050)
{
	CSL_SRIO_SetPacketForwarding(hSrio, 0, 0x0040, 0x0040, 0x40, 0x40, 2);
	hSrio->RIO_MULTIID_REG[0] = 0x400040;
	CSL_SRIO_GetTLMPortControl(hSrio, 2, &TLMCtl);
	TLMCtl.tgtIDDis = 1;
	CSL_SRIO_SetTLMPortControl(hSrio, 2, &TLMCtl);
}
	
    SRIO_REGS->RIO_DEV_ID	= (0x009D) | 0x0030;  // id=device jtag identifier, ti=0x0030
    SRIO_REGS->RIO_DEV_INFO	= 0x00000000;  // 0
    SRIO_REGS->RIO_ASBLY_ID	= 0x00000030;  // ti=0x0030
    SRIO_REGS->RIO_ASBLY_INFO = 0x00000100;
    SRIO_REGS->RIO_PE_FEAT	=  0x200001D9; //0x20000119 ;  // proc, bu ext, 16-bit ID, 34-bit addr
    SRIO_REGS->RIO_SW_PORT    = 0x00000400;
    SRIO_REGS->RIO_SRC_OP     = 0x0004FDF4;
    SRIO_REGS->RIO_DEST_OP    = 0x0000FC04;
        
    // Change the srio ID
    if (Srio_ChangeID(ID)!=0)
        return(2);
    
    for(i=0;i<4;i++)
    {
        // !!! M.T. check for ID here?
        SRIO_REGS->RIO_TLM[i].brr[0].RIO_TLM_SP_BRR_CTL = 0x84000000;
        SRIO_REGS->RIO_TLM[i].brr[0].RIO_TLM_SP_BRR_PATTERN_MATCH = 0xBEEABEEA;
        SRIO_REGS->RIO_TLM[i].brr[1].RIO_TLM_SP_BRR_CTL = 0x84000000;
        SRIO_REGS->RIO_TLM[i].brr[1].RIO_TLM_SP_BRR_PATTERN_MATCH = 0xBEEBBEEB;
        SRIO_REGS->RIO_TLM[i].brr[2].RIO_TLM_SP_BRR_CTL = 0x84000000;
        SRIO_REGS->RIO_TLM[i].brr[2].RIO_TLM_SP_BRR_PATTERN_MATCH = 0xBEECBEEC;
        SRIO_REGS->RIO_TLM[i].brr[3].RIO_TLM_SP_BRR_CTL = 0x84000000;
        SRIO_REGS->RIO_TLM[i].brr[3].RIO_TLM_SP_BRR_PATTERN_MATCH = 0xBEEDBEED;
    }
	
 
    rdata = SRIO_REGS->RIO_SP_MB_HEAD;
    wdata = 0x10000002;
    mask  = 0xFFFFFFFF;
    mdata = (wdata & mask) | (rdata & ~mask);
    SRIO_REGS->RIO_SP_MB_HEAD	 		= mdata ;  
	
    rdata = SRIO_REGS->RIO_COMP_TAG;
    wdata = 0x00000000;
    mask  = 0x00000000;
    mdata = (wdata & mask) | (rdata & ~mask);
    SRIO_REGS->RIO_COMP_TAG		 		= mdata ;  // not touched
  
	for(i=0;i<4;i++)
	{
	    rdata = SRIO_REGS->RIO_PLM[i].RIO_PLM_SP_SILENCE_TIMER;
	    wdata = 0x20000000;
	    mask  = 0xF0000000;
	    mdata = (wdata & mask) | (rdata & ~mask);
	    SRIO_REGS->RIO_PLM[i].RIO_PLM_SP_SILENCE_TIMER		 	= mdata ;  // 0, short cycles for sim
	    
	    rdata = SRIO_REGS->RIO_SP[i].RIO_SP_CTL;
	    if(i==0)
	      wdata = 0xD0600001;
	    else
	      wdata = 0x00600001; 
	    mask  = 0xF0600003;
	    mdata = (wdata & mask) | (rdata & ~mask);
	    SRIO_REGS->RIO_SP[i].RIO_SP_CTL	 		= mdata ;  // enable i/o
	    
	    rdata = SRIO_REGS->RIO_PLM[i].RIO_PLM_SP_DISCOVERY_TIMER;
	    wdata = 0x20000000;
	    mask  = 0xF0000000;
	    mdata = (wdata & mask) | (rdata & ~mask);
	    SRIO_REGS->RIO_PLM[i].RIO_PLM_SP_DISCOVERY_TIMER = mdata ;
	    
	    rdata = SRIO_REGS->RIO_PW_RX_CAPT[i];
	    wdata = 0x00000000;
	    mask  = 0xFFFFFFFF;
	    mdata = (wdata & mask) | (rdata & ~mask);
	    SRIO_REGS->RIO_PW_RX_CAPT[i] = mdata ;
    }
    
    rdata = SRIO_REGS->RIO_SP_LT_CTL;
    wdata = 0x000FFF00;  //long
    mask  = 0x00FFFFFF;
    mdata = (wdata & mask) | (rdata & ~mask);
    SRIO_REGS->RIO_SP_LT_CTL  		= mdata ;  // 3.1us

    rdata = SRIO_REGS->RIO_SP_RT_CTL;
    wdata = 0x000FFF00;  //long
    mask  = 0x00FFFFFF;
    mdata = (wdata & mask) | (rdata & ~mask);
    SRIO_REGS->RIO_SP_RT_CTL  		= mdata ;  // 3.1us */


    rdata = SRIO_REGS->RIO_SP_GEN_CTL;
    wdata = 0x40000000;
    mask  = 0xE0000000;
    mdata = (wdata & mask) | (rdata & ~mask);
    SRIO_REGS->RIO_SP_GEN_CTL 		= mdata ;  // agent, master, undiscovered
	
 
    rdata = SRIO_REGS->RIO_REG_RST_CTL;
    wdata = 0x00000001;
    mask  = 0x00000001;
    mdata = (wdata & mask) | (rdata & ~mask);
    SRIO_REGS->RIO_REG_RST_CTL = mdata ;
    
    rdata = SRIO_REGS->RIO_ERR_RPT_BH;
    wdata = 0x30000007;
    mask  = 0xF000000F;
    mdata = (wdata & mask) | (rdata & ~mask);
    SRIO_REGS->RIO_ERR_RPT_BH = mdata ;

    rdata = SRIO_REGS->RIO_ERR_DET;
    wdata = 0x00000000;
    mask  = 0xF000000F;
    mdata = (wdata & mask) | (rdata & ~mask);
    SRIO_REGS->RIO_ERR_DET= mdata ;

    rdata = SRIO_REGS->RIO_ERR_EN;
    wdata = 0x00000000;
    mask  = 0xF000000F;
    mdata = (wdata & mask) | (rdata & ~mask);
    SRIO_REGS->RIO_ERR_EN= mdata ;

    rdata = SRIO_REGS->RIO_H_ADDR_CAPT;
    wdata = 0x00000000;
    mask  = 0xF000000F;
    mdata = (wdata & mask) | (rdata & ~mask);
    SRIO_REGS->RIO_H_ADDR_CAPT= mdata ;
  
    rdata = SRIO_REGS->RIO_ADDR_CAPT;
    wdata = 0x00000000;
    mask  = 0xF000000F;
    mdata = (wdata & mask) | (rdata & ~mask);
    SRIO_REGS->RIO_ADDR_CAPT= mdata ;

    rdata = SRIO_REGS->RIO_ID_CAPT;
    wdata = 0x00000000;
    mask  = 0xF000000F;
    mdata = (wdata & mask) | (rdata & ~mask);
    SRIO_REGS->RIO_ID_CAPT= mdata ;

    rdata = SRIO_REGS->RIO_CTRL_CAPT;
    wdata = 0x00000000;
    mask  = 0xF000000F;
    mdata = (wdata & mask) | (rdata & ~mask);
    SRIO_REGS->RIO_CTRL_CAPT= mdata ;

   rdata = SRIO_REGS->RIO_PW_RX_CAPT[0];
    wdata = 0x00000000;
    mask  = 0xF000000F;
    mdata = (wdata & mask) | (rdata & ~mask);
    SRIO_REGS->RIO_PW_RX_CAPT[0]= mdata ;

    rdata = SRIO_REGS->RIO_PW_RX_CAPT[1];
    wdata = 0x00000000;
    mask  = 0xF000000F;
    mdata = (wdata & mask) | (rdata & ~mask);
    SRIO_REGS->RIO_PW_RX_CAPT[1]= mdata ;

    rdata = SRIO_REGS->RIO_PW_RX_CAPT[2];
    wdata = 0x00000000;
    mask  = 0xF000000F;
    mdata = (wdata & mask) | (rdata & ~mask);
    SRIO_REGS->RIO_PW_RX_CAPT[2]= mdata ;

    rdata = SRIO_REGS->RIO_PW_RX_CAPT[3];
    wdata = 0x00000000;
    mask  = 0xF000000F;
    mdata = (wdata & mask) | (rdata & ~mask);
    SRIO_REGS->RIO_PW_RX_CAPT[3]= mdata ; 

    rdata = SRIO_REGS->RIO_PW_TGT_ID;
    wdata = 0x00000000;
    mask  = 0xFFFFFFFF;
    mdata = (wdata & mask) | (rdata & ~mask);
    SRIO_REGS->RIO_PW_TGT_ID = mdata ;
    
    /*  rdata = SRIO_REGS->RIO_DS_LL_CTL;
    wdata = 0x00000040;
    mask  = 0x000000FF;
    mdata = (wdata & mask) | (rdata & ~mask);
    SRIO_REGS->RIO_DS_LL_CTL = mdata ; */
    
     // Program the mode and configuration
    for(i = 0;i<4;i++)
    {
    	rdata = SRIO_REGS->RIO_PLM[i].RIO_PLM_SP_PATH_CTL;
        SRIO_REGS->RIO_PLM[i].RIO_PLM_SP_PATH_CTL = (config <<8 | mode << 0);
        rdata = (config <<8 | mode << 0);
     }
   	SRIO_REGS->RIO_PRESCALAR_SRV_CLK = 0x00000021;
   
    SRIO_REGS->RIO_INTDST_RATE_DIS = 0xFFFFFF;

    for(i=0;i<4;i++)
    {
	     SRIO_REGS->RIO_PBM[i].RIO_PBM_SP_IG_WATERMARK0 = 0x240024;
	     SRIO_REGS->RIO_PBM[i].RIO_PBM_SP_IG_WATERMARK1 = 0x1B001B;
	     SRIO_REGS->RIO_PBM[i].RIO_PBM_SP_IG_WATERMARK2 = 0x120012;
	     SRIO_REGS->RIO_PBM[i].RIO_PBM_SP_IG_WATERMARK3 = 0x90009;
    }

	// Set the LEND modes
	
	/*	Add a 6 programmable bits to register 0x00020.
	Field:	Definition:
	lend_swap_mode[31:30]	MAU little-endian swapping mode:
	  2’b00: Mode A
	  2’b01: Mode B
	  2’b10: Mode C
	  2’b11: Mode D
	lend_swap_mode[29:28]	LSU little-endian swapping mode:
	  2’b00: Mode A
	  2’b01: Mode B
	  2’b10: Mode C
	  2’b11: Mode D
	lend_swap_mode[23:22]	TXU/RXU little-endian swapping mode:
	  2’b00: Mode A
	  2’b01: Mode B
	  2’b10: Mode C
	  2’b11: Mode D
	3)	Therefore, during runtime, if little-mode is enabled, each protocol
	unit can support different swapping mode concurrently.
	*/
	switch(LEND) 
	{
		case 0:
	
			rdata = SRIO_REGS->RIO_PER_SET_CNTL;
			wdata = 0x00000000;  //MAU ModeA, LSU ModeA, TXURXU ModeA
			mask  = 0xF0C00000;
			mdata = (wdata & mask) | (rdata & ~mask);
			SRIO_REGS->RIO_PER_SET_CNTL	= mdata ;
			break; 
	
		case 1:
	
			rdata = SRIO_REGS->RIO_PER_SET_CNTL;
			wdata = 0x00400000;  //MAU ModeA, LSU ModeA, TXURXU ModeB
			mask  = 0xF0C00000;
			mdata = (wdata & mask) | (rdata & ~mask);
			SRIO_REGS->RIO_PER_SET_CNTL	= mdata ;
			break;
	
		case 2:
	
			rdata = SRIO_REGS->RIO_PER_SET_CNTL;
			wdata = 0x00800000;  //MAU ModeA, LSU ModeA, TXURXU ModeC
			mask  = 0xF0C00000;
			mdata = (wdata & mask) | (rdata & ~mask);
			SRIO_REGS->RIO_PER_SET_CNTL	= mdata ;
			break; 
	
		case 3:
			rdata = SRIO_REGS->RIO_PER_SET_CNTL;
			wdata = 0x00C00000;  //MAU ModeA, LSU ModeA, TXURXU ModeD
			mask  = 0xF0C00000;
			mdata = (wdata & mask) | (rdata & ~mask);
			SRIO_REGS->RIO_PER_SET_CNTL	= mdata ;
			break; 
	}

    CSL_SRIO_EnablePeripheral(hSrio);
   
	CSL_SRIO_SetBootComplete(hSrio,1);
    
    return(0);
}


/****************************************************************************
 *   NAME : Srio_CheckForLink
 ************************************************************************//**
 *
 *   This function verifies if a link (PORT_OK) is detected on all configured
 *   ports according to config and mode paramaters. These parameters must
 *   be the same values than the one uned with Srio_Init function.
 *   Srio_Init must be used before calling this function.
 *
 *   @param [in] config
 *      SRIO lane configuration (default value=4 on evm)
 *   
 *   @param [in] mode
 *      SRIO lane configuration mode (for config=4, 0: all =1X mode,
 *                                                  2: p0,p1=1x, p2=2x, 4: p0=4X)
 *   
 *   @param [in] checkloop
 *      Number of checking loop to wait for a link on all used ports
 *   
 *   @return
 *      error code: 0 if OK, !=0 in case of problem
 *   
 *   @ingroup EVM_SRIO_UTIL
 *
 ***************************************************************************/        
Int32 Srio_CheckForLink(Int32 config, Int32 mode, Int32 checkloop)
{
    volatile Int32 nloop;
    Int32 retcode=0;
    Uint32 rdata;
   
    // Get the number of loop to use
    nloop = checkloop;  

    // Check the SRIO handler
    if (hSrio == NULL)
        return(1);
 
    // Check according how the lane are configured
    if (config == 4) 
    {
	    switch(mode)
	    {
	        // ports 0-4 in 1x mode
	    	case 0: 
	    	    //if (nloop>0)
	    	  	//{
	    	  	    //retcode = 10;
	    	  	    //do
	    	  	    //{	
		      		//	rdata = SRIO_REGS->RIO_SP[0].RIO_SP_ERR_STAT;
		     		//} while ((rdata & 0x00000002) != 0x00000002 && (nloop--)>0);
	    	  	//}
	    	  	//if (nloop>0)
	    	  	//{
	    	  	    //retcode = 11;
		     		//do
		     		//{	
		      		//	rdata = SRIO_REGS->RIO_SP[1].RIO_SP_ERR_STAT;
		     		//} while ((rdata & 0x00000002) != 0x00000002 && (nloop--)>0);
	    	  	//}
	    	  	if (nloop>0)
	    	  	{
	    	  	    retcode = 12;
		     		do
		     		{	
		      			rdata = SRIO_REGS->RIO_SP[2].RIO_SP_ERR_STAT;
		     		} while ((rdata & 0x00000002) != 0x00000002 && (nloop--)>0);	
	    	  	}
	    	  	if (nloop>0)
	    	  	{
	    	  	    retcode = 13;
		     	    do
		     	    {	
		      			rdata = SRIO_REGS->RIO_SP[3].RIO_SP_ERR_STAT;
		     		} while ((rdata & 0x00000002) != 0x00000002 && (nloop--)>0);
	    	  	}
		     	break;

	    	  	// port 0 and 1 in 1x mode, port 2 in 2x mode
	    	  	case 1 :
   	  			    //if (nloop>0)
   	  			    //{
    	    	  	//    retcode = 10;
     	  			//    do
     	  			//    {	
	      			//		rdata = SRIO_REGS->RIO_SP[0].RIO_SP_ERR_STAT;
	     			//	} while ((rdata & 0x00000002) != 0x00000002 && (nloop--)>0);
	    	  		//}
	    	  		//if (nloop>0)
	    	  		//{
		    	  	//    retcode = 11;
		     		//	do
		     		//	{	
		      		//		rdata = SRIO_REGS->RIO_SP[1].RIO_SP_ERR_STAT;
		     		//	} while ((rdata & 0x00000002) != 0x00000002 && (nloop--)>0);	
	    	  		//}
	    	  		if (nloop>0)
	    	  		{
		    	  	    retcode = 12;
		     			do
		     			{	
		      			    rdata = SRIO_REGS->RIO_SP[2].RIO_SP_ERR_STAT;
		     			} while ((rdata & 0x00000002) != 0x00000002 && (nloop--)>0);
	    	  		}
		     		break;

	    	  	// port 0 in 2x mode, ports 2 and 3 in 1x mode	
	    	  	case 2 :
   	  			    //if (nloop>0)
   	  			    //{
    	    	  	    //retcode = 10;
    	  			    //do
    	  			    //{	
	      				//	rdata = SRIO_REGS->RIO_SP[0].RIO_SP_ERR_STAT;
	     				//} while ((rdata & 0x00000002) != 0x00000002 && (nloop--)>0);
	    	  		//}
	    	  		if (nloop>0)
	    	  		{
		    	  	    retcode = 12;
		     			do
		     			{	
		      				rdata = SRIO_REGS->RIO_SP[2].RIO_SP_ERR_STAT;
		     			} while ((rdata & 0x00000002) != 0x00000002 && (nloop--)>0);
	    	  		}
	    	  		if (nloop>0)
	    	  		{
		    	  	    retcode = 13;
		     			do
		     			{	
		      				rdata = SRIO_REGS->RIO_SP[3].RIO_SP_ERR_STAT;
		     			} while ((rdata & 0x00000002) != 0x00000002 && (nloop--)>0);
	    	  		}
		     		break;

	    	    // ports 0 and 2 in 2x mode
	    	  	case 3 :
   	  			    //if (nloop>0)
   	  			    //{
 		    	  	    //retcode = 10;
 	  				    //do
 	  				    //{	
	      				//	rdata = SRIO_REGS->RIO_SP[0].RIO_SP_ERR_STAT;
	     				//} while ((rdata & 0x00000002) != 0x00000002 && (nloop--)>0);
	    	  	    //}
   	  			    if (nloop>0)
   	  			    {
		    	  	    retcode = 12;
	     				do
	     				{	
	      					rdata = SRIO_REGS->RIO_SP[2].RIO_SP_ERR_STAT;
	     				} while ((rdata & 0x00000002) != 0x00000002 && (nloop--)>0);
   	  			    }
		     		break;	

	    	  	// port 0 in 4x mode			
	    	  	case 4 :
   	  			    if (nloop>0)
   	  			    {
    	    	  	    retcode = 10;
    	  				do
    	  				{	
		      				rdata = SRIO_REGS->RIO_SP[0].RIO_SP_ERR_STAT;
		     			} while ((rdata & 0x00000002) != 0x00000002 && (nloop--)>0);
	    	  	    }
		     		break;
	    }
	}
	else if (config == 2) 
	{
	    switch(mode)
	    {
	    	  // port 0 1x mode, port 1 1x mode
	    	  case 0:
  			      //if (nloop>0)
	    	  	  //{
		    	  	  //retcode = 10;
	    	  	      //do
	    	  	      //{	
		      		  //	rdata = SRIO_REGS->RIO_SP[0].RIO_SP_ERR_STAT;
		     		  //} while ((rdata & 0x00000002) != 0x00000002 && (nloop--)>0);
	    	  	  //}
	    	  	  //if (nloop>0)
	    	  	  //{
		    	  	  //retcode = 11;
		     	      //do
		     	      //{	
		      		  //	rdata = SRIO_REGS->RIO_SP[1].RIO_SP_ERR_STAT;
		     		  //} while ((rdata & 0x00000002) != 0x00000002 && (nloop--)>0);
	    	  	  //}
		     	  break;

	    	  // port 0 2x mode
	    	  case 1:
  	  			  //if (nloop>0)
	    	  	  //{
		    	  	  //retcode = 10;
	    	  	      //do
	    	  	      //{	
		      		  //	rdata = SRIO_REGS->RIO_SP[0].RIO_SP_ERR_STAT;
		     		  //} while ((rdata & 0x00000002) != 0x00000002 && (nloop--)>0);
	    	  	  //}
		     	  break;
	    }	    	  		   
	}
	else if (config == 1)
	{
		switch(mode)
	    {
	        // port 0 1x mode
	    	case 0:
  			    //if (nloop>0)
  			    //{
   	    	  	    //retcode = 10;
   	  			    //do
   	  			    //{	
      				//	rdata = SRIO_REGS->RIO_SP[0].RIO_SP_ERR_STAT;
     				//} while ((rdata & 0x00000002) != 0x00000002 && (nloop--)>0);
	    	  	//}
		     	break;
	    }	
	}
    
    // Check if we got a timeout
    if (nloop>0)
         retcode = 0; 
    
    return(retcode);
}

/****************************************************************************
 *   NAME : Srio_ChangeID
 ************************************************************************//**
 *
 *   This function allows to change SRIO base ID of an already initialized
 *   SRIO peripherals. It could be used only after Srio_Init function
 *   has been used.
 *
 *   @param [in] ID
 *      16 bits srio identification (8 bits ID is compute from LSB of this value)
 *   
 *   @return
 *      error code: 0 if OK, !=0 in case of problem
 *   
 *   @ingroup EVM_SRIO
 *
 ***************************************************************************/        
Int32 Srio_ChangeID(Uint16 ID)
{
   Uint32 wdata,rdata,mask,mdata;

   // Check if we have valid srio handle
   if (hSrio == NULL)
   {
       return(1);
   }

    rdata = SRIO_REGS->RIO_BASE_ID;
    wdata = (ID & 0x00FF)<<16 | ID;
    mask  = 0x00FFFFFF;
    mdata = (wdata & mask) | (rdata & ~mask);
    SRIO_REGS->RIO_BASE_ID = mdata ;  // 16bid=as specified, 08b-id = lower part of 16 bits
	
    rdata = SRIO_REGS->RIO_HOST_BASE_ID_LOCK;
    wdata = ID;
    mask  = 0x0000FFFF;
    mdata = (wdata & mask) | (rdata & ~mask);
    SRIO_REGS->RIO_HOST_BASE_ID_LOCK	= mdata ;  // id=16 bits, lock
    
    return(0);
}


/****************************************************************************
 *   NAME : Srio_Transfer
 ************************************************************************//**
 *
 *   This function transfers an array of data in a connected Srio device between
 *   dsp and a specified remote address. The transfer type (read, write, etc is
 *   given by parameter type and must be a valid transaction for data exchange
 *   between dsp and remote destination)
 *
 *   @param [in] src
 *      Address of the dsp buffer for the transfer (this address will be converted
 *      internally to a dsp global memory address i.e. physical address and not a local
 *      address to an internal memory of a specific core)
 *
 *   @param [in] dst
 *      Destination address in the remote target's memory space 
 *
 *   @param [in] len
 *      Data buffer length (max 1MBytes)
 * 
 *   @param [in] type
 *      Transaction packet type (SRIO_REQ_NREAD, SRIO_REQ_NWRITE, SRIO_REQ_NWRITE_R, SRIO_REQ_SWRITE)
 * 
 *   @param [in] srcport
 *      Physical port index used for DSP transaction
 * 
 *   @param [in] dstID
 *      Destination ID of the target
 *   
 *   @param [in] iDoorBellInfo
 *      Doorbell info to send (if >= 0 will trigger a doorbell after a correct data
 *                             transfer, negative value won't trigger a doorbell to the remote )
 * 
 *   @return
 *      error code: 0 if OK, !=0 in case of problem
 *   
 *   @ingroup EVM_SRIO
 *
 ***************************************************************************/     
extern Uint32 t0,t1,t2;
extern Uint8 LSUno;
Uint32 LSUTSC[32];
Uint32 LSUcnt=0;
Int32 Srio_Transfer(Uint32 src, Uint32 dst, Int32 len, Int32 type, Int32 srcport, Int32 dstID,Int32 iDoorBellInfo)
{
    Uint8 lsu_no;
    SRIO_LSU_TRANSFER lsuTransfer = {0};
    Uint32 lcb,srcgbl; 
    Uint8 ltid,contextBit,compCode; 
    Int32 retcode; 
    volatile int iTimeout;
	Uint32 tt0, tt1;
     
    // Convert to a global address if needed
    _SRIO_GLOBALADDR(src,srcgbl);
    
    /* Create an LSU configuration */
    lsuTransfer.dspAddress            = srcgbl;          /* Global dsp address */
    lsuTransfer.rapidIOMSB            = 0;
    lsuTransfer.rapidIOLSB            = dst;             /* Remote address */
    lsuTransfer.bytecount             = len;
    lsuTransfer.idSize                = 1;               /* 16 bit device id */
    lsuTransfer.priority              = 0;               /* PKT priority is 0 */
    lsuTransfer.xambs                 = 0;               /* Not an extended
                                                          address */
    lsuTransfer.dstID                 = dstID;
    lsuTransfer.intrRequest           = 0;               /*  no interrupt */
    lsuTransfer.ftype                 = ((type & 0xF0)>>4);
    lsuTransfer.ttype                 = (type & 0x0F);
                                                         /* write with no
                                                            response */
    lsuTransfer.hopCount              = 0;               /* Valid for
                                                          maintainance pkt */
    lsuTransfer.outPortID             = srcport;         /* Tx on Port 0 */
    lsuTransfer.srcIDMap              = 0;               /* Use a single source ID */
    lsuTransfer.supInt                = 0;

    // Check if we have to trigger a doorbell interrupt to the remote after the transaction
    if (iDoorBellInfo>=0)
    {
        lsuTransfer.doorbellValid     = 1;
        lsuTransfer.doorbellInfo      = iDoorBellInfo;  /* Set >=0 to force a doorbell at end of transfer */
    }
    else
        lsuTransfer.doorbellValid     = 0;

    lsu_no = LSUno;//SELECTED_LSU;
    
    // Wait for our lsu ready to be used (this will give out transaction ID and context info)...
    // (this means that some shadow registers are free and not already busy)
    iTimeout = TOUT1;
    _SRIO_WAITLSURDY(lsu_no,ltid,lcb,iTimeout);
    
    // Check if we got a timeout with the last operation
    if (iTimeout >=0)
    {
        // Setup the LSU shadow registers with this transaction
        CSL_SRIO_SetLSUTransfer(hSrio, lsu_no, &lsuTransfer);

        // Wait till we have the completion code for our current transaction (with same context bit)
        iTimeout = TOUT1;
        do
        {
            // Get the completion code for the previous transaction
            CSL_SRIO_GetLSUCompletionCode (hSrio, lsu_no, ltid, &compCode, &contextBit);
        }while(0);//contextBit!=lcb && iTimeout-- >= 0);
    }
    
    // Check if we got a timeout
    //if (iTimeout >= 0)
    //    retcode = compCode;
    //else
    //    retcode = 1;
    
    // Returns the completion code
    retcode = 0;
    return(retcode);
}

/****************************************************************************
 *   NAME : Srio_DoorBell
 ************************************************************************//**
 *
 *   This function generates a doorbell interrupt to Srio device at the 
 *     specified address
 *
 *   @param [in] iDoorBellInfo
 *      Doorbell info to send
 * 
 *   @param [in] srcport
 *      Physical port index used for DSP transaction
 * 
 *   @param [in] dstID
 *      Destination ID of the target
 *   
 *   @return
 *      error code: 0 if OK, !=0 in case of problem
 *   
 *   @ingroup EVM_SRIO
 *
 ***************************************************************************/     
Int32 Srio_DoorBell(Int32 iDoorBellInfo, Int32 srcport, Int32 dstID)
{
    Uint8 lsu_no;
    SRIO_LSU_TRANSFER lsuTransfer = {0};
    Uint32 lcb; 
    Uint8 ltid,contextBit,compCode; 
    Int32 retcode; 
    volatile int iTimeout;
     
    /* Create an LSU configuration */
    lsuTransfer.dspAddress            = 0;               /* Global address */
    lsuTransfer.rapidIOMSB            = 0;
    lsuTransfer.rapidIOLSB            = 0;               /* Remote address */
    lsuTransfer.bytecount             = 0;
    lsuTransfer.idSize                = 1;               /* 16 bit device id */
    lsuTransfer.priority              = 0;               /* PKT priority is 0 */
    lsuTransfer.xambs                 = 0;               /* Not an extended
                                                          address */
    lsuTransfer.dstID                 = dstID;
    lsuTransfer.intrRequest           = 0;               /*  no interrupt */
    lsuTransfer.ftype                 = ((SRIO_REQ_DOORBELL & 0xF0)>>4);
    lsuTransfer.ttype                 = (SRIO_REQ_DOORBELL & 0x0F);
                                                         /* write with no
                                                            response */
    lsuTransfer.hopCount              = 0;               /* Valid for
                                                          maintainance pkt */
    lsuTransfer.outPortID             = srcport;         /* Tx on Port 0 */
    lsuTransfer.srcIDMap              = 0;               /* Use a single source ID */
    lsuTransfer.supInt                = 0;
    lsuTransfer.doorbellValid         = 0;
    lsuTransfer.doorbellInfo          = iDoorBellInfo;

    lsu_no = LSUno;
    
    // Wait for our lsu ready to be used (this will give out transaction ID and context info)...
    // (this means that some shadow registers are free and not already busy)
    iTimeout = TOUT1;
    _SRIO_WAITLSURDY(lsu_no,ltid,lcb,iTimeout);
    
    // Check if we got a timeout with the last operation
    if (iTimeout >=0)
    {
        // Setup the LSU shadow registers with this transaction
        CSL_SRIO_SetLSUTransfer(hSrio, lsu_no, &lsuTransfer);
    
        // Wait till we have the completion code for our current transaction (with same context bit)
        iTimeout = TOUT1;
        do
        {
            // Get the completion code for the previous transaction
            CSL_SRIO_GetLSUCompletionCode (hSrio, lsu_no, ltid, &compCode, &contextBit);
        }while(0);//contextBit!=lcb && iTimeout-- >= 0);
    }
    
    // Check if we got a timeout
    if (iTimeout >= 0)
        retcode = compCode;
    else
        retcode = 1;
    
    // Returns the completion code
    return(retcode);
}

/****************************************************************************
 *   NAME : Srio_MaintenanceWrite
 ************************************************************************//**
 *
 *   This function writes a CSR or a CAR register in the destination SRIO
 *           registers
 *
 *   @param [in] i32Port
 *      Physical port index used for write
 *
 *   @param [in] i32ID
 *      Destination ID of the target
 *
 *   @param [in] i32HopCount
 *      Hop Count to reach target (0 = first node on the route) 
 * 
 *   @param [in] i32Reg
 *      Register to be written
 * 
 *   @param [in] i32Val
 *      Value to be written
 *   
 *   @return
 *      error code: 0 if OK, !=0 in case of problem
 *   
 *   @ingroup EVM_SRIO
 *
 ***************************************************************************/ 
Int32 Srio_MaintenanceWrite(Int32 i32Port, Int32 i32ID, Int32 i32HopCount, 
                                        Int32 i32DestReg, Int32 i32Val)
{        
    Uint8 lsu_no;
    SRIO_LSU_TRANSFER lsuTransfer = {0};
    Uint32 lcb; 
    Uint8 ltid,contextBit,compCode; 
    Int32 retcode; 
    volatile int iTimeout;
     
    i32MaintenanceSrcBuf = i32Val;

    /* Create an LSU configuration */
    lsuTransfer.dspAddress            = srcgblMaintW;    /* Global dsp address */
    lsuTransfer.rapidIOMSB            = 0;
    lsuTransfer.rapidIOLSB            = i32DestReg;      /* Remote address */
    lsuTransfer.bytecount             = 4;
    lsuTransfer.idSize                = 1;               /* 16 bit device id */
    lsuTransfer.priority              = 0;               /* PKT priority is 0 */
    lsuTransfer.xambs                 = 0;               /* Not an extended
                                                          address */
    lsuTransfer.dstID                 = i32ID;
    lsuTransfer.intrRequest           = 0;               /*  no interrupt */
    lsuTransfer.ftype                 = ((SRIO_REQ_MAINTENANCE_W & 0xF0)>>4);
    lsuTransfer.ttype                 = (SRIO_REQ_MAINTENANCE_W & 0x0F);
                                                         /* write with no
                                                            response */
    lsuTransfer.hopCount              = i32HopCount;     /* Valid for
                                                          maintainance pkt */
    lsuTransfer.outPortID             = i32Port;         /* Tx on Port 0 */
    lsuTransfer.srcIDMap              = 0;               /* Use a single source ID */
    lsuTransfer.supInt                = 0;
    lsuTransfer.doorbellValid         = 0;
    lsuTransfer.doorbellInfo          = 0;  /* Set >=0 to force a doorbell at end of transfer */

    lsu_no = SELECTED_LSU;
    
    // Wait for our lsu ready to be used (this will give out transaction ID and context info)...
    // (this means that some shadow registers are free and not already busy)
    iTimeout = TOUT1;
    _SRIO_WAITLSURDY(lsu_no,ltid,lcb,iTimeout);
    
    // Check if we got a timeout with the last operation
    if (iTimeout >=0)
    {
        // Setup the LSU shadow registers with this transaction
        CSL_SRIO_SetLSUTransfer(hSrio, lsu_no, &lsuTransfer);
    
        // Wait till we have the completion code for our current transaction (with same context bit)
        iTimeout = TOUT1;
        do
        {
            // Get the completion code for the previous transaction
            CSL_SRIO_GetLSUCompletionCode (hSrio, lsu_no, ltid, &compCode, &contextBit);
        }while(contextBit!=lcb && iTimeout-- >= 0);
    }
    
    // Check if we got a timeout
    if (iTimeout >= 0)
        retcode = compCode;
    else
        retcode = 1;
    
    // Returns the completion code
    return(retcode);
}

/****************************************************************************
 *   NAME : Srio_MaintenanceRead
 ************************************************************************//**
 *
 *   This function reads a CSR or a CAR register in the destination SRIO
 *           registers
 *
*   @param [in] i32Port
 *      Physical port index used for read
 *
 *   @param [in] i32ID
 *      Destination ID of the target
 *
 *   @param [in] i32HopCount
 *      Hop Count to reach target (0 = first node on the route) 
 * 
 *   @param [in] i32Reg
 *      Register to be read
 * 
 *   @param [out] i32Val
 *      Value read
 *   
 *   @return
 *      error code: 0 if OK, !=0 in case of problem
 *   
 *   @ingroup EVM_SRIO
 *
 ***************************************************************************/ 
Int32 Srio_MaintenanceRead(Int32 i32Port, Int32 i32ID, Int32 i32HopCount, 
                                        Int32 i32Reg, Int32 *i32Val)
{        
    Uint8 lsu_no;
    SRIO_LSU_TRANSFER lsuTransfer = {0};
    Uint32 lcb; 
    Uint8 ltid,contextBit,compCode; 
    Int32 retcode; 
    volatile int iTimeout;
     
    /* Create an LSU configuration */
    lsuTransfer.dspAddress            = srcgblMaintR;    /* Global dsp address */
    lsuTransfer.rapidIOMSB            = 0;
    lsuTransfer.rapidIOLSB            = i32Reg;          /* Remote address */
    lsuTransfer.bytecount             = 4;
    lsuTransfer.idSize                = 1;               /* 16 bit device id */
    lsuTransfer.priority              = 0;               /* PKT priority is 0 */
    lsuTransfer.xambs                 = 0;               /* Not an extended
                                                          address */
    lsuTransfer.dstID                 = i32ID;
    lsuTransfer.intrRequest           = 0;               /*  no interrupt */
    lsuTransfer.ftype                 = ((SRIO_REQ_MAINTENANCE_R & 0xF0)>>4);
    lsuTransfer.ttype                 = (SRIO_REQ_MAINTENANCE_R & 0x0F);
                                                         /* write with no
                                                            response */
    lsuTransfer.hopCount              = i32HopCount;     /* Valid for
                                                          maintainance pkt */
    lsuTransfer.outPortID             = i32Port;         /* Tx on Port 0 */
    lsuTransfer.srcIDMap              = 0;               /* Use a single source ID */
    lsuTransfer.supInt                = 0;
    lsuTransfer.doorbellValid         = 0;
    lsuTransfer.doorbellInfo          = 0;  /* Set >=0 to force a doorbell at end of transfer */

    lsu_no = SELECTED_LSU;

    // Wait for our lsu ready to be used (this will give out transaction ID and context info)...
    // (this means that some shadow registers are free and not already busy)
    iTimeout = TOUT1;
    _SRIO_WAITLSURDY(lsu_no,ltid,lcb,iTimeout);
    
    // Check if we got a timeout with the last operation
    if (iTimeout >=0)
    {
        // Setup the LSU shadow registers with this transaction
        CSL_SRIO_SetLSUTransfer(hSrio, lsu_no, &lsuTransfer);
    
        // Wait till we have the completion code for our current transaction (with same context bit)
        iTimeout = TOUT1;
        do
        {
            // Get the completion code for the previous transaction
            CSL_SRIO_GetLSUCompletionCode (hSrio, lsu_no, ltid, &compCode, &contextBit);
        }while(contextBit!=lcb && iTimeout-- >= 0);
    }
    
    // Check if we got a timeout
    if (iTimeout >= 0)
    {
        retcode = compCode;
        *i32Val = i32MaintenanceDstBuf;
    }
    else
        retcode = 1;
    
    // Returns the completion code
    return(retcode);
}
