packet.cpp

pppoe client

//********************************************************************
//	日期:	2004/08/13 - 13:8:2004   1:03
//	名前:	tiamo
//	描述:	packet manipulate
//*********************************************************************

#include "stdafx.h"

// packet pool
PACKET_POOL g_packetPool;

// buffer pool
BUFFER_POOL g_bufferPool;

// PPPOE_PACKET look aside list buffer
NPAGED_LOOKASIDE_LIST g_lookasidePPPOEPacket;

// get simple PPPOE Packet
PPPPOE_PACKET GetSimplePPPOEPacket()
{
	PPPPOE_PACKET pRetPacket = NULL;

	__try
	{
		pRetPacket = AllocPPPOEPacketStorage();

		pRetPacket->m_pNdisPacket = GetNdisPacketFromPool(&g_packetPool,&pRetPacket->m_pPacketItem);
		
		reinterpret_cast<PPROTOCOL_RESERVED>(pRetPacket->m_pNdisPacket->ProtocolReserved)->m_pPPPOEPacket = pRetPacket;
		pRetPacket->m_ulFlags |= PPPOE_PACKET_FROM_POOL;

		pRetPacket->m_pFrame = GetFrameBufferFromPool(&g_bufferPool,&pRetPacket->m_pNdisBuffer);
		
		NdisChainBufferAtFront(pRetPacket->m_pNdisPacket,pRetPacket->m_pNdisBuffer);
		pRetPacket->m_ulFlags |= PPPOE_PACKET_BUFFER_FROM_POOL;

		pRetPacket->m_ulFlags |= PPPOE_PACKET_BUFFER_CHAINED;

		pRetPacket->m_pucDataBuffer = pRetPacket->m_pucFrame + PPPOE_HEADER_LEN;
	}
	__except(EXCEPTION_EXECUTE_HANDLER)
	{
		if(pRetPacket)
		{
			if(pRetPacket->m_pNdisPacket)
			{
				FreePacketToPool(&g_packetPool,pRetPacket);
			}

			FreePPPOEPacketStorage(pRetPacket);
		}
	}

	return pRetPacket;
}

// get ndis packet from pool
PNDIS_PACKET GetNdisPacketFromPool(PPACKET_POOL pPacketPool,PPACKET_ARRAY_ITEM *pOutItem)
{
	PNDIS_PACKET pRet = NULL;
	*pOutItem = NULL;

	NdisAcquireSpinLock(&pPacketPool->m_lockSelf);

	// no free packets
	if(IsListEmpty(&pPacketPool->m_ltFreePackets))
	{
		// allocate a block
		pRet = AddPacketBlockToPool(pPacketPool,pOutItem);
	}
	else
	{
		PLIST_ENTRY pEntry = RemoveHeadList(&pPacketPool->m_ltFreePackets);

		PPACKET_ARRAY_ITEM pItem = CONTAINING_RECORD(pEntry,PACKET_ARRAY_ITEM,m_ltFreePacket);

		pRet = pItem->m_pNdisPacket;
		*pOutItem = pItem;

		pPacketPool->m_ulFreePackets --;

		pItem->m_pPacketBlock->m_ulFreePackets --;
	}
	NdisReleaseSpinLock(&pPacketPool->m_lockSelf);

	return pRet;
}

// free packet
VOID FreePacketToPool(PPACKET_POOL pPacketPool,PPPPOE_PACKET pPPPOEPacket)
{
	NdisAcquireSpinLock(&pPacketPool->m_lockSelf);

	if(pPacketPool->m_ulFreePackets > pPacketPool->m_ulMaxFreeGuard)
		FreeUnusedPacketBlock(pPacketPool);

	PPACKET_ARRAY_ITEM pItem = pPPPOEPacket->m_pPacketItem;

	InsertHeadList(&pPacketPool->m_ltFreePackets,&pItem->m_ltFreePacket);

	pPacketPool->m_ulFreePackets ++;
	pItem->m_pPacketBlock->m_ulFreePackets ++;

	NdisReleaseSpinLock(&pPacketPool->m_lockSelf);
}

