pppoepkt.cpp

pppoe client

//********************************************************************
//	日期:	2004/08/24 - 24:8:2004   22:30
//	名前:	tiamo
//	描述:	pppoe
//*********************************************************************

#include "Stdafx.h"

// insert tag in packet
VOID PacketInsertTag(USHORT ucTag,PPACKET pPacket,PUCHAR pBuffer,USHORT usLen,PUCHAR *ppOutBuffer)
{
	if(ppOutBuffer)
		*ppOutBuffer = NULL;

	USHORT usCurrentLen = ntohs(pPacket->m_pFrame->m_pppFrame.m_usLen);
	USHORT usMaxLen = pPacket->m_pFrame->m_pppFrame.m_ucCode == PPPOE_CODE_PADI ? PPPOE_MAX_DATA_SIZE : PPPOE_MAX_PADI_DATA_SIZE;

	// check size
	if(usCurrentLen + usLen + sizeof(PPP_TAG) > usMaxLen)
	{
		// must have a seh frame
		ExRaiseStatus(NDIS_STATUS_BUFFER_OVERFLOW);
	}

	// set tag
	PPPP_TAG pTag = reinterpret_cast<PPPP_TAG>(pPacket->m_pucDataBuffer + usCurrentLen);
	pTag->m_usType = ucTag;
	pTag->m_usLen = htons(usLen);

	if(ppOutBuffer)
		*ppOutBuffer = pPacket->m_pucDataBuffer + usCurrentLen + sizeof(PPP_TAG);

	// check buffer pointer first,then copy it
	if(pBuffer)
	{
		NdisMoveMemory(pTag + 1,pBuffer,usLen);
	}

	// modify len
	pPacket->m_pFrame->m_pppFrame.m_usLen = htons(usCurrentLen + usLen + sizeof(PPP_TAG));
}

// prepare packet for send
VOID PreparePacketForSend(PPACKET pPacket)
{
	/*
	*				Serive	AC		Host	ACCookie	Relay		
	*				101		102		103		104			110
	*	PAY_LOAD 
	*	PADT
	*	PADI:		M				O
	*	PADO:		M		O		O		O			O
	*	PADR:		M				O		O			O
	*	PADS:		M				O					O
	*/

	switch(pPacket->m_pFrame->m_pppFrame.m_ucCode)
	{
		// pay load
	case PPPOE_CODE_PAY_LOAD:
		// PADT
	case PPPOE_CODE_PADT:
		break;

		// PADO
	case PPPOE_CODE_PADO:
		// 0x102 AC name
		PacketInsertTag(PPPOE_TAG_AC,pPacket,pPacket->m_pACName,pPacket->m_usACName,&pPacket->m_pACName);

		// fall through

		// PADR
	case PPPOE_CODE_PADR:
		// 0x104 AC cookie
		if(pPacket->m_pFrame->m_pppFrame.m_ucCode != PPPOE_CODE_PADR || pPacket->m_usACCookie)
			PacketInsertTag(PPPOE_TAG_AC_COOKIE,pPacket,pPacket->m_pACCookie,pPacket->m_usACCookie,&pPacket->m_pACCookie);

		// fall through

		// PADS
	case PPPOE_CODE_PADS:
		// 0x110 relay session id
		if(pPacket->m_usRelaySessionId)
			PacketInsertTag(PPPOE_TAG_RELAY_SESSION_ID,pPacket,pPacket->m_pRelaySessionId,pPacket->m_usRelaySessionId,
								 &pPacket->m_pRelaySessionId);

		// fall through

		// PADI
	case PPPOE_CODE_PADI:	
		// 0x101 service name
		PacketInsertTag(PPPOE_TAG_SERVICE,pPacket,pPacket->m_pServiceName,pPacket->m_usServiceName,&pPacket->m_pServiceName);

		// 0x103 host unique
		if(pPacket->m_usHostUnique)
			PacketInsertTag(PPPOE_TAG_HOST_UNIQUE,pPacket,pPacket->m_pHostUnique,pPacket->m_usHostUnique,
								 &pPacket->m_pHostUnique);

		break;
	}

	// set buffer len
	NdisAdjustBufferLength(pPacket->m_pNdisDataBuffer,ntohs(pPacket->m_pFrame->m_pppFrame.m_usLen));
}

