/*
  * Copyright (c) 2016 MediaTek Inc.  All rights reserved.
  *
  * This software is available to you under a choice of one of two
  * licenses.  You may choose to be licensed under the terms of the GNU
  * General Public License (GPL) Version 2, available from the file
  * COPYING in the main directory of this source tree, or the
  * BSD license below:
  *
  *     Redistribution and use in source and binary forms, with or
  *     without modification, are permitted provided that the following
  *     conditions are met:
  *
  *      - Redistributions of source code must retain the above
  *        copyright notice, this list of conditions and the following
  *        disclaimer.
  *
  *      - Redistributions in binary form must reproduce the above
  *        copyright notice, this list of conditions and the following
  *        disclaimer in the documentation and/or other materials
  *        provided with the distribution.
  *
  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
  * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
  * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
  * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
  * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
  * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
  * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
  * SOFTWARE.
  */


#ifdef MAT_SUPPORT

#include "rt_config.h"

static NDIS_STATUS MATProto_PPPoEDis_Init(MAT_STRUCT *pMatStruct);
static NDIS_STATUS MATProto_PPPoEDis_Exit(MAT_STRUCT *pMatStruct);
static PUCHAR MATProto_PPPoEDis_Rx(MAT_STRUCT *pMatStruct, PNDIS_PACKET pSkb, PUCHAR pLayerHdr, PUCHAR pDevMacAdr);
static PUCHAR MATProto_PPPoEDis_Tx(MAT_STRUCT *pMatStruct, PNDIS_PACKET pSkb, PUCHAR pLayerHdr, PUCHAR pDevMacAdr);

static NDIS_STATUS MATProto_PPPoESes_Init(MAT_STRUCT *pMatStruct);
static NDIS_STATUS MATProto_PPPoESes_Exit(MAT_STRUCT *pMatStruct);
static PUCHAR MATProto_PPPoESes_Rx(MAT_STRUCT *pMatStruct, PNDIS_PACKET pSkb, PUCHAR pLayerHdr, PUCHAR pDevMacAdr);
static PUCHAR MATProto_PPPoESes_Tx(MAT_STRUCT *pMatStruct, PNDIS_PACKET pSkb, PUCHAR pLayerHdr, PUCHAR pDevMacAdr);


/*
                  1                 2               3             4
    0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
   +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
   |  VER  | TYPE  |      CODE     |          SESSION_ID           |
   +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
   |            LENGTH             |           payload             ~
   +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+

	VER = 0x1, TYPE =0x1

PPPoE Discovery Stage(Ethernet protocol type = 0x8863):
	PADI:
		DESTINATION_ADDR = 0xffffffff
		CODE = 0x09, SESSION_ID = 0x0000
		LENGTH = payload length

	PADO:
		DESTINATION_ADDR = Unicast Ethernet address of sender
		CODE = 0x07, SESSION_ID = 0x0000
		LENGTH = payload length
		NEcessary TAGS: AC-NAME(0x0102), Sevice-Name(0x0101), and other service names.

		Note: if the PPPoE server cannot serve the PADI it MUST NOT respond with a PADO


	PADR:
		DESTINATION_ADDR = unicast Ethernet address
		CODE = 0x19, SESSION_ID = 0x0000
		LENGTH = payload length
		Necessary TAGS: Service-Name(0x0101)
		Optional TAGS: ....

	PADS:
		If success:
			DESTINATION_ADDR = unicast Ethernet address
			CODE = 0x65, SESSION_ID = unique value for this pppoe session.(16 bits)
			LENGHT - payload length
			Necessary TAGS: Service-Name(0x0101)

		if failed:
			SESSION_ID = 0x0000
			Necessary TAGS: Service-Name-Error(0x0201).

	PADT:
		DESTINATION_ADDR = unicast Ethernet address
		CODE = 0xa7, SESSION_ID = previous assigned 16 bits session ID.
		Necessary TAGS: NO.

PPPoE Session Stage(Ethernet protocol type = 0x8864):
	PPP data:
		DESTINATION_ADDR = unicast Ethernet address
		CODE = 0x00,
	LCP:
		DESTINATION_ADDR = unicast Ethernet address
		CODE = 0x00,

*/

#define PPPOE_CODE_PADI			0x09
#define PPPOE_CODE_PADO			0x07
#define PPPOE_CODE_PADR			0x19
#define PPPOE_CODE_PADS			0x65
#define PPPOE_CODE_PADT			0xa7
#define PPPOE_TAG_ID_HOST_UNIQ	0x0103
#define PPPOE_TAG_ID_AC_COOKIE	0x0104

#define PPPoE_SES_ENTRY_AGEOUT_TIME 3000

/* Data structure used for PPPoE discovery stage */
#define PPPOE_DIS_UID_LEN		6
typedef struct _UidMacMappingEntry
{
	UCHAR isServer;
	UCHAR uIDAddByUs;				 /* If the host-uniq or AC-cookie is add by our driver, set it as 1, else set as 0. */
	UCHAR uIDStr[PPPOE_DIS_UID_LEN]; /* String used for identify who sent this pppoe packet in discovery stage. */
	UCHAR macAddr[MAC_ADDR_LEN];	 /* Mac address associated to this uid string. */
	ULONG lastTime;
	struct _UidMacMappingEntry *pNext;	/*Pointer to next entry in link-list of Uid hash table. */
}UidMacMappingEntry, *PUidMacMappingEntry;