// get frame buffer from pool
PPPPOE_FRAME GetFrameBufferFromPool(PBUFFER_POOL pBufferPool,PNDIS_BUFFER *pOutBuffer)
{
	PPPPOE_FRAME pRet = NULL;
	*pOutBuffer = NULL;
	NdisAcquireSpinLock(&pBufferPool->m_lockSelf);

	// no free packets
	if(IsListEmpty(&pBufferPool->m_ltFreeBuffers))
	{
		// allocate a block
		pRet = AddBufferBlockToPool(pBufferPool,pOutBuffer);
	}
	else
	{
		// get a free buffer
		PLIST_ENTRY pEntry = RemoveHeadList(&pBufferPool->m_ltFreeBuffers);

		PBUFFER_ARRAY_ITEM pItem = CONTAINING_RECORD(pEntry,BUFFER_ARRAY_ITEM,m_ltFreeBuffer);

		pRet = reinterpret_cast<PPPPOE_FRAME>(pItem->m_pFrame);

		*pOutBuffer = pItem->m_pNdisBuffer;

		pBufferPool->m_ulFreeBuffers --;

		pItem->m_pBufferBlock->m_ulFreeBuffers --;
	}

	NdisReleaseSpinLock(&pBufferPool->m_lockSelf);

	return pRet;
}

// free buffer
VOID FreeBufferToPool(PBUFFER_POOL pBufferPool,PPPPOE_FRAME pFrame)
{
	NdisAcquireSpinLock(&pBufferPool->m_lockSelf);

	if(pBufferPool->m_ulFreeBuffers > pBufferPool->m_ulMaxFreeGuard)
		FreeUnusedBufferBlock(pBufferPool);

	PBUFFER_ARRAY_ITEM pItem = CONTAINING_RECORD(pFrame,BUFFER_ARRAY_ITEM,m_pFrame);

	InsertHeadList(&pBufferPool->m_ltFreeBuffers,&pItem->m_ltFreeBuffer);

	pBufferPool->m_ulFreeBuffers ++;
	pItem->m_pBufferBlock->m_ulFreeBuffers ++;

	NdisReleaseSpinLock(&pBufferPool->m_lockSelf);
}

// get a pppoe packet storage only
PPPPOE_PACKET AllocPPPOEPacketStorage()
{
	PPPPOE_PACKET pRet = static_cast<PPPPOE_PACKET>(NdisAllocateFromNPagedLookasideList(&g_lookasidePPPOEPacket));
	NdisZeroMemory(pRet,sizeof(PPPOE_PACKET));

	NdisInterlockedIncrement(&pRet->m_lRefCount);
	NdisInitializeListHead(&pRet->m_ltPackets);

	return pRet;
}

// free a pppoe packet storage
VOID FreePPPOEPacketStorage(PPPPOE_PACKET pPPPOEPacket)
{
	NdisFreeToNPagedLookasideList(&g_lookasidePPPOEPacket,pPPPOEPacket);
}