// build packet for send
PPACKET BuildPacketForSend(PCHANNEL pChannel,PNDIS_PACKET pOrgPacket)
{
	PPACKET pRetPacket = NULL;

	NDIS_STATUS status;

	__try
	{
		// alloc the packet
		pRetPacket = AllocPacket();

		pRetPacket->m_pNdisPacket = GetNdisPacketFromPool(&g_poolPacket,&pRetPacket->m_pPacketItem);

		PPROTOCOL_RESERVED pResv = reinterpret_cast<PPROTOCOL_RESERVED>(pRetPacket->m_pNdisPacket->ProtocolReserved);

		pResv->m_pPacket = pRetPacket;
		pResv->m_pOrginalPacket = pOrgPacket;
		pResv->m_pChannel = pChannel;

		// packet from pool
		pRetPacket->m_ulFlags |= PPPOE_PACKET_FROM_POOL;

		// packet len
		UINT uLen;
		NdisQueryPacketLength(pOrgPacket,&uLen);

		// get data buffer,unchain if
		NdisUnchainBufferAtFront(pOrgPacket,&pRetPacket->m_pNdisDataBuffer);

		// send complete needed
		pRetPacket->m_ulFlags |= PPPOE_PACKET_NEED_CALL_SEND_COMPLETE;

		// chain data buffer first
		NdisChainBufferAtFront(pRetPacket->m_pNdisPacket,pRetPacket->m_pNdisDataBuffer);
		pRetPacket->m_ulFlags |= PPPOE_PACKET_DATA_BUFFER_CHAINED;

		// allocate header buffer
		NdisAllocateBuffer(&status,&pRetPacket->m_pNdisHeaderBuffer,g_hNdisBufferPool,&pRetPacket->m_ethFrame,PPPOE_HEADER_LEN);

		// chain header buffer
		NdisChainBufferAtFront(pRetPacket->m_pNdisPacket,pRetPacket->m_pNdisHeaderBuffer);
		pRetPacket->m_ulFlags |= (PPPOE_PACKET_HEADER_BUFFER_CHAINED | PPPOE_PACKET_HEADER_BUFFER_FROM_NDIS);

		// set frame pointer,data buffer do not need to set,we will not touch it;
		pRetPacket->m_pFrame = &pRetPacket->m_ethFrame;

		// copy dst and src mac addr
		NdisMoveMemory(pRetPacket->m_pFrame->m_dstMac,pChannel->m_macPeer,ETH_ADDR_LEN);

		NdisMoveMemory(pRetPacket->m_pFrame->m_srcMac,pChannel->m_macSelf,ETH_ADDR_LEN);

		// set ethernet type
		pRetPacket->m_pFrame->m_usProtocolType = PPPOE_SESSION;

		// set session id
		pRetPacket->m_pFrame->m_pppFrame.m_usSession = htons(pChannel->m_usSessionId);

		// set other
		pRetPacket->m_pFrame->m_pppFrame.m_ucCode = PPPOE_CODE_PAY_LOAD;
		pRetPacket->m_pFrame->m_pppFrame.m_ucType = PPPOE_TYPE;
		pRetPacket->m_pFrame->m_pppFrame.m_ucVer = PPPOE_VER;
		pRetPacket->m_pFrame->m_pppFrame.m_usLen = htons(static_cast<USHORT>(uLen));

		// prepare it
		PreparePacketForSend(pRetPacket);

		// inc send packets
		NdisInterlockedIncrement(&pChannel->m_lSendingPackets);
	}
	__except(EXCEPTION_EXECUTE_HANDLER)
	{
		if(pRetPacket)
		{
			DereferencePacket(pRetPacket);

			pRetPacket = NULL;
		}
	}

	return pRetPacket;
}