typedef struct _UidMacMappingTable
{
	BOOLEAN valid;
	UidMacMappingEntry *uidHash[MAT_MAX_HASH_ENTRY_SUPPORT];
}UidMacMappingTable;

/* "Host-Uniq <-> Mac Address" Mapping table used for PPPoE Discovery stage */

/* Data struct used for PPPoE session stage */
typedef struct _SesMacMappingEntry
{
	UINT16	sessionID;	/* In network order */
	UCHAR	outMacAddr[MAC_ADDR_LEN];
	UCHAR	inMacAddr[MAC_ADDR_LEN];
	ULONG 	lastTime;
	struct	_SesMacMappingEntry *pNext;
}SesMacMappingEntry, *PSesMacMappingEntry;

typedef struct _SesMacMappingTable
{
	BOOLEAN valid;
	SesMacMappingEntry *sesHash[MAT_MAX_HASH_ENTRY_SUPPORT];
}SesMacMappingTable;

/* Declaration of protocol handler for PPPoE Discovery stage */
struct _MATProtoOps MATProtoPPPoEDisHandle =
{
	.init = MATProto_PPPoEDis_Init,
	.tx = MATProto_PPPoEDis_Tx,
	.rx = MATProto_PPPoEDis_Rx,
	.exit = MATProto_PPPoEDis_Exit,
};

/* Declaration of protocol handler for PPPoE Session stage */
struct _MATProtoOps MATProtoPPPoESesHandle =
{
	.init = MATProto_PPPoESes_Init,
	.tx = MATProto_PPPoESes_Tx,
	.rx = MATProto_PPPoESes_Rx,
	.exit =MATProto_PPPoESes_Exit,
};


NDIS_STATUS dumpSesMacTb(
	IN MAT_STRUCT *pMatCfg,
	IN int hashIdx)
{
	SesMacMappingTable *pSesMacTable;
	SesMacMappingEntry *pHead;
	int startIdx, endIdx;


	pSesMacTable = (SesMacMappingTable *)pMatCfg->MatTableSet.SesMacTable;
	if ((!pSesMacTable) || (!pSesMacTable->valid))
	{
		DBGPRINT(RT_DEBUG_OFF, ("SesMacTable not init yet, so cannot do dump!\n"));
		return FALSE;
	}

	if(hashIdx < 0)
	{	/* dump all. */
		startIdx = 0;
		endIdx = MAT_MAX_HASH_ENTRY_SUPPORT - 1;
	}
	else
	{	/* dump specific hash index. */
		startIdx = endIdx = hashIdx;
	}

	DBGPRINT(RT_DEBUG_OFF, ("%s():\n", __FUNCTION__));
	for (; startIdx<= endIdx; startIdx++)
	{
		pHead = pSesMacTable->sesHash[startIdx];
		while(pHead)
		{
				DBGPRINT(RT_DEBUG_OFF, ("SesMac[%d]:\n", startIdx));
				DBGPRINT(RT_DEBUG_OFF, ("\tsesID=%d,inMac=%02x:%02x:%02x:%02x:%02x:%02x,outMac=%02x:%02x:%02x:%02x:%02x:%02x,lastTime=0x%lx, pNext=%p\n",
								pHead->sessionID, PRINT_MAC(pHead->inMacAddr), PRINT_MAC(pHead->outMacAddr), pHead->lastTime, pHead->pNext));
			pHead = pHead->pNext;
		}
	}
	DBGPRINT(RT_DEBUG_OFF, ("\t----EndOfDump!\n"));

	return TRUE;

}


NDIS_STATUS dumpUidMacTb(MAT_STRUCT *pMatCfg, int hashIdx)
{
	UidMacMappingTable *pUidMacTable;
	UidMacMappingEntry *pHead;
	int i;
	int startIdx, endIdx;


	pUidMacTable = (UidMacMappingTable *)pMatCfg->MatTableSet.UidMacTable;
	if ((!pUidMacTable) || (!pUidMacTable->valid))
	{
		DBGPRINT(RT_DEBUG_OFF, ("UidMacTable not init yet, so cannot do dump!\n"));
		return FALSE;
	}

	if(hashIdx < 0)
	{	/* dump all. */
		startIdx = 0;
		endIdx = MAT_MAX_HASH_ENTRY_SUPPORT-1;
	}
	else
	{	/* dump specific hash index. */
		startIdx = endIdx = hashIdx;
	}

	DBGPRINT(RT_DEBUG_OFF, ("%s():\n", __FUNCTION__));
	for (; startIdx<= endIdx; startIdx++)
	{
		pHead = pUidMacTable->uidHash[startIdx];
		while(pHead)
		{
			DBGPRINT(RT_DEBUG_OFF, ("UidMac[%d]:\n", startIdx));
			DBGPRINT(RT_DEBUG_OFF, ("\tisSrv=%d, uIDAddbyUs=%d, Mac=%02x:%02x:%02x:%02x:%02x:%02x, lastTime=0x%lx, pNext=%p\n",
									pHead->isServer, pHead->uIDAddByUs, PRINT_MAC(pHead->macAddr), pHead->lastTime, pHead->pNext));
			DBGPRINT(RT_DEBUG_OFF, ("\tuIDStr="));
			for(i=0; i< PPPOE_DIS_UID_LEN; i++)
				DBGPRINT(RT_DEBUG_OFF, ("%02x", pHead->uIDStr[i]));
			DBGPRINT(RT_DEBUG_OFF, ("\n"));
			pHead = pHead->pNext;
		}
	}
	DBGPRINT(RT_DEBUG_OFF, ("\t----EndOfDump!\n"));

	return TRUE;
}


