/******************************************************************************

  Copyright (C), 2013-2014, Texas Instrument.

 ******************************************************************************
  File Name     : K2_GE_Init_drv.c
  Version       : Initial Draft
  Author        : Brighton Feng
  Created       : June 8, 2013
  Last Modified :
  Description   : example for Gigbit ethernet configuration and 
                  transfer driver on KeyStone 2 device

  History       :
  1.Date        : June 8, 2013
    Author      : Brighton Feng
    Modification: Created file
  2.Date        : October 16, 2014
    Author      : Brighton Feng
    Modification: Updated for K2E/K2L
******************************************************************************/
#include <stdio.h>
#include <stdint.h>
#include <string.h>
#include <csl_cpgmac_slAux.h>
#include <csl_pscAux.h>
#include <csl_serdes_ethernet.h>
#include "K2_common.h"
#include "K2_board_init.h"
#include "K2_Navigator_init_drv.h"
#include "K2_GE_Init_drv.h"

CSL_Pa_ssRegs * gpNetCP_regs = (CSL_Pa_ssRegs *)CSL_NETCP_CFG_REGS;

#if defined(DEVICE_K2H) || defined(DEVICE_K2K)
CSL_CpsgmiiRegs *  gpSGMII_regs[4] = 
{
	(CSL_CpsgmiiRegs *)(CSL_NETCP_CFG_REGS + 0x00090100),
	(CSL_CpsgmiiRegs *)(CSL_NETCP_CFG_REGS + 0x00090200),
	(CSL_CpsgmiiRegs *)(CSL_NETCP_CFG_REGS + 0x00090400),
	(CSL_CpsgmiiRegs *)(CSL_NETCP_CFG_REGS + 0x00090500)
};
CSL_MdioRegs *  gpMDIO_regs = ((CSL_MdioRegs *) (CSL_NETCP_CFG_REGS + 0x00090300));
CSL_Cpsw_5gfRegs *  gpCPSW_regs = ((CSL_Cpsw_5gfRegs *) (CSL_NETCP_CFG_REGS + 0x00090800));
CSL_Cpsw_5gfPort1_infoRegs *  gpCPSW_port_regs[4] = 
{
	(CSL_Cpsw_5gfPort1_infoRegs *)(CSL_NETCP_CFG_REGS + 0x00090860),
	(CSL_Cpsw_5gfPort1_infoRegs *)(CSL_NETCP_CFG_REGS + 0x00090890),
	(CSL_Cpsw_5gfPort1_infoRegs *)(CSL_NETCP_CFG_REGS + 0x00090A00),
	(CSL_Cpsw_5gfPort1_infoRegs *)(CSL_NETCP_CFG_REGS + 0x00090A30)
};
CSL_Cpgmac_slRegs *  gpMAC_regs[4] = 
{
	(CSL_Cpgmac_slRegs *)(CSL_NETCP_CFG_REGS + 0x00090900),
	(CSL_Cpgmac_slRegs *)(CSL_NETCP_CFG_REGS + 0x00090940),
	(CSL_Cpgmac_slRegs *)(CSL_NETCP_CFG_REGS + 0x00090980),
	(CSL_Cpgmac_slRegs *)(CSL_NETCP_CFG_REGS + 0x000909C0)
};

/*STATC and STATD are at the same address as STATA and STATB respectively. 
They are paged. You can see STATA and STATB or STATC and STATD at any given time. 
You have to write a 1 to bit 28 of stat_port_en register in order to see STATC/D.*/
CSL_Cpsw_5gfPort_stats_groupRegs * gpStats_regs[4]=
{
	(CSL_Cpsw_5gfPort_stats_groupRegs *)(CSL_NETCP_CFG_REGS + 0x00090B00),
	(CSL_Cpsw_5gfPort_stats_groupRegs *)(CSL_NETCP_CFG_REGS + 0x00090C00),
	(CSL_Cpsw_5gfPort_stats_groupRegs *)(CSL_NETCP_CFG_REGS + 0x00090B00),
	(CSL_Cpsw_5gfPort_stats_groupRegs *)(CSL_NETCP_CFG_REGS + 0x00090C00)
};

//accumulation of the statistics values
Ethernet_Statistics statistics[GE_NUM_ETHERNET_PORT];

//enable stauts of statistics modules
Bool STATS_EN[4]= {FALSE, FALSE, FALSE, FALSE};

/*address look up table initialize*/
void K2_GE_ALE_Init(Ethernet_ALE_Config *   ale_cfg)
{
	int i;
	Uint32 uiALE_entry_index;

	/*Note: Even you do not want to use the features of ALE, ALE should always 
	be enabled and port should be enabled to forward packet. Otherwise, all
	packets to ALE ports will be dropped.
	If you do not need the features of ALE, you should enable ALE and ports and
	enable the BYPASS mode*/

	/*set port state as forward*/
	for(i=0; i< GE_NUM_ETHERNET_PORT+1; i++)
		gpCPSW_regs->ALE_PORT_CONTROL_REG[i]= 
			ALE_PORTSTATE_FORWARD<<CSL_CPSW_5GF_ALE_PORT_CONTROL_REG_PORT_STATE_SHIFT;

	if(NULL==ale_cfg)
	{
		//no ALE entries, set ALE in bypass mode
		gpCPSW_regs->ALE_CONTROL_REG = CSL_CPSW_5GF_ALE_CONTROL_REG_ENABLE_ALE_MASK
			|CSL_CPSW_5GF_ALE_CONTROL_REG_ALE_BYPASS_MASK;
		return;
	}

	/*The input clock is divided by this value for use in the 
	multicast/broadcast rate limiters. The minimum operating value 
	is 10h. The prescaler is off when the value is zero.*/
	gpCPSW_regs->ALE_PRESCALE_REG= 0x1e848;

	/*Clear ALE address table. Setting this bit causes the ALE hardware to write 
	all table bit values to zero. Software must perform a clear table operation 
	as part of the ALE setup/configuration process. Setting this bit causes all 
	ALE accesses to be held up for 64 clocks while the clear is performed. 
	Access to all ALE registers will be blocked (wait states) until the 64 clocks 
	have completed. This bit cannot be read as one because the read is blocked 
	until the clear table is completed at which time this bit is cleared to zero.*/
	gpCPSW_regs->ALE_CONTROL_REG = CSL_CPSW_5GF_ALE_CONTROL_REG_ENABLE_ALE_MASK
		|CSL_CPSW_5GF_ALE_CONTROL_REG_CLEAR_TABLE_MASK;

	uiALE_entry_index= 0;

	for(i=0; i< ale_cfg->num_multicastEntries; i++)
	{
		CSL_CPSW_nGF_setAleMcastAddrEntry(uiALE_entry_index,
			&ale_cfg->multicastEntries[i]);
		uiALE_entry_index++;
	}
	
	for(i=0; i< ale_cfg->num_OUI_entries; i++)
	{
		CSL_CPSW_nGF_setAleOUIAddrEntry(uiALE_entry_index,
			&ale_cfg->OUI_entries[i]);
		uiALE_entry_index++;
	}
	
	for(i=0; i< ale_cfg->num_unicastEntries; i++)
	{
		CSL_CPSW_nGF_setAleUnicastAddrEntry(uiALE_entry_index,
			&ale_cfg->unicastEntries[i]);
		uiALE_entry_index++;
	}
	
	for(i=0; i< ale_cfg->num_VLAN_entires; i++)
	{
		CSL_CPSW_nGF_setAleVlanEntry(uiALE_entry_index,
			&ale_cfg->VLAN_entires[i]);
		uiALE_entry_index++;
	}
	
	for(i=0; i< ale_cfg->num_VLAN_multicastEntries; i++)
	{
		CSL_CPSW_nGF_setAleVlanMcastAddrEntry(uiALE_entry_index,
			&ale_cfg->VLAN_multicastEntries[i]);
		uiALE_entry_index++;
	}
	
	for(i=0; i< ale_cfg->num_VLAN_UnicastEntries; i++)
	{
		CSL_CPSW_nGF_setAleVlanUnicastAddrEntry(uiALE_entry_index,
			&ale_cfg->VLAN_UnicastEntries[i]);
		uiALE_entry_index++;
	}

}

