Aquaris-M10-4G/drivers/staging/bcm/IPv6Protocol.c

#include "headers.h"

static bool MatchSrcIpv6Address(struct bcm_classifier_rule *pstClassifierRule,
	struct bcm_ipv6_hdr *pstIpv6Header);
static bool MatchDestIpv6Address(struct bcm_classifier_rule *pstClassifierRule,
	struct bcm_ipv6_hdr *pstIpv6Header);
static VOID DumpIpv6Header(struct bcm_ipv6_hdr *pstIpv6Header);

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

	if ((ppucPayload == NULL) || (*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(struct bcm_ipv6_options_hdr);
		break;

	case IPV6HDR_TYPE_ROUTING:
		{
			struct bcm_ipv6_routing_hdr *pstIpv6RoutingHeader;

			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG,
					DBG_LVL_ALL, "\nIPv6 Routing Header");
			pstIpv6RoutingHeader =
				(struct bcm_ipv6_routing_hdr *)pucPayloadPtr;
			usNextHeaderOffset += sizeof(struct bcm_ipv6_routing_hdr);
			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(struct bcm_ipv6_fragment_hdr);
		break;

	case IPV6HDR_TYPE_DESTOPTS:
		{
			struct bcm_ipv6_dest_options_hdr *pstIpv6DestOptsHdr =
				(struct bcm_ipv6_dest_options_hdr *)pucPayloadPtr;
			int nTotalOptions = pstIpv6DestOptsHdr->ucHdrExtLen;

			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG,
					DBG_LVL_ALL,
					"\nIPv6 DestOpts Header Header");
			usNextHeaderOffset += sizeof(struct bcm_ipv6_dest_options_hdr);
			usNextHeaderOffset += nTotalOptions *
					      IPV6_DESTOPTS_HDR_OPTIONSIZE;
		}
		break;


	case IPV6HDR_TYPE_AUTHENTICATION:
		{
			struct bcm_ipv6_authentication_hdr *pstIpv6AuthHdr =
				(struct bcm_ipv6_authentication_hdr *)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, "\nICMP 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;
	bool bDone = false;
	UCHAR ucHeaderType = 0;
	UCHAR *pucNextHeader = NULL;
	struct bcm_mini_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;
}


/*
 * Arg 1 struct bcm_mini_adapter *Adapter is a pointer ot the driver control
 * structure
 * Arg 2 PVOID pcIpHeader is a pointer to the IP header of the packet
 */
USHORT	IpVersion6(struct bcm_mini_adapter *Adapter, PVOID pcIpHeader,
		   struct bcm_classifier_rule *pstClassifierRule)
{
	USHORT	ushDestPort = 0;
	USHORT	ushSrcPort = 0;
	UCHAR   ucNextProtocolAboveIP = 0;
	struct bcm_ipv6_hdr *pstIpv6Header = NULL;
	bool bClassificationSucceed = false;

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

	pstIpv6Header = pcIpHeader;

	DumpIpv6Header(pstIpv6Header);

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

	do {
		if (pstClassifierRule->ucDirection == 0) {
			/*
			 * 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 (bClassificationSucceed == TRUE) {
		INT iMatchedSFQueueIndex = 0;

		iMatchedSFQueueIndex = SearchSfid(Adapter,
						  pstClassifierRule->ulSFID);
		if ((iMatchedSFQueueIndex >= NO_OF_QUEUES) ||
		    (Adapter->PackInfo[iMatchedSFQueueIndex].bActive == false))
			bClassificationSucceed = false;
	}

	return bClassificationSucceed;
}


static bool MatchSrcIpv6Address(struct bcm_classifier_rule *pstClassifierRule,
				struct bcm_ipv6_hdr *pstIpv6Header)
{
	UINT uiLoopIndex = 0;
	UINT uiIpv6AddIndex = 0;
	UINT uiIpv6AddrNoLongWords = 4;
	ULONG aulSrcIP[4];
	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
	union u_ip_address *src_addr = &pstClassifierRule->stSrcIpAddress;

	/*
	 * 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 (uiCountIPSrcAddresses == 0)
		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(&src_addr->ulIpv6Mask[uiLoopIndex]);
		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL,
				"\n Src Ipv6 Address In Classifier Rule :\n");
		DumpIpv6Address(&src_addr->ulIpv6Addr[uiLoopIndex]);

		for (uiIpv6AddIndex = 0;
		     uiIpv6AddIndex < uiIpv6AddrNoLongWords;
		     uiIpv6AddIndex++) {
			if ((src_addr->ulIpv6Mask[uiLoopIndex+uiIpv6AddIndex] &
				aulSrcIP[uiIpv6AddIndex]) !=
			    src_addr->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 bool MatchDestIpv6Address(struct bcm_classifier_rule *pstClassifierRule,
				 struct bcm_ipv6_hdr *pstIpv6Header)
{
	UINT uiLoopIndex = 0;
	UINT uiIpv6AddIndex = 0;
	UINT uiIpv6AddrNoLongWords = 4;
	ULONG aulDestIP[4];
	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
	union u_ip_address *dest_addr = &pstClassifierRule->stDestIpAddress;

	/*
	 * 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 (uiCountIPDestinationAddresses == 0)
		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(&dest_addr->ulIpv6Mask[uiLoopIndex]);
		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL,
				"\n Destination Ipv6 Address In Classifier Rule :\n");
		DumpIpv6Address(&dest_addr->ulIpv6Addr[uiLoopIndex]);

		for (uiIpv6AddIndex = 0;
		     uiIpv6AddIndex < uiIpv6AddrNoLongWords;
		     uiIpv6AddIndex++) {
			if ((dest_addr->ulIpv6Mask[uiLoopIndex+uiIpv6AddIndex] &
				aulDestIP[uiIpv6AddIndex]) !=
			    dest_addr->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;
	struct bcm_mini_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(struct bcm_ipv6_hdr *pstIpv6Header)
{
	UCHAR ucVersion;
	UCHAR ucPrio;
	struct bcm_mini_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---");


}