static NDIS_STATUS UidMacTable_RemoveAll(
	IN MAT_STRUCT *pMatCfg)
{
	UidMacMappingTable *pUidMacTable;
	UidMacMappingEntry *pEntry;
	INT             i;

	pUidMacTable = (UidMacMappingTable *)pMatCfg->MatTableSet.UidMacTable;

	if(!pUidMacTable)
		return TRUE;

	if (pUidMacTable->valid)
	{
		pUidMacTable->valid = FALSE;
		for (i=0; i<MAT_MAX_HASH_ENTRY_SUPPORT; i++)
		{
			while((pEntry = pUidMacTable->uidHash[i]) != NULL)
			{
				pUidMacTable->uidHash[i] = pEntry->pNext;
				MATDBEntryFree(pMatCfg, (PUCHAR)pEntry);
			}
		}
	}

/*	kfree(pMatCfg->MatTableSet.UidMacTable); */
	os_free_mem(NULL, pMatCfg->MatTableSet.UidMacTable);
	pMatCfg->MatTableSet.UidMacTable = NULL;

	return TRUE;
}


static NDIS_STATUS SesMacTable_RemoveAll(
	IN MAT_STRUCT *pMatCfg)
{
	SesMacMappingTable *pSesMacTable;
	SesMacMappingEntry *pEntry;
	INT             i;

	pSesMacTable = (SesMacMappingTable *)pMatCfg->MatTableSet.SesMacTable;
	if (!pSesMacTable)
		return TRUE;

	if (pSesMacTable->valid)
	{
		pSesMacTable->valid = FALSE;
		for (i=0; i<MAT_MAX_HASH_ENTRY_SUPPORT; i++)
		{
			while((pEntry = pSesMacTable->sesHash[i]) != NULL)
		{
				pSesMacTable->sesHash[i] = pEntry->pNext;
				MATDBEntryFree(pMatCfg, (PUCHAR)pEntry);
			}
		}
	}

/*	kfree(pMatCfg->MatTableSet.SesMacTable); */
	os_free_mem(NULL, pMatCfg->MatTableSet.SesMacTable);
	pMatCfg->MatTableSet.SesMacTable = NULL;

	return TRUE;

}


static PUidMacMappingEntry UidMacTableUpdate(
	IN MAT_STRUCT		*pMatCfg,
	IN PUCHAR			pInMac,
	IN PUCHAR			pOutMac,
	IN PUCHAR			pTagInfo,
	IN UINT16			tagLen,
	IN UINT16			isServer)
{
	UINT 				hashIdx, i=0, uIDAddByUs = 0;
	UidMacMappingTable 	*pUidMacTable;
	UidMacMappingEntry	*pEntry = NULL, *pPrev = NULL, *pNewEntry =NULL;
	UCHAR 				hashVal = 0;
	PUCHAR				pUIDStr= NULL;
	ULONG				now;


	pUidMacTable = (UidMacMappingTable *)pMatCfg->MatTableSet.UidMacTable;
	if ((!pUidMacTable) || (!pUidMacTable->valid))
		return NULL;

	if (pTagInfo && tagLen >0)
	{
		pUIDStr = pTagInfo;
		uIDAddByUs = 0;
		tagLen = (tagLen > PPPOE_DIS_UID_LEN ? PPPOE_DIS_UID_LEN : tagLen);
	}
	else
	{
		/*
			We assume the station just have one role,i.e., just a PPPoE server or just a PPPoE client.
			For a packet send by server, we use the destination MAC as our uIDStr
			For a packet send by client, we use the source MAC as our uIDStr.
		*/
		pUIDStr = isServer ? pOutMac: pInMac;
		tagLen = MAC_ADDR_LEN;
		uIDAddByUs = 1;
	}

	for (i=0; i<tagLen; i++)
		hashVal ^= (pUIDStr[i] & 0xff);
	hashIdx = hashVal % MAT_MAX_HASH_ENTRY_SUPPORT;

	/*First, check if the hashIdx exists */
	if (hashIdx < MAT_MAX_HASH_ENTRY_SUPPORT)
	{
		pEntry = pPrev = pUidMacTable->uidHash[hashIdx];
		while(pEntry)
		{
			NdisGetSystemUpTime(&now);

			/* Find the existed UidMac Mapping entry */
			if (NdisEqualMemory(pUIDStr, pEntry->uIDStr, tagLen) && IS_EQUAL_MAC(pEntry->macAddr, pInMac))
    		{
				/* Update info of this entry */
				pEntry->isServer = isServer;
				pEntry->uIDAddByUs = uIDAddByUs;
				pEntry->lastTime = now;

				return pEntry;
			}
			else
        	{	/* handle the age-out situation */
				if (RTMP_TIME_AFTER(now, (pEntry->lastTime + MAT_TB_ENTRY_AGEOUT_TIME)))
				{
					/* Remove the aged entry from the uidHash */
					if (pEntry == pUidMacTable->uidHash[hashIdx])
					{
						pUidMacTable->uidHash[hashIdx]= pEntry->pNext;
						pPrev = pUidMacTable->uidHash[hashIdx];
	        		}
					else
					{
						pPrev->pNext = pEntry->pNext;
					}

					/*After remove this entry from macHash list and uidHash list, now free it! */
					MATDBEntryFree(pMatCfg, (PUCHAR)pEntry);
					pMatCfg->nodeCount--;
					pEntry = (pPrev == NULL ? NULL: pPrev->pNext);
	        		}
				else
				{
					pPrev = pEntry;
					pEntry = pEntry->pNext;
				}
			}
		}
	}


	/* Allocate a new UidMacMapping entry and insert into the double-hash */
	pNewEntry = (UidMacMappingEntry *)MATDBEntryAlloc(pMatCfg, sizeof(UidMacMappingEntry));
	if (pNewEntry)
	{
		NdisZeroMemory(pNewEntry, sizeof(UidMacMappingEntry));

		pNewEntry->isServer = isServer;
		pNewEntry->uIDAddByUs = uIDAddByUs;
		NdisMoveMemory(pNewEntry->macAddr, pInMac, MAC_ADDR_LEN);
		NdisMoveMemory(pNewEntry->uIDStr, pUIDStr, tagLen);
		pNewEntry->pNext = NULL;
		NdisGetSystemUpTime(&pNewEntry->lastTime);

		/* Update mac-side hash link list */
		if (pUidMacTable->uidHash[hashIdx] == NULL)
		{	/* Hash list is empty, directly assign it. */
			pUidMacTable->uidHash[hashIdx] = pNewEntry;
		}
		else
		{
			/* Ok, we insert the new entry into the root of uidHash[hashIdx] */
			pNewEntry->pNext = pUidMacTable->uidHash[hashIdx];
			pUidMacTable->uidHash[hashIdx] = pNewEntry;
		}
		/*dumpUidMacTb(pMatCfg, hashIdx); //for debug */

		pMatCfg->nodeCount++;

		return pNewEntry;
	}

	return NULL;
}