/*statistics enable*/
void K2_GE_Statistics_Init(Ethernet_Port_Config * ethernet_port_cfg[])
{
	int i;
	Uint32 uiReg_value= 0;

	memset(statistics, 0, sizeof(statistics));
	STATS_EN[0]= FALSE;
	STATS_EN[1]= FALSE;
	STATS_EN[2]= FALSE;
	STATS_EN[3]= FALSE;

	gpCPSW_regs->STAT_PORT_EN_REG= 0; 	//disable statistics before configuration

	if(ethernet_port_cfg[0])
	{
		if(ethernet_port_cfg[0]->ethenet_port_statistics_enable)
		{
			uiReg_value |= CSL_CPSW_5GF_STAT_PORT_EN_REG_P1_STAT_EN_MASK;
			STATS_EN[1]= TRUE;
		}
		if(ethernet_port_cfg[0]->host_port_statistics_enable)
		{
			uiReg_value |= CSL_CPSW_5GF_STAT_PORT_EN_REG_P0A_STAT_EN_MASK;
			STATS_EN[0]= TRUE;
		}
	}

	if(ethernet_port_cfg[1])
	{
		if(ethernet_port_cfg[1]->ethenet_port_statistics_enable)
		{
			uiReg_value |= CSL_CPSW_5GF_STAT_PORT_EN_REG_P2_STAT_EN_MASK;
			STATS_EN[1]= TRUE;
		}
		if(ethernet_port_cfg[1]->host_port_statistics_enable)
		{
			uiReg_value |= CSL_CPSW_5GF_STAT_PORT_EN_REG_P0B_STAT_EN_MASK;
			STATS_EN[0]= TRUE;
		}
	}

#if (4==GE_NUM_ETHERNET_PORT)
	if(ethernet_port_cfg[2])
	{
		if(ethernet_port_cfg[2]->ethenet_port_statistics_enable)
		{
			uiReg_value |= CSL_CPSW_5GF_STAT_PORT_EN_REG_P3_STAT_EN_MASK;
			STATS_EN[3]= TRUE;
		}
		if(ethernet_port_cfg[2]->host_port_statistics_enable)
		{
			uiReg_value |= CSL_CPSW_5GF_STAT_PORT_EN_REG_P0C_STAT_EN_MASK;
			STATS_EN[0]= TRUE;
			STATS_EN[2]= TRUE;
		}
	}

	if(ethernet_port_cfg[3])
	{
		if(ethernet_port_cfg[3]->ethenet_port_statistics_enable)
		{
			uiReg_value |= CSL_CPSW_5GF_STAT_PORT_EN_REG_P4_STAT_EN_MASK;
			STATS_EN[3]= TRUE;
		}
		if(ethernet_port_cfg[3]->host_port_statistics_enable)
		{
			uiReg_value |= CSL_CPSW_5GF_STAT_PORT_EN_REG_P0D_STAT_EN_MASK;
			STATS_EN[0]= TRUE;
			STATS_EN[2]= TRUE;
		}
	}
#endif

	//clear statistics values
	for(i=0; i<GE_NUM_ETHERNET_PORT; i++)
	{
		/*select Statistics module for register access*/
		K2_GE_Statistics_Select(i);
		
		/*While disabled, all statistics registers can be read and written 
		normally, so writing to 0x00000000 clears a statistics register.*/
		memset(gpStats_regs[i], 0, sizeof(CSL_CPSW_5GF_STATS));
	}

	gpCPSW_regs->STAT_PORT_EN_REG= uiReg_value;
}

/*select Statistics module for register access*/
void K2_GE_Statistics_Select(GE_Statistics_Module STATS)
{
	/*STATC and STATD are at the same address as STATA and STATB respectively. 
	They are paged. You can see STATA and STATB or STATC and STATD at any given time. 
	You have to write a 1 to bit 28 of stat_port_en register in order to see STATC/D.*/
	if(STATS<=GE_STATSB)
		gpCPSW_regs->STAT_PORT_EN_REG &= ~(1<<28); //clear the bit
	else
		gpCPSW_regs->STAT_PORT_EN_REG |= (1<<28); //clear the bit

}

/*accumulate the statistics values in the registers to the software data structure.
application should call this function before the 32-bit register counters overflow*/
void K2_GE_Accumulate_Statistics()
{
	int i;
	CSL_CPSW_5GF_STATS stats_regs_copy;
	
	for(i=0; i< GE_NUM_ETHERNET_PORT; i++)
	{
		if(STATS_EN[i])
		{
			/*select Statistics module for register access*/
			K2_GE_Statistics_Select(i);
			
			//copy the value to local data structure
			memcpy((void *)&stats_regs_copy, (void *)gpStats_regs[i], sizeof(CSL_CPSW_5GF_STATS));

			//decrease the values in the registers
			memcpy((void *)gpStats_regs[i], (void *)&stats_regs_copy, sizeof(CSL_CPSW_5GF_STATS));

			//accumulate the values in register to 64-bit data structure
		    statistics[i].RxGoodFrames      += stats_regs_copy.RxGoodFrames     ;
		    statistics[i].RxBCastFrames     += stats_regs_copy.RxBCastFrames    ;
		    statistics[i].RxMCastFrames     += stats_regs_copy.RxMCastFrames    ;
		    statistics[i].RxPauseFrames     += stats_regs_copy.RxPauseFrames    ;
		    statistics[i].RxCRCErrors       += stats_regs_copy.RxCRCErrors      ;
		    statistics[i].RxAlignCodeErrors += stats_regs_copy.RxAlignCodeErrors;
		    statistics[i].RxOversized       += stats_regs_copy.RxOversized      ;
		    statistics[i].RxJabber          += stats_regs_copy.RxJabber         ;
		    statistics[i].RxUndersized      += stats_regs_copy.RxUndersized     ;
		    statistics[i].RxFragments       += stats_regs_copy.RxFragments      ;
		    statistics[i].RxOctets          += stats_regs_copy.RxOctets         ;
		    statistics[i].TxGoodFrames      += stats_regs_copy.TxGoodFrames     ;
		    statistics[i].TxBCastFrames     += stats_regs_copy.TxBCastFrames    ;
		    statistics[i].TxMCastFrames     += stats_regs_copy.TxMCastFrames    ;
		    statistics[i].TxPauseFrames     += stats_regs_copy.TxPauseFrames    ;
		    statistics[i].TxDeferred        += stats_regs_copy.TxDeferred       ;
		    statistics[i].TxCollision       += stats_regs_copy.TxCollision      ;
		    statistics[i].TxSingleColl      += stats_regs_copy.TxSingleColl     ;
		    statistics[i].TxMultiColl       += stats_regs_copy.TxMultiColl      ;
		    statistics[i].TxExcessiveColl   += stats_regs_copy.TxExcessiveColl  ;
		    statistics[i].TxLateColl        += stats_regs_copy.TxLateColl       ;
		    statistics[i].TxCarrierSLoss    += stats_regs_copy.TxCarrierSLoss   ;
		    statistics[i].TxOctets          += stats_regs_copy.TxOctets         ;
		    statistics[i].Frame64           += stats_regs_copy.Frame64          ;
		    statistics[i].Frame65t127       += stats_regs_copy.Frame65t127      ;
		    statistics[i].Frame128t255      += stats_regs_copy.Frame128t255     ;
		    statistics[i].Frame256t511      += stats_regs_copy.Frame256t511     ;
		    statistics[i].Frame512t1023     += stats_regs_copy.Frame512t1023    ;
		    statistics[i].Frame1024tUp      += stats_regs_copy.Frame1024tUp     ;
		    statistics[i].NetOctets         += stats_regs_copy.NetOctets        ;
		}
	}	
}


/*MAC address initialization for flow control*/
void K2_GE_Flow_Control_MAC_Address_Init(
	Ethernet_Port_Config * ethernet_port_cfg[])
{
	int i;
	Uint32 uiReg_value= 0;

	for(i=0; i< GE_NUM_ETHERNET_PORT; i++)
	{
		if(ethernet_port_cfg[i])	
		{
			if(ethernet_port_cfg[i]->RX_flow_control_enable||
				ethernet_port_cfg[i]->RX_flow_control_enable)
			{
				gpCPSW_port_regs[i]->SL_SA_LO_REG= 
					_hill(ethernet_port_cfg[i]->flow_control_MAC_Address);
				gpCPSW_port_regs[i]->SL_SA_HI_REG= 
					_loll(ethernet_port_cfg[i]->flow_control_MAC_Address);
			}
			
			/*setup value for gpCPSW_regs->FLOW_CONTROL_REG*/
			if(ethernet_port_cfg[i]->RX_flow_control_enable)
				uiReg_value |= (1<<(i+1));
		}
	}
	/*enable port 0 and ethernet port flow control.
	port 0 flow control should always be enabled, it will push back to 
	packet DMA to avoid it transmit too many packet to overflow the FIFO in CPSW*/
	gpCPSW_regs->FLOW_CONTROL_REG= uiReg_value
		|CSL_CPSW_5GF_FLOW_CONTROL_REG_P0_FLOW_EN_MASK;
}

