// ATMBonding.cpp : Defines the entry point for the console application.
//

//#define VCSimu

#ifdef VCSimu
#include "ATMBonding.h"
char adslup=1;
int ASMTxHandler(unsigned char *pCell, unsigned char intrlv, unsigned char atmport)
{return 1;}

#define IDLE_CELL_HEC    0x52
#define NUM_BYTE 4					// For first 4 bytes of the header
#define FRAMESIZE 12				// For scrambler and descrambler 
#define CELL_HEADER_SIZE 5			// size of header of cell
#define CELL_DATA_SIZE 48			// size of data array in cell
#define CELLSIZE 53
#define HEC 4
#define PATTERN 0x55				// For the remainder of CRC

unsigned char	ucArrayHECTable[256]={
0,7,14,9,28,27,18,21,56,63,54,49,36,35,42,45,
112,119,126,121,108,107,98,101,72,79,70,65,84,83,90,93,
224,231,238,233,252,251,242,245,216,223,214,209,196,195,202,205,
144,151,158,153,140,139,130,133,168,175,166,161,180,179,186,189,
199,192,201,206,219,220,213,210,255,248,241,246,227,228,237,234,
183,176,185,190,171,172,165,162,143,136,129,134,147,148,157,154,
39,32,41,46,59,60,53,50,31,24,17,22,3,4,13,10,
87,80,89,94,75,76,69,66,111,104,97,102,115,116,125,122,
137,142,135,128,149,146,155,156,177,182,191,184,173,170,163,164,
249,254,247,240,229,226,235,236,193,198,207,200,221,218,211,212,
105,110,103,96,117,114,123,124,81,86,95,88,77,74,67,68,
25,30,23,16,5,2,11,12,33,38,47,40,61,58,51,52,
78,73,64,71,82,85,92,91,118,113,120,127,106,109,100,99,
62,57,48,55,34,37,44,43,6,1,8,15,26,29,20,19,
174,169,160,167,178,181,188,187,150,145,152,159,138,141,132,131,
222,217,208,215,194,197,204,203,230,225,232,239,250,253,244,243
};	// Hec table
void GenerateHEC(unsigned char *pSynTable,unsigned char* ucArrayData)
{
	unsigned char	ucHecAccum = 0;
	int p;
	// calculate CRC-8 remainder over first four bytes of cell header
	// exclusive-or with coset leader & insert into fifth header byte
	for( p = 0; p <  NUM_BYTE; p++)
		ucHecAccum= pSynTable[ucHecAccum ^ ucArrayData[p]];
	
	ucArrayData[HEC] = ucHecAccum ^ PATTERN;
	return;
}
#endif

#ifdef __KERNEL__
#include <linux/config.h>
#include <linux/kernel.h>
#include <linux/string.h>
#include "ATMBonding.h"
#define printf printk
extern char adslup;
#else
#include "stdafx.h"
#include "string.h"
#endif /*__KERNEL__*/



#define BondingCo
#define Bond_R_

#define MaxLinkPerGroup 2//4
#define MaxBondingGroup 4
enum {BGDCpeNotUsed=0,BGDCpeReset,BGDCpeGet1stASM,BGDCpeGetAllASM,BGDCpeStartTxASM};//Bonding Group Data CPE state
enum {BLD_Idle,BLD_Standby,BLD_Showtime};//Bonding Link Data state
enum {BGDCoNotUsed=0,BGDCoReset,BGDCoProposeTxLink,BGDCoGetLinkId,BGDCoSelectTxLink,BGDCoShowtime};//Bonding Group Data CO state
unsigned char gBond_R_Tx[53];
unsigned char gBond_C_Tx[53];

typedef struct _BondingLinkData
{
	char txNumber;
	char rxStatusNE;
	char txStatusNE;
	char rxStatusFE;
	char txStatusFE;
	char mapToLinkStatus;//0~31
	int actTxDelay;
	char rxAsmStatus;
	char utopiaId;
	int rxTimeStamp;
	int rxASMcnt;
//	int rxASMtimeout;
	char state;
	char timeout;
//	unsigned char prevAsm[53];
} BondingLinkData;

typedef struct _BondingGroupData
{
	int groupID;
	char sidFormat;
	char linkNum;
	char asmIdfr;
	char linkMap[MaxLinkPerGroup];

	BondingLinkData bdLkData[MaxLinkPerGroup];
	
//	char txLinkNumMap[MaxLinkPerGroup];//only link'0',link'1',link'2',link'3'
//	int rxASMcnt[MaxLinkPerGroup];
//	int rxASMtimeout[MaxLinkPerGroup];
	int BGDstate;
	
	unsigned char prevAsmCell[53];
} BondingGroupData;
BondingGroupData gbgdCpeData[MaxBondingGroup];
BondingGroupData gbgdCoData[MaxBondingGroup];

void Bond_C_MeRx(void);//forward declaration
void Bond_R_MeRx(void);//forward declaration
void PrintAtmCell(unsigned char* asmCell,char* string)
{
	int i;

	printf("\n%s",string);
	for(i=0;i<53;i++)
	{
		if((i%16)==0) printf("\n");
		if((i%8)==0) printf(" - ");
		printf("%02x ",asmCell[i]);
	}
	return;
}