static PUidMacMappingEntry UidMacTableLookUp(
	IN MAT_STRUCT 		*pMatCfg,
	IN PUCHAR			pTagInfo,
	IN UINT16			tagLen)
{
    UINT 				hashIdx;
	UINT16				len;
	UCHAR				hashValue = 0;
    UidMacMappingEntry	*pEntry = NULL;
	UidMacMappingTable *pUidMacTable;

	pUidMacTable = (UidMacMappingTable *)pMatCfg->MatTableSet.UidMacTable;
	if ((!pUidMacTable) || (!pUidMacTable->valid))
		return NULL;

	/* Use hash to find out the location of that entry and get the Mac address. */
	len = tagLen;
	while(len)
		hashValue ^= pTagInfo[--len];
	hashIdx = hashValue % MAT_MAX_HASH_ENTRY_SUPPORT;

	pEntry = pUidMacTable->uidHash[hashIdx];
	while(pEntry)
	{
		if (NdisEqualMemory(pEntry->uIDStr, pTagInfo, tagLen))
		{
/*			DBGPRINT(RT_DEBUG_TRACE,("%s(): dstMac=%02x:%02x:%02x:%02x:%02x:%02x for mapped dstIP(%d.%d.%d.%d)\n",
					__FUNCTION__, pEntry->macAddr[0],pEntry->macAddr[1],pEntry->macAddr[2],
					pEntry->macAddr[3],pEntry->macAddr[4],pEntry->macAddr[5],
					(ipAddr>>24) & 0xff, (ipAddr>>16) & 0xff, (ipAddr>>8) & 0xff, ipAddr & 0xff));
*/
			/*Update the lastTime to prevent the aging before pDA processed! */
			NdisGetSystemUpTime(&pEntry->lastTime);

			return pEntry;
        }
        else
			pEntry = pEntry->pNext;
	}

	/* We didn't find any matched Mac address. */
	return NULL;

}


static PUCHAR getInMacByOutMacFromSesMacTb(
	IN MAT_STRUCT *pMatCfg,
	IN PUCHAR outMac,
	IN UINT16 sesID)
{
	UINT16 				hashIdx;
	SesMacMappingEntry *pEntry = NULL;
	SesMacMappingTable *pSesMacTable;

	pSesMacTable = (SesMacMappingTable *)pMatCfg->MatTableSet.SesMacTable;

	if (!pSesMacTable->valid)
		return NULL;

	/* Use hash to find out the location of that entry and get the Mac address. */
	hashIdx = sesID % MAT_MAX_HASH_ENTRY_SUPPORT;

	pEntry = pSesMacTable->sesHash[hashIdx];
	while(pEntry)
	{
		if ((pEntry->sessionID == sesID) &&  IS_EQUAL_MAC(pEntry->outMacAddr, outMac))
		{
			DBGPRINT(RT_DEBUG_TRACE, ("%s(): find it! dstMac=%02x:%02x:%02x:%02x:%02x:%02x\n",
				__FUNCTION__, pEntry->inMacAddr[0],pEntry->inMacAddr[1],pEntry->inMacAddr[2],
				pEntry->inMacAddr[3],pEntry->inMacAddr[4],pEntry->inMacAddr[5]));

			/*Update the lastTime to prevent the aging before pDA processed! */
			NdisGetSystemUpTime(&pEntry->lastTime);

			return pEntry->inMacAddr;
		}
		else
		{
			pEntry = pEntry->pNext;
		}
	}

	/* We didn't find any matched Mac address, just return and didn't do any modification */
	return NULL;
}