/*initialize the CPPI Info Word 0 SRC_ID field for received packets*/
void K2_GE_CPPI_Src_ID_Init(
	Ethernet_Port_Config * ethernet_port_cfg[])
{
	Uint32 uiReg_value= 0;

	if(ethernet_port_cfg[0])	
		uiReg_value |= (ethernet_port_cfg[0]->CPPI_Src_ID
				<<CSL_CPSW_5GF_P0_CPPI_SRC_ID_REG_TXA_SRC_ID_SHIFT);
	
	if(ethernet_port_cfg[1])	
		uiReg_value |= (ethernet_port_cfg[1]->CPPI_Src_ID
				<<CSL_CPSW_5GF_P0_CPPI_SRC_ID_REG_TXB_SRC_ID_SHIFT);

#if (4==GE_NUM_ETHERNET_PORT)
	if(ethernet_port_cfg[2])	
		uiReg_value |= (ethernet_port_cfg[2]->CPPI_Src_ID
				<<CSL_CPSW_5GF_P0_CPPI_SRC_ID_REG_TXC_SRC_ID_SHIFT);
	
	if(ethernet_port_cfg[3])	
		uiReg_value |= (ethernet_port_cfg[3]->CPPI_Src_ID
				<<CSL_CPSW_5GF_P0_CPPI_SRC_ID_REG_TXD_SRC_ID_SHIFT);
#endif	
	gpCPSW_regs->P0_CPPI_SRC_ID_REG= uiReg_value;
}
#else 	//K2E or K2L
CSL_CpsgmiiRegs *  gpSGMII_regs[GE_NUM_ETHERNET_PORT] = 
{
	(CSL_CpsgmiiRegs *)(CSL_NETCP_CFG_REGS + 0x00200100),
	(CSL_CpsgmiiRegs *)(CSL_NETCP_CFG_REGS + 0x00200200),
	(CSL_CpsgmiiRegs *)(CSL_NETCP_CFG_REGS + 0x00200300),
	(CSL_CpsgmiiRegs *)(CSL_NETCP_CFG_REGS + 0x00200400)
#ifdef DEVICE_K2E
	,
	(CSL_CpsgmiiRegs *)(CSL_NETCP_CFG_REGS + 0x00200500),
	(CSL_CpsgmiiRegs *)(CSL_NETCP_CFG_REGS + 0x00200600),
	(CSL_CpsgmiiRegs *)(CSL_NETCP_CFG_REGS + 0x00200700),
	(CSL_CpsgmiiRegs *)(CSL_NETCP_CFG_REGS + 0x00200800)
#endif
};
CSL_MdioRegs *  gpMDIO_regs = (CSL_MdioRegs *) (CSL_NETCP_CFG_REGS + 0x00200F00);

CSL_Xge_cpswRegs * gpCPSW_regs = (CSL_Xge_cpswRegs *) (CSL_NETCP_CFG_REGS + 0x00220000);

CSL_AleRegs * gpCPSW_ALE_regs = (CSL_AleRegs *) (CSL_NETCP_CFG_REGS + 0x0023e000);


//accumulation of the statistics values
Ethernet_Statistics statistics[GE_NUM_ETHERNET_PORT+1];

/*address look up table initialize*/
void K2_GE_ALE_Init(Ethernet_ALE_Config *   ale_cfg)
{
	int i;
	Uint32 uiALE_entry_index;

	/*Note: Even you do not want to use the features of ALE, ALE should always 
	be enabled and port should be enabled to forward packet. Otherwise, all
	packets to ALE ports will be dropped.
	If you do not need the features of ALE, you should enable ALE and ports and
	enable the BYPASS mode*/

	/*set port state as forward*/
	for(i=0; i< GE_NUM_ETHERNET_PORT+1; i++)
		gpCPSW_ALE_regs->PORT_CONTROL_REG[i]= 
			ALE_PORTSTATE_FORWARD<<CSL_ALE_PORT_CONTROL_REG_PORT_STATE_SHIFT;

	//gpCPSW_ALE_regs->UNKNOWN_VLAN_REG= 0x1FF;
	
	if(NULL==ale_cfg)
	{
		//no ALE entries, set ALE in bypass mode
		gpCPSW_ALE_regs->CONTROL_REG = CSL_ALE_CONTROL_REG_ENABLE_MASK
			|CSL_ALE_CONTROL_REG_BYPASS_MASK;
		return;
	}

	/*The input clock is divided by this value for use in the 
	multicast/broadcast rate limiters. The minimum operating value 
	is 10h. The prescaler is off when the value is zero.*/
	gpCPSW_ALE_regs->PRESCALE_REG= 0x1e848;

	/*Clear ALE address table. Setting this bit causes the ALE hardware to write 
	all table bit values to zero. Software must perform a clear table operation 
	as part of the ALE setup/configuration process. Setting this bit causes all 
	ALE accesses to be held up for 64 clocks while the clear is performed. 
	Access to all ALE registers will be blocked (wait states) until the 64 clocks 
	have completed. This bit cannot be read as one because the read is blocked 
	until the clear table is completed at which time this bit is cleared to zero.*/
	gpCPSW_ALE_regs->CONTROL_REG = CSL_ALE_CONTROL_REG_ENABLE_MASK
		|CSL_ALE_CONTROL_REG_CLEAR_TABLE_MASK;

	uiALE_entry_index= 0;

	for(i=0; i< ale_cfg->num_multicastEntries; i++)
	{
		CSL_CPSW_setAleMcastAddrEntry(uiALE_entry_index,
			&ale_cfg->multicastEntries[i]);
		uiALE_entry_index++;
	}
	
	for(i=0; i< ale_cfg->num_OUI_entries; i++)
	{
		CSL_CPSW_setAleOUIAddrEntry(uiALE_entry_index,
			&ale_cfg->OUI_entries[i]);
		uiALE_entry_index++;
	}
	
	for(i=0; i< ale_cfg->num_unicastEntries; i++)
	{
		CSL_CPSW_setAleUnicastAddrEntry(uiALE_entry_index,
			&ale_cfg->unicastEntries[i]);
		uiALE_entry_index++;
	}
	
	for(i=0; i< ale_cfg->num_VLAN_entires; i++)
	{
		CSL_CPSW_setAleVlanEntry(uiALE_entry_index,
			&ale_cfg->VLAN_entires[i]);
		uiALE_entry_index++;
	}
	
	for(i=0; i< ale_cfg->num_VLAN_multicastEntries; i++)
	{
		CSL_CPSW_setAleVlanMcastAddrEntry(uiALE_entry_index,
			&ale_cfg->VLAN_multicastEntries[i]);
		uiALE_entry_index++;
	}
	
	for(i=0; i< ale_cfg->num_VLAN_UnicastEntries; i++)
	{
		CSL_CPSW_setAleVlanUnicastAddrEntry(uiALE_entry_index,
			&ale_cfg->VLAN_UnicastEntries[i]);
		uiALE_entry_index++;
	}


}

