/*
 * DrvSrioInit.c
 *
 *  Created on: 2016-3-17
 *      Author: wangyuchao
 */

#include "Drv_Common.h"
#include "DrvPsc.h"
#include "DrvSrio.h"
#include "DrvSrioStruct.h"
#include "DrvSerdes.h"
#include "DrvTsc.h"
#include <csl_srioAuxPhyLayer.h>

extern CSL_BootcfgRegs *boot_cfg_regs;

CSL_SrioRegs *  gp_srioRegs =   (CSL_SrioRegs *)CSL_SRIO_CONFIG_REGS;
SerdesRegs * gp_srioSerdesRegs =
		(SerdesRegs*)&((CSL_BootcfgRegs*)CSL_BOOT_CFG_REGS)->SRIO_SERDES_CFGPLL;

/*****************************************************************************
 Prototype    : KeyStone_SRIO_GlobalEnable
 Description  : enable globally used blocks including MMR block in SRIO
 Input        : void
 Output       : None
 Return Value :

  History        :
  1.Date         : 2011/6/11
    Author       : Zhan
    Modification : Created function

*****************************************************************************/
void KeyStone_SRIO_GlobalEnable(void)
{
  	gp_srioRegs->RIO_GBL_EN = 1;
	gp_srioRegs->BLOCK_ENABLE_STATUS[0].RIO_BLK_EN= 1; //MMR_EN

	//wait for enable completion
	while(0x3 != (gp_srioRegs->RIO_GBL_EN_STAT&0x3));

}


/*****************************************************************************
 Prototype    : KeyStone_SRIO_enable_blocks
 Description  : Enable SRIO blocks
 Input        : SRIO_Block_Enable * blockEn
 Output       : None
 Return Value :

  History        :
  1.Date         : 2011/6/11
    Author       : Brighton Feng
    Modification : Created function

*****************************************************************************/
void KeyStone_SRIO_enable_blocks(
	SRIO_Block_Enable * blockEn)
{
	gp_srioRegs->BLOCK_ENABLE_STATUS[5].RIO_BLK_EN= blockEn->bBLK5_8_Port_Datapath_EN[0];
	gp_srioRegs->BLOCK_ENABLE_STATUS[6].RIO_BLK_EN= blockEn->bBLK5_8_Port_Datapath_EN[1];
	gp_srioRegs->BLOCK_ENABLE_STATUS[7].RIO_BLK_EN= blockEn->bBLK5_8_Port_Datapath_EN[2];
	gp_srioRegs->BLOCK_ENABLE_STATUS[8].RIO_BLK_EN= blockEn->bBLK5_8_Port_Datapath_EN[3];

	gp_srioRegs->BLOCK_ENABLE_STATUS[1].RIO_BLK_EN= blockEn->bBLK1_LSU_EN  ;
	gp_srioRegs->BLOCK_ENABLE_STATUS[2].RIO_BLK_EN= blockEn->bBLK2_MAU_EN  ;
	gp_srioRegs->BLOCK_ENABLE_STATUS[3].RIO_BLK_EN= blockEn->bBLK3_TXU_EN  ;
	gp_srioRegs->BLOCK_ENABLE_STATUS[4].RIO_BLK_EN= blockEn->bBLK4_RXU_EN  ;

	while(gp_srioRegs->BLOCK_ENABLE_STATUS[5].RIO_BLK_EN_STAT != blockEn->bBLK5_8_Port_Datapath_EN[0]);
	while(gp_srioRegs->BLOCK_ENABLE_STATUS[6].RIO_BLK_EN_STAT != blockEn->bBLK5_8_Port_Datapath_EN[1]);
	while(gp_srioRegs->BLOCK_ENABLE_STATUS[7].RIO_BLK_EN_STAT != blockEn->bBLK5_8_Port_Datapath_EN[2]);
	while(gp_srioRegs->BLOCK_ENABLE_STATUS[8].RIO_BLK_EN_STAT != blockEn->bBLK5_8_Port_Datapath_EN[3]);

	while(gp_srioRegs->BLOCK_ENABLE_STATUS[1].RIO_BLK_EN_STAT != blockEn->bBLK1_LSU_EN  );
	while(gp_srioRegs->BLOCK_ENABLE_STATUS[2].RIO_BLK_EN_STAT != blockEn->bBLK2_MAU_EN  );
	while(gp_srioRegs->BLOCK_ENABLE_STATUS[3].RIO_BLK_EN_STAT != blockEn->bBLK3_TXU_EN  );
	while(gp_srioRegs->BLOCK_ENABLE_STATUS[4].RIO_BLK_EN_STAT != blockEn->bBLK4_RXU_EN  );

}


void SRIO_HyperLink_Serdes_init(SerdesSetup_4links* serdes_cfg, SerdesRegs* serdesRegs)
{
	int i;
	float	pllMpy, serdesPllClock_GHz=12.5, rateScale;
	SerdesLinkRate linkRateScale;
	SerdesVcoRange vcoRange;
	Uint32 CFGTX;

	if(NULL== serdes_cfg)
		return;

	CSL_BootCfgUnlockKicker();

	/*find lowest link speed, the SERDES PLL output should be less or equal to it*/
	for(i= 0; i<4; i++)
	{
		if(serdes_cfg->linkSetup[i])
			if(serdesPllClock_GHz >serdes_cfg->linkSetup[i]->linkSpeed_GHz)
				serdesPllClock_GHz = serdes_cfg->linkSetup[i]->linkSpeed_GHz;
	}

	/*Serdes PLL output must be between 1.5625 and 3.125*/
	if(serdesPllClock_GHz>6.25)
		serdesPllClock_GHz /= 4;
	else if(serdesPllClock_GHz>3.125)
		serdesPllClock_GHz /= 2;
	else if(serdesPllClock_GHz<1.5625)
		serdesPllClock_GHz *= 2;
	else
		;

	/*set VCO range according to the PLL output speed*/
	if(serdesPllClock_GHz<2.17)
		vcoRange= SERDES_PLL_VCO_RANGE_HIGH;
	else
		vcoRange= SERDES_PLL_VCO_RANGE_LOW;

	pllMpy = serdesPllClock_GHz*1000/serdes_cfg->commonSetup.inputRefClock_MHz;
	/*PLL multiply factors between 4 and 60*/
	pllMpy = (pllMpy<4 || pllMpy>60)?4:pllMpy;


	serdesRegs->CFGPLL = (1<<SERDES_PLL_CFG_ENPLL_SHIFT)|
		(serdes_cfg->commonSetup.loopBandwidth<<SERDES_PLL_CFG_LOOPBANDWIDTH_SHIFT)|
		(vcoRange<<SERDES_PLL_CFG_VRANGE_SHIFT)|
		((Uint32)(pllMpy*4)<<SERDES_PLL_CFG_MPY_SHIFT);

	for(i=0; i<4; i++)
	{
		if(serdes_cfg->linkSetup[i])
		{
			rateScale = 1.0;
			rateScale=serdes_cfg->linkSetup[i]->linkSpeed_GHz/serdesPllClock_GHz;
			if(rateScale<1)
				linkRateScale = SRIO_SERDES_LINK_RATE_div2;
			else if(rateScale<2)
				linkRateScale = SRIO_SERDES_LINK_RATE_x1;
			else if(rateScale<4)
				linkRateScale = SRIO_SERDES_LINK_RATE_x2;
			else
				linkRateScale = SRIO_SERDES_LINK_RATE_x4;

			serdesRegs->link[i].CFGRX= (1<<SERDES_RX_CFG_ENRX_SHIFT)|
				(2<<SERDES_RX_CFG_BUSWIDTH_SHIFT)| 	/*Bus width must be 010b (20 bit)*/
				(serdes_cfg->linkSetup[i]->testPattern<<SERDES_RX_CFG_TESTPATTERN_SHIFT)|
				(serdes_cfg->linkSetup[i]->loopBack<<SERDES_RX_CFG_LOOPBACK_SHIFT)|
				(1<<SERDES_RX_CFG_ENOC_SHIFT)| 	/*Enable offset compensation*/
				(serdes_cfg->linkSetup[i]->rxEqualizerConfig<<SERDES_RX_CFG_EQ_SHIFT)|
				(serdes_cfg->linkSetup[i]->rxCDR<<SERDES_RX_CFG_CDR_SHIFT)|
				(serdes_cfg->linkSetup[i]->rxLos<<SERDES_RX_CFG_LOS_SHIFT)|
				(serdes_cfg->linkSetup[i]->rxAlign<<SERDES_RX_CFG_ALIGN_SHIFT)|
				(serdes_cfg->linkSetup[i]->rxTermination<<SERDES_RX_CFG_TERM_SHIFT)|
				(serdes_cfg->linkSetup[i]->rxInvertPolarity<<SERDES_RX_CFG_INVPAIR_SHIFT)|
				(linkRateScale<<SERDES_RX_CFG_RATE_SHIFT);

			CFGTX= (1<<SERDES_TX_CFG_ENTX_SHIFT)|
				(2<<SERDES_TX_CFG_BUSWIDTH_SHIFT)| 	/*Bus width must be 010b (20 bit)*/
				(serdes_cfg->linkSetup[i]->testPattern<<SERDES_TX_CFG_TESTPATTERN_SHIFT)|
				(serdes_cfg->linkSetup[i]->loopBack<<SERDES_TX_CFG_LOOPBACK_SHIFT)|
				(1<<SERDES_TX_CFG_FIRUPT_SHIFT)| 	/*FIRUPT = 1 Transmitter pre and post cursor FIR filter update*/
				/*TWPST1: Adjacent Post Cursor Tap Weight.
				If trace length is 4Ħħ, start with 20 (-10%).
				If trace length is between 4Ħħ and 10Ħħ, start with 27 (-27.5%).*/
//				(18<<SERDES_TX_CFG_TWPST1_SHIFT)|
//				(1<<SERDES_TX_CFG_TWPRE_SHIFT)| 	/*TWPRE: The settings range from 0 to -17.5% in 2.5% steps.*/
				(serdes_cfg->linkSetup[i]->txOutputSwing<<SERDES_TX_CFG_SWING_SHIFT)|
				(serdes_cfg->linkSetup[i]->txInvertPolarity<<SERDES_TX_CFG_INVPAIR_SHIFT)|
				(linkRateScale<<SERDES_TX_CFG_RATE_SHIFT);

			if(i==0) //lane 0 is Synchronization
				serdesRegs->link[i].CFGTX= CFGTX|
					(1<<SERDES_TX_CFG_MYSNC_SHIFT);
			else
				serdesRegs->link[i].CFGTX= CFGTX|
					(0<<SERDES_TX_CFG_MYSNC_SHIFT);
		}
	}


}