/* This function used to maintain the pppoe convert table which incoming node
	is a pppoe client and want to connect to use inner pppoe server.
*/
static NDIS_STATUS SesMacTableUpdate(
	IN MAT_STRUCT 	*pMatCfg,
	IN PUCHAR 		inMacAddr,
	IN UINT16 		sesID,
	IN PUCHAR 		outMacAddr)
{
	UINT16 hashIdx;
	SesMacMappingEntry *pEntry, *pPrev, *pNewEntry;
	SesMacMappingTable *pSesMacTable;
	ULONG	now;

	pSesMacTable = (SesMacMappingTable *)pMatCfg->MatTableSet.SesMacTable;
	if ((!pSesMacTable) || (!pSesMacTable->valid))
		return FALSE;

	hashIdx = sesID % MAT_MAX_HASH_ENTRY_SUPPORT;

/*
	DBGPRINT(RT_DEBUG_TRACE,("%s():sesID=0x%04x,inMac=%02x%02x:%02x:%02x:%02x:%02x,
			outMac=%02x:%02x:%02x:%02x:%02x:%02x\n", __FUNCTION__, sesID,
			inMacAddr[0],inMacAddr[1],inMacAddr[2],inMacAddr[3],inMacAddr[4],inMacAddr[5],
			outMacAddr[0],outMacAddr[1],outMacAddr[2],outMacAddr[3],outMacAddr[4],outMacAddr[5]));
*/

	pEntry = pPrev = pSesMacTable->sesHash[hashIdx];
	while(pEntry)
	{
		NdisGetSystemUpTime(&now);

		/* Find a existed IP-MAC Mapping entry */
		if ((sesID == pEntry->sessionID) &&
			 IS_EQUAL_MAC(pEntry->inMacAddr, inMacAddr) &&
			 IS_EQUAL_MAC(pEntry->outMacAddr, outMacAddr))
    	{
			/* compare is useless. So we directly copy it into the entry. */
			pEntry->lastTime = now;

	        return TRUE;
		}
        else
        {	/* handle the age-out situation */
			if (RTMP_TIME_AFTER(now, (pEntry->lastTime + PPPoE_SES_ENTRY_AGEOUT_TIME)))
        	{
        		/* Remove the aged entry */
				if (pEntry == pSesMacTable->sesHash[hashIdx])
				{
					pSesMacTable->sesHash[hashIdx]= pEntry->pNext;
					pPrev = pSesMacTable->sesHash[hashIdx];
        		}
				else
				{
	        		pPrev->pNext = pEntry->pNext;
				}
				MATDBEntryFree(pMatCfg, (PUCHAR)pEntry);
				pMatCfg->nodeCount--;
				pEntry = (pPrev == NULL ? NULL: pPrev->pNext);
        	}
			else
			{
				pPrev = pEntry;
	            pEntry = pEntry->pNext;
        	}
        }
	}


	/* Allocate a new IPMacMapping entry and insert into the hash */
	pNewEntry = (SesMacMappingEntry *)MATDBEntryAlloc(pMatCfg, sizeof(SesMacMappingEntry));
	if (pNewEntry != NULL)
	{
		pNewEntry->sessionID= sesID;
		NdisMoveMemory(pNewEntry->inMacAddr, inMacAddr, MAC_ADDR_LEN);
		NdisMoveMemory(pNewEntry->outMacAddr, outMacAddr, MAC_ADDR_LEN);
		pNewEntry->pNext = NULL;
		NdisGetSystemUpTime(&pNewEntry->lastTime);

		if (pSesMacTable->sesHash[hashIdx] == NULL)
		{	/* Hash list is empty, directly assign it. */
			pSesMacTable->sesHash[hashIdx] = pNewEntry;
		}
		else
		{
			/* Ok, we insert the new entry into the root of hash[hashIdx] */
			pNewEntry->pNext = pSesMacTable->sesHash[hashIdx];
			pSesMacTable->sesHash[hashIdx] = pNewEntry;
		}

		/*dumpSesMacTb(pMatCfg, hashIdx); */
		pMatCfg->nodeCount++;

		return TRUE;
	}

	return FALSE;
}


/* PPPoE discovery stage Rx handler.
	When Rx, check if the PPPoE tag "Host-uniq" exists or not.
	If exists, we check our database and convert the dstMac to correct one.
 */