/*statistics enable*/
void K2_GE_Statistics_Init(Ethernet_Port_Config * ethernet_port_cfg[])
{
	int i, j;
	Uint32 uiStatEnMask= 1; 	//enable port 0 statistics by default
	volatile Uint32 * uipReg;

	memset(statistics, 0, sizeof(statistics));

	gpCPSW_regs->STAT_PORT_EN_REG= 0; 	//disable statistics before configuration

	/*While disabled, all statistics registers can be read and written 
	normally, so writing to 0x00000000 clears a statistics register for host port 0.*/
	uipReg= &gpCPSW_regs->STATS[0].RXGOODFRAMES;
	for(j=0; j< sizeof(CSL_Xge_cpswStatsRegs)/4; j++)
		*uipReg++= 0;

	for(i=0; i<GE_NUM_ETHERNET_PORT; i++)
	{
		/*While disabled, all statistics registers can be read and written 
		normally, so writing to 0x00000000 clears a statistics register.*/
		uipReg= &gpCPSW_regs->STATS[i+1].RXGOODFRAMES;
		for(j=0; j< sizeof(CSL_Xge_cpswStatsRegs)/4; j++)
			*uipReg++= 0;

		if(ethernet_port_cfg[i])
		{
			if(ethernet_port_cfg[i]->statistics_enable)
			{
				uiStatEnMask |= 1<<(i+1);
			}			
		}
	}

	gpCPSW_regs->STAT_PORT_EN_REG= uiStatEnMask;
}
/*accumulate the statistics values in the registers to the software data structure.
application should call this function before the 32-bit register counters overflow*/
void K2_GE_Accumulate_Statistics()
{
	int i;
	CSL_Xge_cpswStatsRegs stats_regs_copy;

	Uint32 uiPortStatEn= gpCPSW_regs->STAT_PORT_EN_REG;
	
	for(i=0; i< GE_NUM_ETHERNET_PORT+1; i++)
	{
		if(uiPortStatEn&(1<<i))
		{
			//copy the value to local data structure
			memcpy((void *)&stats_regs_copy, (void *)&gpCPSW_regs->STATS[i], sizeof(CSL_Xge_cpswStatsRegs));

			//decrease the values in the registers
			memcpy((void *)&gpCPSW_regs->STATS[i], (void *)&stats_regs_copy, sizeof(CSL_Xge_cpswStatsRegs));

			//accumulate the values in register to 64-bit data structure
		    statistics[i].RxGoodFrames      += stats_regs_copy.RXGOODFRAMES     ;
		    statistics[i].RxBCastFrames     += stats_regs_copy.RXBROADCASTFRAMES;
		    statistics[i].RxMCastFrames     += stats_regs_copy.RXMULTICASTFRAMES;
		    statistics[i].RxPauseFrames     += stats_regs_copy.RXPAUSEFRAMES    ;
		    statistics[i].RxCRCErrors       += stats_regs_copy.RXCRCERRORS      ;
		    statistics[i].RxAlignCodeErrors += stats_regs_copy.RXALIGNCODEERRORS;
		    statistics[i].RxOversized       += stats_regs_copy.RXOVERSIZEDFRAMES;
		    statistics[i].RxJabber          += stats_regs_copy.RXJABBERFRAMES   ;
		    statistics[i].RxUndersized      += stats_regs_copy.RXUNDERSIZEDFRAMES;
		    statistics[i].RxFragments       += stats_regs_copy.RXFRAGMENTS      ;
		    statistics[i].RxOctets          += stats_regs_copy.RXOCTETS         ;
		    statistics[i].TxGoodFrames      += stats_regs_copy.TXGOODFRAMES     ;
		    statistics[i].TxBCastFrames     += stats_regs_copy.TXBROADCASTFRAMES;
		    statistics[i].TxMCastFrames     += stats_regs_copy.TXMULTICASTFRAMES;
		    statistics[i].TxPauseFrames     += stats_regs_copy.TXPAUSEFRAMES    ;
		    statistics[i].TxDeferred        += stats_regs_copy.TXDEFERREDFRAMES ;
		    statistics[i].TxCollision       += stats_regs_copy.TXCOLLISIONFRAMES;
		    statistics[i].TxSingleColl      += stats_regs_copy.TXSINGLECOLLFRAMES;
		    statistics[i].TxMultiColl       += stats_regs_copy.TXMULTCOLLFRAMES ;
		    statistics[i].TxExcessiveColl   += stats_regs_copy.TXEXCESSIVECOLLISIONS;
		    statistics[i].TxLateColl        += stats_regs_copy.TXLATECOLLISIONS ;
		    statistics[i].TxCarrierSLoss    += stats_regs_copy.TXCARRIERSENSEERRORS;
		    statistics[i].TxOctets          += stats_regs_copy.TXOCTETS         ;
		    statistics[i].Frame64           += stats_regs_copy.OCTETFRAMES64    ;
		    statistics[i].Frame65t127       += stats_regs_copy.OCTETFRAMES65T127;
		    statistics[i].Frame128t255      += stats_regs_copy.OCTETFRAMES128T255;
		    statistics[i].Frame256t511      += stats_regs_copy.OCTETFRAMES256T511;
		    statistics[i].Frame512t1023     += stats_regs_copy.OCTETFRAMES512T1023;
		    statistics[i].Frame1024tUp      += stats_regs_copy.OCTETFRAMES1024TUP;
		    statistics[i].NetOctets         += stats_regs_copy.NETOCTETS        ;
		}
	}	
}


/*MAC address initialization for flow control*/
void K2_GE_Flow_Control_MAC_Address_Init(
	Ethernet_Port_Config * ethernet_port_cfg[])
{
	int i;

	for(i=0; i< GE_NUM_ETHERNET_PORT; i++)
	{
		if(ethernet_port_cfg[i])	
		{
			if(ethernet_port_cfg[i]->RX_flow_control_enable)
			{
				gpCPSW_regs->ENETPORT[i].PN_SA_L_REG=
					_hill(ethernet_port_cfg[i]->flow_control_MAC_Address);
				gpCPSW_regs->ENETPORT[i].PN_SA_H_REG=
					_loll(ethernet_port_cfg[i]->flow_control_MAC_Address);
			}
		}
	}
}

/*initialize the CPPI Info Word 0 SRC_ID field for received packets*/
void K2_GE_CPPI_Src_ID_Init(
	Ethernet_Port_Config * ethernet_port_cfg[])
{
	int i;
	Uint32 uiReg_value[2]= {0, 0};

	for(i=0; i< GE_NUM_ETHERNET_PORT; i++)
	{
		if(ethernet_port_cfg[i])
			uiReg_value[i/4] |= (ethernet_port_cfg[i]->CPPI_Src_ID<<((i&3)*8));
	}

	gpCPSW_regs->P0_SRC_ID_A_REG= uiReg_value[0];
#if (GE_NUM_ETHERNET_PORT>4)
	gpCPSW_regs->P0_SRC_ID_B_REG= uiReg_value[1];
#endif	
}
void marvell_88e151x_init_phy(int phy_addr)
{
	Uint16 uiRegValue;
	
	/* As per Marvell Release Notes - Alaska 88E1510/88E1518/88E1512/88E1514
	   Rev A0, Errata Section 3.1 */
	KeyStone_MDIO_PHY_Set_Reg(phy_addr, 22, 0x00ff);	/* reg page 0xff */
	KeyStone_MDIO_PHY_Set_Reg(phy_addr, 17, 0x214B);
	KeyStone_MDIO_PHY_Set_Reg(phy_addr, 16, 0x2144);
	KeyStone_MDIO_PHY_Set_Reg(phy_addr, 17, 0x0C28);
	KeyStone_MDIO_PHY_Set_Reg(phy_addr, 16, 0x2146);
	KeyStone_MDIO_PHY_Set_Reg(phy_addr, 17, 0xB233);
	KeyStone_MDIO_PHY_Set_Reg(phy_addr, 16, 0x214D);
	KeyStone_MDIO_PHY_Set_Reg(phy_addr, 17, 0xCC0C);
	KeyStone_MDIO_PHY_Set_Reg(phy_addr, 16, 0x2159);
	KeyStone_MDIO_PHY_Set_Reg(phy_addr, 22, 0x0000);	/* reg page 0 */

	KeyStone_MDIO_PHY_Set_Reg(phy_addr, 22, 18);	/* reg page 18 */
	/* Write HWCFG_MODE = SGMII to Copper */
	uiRegValue = KeyStone_MDIO_PHY_Get_Reg(phy_addr, 20);
	uiRegValue &= (~0x7);
	uiRegValue |= 1;
	KeyStone_MDIO_PHY_Set_Reg(phy_addr, 20, uiRegValue);

	/* Phy reset */
	uiRegValue = KeyStone_MDIO_PHY_Get_Reg(phy_addr, 20);
	uiRegValue |= (1<<15);
	KeyStone_MDIO_PHY_Set_Reg(phy_addr, 20, uiRegValue);

	KeyStone_MDIO_PHY_Set_Reg(phy_addr, 22, 0);	/* reg page 0 */

	/*KeyStone EMAC does not support 1000M half-duplex.
	Configure the PHY register 9_0.8 to 0, which means 1000Mbps half duplex will 
	not be advertised to the remote side which connects to the copper media of 
	this PHY. This step will make sure that the copper side of PHY will not 
	negotiate 1000Mbps half duplex capability with remote media.*/
	uiRegValue = KeyStone_MDIO_PHY_Get_Reg(phy_addr, 9);
	uiRegValue &= ~(1<<8);
	KeyStone_MDIO_PHY_Set_Reg(phy_addr, 9, uiRegValue);

	KeyStone_MDIO_PHY_Set_Reg(phy_addr, 22, 2);	/* reg page 2 */

	/*Configure the PHY register 21_2 "default MAC interface speed" to 01, which 
	means 100Mbps. This step will make sure when the Fiber/SGMII side of PHY status 
	switch between link up and link down, it will not issue 1000M half duplex request 
	to CPSGMII module of the device. (Note, this configuration will NEVER force the 
	link rate to 100Mbps, the link rate depends on the final negotiation result) */
	uiRegValue = KeyStone_MDIO_PHY_Get_Reg(phy_addr, 21);
	uiRegValue |= (1<<13);
	uiRegValue &= ~(1<<6);
	KeyStone_MDIO_PHY_Set_Reg(phy_addr, 21, uiRegValue);

	KeyStone_MDIO_PHY_Set_Reg(phy_addr, 22, 0);	/* reg page 0 */
	
	/* software reset to make configuration take effect*/
	uiRegValue = KeyStone_MDIO_PHY_Get_Reg(phy_addr, 0);
	uiRegValue |= (1<<15);
	KeyStone_MDIO_PHY_Set_Reg(phy_addr, 0, uiRegValue);

}
#endif

