pppoepkt.cpp

pppoe client

		pRet->m_pRealBuffer = pFrame + 1;

		// bind context
		pRet->m_pBindContext = pBind;

		NDIS_STATUS status;

		// allocate data buffer
		NdisAllocateBuffer(&status,&pRet->m_pNdisDataBuffer,g_hNdisBufferPool,pRet->m_pucDataBuffer,uLen - PPPOE_HEADER_LEN);

		if(status != NDIS_STATUS_SUCCESS)
			ExRaiseStatus(status);

		pRet->m_ulFlags |= PPPOE_PACKET_DATA_BUFFER_FROM_NDIS;

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

		if(status != NDIS_STATUS_SUCCESS)
			ExRaiseStatus(status);

		pRet->m_ulFlags |= PPPOE_PACKET_HEADER_BUFFER_FROM_NDIS;

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

		// chain header buffer
		NdisChainBufferAtFront(pRet->m_pNdisPacket,pRet->m_pNdisHeaderBuffer);
		pRet->m_ulFlags |= PPPOE_PACKET_HEADER_BUFFER_CHAINED;
	}
	__except(EXCEPTION_EXECUTE_HANDLER)
	{
		if(pRet)
			DereferencePacket(pRet);

		pRet = NULL;
	}

	return pRet;
}

// get tag from buffer
VOID RetrieveTag(USHORT usTag,PPACKET pPacket,PUCHAR *ppBuffer,PUSHORT pBufferLen,BOOLEAN bSaveToPacket)
{
	*ppBuffer = NULL;
	*pBufferLen = 0;

	// check code first
	if(!pPacket->m_pFrame->m_pppFrame.m_ucCode)
		return;

	PUCHAR pTagBuffer = NULL;
	USHORT usTagLen = 0;

	PUCHAR pCurrent = pPacket->m_pucDataBuffer;
	PUCHAR pEnd = pCurrent + ntohs(pPacket->m_pFrame->m_pppFrame.m_usLen);

	// loop find
	while(pCurrent < pEnd)
	{
		PPPP_TAG pTag = reinterpret_cast<PPPP_TAG>(pCurrent);
		if(pTag->m_usType == usTag)
		{
			pTagBuffer = pCurrent + sizeof(PPP_TAG);
			usTagLen = ntohs(pTag->m_usLen);
			break;
		}

		pCurrent += ntohs(pTag->m_usLen) + sizeof(PPP_TAG);
	}

	// save
	*ppBuffer = pTagBuffer;
	*pBufferLen = usTagLen;

	// save it to packet
	if(pTagBuffer && bSaveToPacket)
	{
		switch(usTag)
		{
		case PPPOE_TAG_SERVICE:
			pPacket->m_pServiceName = pTagBuffer;
			pPacket->m_usServiceName = usTagLen;
			break;

		case PPPOE_TAG_AC:
			pPacket->m_pACName = pTagBuffer;
			pPacket->m_usACName = usTagLen;
			break;

		case PPPOE_TAG_AC_COOKIE:
			pPacket->m_pACCookie = pTagBuffer;
			pPacket->m_usACCookie = usTagLen;
			break;

		case PPPOE_TAG_HOST_UNIQUE:
			pPacket->m_pHostUnique = pTagBuffer;
			pPacket->m_usHostUnique = usTagLen;
			break;

		case PPPOE_TAG_RELAY_SESSION_ID:
			pPacket->m_pRelaySessionId = pTagBuffer;
			pPacket->m_usRelaySessionId = usTagLen;
			break;

		case PPPOE_TAG_SERVICE_NAME_ERROR:
		case PPPOE_TAG_AC_SYSTEM_ERROR:
		case PPPOE_TAG_GEN_ERROR:
			pPacket->m_pError = pTagBuffer;
			pPacket->m_usError = usTagLen;
			pPacket->m_ulErrorTag = usTag;
			break;
		}
	}
}