static PUCHAR MATProto_PPPoEDis_Rx(
	IN MAT_STRUCT 		*pMatCfg,
	IN PNDIS_PACKET		pSkb,
	IN PUCHAR			pLayerHdr,
	IN PUCHAR			pDevMacAdr)
{
	PUCHAR pData, pSrvMac = NULL, pCliMac= NULL, pOutMac=NULL, pInMac = NULL, pTagContent = NULL, pPayloadLen;
	UINT16 payloadLen, leftLen;
	UINT16 tagID, tagLen =0;
	UINT16 needUpdateSesTb= 0, sesID=0, isPADT = 0;
	UINT16 findTag=0;
	PUidMacMappingEntry pEntry = NULL;

	pData = pLayerHdr;
	if (*(pData) != 0x11)
		return NULL;

	/* Check the Code type. */
	pData++;
	switch(*pData)
	{
		case PPPOE_CODE_PADO:
			/*It's a packet send by a PPPoE server which behind of our device. */
			findTag = PPPOE_TAG_ID_HOST_UNIQ;
			break;
		case PPPOE_CODE_PADS:
			needUpdateSesTb = 1;
			findTag = PPPOE_TAG_ID_HOST_UNIQ;
			pCliMac = (PUCHAR)(GET_OS_PKT_DATAPTR(pSkb));
			pSrvMac = (PUCHAR)(GET_OS_PKT_DATAPTR(pSkb) + 6);
			break;
		case PPPOE_CODE_PADR:
			/*It's a packet send by a PPPoE client which in front of our device. */
			findTag = PPPOE_TAG_ID_AC_COOKIE;
			break;
		case PPPOE_CODE_PADI:
			/*Do nothing! Just forward this packet to upper layer directly. */
			return NULL;
		case PPPOE_CODE_PADT:
			isPADT = 1;
			pOutMac= (PUCHAR)(GET_OS_PKT_DATAPTR(pSkb) + 6);
			break;
		default:
		return NULL;
	}

	/* Ignore the Code field(length=1) */
	pData ++;
	if (needUpdateSesTb || isPADT)
		sesID = OS_NTOHS(get_unaligned((PUINT16)(pData)));

	if (isPADT)
	{
		pInMac = getInMacByOutMacFromSesMacTb(pMatCfg, pOutMac, sesID);
		return pInMac;
	}
	/* Ignore the session ID field.(length = 2) */
	pData += 2;

	/* Get the payload length, ignore the payload length field.(length = 2) */
	payloadLen = OS_NTOHS(get_unaligned((PUINT16)(pData)));
	pPayloadLen = pData;
	pData += 2;


	/* First parsing the PPPoE paylod to find out the required tag(e.g., x0103 or 0x0104) */
	leftLen = payloadLen;
	while (leftLen)
	{
		tagID = OS_NTOHS(get_unaligned((PUINT16)(pData)));
		tagLen = OS_NTOHS(get_unaligned((PUINT16)(pData+2)));

		if (tagID== findTag && tagLen>0)
		{

			/*shift to the tag value field. */
			pTagContent = pData + 4;
			tagLen = tagLen > PPPOE_DIS_UID_LEN ? PPPOE_DIS_UID_LEN : tagLen;
			break;
		}
		else
		{
			pData += (tagLen + 4);
			leftLen -= (tagLen + 4);
		}
	}


	/* Now update our pppoe discovery table "UidMacTable" */
	if (pTagContent)
	{
		pEntry  = UidMacTableLookUp(pMatCfg, pTagContent, tagLen);

		/* Remove the AC-Cookie or host-uniq if we ever add the field for this session. */
		if (pEntry)
		{
			if (pEntry->uIDAddByUs)
			{
				PUCHAR tagHead, nextTagHead;
				UINT removedTagLen, tailLen;

				removedTagLen = 4 + tagLen;  	/*The total length tag ID/info we want to remove. */
				tagHead = pTagContent - 4;	/*The start address of the tag we want to remove in sk bufffer */
				tailLen = GET_OS_PKT_LEN(pSkb) - (pTagContent - (PUCHAR)(GET_OS_PKT_DATAPTR(pSkb))) - removedTagLen; /*Total left bytes we want to move. */
				if (tailLen)
				{
					nextTagHead = pTagContent + tagLen;	/*The start address of next tag ID/info in sk buffer. */
					memmove(tagHead, nextTagHead, tailLen);
				}
/*				SET_OS_PKT_DATATAIL(pSkb, GET_OS_PKT_DATATAIL(pSkb), (-removedTagLen)); */
/*				GET_OS_PKT_LEN(pSkb) -= removedTagLen; */
				OS_PKT_TAIL_ADJUST(pSkb, removedTagLen);

				*((UINT16 *)pPayloadLen) = OS_HTONS(payloadLen - removedTagLen);
			}

			if (needUpdateSesTb) {

				SesMacTableUpdate(pMatCfg, pEntry->macAddr,sesID, pSrvMac);
			}

			return pEntry->macAddr;
		}
	}

	return NULL;
}



/* PPPoE discovery stage Tx handler.
	If the pakcet is PADI/PADR, check if the PPPoE tag "Host-uniq" exists or not.
		If exists, we just record it in our table, else we insert the Mac address
		 of Sender as well as the host-uniq, then forward to the destination. It's
		 a one(MAC)-to-one(Host-uniq) mapping in our table.
	If the packet is PADO/PADS, check if the PPPoE tag "AC-Cookie" exists or not.
		If exists, we just record it in our table, else we insert the Mac address
		of Sender as well as the AC-Cookie, then forward to the destination. It may
		one(MAC)-to-many(AC-Cookie) mapping in our table.

    Host-uniq TAG ID= 0x0103
    AC-Cookie TAG ID= 0x0104
 */