// add packet block to pool
PNDIS_PACKET AddPacketBlockToPool(PPACKET_POOL pPacketPool,PPACKET_ARRAY_ITEM *pOutItem)
{
	PNDIS_PACKET pRetPacket = NULL;

	ULONG ulSize = pPacketPool->m_ulPacketsPerBlock * sizeof(PACKET_ARRAY_ITEM) + sizeof(PACKET_BLOCK);

	PPACKET_BLOCK pBlock;

	// allocate block memory
	NDIS_STATUS status = NdisAllocateMemoryWithTag((PVOID*)&pBlock,ulSize,'PABL');
	if(status != NDIS_STATUS_SUCCESS)
		return NULL;

	// allocate packet pool
	NdisAllocatePacketPool(&status,&pBlock->m_hPacketPool,pPacketPool->m_ulPacketsPerBlock,pPacketPool->m_ulProtocolReservedLen);
	if(status != NDIS_STATUS_SUCCESS)
	{
		NdisFreeMemory(pBlock,ulSize,0);
		return NULL;
	}

	// set value
	pBlock->m_ulPackets = pPacketPool->m_ulPacketsPerBlock;
	pBlock->m_ulFreePackets = pPacketPool->m_ulPacketsPerBlock - 1;
	pBlock->m_pPacketPool = pPacketPool;

	ULONG i;
	for(i = 0; i < pBlock->m_ulPackets; i ++)
	{
		// prepare item
		PPACKET_ARRAY_ITEM pItem = pBlock->m_arrayPacket + i;
		pItem->m_pPacketBlock = pBlock;

		NdisAllocatePacket(&status,&pItem->m_pNdisPacket,pBlock->m_hPacketPool);
		if(status != NDIS_STATUS_SUCCESS)
		{
			break;
		}
	}

	// unsuccessful
	if(status != NDIS_STATUS_SUCCESS)
	{
		for(ULONG j = 0; j < i ; j ++)
		{
			// free it
			PPACKET_ARRAY_ITEM pItem = pBlock->m_arrayPacket + j;
			NdisFreePacket(pItem->m_pNdisPacket);
		}

		// free block
		NdisFreeMemory(pBlock,ulSize,0);
	}
	else
	{
		// update pool value
		pPacketPool->m_ulFreePackets += pBlock->m_ulFreePackets;
		pPacketPool->m_ulTotalPackets += pPacketPool->m_ulPacketsPerBlock;

		// link block
		InsertHeadList(&pPacketPool->m_ltBlocks,&pBlock->m_ltBlocks);

		// link free packet
		PPACKET_ARRAY_ITEM pItem;
		for(i = 1; i < pBlock->m_ulPackets; i ++)
		{
			pItem = pBlock->m_arrayPacket + i;
			InsertHeadList(&pPacketPool->m_ltFreePackets,&pItem->m_ltFreePacket);
		}

		// return the first one
		pItem = pBlock->m_arrayPacket;

		InitializeListHead(&(pItem->m_ltFreePacket));

		pRetPacket = pItem->m_pNdisPacket;

		*pOutItem = pItem;
	}

	return pRetPacket;
}

