drivers/staging/bcm/IPv6Protocol.c

#include "headers.h"

static BOOLEAN MatchSrcIpv6Address(S_CLASSIFIER_RULE *pstClassifierRule,IPV6Header *pstIpv6Header);
static BOOLEAN MatchDestIpv6Address(S_CLASSIFIER_RULE *pstClassifierRule,IPV6Header *pstIpv6Header);
static VOID DumpIpv6Header(IPV6Header *pstIpv6Header);

static UCHAR * GetNextIPV6ChainedHeader(UCHAR **ppucPayload,UCHAR *pucNextHeader,BOOLEAN *bParseDone,USHORT *pusPayloadLength)
{
	UCHAR *pucRetHeaderPtr = NULL;
	UCHAR *pucPayloadPtr = NULL;
	USHORT  usNextHeaderOffset = 0 ;
	PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(gblpnetdev);

	if ((NULL == ppucPayload) || (*pusPayloadLength == 0) ||
		(*bParseDone)) {
		*bParseDone = TRUE;
		return NULL;
	}

	pucRetHeaderPtr = *ppucPayload;
	pucPayloadPtr = *ppucPayload;

	if (!pucRetHeaderPtr || !pucPayloadPtr) {
		*bParseDone = TRUE;
		return NULL;
	}

	//Get the Nextt Header Type
	*bParseDone = FALSE;


	switch (*pucNextHeader) {
	case IPV6HDR_TYPE_HOPBYHOP:
		{

			BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, "\nIPv6 HopByHop Header");
			usNextHeaderOffset+=sizeof(IPV6HopByHopOptionsHeader);
		}
		break;

	case IPV6HDR_TYPE_ROUTING:
		{
			IPV6RoutingHeader *pstIpv6RoutingHeader;
			BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, "\nIPv6 Routing Header");
			pstIpv6RoutingHeader = (IPV6RoutingHeader *)pucPayloadPtr;
			usNextHeaderOffset += sizeof(IPV6RoutingHeader);
			usNextHeaderOffset += pstIpv6RoutingHeader->ucNumAddresses * IPV6_ADDRESS_SIZEINBYTES;

		}
		break;
	case IPV6HDR_TYPE_FRAGMENTATION:
		{
			BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, "\nIPv6 Fragmentation Header");
			usNextHeaderOffset+= sizeof(IPV6FragmentHeader);

		}
		break;
	case IPV6HDR_TYPE_DESTOPTS:
		{
			IPV6DestOptionsHeader *pstIpv6DestOptsHdr = (IPV6DestOptionsHeader *)pucPayloadPtr;
			int nTotalOptions = pstIpv6DestOptsHdr->ucHdrExtLen;
			BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, "\nIPv6 DestOpts Header Header");
			usNextHeaderOffset+= sizeof(IPV6DestOptionsHeader);
			usNextHeaderOffset+= nTotalOptions * IPV6_DESTOPTS_HDR_OPTIONSIZE ;

		}
		break;
	case IPV6HDR_TYPE_AUTHENTICATION:
		{
			IPV6AuthenticationHeader *pstIpv6AuthHdr = (IPV6AuthenticationHeader *)pucPayloadPtr;
			int nHdrLen = pstIpv6AuthHdr->ucLength;
			BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, "\nIPv6 Authentication Header");
			usNextHeaderOffset+= nHdrLen * 4;
		}
		break;
	case IPV6HDR_TYPE_ENCRYPTEDSECURITYPAYLOAD:
		{
			BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, "\nIPv6 Encrypted Security Payload Header");
			*bParseDone = TRUE;

		}
		break;
	case IPV6_ICMP_HDR_TYPE:
		{
			BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, " ICMP Header");
			*bParseDone = TRUE;
		}
		break;
	case TCP_HEADER_TYPE:
		{
			BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, " \nTCP Header");
			*bParseDone = TRUE;
		}
		break;
	case UDP_HEADER_TYPE:
		{
			BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, " \nUDP Header");
			*bParseDone = TRUE;
		}
		break;
	default:
		{
			*bParseDone = TRUE;

		}
		break;


	}

	if (*bParseDone == FALSE) {
		if(*pusPayloadLength <= usNextHeaderOffset) {
			*bParseDone = TRUE;
		} else {
			*pucNextHeader = *pucPayloadPtr;
			pucPayloadPtr += usNextHeaderOffset;
			(*pusPayloadLength) -= usNextHeaderOffset;
		}

	}

	*ppucPayload = pucPayloadPtr;
	return pucRetHeaderPtr;
}