/*****************************************************************************
 Prototype    : KeyStone_SRIO_set_1x2x4x_Path
 Description  : configure SRIO 1x 2x or 4x path mode
 Input        : SRIO_1x2x4x_Path_Control srio_1x2x4x_path_control
 Output       : None
 Return Value :

  History        :
  1.Date         : 2011/6/11
    Author       : Brighton Feng
    Modification : Created function

*****************************************************************************/
void KeyStone_SRIO_set_1x2x4x_Path(
	SRIO_1x2x4x_Path_Control srio_1x2x4x_path_control)
{
	/*This register is a global register, even though it can be accessed
	from any port. So you do not need to program from each port, it is
	basically a single register.
	The PathID is a RO value, that is driven by the H/W. You cannot modify it*/
	gp_srioRegs->RIO_PLM[0].RIO_PLM_SP_PATH_CTL=
		(gp_srioRegs->RIO_PLM[0].RIO_PLM_SP_PATH_CTL&(~SRIO_1x2x4x_PATH_CONTROL_MASK))|
		srio_1x2x4x_path_control;

}

/*****************************************************************************
 Prototype    : KeyStone_SRIO_set_device_ID
 Description  : configure SRIO device ID
 Input        : SRIO_Device_ID_Routing_Config * device_id_routing_config
                Uint32 uiDeviceIdNum
 Output       : None
 Return Value :

  History        :
  1.Date         : 2011/6/11
    Author       : Brighton Feng
    Modification : Created function

*****************************************************************************/
void KeyStone_SRIO_set_device_ID(
	SRIO_Device_ID_Routing_Config * device_id_routing_config,
	Uint32 uiDeviceIdNum)
{
	int i;

	/*The TLM_SP(n)_BRR_x_PATTERN_MATCH registers hold the 15 allowable DestIDs,
	note that the first register is not used.  We use the RIO_BASE_ID register
	to hold the first ID */
	gp_srioRegs->RIO_BASE_ID= device_id_routing_config[0].idPattern| 	/*Large ID*/
		((device_id_routing_config[0].idPattern&0xFF)<<16); 		/*small ID*/

	/*The host base device ID lock CSR contains the base device ID value for
	the processing element in the system that is responsible for initializing
	this processing element.*/
    gp_srioRegs->RIO_HOST_BASE_ID_LOCK	=
    		device_id_routing_config[0].idPattern;

	uiDeviceIdNum= _min2(SRIO_MAX_DEVICEID_NUM, uiDeviceIdNum);
	for(i= 1; i<uiDeviceIdNum; i++)
	{
		/*please note, SRIO block 5~8 must be eanbled for corresponding
		RIO_TLM[0:3] taking effect*/
	    gp_srioRegs->RIO_TLM[i/4].brr[i&3].RIO_TLM_SP_BRR_CTL =
			(device_id_routing_config[i].routeMaintenance<<
				CSL_SRIO_RIO_TLM_SP_BRR_1_CTL_ROUTE_MR_TO_LLM_SHIFT)|
			(0<<CSL_SRIO_RIO_TLM_SP_BRR_1_CTL_PRIVATE_SHIFT)|
			(1<<CSL_SRIO_RIO_TLM_SP_BRR_1_CTL_ENABLE_SHIFT);

	    gp_srioRegs->RIO_TLM[i/4].brr[i&3].RIO_TLM_SP_BRR_PATTERN_MATCH =
			(device_id_routing_config[i].idPattern<<
				CSL_SRIO_RIO_TLM_SP_BRR_1_PATTERN_MATCH_PATTERN_SHIFT)|
			(device_id_routing_config[i].idMatchMask<<
				CSL_SRIO_RIO_TLM_SP_BRR_1_PATTERN_MATCH_MATCH_SHIFT);

	}
}