// init packet member value
BOOLEAN InitializePacketForRecved(PPACKET pPacket)
{
	/*
	*				Serive	AC		Host	ACCookie	Relay		
	*				101		102		103		104			110
	*	PAY_LOAD 
	*	PADT
	*	PADI:		M				O					O
	*	PADO:		M		M		O		O			O
	*	PADR:		M				O		O			O
	*	PADS:		M				O					
	*/

	// check pppoe format
	if(!CheckIsPPPoEPacket(pPacket))
		return FALSE;


	// check len
	UCHAR ucCode = pPacket->m_pFrame->m_pppFrame.m_ucCode;

	// ctrl packet only check len
	if(ucCode)
	{
		PUCHAR pEnd = pPacket->m_pucDataBuffer + ntohs(pPacket->m_pFrame->m_pppFrame.m_usLen);
		PUCHAR pCurrent = pPacket->m_pucDataBuffer;

		while(pCurrent < pEnd)
		{
			PPPP_TAG pTag = reinterpret_cast<PPPP_TAG>(pCurrent);
			pCurrent += ntohs(pTag->m_usLen) + sizeof(PPP_TAG);
		}

		if(pCurrent != pEnd)
			return FALSE;
	}

	// build member value
	switch(pPacket->m_pFrame->m_pppFrame.m_ucCode)
	{
	case PPPOE_CODE_PAY_LOAD:
	case PPPOE_CODE_PADT:
		break;

		// PADO
	case PPPOE_CODE_PADO:
		{
			// get service name
			RetrieveTag(PPPOE_TAG_SERVICE,pPacket,&pPacket->m_pServiceName,&pPacket->m_usServiceName,TRUE);

			if(pPacket->m_pServiceName)
			{
				// get ac name
				RetrieveTag(PPPOE_TAG_AC,pPacket,&pPacket->m_pACName,&pPacket->m_usACName,TRUE);

				if(pPacket->m_pACName)
				{
					// ac cookie
					RetrieveTag(PPPOE_TAG_AC_COOKIE,pPacket,&pPacket->m_pACCookie,&pPacket->m_usACCookie,TRUE);

					// host unique
					RetrieveTag(PPPOE_TAG_HOST_UNIQUE,pPacket,&pPacket->m_pHostUnique,&pPacket->m_usHostUnique,TRUE);

					// relay session id
					RetrieveTag(PPPOE_TAG_RELAY_SESSION_ID,pPacket,&pPacket->m_pRelaySessionId,&pPacket->m_usRelaySessionId,TRUE);
				}
			}
		}
		break;

		// PADI
	case PPPOE_CODE_PADI:
		{
			// get service name
			RetrieveTag(PPPOE_TAG_SERVICE,pPacket,&pPacket->m_pServiceName,&pPacket->m_usServiceName,TRUE);

			if(pPacket->m_pServiceName)
			{
				// host unique
				RetrieveTag(PPPOE_TAG_HOST_UNIQUE,pPacket,&pPacket->m_pHostUnique,&pPacket->m_usHostUnique,TRUE);

				// relay session id
				RetrieveTag(PPPOE_TAG_RELAY_SESSION_ID,pPacket,&pPacket->m_pRelaySessionId,&pPacket->m_usRelaySessionId,TRUE);
			}
		}
		break;

		// PADR
	case PPPOE_CODE_PADR:
		{
			// get service name
			RetrieveTag(PPPOE_TAG_SERVICE,pPacket,&pPacket->m_pServiceName,&pPacket->m_usServiceName,TRUE);

			if(pPacket->m_pServiceName)
			{
				// ac cookie
				RetrieveTag(PPPOE_TAG_AC_COOKIE,pPacket,&pPacket->m_pACCookie,&pPacket->m_usACCookie,TRUE);

				// host unique
				RetrieveTag(PPPOE_TAG_HOST_UNIQUE,pPacket,&pPacket->m_pHostUnique,&pPacket->m_usHostUnique,TRUE);

				// relay session id
				RetrieveTag(PPPOE_TAG_RELAY_SESSION_ID,pPacket,&pPacket->m_pRelaySessionId,&pPacket->m_usRelaySessionId,TRUE);
			}
		}
		break;

		// PADS
	case PPPOE_CODE_PADS:
		{
			// get service name
			RetrieveTag(PPPOE_TAG_SERVICE,pPacket,&pPacket->m_pServiceName,&pPacket->m_usServiceName,TRUE);

			if(pPacket->m_pServiceName)
			{
				// host unique
				RetrieveTag(PPPOE_TAG_HOST_UNIQUE,pPacket,&pPacket->m_pHostUnique,&pPacket->m_usHostUnique,TRUE);
			}
		}
		break;
	}

	// get error tag
	RetrieveErrorTag(pPacket);

	return TRUE;
}