static UCHAR GetIpv6ProtocolPorts(UCHAR *pucPayload,USHORT *pusSrcPort,USHORT *pusDestPort,USHORT usPayloadLength,UCHAR ucNextHeader)
{
	UCHAR *pIpv6HdrScanContext = pucPayload;
	BOOLEAN bDone = FALSE;
	UCHAR ucHeaderType =0;
	UCHAR *pucNextHeader = NULL;
	PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(gblpnetdev);

	if ( !pucPayload || (usPayloadLength == 0)) {
		return 0;
	}

	*pusSrcPort = *pusDestPort = 0;
	ucHeaderType = ucNextHeader;
	while (!bDone) {
		pucNextHeader = GetNextIPV6ChainedHeader(&pIpv6HdrScanContext,&ucHeaderType,&bDone,&usPayloadLength);
		if(bDone) {
			if((ucHeaderType==TCP_HEADER_TYPE) || (ucHeaderType == UDP_HEADER_TYPE)) {
				 *pusSrcPort=*((PUSHORT)(pucNextHeader));
				 *pusDestPort=*((PUSHORT)(pucNextHeader+2));
				 BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, " \nProtocol Ports - Src Port :0x%x Dest Port : 0x%x",ntohs(*pusSrcPort),ntohs(*pusDestPort));
			}
			break;

		}
	}
	return ucHeaderType;
}



USHORT	IpVersion6(PMINI_ADAPTER Adapter, /**< Pointer to the driver control structure */
					PVOID pcIpHeader, /**<Pointer to the IP Hdr of the packet*/
					S_CLASSIFIER_RULE *pstClassifierRule )
{
	USHORT	ushDestPort = 0;
	USHORT	ushSrcPort = 0;
	UCHAR   ucNextProtocolAboveIP =0;
	IPV6Header *pstIpv6Header = NULL;
	BOOLEAN bClassificationSucceed = FALSE;

	BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, "IpVersion6 ==========>\n");

	pstIpv6Header = (IPV6Header *)pcIpHeader;

	DumpIpv6Header(pstIpv6Header);

	//Try to get the next higher layer protocol and the Ports Nos if TCP or UDP
	ucNextProtocolAboveIP = GetIpv6ProtocolPorts((UCHAR *)(pcIpHeader + sizeof(IPV6Header)),
							&ushSrcPort,
							&ushDestPort,
							pstIpv6Header->usPayloadLength,
							pstIpv6Header->ucNextHeader);

	do {
		if (0 == pstClassifierRule->ucDirection) {
			//cannot be processed for classification.
		   // it is a down link connection
			break;
		}

		if (!pstClassifierRule->bIpv6Protocol) {
			//We are looking for Ipv6 Classifiers . Lets ignore this classifier and try the next one.
			break;
		}

		bClassificationSucceed=MatchSrcIpv6Address(pstClassifierRule,pstIpv6Header);
        	if (!bClassificationSucceed)
	            break;

	        bClassificationSucceed=MatchDestIpv6Address(pstClassifierRule,pstIpv6Header);
	        if (!bClassificationSucceed)
        	    break;

		//Match the protocol type.For IPv6 the next protocol at end of Chain of IPv6 prot headers
		bClassificationSucceed=MatchProtocol(pstClassifierRule,ucNextProtocolAboveIP);
	        if (!bClassificationSucceed)
	            break;
        	BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, "\nIPv6 Protocol Matched");

		if ((ucNextProtocolAboveIP == TCP_HEADER_TYPE) || (ucNextProtocolAboveIP == UDP_HEADER_TYPE)) {
			//Match Src Port
			BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, "\nIPv6 Source Port:%x\n",ntohs(ushSrcPort));
			bClassificationSucceed=MatchSrcPort(pstClassifierRule,ntohs(ushSrcPort));
			if (!bClassificationSucceed)
				break;

			BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, "\nIPv6 Src Port Matched");

			//Match Dest Port
			BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, "\nIPv6 Destination Port:%x\n",ntohs(ushDestPort));
			bClassificationSucceed=MatchDestPort(pstClassifierRule,ntohs(ushDestPort));
			if (!bClassificationSucceed)
				break;
			BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, "\nIPv6 Dest Port Matched");
		}
	} while (0);

	if (TRUE == bClassificationSucceed) {
		INT iMatchedSFQueueIndex = 0;
		iMatchedSFQueueIndex = SearchSfid(Adapter,pstClassifierRule->ulSFID);
		if(iMatchedSFQueueIndex >= NO_OF_QUEUES) {
			bClassificationSucceed = FALSE;
		} else {
			if (FALSE == Adapter->PackInfo[iMatchedSFQueueIndex].bActive) {
				bClassificationSucceed = FALSE;
			}
		}
	}

	return bClassificationSucceed;
}