/*****************************************************************************
 Prototype    : KeyStone_SRIO_CSR_CAR_Config
 Description  : configure SRIO standard Command Status Capability registers
 Input        : SRIO_Config * srio_cfg
 Output       : None
 Return Value :

  History        :
  1.Date         : 2011/6/11
    Author       : Brighton Feng
    Modification : Created function

*****************************************************************************/
void KeyStone_SRIO_CSR_CAR_Config(SRIO_Config * srio_cfg)
{
	int i;
	Uint32 uiRegisterValue;
	Uint32 uiSpeed_MHz;

    gp_srioRegs->RIO_DEV_ID	= (0x009D << CSL_SRIO_RIO_DEV_ID_DEV_ID_SHIFT)
                             |(0x0030 << CSL_SRIO_RIO_DEV_ID_DEV_VEN_ID_SHIFT);

	/*The lower 4b should match the 4b from the JTAG Variant field of
	the DeviceID register.*/
    gp_srioRegs->RIO_DEV_INFO	= boot_cfg_regs->DEVICE_ID_REG0>>28;

    gp_srioRegs->RIO_PE_FEAT	=
		(0 << CSL_SRIO_RIO_PE_FEAT_BRDG_SHIFT) 	/*PE is not a bridge*/
		|(0 << CSL_SRIO_RIO_PE_FEAT_MEM_SHIFT) 	/*PE is not a memory*/
		|(1 << CSL_SRIO_RIO_PE_FEAT_PROC_SHIFT)	/*PE is a processor*/
		|(0 << CSL_SRIO_RIO_PE_FEAT_SW_SHIFT) 	/*PE is not a switch*/
		|(0 << CSL_SRIO_RIO_PE_FEAT_MULT_P_SHIFT) 	//???
		|(0 << CSL_SRIO_RIO_PE_FEAT_FLOW_ARB_SHIFT) //???
		|(0 << CSL_SRIO_RIO_PE_FEAT_MC_SHIFT) 		//???
		|(0 << CSL_SRIO_RIO_PE_FEAT_ERTC_SHIFT) 	//???
		|(1 << CSL_SRIO_RIO_PE_FEAT_SRTC_SHIFT) 	//???
		|(1 << CSL_SRIO_RIO_PE_FEAT_FLOW_CTRL_SHIFT)/*PE supports flow control*/
		|(1 << 6) 	//???
		|(1 << CSL_SRIO_RIO_PE_FEAT_CRF_SHIFT) 		/*PE supports CRF Function*/
		|(1 << CSL_SRIO_RIO_PE_FEAT_CTLS_SHIFT) 	/*supports common transport large systems*/
		|(1 << CSL_SRIO_RIO_PE_FEAT_EXT_FEA_SHIFT)  /*PE has extended features list*/
		|(1 << CSL_SRIO_RIO_PE_FEAT_EXT_AS_SHIFT); 	/*PE supports 34 bit addresses*/

    gp_srioRegs->RIO_SW_PORT    = (0 << CSL_SRIO_RIO_SW_PORT_PORT_TOTAL_SHIFT) //???
                              |(4 << CSL_SRIO_RIO_SW_PORT_PORT_NUM_SHIFT); 	//???

    gp_srioRegs->RIO_SRC_OP     =
		(0 << CSL_SRIO_RIO_SRC_OP_G_READ_SHIFT) 	//???
		|(0 << CSL_SRIO_RIO_SRC_OP_G_IREAD_SHIFT) 	//???
		|(0 << CSL_SRIO_RIO_SRC_OP_G_READ_OWN_SHIFT)//???
		|(0 << CSL_SRIO_RIO_SRC_OP_G_DC_INVALIDATE_SHIFT)	//???
		|(0 << CSL_SRIO_RIO_SRC_OP_G_CASTOUT_SHIFT) 		//???
		|(0 << CSL_SRIO_RIO_SRC_OP_G_DC_FLUSH_SHIFT) 		//???
		|(0 << CSL_SRIO_RIO_SRC_OP_G_IO_READ_SHIFT) 		//???
		|(0 << CSL_SRIO_RIO_SRC_OP_G_IC_INVALIDATE_SHIFT) 	//???
		|(0 << CSL_SRIO_RIO_SRC_OP_G_TLB_INVALIDATE_SHIFT) 	//???
		|(0 << CSL_SRIO_RIO_SRC_OP_G_TLB_SYNC_SHIFT) 		//???
		|(0 << CSL_SRIO_RIO_SRC_OP_G_RIO_RSVD_10_SHIFT) 	//???
		|(0 << CSL_SRIO_RIO_SRC_OP_G_RIO_RSVD_11_SHIFT) 	//???
		|(0 << CSL_SRIO_RIO_SRC_OP_DS_TM_SHIFT) 		//???
		|(1 << CSL_SRIO_RIO_SRC_OP_DS_SHIFT) 			//???
		|(0 << CSL_SRIO_RIO_SRC_OP_IMPLEMENT_DEF_SHIFT) //???
		|(1 << CSL_SRIO_RIO_SRC_OP_READ_SHIFT) 		/*support a read operation*/
		|(1 << CSL_SRIO_RIO_SRC_OP_WRITE_SHIFT) 	/*support a write operation*/
		|(1 << CSL_SRIO_RIO_SRC_OP_STRM_WR_SHIFT) 	/*support a streaming write operation*/
		|(1 << CSL_SRIO_RIO_SRC_OP_WR_RES_SHIFT) 	/*support a write with response operation*/
		|(1 << CSL_SRIO_RIO_SRC_OP_D_MSG_SHIFT) 	/*support a data message operation*/
		|(1 << CSL_SRIO_RIO_SRC_OP_DBELL_SHIFT) 	/*support a doorbell operation*/
		|(0 << CSL_SRIO_RIO_SRC_OP_ACSWAP_SHIFT) 	//???
		|(1 << CSL_SRIO_RIO_SRC_OP_ATSWAP_SHIFT) 	/*support an atomic test-and-swap operation*/
		|(1 << CSL_SRIO_RIO_SRC_OP_A_INC_SHIFT) 	/*support an atomic increment operation*/
		|(1 << CSL_SRIO_RIO_SRC_OP_A_DEC_SHIFT) 	/*support an atomic decrement operation*/
		|(1 << CSL_SRIO_RIO_SRC_OP_A_SET_SHIFT) 	/*support an atomic set operation*/
		|(1 << CSL_SRIO_RIO_SRC_OP_A_CLEAR_SHIFT) 	/*support an atomic clear operation*/
		|(0 << CSL_SRIO_RIO_SRC_OP_A_SWAP_SHIFT) 	//???
		|(1 << CSL_SRIO_RIO_SRC_OP_PORT_WR_SHIFT)	/*support a port-write generation*/
		|(0 << CSL_SRIO_RIO_SRC_OP_IMPLEMENT_DEF2_SHIFT); //???     0x0004FDF4;


    gp_srioRegs->RIO_DEST_OP    =
		(0 << CSL_SRIO_RIO_DEST_OP_G_READ_SHIFT)       	  //???
		|(0 << CSL_SRIO_RIO_DEST_OP_G_IREAD_SHIFT)          //???
		|(0 << CSL_SRIO_RIO_DEST_OP_G_READ_OWN_SHIFT)       //???
		|(0 << CSL_SRIO_RIO_DEST_OP_G_DC_INVALIDATE_SHIFT)  //???
		|(0 << CSL_SRIO_RIO_DEST_OP_G_CASTOUT_SHIFT)        //???
		|(0 << CSL_SRIO_RIO_DEST_OP_G_DC_FLUSH_SHIFT)       //???
		|(0 << CSL_SRIO_RIO_DEST_OP_G_IO_READ_SHIFT)        //???
		|(0 << CSL_SRIO_RIO_DEST_OP_G_IC_INVALIDATE_SHIFT)  //???
		|(0 << CSL_SRIO_RIO_DEST_OP_G_TLB_INVALIDATE_SHIFT) //???
		|(0 << CSL_SRIO_RIO_DEST_OP_G_TLB_SYNC_SHIFT)       //???
		|(0 << CSL_SRIO_RIO_DEST_OP_G_RIO_RSVD_10_SHIFT)    //???
		|(0 << CSL_SRIO_RIO_DEST_OP_G_RIO_RSVD_11_SHIFT)    //???
		|(0 << CSL_SRIO_RIO_DEST_OP_DS_TM_SHIFT)            //???
		|(0 << CSL_SRIO_RIO_DEST_OP_DS_SHIFT)               //???
		|(0 << CSL_SRIO_RIO_DEST_OP_IMPLEMENT_DEF_SHIFT)    //???
		|(1 << CSL_SRIO_RIO_DEST_OP_READ_SHIFT)      /*support a read operation*/
		|(1 << CSL_SRIO_RIO_DEST_OP_WRITE_SHIFT)     /*support a write operation*/
		|(1 << CSL_SRIO_RIO_DEST_OP_STRM_WR_SHIFT)   /*support a streaming write operation*/
		|(1 << CSL_SRIO_RIO_DEST_OP_WR_RES_SHIFT)    /*support a write with response operation*/
		|(1 << CSL_SRIO_RIO_DEST_OP_D_MSG_SHIFT)     /*support a data message operation*/
		|(1 << CSL_SRIO_RIO_DEST_OP_DBELL_SHIFT)     /*support a doorbell operation*/
		|(0 << CSL_SRIO_RIO_DEST_OP_ACSWAP_SHIFT)    //???
		|(0 << CSL_SRIO_RIO_DEST_OP_ATSWAP_SHIFT)    /*support an atomic test-and-swap operation*/
		|(0 << CSL_SRIO_RIO_DEST_OP_A_INC_SHIFT)     /*support an atomic increment operation*/
		|(0 << CSL_SRIO_RIO_DEST_OP_A_DEC_SHIFT)     /*support an atomic decrement operation*/
		|(0 << CSL_SRIO_RIO_DEST_OP_A_SET_SHIFT)     /*support an atomic set operation*/
		|(0 << CSL_SRIO_RIO_DEST_OP_A_CLEAR_SHIFT)   /*support an atomic clear operation*/
		|(0 << CSL_SRIO_RIO_DEST_OP_A_SWAP_SHIFT) 	//???
		|(1 << CSL_SRIO_RIO_DEST_OP_PORT_WR_SHIFT) 	/*support a port-write operation*/
		|(0 << CSL_SRIO_RIO_DEST_OP_IMPLEMENT_DEF2_SHIFT); //??? 0x0000FC04;

	/*PE supports 34 bit addresses*/
	gp_srioRegs->RIO_PE_LL_CTL= (1<<CSL_SRIO_RIO_PE_LL_CTL_EXT_ADDR_CTL_SHIFT);

	/*Software defined component tag for the PE.
	Useful for devices without device IDs.*/
	gp_srioRegs->RIO_COMP_TAG	= 0 ;  // not touched

    /* port general control */
    gp_srioRegs->RIO_SP_GEN_CTL 		=  // agent, master, undiscovered
		(0 << CSL_SRIO_RIO_SP_GEN_CTL_HOST_SHIFT)
		|(1 << CSL_SRIO_RIO_SP_GEN_CTL_MAST_EN_SHIFT)
		|(0 << CSL_SRIO_RIO_SP_GEN_CTL_DISC_SHIFT);

	/*port control*/
	for(i= 0; i< SRIO_MAX_PORT_NUM; i++)
	{
		if(FALSE==srio_cfg->blockEn.bLogic_Port_EN[i])
			continue;

		uiRegisterValue=
			(0<<CSL_SRIO_RIO_SP_CTL_PORT_DIS_SHIFT    )
			|(1<<CSL_SRIO_RIO_SP_CTL_OTP_EN_SHIFT      )
			|(1<<CSL_SRIO_RIO_SP_CTL_INP_EN_SHIFT      )
			|(0<<CSL_SRIO_RIO_SP_CTL_STOP_FAIL_EN_SHIFT)
			|(1<<CSL_SRIO_RIO_SP_CTL_PTYP_SHIFT        );
		if(NULL!=srio_cfg->flowControlID)
			uiRegisterValue|=
				(1<<CSL_SRIO_RIO_SP_CTL_FLOW_CTRL_SHIFT   )
				|(1<<CSL_SRIO_RIO_SP_CTL_FLOW_ARB_SHIFT    );
		gp_srioRegs->RIO_SP[i].RIO_SP_CTL = uiRegisterValue;

		uiSpeed_MHz= (Uint32)(srio_cfg->serdes_cfg->linkSetup[i]->linkSpeed_GHz*1000);
		if(1250==uiSpeed_MHz)
			gp_srioRegs->RIO_SP[i].RIO_SP_CTL2 =
				1<<CSL_SRIO_RIO_SP_CTL2_GB_1P25_EN_SHIFT;
		if(2500==uiSpeed_MHz)
			gp_srioRegs->RIO_SP[i].RIO_SP_CTL2 =
				1<<CSL_SRIO_RIO_SP_CTL2_GB_2P5_EN_SHIFT;
		if(3125==uiSpeed_MHz)
			gp_srioRegs->RIO_SP[i].RIO_SP_CTL2 =
				1<<CSL_SRIO_RIO_SP_CTL2_GB_3P125_EN_SHIFT;
		if(5000==uiSpeed_MHz)
			gp_srioRegs->RIO_SP[i].RIO_SP_CTL2 =
				1<<CSL_SRIO_RIO_SP_CTL2_GB_5P0_EN_SHIFT;
		if(6250==uiSpeed_MHz)
			gp_srioRegs->RIO_SP[i].RIO_SP_CTL2 =
				1<<CSL_SRIO_RIO_SP_CTL2_GB_6P25_EN_SHIFT;

		/*disable port write*/
		gp_srioRegs->RIO_PLM[i].RIO_PLM_SP_ALL_PW_EN = 0;
	}

	/*clear port write dest Id, port write not used*/
    gp_srioRegs->RIO_PW_TGT_ID= 0;
	/*disable port write*/
    gp_srioRegs->RIO_EM_DEV_PW_EN = 0;

    /* Register Reset Control
    Allows the SELF_RST and PWDN_PORT resets to clear sticky register bits
    in addition to the normal configuration registers.*/
    gp_srioRegs->RIO_REG_RST_CTL =
    	1 << CSL_SRIO_RIO_REG_RST_CTL_CLEAR_STICKY_SHIFT;
}