int AsmGetRxLinkStatus(unsigned char* linkStatus,int linkX)
{
	int idx=linkX/4;
	int idx2=(3-linkX%4)*2;
	return ((linkStatus[idx]>>idx2)&0x3);
}
void AsmSetTxLinkStatus(unsigned char* linkStatus,int linkX,int newStatus)
{
	int idx=linkX/4;
	int idx2=(3-linkX%4)*2;
	int mask=3<<idx2;
	linkStatus[idx]=(newStatus<<idx2)|(linkStatus[idx]&(~mask));
}
int AsmGetRxAsmStatus(unsigned char* linkStatus,int linkX)
{
	int idx=linkX/8;
	int idx2=7-linkX%8;
	return ((linkStatus[idx]>>idx2)&0x3);
}
void AsmSetRxAsmStatus(unsigned char* linkStatus,int linkX,int newStatus)
{
	int idx=linkX/8;
	int idx2=7-linkX%8;
	int mask=1<<idx2;
	linkStatus[idx]=(newStatus<<idx2)|(linkStatus[idx]&(~mask));
}

int AsmCheck(unsigned char* asmCell)
{
	int AsmValid=0;
	if(asmCell[0]==0x00 &&
		asmCell[1]==0x00 &&
		asmCell[2]==0x01 &&
//		asmCell[3]==0x48) //Peter 07-12-12
		asmCell[3]==0x42)
		AsmValid=1;
	if(!(asmCell[5]==0x0 || asmCell[5]==0x1 || asmCell[5]==0xff))
		AsmValid=0;

//skip the older ASM
//	if((asmCell[6]<bdgData.prevAsmCell[6]) && (bdgData.prevAsmCell[6]<250))
//		return;

	return AsmValid;
}
//assume the period is 1ms
void AsmBuild(BondingGroupData* atmEntity,BondingLinkData* bdLkData, unsigned char* atmCellOut)
{
	int ii;
			atmCellOut[0]=0;
			atmCellOut[1]=0;
			//Test VCI=d'20 (01 42)
			atmCellOut[2]=(unsigned char)((20&0xf0)>>4);
			atmCellOut[3]=(unsigned char)(((20&0xf)<<4)|(1<<1));
			atmCellOut[4]=0;
#ifdef VCSimu			
			GenerateHEC(ucArrayHECTable,atmCellOut);
#endif
#if 1
			atmCellOut[6-1]=atmEntity->sidFormat;
			atmCellOut[7-1]=atmEntity->asmIdfr++;
			atmCellOut[8-1]=(bdLkData->mapToLinkStatus&0x1f)|(0<<7);
			atmCellOut[9-1]=atmEntity->linkNum;
			for(ii=0;ii<8;ii++)//Rx
			atmCellOut[10+ii-1]=0;//(atmEntity.asmInfo->rxLinkStatus[0+4*ii]<<6)|(atmEntity.asmInfo->rxLinkStatus[1+4*ii]<<4)|(atmEntity.asmInfo->rxLinkStatus[2+4*ii]<<2)|(atmEntity.asmInfo->rxLinkStatus[3+4*ii]<<0);

			for(ii=0;ii<8;ii++)
			atmCellOut[18+ii-1]=0;//(atmEntity.asmInfo->txLinkStatus[0+4*ii]<<6)|(atmEntity.asmInfo->txLinkStatus[1+4*ii]<<4)|(atmEntity.asmInfo->txLinkStatus[2+4*ii]<<2)|(atmEntity.asmInfo->txLinkStatus[3+4*ii]<<0);

			for(ii=0;ii<atmEntity->linkNum;ii++)//update rx link status
			{
				AsmSetTxLinkStatus(&atmCellOut[9],atmEntity->bdLkData[ii].mapToLinkStatus,atmEntity->bdLkData[ii].rxStatusNE);
				AsmSetTxLinkStatus(&atmCellOut[17],atmEntity->bdLkData[ii].mapToLinkStatus,atmEntity->bdLkData[ii].txStatusNE);
			}
			
			atmCellOut[26-1]=(unsigned char)((atmEntity->groupID&0xff00)>>8);
			atmCellOut[27-1]=(unsigned char)(atmEntity->groupID&0xff);

			for(ii=0;ii<4;ii++)
			{
			atmCellOut[28+ii-1]=((atmEntity->bdLkData[0+8*ii].rxAsmStatus)<<7)|((atmEntity->bdLkData[1+8*ii].rxAsmStatus)<<6);//|((atmEntity->bdLkData[2+8*ii].rxAsmStatus)<<5)|((atmEntity->bdLkData[3+8*ii].rxAsmStatus)<<4)|
								//((atmEntity->bdLkData[4+8*ii].rxAsmStatus)<<3)|((atmEntity->bdLkData[5+8*ii].rxAsmStatus)<<2)|((atmEntity->bdLkData[6+8*ii].rxAsmStatus)<<1)|(atmEntity->bdLkData[7+8*ii].rxAsmStatus);
			atmCellOut[28+ii-1]=~atmCellOut[28+ii-1];	// negative ACK
			}
			//atmCellOut[28-1]=0x3f;
			//atmCellOut[29-1]=0xff;
			//atmCellOut[30-1]=0xff;
			//atmCellOut[31-1]=0xff;

			atmCellOut[32-1]=0;//atmEntity.asmInfo->groupLostCells;
			atmCellOut[33-1]=0;
			atmCellOut[34-1]=0;//timestamp by HW
			atmCellOut[35-1]=0;//timestamp by HW
			atmCellOut[36-1]=0;//timestamp by HW
			atmCellOut[37-1]=0;//timestamp by HW
			atmCellOut[38-1]=(unsigned char)((0>>8)&0xff); //request delay=0
			atmCellOut[39-1]=(unsigned char)(0&0xff);//request delay=0
			atmCellOut[40-1]=(unsigned char)((bdLkData->actTxDelay>>8)&0xff);//use local delay
			atmCellOut[41-1]=(unsigned char)(bdLkData->actTxDelay&0xff);//use local delay
			atmCellOut[42-1]=0;
			atmCellOut[43-1]=0;
			atmCellOut[44-1]=0;
			atmCellOut[45-1]=0;
			atmCellOut[46-1]=0;
			atmCellOut[47-1]=0;
			atmCellOut[48-1]=0x00;
			atmCellOut[49-1]=0x28;
#else//test ASM cell
			for(ii=5;ii<=48;ii++)
				atmCellOut[ii]=(unsigned char)ii;
			atmCellOut[7-1]=atmEntity->asmIdfr++;
			atmCellOut[48-1]=0x00;
			atmCellOut[49-1]=0x28;
#endif
	return;
}