/*initialize ethernet ports*/
void K2_Ethernet_Ports_Init(K2_GE_Config * ge_cfg)
{
	int i;
	Bool bMAC_loopback=FALSE, bFullDuplex=TRUE;
	Uint32 speed_mode;
	
	for(i=0; i<GE_NUM_ETHERNET_PORT; i++)
	{
		if(ge_cfg->ethernet_port_cfg[i])
		{
#if 1
		    /* Reset the port before configuring it */
			gpSGMII_regs[i]->SOFT_RESET_REG = CSL_CPSGMII_SOFT_RESET_REG_SOFT_RESET_MASK;

		    /* Reset MAC port*/            
		    CSL_CPGMAC_SL_resetMac (i);

			_mfence(); 		//make sure the register access complete
			_mfence();
			//TSC_delay_ms(10);

			//wait for EMAC reset complete
		    while (CSL_CPGMAC_SL_isMACResetDone (i) != TRUE);
#endif
#if defined(DEVICE_K2H) || defined(DEVICE_K2K)
		    /*Receive FIFO Maximum Blocks: 3~15.
		    TX_MAX_BLKS= 20 - RX_MAX_BLKS*/
			gpCPSW_port_regs[i]->P_MAX_BLKS_REG=
				(ge_cfg->ethernet_port_cfg[i]->RX_FIFO_Max_blocks
				 <<CSL_CPSW_5GF_P_MAX_BLKS_REG_RX_MAX_BLKS_SHIFT)
				|((20-ge_cfg->ethernet_port_cfg[i]->RX_FIFO_Max_blocks)
				 <<CSL_CPSW_5GF_P_MAX_BLKS_REG_TX_MAX_BLKS_SHIFT);

			gpMAC_regs[i]->RX_MAXLEN_REG= ge_cfg->RX_MAX_length;
#else 	//K2E or K2L
			gpCPSW_regs->ENETPORT[i].PN_RX_MAXLEN_REG= ge_cfg->RX_MAX_length;
			_mfence(); 		//make sure the register access complete
			_mfence();
			/*printf("gpCPSW_regs->ENETPORT[i].PN_RX_MAXLEN_REG= 0x%x vs 0x%x\n", 
				gpCPSW_regs->ENETPORT[i].PN_RX_MAXLEN_REG,
				ge_cfg->RX_MAX_length);*/
#endif
		
			speed_mode= 2; //1000M by default
#if 0
			if((ETHERNET_10M_HALFDUPLEX==ge_cfg->ethernet_port_cfg[i]->mode)
				||(ETHERNET_100M_HALFDUPLEX==ge_cfg->ethernet_port_cfg[i]->mode))
				bFullDuplex=FALSE;
#endif		 	
			if((ETHERNET_10M_FULLDUPLEX==ge_cfg->ethernet_port_cfg[i]->mode)
				/*||(ETHERNET_10M_HALFDUPLEX==ge_cfg->ethernet_port_cfg[i]->mode)*/)
			{
				speed_mode= 0; //10Mbps
			}
			else if((ETHERNET_100M_FULLDUPLEX==ge_cfg->ethernet_port_cfg[i]->mode)
				/*||(ETHERNET_100M_HALFDUPLEX==ge_cfg->ethernet_port_cfg[i]->mode)*/)
			{
				speed_mode= 1; //100Mbps
			}
			
			if(ETHERNET_MAC_LOOPBACK==ge_cfg->ethernet_port_cfg[i]->loopback_mode)
				bMAC_loopback= TRUE;

#if defined(DEVICE_K2H) || defined(DEVICE_K2K)
			gpMAC_regs[i]->MACCONTROL_REG= ge_cfg->ethernet_port_cfg[i]->prmiscuous_mode
				|((speed_mode>>1)<<CSL_CPGMAC_SL_MACCONTROL_REG_GIG_SHIFT)
				|(ge_cfg->ethernet_port_cfg[i]->TX_flow_control_enable
				  <<CSL_CPGMAC_SL_MACCONTROL_REG_TX_FLOW_EN_SHIFT)
				|(ge_cfg->ethernet_port_cfg[i]->RX_flow_control_enable
				  <<CSL_CPGMAC_SL_MACCONTROL_REG_RX_FLOW_EN_SHIFT)
				|(bMAC_loopback<<CSL_CPGMAC_SL_MACCONTROL_REG_LOOPBACK_SHIFT)
				|(bFullDuplex<<CSL_CPGMAC_SL_MACCONTROL_REG_FULLDUPLEX_SHIFT)
				|(1<<CSL_CPGMAC_SL_MACCONTROL_REG_CTL_EN_SHIFT)
				|(1<<CSL_CPGMAC_SL_MACCONTROL_REG_GMII_EN_SHIFT);
#else 	//K2E or K2L
			gpCPSW_regs->ENETPORT[i].PN_MAC_CONTROL_REG= ge_cfg->ethernet_port_cfg[i]->prmiscuous_mode
				|((speed_mode>>1)<<CSL_XGE_CPSW_PN_MAC_CONTROL_REG_GIG_SHIFT)
				|(ge_cfg->ethernet_port_cfg[i]->TX_flow_control_enable
				  <<CSL_XGE_CPSW_PN_MAC_CONTROL_REG_TX_FLOW_EN_SHIFT)
				|(ge_cfg->ethernet_port_cfg[i]->RX_flow_control_enable
				  <<CSL_XGE_CPSW_PN_MAC_CONTROL_REG_RX_FLOW_EN_SHIFT)
				|(bMAC_loopback<<CSL_XGE_CPSW_PN_MAC_CONTROL_REG_LOOPBACK_SHIFT)
				|(bFullDuplex<<CSL_XGE_CPSW_PN_MAC_CONTROL_REG_FULLDUPLEX_SHIFT)
				|((!bMAC_loopback)<<CSL_XGE_CPSW_PN_MAC_CONTROL_REG_CTL_EN_SHIFT)
				|(1<<CSL_XGE_CPSW_PN_MAC_CONTROL_REG_GMII_EN_SHIFT);
#endif

			if((ETHERNET_AUTO_NEGOTIAT_SLAVE==ge_cfg->ethernet_port_cfg[i]->mode)
				||(ETHERNET_AUTO_NEGOTIAT_MASTER==ge_cfg->ethernet_port_cfg[i]->mode))
			{
				/*1 Setup the SGMII as slave and enable autonegotiation:                         */
				/*1a Set bit 0 of the MR_ADV_ABILITY register                           */
				gpSGMII_regs[i]->MR_ADV_ABILITY_REG &= 0xFFFF0000; /* Clear the register contents */

				if(ETHERNET_AUTO_NEGOTIAT_SLAVE==ge_cfg->ethernet_port_cfg[i]->mode)
				{
					gpSGMII_regs[i]->MR_ADV_ABILITY_REG |= 0x00000001;

					/*1b Enable autonegotiation by setting the MR_AN_ENABLE bit in the*/
					gpSGMII_regs[i]->CONTROL_REG |= CSL_CPSGMII_CONTROL_REG_MR_AN_ENABLE_MASK;
				}
				else
				{
					gpSGMII_regs[i]->MR_ADV_ABILITY_REG |= 0x00009801;

					/*1b Enable autonegotiation by setting the MR_AN_ENABLE bit in the*/
					gpSGMII_regs[i]->CONTROL_REG |= CSL_CPSGMII_CONTROL_REG_MR_AN_ENABLE_MASK
						|CSL_CPSGMII_CONTROL_REG_MASTER_MASK;
				}

				/*2 Poll the SGMII_STATUS register to determine when autonegotiation is
				complete without error. The AN_ERROR bit in the SGMII_STATUS register
				will be set if the mode was commanded to be half-duplex gigabit.*/
				while(0==(gpSGMII_regs[i]->STATUS_REG&CSL_CPSGMII_STATUS_REG_LOCK_MASK));
#if 0 		//for debug
				while(0==(gpSGMII_regs[i]->STATUS_REG&CSL_CPSGMII_STATUS_REG_MR_AN_COMPLETE_MASK))
				{
					TSC_delay_ms(1); 	//wait for autonegotiation
				}
				while(0==(gpSGMII_regs[i]->STATUS_REG&CSL_CPSGMII_STATUS_REG_LINK_MASK));
#endif

				if(gpSGMII_regs[i]->STATUS_REG&CSL_CPSGMII_STATUS_REG_AN_ERROR_MASK)
					puts("Half dulpex mode was negotiated!");
#if 0 		//for debug
				printf("SGMII configure at %u\n", TSCL);
				while(0==(gpSGMII_regs[i]->STATUS_REG&CSL_CPSGMII_STATUS_REG_AN_ERROR_MASK));
					printf("Half dulpex mode was negotiated at %u!\n", TSCL);
#endif

				/*3 In the MAC module, set the EXT_EN bit in the MAC_CONTROL register to 
				allow the speed and duplex mode to be set by the signals from the SGMII.*/
			}
			else 	//force mode
			{
				/*1 Setup the SGMII in force mode*/
				/*1a Set the MR_ADV_ABILITY register*/
				gpSGMII_regs[i]->MR_ADV_ABILITY_REG &= 0xFFFF0000; /* Clear the register contents */
				_mfence(); 		//make sure the register access complete
				_mfence();
				//TSC_delay_ms(1);
				gpSGMII_regs[i]->MR_ADV_ABILITY_REG |= (0x8001
					|(speed_mode<<10)|(bFullDuplex<<12));
				_mfence(); 		//make sure the register access complete
				_mfence();

				//printf("gpSGMII_regs[i]->MR_ADV_ABILITY_REG= 0x%x\n", gpSGMII_regs[i]->MR_ADV_ABILITY_REG);

				/*1b Set the device in master mode without autonegotiation*/
				gpSGMII_regs[i]->CONTROL_REG |= CSL_CPSGMII_CONTROL_REG_MASTER_MASK;

				/*2 Poll the LINK bit in the SGMII_STATUS register to determine when the link is up.*/
				while(0==(gpSGMII_regs[i]->STATUS_REG&CSL_CPSGMII_STATUS_REG_LOCK_MASK));
				if(ETHERNET_MAC_LOOPBACK!=ge_cfg->ethernet_port_cfg[i]->loopback_mode)
					while(0==(gpSGMII_regs[i]->STATUS_REG&CSL_CPSGMII_STATUS_REG_LINK_MASK));

				/*3 In the MAC module, the user must set the EXT_EN bit in the 
				MAC_CONTROL register to allow the speed and duplex mode to be 
				set by the signals from the SGMII.*/

			}
			
			if(ETHERNET_SGMII_LOOPBACK==ge_cfg->ethernet_port_cfg[i]->loopback_mode)
			{
				/*1 Clear to zero the MR_AN_ENABLE bit in the SGMII_CONTROL*/
				gpSGMII_regs[i]->CONTROL_REG &= ~CSL_CPSGMII_CONTROL_REG_MR_AN_ENABLE_MASK;
				
				/*2 Write to one the RT_SOFT_RESET bit in the SOFT_RESET   */
				gpSGMII_regs[i]->SOFT_RESET_REG |= CSL_CPSGMII_SOFT_RESET_REG_RT_SOFT_RESET_MASK;

				/*3 Write to one the LOOPBACK bit in the SGMII_CONTROL     */
				gpSGMII_regs[i]->CONTROL_REG |= CSL_CPSGMII_CONTROL_REG_LOOPBACK_MASK;
				
				/*4 Write to zero the RT_SOFT_RESET bit in the SOFT_RESET  */
				gpSGMII_regs[i]->SOFT_RESET_REG &= (~CSL_CPSGMII_SOFT_RESET_REG_RT_SOFT_RESET_MASK);
			}		 	

		}
	}
}