/*****************************************************************************
 Prototype    : KeyStone_SRIO_Error_Capture_Enable
 Description  : enable error capture for SRIO
 Input        : SRIO_Block_Enable * blockEn
 Output       : None
 Return Value :

  History        :
  1.Date         : 2011/6/11
    Author       : Brighton Feng
    Modification : Created function

*****************************************************************************/
void KeyStone_SRIO_Error_Capture_Enable(SRIO_Block_Enable * blockEn)
{
	Int32 i;

	/*Enable specific error reporting, save and lock capture information
	in the appropriate Logical/Transport Layer Capture CSRs*/
    gp_srioRegs->RIO_ERR_EN=
		(1 << CSL_SRIO_RIO_ERR_EN_IO_ERR_RESP_EN_SHIFT) 	/*"ERROR" response for an direct IO*/
		|(1 << CSL_SRIO_RIO_ERR_EN_MSG_ERR_RESP_EN_SHIFT) 	/*"ERROR" response for an message*/
		|(0 << CSL_SRIO_RIO_ERR_EN_GSM_ERR_RESP_EN_SHIFT)
		|(1 << CSL_SRIO_RIO_ERR_EN_MSG_FMT_ERR_EN_SHIFT) 	/*received message with invalid fromat*/
		|(1 << CSL_SRIO_RIO_ERR_EN_ILL_TRANS_DECODE_EN_SHIFT) 	/*Illegal transaction decode*/
		|(1 << CSL_SRIO_RIO_ERR_EN_ILL_TRANS_TGT_ERR_EN_SHIFT) 	/*Illegal transaction target error*/
		|(1 << CSL_SRIO_RIO_ERR_EN_MSG_REQ_TIMEOUT_EN_SHIFT) 	/*Message Request Time-out*/
		|(1 << CSL_SRIO_RIO_ERR_EN_PKT_RESP_TIMEOUT_EN_SHIFT) 	/*Packet Response Time-out*/
		|(1 << CSL_SRIO_RIO_ERR_EN_UNSOLICITED_RESP_EN_SHIFT) 	/*unsolicited/unexpected Response*/
		|(1 << CSL_SRIO_RIO_ERR_EN_UNSUPPORTED_TRANS_EN_SHIFT) 	/*Unsupported Transaction*/
		|(1 << CSL_SRIO_RIO_ERR_EN_PDU_LEN_ERR_EN_SHIFT) 		/*Data streaming PDU length error*/
		|(1 << CSL_SRIO_RIO_ERR_EN_SHORT_STREAM_SEG_EN_SHIFT) 	/*Short data streaming segment*/
		|(1 << CSL_SRIO_RIO_ERR_EN_LONG_STREAM_SEG_EN_SHIFT) 	/*Long data streaming segment*/
		|(1 << CSL_SRIO_RIO_ERR_EN_OPEN_STREAM_CONTEXT_EN_SHIFT) 	/*Open existing data streaming context*/
		|(1 << CSL_SRIO_RIO_ERR_EN_MISSING_STREAM_CONTEXT_EN_SHIFT) /*Missing data streaming context*/
		|(1 << CSL_SRIO_RIO_ERR_EN_CPPI_SECURITY_VIOLATION_EN_SHIFT)/*RX CPPI Security Violation*/
		|(1 << CSL_SRIO_RIO_ERR_EN_RX_DMA_ERR_EN_SHIFT); 			/*RX I/O DMA Access Error*/

	/*Error detect register, write 0 to clear it.*/
    gp_srioRegs->RIO_ERR_DET= 0 ;

	/*clear error capture registers*/
    gp_srioRegs->RIO_H_ADDR_CAPT= 0;
    gp_srioRegs->RIO_ADDR_CAPT= 0 ;
    gp_srioRegs->RIO_ID_CAPT= 0 ;
    gp_srioRegs->RIO_CTRL_CAPT= 0;

	//enable physical layer error capture
	for(i=0; i< SRIO_MAX_PORT_NUM; i++)
	{
		if(FALSE == blockEn->bLogic_Port_EN[i])
			continue;

		/*Enable error rate counting*/
		gp_srioRegs->RIO_SP_ERR[i].RIO_SP_RATE_EN=
			(1<<CSL_SRIO_RIO_SP_RATE_EN_IMP_SPEC_EN_SHIFT     )
			|(1<<CSL_SRIO_RIO_SP_RATE_EN_CS_CRC_EN_SHIFT       )
			|(1<<CSL_SRIO_RIO_SP_RATE_EN_CS_ILL_ID_EN_SHIFT    )
			|(1<<CSL_SRIO_RIO_SP_RATE_EN_CS_NOT_ACC_EN_SHIFT   )
			|(1<<CSL_SRIO_RIO_SP_RATE_EN_PKT_ILL_ACKID_EN_SHIFT)
			|(1<<CSL_SRIO_RIO_SP_RATE_EN_PKT_CRC_ERR_EN_SHIFT  )
			|(1<<CSL_SRIO_RIO_SP_RATE_EN_PKT_ILL_SIZE_EN_SHIFT )
			|(1<<CSL_SRIO_RIO_SP_RATE_EN_DSCRAM_LOS_EN_SHIFT   )
			|(1<<CSL_SRIO_RIO_SP_RATE_EN_LR_ACKID_ILL_EN_SHIFT )
			|(1<<CSL_SRIO_RIO_SP_RATE_EN_PROT_ERR_EN_SHIFT     )
			|(1<<CSL_SRIO_RIO_SP_RATE_EN_DELIN_ERR_EN_SHIFT    )
			|(1<<CSL_SRIO_RIO_SP_RATE_EN_CS_ACK_ILL_EN_SHIFT   )
			|(1<<CSL_SRIO_RIO_SP_RATE_EN_LINK_TO_EN_SHIFT      );

		/*the threshold value for reporting an error condition*/
		gp_srioRegs->RIO_SP_ERR[i].RIO_SP_ERR_THRESH=
			(0xF<<CSL_SRIO_RIO_SP_ERR_THRESH_ERR_RFT_SHIFT)
			|(0x4<<CSL_SRIO_RIO_SP_ERR_THRESH_ERR_RDT_SHIFT);

		/*Error detect register, write 0 to clear it.*/
	    gp_srioRegs->RIO_SP_ERR[i].RIO_SP_ERR_DET= 0;

		/*clear error capture registers*/
	    gp_srioRegs->RIO_SP_ERR[i].RIO_SP_ERR_ATTR_CAPT= 0;
	    gp_srioRegs->RIO_SP_ERR[i].RIO_SP_ERR_CAPT_0= 0;
	    gp_srioRegs->RIO_SP_ERR[i].RIO_SP_ERR_CAPT_1= 0;
	    gp_srioRegs->RIO_SP_ERR[i].RIO_SP_ERR_CAPT_2= 0;
	    gp_srioRegs->RIO_SP_ERR[i].RIO_SP_ERR_CAPT_3= 0;

		/*clear error rate counter*/
	    gp_srioRegs->RIO_SP_ERR[i].RIO_SP_ERR_RATE= 0;

		/*read this register to clear the error count in it*/
		i = gp_srioRegs->RIO_LANE[i].RIO_LANE_STAT0;
#if 0
		gp_srioRegs->RIO_PORT_OPTION[i].SP_CTL_INDEP= gp_srioRegs->PORT_OPTION[i].SP_CTL_INDEP|
			(CSL_SRIO_SP_CTL_INDEP_ILL_TRANS_EN_ENABLE<<CSL_SRIO_SP_CTL_INDEP_ILL_TRANS_EN_SHIFT)|
			(CSL_SRIO_SP_CTL_INDEP_MAX_RETRY_EN_ENABLE<<CSL_SRIO_SP_CTL_INDEP_MAX_RETRY_EN_SHIFT)|
			(255<<CSL_SRIO_SP_CTL_INDEP_MAX_RETRY_THR_SHIFT)|
			(CSL_SRIO_SP_CTL_INDEP_IRQ_EN_ENABLE<<CSL_SRIO_SP_CTL_INDEP_IRQ_EN_SHIFT);
#endif
	}
}

