rtmp_apcli_pppoe.c
来自「Ralink RT61 SoftAP Driver source code. 」· C语言 代码 · 共 984 行 · 第 1/2 页
C
984 行
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.
*/
NDIS_STATUS updateSesMacTable(
IN PUCHAR inMacAddr,
IN UINT16 sesID,
IN PUCHAR outMacAddr)
{
UINT16 hashIdx;
SesMacMappingEntry *pEntry, *pPrev, *pNewEntry;
if (!SesMacTable.valid)
return FALSE;
hashIdx = sesID % MAX_SUPPORT_APCLI_STA;
/*
printk("%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 = SesMacTable.sesHash[hashIdx];
while(pEntry)
{
// 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 = jiffies;
return TRUE;
}
else
{ // handle the age-out situation
if ((jiffies - pEntry->lastTime) > PPPoE_SES_ENTRY_AGEOUT_TIME)
{
// Remove the aged entry
if (pEntry == SesMacTable.sesHash[hashIdx])
{
SesMacTable.sesHash[hashIdx]= pEntry->pNext;
pPrev = SesMacTable.sesHash[hashIdx];
}
else
{
pPrev->pNext = pEntry->pNext;
}
#ifdef KMALLOC_BATCH
UWRNodeEntryFree((PUCHAR)pEntry);
#else
kfree(pEntry);
#endif
pEntry = (pPrev == NULL ? NULL: pPrev->pNext);
}
else
{
pPrev = pEntry;
pEntry = pEntry->pNext;
}
}
}
// Allocate a new IPMacMapping entry and insert into the hash
#ifdef KMALLOC_BATCH
pNewEntry = (SesMacMappingEntry *)UWRNodeEntryAlloc();
#else
pNewEntry = (SesMacMappingEntry *)kmalloc(sizeof(SesMacMappingEntry), MEM_ALLOC_FLAG);
#endif
if (pNewEntry != NULL)
{
pNewEntry->sessionID= sesID;
memcpy(pNewEntry->inMacAddr, inMacAddr, MAC_ADDR_LEN);
memcpy(pNewEntry->outMacAddr, outMacAddr, MAC_ADDR_LEN);
pNewEntry->pNext = NULL;
pNewEntry->lastTime = jiffies;
if (SesMacTable.sesHash[hashIdx] == NULL)
{ // Hash list is empty, directly assign it.
SesMacTable.sesHash[hashIdx] = pNewEntry;
}
else
{
// Ok, we insert the new entry into the root of hash[hashIdx]
pNewEntry->pNext = SesMacTable.sesHash[hashIdx];
SesMacTable.sesHash[hashIdx] = pNewEntry;
}
//dumpSesMacTb(hashIdx);
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.
*/
PUCHAR APCLI_PPPoE_Dis_rx(
IN PRTMP_ADAPTER pAd,
IN struct sk_buff *pSkb,
IN PUCHAR pLayerHdr)
{
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)(pSkb->data);
pSrvMac = (PUCHAR)(pSkb->data + 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)(pSkb->data + 6);
break;
default:
return NULL;
}
// Ignore the Code field(length=1)
pData ++;
if (needUpdateSesTb || isPADT)
sesID = ntohs(*((UINT16 *)pData));
if (isPADT)
{
pInMac = getInMacByOutMacFromSesMacTb(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 = ntohs(*((UINT16 *)pData));
pPayloadLen = pData;
pData += 2;
#if 0
printk("%s():needUpdateSesTb=%d, payloadLen=%d, findTag=0x%04x\n", __FUNCTION__, needUpdateSesTb, payloadLen, findTag);
printk("%s():Dump pppoe payload==>\n", __FUNCTION__);
dumpPkt(pData, payloadLen);
#endif
// First parsing the PPPoE paylod to find out the required tag(e.g., x0103 or 0x0104)
leftLen = payloadLen;
while (leftLen)
{
tagID = ntohs(*((UINT16 *)pData));
tagLen = ntohs(*((UINT16 *)(pData+2)));
DBGPRINT(RT_DEBUG_INFO, "%s():tagID=0x%4x, tagLen= 0x%4x\n", __FUNCTION__, tagID, tagLen);
if (tagID== findTag && tagLen>0)
{
DBGPRINT(RT_DEBUG_INFO, "%s():Find a PPPoE packet which have required tag(0x%04x)! tagLen=%d\n", __FUNCTION__, findTag, tagLen);
//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);
}
}
DBGPRINT(RT_DEBUG_INFO, "%s():After parsing, the pTagConent=0x%p, tagLen=%d!\n", __FUNCTION__, pTagContent, tagLen);
// Now update our pppoe discovery table "UidMacTable"
if (pTagContent)
{
pEntry = UidMacTableLookUp(pAd, pTagContent, tagLen);
DBGPRINT(RT_DEBUG_INFO, "%s():After UidMacTableLookUp(), the pEntry=%p, pTagContent=%p\n", __FUNCTION__, pEntry, pTagContent);
// 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 = pSkb->len - (pTagContent - pSkb->data) - 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);
}
pSkb->tail -= removedTagLen;
pSkb->len -= removedTagLen;
*((UINT16 *)pPayloadLen) = htons(payloadLen - removedTagLen);
}
if (needUpdateSesTb) {
DBGPRINT(RT_DEBUG_INFO,"%s(): This's a PADS packet, so we need to create the SesMacTable!\n", __FUNCTION__);
updateSesMacTable(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
*/
PUCHAR APCLI_PPPoE_Dis_tx(
IN PRTMP_ADAPTER pAd,
IN struct sk_buff *pSkb,
IN PUCHAR pLayerHdr)
{
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;
struct sk_buff *pModSkb = NULL;
pPktHdr = pSkb->data;
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 = ntohs(*((UINT16 *)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 = ntohs(*((UINT16 *)pData));
pPayloadLen = pData;
offset = pPayloadLen - pSkb->data;
pData += 2;
#if 0
printk("%s():isServer=%d, payloadLen=%d, findTag=0x%04x\n", __FUNCTION__, isServer, payloadLen, findTag);
printk("%s():Dump pppoe payload==>\n", __FUNCTION__);
dumpPkt(pData, payloadLen);
#endif
// First parsing the PPPoE paylod to find out the required tag(e.g., x0103 or 0x0104)
leftLen = payloadLen;
while (leftLen)
{
tagID = ntohs(*((UINT16 *)pData));
tagLen = ntohs(*((UINT16 *)(pData+2)));
DBGPRINT(RT_DEBUG_INFO, "%s():tagID=0x%04x, tagLen= 0x%04x\n", __FUNCTION__, tagID, tagLen);
if (tagID== findTag && tagLen>0)
{
DBGPRINT(RT_DEBUG_INFO, "%s(): Find a PPPoE packet which have required tag(0x%04x)! tagLen=%d\n", __FUNCTION__, findTag, tagLen);
// 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);
}
}
DBGPRINT(RT_DEBUG_INFO, "%s(): After parsing the pppoe payload, the pTagConent=0x%p, tagLen=%d!\n", __FUNCTION__, pTagContent, tagLen);
// Now update our pppoe discovery table "UidMacTable"
pEntry = UidMacTableUpdate(pAd, pSrcMac, pDstMac, pTagContent, tagLen, isServer);
DBGPRINT(RT_DEBUG_INFO, "%s(): after UidMacTableUpdate(), the pEntry=%p, pTagContent=%p\n", __FUNCTION__, pEntry, pTagContent);
if (pEntry && (pTagContent == NULL))
{
PUCHAR tailHead;
if(skb_cloned(pSkb))
pModSkb = skb_copy(pSkb, MEM_ALLOC_FLAG);
else
pModSkb = pSkb;
if(!pModSkb)
return NULL;
tailHead = skb_put(pModSkb, (PPPOE_DIS_UID_LEN + 4));
if (tailHead)
{
pPayloadLen = pModSkb->data + 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;
memcpy(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;
memcpy(tailHead, pEntry->uIDStr, PPPOE_DIS_UID_LEN);
}
*(UINT16 *)pPayloadLen = htons(payloadLen + 4 + PPPOE_DIS_UID_LEN);
}
}
if (needUpdateSesTb)
updateSesMacTable(pSrcMac, sesID, pDstMac);
return (PUCHAR)pModSkb;
}
/* PPPoE discovery stage init function */
NDIS_STATUS APCLI_PPPoE_Dis_init(VOID)
{
memset(&UidMacTable, 0, sizeof(UidMacTable));
UidMacTable.valid = TRUE;
memset(&SesMacTable, 0, sizeof(SesMacTable));
SesMacTable.valid = TRUE;
return TRUE;
}
/* PPPoE discovery stage exit function */
NDIS_STATUS APCLI_PPPoE_Dis_exit(VOID)
{
UidMacTable_RemoveAll();
SesMacTable_RemoveAll();
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
*/
PUCHAR APCLI_PPPoE_Ses_rx(
IN PRTMP_ADAPTER pAd,
IN struct sk_buff *pSkb,
IN PUCHAR pLayerHdr)
{
PUCHAR srcMac, dstMac = NULL, pData;
UINT16 sesID;
srcMac = (pSkb->data + 6);
pData = pLayerHdr;
//skip the first two bytes.(version/Type/Code)
pData += 2;
//get the session ID
sesID = ntohs( *((UINT16 *)pData));
// Try to find the dstMac from SesMacHash table.
dstMac = getInMacByOutMacFromSesMacTb(srcMac, sesID);
return dstMac;
}
// PPPoE Session stage Tx handler
PUCHAR APCLI_PPPoE_Ses_tx(
IN PRTMP_ADAPTER pAd,
IN struct sk_buff *pSkb,
IN PUCHAR pLayerHdr)
{
/*
For transmit packet, we do nothing.
*/
return NULL;
}
/* PPPoE session stage init function */
NDIS_STATUS APCLI_PPPoE_Ses_init(VOID)
{
return TRUE;
}
/* PPPoE session stage exit function */
NDIS_STATUS APCLI_PPPoE_Ses_exit(VOID)
{
return TRUE;
}
#endif //endif of "APCLI_SUPPORT"
⌨️ 快捷键说明
复制代码Ctrl + C
搜索代码Ctrl + F
全屏模式F11
增大字号Ctrl + =
减小字号Ctrl + -
显示快捷键?