int Bond_R_Alive(int bdGroup)
{
	int i, isAlive=0;
	BondingGroupData* bdgData=&gbgdCpeData[bdGroup];

	for(i=0;i<bdgData->linkNum;i++)
	{
		if(bdgData->bdLkData->rxStatusNE==3 && bdgData->bdLkData->txStatusNE==3)
		{
			isAlive=1;
			break;
		}
	}
	return isAlive;
}

void Bond_R_ResetLinkData(BondingLinkData* bdLkData)
{
	bdLkData->rxStatusNE=1;
	bdLkData->txStatusNE=1;
	bdLkData->rxStatusFE=0;
	bdLkData->txStatusFE=0;
	bdLkData->rxAsmStatus=0;
	bdLkData->utopiaId=0;
	bdLkData->mapToLinkStatus=0;
	bdLkData->rxASMcnt=0;
	bdLkData->state=BLD_Idle;
	bdLkData->timeout=0;
	bdLkData->actTxDelay=0;
//	for(int j=0;j<53;j++)
//		bdLkData->prevAsm[j]=0;
	return;
}

void Bond_R_ResetBdgData(BondingGroupData* bdgData)
{
	int i;
	bdgData->groupID=0xf;//-1;;
	bdgData->sidFormat=0;//00:12-bit SID, 01:8-bit SID
	bdgData->linkNum=2;//0;//total link number
	bdgData->BGDstate=BGDCpeNotUsed;//BGDCpeReset;

	for(i=0;i<MaxLinkPerGroup;i++)
		Bond_R_ResetLinkData(&bdgData->bdLkData[i]);
	
	return;
}
void Bond_R_Send(unsigned char* asmCell)//send to HW
{
	Bond_C_MeRx();//trigger CO ASM Rx function
	return;
}

void Bond_R_UpdateLinkState(BondingLinkData* bdLkData)
{
	char rxStatusNE_cur=bdLkData->rxStatusNE;
	char txStatusNE_cur=bdLkData->txStatusNE;
	
	char rxStatusNE_next=rxStatusNE_cur;
	char txStatusNE_next=txStatusNE_cur;
	char state_next=bdLkData->state;

	if (txStatusNE_next > bdLkData->rxStatusFE)
		txStatusNE_next = bdLkData->rxStatusFE;
	else
		txStatusNE_next = bdLkData->txStatusFE;
	rxStatusNE_next = txStatusNE_next;	// advance rx if rx < tx

/*	// not used
	if(bdLkData->timeout)
	{
		bdLkData->timeout=0;
		txStatusNE_next=1;
		rxStatusNE_next=1;
		state_next=BLD_Idle;
	}
	else
	{
		switch(bdLkData->state)
		{
			case BLD_Idle:
				if(bdLkData->txStatusFE==2 && bdLkData->rxStatusFE==1 && txStatusNE_cur==1 && rxStatusNE_cur==1)
				{
					txStatusNE_next=2;
					rxStatusNE_next=2;
					state_next=BLD_Standby;
				}
				break;
			case BLD_Standby:
				if(bdLkData->txStatusFE==3 && bdLkData->rxStatusFE==2 && txStatusNE_cur==2 && rxStatusNE_cur==2)
				{
					txStatusNE_next=3;
					rxStatusNE_next=3;
					state_next=BLD_Showtime;
				}
				else if(bdLkData->txStatusFE==2 && bdLkData->rxStatusFE==1 && txStatusNE_cur==2 && rxStatusNE_cur==2)
				{
					txStatusNE_next=2;
					rxStatusNE_next=2;
					state_next=BLD_Standby;
				}
				else
				{
					txStatusNE_next=1;
					rxStatusNE_next=1;
					state_next=BLD_Idle;
				}
				break;
			case BLD_Showtime:
				if(bdLkData->txStatusFE==3 && bdLkData->rxStatusFE==3)
				{
					//do nothing
				}
				else if(bdLkData->txStatusFE==2 || bdLkData->rxStatusFE==2)
				{
					txStatusNE_next=2;
					rxStatusNE_next=2;
					state_next=BLD_Standby;
				}
				else
				{
					txStatusNE_next=1;
					rxStatusNE_next=1;
					state_next=BLD_Idle;
				}

				break;
			default: break;
		}
	}
*/
	bdLkData->rxStatusNE=rxStatusNE_next;
	bdLkData->txStatusNE=txStatusNE_next;
	bdLkData->state=state_next;

}