// add buffer block to pool
PPPPOE_FRAME AddBufferBlockToPool(PBUFFER_POOL pBufferPool,PNDIS_BUFFER *pOutBuffer)
{
	PPPPOE_FRAME pRetFrame = NULL;

	ULONG ulSize = pBufferPool->m_ulBuffersPerBlock *(pBufferPool->m_ulBufferSize + sizeof(BUFFER_ARRAY_ITEM))
					+ sizeof(BUFFER_BLOCK);

	// round it
	ulSize = (ulSize + 7) & 0xfffffff8;

	PBUFFER_BLOCK pBlock;

	// allocate block memory
	NDIS_STATUS status = NdisAllocateMemoryWithTag((PVOID*)&pBlock,ulSize,'BUBL');
	if(status != NDIS_STATUS_SUCCESS)
		return NULL;

	// allocate buffer pool
	NdisAllocateBufferPool(&status,&pBlock->m_hBufferPool,pBufferPool->m_ulBuffersPerBlock);
	if(status != NDIS_STATUS_SUCCESS)
	{
		NdisFreeMemory(pBlock,ulSize,0);
		return NULL;
	}

	// set value
	pBlock->m_ulBuffers = pBufferPool->m_ulBuffersPerBlock;
	pBlock->m_ulFreeBuffers = pBufferPool->m_ulBuffersPerBlock - 1;
	pBlock->m_pBufferPool = pBufferPool;

	ULONG i;
	for(i = 0; i < pBlock->m_ulBuffers; i ++)
	{
		// prepare item
		PBUFFER_ARRAY_ITEM pItem =  reinterpret_cast<PBUFFER_ARRAY_ITEM>(reinterpret_cast<PUCHAR>(pBlock->m_arrayBuffer) + 
									i * (pBufferPool->m_ulBufferSize + sizeof(BUFFER_ARRAY_ITEM)));

		pItem->m_pBufferBlock = pBlock;

		NdisAllocateBuffer(&status,&pItem->m_pNdisBuffer,pBlock->m_hBufferPool,pItem->m_pFrame,pBufferPool->m_ulBufferSize);
		if(status != NDIS_STATUS_SUCCESS)
		{
			break;
		}
	}

	// unsuccessful
	if(status != NDIS_STATUS_SUCCESS)
	{
		for(ULONG j = 0; j < i ; j ++)
		{
			// free it
			PBUFFER_ARRAY_ITEM pItem =  reinterpret_cast<PBUFFER_ARRAY_ITEM>(reinterpret_cast<PUCHAR>(pBlock->m_arrayBuffer) + 
										j * (pBufferPool->m_ulBufferSize + sizeof(BUFFER_ARRAY_ITEM)));
			NdisFreeBuffer(pItem->m_pNdisBuffer);
		}

		// free block
		NdisFreeMemory(pBlock,ulSize,0);
	}
	else
	{
		// update pool value
		pBufferPool->m_ulFreeBuffers += pBlock->m_ulFreeBuffers;
		pBufferPool->m_ulTotalBuffers += pBufferPool->m_ulBuffersPerBlock;

		// link block
		InsertHeadList(&pBufferPool->m_ltBlocks,&pBlock->m_ltBlocks);

		// link free packet
		PBUFFER_ARRAY_ITEM pItem;
		for(i = 1; i < pBlock->m_ulBuffers; i ++)
		{
			PBUFFER_ARRAY_ITEM pItem =  reinterpret_cast<PBUFFER_ARRAY_ITEM>(reinterpret_cast<PUCHAR>(pBlock->m_arrayBuffer) + 
										i * (pBufferPool->m_ulBufferSize + sizeof(BUFFER_ARRAY_ITEM)));
			InsertHeadList(&pBufferPool->m_ltFreeBuffers,&pItem->m_ltFreeBuffer);
		}

		// return the first one
		pItem = pBlock->m_arrayBuffer;

		InitializeListHead(&(pItem->m_ltFreeBuffer));

		*pOutBuffer = pItem->m_pNdisBuffer;

		pRetFrame = reinterpret_cast<PPPPOE_FRAME>(pItem->m_pFrame);
	}

	return pRetFrame;
}

// free unused block
VOID FreeUnusedPacketBlock(PPACKET_POOL pPacketPool)
{
	ULONG ulSize = pPacketPool->m_ulPacketsPerBlock * sizeof(PACKET_ARRAY_ITEM) + sizeof(PACKET_BLOCK);

	PLIST_ENTRY pEntry = pPacketPool->m_ltBlocks.Flink;
	
	while(pEntry != &pPacketPool->m_ltBlocks)
	{
		PPACKET_BLOCK pBlock = CONTAINING_RECORD(pEntry,PACKET_BLOCK,m_ltBlocks);

		pEntry = pEntry->Flink;

		if(pBlock->m_ulFreePackets == pBlock->m_ulPackets)
		{
			for(ULONG j = 0; j < pBlock->m_ulPackets ; j ++)
			{
				// free it
				PPACKET_ARRAY_ITEM pItem = pBlock->m_arrayPacket + j;
				NdisFreePacket(pItem->m_pNdisPacket);

				RemoveEntryList(&pItem->m_ltFreePacket);
			}

			pPacketPool->m_ulTotalPackets -= pBlock->m_ulPackets;
			pPacketPool->m_ulFreePackets -= pBlock->m_ulFreePackets;

			RemoveEntryList(&pBlock->m_ltBlocks);

			// free block
			NdisFreeMemory(pBlock,ulSize,0);
		}
	}
}