/*****************************************************************************
 Prototype    : KeyStone_SRIO_RxMode_Setup
 Description  : Rx Mode configuration
 Input        : SRIO_RX_Mode * rxMode
 Output       : None
 Return Value :

  History        :
  1.Date         : 2011/6/11
    Author       : Brighton Feng
    Modification : Created function

*****************************************************************************/
void KeyStone_SRIO_RxMode_Setup(SRIO_RX_Mode * rxMode)
{
	int i;

	if(rxMode)
	{
		gp_srioRegs->RIO_PER_SET_CNTL = (gp_srioRegs->RIO_PER_SET_CNTL&
			(~CSL_SRIO_RIO_PER_SET_CNTL_LOG_TGT_ID_DIS_MASK))|
			(rxMode->accept_data_with_any_ID
				<<CSL_SRIO_RIO_PER_SET_CNTL_LOG_TGT_ID_DIS_SHIFT);

		/*set RX mode for all ports*/
		for(i=0; i<SRIO_MAX_PORT_NUM; i++)
		{
			gp_srioRegs->RIO_TLM[i].RIO_TLM_SP_CONTROL =
				(gp_srioRegs->RIO_TLM[i].RIO_TLM_SP_CONTROL&
				(~(CSL_SRIO_RIO_TLM_SP_CONTROL_TGT_ID_DIS_MASK
				|CSL_SRIO_RIO_TLM_SP_CONTROL_MTC_TGT_ID_DIS_MASK)))
				|(rxMode->port_rx_mode[i].accept_maintenance_with_any_ID
				<<CSL_SRIO_RIO_TLM_SP_CONTROL_MTC_TGT_ID_DIS_SHIFT)
				|(rxMode->port_rx_mode[i].support_multicast_forwarding
				<<CSL_SRIO_RIO_TLM_SP_CONTROL_TGT_ID_DIS_SHIFT);
		}
	}
}

/*****************************************************************************
 Prototype    : KeyStone_SRIO_packet_forwarding_Cfg
 Description  : configure SRIO packet forwarding
 Input        : SRIO_PktForwarding_Cfg * PktForwardingEntry_cfg
                Uint32 pktForwardingEntryNum
 Output       : None
 Return Value :

  History        :
  1.Date         : 2011/6/11
    Author       : Brighton Feng
    Modification : Created function

*****************************************************************************/
void KeyStone_SRIO_packet_forwarding_Cfg(
	SRIO_PktForwarding_Cfg * PktForwardingEntry_cfg,
	Uint32 pktForwardingEntryNum)
{
    int i = 0;

	pktForwardingEntryNum= _min2(SRIO_MAX_FORWARDING_ENTRY_NUM,
		pktForwardingEntryNum);
    for(i=0; i<pktForwardingEntryNum; i++)
    {
		gp_srioRegs->PF_CNTL[i].RIO_PF_16B_CNTL =
			(PktForwardingEntry_cfg[i].forwardingID_up_16
				<< CSL_SRIO_RIO_PF_16B_CNTL_DEVID_16B_UP_SHIFT)
			|(PktForwardingEntry_cfg[i].forwardingID_lo_16
				<< CSL_SRIO_RIO_PF_16B_CNTL_DEVID_16B_LO_SHIFT);

		gp_srioRegs->PF_CNTL[i].RIO_PF_8B_CNTL =
			(PktForwardingEntry_cfg[i].forwardingID_up_8
				<< CSL_SRIO_RIO_PF_8B_CNTL_DEVID_8B_UP_SHIFT)
			|(PktForwardingEntry_cfg[i].forwardingID_lo_8
				<< CSL_SRIO_RIO_PF_8B_CNTL_DEVID_8B_LO_SHIFT)
			|(PktForwardingEntry_cfg[i].outport
				<< CSL_SRIO_RIO_PF_8B_CNTL_OUT_PORT_SHIFT);
    }
}

/*****************************************************************************
 Prototype    : KeyStone_SRIO_MulticastID_Cfg
 Description  : configure SRIO MulticastID
 Input        : SRIO_Multicast_ID_Cfg * multicastID
                Uint32 uiNumMulticastID
 Output       : None
 Return Value :

  History        :
  1.Date         : 2011/6/11
    Author       : Brighton Feng
    Modification : Created function

*****************************************************************************/
void KeyStone_SRIO_MulticastID_Cfg(
    SRIO_Multicast_ID_Cfg * multicastID,
    Uint32 uiNumMulticastID)
{
    int i = 0;

	uiNumMulticastID= _min2(SRIO_MAX_MULTICAST_ID_NUM, uiNumMulticastID);
	for(i=0; i<uiNumMulticastID; i++)
	{
		gp_srioRegs->RIO_MULTIID_REG[i] =
			(multicastID[i].multicast_8b_ID
				<< CSL_SRIO_RIO_MULTIID_REG_16B_NODEID_SHIFT)
			|(multicastID[i].multicast_8b_ID
				<< CSL_SRIO_RIO_MULTIID_REG_8B_NODEID_SHIFT);
	}
}