static BOOLEAN MatchSrcIpv6Address(S_CLASSIFIER_RULE *pstClassifierRule,IPV6Header *pstIpv6Header)
{
	UINT uiLoopIndex = 0;
	UINT uiIpv6AddIndex = 0;
	UINT uiIpv6AddrNoLongWords = 4;
	ULONG aulSrcIP[4];
	PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(gblpnetdev);
	/*
	//This is the no. of Src Addresses ie Range of IP Addresses contained
	//in the classifier rule for which we need to match
	*/
	UINT  uiCountIPSrcAddresses = (UINT)pstClassifierRule->ucIPSourceAddressLength;


	if (0 == uiCountIPSrcAddresses)
		return TRUE;


	//First Convert the Ip Address in the packet to Host Endian order
	for (uiIpv6AddIndex = 0; uiIpv6AddIndex<uiIpv6AddrNoLongWords; uiIpv6AddIndex++) {
		aulSrcIP[uiIpv6AddIndex]=ntohl(pstIpv6Header->ulSrcIpAddress[uiIpv6AddIndex]);
	}

	for (uiLoopIndex = 0; uiLoopIndex<uiCountIPSrcAddresses; uiLoopIndex += uiIpv6AddrNoLongWords) {
		BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, "\n Src Ipv6 Address In Received Packet : \n ");
		DumpIpv6Address(aulSrcIP);
		BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, "\n Src Ipv6 Mask In Classifier Rule: \n");
		DumpIpv6Address(&pstClassifierRule->stSrcIpAddress.ulIpv6Mask[uiLoopIndex]);
		BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, "\n Src Ipv6 Address In Classifier Rule : \n");
		DumpIpv6Address(&pstClassifierRule->stSrcIpAddress.ulIpv6Addr[uiLoopIndex]);

		for (uiIpv6AddIndex = 0; uiIpv6AddIndex < uiIpv6AddrNoLongWords; uiIpv6AddIndex++) {
			if ((pstClassifierRule->stSrcIpAddress.ulIpv6Mask[uiLoopIndex+uiIpv6AddIndex] & aulSrcIP[uiIpv6AddIndex])
				!= pstClassifierRule->stSrcIpAddress.ulIpv6Addr[uiLoopIndex+uiIpv6AddIndex]) {
				//Match failed for current Ipv6 Address.Try next Ipv6 Address
				break;
			}

			if (uiIpv6AddIndex ==  uiIpv6AddrNoLongWords-1) {
				//Match Found
				BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, "Ipv6 Src Ip Address Matched\n");
				return TRUE;
			}
		}
	}
	return FALSE;
}