// get error tags
VOID RetrieveErrorTag(PPACKET pPacket)
{
	PUCHAR pBuffer;
	USHORT usLen;

	// service name error = 201
	RetrieveTag(PPPOE_TAG_SERVICE_NAME_ERROR,pPacket,&pBuffer,&usLen,TRUE);
	if(pBuffer)
		pPacket->m_ulFlags |= PPPOE_PACKET_ERROR_TAG_RECV;

	if(pPacket->m_ulFlags & PPPOE_PACKET_ERROR_TAG_RECV)
		return;

	// ac system error = 202
	RetrieveTag(PPPOE_TAG_AC_SYSTEM_ERROR,pPacket,&pBuffer,&usLen,TRUE);
	if(pBuffer)
		pPacket->m_ulFlags |= PPPOE_PACKET_ERROR_TAG_RECV;

	if(pPacket->m_ulFlags & PPPOE_PACKET_ERROR_TAG_RECV)
		return;

	// generice error = 203
	RetrieveTag(PPPOE_TAG_GEN_ERROR,pPacket,&pBuffer,&usLen,TRUE);
	if(pBuffer)
		pPacket->m_ulFlags |= PPPOE_PACKET_ERROR_TAG_RECV;

	if(pPacket->m_ulFlags & PPPOE_PACKET_ERROR_TAG_RECV)
		return;

	// ac cookie error = 204
	RetrieveTag(PPPOE_TAG_AC_COOKIE_ERROR,pPacket,&pBuffer,&usLen,TRUE);
	if(pBuffer)
		pPacket->m_ulFlags |= PPPOE_PACKET_ERROR_TAG_RECV;
}

// get service name from buffer
VOID RetrieveServiceName(PPACKET pPacket,PUCHAR *ppBuffer,PUSHORT pBufferLen,PUCHAR pStart,USHORT usTotalLen)
{
	if(!pStart || !usTotalLen)
	{
		*ppBuffer = pPacket->m_pServiceName;
		*pBufferLen = pPacket->m_usServiceName;
	}
	else
	{
		PUCHAR pEnd = pStart + usTotalLen;

		PUCHAR pTagBuffer = NULL;
		USHORT usTagLen = 0;

		// loop find
		while(pStart < pEnd)
		{
			PPPP_TAG pTag = reinterpret_cast<PPPP_TAG>(pStart);
			if(pTag->m_usType == PPPOE_TAG_SERVICE && ntohs(pTag->m_usLen) + pStart + sizeof(PPP_TAG) <= pEnd)
			{
				pTagBuffer = pStart + sizeof(PPP_TAG);
				usTagLen = ntohs(pTag->m_usLen);
				break;
			}

			pStart += ntohs(pTag->m_usLen) + sizeof(PPP_TAG);
		}

		// save
		*ppBuffer = pTagBuffer;
		*pBufferLen = usTagLen;
	}
}

// verify ac cookie
BOOLEAN VerifyACCookie(PPACKET pPacket)
{
	if(pPacket->m_usACCookie != ETH_ADDR_LEN)
		return FALSE;

	// ac cookie will be client mac address
	return RtlCompareMemory(pPacket->m_pACCookie,pPacket->m_pFrame->m_srcMac,ETH_ADDR_LEN) == ETH_ADDR_LEN;
}

// verify service name
BOOLEAN VerifyServiceName(PUCHAR pServiceName,USHORT usServiceNameLen,PPACKET pPacket,BOOLEAN bStrict)
{
	PUCHAR pBuffer;
	USHORT usLen;

	RetrieveTag(PPPOE_TAG_SERVICE,pPacket,&pBuffer,&usLen,FALSE);

	BOOLEAN bRet = FALSE;
	if(!bStrict)
	{
		bRet = RtlCompareMemory(pBuffer,pServiceName,usLen) == usLen;
	}
	else
	{
		bRet = usLen == usServiceNameLen && pBuffer && RtlCompareMemory(pBuffer,pServiceName,usLen) == usLen;
	}

	return bRet;
}

// build a PADI to send
PPACKET InitializePADIToSend(PUCHAR pServiceName,USHORT usServiceName,PUCHAR pHostUnique,USHORT usHostUnique)
{
	static UCHAR _s_macBroadcast[6] = {0xff,0xff,0xff,0xff,0xff,0xff};

	PPACKET pRet = NULL;
	__try
	{
		pRet = GetSimplePacket();

		NdisMoveMemory(pRet->m_pFrame->m_dstMac,_s_macBroadcast,6);
		pRet->m_pFrame->m_usProtocolType = PPPOE_DISCOVERY;

		pRet->m_pFrame->m_pppFrame.m_ucVer = PPPOE_VER;
		pRet->m_pFrame->m_pppFrame.m_ucType = PPPOE_TYPE;
		pRet->m_pFrame->m_pppFrame.m_usSession = 0;
		pRet->m_pFrame->m_pppFrame.m_ucCode = PPPOE_CODE_PADI;
		pRet->m_pFrame->m_pppFrame.m_usLen = 0;

		pRet->m_pServiceName = pServiceName;
		pRet->m_usServiceName = usServiceName;

		pRet->m_pHostUnique = pHostUnique;
		pRet->m_usHostUnique = usHostUnique;

		PreparePacketForSend(pRet);
	}
	__except(EXCEPTION_EXECUTE_HANDLER)
	{
		if(pRet)
			DereferencePacket(pRet);
	}

	return pRet;
}