void Bond_R_UpdateTimeout(BondingLinkData* bdLkData)
{
//	bdLkData->timeout=1;
//	Bond_R_UpdateLinkState(bdLkData);
	bdLkData->rxStatusNE=1;
	bdLkData->txStatusNE=1;
	bdLkData->rxAsmStatus=0;
}



void Bond_R_UpdateLinkData(BondingLinkData* bdLkData,unsigned char* asmRx)
{
	BondingLinkData* BdLkDataPtr;
	int j;
	j=asmRx[7]&0xf;
	j%=MaxLinkPerGroup;
	BdLkDataPtr=&bdLkData[j];

	BdLkDataPtr->rxStatusFE=AsmGetRxLinkStatus(&asmRx[9],BdLkDataPtr->mapToLinkStatus);;
	BdLkDataPtr->txStatusFE=AsmGetRxLinkStatus(&asmRx[17],BdLkDataPtr->mapToLinkStatus);
	BdLkDataPtr->rxAsmStatus=1;

	BdLkDataPtr->timeout = 10;	// 10 sec countdown
	
	BdLkDataPtr->utopiaId=asmRx[45]&0xf;
	BdLkDataPtr->rxTimeStamp=(asmRx[41]<<24)|(asmRx[42]<<16)|(asmRx[43]<<8)|(asmRx[44]<<0);
		
	BdLkDataPtr->rxASMcnt++;
//	BdLkDataPtr->rxASMtimeout=0;
//	for(j=0;j<53;j++)
//		bdLkData->prevAsm[j]=asmRx[j];
	
//	Bond_R_UpdateLinkState(BdLkDataPtr);
	return;
}


//void ASMStateMachineCPE(BondingGroupData& bdgData,unsigned char* asmCell)
//void Bond_R_MeRx(BondingGroupData& bdgData,unsigned char* asmCell)
void Bond_R_MeRx()
{
#if 0
	unsigned char* asmCell=gBond_C_Tx;
	int i,j,k;

	//return when non-ASM comes
	if(AsmCheck(asmCell)==0)
	{
		printf("Bond_R_MeRx: Error Asm\n");
		return;
	}

	//if (asmCell[5] == 0xff) asmCell[5] = 0;	// 12-bit SID

	i = asmCell[9];// RX status
	j = asmCell[17];// TX status
	if (i < j) i = j;
	asmCell[9] = i;
	//if (asmCell[27] == 0xff) asmCell[27] = 0x7f;	// error-free ASM
	k = asmCell[45];	// utopiaId

	ASMTx(asmCell, 0, 0+k);	// check atmport/utopiaId??

	return;
#else
	BondingGroupData* bdgData=&gbgdCpeData[0];
	unsigned char* asmCell=gBond_C_Tx;
	int i,j,k;

	//return when non-ASM comes
	if(AsmCheck(asmCell)==0)
	{
		printf("Bond_R_MeRx: Error Asm\n");
		return;
	}


	j=-1;
	k=(asmCell[25]<<8)|asmCell[26];//group ID
	for(i=0;i<MaxBondingGroup;i++)
	{
		if(gbgdCpeData[i].BGDstate>=BGDCpeGet1stASM && 
			gbgdCpeData[i].groupID==k)//group id matched
		{
			bdgData=&gbgdCpeData[i];
			j=i;
			break;
		}
	}
	if(j==-1)//new bonding group proposed by DSLAM
	{
		for(i=0;i<MaxBondingGroup;i++)
		{
			if(gbgdCpeData[i].BGDstate==BGDCpeNotUsed)
			{
				bdgData=&gbgdCpeData[i];
				bdgData->BGDstate=BGDCpeReset;
				j=i;
				break;
			}
		}
	}
	if(j==-1)//no space for new bonding group proposed by DSLAM
	{
		printf("no space for new bonding group proposed by DSLAM\n");
		return;
	}
	
	if(asmCell[5]==0xff) 
	{
		//printf("bdgData->BGDstate=BGDCpeReset\n");
//		bdgData->BGDstate=BGDCpeReset;
	}
	
	switch(bdgData->BGDstate)
	{
		case BGDCpeReset:
			Bond_R_ResetBdgData(bdgData);
//			if(asmCell[5]==0xff) 
//				break;//wait for next normal asm
			bdgData->BGDstate=BGDCpeGet1stASM;
		case BGDCpeGet1stASM:

			bdgData->groupID=(asmCell[25]<<8)|asmCell[26];
			bdgData->sidFormat=asmCell[5];//00:12-bit SID, 01:8-bit SID

			if (bdgData->sidFormat == 0)
				*((unsigned int*)(0xb8302100)) |= 0x40000000;	// bit 30
			else
				*((unsigned int*)(0xb8302100)) &= ~0x40000000;	// bit 30

			bdgData->linkNum=asmCell[8];//total link number
			if(bdgData->linkNum>MaxLinkPerGroup)
			{
				printf("ERROR: bdgData->linkNum>MaxLinkPerGroup \n");
				bdgData->linkNum=MaxLinkPerGroup;
			}

			for(i=0;i<bdgData->linkNum;i++)
				Bond_R_ResetLinkData(&bdgData->bdLkData[i]);
/*			
			k=asmCell[7]&0xf;//expect 0,1,2,...,(MaxLinkPerGroup-1)
			if(k>=MaxLinkPerGroup)
			{
				printf("ERROR: txLinkNum>=MaxLinkPerGroup (%d %d)\n",k,MaxLinkPerGroup);
				k%=MaxLinkPerGroup;
			}
*/			
			j=0;
			for(i=0;i<32;i++)
			{
				if(AsmGetRxLinkStatus(&asmCell[17],i)>0)//Tx Link Status
					bdgData->linkMap[j++]=i;
				if(j>=bdgData->linkNum) 
					break;
			}
			for(i=0;i<bdgData->linkNum;i++)
				bdgData->bdLkData[i].mapToLinkStatus=bdgData->linkMap[i];
			
			Bond_R_UpdateLinkData(bdgData->bdLkData,asmCell);

			bdgData->BGDstate=BGDCpeGetAllASM;
			break;
		case BGDCpeGetAllASM:
			/*
			k=asmCell[7]&0xf;
			if(k>=MaxLinkPerGroup)
			{
				printf("ERROR: asmFromLinkId>=MaxLinkPerGroup (%d %d)\n",k,MaxLinkPerGroup);
				k%=MaxLinkPerGroup;
			}*/
			Bond_R_UpdateLinkData(bdgData->bdLkData,asmCell);

			j=0;
			for(i=0;i<bdgData->linkNum;i++)
			{
				if(bdgData->bdLkData[i].rxASMcnt>0)
					j++;
			}
			if(j==bdgData->linkNum)//receive at least one ASM from every link
				bdgData->BGDstate=BGDCpeStartTxASM;
			break;
		case BGDCpeStartTxASM:
			Bond_R_UpdateLinkData(bdgData->bdLkData,asmCell);
			break;
		case BGDCpeNotUsed: break;
		default: break;
	}
	for(i=0;i<53;i++)
		bdgData->prevAsmCell[i]=asmCell[i];
	return;
#endif
}