/*setup MDIO for PHY controlling*/
void KeyStone_MDIO_Init(Ethernet_MDIO_Config *  mdio_cfg)
{
	if(NULL == mdio_cfg)
		return;

	gpMDIO_regs->CONTROL_REG= CSL_MDIO_CONTROL_REG_ENABLE_MASK
		|CSL_MDIO_CONTROL_REG_FAULT_MASK 	/*write 1 to clear this bit*/
		|CSL_MDIO_CONTROL_REG_FAULT_DETECT_ENABLE_MASK
		|(mdio_cfg->clock_div<<CSL_MDIO_CONTROL_REG_CLKDIV_SHIFT);

	//link INT0 setup
	if(mdio_cfg->link_INT0_PHY_select<=MDIO_INT_SELECT_PHY_31)
		gpMDIO_regs->USER_GROUP[0].USER_PHY_SEL_REG= 
			CSL_MDIO_USER_PHY_SEL_REG_LINKINT_ENABLE_MASK
			|(mdio_cfg->link_INT0_PHY_select<<CSL_MDIO_USER_PHY_SEL_REG_PHYADR_MON_SHIFT);

	//link INT1 setup
	if(mdio_cfg->link_INT1_PHY_select<=MDIO_INT_SELECT_PHY_31)
		gpMDIO_regs->USER_GROUP[1].USER_PHY_SEL_REG= 
			CSL_MDIO_USER_PHY_SEL_REG_LINKINT_ENABLE_MASK
			|(mdio_cfg->link_INT1_PHY_select<<CSL_MDIO_USER_PHY_SEL_REG_PHYADR_MON_SHIFT);

	/*The MDIO module powers up in an idle state before it is enabled.
	wait for it is not idle (really enabled)*/
	while(gpMDIO_regs->CONTROL_REG&CSL_MDIO_CONTROL_REG_IDLE_MASK);
	
#if defined(K2L_EVM)||defined(K2E_EVM) 
	marvell_88e151x_init_phy(0);
	marvell_88e151x_init_phy(1);
	puts("Wait for Ethernet PHY ready...");
	TSC_delay_ms(100);
#endif
}