// build a PADO to send
PPACKET InitializePADOToSend(PPACKET pRecvedPacket,PUCHAR pSelfMac,PUCHAR pServiceName,USHORT usServiceName,
							 PUCHAR pACName,USHORT usACNameLen,BOOLEAN bSetACCookie)
{
	PPACKET pRet = NULL;
	__try
	{
		pRet = GetSimplePacket();

		NdisMoveMemory(pRet->m_pFrame->m_dstMac,pRecvedPacket->m_pFrame->m_srcMac,6);
		NdisMoveMemory(pRet->m_pFrame->m_srcMac,pSelfMac,6);

		pRet->m_pFrame->m_usProtocolType = PPPOE_DISCOVERY;

		pRet->m_pFrame->m_pppFrame.m_ucVer = PPPOE_VER;
		pRet->m_pFrame->m_pppFrame.m_ucType = PPPOE_TYPE;
		pRet->m_pFrame->m_pppFrame.m_usSession = 0;
		pRet->m_pFrame->m_pppFrame.m_ucCode = PPPOE_CODE_PADO;
		pRet->m_pFrame->m_pppFrame.m_usLen = 0;

		pRet->m_pACName = pACName;

		pRet->m_usACName = usACNameLen;

		pRet->m_pServiceName = pServiceName;

		pRet->m_usServiceName = usServiceName;

		if(bSetACCookie)
		{
			pRet->m_pACCookie = pRecvedPacket->m_pFrame->m_srcMac;
			pRet->m_usACCookie = 6;
		}

		pRet->m_pHostUnique = pRecvedPacket->m_pHostUnique;
		pRet->m_usHostUnique = pRecvedPacket->m_usHostUnique;

		pRet->m_pRelaySessionId = pRecvedPacket->m_pRelaySessionId;
		pRet->m_usRelaySessionId = pRecvedPacket->m_usRelaySessionId;

		PreparePacketForSend(pRet);
	}
	__except(EXCEPTION_EXECUTE_HANDLER)
	{
		if(pRet)
			DereferencePacket(pRet);
	}

	return pRet;
}

// build PADR to send
PPACKET InitializePADRToSend(PPACKET pRecvedPacket,PUCHAR pServiceName,USHORT usServiceName,PUCHAR pHostUnique,USHORT usHostUnique)
{
	PPACKET pRet = NULL;
	__try
	{
		pRet = GetSimplePacket();

		NdisMoveMemory(pRet->m_pFrame->m_dstMac,pRecvedPacket->m_pFrame->m_srcMac,6);
		NdisMoveMemory(pRet->m_pFrame->m_srcMac,pRecvedPacket->m_pFrame->m_dstMac,6);

		pRet->m_pFrame->m_usProtocolType = PPPOE_DISCOVERY;

		pRet->m_pFrame->m_pppFrame.m_ucVer = PPPOE_VER;
		pRet->m_pFrame->m_pppFrame.m_ucType = PPPOE_TYPE;
		pRet->m_pFrame->m_pppFrame.m_usSession = 0;
		pRet->m_pFrame->m_pppFrame.m_ucCode = PPPOE_CODE_PADR;
		pRet->m_pFrame->m_pppFrame.m_usLen = 0;

		pRet->m_pServiceName = pServiceName;
		pRet->m_usServiceName = usServiceName;

		pRet->m_pHostUnique = pHostUnique;
		pRet->m_usHostUnique = usHostUnique;

		pRet->m_pRelaySessionId = pRecvedPacket->m_pRelaySessionId;
		pRet->m_usRelaySessionId = pRecvedPacket->m_usRelaySessionId;

		pRet->m_pACCookie = pRecvedPacket->m_pACCookie;
		pRet->m_usACCookie = pRecvedPacket->m_usACCookie;

		PreparePacketForSend(pRet);
	}
	__except(EXCEPTION_EXECUTE_HANDLER)
	{
		if(pRet)
			DereferencePacket(pRet);
	}

	return pRet;
}