// convert recved ndis packet
PPACKET ConvertRecvedNdisPacket(PBIND_CONTEXT pBind,PNDIS_PACKET pNdisPacket,PBOOLEAN pNeedCallReturn)
{
	*pNeedCallReturn = FALSE;

	PPACKET pPacket = NULL;

	PNDIS_BUFFER pFirstBuffer = NULL;

	UINT nBufferCount = 0;
	UINT nTotalLen = 0;

	// query packet
	NdisQueryPacket(pNdisPacket,NULL,&nBufferCount,&pFirstBuffer,&nTotalLen);

	// check len
	if(nTotalLen > PPPOE_MAX_ETH_FRAME_SIZE)
		return NULL;

	// get status
	NDIS_STATUS status = NDIS_GET_PACKET_STATUS(pNdisPacket);

	// we can use the buffer in the packet
	if(status != NDIS_STATUS_RESOURCES && nBufferCount == 1)
	{
		PPPPOE_FRAME pFrame;
		UINT uLen;

		// query buffer
		NdisQueryBufferSafe(pFirstBuffer,(PVOID *)&pFrame,&uLen,NormalPagePriority);

		// check return value
		if(pFrame && uLen > PPPOE_MIN_ETH_FRAME_SIZE)
		{
			// check pppoe frame
			if(FastCheckIsPPPoEFrame(pFrame,PPPOE_MIN_ETH_FRAME_SIZE))
			{
				// check frame size
				if((UINT)ntohs(pFrame->m_pppFrame.m_usLen) <= uLen)
				{
					// reuse the buffers
					pPacket = ReuseRecvedPacketBuffer(pBind,pNdisPacket,pFrame,nTotalLen);
				}
			}
		}

		if(pPacket)
		{
			*pNeedCallReturn = TRUE;
			NdisInterlockedIncrement(&pBind->m_lPacketNeedReturn);
		}
	}
	else
	{
		// create new packet
		pPacket = GetSimplePacket();

		UINT uCurrentOffset = 0;

		PUCHAR pFrame;
		UINT uLen;

		for(;pFirstBuffer;)
		{	
			// query buffer
			NdisQueryBufferSafe(pFirstBuffer,(PVOID *)&pFrame,&uLen,NormalPagePriority);

			NdisMoveMemory(pPacket->m_pucFrame + uCurrentOffset,pFrame,uLen);

			uCurrentOffset += uLen;

			NdisGetNextBuffer(pFirstBuffer,&pFirstBuffer);

			if(uCurrentOffset < PPPOE_MIN_ETH_FRAME_SIZE)
				continue;

			if(!FastCheckIsPPPoEFrame(pPacket->m_pFrame,PPPOE_MIN_ETH_FRAME_SIZE))
			{
				uCurrentOffset = 0;
				break;
			}
		}

		// check the packet
		if(uCurrentOffset < PPPOE_MIN_ETH_FRAME_SIZE || uCurrentOffset < nTotalLen)
			DereferencePacket(pPacket);
	}

	return pPacket;
}

// fast check pppoe frame
BOOLEAN FastCheckIsPPPoEFrame(PPPPOE_FRAME pFrame,ULONG ulSize)
{
	if(ulSize != PPPOE_MIN_ETH_FRAME_SIZE)
		return FALSE;

	if(pFrame->m_usProtocolType != PPPOE_DISCOVERY && pFrame->m_usProtocolType != PPPOE_SESSION)
		return FALSE;

	return TRUE;
}

// full check pppoe packet
BOOLEAN CheckIsPPPoEPacket(PPACKET pPacket)
{
	if(pPacket->m_pFrame->m_usProtocolType != PPPOE_DISCOVERY && pPacket->m_pFrame->m_usProtocolType != PPPOE_SESSION)
		return FALSE;

	if(pPacket->m_pFrame->m_pppFrame.m_ucVer != PPPOE_VER || pPacket->m_pFrame->m_pppFrame.m_ucType != PPPOE_TYPE)
		return FALSE;

	if(ntohs(pPacket->m_pFrame->m_pppFrame.m_usLen) > PPPOE_MAX_DATA_SIZE)
		return FALSE;

	BOOLEAN bRet = FALSE;

	if( pPacket->m_pFrame->m_pppFrame.m_ucCode == PPPOE_CODE_PAY_LOAD || 
		pPacket->m_pFrame->m_pppFrame.m_ucCode == PPPOE_CODE_PADT || 
		pPacket->m_pFrame->m_pppFrame.m_ucCode == PPPOE_CODE_PADS)
	{
		bRet = pPacket->m_pFrame->m_pppFrame.m_usSession != 0;
	}
	else
	{
		bRet = pPacket->m_pFrame->m_pppFrame.m_usSession == 0;
	}

	return bRet;
}

// reuse recved packet buffer
PPACKET ReuseRecvedPacketBuffer(PBIND_CONTEXT pBind,PNDIS_PACKET pNdisPacket,PPPPOE_FRAME pFrame,UINT uLen)
{
	PPACKET pRet = NULL;
	__try
	{
		pRet = AllocPacket();

		// unchain org buffer
		NdisUnchainBufferAtFront(pNdisPacket,&pRet->m_pNdisReturnBuffer);

		// need return
		pRet->m_ulFlags |= PPPOE_PACKET_NEED_RETURN_PACKET;

		// reuse packet
		pRet->m_pNdisPacket = pNdisPacket;

		// copy frame
		pRet->m_pFrame = &pRet->m_ethFrame;

		NdisMoveMemory(pRet->m_pFrame,pFrame,PPPOE_HEADER_LEN);

		// set data buffer