/*****************************************************************************
 Prototype    : KeyStone_SRIO_Flow_Control
 Description  : SRIO flow control setup
 Input        : SRIO_Flow_Control_ID * flowControlID
                Uint32 uiNumFlowControlID
 Output       : None
 Return Value :

  History        :
  1.Date         : 2011/6/11
    Author       : Brighton Feng
    Modification : Created function

*****************************************************************************/
void KeyStone_SRIO_Flow_Control(
    SRIO_Flow_Control_ID * flowControlID,
    Uint32 uiNumFlowControlID)
{
	int i;
	Uint32 uiFlowMask = 0;

	if(NULL != flowControlID)
	{
		uiNumFlowControlID= _min2(14, uiNumFlowControlID);
		for(i= 0; i< uiNumFlowControlID; i++)
			gp_srioRegs->RIO_FLOW_CNTL[i]=
				(flowControlID[i].tt<<CSL_SRIO_RIO_FLOW_CNTL_TT_SHIFT)
				|flowControlID[i].flow_control_ID;

		uiFlowMask= _set(uiFlowMask, 0, uiNumFlowControlID-1);
		uiFlowMask= _pack2(uiFlowMask, uiFlowMask);

		/*enable flow control in all TX modules*/
		for(i= 0; i< SRIO_MAX_LSU_NUM/2; i++)
			gp_srioRegs->RIO_LSU_FLOW_MASKS[i]= uiFlowMask;

		for(i= 0; i< SRIO_PKTDMA_MAX_CH_NUM/2; i++)
			gp_srioRegs->RIO_TX_CPPI_FLOW_MASKS[i]= uiFlowMask;
	}
}

/*****************************************************************************
 Prototype    : KeyStone_SRIO_Prioirity_Permission_Setup
 Description  : Priority setup
 Input        : SRIO_priority_permission * priority_permission
 Output       : None
 Return Value :

  History        :
  1.Date         : 2011/6/11
    Author       : Brighton Feng
    Modification : Created function

*****************************************************************************/
void KeyStone_SRIO_Prioirity_Permission_Setup(
	SRIO_priority_permission * priority_permission)
{
	if(NULL != priority_permission)
	{
		gp_srioRegs->RIO_PER_SET_CNTL = (gp_srioRegs->RIO_PER_SET_CNTL
			&(~(CSL_SRIO_RIO_PER_SET_CNTL_CBA_TRANS_PRI_MASK))) 	/*clear the field*/
			|(priority_permission->uiVbusPriority
				<<CSL_SRIO_RIO_PER_SET_CNTL_CBA_TRANS_PRI_SHIFT);
		gp_srioRegs->RIO_SUPERVISOR_ID =
			(priority_permission->supervisorHostID_8b<<
				CSL_SRIO_RIO_SUPERVISOR_ID_8B_SUPRVSR_ID_SHIFT)
			|(priority_permission->supervisorHostID_16b<<
				CSL_SRIO_RIO_SUPERVISOR_ID_16B_SUPRVSR_ID_SHIFT);
	}
}

/*****************************************************************************
 Prototype    : KeyStone_SRIO_Interrupt_init
 Description  : SRIO interrupt configuration
 Input        : SRIO_Interrupt_Cfg * interrupt_cfg
 Output       : None
 Return Value :

  History        :
  1.Date         : 2011/6/11
    Author       : Brighton Feng
    Modification : Created function

*****************************************************************************/
void KeyStone_SRIO_Interrupt_init(
	SRIO_Interrupt_Cfg * interrupt_cfg)
{
    Uint32 i;
    Uint32 reg, shift;
	volatile Uint32 * ICRR= (volatile Uint32 *)gp_srioRegs->DOORBELL_ICRR;

	if(NULL == interrupt_cfg)
		return;

    /* Clear all the interrupts */
    for(i=0; i<2; i++)
    {
        gp_srioRegs->LSU_ICSR_ICCR[i].RIO_LSU_ICCR	= 0xFFFFFFFF ;
    }
    for(i=0; i<4; i++)
    {
        gp_srioRegs->DOORBELL_ICSR_ICCR[i].RIO_DOORBELL_ICCR = 0xFFFFFFFF;
    }
	gp_srioRegs->RIO_ERR_RST_EVNT_ICCR = 0xFFFFFFFF;

    if(NULL != interrupt_cfg->interrupt_map)
    {
	    for(i=0; i<interrupt_cfg->uiNumInterruptMap; i++)
	    {
	        /* Get register index for the interrupt source*/
	        reg = interrupt_cfg->interrupt_map[i].interrupt_event >> 16;

	        /* Get shift value for the interrupt source*/
	        shift = interrupt_cfg->interrupt_map[i].interrupt_event & 0x0000FFFF;

	    	ICRR[reg]= (ICRR[reg]&(~(0xF<<shift))) 	/*clear the field*/
	    		|(interrupt_cfg->interrupt_map[i].INTDST_number<<shift);

	    }
	}

	gp_srioRegs->RIO_INTERRUPT_CTL = interrupt_cfg->doorbell_route_ctl;

	/*disable interrupt rate control*/
	gp_srioRegs->RIO_INTDST_RATE_DIS= 0xFFFF;
	for(i= 0; i< 16; i++)
	{
		gp_srioRegs->RIO_INTDST_RATE_CNT[i]= 0;
	}

	if(NULL != interrupt_cfg->interrupt_rate)
	{
		/*setup interrupt rate for specific INTDST*/
		for(i= 0; i<interrupt_cfg->uiNumInterruptRateCfg; i++)
		{
			/*enable rate control for this INTDST*/
			gp_srioRegs->RIO_INTDST_RATE_DIS &=
				~(1<<interrupt_cfg->interrupt_rate[i].INTDST_number);

			/*set interrupt rate counter for this INTDST*/
			gp_srioRegs->RIO_INTDST_RATE_CNT[i]=
				interrupt_cfg->interrupt_rate[i].interrupt_rate_counter;
		}
	}

	for(i=0;i<32;i++)
		CSL_SRIO_RouteLSUInterrupts(gp_srioRegs, i, 0);


    return;
}