char * SerdesErrorStr[]=
{
	"SERDES no error",
	"SERDES error: invalid input reference clock speed!",
	"SERDES error: invalid link speed!"
};
/*Serdes initialization for Giga bit ethernet*/
void K2_GE_Serdes_init(K2_GE_SerdesConfig * serdes_cfg, Uint32 uiBase_addr)
{
	int i;
	CSL_SERDES_LINK_RATE linkSpeed;
	CSL_SERDES_LANE_CTRL_RATE laneRateScale;
	Uint32 uiResult;

	// Disable pll before configuring the SerDes registers
	CSL_EthernetSerdesShutdown(uiBase_addr);

	if(serdes_cfg->linkSpeed_GHz== 1.25f)
	{
		linkSpeed= CSL_SERDES_LINK_RATE_1p25G;
		laneRateScale = CSL_SERDES_LANE_QUARTER_RATE;
	}
	else
		printf("Error: link speed %.3fGbps is not supported by SGMII\n",serdes_cfg->linkSpeed_GHz);
	
	uiResult= CSL_EthernetSerdesInit(uiBase_addr,
		serdes_cfg->inputRefClock, linkSpeed);
	if(uiResult)
		printf("  %s\n", SerdesErrorStr[uiResult]);

	for(i=0; i<4; i++)
	{
		if((serdes_cfg->laneEnableMask>>i)&1)
		{
			CSL_EthernetSerdesLaneConfig(uiBase_addr,
				serdes_cfg->inputRefClock, linkSpeed, i);
		}
	}

	CSL_EthernetSerdesComEnable(uiBase_addr);

	for(i=0; i<4; i++)
	{
		if((serdes_cfg->laneEnableMask>>i)&1)
		{
			uiResult= CSL_EthernetSerdesLaneEnable(uiBase_addr,
				i, (serdes_cfg->internalLoopbackDisableMask>>i)&1, laneRateScale);
			if(uiResult)
				printf("  lane %d error: invalid rate scale\n", i);
		}
	}

	// Enable pll via the pll_ctrl 0x0014
	CSL_EthernetSerdesPllEnable(uiBase_addr);

	// Wait the SerDes PLL lock
	while(CSL_SERDES_STATUS_PLL_NOT_LOCKED==K2_SerdesPLLGetStatus(uiBase_addr));
#if 0	
	for(i=0; i<4; i++)
	{
//		if(((serdes_cfg->laneEnableMask>>i)&1)&&((serdes_cfg->internalLoopbackDisableMask>>i)&1))
		if(((serdes_cfg->laneEnableMask>>i)&1))
        {
			while(CSL_SERDES_STATUS_PLL_NOT_LOCKED==K2_SerdesLaneGetStatus(uiBase_addr, i));
		}
	}
#else
	TSC_delay_ms(10); 	//wait for SerDes stable
#endif
}

/*Giga bit ethernet switch subsystem reset*/
void K2_GE_soft_reset()
{
	int i;
	
	/*shut down packet DMA transaction*/
#if defined(DEVICE_K2H) || defined(DEVICE_K2K)
	for(i=0; i<GE_NUM_ETHERNET_PORT; i++)
		KeyStone_pktDma_RxCh_teardown(gpNetCP_DMA_RxChCfgRegs, 
			GE_DIRECT_RX_PORT1_CHANNEL+i, PKTDMA_WAIT_FOREVER);
	KeyStone_pktDma_TxCh_teardown(gpNetCP_DMA_TxChCfgRegs, GE_DIRECT_TX_CHANNEL, PKTDMA_WAIT_FOREVER);
#else 	//K2E or K2L
	/*in K2E/K2L, there are 8 packet DMA channels for each Ethernet port*/
	for(i=0; i<GE_NUM_ETHERNET_PORT*8; i++)
		KeyStone_pktDma_RxCh_teardown(gpNetCP_DMA_RxChCfgRegs, 
			GE_DIRECT_RX_PORT1_CHANNEL+i, PKTDMA_WAIT_FOREVER);
	for(i=0; i<8; i++)
		KeyStone_pktDma_TxCh_teardown(gpNetCP_DMA_TxChCfgRegs, 
			GE_DIRECT_TX_CHANNEL+i, PKTDMA_WAIT_FOREVER);
#endif

	for(i=0; i<GE_NUM_ETHERNET_PORT; i++)
	{
	    /* Reset the SGMII port*/
		gpSGMII_regs[i]->SOFT_RESET_REG = CSL_CPSGMII_SOFT_RESET_REG_SOFT_RESET_MASK;

	    /* Reset MAC port*/            
	    CSL_CPGMAC_SL_resetMac (i);
		//wait for EMAC reset complete
	    while (CSL_CPGMAC_SL_isMACResetDone (i) != TRUE);
	}
	
	/*Wait to finish any current DMA transfer.*/
	TSC_delay_ms(50);

	//disable CPSW and Packet DMA (in PA) through PSC
	KeyStone_disable_PSC_module(CSL_PSC_PD_NETCP, CSL_PSC_LPSC_CPGMAC);
	KeyStone_disable_PSC_module(CSL_PSC_PD_NETCP, CSL_PSC_LPSC_PA);
}

/*Giga bit ethernet initialization*/
void K2_GE_Init(K2_GE_Config * ge_cfg)
{
	int i;
	Uint32 laneEnableMask, internalLoopbackDisableMask; 
	K2_GE_SerdesConfig serdes_cfg;

	/*select the SYSCLK0 driven from CORE PLL before power domain switch*/
	gpBootCfgRegs->PASS_PLL_CTL1 &= ~PLLCTL1_PLLSEL_MASK;

	if ((CSL_PSC_getPowerDomainState(CSL_PSC_PD_NETCP) == PSC_PDSTATE_ON) &&
	   		(CSL_PSC_getModuleState (CSL_PSC_LPSC_CPGMAC) == PSC_MODSTATE_ENABLE))
	{
		// Disable pll before reconfiguring
		K2_GE_soft_reset();	//soft reset GE if it is already enabled
	}

	//enable GE switch subsystem power and clock domain
	KeyStone_enable_PSC_module(CSL_PSC_PD_NETCP, CSL_PSC_LPSC_PA);
	KeyStone_enable_PSC_module(CSL_PSC_PD_NETCP, CSL_PSC_LPSC_CPGMAC);

	//NetCP speed= PASS_PLL_REF_CLK_MHZ*PASS_PLL_MULTIPLIER/PASS_PLL_DIVISOR
	K2_PASS_PLL_init(PASS_PLL_REF_CLK_MHZ, PASS_PLL_MULTIPLIER, PASS_PLL_DIVISOR);

	/*configure Serdes, Serdes should be setup before all other initialization                                                          */
	serdes_cfg.inputRefClock= ge_cfg->serDesInputRefClock;
	serdes_cfg.linkSpeed_GHz= 1.25;
	
	laneEnableMask= 0;
	internalLoopbackDisableMask= 0;
	for(i=0; i<MIN(4, GE_NUM_ETHERNET_PORT); i++)
	{
		if(ge_cfg->ethernet_port_cfg[i])
		{
			if(ge_cfg->ethernet_port_cfg[i]->bSerdesLaneEnable)
				laneEnableMask |= (1<<i);

			if((ETHERNET_LOOPBACK_DISABLE==ge_cfg->ethernet_port_cfg[i]->loopback_mode)
				||(ETHERNET_SERDES_EXTERNAL_LOOPBACK==ge_cfg->ethernet_port_cfg[i]->loopback_mode)
				||(ETHERNET_EXTERNAL_LINE_LOOPBACK==ge_cfg->ethernet_port_cfg[i]->loopback_mode))
			{
				internalLoopbackDisableMask |= (1<<i);
			}
		}
	}

#if defined(DEVICE_K2H) || defined(DEVICE_K2K)
	if(laneEnableMask)
	{
		serdes_cfg.laneEnableMask= laneEnableMask;
		serdes_cfg.internalLoopbackDisableMask= internalLoopbackDisableMask;
	 	K2_GE_Serdes_init(&serdes_cfg, CSL_NETCP_SERDES_CFG_REGS);
	}
#elif defined(DEVICE_K2E)
	if(laneEnableMask)
	{
		serdes_cfg.laneEnableMask= laneEnableMask;
		serdes_cfg.internalLoopbackDisableMask= internalLoopbackDisableMask;
	 	K2_GE_Serdes_init(&serdes_cfg, CSL_NETCP_SERDES_0_CFG_REGS);
	}

	laneEnableMask= 0;
	internalLoopbackDisableMask= 0;
	for(i=4; i<GE_NUM_ETHERNET_PORT; i++)
	{
		if(ge_cfg->ethernet_port_cfg[i])
		{
			if(ge_cfg->ethernet_port_cfg[i]->bSerdesLaneEnable)
				laneEnableMask |= (1<<(i-4));

			if((ETHERNET_LOOPBACK_DISABLE==ge_cfg->ethernet_port_cfg[i]->loopback_mode)
				||(ETHERNET_SERDES_EXTERNAL_LOOPBACK==ge_cfg->ethernet_port_cfg[i]->loopback_mode)
				||(ETHERNET_EXTERNAL_LINE_LOOPBACK==ge_cfg->ethernet_port_cfg[i]->loopback_mode))
			{
				internalLoopbackDisableMask |= (1<<(i-4));
			}
		}
	}
	if(laneEnableMask)
	{
		serdes_cfg.laneEnableMask= laneEnableMask;
		serdes_cfg.internalLoopbackDisableMask= internalLoopbackDisableMask;
	 	K2_GE_Serdes_init(&serdes_cfg, CSL_NETCP_SERDES_1_CFG_REGS);
	}
#else 	//K2L
	if(laneEnableMask&3)
	{
		serdes_cfg.laneEnableMask= laneEnableMask&3;
		serdes_cfg.internalLoopbackDisableMask= internalLoopbackDisableMask&3;
	 	K2_GE_Serdes_init(&serdes_cfg, CSL_CSISC2_2_SERDES_CFG_REGS);
	}
	if((laneEnableMask>>2)&3)
	{
		serdes_cfg.laneEnableMask= (laneEnableMask>>2)&3;
		serdes_cfg.internalLoopbackDisableMask= (internalLoopbackDisableMask>>2)&3;
	 	K2_GE_Serdes_init(&serdes_cfg, CSL_CSISC2_3_SERDES_CFG_REGS);
	}
#endif

#if defined(DEVICE_K2H) || defined(DEVICE_K2K)
	/*1. Configure the CPSW_CONTROL register                                     */
	gpCPSW_regs->CPSW_CONTROL_REG= CSL_CPSW_5GF_CPSW_CONTROL_REG_P0_ENABLE_MASK;

	gpCPSW_regs->RX_MAXLEN_REG= ge_cfg->RX_MAX_length;

#else 	//K2E or K2L
	/*1. Configure the CPSW_CONTROL register                                     */
	gpCPSW_regs->CONTROL_REG= CSL_XGE_CPSW_CONTROL_REG_P0_ENABLE_MASK
		|CSL_XGE_CPSW_CONTROL_REG_P0_TX_CRC_REMOVE_MASK;

	gpCPSW_regs->P0_RX_MAXLEN_REG= ge_cfg->RX_MAX_length;
#endif

	/*initialize the CPPI Info Word 0 SRC_ID field for received packets*/
	K2_GE_CPPI_Src_ID_Init(ge_cfg->ethernet_port_cfg);

	/*2. Configure the MAC1_SA and MAC2_SA (MAC3_SA and MAC4_SA if KeyStone II)  */
	/*  source address hi and lo registers                                      */
	K2_GE_Flow_Control_MAC_Address_Init(ge_cfg->ethernet_port_cfg);

	/*3. Enable the desired statistics ports by programming the CPSW_STAT_PORT_EN*/
	K2_GE_Statistics_Init(ge_cfg->ethernet_port_cfg);

	/*4. Configure the ALE                                                       */
	K2_GE_ALE_Init(ge_cfg->ale_cfg);
	
	/* Configure the MDIO and external PHY (if used)                           */
	KeyStone_MDIO_Init(ge_cfg->mdio_cfg);

	/* Configure the MAC and SGMII modules                                   */
	K2_Ethernet_Ports_Init(ge_cfg);
}