static PUCHAR MATProto_PPPoEDis_Tx(
	IN MAT_STRUCT 		*pMatStruct,
	IN PNDIS_PACKET 		pSkb,
	IN PUCHAR 			pLayerHdr,
	IN PUCHAR			pDevMacAdr)
{
	PUCHAR pData, pTagContent = NULL, pPayloadLen, pPPPPoETail;
	PUCHAR pSrcMac, pDstMac;
	UINT16 payloadLen, leftLen, offset;
	UINT16 tagID, tagLen =0;
	UINT16 isServer = 0, needUpdateSesTb= 0, sesID = 0;
	UINT16 findTag=0;
	PUidMacMappingEntry pEntry = NULL;
	PUCHAR pPktHdr;
	PNDIS_PACKET pModSkb = NULL;

	pPktHdr = GET_OS_PKT_DATAPTR(pSkb);
	pDstMac = pPktHdr;
	pSrcMac = (pPktHdr + 6);
	pData = pLayerHdr;


	/* Check the pppoe version and Type. It should be 0x11 */
	if (*(pData) != 0x11)
		return NULL;

	/* Check the Code type. */
	pData++;
	switch(*pData)
	{
		/* Send by pppoe client */
		case PPPOE_CODE_PADI:
		case PPPOE_CODE_PADR:
			findTag = PPPOE_TAG_ID_HOST_UNIQ;
			break;
		/* Send by pppoe server */
		case PPPOE_CODE_PADO:
		case PPPOE_CODE_PADS:
			isServer = 1;
			findTag = PPPOE_TAG_ID_AC_COOKIE;
			if (*pData == PPPOE_CODE_PADS)  /* For PADS, we need record the session ID. */
				needUpdateSesTb = 1;
			break;
		/* Both server and client can send this packet */
		case PPPOE_CODE_PADT:
			/* TODO:
				currently we didn't handle PADT packet. We just leave the
				session entry and make it age-out automatically. Maybe we
				can remove the entry when we receive this packet.
			 */
			return NULL;
		default:
			return NULL;
	}

	/*
		Ignore the Code field(length=1) and if it's a PADS packet, we
		should hold the session ID and for latter to update our table.
	*/
	pData ++;
	if (needUpdateSesTb)
		sesID = OS_NTOHS(get_unaligned((PUINT16)(pData)));

	/* Ignore the session ID field.(length = 2) */
	pData += 2;

	/* Get the payload length, and  shift the payload length field(length = 2) to next field. */
	payloadLen = OS_NTOHS(get_unaligned((PUINT16)(pData)));
	pPayloadLen = pData;
	offset = pPayloadLen - (PUCHAR)(GET_OS_PKT_DATAPTR(pSkb));
	pData += 2;


	/* First parsing the PPPoE paylod to find out the required tag(e.g., x0103 or 0x0104) */
	leftLen = payloadLen;
	while (leftLen)
	{
		tagID = OS_NTOHS(get_unaligned((PUINT16)(pData)));
		tagLen = OS_NTOHS(get_unaligned((PUINT16)(pData+2)));

		if (tagID== findTag && tagLen>0)
		{

			/* Move the pointer to the tag value field. 4 = 2(TAG ID) + 2(TAG_LEN) */
			pTagContent = pData + 4;
/*			tagLen = tagLen > PPPOE_DIS_UID_LEN ? PPPOE_DIS_UID_LEN : tagLen; */
			break;
		}
		else
		{
			pData += (tagLen + 4);
			leftLen -= (tagLen + 4);
		}
	}


	/* Now update our pppoe discovery table "UidMacTable" */
	pEntry  = UidMacTableUpdate(pMatStruct, pSrcMac, pDstMac, pTagContent, tagLen, isServer);

	if (pEntry && (pTagContent == NULL))
	{
		PUCHAR tailHead;

		if(OS_PKT_CLONED(pSkb))
		{
/*			pModSkb = (PNDIS_PACKET)skb_copy(RTPKT_TO_OSPKT(pSkb), MEM_ALLOC_FLAG); */
			pModSkb = (PNDIS_PACKET)OS_PKT_COPY(RTPKT_TO_OSPKT(pSkb));
		}
		else
			pModSkb = (PNDIS_PACKET)RTPKT_TO_OSPKT(pSkb);

		if(!pModSkb)
			return NULL;

/*		tailHead = skb_put(RTPKT_TO_OSPKT(pModSkb), (PPPOE_DIS_UID_LEN + 4)); */
		tailHead = OS_PKT_TAIL_BUF_EXTEND(pModSkb, (PPPOE_DIS_UID_LEN + 4));
		if (tailHead)
		{
			pPayloadLen = GET_OS_PKT_DATAPTR(pModSkb) + offset;
			pPPPPoETail = pPayloadLen + payloadLen;
			if(tailHead > pPPPPoETail)
				tailHead = pPPPPoETail;

			if (pEntry->isServer)
			{	/*Append the AC-Cookie tag info in the tail of the pppoe packet. */
				tailHead[0] = 0x01;
				tailHead[1] = 0x04;
				tailHead[2] = 0x00;
				tailHead[3] = PPPOE_DIS_UID_LEN;
				tailHead += 4;
				NdisMoveMemory(tailHead, pEntry->uIDStr, PPPOE_DIS_UID_LEN);
			}
			else
			{	/*Append the host-uniq tag info in the tail of the pppoe packet. */
				tailHead[0] = 0x01;
				tailHead[1] = 0x03;
				tailHead[2] = 0x00;
				tailHead[3] = PPPOE_DIS_UID_LEN;
				tailHead += 4;
				NdisMoveMemory(tailHead, pEntry->uIDStr, PPPOE_DIS_UID_LEN);
			}
			*(UINT16 *)pPayloadLen = OS_HTONS(payloadLen + 4 + PPPOE_DIS_UID_LEN);
		}
	}

	if (needUpdateSesTb)
		SesMacTableUpdate(pMatStruct, pSrcMac, sesID, pDstMac);

	return (PUCHAR)pModSkb;
}


