summaryrefslogtreecommitdiff
path: root/cleopatre/devkit/rt5572drv/MODULE/common/cmm_mat_pppoe.c
diff options
context:
space:
mode:
Diffstat (limited to 'cleopatre/devkit/rt5572drv/MODULE/common/cmm_mat_pppoe.c')
-rw-r--r--cleopatre/devkit/rt5572drv/MODULE/common/cmm_mat_pppoe.c1062
1 files changed, 1062 insertions, 0 deletions
diff --git a/cleopatre/devkit/rt5572drv/MODULE/common/cmm_mat_pppoe.c b/cleopatre/devkit/rt5572drv/MODULE/common/cmm_mat_pppoe.c
new file mode 100644
index 0000000000..8988a7dcb6
--- /dev/null
+++ b/cleopatre/devkit/rt5572drv/MODULE/common/cmm_mat_pppoe.c
@@ -0,0 +1,1062 @@
+/*
+ ***************************************************************************
+ * Ralink Tech Inc.
+ * 4F, No. 2 Technology 5th Rd.
+ * Science-based Industrial Park
+ * Hsin-chu, Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * All rights reserved. Ralink's source code is an unpublished work and the
+ * use of a copyright notice does not imply otherwise. This source code
+ * contains confidential trade secret material of Ralink Tech. Any attemp
+ * or participation in deciphering, decoding, reverse engineering or in any
+ * way altering the source code is stricitly prohibited, unless the prior
+ * written consent of Ralink Technology, Inc. is obtained.
+ ***************************************************************************
+
+ Module Name:
+ cmm_mat_pppoe.c
+
+ Abstract:
+ MAT convert engine subroutine for PPPoE protocol.Due to the difference
+ of characteristic of PPPoE discovery stage and session stage, we seperate
+ that as two parts and used different stretegy to handle it.
+
+ Revision History:
+ Who When What
+ -------- ---------- ----------------------------------------------
+ Shiang 02/26/07 Init version
+*/
+
+#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 */
+