/*fill EMAC header to a packet buffer*/
void Fill_EMAC_header(Uint8 *buffer, Ethernet_Packet_Type type,
	unsigned long long sourceMAC, unsigned long long destMAC)
{
#if 1
	/*destination address*/
	buffer[0]= (_hill(destMAC)>>8)&0xFF;
	buffer[1]= (_hill(destMAC)>>0)&0xFF;
	buffer[2]= (_loll(destMAC)>>24)&0xFF;
	buffer[3]= (_loll(destMAC)>>16)&0xFF;
	buffer[4]= (_loll(destMAC)>>8)&0xFF;
	buffer[5]= (_loll(destMAC)>>0)&0xFF;

	/*source address*/
	buffer[6]= (_hill(sourceMAC)>>8)&0xFF;
	buffer[7]= (_hill(sourceMAC)>>0)&0xFF;
	buffer[8]= (_loll(sourceMAC)>>24)&0xFF;
	buffer[9]= (_loll(sourceMAC)>>16)&0xFF;
	buffer[10]= (_loll(sourceMAC)>>8)&0xFF;
	buffer[11]= (_loll(sourceMAC)>>0)&0xFF;
#else
	/*destination address*/
	buffer[5]= (_hill(destMAC)>>8)&0xFF;
	buffer[4]= (_hill(destMAC)>>0)&0xFF;
	buffer[3]= (_loll(destMAC)>>24)&0xFF;
	buffer[2]= (_loll(destMAC)>>16)&0xFF;
	buffer[1]= (_loll(destMAC)>>8)&0xFF;
	buffer[0]= (_loll(destMAC)>>0)&0xFF;

	/*source address*/
	buffer[11]= (_hill(sourceMAC)>>8)&0xFF;
	buffer[10]= (_hill(sourceMAC)>>0)&0xFF;
	buffer[9]= (_loll(sourceMAC)>>24)&0xFF;
	buffer[8]= (_loll(sourceMAC)>>16)&0xFF;
	buffer[7]= (_loll(sourceMAC)>>8)&0xFF;
	buffer[6]= (_loll(sourceMAC)>>0)&0xFF;

#endif

#if 1
	/*packet type*/
	buffer[12]= (type>>8)&0xFF;
	buffer[13]= type&0xFF;
#else
	/*packet type*/
	buffer[12]= type&0xFF;
	buffer[13]= (type>>8)&0xFF;
#endif
}

/*get EMAC header information in a packet buffer*/
void Get_EMAC_header(Uint8 *buffer, Ethernet_Packet_Type * type,
	unsigned long long * sourceMAC, unsigned long long * destMAC)
{
 	*destMAC= _itoll((buffer[0]<<8)|buffer[1], 
 		(buffer[2]<<24)|(buffer[3]<<16)|(buffer[4]<<8)|buffer[5]);

 	*sourceMAC= _itoll((buffer[6]<<8)|buffer[7], 
 		(buffer[8]<<24)|(buffer[9]<<16)|(buffer[10]<<8)|buffer[11]);

 	*type= (Ethernet_Packet_Type)((buffer[12]<<8)|buffer[13]);
}

/*set "data" in to a register of a PHY*/
void KeyStone_MDIO_PHY_Set_Reg(Uint32 phyNum, Uint32 regNum, Uint16 data)
{
	/*wait for idle (GO=0)*/
	while(gpMDIO_regs->USER_GROUP[0].USER_ACCESS_REG&CSL_MDIO_USER_ACCESS_REG_GO_MASK);
	
	gpMDIO_regs->USER_GROUP[0].USER_ACCESS_REG= CSL_MDIO_USER_ACCESS_REG_GO_MASK
		|CSL_MDIO_USER_ACCESS_REG_WRITE_MASK
		|(phyNum<<CSL_MDIO_USER_ACCESS_REG_PHYADR_SHIFT)
		|(regNum<<CSL_MDIO_USER_ACCESS_REG_REGADR_SHIFT)
		|(data<<CSL_MDIO_USER_ACCESS_REG_DATA_SHIFT);

	/*wait for write complete (GO=0)*/
	while(gpMDIO_regs->USER_GROUP[0].USER_ACCESS_REG&CSL_MDIO_USER_ACCESS_REG_GO_MASK);
}

/*read data from a register of a PHY*/
Uint16 KeyStone_MDIO_PHY_Get_Reg(Uint32 phyNum, Uint32 regNum)
{
	/*wait for idle (GO=0)*/
	while(gpMDIO_regs->USER_GROUP[0].USER_ACCESS_REG&CSL_MDIO_USER_ACCESS_REG_GO_MASK);
	
	gpMDIO_regs->USER_GROUP[0].USER_ACCESS_REG= CSL_MDIO_USER_ACCESS_REG_GO_MASK
		|(phyNum<<CSL_MDIO_USER_ACCESS_REG_PHYADR_SHIFT)
		|(regNum<<CSL_MDIO_USER_ACCESS_REG_REGADR_SHIFT);

	/*wait for read complete (GO=0)*/
	while(gpMDIO_regs->USER_GROUP[0].USER_ACCESS_REG&CSL_MDIO_USER_ACCESS_REG_GO_MASK);

	/*wait for read acknowledge (ACK=1)*/
	while(0==(gpMDIO_regs->USER_GROUP[0].USER_ACCESS_REG&CSL_MDIO_USER_ACCESS_REG_ACK_MASK));
	
	return (gpMDIO_regs->USER_GROUP[0].USER_ACCESS_REG&CSL_MDIO_USER_ACCESS_REG_DATA_MASK);
}