//void Bond_R_MeTx(BondingGroupData& bdgData,unsigned char* asmCell)
void Bond_R_MeTx(void)
{
	BondingGroupData* bdgData=&gbgdCpeData[0];
	unsigned char* asmCell=gBond_R_Tx;

	int i;
	char tpsPath=0;//0: fast; 1: interleave
	static int link = 0;	// send one ASM cell at a time
	switch(bdgData->BGDstate)
	{
		case BGDCpeStartTxASM:
		case BGDCpeGetAllASM:
		case BGDCpeGet1stASM:
		case BGDCpeReset:
		case BGDCpeNotUsed:
			if (link == 0)
			{
				for(i=0;i<bdgData->linkNum;i++)
				{
					if (bdgData->bdLkData[i].timeout == 10)	// skip update if no new ASM received
						Bond_R_UpdateLinkState(&bdgData->bdLkData[i]);
					if (bdgData->bdLkData[i].timeout > 0)
					{
						bdgData->bdLkData[i].timeout--;
						if (bdgData->bdLkData[i].timeout == 0)
						{
							printf("bonding timeout! link %d, port %d\n", i, bdgData->bdLkData[i].utopiaId);
							if ( (*((unsigned int*)(0xb8302134)) & 0x40) == 0 )
								Bond_R_UpdateTimeout(&bdgData->bdLkData[i]);

							if ((bdgData->bdLkData[0].rxAsmStatus + bdgData->bdLkData[1].rxAsmStatus) == 0)
							{
								*((unsigned int*)(0xb8302128)) = 0x1000;	// reset RX expect SID
								//*((unsigned int*)(0xb830211c)) = 0x1000;	// reset TX current SID
								printf("reset RX SID!!\n");
								adslup = 0;	// for FPGA
							}
						}
					}
				}
			}

//			for(i=0;i<bdgData->linkNum;i++)
			{
				i = link;
				if (++link >= bdgData->linkNum)
					link = 0;

				if ( (bdgData->bdLkData[i].rxAsmStatus)  || (*((unsigned int*)(0xb8302134)) & 0x40) )
				{
					AsmBuild(bdgData,&bdgData->bdLkData[i],asmCell);
#ifdef VCSimu
					Bond_R_Send(asmCell);
					printf("\nBondingMeCpeTx:linkNum=%d\n",i);
					PrintAtmCell(asmCell,"CPE");
#endif
					ASMTx(asmCell, tpsPath, tpsPath+bdgData->bdLkData[i].utopiaId);

				}
			}
			break;
		default: break;
	}
	return;
}