/* PPPoE discovery stage init function */
static NDIS_STATUS MATProto_PPPoEDis_Init(
	IN MAT_STRUCT	*pMatCfg)
{
	UidMacMappingTable *pUidMacTable;
	SesMacMappingTable *pSesMacTable;

	pUidMacTable = (UidMacMappingTable *)pMatCfg->MatTableSet.UidMacTable;
	if (!pUidMacTable)
	{
/*		pMatCfg->MatTableSet.UidMacTable = (VOID *)kmalloc(sizeof(UidMacMappingTable), GFP_KERNEL); */
		os_alloc_mem_suspend(NULL, (UCHAR **)&(pMatCfg->MatTableSet.UidMacTable), sizeof(UidMacMappingTable));
		if (pMatCfg->MatTableSet.UidMacTable)
		{
			pUidMacTable = (UidMacMappingTable *)pMatCfg->MatTableSet.UidMacTable;
			NdisZeroMemory(pUidMacTable, sizeof(UidMacMappingTable));
		}
		else
		{
			DBGPRINT(RT_DEBUG_ERROR, ("MATProto_PPPoEDis_Init(): Allocate memory for UidMacTable failed!\n"));
			return FALSE;
		}
	}
	pUidMacTable->valid = TRUE;

	pSesMacTable = (SesMacMappingTable *)pMatCfg->MatTableSet.SesMacTable;
	if (!pSesMacTable)
	{
/*		pMatCfg->MatTableSet.SesMacTable = (VOID *)kmalloc(sizeof(SesMacMappingTable), GFP_KERNEL); */
		os_alloc_mem_suspend(NULL, (UCHAR **)&(pMatCfg->MatTableSet.SesMacTable), sizeof(SesMacMappingTable));
		if (pMatCfg->MatTableSet.SesMacTable)
		{
			pSesMacTable = (SesMacMappingTable *)pMatCfg->MatTableSet.SesMacTable;
			NdisZeroMemory(pSesMacTable, sizeof(SesMacMappingTable));
		}
		else
		{
			DBGPRINT(RT_DEBUG_ERROR, ("MATProto_PPPoEDis_Init(): Allocate memory for SesMacTable failed!\n"));
			return FALSE;
		}

	}
	pSesMacTable->valid = TRUE;

	return TRUE;
}


/* PPPoE discovery stage exit function */
static NDIS_STATUS MATProto_PPPoEDis_Exit(
	IN MAT_STRUCT *pMatCfg)
{
	UidMacTable_RemoveAll(pMatCfg);
	SesMacTable_RemoveAll(pMatCfg);

	return TRUE;
}


/* PPPoE Session stage Rx handler
	When we receive a ppp pakcet, first check if the srcMac is a PPPoE server or not.
		if it's a server, check the session ID of specific PPPoEServeryEntry and find out the
			correct dstMac Address.
		if it's not a server, check the session ID and find out the cor

 */
static PUCHAR MATProto_PPPoESes_Rx(
	IN MAT_STRUCT 		*pMatCfg,
	IN PNDIS_PACKET		pSkb,
	IN PUCHAR			pLayerHdr,
	IN PUCHAR			pDevMacAdr)
{
	PUCHAR srcMac, dstMac = NULL, pData;
	UINT16 sesID;

	srcMac = (GET_OS_PKT_DATAPTR(pSkb) + 6);
	pData = pLayerHdr;

	/*skip the first two bytes.(version/Type/Code) */
	pData += 2;

	/*get the session ID */
	sesID = OS_NTOHS(get_unaligned((PUINT16)(pData)));

	/* Try to find the dstMac from SesMacHash table. */
	dstMac = getInMacByOutMacFromSesMacTb(pMatCfg, srcMac, sesID);

	return dstMac;
}

/* PPPoE Session stage Tx handler */
static PUCHAR MATProto_PPPoESes_Tx(
	IN MAT_STRUCT 		*pMatStruct,
	IN PNDIS_PACKET 		pSkb,
	IN PUCHAR 			pLayerHdr,
	IN PUCHAR			pDevMacAdr)
{

	/*
		For transmit packet, we do nothing.
	 */
	return NULL;
}


/* PPPoE session stage init function */
static NDIS_STATUS MATProto_PPPoESes_Init(
	IN MAT_STRUCT	*pMatStruct)
{
	return TRUE;
}

/* PPPoE session stage exit function */
static NDIS_STATUS MATProto_PPPoESes_Exit(
	IN MAT_STRUCT	*pMatStruct)
{

	return TRUE;
}

#endif /* MAT_SUPPORT */