/*****************************************************************************
 Prototype    : KeyStone_SRIO_Timeout_Config
 Description  : SRIO timeout configuration in microsecond
 Input        : SRIO_Config * srio_cfg
                Uint32 logicalRepsonseTimeout_us
                Uint32 physicalPortTimeout_us
                Uint32 linkInitSilenceTimeout_us
 Output       : None
 Return Value :

  History        :
  1.Date         : 2011/6/11
    Author       : Brighton Feng
    Modification : Created function

*****************************************************************************/
void KeyStone_SRIO_Timeout_Config(SRIO_Config * srio_cfg,
	Uint32 logicalRepsonseTimeout_us,
	Uint32 physicalPortTimeout_us,
	Uint32 linkInitSilenceTimeout_us)
{
	Uint32 uiTimeout, uiMaxTimeout;
	Uint32 byteClockMHz;

	/*PRESCALAR_SELECT is used to divide VBUSM clock(normally 333 to 400MHz,
	here use 350MHz), (VBUS_M clock)/(PRESCALAR_SELECT+1),
	to get about 50MHz clock with about 20ns period.*/
	gp_srioRegs->RIO_PER_SET_CNTL= (gp_srioRegs->RIO_PER_SET_CNTL
		&(~CSL_SRIO_RIO_PER_SET_CNTL_PRESCALER_SELECT_MASK))
		|((350/50-1)<<CSL_SRIO_RIO_PER_SET_CNTL_PRESCALER_SELECT_SHIFT);
//		|((gDSP_Core_Speed_Hz/1000000/3/50-1)<<CSL_SRIO_RIO_PER_SET_CNTL_PRESCALER_SELECT_SHIFT);

	/*logical layer response timeout
	logicalRepsonseTimeout = 15 x (scaled VBUS_M clock period) x TIMEOUT_VALUE),
	TIMEOUT_VALUE = logicalRepsonseTimeout/(scaled VBUS_M clock period)/15 */
	uiTimeout= logicalRepsonseTimeout_us*1000/20/15;
	uiMaxTimeout=CSL_SRIO_RIO_SP_RT_CTL_TVAL_MASK
		>>CSL_SRIO_RIO_SP_RT_CTL_TVAL_SHIFT;
	if(uiTimeout>uiMaxTimeout)
		uiTimeout= uiMaxTimeout;
	if(0==uiTimeout)
		uiTimeout= 1;
	gp_srioRegs->RIO_SP_RT_CTL= uiTimeout<<CSL_SRIO_RIO_SP_RT_CTL_TVAL_SHIFT;

	/*SRV_CLK should be scaled to about 10MHz (about 100ns period).
	SRV_CLK= (a SRIO internal IP clock)/ PRESCALAR_SRV_CLK.
	The SRIO internal IP clock is the byte clock of
	one of the lanes (selected by SYS_CLK_SEL).
	(Byte clock) = (link rate)/20.*/
	gp_srioRegs->RIO_PRESCALAR_SRV_CLK=
			(Uint32)srio_cfg->serdes_cfg->commonSetup.inputRefClock_MHz/10;

	/*physical layer response timeout.
	physicalPortTimeout = SRV_CLK period * TIMEOUT_VALUE * 3,
	TIMEOUT_VALUE = physicalPortTimeout/SRV_CLK period/3 */
	uiTimeout= physicalPortTimeout_us*1000/100/3;
	uiMaxTimeout=CSL_SRIO_RIO_SP_LT_CTL_TVAL_MASK
		>>CSL_SRIO_RIO_SP_LT_CTL_TVAL_SHIFT;
	if(uiTimeout>uiMaxTimeout)
		uiTimeout= uiMaxTimeout;
	if(0==uiTimeout)
		uiTimeout= 1;
	gp_srioRegs->RIO_SP_LT_CTL= uiTimeout<<CSL_SRIO_RIO_SP_LT_CTL_TVAL_SHIFT;

	/*port silence timeout
	The SRIO starts in the SILENT state. The link output driver is disabled
	to force the link partner to initialize regardless of its current state.
	The duration of the SILENT state is controlled by the silence_timer.
	The duration must be long enough to ensure that the link partner detects
	the silence (as a loss of lane_sync) and is forced to initialize but short
	enough that it is readily distinguished from a link break.
	linkInitSilenceTimeout is SRV_CLK period X 410 X SILENCE_TIMER,
	SILENCE_TIMER= linkInitSilenceTimeout/SRV_CLK period/410*/
	uiTimeout= linkInitSilenceTimeout_us*1000/100/410;
	uiMaxTimeout=CSL_SRIO_RIO_PLM_SP_SILENCE_TIMER_SILENCE_TIMER_MASK
		>>CSL_SRIO_RIO_PLM_SP_SILENCE_TIMER_SILENCE_TIMER_SHIFT;
	if(uiTimeout>uiMaxTimeout)
		uiTimeout= uiMaxTimeout;
	if(0==uiTimeout)
		uiTimeout= 1;
	uiTimeout= uiTimeout
		<<CSL_SRIO_RIO_PLM_SP_SILENCE_TIMER_SILENCE_TIMER_SHIFT;
	if(srio_cfg->blockEn.bLogic_Port_EN[0])
		gp_srioRegs->RIO_PLM[0].RIO_PLM_SP_SILENCE_TIMER= uiTimeout;
	if(srio_cfg->blockEn.bLogic_Port_EN[1])
		gp_srioRegs->RIO_PLM[1].RIO_PLM_SP_SILENCE_TIMER= uiTimeout;
	if(srio_cfg->blockEn.bLogic_Port_EN[2])
		gp_srioRegs->RIO_PLM[2].RIO_PLM_SP_SILENCE_TIMER= uiTimeout;
	if(srio_cfg->blockEn.bLogic_Port_EN[3])
		gp_srioRegs->RIO_PLM[3].RIO_PLM_SP_SILENCE_TIMER= uiTimeout;


}


Bool SrioWaitPllToLock(long long timeover)
{
	long long totalTime;

	totalTime = timeover;
	if(DRIVER_WAIT_FOREVER == timeover)
	{
		while(1)
		{
			if((boot_cfg_regs->STS_SRIO & 0x00000001) == 0x00000001)
				break;
			else
				tsc_delay_us(1);
		}
	}
	else
	{
		while(1)
		{
			if(((boot_cfg_regs->STS_SRIO & 0x00000001) == 0x00000001)
					|| totalTime < 0)
				break;
			else
			{
				tsc_delay_us(1);
				totalTime -= 1000;
			}
		}
	}

	if(totalTime<0)
		return FALSE;
	else
		return TRUE;
}


Bool SrioWaitPortToLink(int port,long long timeover)
{
	long long totalTime;

	totalTime = timeover;
	if(DRIVER_WAIT_FOREVER == timeover)
	{
		while(1)
		{
			if(0!=(gp_srioRegs->RIO_SP[port].RIO_SP_ERR_STAT&
					CSL_SRIO_RIO_SP_ERR_STAT_PORT_OK_MASK))
				break;
			else
				tsc_delay_us(1);
		}
	}
	else
	{
		while(1)
		{
			if(0!=(gp_srioRegs->RIO_SP[port].RIO_SP_ERR_STAT&
					CSL_SRIO_RIO_SP_ERR_STAT_PORT_OK_MASK)|| totalTime < 0)
				break;
			else
			{
				tsc_delay_us(1);
				totalTime -= 1000;
			}
		}
	}

	if(totalTime<0)
		return FALSE;
	else
		return TRUE;
}


int SrioGetLinkWidth(int port)
{
	Uint8 regVal;

	CSL_SRIO_GetInitializedPortWidth(gp_srioRegs, port, &regVal);

	if((0 == regVal) || (1 == regVal))
		return 1;
	else if(2 == regVal)
		return 4;
	else
		return 0;
}


void SrioFirstTestCode(void)
{
	gp_srioRegs->RIO_PER_SET_CNTL = 0x01053800;
	gp_srioRegs->BLOCK_ENABLE_STATUS[3].RIO_BLK_EN = 1;
	gp_srioRegs->BLOCK_ENABLE_STATUS[4].RIO_BLK_EN = 1;

	gp_srioRegs->RIO_INTDST_RATE_DIS=1;
	gp_srioRegs->RIO_PE_FEAT = 0x20000199;
	gp_srioRegs->RIO_SP[0].RIO_SP_LM_RESP = 0;

	gp_srioRegs->RIO_SP[0].RIO_SP_CTL2 = 0x02AA0000;
    gp_srioRegs->RIO_SP[0].RIO_SP_CTL = 0xD0600001;
	gp_srioRegs->RIO_SP[1].RIO_SP_CTL = 0x600001;
	gp_srioRegs->RIO_SP[2].RIO_SP_CTL = 0x600001;
	gp_srioRegs->RIO_SP[3].RIO_SP_CTL = 0x600001;

	gp_srioRegs->RIO_SP_ERR[0].RIO_SP_RATE_EN = 0;
	gp_srioRegs->RIO_SP_ERR[0].RIO_SP_ERR_ATTR_CAPT = 0;
	gp_srioRegs->RIO_SP_ERR[0].RIO_SP_ERR_THRESH = 0;
	gp_srioRegs->RIO_LANE[0].RIO_LANE_STAT0 = 0x7f88;
	gp_srioRegs->RIO_LANE[1].RIO_LANE_STAT0 = 0x107f88;
	gp_srioRegs->RIO_LANE[3].RIO_LANE_STAT0 = 0x307f88;

	gp_srioRegs->RIO_PLM[0].RIO_PLM_SP_ALL_PW_EN = 1;
	gp_srioRegs->RIO_PLM[0].RIO_PLM_SP_DISCOVERY_TIMER = 0x20000000;
}


void SrioSecondTestCode(void)
{
	int i,j;
	for(i=0;i<0x10000;i++);
	for(j=0;j<5;j++)
	{
		for(i=0; i<SRIO_MAX_PORT_NUM; i++)
		{
			gp_srioRegs->RIO_SP_ERR[i].RIO_SP_ERR_DET = 0;
			gp_srioRegs->RIO_SP[i].RIO_SP_ERR_STAT = 0xfffffff0;
			gp_srioRegs->RIO_PLM[i].RIO_PLM_SP_LONG_CS_TX1 = 0x2003F044;
		}
		for(i=0;i<0x1000;i++);
	}
}

/*****************************************************************************
 Prototype    : KeyStone_SRIO_Init
 Description  : SRIO initialization
 Input        : SRIO_Config * srio_cfg
 Output       : None
 Return Value :

  History        :
  1.Date         : 2011/6/11
    Author       : Brighton Feng
    Modification : Created function

  2.Date         : 2011/6/13
    Author       : Zhan
    Modification : Modify the LSU setup and SERDES configuration
*****************************************************************************/