int ActivePortOAM(void)
{
	BondingGroupData* bdgData=&gbgdCpeData[0];
	int i;
	for(i=0;i<bdgData->linkNum;i++)
	{
		if (bdgData->bdLkData[i].rxAsmStatus)
			return bdgData->bdLkData[i].utopiaId;
	}
	return -1;
}

int Bond_C_Alive(int bdGroup)
{
	int i, isAlive=0;
	BondingGroupData* bdgData=&gbgdCoData[bdGroup];

	for(i=0;i<bdgData->linkNum;i++)
	{
		if(bdgData->bdLkData->rxStatusNE==3 && bdgData->bdLkData->txStatusNE==3)
		{
			isAlive=1;
			break;
		}
	}
	return isAlive;
}

void Bond_C_ResetLinkData(BondingLinkData* bdLkData)
{
	bdLkData->rxStatusNE=0;
	bdLkData->txStatusNE=0;
	bdLkData->rxStatusFE=0;
	bdLkData->txStatusFE=0;
	bdLkData->rxAsmStatus=0;
//	bdLkData->utopiaId=0;
	bdLkData->mapToLinkStatus=0;
	bdLkData->rxASMcnt=0;
	bdLkData->state=BLD_Idle;
	bdLkData->timeout=0;
	bdLkData->actTxDelay=0;
//	for(int j=0;j<53;j++)
//		bdLkData->prevAsm[j]=0;
	return;
}

void Bond_C_Send(unsigned char* asmCell)//send to HW
{
	Bond_R_MeRx();//trigger CPE ASM Rx function
	return;
}
void Bond_C_ResetBdgData(BondingGroupData* bdgData)
{
//	bdgData->groupID=-1;;
//	bdgData->sidFormat=0;//00:12-bit SID, 01:8-bit SID
//	bdgData->linkNum=0;//total link number
	bdgData->BGDstate=BGDCoReset;
	return;
}

void Bond_C_UpdateLinkState(BondingLinkData* bdLkData)
{
	char rxStatusNE_cur=bdLkData->rxStatusNE;
	char txStatusNE_cur=bdLkData->txStatusNE;
	
	char rxStatusNE_next=rxStatusNE_cur;
	char txStatusNE_next=txStatusNE_cur;
	char state_next=bdLkData->state;
	
	if(bdLkData->timeout)
	{
		bdLkData->timeout=0;
		txStatusNE_next=2;
		rxStatusNE_next=1;
		state_next=BLD_Idle;
	}
	else
	{
		switch(bdLkData->state)
		{
			case BLD_Idle:
				if(bdLkData->txStatusFE==2 && bdLkData->rxStatusFE==2 && txStatusNE_cur==2 && rxStatusNE_cur==1)
				{
					txStatusNE_next=3;
					rxStatusNE_next=2;
					state_next=BLD_Standby;
				}
				break;
			case BLD_Standby:
				if(bdLkData->txStatusFE==3 && bdLkData->rxStatusFE==3 && txStatusNE_cur==3 && rxStatusNE_cur==2)
				{
					txStatusNE_next=3;
					rxStatusNE_next=3;
					state_next=BLD_Showtime;
				}
				else if(bdLkData->txStatusFE==2 && bdLkData->rxStatusFE==2 && txStatusNE_cur==3 && rxStatusNE_cur==2)
				{
					txStatusNE_next=2;
					rxStatusNE_next=2;
					state_next=BLD_Standby;
				}
				else
				{
					txStatusNE_next=2;
					rxStatusNE_next=1;
					state_next=BLD_Idle;
				}
				break;
			case BLD_Showtime:
				if(bdLkData->txStatusFE==3 && bdLkData->rxStatusFE==3)
				{
					//do nothing
				}
				else if(bdLkData->txStatusFE==2 || bdLkData->rxStatusFE==2)
				{
					txStatusNE_next=3;
					rxStatusNE_next=2;
					state_next=BLD_Standby;
				}
				else
				{
					txStatusNE_next=2;
					rxStatusNE_next=1;
					state_next=BLD_Idle;
				}

				break;
			default: break;
		}
	}
	
	bdLkData->rxStatusNE=rxStatusNE_next;
	bdLkData->txStatusNE=txStatusNE_next;
	bdLkData->state=state_next;

}

void Bond_C_UpdateTimeout(BondingLinkData* bdLkData)
{
	bdLkData->timeout=1;
	Bond_C_UpdateLinkState(bdLkData);
}



void Bond_C_UpdateLinkData(BondingLinkData* bdLkData,unsigned char* asmRx)
{
	BondingLinkData* BdLkDataPtr;
	int j;
	j=asmRx[7]&0xf;
	j%=MaxLinkPerGroup;
	BdLkDataPtr=&bdLkData[j];

	BdLkDataPtr->rxStatusFE=AsmGetRxLinkStatus(&asmRx[9],BdLkDataPtr->mapToLinkStatus);;
	BdLkDataPtr->txStatusFE=AsmGetRxLinkStatus(&asmRx[17],BdLkDataPtr->mapToLinkStatus);
	BdLkDataPtr->rxAsmStatus=1;
	
	BdLkDataPtr->utopiaId=asmRx[45]&0xf;
	BdLkDataPtr->rxTimeStamp=(asmRx[41]<<24)|(asmRx[42]<<16)|(asmRx[43]<<8)|(asmRx[44]<<0);
		
	BdLkDataPtr->rxASMcnt++;
//	BdLkDataPtr->rxASMtimeout=0;
//	for(j=0;j<53;j++)
//		bdLkData->prevAsm[j]=asmRx[j];
	
	Bond_C_UpdateLinkState(BdLkDataPtr);
	return;
}