// free unused block
VOID FreeUnusedBufferBlock(PBUFFER_POOL pBufferPool)
{
	ULONG ulSize = pBufferPool->m_ulBuffersPerBlock *(pBufferPool->m_ulBufferSize + sizeof(BUFFER_ARRAY_ITEM))
					+ sizeof(BUFFER_BLOCK);

	ulSize = (ulSize + 7) & 0xfffffff8;

	PLIST_ENTRY pEntry = pBufferPool->m_ltBlocks.Flink;

	while(pEntry != &pBufferPool->m_ltBlocks)
	{
		PBUFFER_BLOCK pBlock = CONTAINING_RECORD(pEntry,BUFFER_BLOCK,m_ltBlocks);

		pEntry = pEntry->Flink;

		if(pBlock->m_ulFreeBuffers == pBlock->m_ulBuffers)
		{
			for(ULONG j = 0; j < pBlock->m_ulBuffers ; j ++)
			{
				// free it
				PBUFFER_ARRAY_ITEM pItem =  reinterpret_cast<PBUFFER_ARRAY_ITEM>(reinterpret_cast<PUCHAR>(pBlock->m_arrayBuffer) + 
											j * (pBufferPool->m_ulBufferSize + sizeof(BUFFER_ARRAY_ITEM)));

				NdisFreeBuffer(pItem->m_pNdisBuffer);

				RemoveEntryList(&pItem->m_ltFreeBuffer);
			}

			pBufferPool->m_ulTotalBuffers -= pBlock->m_ulBuffers;
			pBufferPool->m_ulFreeBuffers -= pBlock->m_ulFreeBuffers;

			RemoveEntryList(&pBlock->m_ltBlocks);

			// free block
			NdisFreeMemory(pBlock,ulSize,0);
		}
	}
}

// init pool
NDIS_STATUS InitializePool()
{
	NDIS_STATUS status = NDIS_STATUS_SUCCESS;

	// for ndis buffer pool
	NdisAllocateBufferPool(&status,&g_hBufferPool,30);

	if(status != NDIS_STATUS_SUCCESS)
	{
		drvDbgPrint("can't allocate ndis buffer pool raise status 0x%x.\n",status);
	}
	else
	{
		// pool
		g_packetPool.m_ulSig = 'PAPO';

		NdisInitializeListHead(&g_packetPool.m_ltBlocks);

		NdisInitializeListHead(&g_packetPool.m_ltFreePackets);

		g_packetPool.m_ulFreePackets = 0;
		g_packetPool.m_ulMaxFreeGuard = 100;
		g_packetPool.m_ulPacketsPerBlock = 16;
		g_packetPool.m_ulProtocolReservedLen = sizeof(PROTOCOL_RESERVED);
		g_packetPool.m_ulTotalPackets = 0;

		NdisAllocateSpinLock(&g_packetPool.m_lockSelf);

		g_bufferPool.m_ulSig = 'BUPO';

		NdisAllocateSpinLock(&g_bufferPool.m_lockSelf);

		NdisInitializeListHead(&g_bufferPool.m_ltBlocks);

		NdisInitializeListHead(&g_bufferPool.m_ltFreeBuffers);

		g_bufferPool.m_ulBufferSize = PPPOE_MAX_ETH_FRAME_SIZE;
		g_bufferPool.m_ulBuffersPerBlock = 10;
		g_bufferPool.m_ulFreeBuffers = 0;
		g_bufferPool.m_ulMaxFreeGuard = 100;
		g_bufferPool.m_ulTotalBuffers = 0;

		NdisInitializeNPagedLookasideList(&g_lookasidePPPOEPacket,0,0,0,sizeof(PPPOE_PACKET),'PPPA',0);
	}

	return status;
}

// destroy pool
VOID DestroyPool()
{
	// free buffer pool
	NdisAcquireSpinLock(&g_bufferPool.m_lockSelf);

	FreeUnusedBufferBlock(&g_bufferPool);

	NdisReleaseSpinLock(&g_bufferPool.m_lockSelf);

	// free packet pool
	NdisAcquireSpinLock(&g_packetPool.m_lockSelf);

	FreeUnusedPacketBlock(&g_packetPool);

	NdisReleaseSpinLock(&g_packetPool.m_lockSelf);

	// free npaged list
	NdisDeleteNPagedLookasideList(&g_lookasidePPPOEPacket);

	if(g_hBufferPool)
	{