// build PADS to send
PPACKET InitializePADSToSend(PPACKET pRecvedPacket,USHORT usSessionId)
{
	PPACKET pRet = NULL;
	__try
	{
		pRet = GetSimplePacket();

		NdisMoveMemory(pRet->m_pFrame->m_dstMac,pRecvedPacket->m_pFrame->m_srcMac,6);
		NdisMoveMemory(pRet->m_pFrame->m_srcMac,pRecvedPacket->m_pFrame->m_dstMac,6);

		pRet->m_pFrame->m_usProtocolType = PPPOE_DISCOVERY;

		pRet->m_pFrame->m_pppFrame.m_ucVer = PPPOE_VER;
		pRet->m_pFrame->m_pppFrame.m_ucType = PPPOE_TYPE;
		pRet->m_pFrame->m_pppFrame.m_usSession = htons(usSessionId);
		pRet->m_pFrame->m_pppFrame.m_ucCode = PPPOE_CODE_PADS;
		pRet->m_pFrame->m_pppFrame.m_usLen = 0;


		pRet->m_pServiceName = pRecvedPacket->m_pServiceName;
		pRet->m_usServiceName = pRecvedPacket->m_usServiceName;

		pRet->m_pHostUnique = pRecvedPacket->m_pHostUnique;
		pRet->m_usHostUnique = pRecvedPacket->m_usHostUnique;

		pRet->m_pRelaySessionId = pRecvedPacket->m_pRelaySessionId;
		pRet->m_usRelaySessionId = pRecvedPacket->m_usRelaySessionId;

		PreparePacketForSend(pRet);
	}
	__except(EXCEPTION_EXECUTE_HANDLER)
	{
		if(pRet)
			DereferencePacket(pRet);
	}

	return pRet;
}

// build PADT to send
PPACKET InitializePADTToSend(PUCHAR pDstAddr,PUCHAR pSrcAddr,USHORT usSession)
{
	PPACKET pSendPacket = NULL;
	__try
	{
		pSendPacket = GetSimplePacket();

		NdisMoveMemory(pSendPacket->m_pFrame->m_dstMac,pDstAddr,6);
		NdisMoveMemory(pSendPacket->m_pFrame->m_srcMac,pSrcAddr,6);

		pSendPacket->m_pFrame->m_usProtocolType = PPPOE_DISCOVERY;

		pSendPacket->m_pFrame->m_pppFrame.m_ucVer = PPPOE_VER;
		pSendPacket->m_pFrame->m_pppFrame.m_ucType = PPPOE_TYPE;
		pSendPacket->m_pFrame->m_pppFrame.m_usSession = htons(usSession);
		pSendPacket->m_pFrame->m_pppFrame.m_ucCode = PPPOE_CODE_PADT;
		pSendPacket->m_pFrame->m_pppFrame.m_usLen = 0;

		PreparePacketForSend(pSendPacket);
	}
	__except(EXCEPTION_EXECUTE_HANDLER)
	{
		if(pSendPacket)
			DereferencePacket(pSendPacket);
	}
	return pSendPacket;
}

// send error PADS
VOID SendPADSWithError(USHORT usTag,PBIND_CONTEXT pBind,PPACKET pRecvedPacket)
{
	PPACKET pSendPacket = NULL;
	__try
	{
		pSendPacket = InitializePADSToSend(pRecvedPacket,1234);

		switch(usTag)
		{
		case PPPOE_TAG_SERVICE_NAME_ERROR:
			{
				static CHAR error[] = "Service name error";
				PacketInsertTag(usTag,pSendPacket,reinterpret_cast<PUCHAR>(error),(USHORT)sizeof(error) + 2,NULL);
			}
			break;

		case PPPOE_TAG_AC_SYSTEM_ERROR:
			{
				static CHAR error[] = "AC system error";
				PacketInsertTag(usTag,pSendPacket,reinterpret_cast<PUCHAR>(error),(USHORT)sizeof(error) + 2,NULL);
			}
			break;

		case PPPOE_TAG_GEN_ERROR:
			{
				static CHAR error[] = "Generic error";
				PacketInsertTag(usTag,pSendPacket,reinterpret_cast<PUCHAR>(error),(USHORT)sizeof(error) + 2,NULL);
			}
			break;

		case PPPOE_TAG_AC_COOKIE_ERROR:
			{
				static CHAR error[] = "AC cookie error";
				PacketInsertTag(usTag,pSendPacket,reinterpret_cast<PUCHAR>(error),(USHORT)sizeof(error) + 2,NULL);
			}
			break;
		}

		PreparePacketForSend(pSendPacket);

		ReferencePacket(pSendPacket);

		ReferenceBind(pBind,TRUE);

		SendPacket(pBind,pSendPacket);
	}
	__except(EXCEPTION_EXECUTE_HANDLER)
	{
	}

	if(pSendPacket)
		DereferencePacket(pSendPacket);
}