//void (*Bond_C_Build)(BondingGroupData*, BondingLinkData*, unsigned char*) = &AsmBuild; 
void Bond_C_MeRx(void)
{
	BondingGroupData* bdgData=&gbgdCoData[0];
	unsigned char* asmCell=gBond_R_Tx;
	int i,j,k;

	//return when non-ASM comes
	if(AsmCheck(asmCell)==0)
	{
		printf("Bond_C_MeRx: Error Asm\n");
		return;
	}


	j=-1;
	k=(asmCell[25]<<8)|asmCell[26];//group ID
	for(i=0;i<MaxBondingGroup;i++)
	{
		if(gbgdCoData[i].BGDstate>=BGDCoProposeTxLink && 
			gbgdCoData[i].groupID==k)//group id matched
		{
			bdgData=&gbgdCoData[i];
			j=i;
			break;
		}
	}
	/*
	if(j==-1)//do not accept new bonding group proposed by CPE
	{
		for(i=0;i<MaxBondingGroup;i++)
		{
			if(gbgdCoData[i].BGDstate<BGDCoGet1stASM)//group id matched
			{
				bdgData=&gbgdCoData[i];
				bdgData->BGDstate=BGDCoReset;
				j=i;
				break;
			}
		}
	}*/
	if(j==-1)///do not accept new bonding group proposed by CPE
	{
		printf("do not accept new bonding group proposed by CPE\n");
		return;
	}
	/*
	if(asmCell[5]==0xff) 
	{
		printf("bdgData->BGDstate=BGDCoReset\n");
		bdgData->BGDstate=BGDCoReset;
	}
	*/

//enum {BGDCoNotUsed=0,BGDCoReset,BGDCoGetLinkId,BGDCoSelectTxLink,BGDCoShowtime};
	
	switch(bdgData->BGDstate)
	{
		case BGDCoReset:
			for(i=0;i<bdgData->linkNum;i++)
				bdgData->bdLkData[i].rxASMcnt=0;
			break;
		case BGDCoGetLinkId:

			Bond_C_UpdateLinkData(bdgData->bdLkData,asmCell);
			j=0;
			for(i=0;i<bdgData->linkNum;i++)
			{
				if(bdgData->bdLkData[i].rxASMcnt>0)
					j++;
			}
			if(j==bdgData->linkNum)//receive at least one ASM from every link
				bdgData->BGDstate=BGDCoSelectTxLink;

			break;
		case BGDCoSelectTxLink:
			Bond_C_UpdateLinkData(bdgData->bdLkData,asmCell);
			break;
		case BGDCoNotUsed: break;
		default: break;
	}
	for(i=0;i<53;i++)
		bdgData->prevAsmCell[i]=asmCell[i];
	return;
}



//void Bond_C_MeTx(BondingGroupData& bdgData,unsigned char* asmCell)
void Bond_C_MeTx(void)
{
	BondingGroupData* bdgData=&gbgdCoData[0];
	unsigned char* asmCell=gBond_C_Tx;

	int i;
	int timeElapse=0;
	switch(bdgData->BGDstate)
	{
		case BGDCoReset:
			for(i=0;i<bdgData->linkNum;i++)
			{
				Bond_C_ResetLinkData(&bdgData->bdLkData[i]);
				bdgData->sidFormat=0xff;
				AsmBuild(bdgData,&bdgData->bdLkData[i],asmCell);
#ifdef VCSimu
				Bond_C_Send(asmCell);
#endif
			}
			//PHY retrain
			bdgData->BGDstate=BGDCoProposeTxLink;
			bdgData->sidFormat=0;
			for(i=0;i<bdgData->linkNum;i++)
			{
				bdgData->bdLkData[i].rxStatusNE=1;
				bdgData->bdLkData[i].txStatusNE=2;
			}
			for(i=0;i<bdgData->linkNum;i++)
				bdgData->bdLkData[i].rxASMcnt=0;
			break;
		case BGDCoGetLinkId: 
//			break;//no asm sent
		default:
			for(i=0;i<bdgData->linkNum;i++)
			{
				timeElapse=0;
				if(timeElapse>1)//no ASM from FE in 1 second
				{
					Bond_C_UpdateTimeout(&bdgData->bdLkData[i]);
				}
				else
				{
					AsmBuild(bdgData,&bdgData->bdLkData[i],asmCell);
#ifdef VCSimu
					Bond_C_Send(asmCell);
#endif
					printf("\nBond_C_MeTx:linkNum=%d\n",i);
					PrintAtmCell(asmCell,"Co");
				}
			}
			if(bdgData->BGDstate==BGDCoProposeTxLink)
				bdgData->BGDstate=BGDCoGetLinkId;
			break;
	}
	return;
}