static BOOLEAN MatchDestIpv6Address(S_CLASSIFIER_RULE *pstClassifierRule,IPV6Header *pstIpv6Header)
{
	UINT uiLoopIndex = 0;
	UINT uiIpv6AddIndex = 0;
	UINT uiIpv6AddrNoLongWords = 4;
	ULONG aulDestIP[4];
	PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(gblpnetdev);
	/*
	//This is the no. of Destination Addresses ie Range of IP Addresses contained
	//in the classifier rule for which we need to match
	*/
	UINT  uiCountIPDestinationAddresses = (UINT)pstClassifierRule->ucIPDestinationAddressLength;


	if (0 == uiCountIPDestinationAddresses)
		return TRUE;


	//First Convert the Ip Address in the packet to Host Endian order
	for (uiIpv6AddIndex = 0;uiIpv6AddIndex < uiIpv6AddrNoLongWords; uiIpv6AddIndex++) {
		aulDestIP[uiIpv6AddIndex]=ntohl(pstIpv6Header->ulDestIpAddress[uiIpv6AddIndex]);
	}

	for (uiLoopIndex = 0;uiLoopIndex < uiCountIPDestinationAddresses; uiLoopIndex += uiIpv6AddrNoLongWords) {
		BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, "\n Destination Ipv6 Address In Received Packet : \n ");
		DumpIpv6Address(aulDestIP);
		BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, "\n Destination Ipv6 Mask In Classifier Rule: \n");
		DumpIpv6Address(&pstClassifierRule->stDestIpAddress.ulIpv6Mask[uiLoopIndex]);
		BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, "\n Destination Ipv6 Address In Classifier Rule : \n");
		DumpIpv6Address(&pstClassifierRule->stDestIpAddress.ulIpv6Addr[uiLoopIndex]);

		for (uiIpv6AddIndex = 0; uiIpv6AddIndex < uiIpv6AddrNoLongWords; uiIpv6AddIndex++) {
			if ((pstClassifierRule->stDestIpAddress.ulIpv6Mask[uiLoopIndex+uiIpv6AddIndex] & aulDestIP[uiIpv6AddIndex])
				!= pstClassifierRule->stDestIpAddress.ulIpv6Addr[uiLoopIndex+uiIpv6AddIndex]) {
				//Match failed for current Ipv6 Address.Try next Ipv6 Address
				break;
			}

			if (uiIpv6AddIndex ==  uiIpv6AddrNoLongWords-1) {
				//Match Found
				BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, "Ipv6 Destination Ip Address Matched\n");
				return TRUE;
			}
		}
	}
	return FALSE;

}

VOID DumpIpv6Address(ULONG *puIpv6Address)
{
	UINT uiIpv6AddrNoLongWords = 4;
	UINT uiIpv6AddIndex = 0;
	PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(gblpnetdev);
	for (uiIpv6AddIndex = 0; uiIpv6AddIndex < uiIpv6AddrNoLongWords; uiIpv6AddIndex++) {
		BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, ":%lx",puIpv6Address[uiIpv6AddIndex]);
	}

}

static VOID DumpIpv6Header(IPV6Header *pstIpv6Header)
{
	UCHAR ucVersion;
	UCHAR ucPrio;
	PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(gblpnetdev);
	BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, "----Ipv6 Header---");
	ucVersion = pstIpv6Header->ucVersionPrio & 0xf0;
	BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, "Version : %x \n",ucVersion);
	ucPrio = pstIpv6Header->ucVersionPrio & 0x0f;
	BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, "Priority : %x \n",ucPrio);
	//BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, "Flow Label : %x \n",(pstIpv6Header->ucVersionPrio &0xf0);
	BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, "Payload Length : %x \n",ntohs(pstIpv6Header->usPayloadLength));
	BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, "Next Header : %x \n",pstIpv6Header->ucNextHeader);
	BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, "Hop Limit : %x \n",pstIpv6Header->ucHopLimit);
	BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, "Src Address :\n");
	DumpIpv6Address(pstIpv6Header->ulSrcIpAddress);
	BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, "Dest Address :\n");
	DumpIpv6Address(pstIpv6Header->ulDestIpAddress);
	BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL, "----Ipv6 Header End---");


}