int KeyStone_Srio_Init(SRIO_Config* srio_cfg, long long timeover)
{
    Uint32 cfgValue = 0;
    Uint32 i = 0;

    /* Route LSU0 ICR0 to INTDST0 */
    CSL_SRIO_RouteLSUInterrupts(gp_srioRegs, 0, 0);
    /* Route LSU1 ICR0 to INTDST0 */
    CSL_SRIO_RouteLSUInterrupts(gp_srioRegs, 1, 0);
    /* Route LSU2 ICR0 to INTDST0 */
    CSL_SRIO_RouteLSUInterrupts(gp_srioRegs, 2, 0);

	//enable SRIO power and clock domain
	KeyStone_enable_PSC_module(CSL_PSC_PD_SRIO, CSL_PSC_LPSC_SRIO);

    /*Clear BOOT_COMPLETE bit*/
	gp_srioRegs->RIO_PER_SET_CNTL &= (~(1 << CSL_SRIO_RIO_PER_SET_CNTL_BOOT_COMPLETE_SHIFT));

    //bring SRIO from reset isolation
	gp_srioRegs->RIO_PCR = 0x11;

	/*enable globally used blocks including MMR block in SRIO*/
	KeyStone_SRIO_GlobalEnable();

    /*The LSU setup registers are only programmable
    while the LSU is disabled while the peripheral is enabled.*/
	if(srio_cfg->lsu_cfg)
	{
	    /*setup the shadow registers allocation between LSU*/
	    gp_srioRegs->RIO_LSU_SETUP_REG0 =
	    	(srio_cfg->lsu_cfg->lsuGrp0ShadowRegsSetup << CSL_SRIO_RIO_LSU_SETUP_REG0_SHADOW_GRP0_SHIFT)|
	        (srio_cfg->lsu_cfg->lsuGrp1ShadowRegsSetup << CSL_SRIO_RIO_LSU_SETUP_REG0_SHADOW_GRP1_SHIFT);

	    /*setup LSU interrupt based on LSU number or Source ID*/
	    cfgValue = 0;
	    for(i=0; i<SRIO_MAX_LSU_NUM; i++)
	    {
	        cfgValue |= srio_cfg->lsu_cfg->lsuIntSetup[i] << i;
	    }
	    gp_srioRegs->RIO_LSU_SETUP_REG1 = cfgValue;
	}

	/*enable other optional blocks*/
	KeyStone_SRIO_enable_blocks(&srio_cfg->blockEn);

	if(SRIO_SERDES_LOOPBACK==srio_cfg->loopback_mode)
	{
		if(srio_cfg->serdes_cfg->linkSetup[0])
			srio_cfg->serdes_cfg->linkSetup[0]->loopBack = SERDES_LOOPBACK_ENABLE;
		if(srio_cfg->serdes_cfg->linkSetup[1])
			srio_cfg->serdes_cfg->linkSetup[1]->loopBack = SERDES_LOOPBACK_ENABLE;
		if(srio_cfg->serdes_cfg->linkSetup[2])
			srio_cfg->serdes_cfg->linkSetup[2]->loopBack = SERDES_LOOPBACK_ENABLE;
		if(srio_cfg->serdes_cfg->linkSetup[3])
			srio_cfg->serdes_cfg->linkSetup[3]->loopBack = SERDES_LOOPBACK_ENABLE;
	}

	SRIO_HyperLink_Serdes_init(srio_cfg->serdes_cfg, gp_srioSerdesRegs);

	if(!SrioWaitPllToLock(timeover))
		return DRIVER_RESULT_PLL_UNLOCK;

	KeyStone_SRIO_set_1x2x4x_Path(srio_cfg->srio_1x2x4x_path_control);

	KeyStone_SRIO_set_device_ID(srio_cfg->device_ID_routing_config,
			srio_cfg->uiNumDeviceId);

	KeyStone_SRIO_CSR_CAR_Config(srio_cfg);


	/*Allocates ingress Data Nodes and Tags based on priority.
	These registers must only be changed while boot_complete is
	deasserted or while the port is held in reset.*/
	for(i=0;i<SRIO_MAX_PORT_NUM;i++)
	{
		if(FALSE == srio_cfg->blockEn.bBLK5_8_Port_Datapath_EN[i])
			continue;

		/*maximum data nodes and tags are 72 (0x48).
		 Each data node stores 32 bytes of data*/
		gp_srioRegs->RIO_PBM[i].RIO_PBM_SP_IG_WATERMARK0 =
			(36<<CSL_SRIO_RIO_PBM_SP_IG_WATERMARK0_PRIO0_WM_SHIFT)
			|(32<<CSL_SRIO_RIO_PBM_SP_IG_WATERMARK0_PRIO0CRF_WM_SHIFT);
		gp_srioRegs->RIO_PBM[i].RIO_PBM_SP_IG_WATERMARK1 =
			(28<<CSL_SRIO_RIO_PBM_SP_IG_WATERMARK1_PRIO1_WM_SHIFT)
			|(24<<CSL_SRIO_RIO_PBM_SP_IG_WATERMARK1_PRIO1CRF_WM_SHIFT);
		gp_srioRegs->RIO_PBM[i].RIO_PBM_SP_IG_WATERMARK2 =
			(20<<CSL_SRIO_RIO_PBM_SP_IG_WATERMARK2_PRIO2_WM_SHIFT)
			|(16<<CSL_SRIO_RIO_PBM_SP_IG_WATERMARK2_PRIO2CRF_WM_SHIFT);
		gp_srioRegs->RIO_PBM[i].RIO_PBM_SP_IG_WATERMARK3 =
			(12<<CSL_SRIO_RIO_PBM_SP_IG_WATERMARK3_PRIO3_WM_SHIFT)
			|(8<<CSL_SRIO_RIO_PBM_SP_IG_WATERMARK3_PRIO3CRF_WM_SHIFT);
	}

	/*setup timeout value in microsecond*/
	KeyStone_SRIO_Timeout_Config(srio_cfg, 500000, 50, 100);

    KeyStone_SRIO_RxMode_Setup(srio_cfg->rxMode);

    KeyStone_SRIO_packet_forwarding_Cfg(srio_cfg->PktForwardingEntry_cfg,
        srio_cfg->uiNumPktForwardingEntry);

    KeyStone_SRIO_MulticastID_Cfg(srio_cfg->multicastID,
        srio_cfg->uiNumMulticastID);

    KeyStone_SRIO_Flow_Control(srio_cfg->flowControlID,
    	srio_cfg->uiNumFlowControlID);

	KeyStone_SRIO_Prioirity_Permission_Setup(
		srio_cfg->priority_permission);

    KeyStone_SRIO_Interrupt_init(srio_cfg->interrupt_cfg);

	if(SRIO_DIGITAL_LOOPBACK==srio_cfg->loopback_mode)
	{
		gp_srioRegs->RIO_PER_SET_CNTL1 |=
			(0xF<<CSL_SRIO_RIO_PER_SET_CNTL1_LOOPBACK_SHIFT);
	}
	else if(SRIO_EXTERNAL_LINE_LOOPBACK==srio_cfg->loopback_mode)
	{
		for(i=0; i<SRIO_MAX_PORT_NUM; i++)
		{
			gp_srioRegs->RIO_PLM[i].RIO_PLM_SP_IMP_SPEC_CTL=
				(1<<CSL_SRIO_RIO_PLM_SP_IMP_SPEC_CTL_LLB_EN_SHIFT);
		}
	}
    /*Set BOOT_COMPLETE bit*/
    gp_srioRegs->RIO_PER_SET_CNTL |= (1 << CSL_SRIO_RIO_PER_SET_CNTL_BOOT_COMPLETE_SHIFT);

    /*This should be the last enable bit to toggle when bringing the
    device out of reset to begin normal operation.*/
    gp_srioRegs->RIO_PCR|= CSL_SRIO_RIO_PCR_PEREN_MASK;

    SrioFirstTestCode();

    for(i=0; i<SRIO_MAX_PORT_NUM; i++)
    {
    	if(srio_cfg->blockEn.bLogic_Port_EN[i])
    	{
    		if(!SrioWaitPortToLink(i,timeover))
    			return DRIVER_RESULT_LINK_TIMEOUT;
    	}
    }

    int linkWidth = 0;
    if(SRIO_PATH_CTL_4xLaneABCD == srio_cfg->srio_1x2x4x_path_control)
    {
    	linkWidth = SrioGetLinkWidth(0);
    	if(4 != linkWidth)
    		return DRIVER_LINK_STAT_ERROR;
    }

	//for debug
    KeyStone_SRIO_Error_Capture_Enable(&srio_cfg->blockEn);

    SrioSecondTestCode();

    return DRIVER_RESULT_OK;
}