#if defined(__KERNEL__) || defined(VCSimu)
/**************************************************************************
 *   the interface between sar & asm  *
 **************************************************************************/
void ASMTimeOut(void)
{
//	if(adslup)
	{
#ifdef __KERNEL__
//		printf( "enter %s()\n", __FUNCTION__ );
#endif

		Bond_R_MeTx();
	}
	return;
}


int ASMTx(unsigned char *pcell, unsigned char intrlv, unsigned char atmport)
{
	unsigned char asmCell[52];

if ( *((unsigned int*)(0xb8302134)) & 0x100 )
{
#ifdef __KERNEL__
	printf( "enter %s()\n", __FUNCTION__ );
#endif
	PrintAtmCell(pcell,  "ASMTx" );
}
else
{
	if ( *((unsigned int*)(0xb8302134)) & 0x20 )
		printf("ASMTx: RX TX STS: %x %x %x, port %d, regs: %x %x %x\n", pcell[9], pcell[17], pcell[27], atmport,
			*((unsigned int*)(0xb8302104)), *((unsigned int*)(0xb830212c)), *((unsigned int*)(0xb8302130)));
	// Registers:
	// 2104: RX expect SID
	// 211c: TX SID (and force)
	// 2128: RX force SID
	// 212c: RX cell discard
	// 2130: RX SIDs from 2 links
	// 2134: bit[0] to enable one-link SID into ready state; other bits for SW debug use
}

	memcpy( asmCell, pcell, 4 );
	memcpy( &asmCell[4], &pcell[5], 48 );
	return ASMTxHandler( asmCell, intrlv, atmport );	
}

void ASMRxHandler( unsigned char *pcell, unsigned char is_intrlv )
{	
	unsigned char* asmCell=gBond_C_Tx;

#ifdef __KERNEL__
//	printf( "enter %s()\n", __FUNCTION__ );
#endif
	memcpy( asmCell, pcell, 4 );
	memcpy( &asmCell[5], &pcell[4], 48 );
if ( *((unsigned int*)(0xb8302134)) & 0x100 )
	PrintAtmCell(asmCell,  "ASMRxHandler" );	
else if ( *((unsigned int*)(0xb8302134)) & 0x10 )
	printf("ASMRxHandler: RX TX STS: %x %x %x, ID %d, byte678 (%x %x %x)\n", asmCell[9], asmCell[17], asmCell[27], asmCell[45],
		asmCell[5], asmCell[6], asmCell[7]);

	Bond_R_MeRx();

	adslup = 1;	// for FPGA

	return;
}

void ASMInit(void)
{
#ifdef __KERNEL__
	printf( "enter %s()\n", __FUNCTION__ );
#endif
	//init Cpe 
	Bond_R_ResetBdgData(&gbgdCpeData[0]);
	//init Cpe END
	//asm_main();

	gbgdCpeData[0].linkNum=2;//test
	return;
}
/**************************************************************************
 *   end the interface between sar & asm  *
 **************************************************************************/
#endif /*__KERNEL__*/

#ifdef __KERNEL__
int asm_main(void)
#else
int main(int argc, char* argv[])
#endif /*__KERNEL__*/
{
	int jj;
	int t;

	for(t=0;t<20;t++)
	{
		printf("\nt=%d\n",t);
		for(jj=0;jj<53;jj++)
			gBond_C_Tx[jj]=0;

		if(t==0)
		{
			int i;

			//init CO bonding profile
			Bond_R_ResetBdgData(&gbgdCoData[0]);
			gbgdCoData[0].groupID=0;
			gbgdCoData[0].sidFormat=0;
			gbgdCoData[0].linkNum=2;
			gbgdCoData[0].linkMap[0]=0;
			gbgdCoData[0].linkMap[1]=1;
			gbgdCoData[0].linkMap[2]=2;
			gbgdCoData[0].BGDstate=BGDCoProposeTxLink;

			for(i=0;i<gbgdCoData[0].linkNum;i++)
			{
				gbgdCoData[0].bdLkData[i].rxStatusNE=1;
				gbgdCoData[0].bdLkData[i].txStatusNE=2;
				gbgdCoData[0].bdLkData[i].mapToLinkStatus=i;
			}
			for(i=0;i<gbgdCoData[0].linkNum;i++)
				gbgdCoData[0].bdLkData[i].rxASMcnt=0;
			//init CO bonding profile END

			//init Cpe 
				Bond_R_ResetBdgData(&gbgdCpeData[0]);
			//to make CPE send out ASM cell before CO's first ASM arrives
//				gbgdCpeData->BGDstate=BGDCpeGet1stASM;
				gbgdCpeData->linkNum=2;

			//init Cpe END

		}

		if(t==1)
		{
//			Bond_C_MeTx();
		}
		if(t==2)
		{
			Bond_R_MeTx();
			Bond_R_MeTx();
		}
		if(t==3)
		{
//			Bond_C_MeTx();
		}
		if(t==4)
		{
			Bond_R_MeTx();
		}
		if(t==5)
		{
			Bond_C_MeTx();

		}
		if(t==6)
		{
			Bond_R_MeTx();
		}

	}

	return 1;
}