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

/**
 * @file Qos.C
 * This file contains the routines related to Quality of Service.
*/
#include "headers.h"

static void EThCSGetPktInfo(struct bcm_mini_adapter *Adapter,
			    PVOID pvEthPayload,
			    struct bcm_eth_packet_info *pstEthCsPktInfo);

static bool EThCSClassifyPkt(struct bcm_mini_adapter *Adapter,
			     struct sk_buff *skb,
			     struct bcm_eth_packet_info *pstEthCsPktInfo,
			     struct bcm_classifier_rule *pstClassifierRule,
			     B_UINT8 EthCSCupport);

static USHORT IpVersion4(struct bcm_mini_adapter *Adapter, struct iphdr *iphd,
			 struct bcm_classifier_rule *pstClassifierRule);

static VOID PruneQueue(struct bcm_mini_adapter *Adapter, INT iIndex);


/*******************************************************************
* Function    - MatchSrcIpAddress()
*
* Description - Checks whether the Source IP address from the packet
*				matches with that of Queue.
*
* Parameters  - pstClassifierRule: Pointer to the packet info structure.
*		- ulSrcIP	    : Source IP address from the packet.
*
* Returns     - TRUE(If address matches) else FAIL .
*********************************************************************/
static bool MatchSrcIpAddress(struct bcm_classifier_rule *pstClassifierRule,
			      ULONG ulSrcIP)
{
	UCHAR ucLoopIndex = 0;

	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
	union u_ip_address	*src_addr;

	ulSrcIP = ntohl(ulSrcIP);
	if (0 == pstClassifierRule->ucIPSourceAddressLength)
		return TRUE;
	for (ucLoopIndex = 0;
	     ucLoopIndex < (pstClassifierRule->ucIPSourceAddressLength);
	     ucLoopIndex++) {
		src_addr = &pstClassifierRule->stSrcIpAddress;
		BCM_DEBUG_PRINT(Adapter,
				DBG_TYPE_TX,
				IPV4_DBG,
				DBG_LVL_ALL,
				"Src Ip Address Mask:0x%x PacketIp:0x%x and Classification:0x%x",
				(UINT)src_addr->ulIpv4Mask[ucLoopIndex],
				(UINT)ulSrcIP,
				(UINT)src_addr->ulIpv6Addr[ucLoopIndex]);

		if ((src_addr->ulIpv4Mask[ucLoopIndex] & ulSrcIP) ==
				(src_addr->ulIpv4Addr[ucLoopIndex] &
				 src_addr->ulIpv4Mask[ucLoopIndex]))
			return TRUE;
	}
	BCM_DEBUG_PRINT(Adapter,
			DBG_TYPE_TX,
			IPV4_DBG,
			DBG_LVL_ALL,
			"Src Ip Address Not Matched");
	return false;
}


/*******************************************************************
* Function    - MatchDestIpAddress()
*
* Description - Checks whether the Destination IP address from the packet
*				matches with that of Queue.
*
* Parameters  - pstClassifierRule: Pointer to the packet info structure.
*		- ulDestIP    : Destination IP address from the packet.
*
* Returns     - TRUE(If address matches) else FAIL .
*********************************************************************/
static bool MatchDestIpAddress(struct bcm_classifier_rule *pstClassifierRule, ULONG ulDestIP)
{
	UCHAR ucLoopIndex = 0;
	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
	union u_ip_address	*dest_addr = &pstClassifierRule->stDestIpAddress;

	ulDestIP = ntohl(ulDestIP);
	if (0 == pstClassifierRule->ucIPDestinationAddressLength)
		return TRUE;
	BCM_DEBUG_PRINT(Adapter,
			DBG_TYPE_TX,
			IPV4_DBG,
			DBG_LVL_ALL,
			"Destination Ip Address 0x%x 0x%x 0x%x  ",
			(UINT)ulDestIP,
			(UINT)dest_addr->ulIpv4Mask[ucLoopIndex],
			(UINT)dest_addr->ulIpv4Addr[ucLoopIndex]);

	for (ucLoopIndex = 0;
	     ucLoopIndex < (pstClassifierRule->ucIPDestinationAddressLength);
	     ucLoopIndex++) {
		if ((dest_addr->ulIpv4Mask[ucLoopIndex] & ulDestIP) ==
				(dest_addr->ulIpv4Addr[ucLoopIndex] &
				 dest_addr->ulIpv4Mask[ucLoopIndex]))
			return TRUE;
	}
	BCM_DEBUG_PRINT(Adapter,
			DBG_TYPE_TX,
			IPV4_DBG,
			DBG_LVL_ALL,
			"Destination Ip Address Not Matched");
	return false;
}


/************************************************************************
* Function    - MatchTos()
*
* Description - Checks the TOS from the packet matches with that of queue.
*
* Parameters  - pstClassifierRule   : Pointer to the packet info structure.
*		- ucTypeOfService: TOS from the packet.
*
* Returns     - TRUE(If address matches) else FAIL.
**************************************************************************/
static bool MatchTos(struct bcm_classifier_rule *pstClassifierRule,
		     UCHAR ucTypeOfService)
{
	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);

	if (3 != pstClassifierRule->ucIPTypeOfServiceLength)
		return TRUE;

	if (((pstClassifierRule->ucTosMask & ucTypeOfService) <=
				pstClassifierRule->ucTosHigh) &&
			((pstClassifierRule->ucTosMask & ucTypeOfService) >=
				pstClassifierRule->ucTosLow))
		return TRUE;

	BCM_DEBUG_PRINT(Adapter,
			DBG_TYPE_TX,
			IPV4_DBG,
			DBG_LVL_ALL,
			"Type Of Service Not Matched");
	return false;
}


/***************************************************************************
* Function    - MatchProtocol()
*
* Description - Checks the protocol from the packet matches with that of queue.
*
* Parameters  - pstClassifierRule: Pointer to the packet info structure.
*		- ucProtocol	: Protocol from the packet.
*
* Returns     - TRUE(If address matches) else FAIL.
****************************************************************************/
bool MatchProtocol(struct bcm_classifier_rule *pstClassifierRule,
		   UCHAR ucProtocol)
{
	UCHAR ucLoopIndex = 0;
	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);

	if (0 == pstClassifierRule->ucProtocolLength)
		return TRUE;
	for (ucLoopIndex = 0;
	     ucLoopIndex < pstClassifierRule->ucProtocolLength;
	     ucLoopIndex++) {
		BCM_DEBUG_PRINT(Adapter,
				DBG_TYPE_TX,
				IPV4_DBG,
				DBG_LVL_ALL,
				"Protocol:0x%X Classification Protocol:0x%X",
				ucProtocol,
				pstClassifierRule->ucProtocol[ucLoopIndex]);
		if (pstClassifierRule->ucProtocol[ucLoopIndex] == ucProtocol)
			return TRUE;
	}
	BCM_DEBUG_PRINT(Adapter,
			DBG_TYPE_TX,
			IPV4_DBG,
			DBG_LVL_ALL,
			"Protocol Not Matched");
	return false;
}


/***********************************************************************
* Function    - MatchSrcPort()
*
* Description - Checks, Source port from the packet matches with that of queue.
*
* Parameters  - pstClassifierRule: Pointer to the packet info structure.
*		- ushSrcPort	: Source port from the packet.
*
* Returns     - TRUE(If address matches) else FAIL.
***************************************************************************/
bool MatchSrcPort(struct bcm_classifier_rule *pstClassifierRule,
		  USHORT ushSrcPort)
{
	UCHAR ucLoopIndex = 0;

	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);


	if (0 == pstClassifierRule->ucSrcPortRangeLength)
		return TRUE;
	for (ucLoopIndex = 0;
	     ucLoopIndex < pstClassifierRule->ucSrcPortRangeLength;
	     ucLoopIndex++) {
		if (ushSrcPort <= pstClassifierRule->usSrcPortRangeHi[ucLoopIndex] &&
			ushSrcPort >= pstClassifierRule->usSrcPortRangeLo[ucLoopIndex])
			return TRUE;
	}
	BCM_DEBUG_PRINT(Adapter,
			DBG_TYPE_TX,
			IPV4_DBG,
			DBG_LVL_ALL,
			"Src Port: %x Not Matched ",
			ushSrcPort);
	return false;
}


/***********************************************************************
* Function    - MatchDestPort()
*
* Description - Checks, Destination port from packet matches with that of queue.
*
* Parameters  - pstClassifierRule: Pointer to the packet info structure.
*		- ushDestPort	: Destination port from the packet.
*
* Returns     - TRUE(If address matches) else FAIL.
***************************************************************************/
bool MatchDestPort(struct bcm_classifier_rule *pstClassifierRule,
		   USHORT ushDestPort)
{
	UCHAR ucLoopIndex = 0;
	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);

	if (0 == pstClassifierRule->ucDestPortRangeLength)
		return TRUE;

	for (ucLoopIndex = 0;
	     ucLoopIndex < pstClassifierRule->ucDestPortRangeLength;
	     ucLoopIndex++) {
		BCM_DEBUG_PRINT(Adapter,
				DBG_TYPE_TX,
				IPV4_DBG,
				DBG_LVL_ALL,
				"Matching Port:0x%X   0x%X  0x%X",
				ushDestPort,
				pstClassifierRule->usDestPortRangeLo[ucLoopIndex],
				pstClassifierRule->usDestPortRangeHi[ucLoopIndex]);

		if (ushDestPort <= pstClassifierRule->usDestPortRangeHi[ucLoopIndex] &&
			ushDestPort >= pstClassifierRule->usDestPortRangeLo[ucLoopIndex])
			return TRUE;
	}
	BCM_DEBUG_PRINT(Adapter,
			DBG_TYPE_TX,
			IPV4_DBG,
			DBG_LVL_ALL,
			"Dest Port: %x Not Matched",
			ushDestPort);
	return false;
}
/**
 * @ingroup tx_functions
 * Compares IPV4 Ip address and port number
 * @return Queue Index.
*/
static USHORT	IpVersion4(struct bcm_mini_adapter *Adapter,
			   struct iphdr *iphd,
			   struct bcm_classifier_rule *pstClassifierRule)
{
	struct bcm_transport_header *xprt_hdr = NULL;
	bool	bClassificationSucceed = false;

	BCM_DEBUG_PRINT(Adapter,
			DBG_TYPE_TX,
			IPV4_DBG,
			DBG_LVL_ALL,
			"========>");

	xprt_hdr = (struct bcm_transport_header *)((PUCHAR)iphd + sizeof(struct iphdr));

	BCM_DEBUG_PRINT(Adapter,
			DBG_TYPE_TX,
			IPV4_DBG,
			DBG_LVL_ALL,
			"Trying to see Direction = %d %d",
			pstClassifierRule->ucDirection,
			pstClassifierRule->usVCID_Value);

	/* Checking classifier validity */
	if (!pstClassifierRule->bUsed ||
			pstClassifierRule->ucDirection == DOWNLINK_DIR)
		goto out;

	BCM_DEBUG_PRINT(Adapter,
			DBG_TYPE_TX,
			IPV4_DBG,
			DBG_LVL_ALL,
			"is IPv6 check!");
	if (pstClassifierRule->bIpv6Protocol)
		goto out;

	/* Checking IP header parameter */
	BCM_DEBUG_PRINT(Adapter,
			DBG_TYPE_TX,
			IPV4_DBG,
			DBG_LVL_ALL,
			"Trying to match Source IP Address");
	if (!MatchSrcIpAddress(pstClassifierRule, iphd->saddr))
		goto out;
	BCM_DEBUG_PRINT(Adapter,
			DBG_TYPE_TX,
			IPV4_DBG,
			DBG_LVL_ALL,
			"Source IP Address Matched");

	if (!MatchDestIpAddress(pstClassifierRule, iphd->daddr))
		goto out;
	BCM_DEBUG_PRINT(Adapter,
			DBG_TYPE_TX,
			IPV4_DBG,
			DBG_LVL_ALL,
			"Destination IP Address Matched");

	if (!MatchTos(pstClassifierRule, iphd->tos)) {
		BCM_DEBUG_PRINT(Adapter,
				DBG_TYPE_TX,
				IPV4_DBG,
				DBG_LVL_ALL,
				"TOS Match failed\n");
		goto out;
	}
	BCM_DEBUG_PRINT(Adapter,
			DBG_TYPE_TX,
			IPV4_DBG,
			DBG_LVL_ALL,
			"TOS Matched");

	if (!MatchProtocol(pstClassifierRule, iphd->protocol))
		goto out;
	BCM_DEBUG_PRINT(Adapter,
			DBG_TYPE_TX,
			IPV4_DBG,
			DBG_LVL_ALL,
			"Protocol Matched");

	/*
	 * if protocol is not TCP or UDP then no
	 * need of comparing source port and destination port
	 */
	if (iphd->protocol != TCP && iphd->protocol != UDP) {
		bClassificationSucceed = TRUE;
		goto out;
	}
	/* Checking Transport Layer Header field if present */
	BCM_DEBUG_PRINT(Adapter,
			DBG_TYPE_TX,
			IPV4_DBG,
			DBG_LVL_ALL,
			"Source Port %04x",
			(iphd->protocol == UDP) ? xprt_hdr->uhdr.source : xprt_hdr->thdr.source);

	if (!MatchSrcPort(pstClassifierRule,
			  ntohs((iphd->protocol == UDP) ?
			  xprt_hdr->uhdr.source : xprt_hdr->thdr.source)))
		goto out;
	BCM_DEBUG_PRINT(Adapter,
			DBG_TYPE_TX,
			IPV4_DBG,
			DBG_LVL_ALL,
			"Src Port Matched");

	BCM_DEBUG_PRINT(Adapter,
			DBG_TYPE_TX,
			IPV4_DBG,
			DBG_LVL_ALL,
			"Destination Port %04x",
			(iphd->protocol == UDP) ? xprt_hdr->uhdr.dest :
				xprt_hdr->thdr.dest);

	if (!MatchDestPort(pstClassifierRule,
			   ntohs((iphd->protocol == UDP) ?
			   xprt_hdr->uhdr.dest : xprt_hdr->thdr.dest)))
		goto out;
	bClassificationSucceed = TRUE;

out:
	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;
	}

	BCM_DEBUG_PRINT(Adapter,
			DBG_TYPE_TX,
			IPV4_DBG,
			DBG_LVL_ALL,
			"IpVersion4 <==========");

	return bClassificationSucceed;
}

VOID PruneQueueAllSF(struct bcm_mini_adapter *Adapter)
{
	UINT iIndex = 0;

	for (iIndex = 0; iIndex < HiPriority; iIndex++) {
		if (!Adapter->PackInfo[iIndex].bValid)
			continue;

		PruneQueue(Adapter, iIndex);
	}
}


/**
 * @ingroup tx_functions
 * This function checks if the max queue size for a queue
 * is less than number of bytes in the queue. If so -
 * drops packets from the Head till the number of bytes is
 * less than or equal to max queue size for the queue.
 */
static VOID PruneQueue(struct bcm_mini_adapter *Adapter, INT iIndex)
{
	struct sk_buff *PacketToDrop = NULL;
	struct net_device_stats *netstats;
	struct bcm_packet_info	*curr_pack_info = &Adapter->PackInfo[iIndex];

	BCM_DEBUG_PRINT(Adapter,
			DBG_TYPE_TX,
			PRUNE_QUEUE,
			DBG_LVL_ALL,
			"=====> Index %d",
			iIndex);

	if (iIndex == HiPriority)
		return;

	if (!Adapter || (iIndex < 0) || (iIndex > HiPriority))
		return;

	/* To Store the netdevice statistic */
	netstats = &Adapter->dev->stats;

	spin_lock_bh(&curr_pack_info->SFQueueLock);

	while (1) {
/*	while((UINT)curr_pack_info->uiCurrentPacketsOnHost >
		SF_MAX_ALLOWED_PACKETS_TO_BACKUP) { */

		BCM_DEBUG_PRINT(Adapter,
				DBG_TYPE_TX,
				PRUNE_QUEUE,
				DBG_LVL_ALL,
				"uiCurrentBytesOnHost:%x uiMaxBucketSize :%x",
				curr_pack_info->uiCurrentBytesOnHost,
				curr_pack_info->uiMaxBucketSize);

		PacketToDrop = curr_pack_info->FirstTxQueue;

		if (PacketToDrop == NULL)
			break;
		if ((curr_pack_info->uiCurrentPacketsOnHost <
					SF_MAX_ALLOWED_PACKETS_TO_BACKUP) &&
			((1000*(jiffies - *((B_UINT32 *)(PacketToDrop->cb) +
					    SKB_CB_LATENCY_OFFSET))/HZ) <=
				curr_pack_info->uiMaxLatency))
			break;

		if (PacketToDrop) {
			if (netif_msg_tx_err(Adapter))
				pr_info(PFX "%s: tx queue %d overlimit\n",
					Adapter->dev->name, iIndex);

			netstats->tx_dropped++;

			DEQUEUEPACKET(curr_pack_info->FirstTxQueue,
				      curr_pack_info->LastTxQueue);
			/* update current bytes and packets count */
			curr_pack_info->uiCurrentBytesOnHost -=
				PacketToDrop->len;
			curr_pack_info->uiCurrentPacketsOnHost--;
			/* update dropped bytes and packets counts */
			curr_pack_info->uiDroppedCountBytes += PacketToDrop->len;
			curr_pack_info->uiDroppedCountPackets++;
			dev_kfree_skb(PacketToDrop);

		}

		BCM_DEBUG_PRINT(Adapter,
				DBG_TYPE_TX,
				PRUNE_QUEUE,
				DBG_LVL_ALL,
				"Dropped Bytes:%x Dropped Packets:%x",
				curr_pack_info->uiDroppedCountBytes,
				curr_pack_info->uiDroppedCountPackets);

		atomic_dec(&Adapter->TotalPacketCount);
	}

	spin_unlock_bh(&curr_pack_info->SFQueueLock);

	BCM_DEBUG_PRINT(Adapter,
			DBG_TYPE_TX,
			PRUNE_QUEUE,
			DBG_LVL_ALL,
			"TotalPacketCount:%x",
			atomic_read(&Adapter->TotalPacketCount));
	BCM_DEBUG_PRINT(Adapter,
			DBG_TYPE_TX,
			PRUNE_QUEUE,
			DBG_LVL_ALL,
			"<=====");
}

VOID flush_all_queues(struct bcm_mini_adapter *Adapter)
{
	INT	iQIndex;
	UINT uiTotalPacketLength;
	struct sk_buff *PacketToDrop = NULL;
	struct bcm_packet_info *curr_packet_info;

	BCM_DEBUG_PRINT(Adapter,
			DBG_TYPE_OTHERS,
			DUMP_INFO,
			DBG_LVL_ALL,
			"=====>");

	/* down(&Adapter->data_packet_queue_lock); */
	for (iQIndex = LowPriority; iQIndex < HiPriority; iQIndex++) {
		struct net_device_stats *netstats = &Adapter->dev->stats;

		curr_packet_info = &Adapter->PackInfo[iQIndex];

		spin_lock_bh(&curr_packet_info->SFQueueLock);
		while (curr_packet_info->FirstTxQueue) {
			PacketToDrop = curr_packet_info->FirstTxQueue;
			if (PacketToDrop) {
				uiTotalPacketLength = PacketToDrop->len;
				netstats->tx_dropped++;
			} else
				uiTotalPacketLength = 0;

			DEQUEUEPACKET(curr_packet_info->FirstTxQueue,
				      curr_packet_info->LastTxQueue);

			/* Free the skb */
			dev_kfree_skb(PacketToDrop);

			/* update current bytes and packets count */
			curr_packet_info->uiCurrentBytesOnHost -= uiTotalPacketLength;
			curr_packet_info->uiCurrentPacketsOnHost--;

			/* update dropped bytes and packets counts */
			curr_packet_info->uiDroppedCountBytes += uiTotalPacketLength;
			curr_packet_info->uiDroppedCountPackets++;

			BCM_DEBUG_PRINT(Adapter,
					DBG_TYPE_OTHERS,
					DUMP_INFO,
					DBG_LVL_ALL,
					"Dropped Bytes:%x Dropped Packets:%x",
					curr_packet_info->uiDroppedCountBytes,
					curr_packet_info->uiDroppedCountPackets);
			atomic_dec(&Adapter->TotalPacketCount);
		}
		spin_unlock_bh(&curr_packet_info->SFQueueLock);
	}
	/* up(&Adapter->data_packet_queue_lock); */
	BCM_DEBUG_PRINT(Adapter,
			DBG_TYPE_OTHERS,
			DUMP_INFO,
			DBG_LVL_ALL,
			"<=====");
}

USHORT ClassifyPacket(struct bcm_mini_adapter *Adapter, struct sk_buff *skb)
{
	INT uiLoopIndex = 0;
	struct bcm_classifier_rule *pstClassifierRule = NULL;
	struct bcm_eth_packet_info stEthCsPktInfo;
	PVOID pvEThPayload = NULL;
	struct iphdr *pIpHeader = NULL;
	INT uiSfIndex = 0;
	USHORT usIndex = Adapter->usBestEffortQueueIndex;
	bool bFragmentedPkt = false, bClassificationSucceed = false;
	USHORT usCurrFragment = 0;

	struct bcm_tcp_header *pTcpHeader;
	UCHAR IpHeaderLength;
	UCHAR TcpHeaderLength;

	pvEThPayload = skb->data;
	*((UINT32 *) (skb->cb) + SKB_CB_TCPACK_OFFSET) = 0;
	EThCSGetPktInfo(Adapter, pvEThPayload, &stEthCsPktInfo);

	switch (stEthCsPktInfo.eNwpktEthFrameType) {
	case eEth802LLCFrame:
		BCM_DEBUG_PRINT(Adapter,
				DBG_TYPE_TX,
				IPV4_DBG,
				DBG_LVL_ALL,
				"ClassifyPacket : 802LLCFrame\n");
		pIpHeader = pvEThPayload + sizeof(struct bcm_eth_llc_frame);
		break;
	case eEth802LLCSNAPFrame:
		BCM_DEBUG_PRINT(Adapter,
				DBG_TYPE_TX,
				IPV4_DBG,
				DBG_LVL_ALL,
				"ClassifyPacket : 802LLC SNAP Frame\n");
		pIpHeader = pvEThPayload +
			sizeof(struct bcm_eth_llc_snap_frame);
		break;
	case eEth802QVLANFrame:
		BCM_DEBUG_PRINT(Adapter,
				DBG_TYPE_TX,
				IPV4_DBG,
				DBG_LVL_ALL,
				"ClassifyPacket : 802.1Q VLANFrame\n");
		pIpHeader = pvEThPayload + sizeof(struct bcm_eth_q_frame);
		break;
	case eEthOtherFrame:
		BCM_DEBUG_PRINT(Adapter,
				DBG_TYPE_TX,
				IPV4_DBG,
				DBG_LVL_ALL,
				"ClassifyPacket : ETH Other Frame\n");
		pIpHeader = pvEThPayload + sizeof(struct bcm_ethernet2_frame);
		break;
	default:
		BCM_DEBUG_PRINT(Adapter,
				DBG_TYPE_TX,
				IPV4_DBG,
				DBG_LVL_ALL,
				"ClassifyPacket : Unrecognized ETH Frame\n");
		pIpHeader = pvEThPayload + sizeof(struct bcm_ethernet2_frame);
		break;
	}

	if (stEthCsPktInfo.eNwpktIPFrameType == eIPv4Packet) {
		usCurrFragment = (ntohs(pIpHeader->frag_off) & IP_OFFSET);
		if ((ntohs(pIpHeader->frag_off) & IP_MF) || usCurrFragment)
			bFragmentedPkt = TRUE;

		if (bFragmentedPkt) {
			/* Fragmented  Packet. Get Frag Classifier Entry. */
			pstClassifierRule = GetFragIPClsEntry(Adapter,
							      pIpHeader->id,
							      pIpHeader->saddr);
			if (pstClassifierRule) {
					BCM_DEBUG_PRINT(Adapter,
							DBG_TYPE_TX,
							IPV4_DBG,
							DBG_LVL_ALL,
							"It is next Fragmented pkt");
					bClassificationSucceed = TRUE;
			}
			if (!(ntohs(pIpHeader->frag_off) & IP_MF)) {
				/* Fragmented Last packet . Remove Frag Classifier Entry */
				BCM_DEBUG_PRINT(Adapter,
						DBG_TYPE_TX,
						IPV4_DBG,
						DBG_LVL_ALL,
						"This is the last fragmented Pkt");
				DelFragIPClsEntry(Adapter,
						  pIpHeader->id,
						  pIpHeader->saddr);
			}
		}
	}

	for (uiLoopIndex = MAX_CLASSIFIERS - 1; uiLoopIndex >= 0; uiLoopIndex--) {
		if (bClassificationSucceed)
			break;
		/*
		 * Iterate through all classifiers which are already in order of priority
		 * to classify the packet until match found
		 */
		if (false == Adapter->astClassifierTable[uiLoopIndex].bUsed) {
			bClassificationSucceed = false;
			continue;
		}
		BCM_DEBUG_PRINT(Adapter,
				DBG_TYPE_TX,
				IPV4_DBG,
				DBG_LVL_ALL,
				"Adapter->PackInfo[%d].bvalid=True\n",
				uiLoopIndex);

		if (0 == Adapter->astClassifierTable[uiLoopIndex].ucDirection) {
			bClassificationSucceed = false; /* cannot be processed for classification. */
			continue;	/* it is a down link connection */
		}

		pstClassifierRule = &Adapter->astClassifierTable[uiLoopIndex];

		uiSfIndex = SearchSfid(Adapter, pstClassifierRule->ulSFID);
		if (uiSfIndex >= NO_OF_QUEUES) {
			BCM_DEBUG_PRINT(Adapter,
					DBG_TYPE_TX,
					IPV4_DBG,
					DBG_LVL_ALL,
					"Queue Not Valid. SearchSfid for this classifier Failed\n");
			continue;
		}

		if (Adapter->PackInfo[uiSfIndex].bEthCSSupport) {

			if (eEthUnsupportedFrame == stEthCsPktInfo.eNwpktEthFrameType) {
				BCM_DEBUG_PRINT(Adapter,
						DBG_TYPE_TX,
						IPV4_DBG,
						DBG_LVL_ALL,
						" ClassifyPacket : Packet Not a Valid Supported Ethernet Frame\n");
				bClassificationSucceed = false;
				continue;
			}



			BCM_DEBUG_PRINT(Adapter,
					DBG_TYPE_TX,
					IPV4_DBG,
					DBG_LVL_ALL,
					"Performing ETH CS Classification on Classifier Rule ID : %x Service Flow ID : %lx\n",
					pstClassifierRule->uiClassifierRuleIndex,
					Adapter->PackInfo[uiSfIndex].ulSFID);
			bClassificationSucceed = EThCSClassifyPkt(Adapter,
								  skb,
								  &stEthCsPktInfo,
								  pstClassifierRule,
								  Adapter->PackInfo[uiSfIndex].bEthCSSupport);

			if (!bClassificationSucceed) {
				BCM_DEBUG_PRINT(Adapter,
						DBG_TYPE_TX,
						IPV4_DBG,
						DBG_LVL_ALL,
						"ClassifyPacket : Ethernet CS Classification Failed\n");
				continue;
			}
		} else { /* No ETH Supported on this SF */
			if (eEthOtherFrame != stEthCsPktInfo.eNwpktEthFrameType) {
				BCM_DEBUG_PRINT(Adapter,
						DBG_TYPE_TX,
						IPV4_DBG,
						DBG_LVL_ALL,
						" ClassifyPacket : Packet Not a 802.3 Ethernet Frame... hence not allowed over non-ETH CS SF\n");
				bClassificationSucceed = false;
				continue;
			}
		}

		BCM_DEBUG_PRINT(Adapter,
				DBG_TYPE_TX,
				IPV4_DBG,
				DBG_LVL_ALL,
				"Proceeding to IP CS Clasification");

		if (Adapter->PackInfo[uiSfIndex].bIPCSSupport) {

			if (stEthCsPktInfo.eNwpktIPFrameType == eNonIPPacket) {
				BCM_DEBUG_PRINT(Adapter,
						DBG_TYPE_TX,
						IPV4_DBG,
						DBG_LVL_ALL,
						" ClassifyPacket : Packet is Not an IP Packet\n");
				bClassificationSucceed = false;
				continue;
			}
			BCM_DEBUG_PRINT(Adapter,
					DBG_TYPE_TX,
					IPV4_DBG,
					DBG_LVL_ALL,
					"Dump IP Header :\n");
			DumpFullPacket((PUCHAR)pIpHeader, 20);

			if (stEthCsPktInfo.eNwpktIPFrameType == eIPv4Packet)
				bClassificationSucceed = IpVersion4(Adapter,
								    pIpHeader,
								    pstClassifierRule);
			else if (stEthCsPktInfo.eNwpktIPFrameType == eIPv6Packet)
				bClassificationSucceed = IpVersion6(Adapter,
								    pIpHeader,
								    pstClassifierRule);
		}
	}

	if (bClassificationSucceed == TRUE) {
		BCM_DEBUG_PRINT(Adapter,
				DBG_TYPE_TX,
				IPV4_DBG,
				DBG_LVL_ALL,
				"CF id : %d, SF ID is =%lu",
				pstClassifierRule->uiClassifierRuleIndex,
				pstClassifierRule->ulSFID);

		/* Store The matched Classifier in SKB */
		*((UINT32 *)(skb->cb)+SKB_CB_CLASSIFICATION_OFFSET) =
			pstClassifierRule->uiClassifierRuleIndex;
		if ((TCP == pIpHeader->protocol) && !bFragmentedPkt &&
				(ETH_AND_IP_HEADER_LEN + TCP_HEADER_LEN <=
					skb->len)) {
			IpHeaderLength = pIpHeader->ihl;
			pTcpHeader =
				(struct bcm_tcp_header *)(((PUCHAR)pIpHeader) +
						(IpHeaderLength*4));
			TcpHeaderLength = GET_TCP_HEADER_LEN(pTcpHeader->HeaderLength);

			if ((pTcpHeader->ucFlags & TCP_ACK) &&
				   (ntohs(pIpHeader->tot_len) ==
				    (IpHeaderLength*4)+(TcpHeaderLength*4)))
				*((UINT32 *) (skb->cb) + SKB_CB_TCPACK_OFFSET) =
					TCP_ACK;
		}

		usIndex = SearchSfid(Adapter, pstClassifierRule->ulSFID);
		BCM_DEBUG_PRINT(Adapter,
				DBG_TYPE_TX,
				IPV4_DBG,
				DBG_LVL_ALL,
				"index is =%d",
				usIndex);

		/*
		 * If this is the first fragment of a Fragmented pkt,
		 * add this CF. Only This CF should be used for all other
		 * fragment of this Pkt.
		 */
		if (bFragmentedPkt && (usCurrFragment == 0)) {
			/*
			 * First Fragment of Fragmented Packet.
			 * Create Frag CLS Entry
			 */
			struct bcm_fragmented_packet_info stFragPktInfo;

			stFragPktInfo.bUsed = TRUE;
			stFragPktInfo.ulSrcIpAddress = pIpHeader->saddr;
			stFragPktInfo.usIpIdentification = pIpHeader->id;
			stFragPktInfo.pstMatchedClassifierEntry =
				pstClassifierRule;
			stFragPktInfo.bOutOfOrderFragment = false;
			AddFragIPClsEntry(Adapter, &stFragPktInfo);
		}


	}

	return bClassificationSucceed ? usIndex : INVALID_QUEUE_INDEX;
}

static bool EthCSMatchSrcMACAddress(struct bcm_classifier_rule *pstClassifierRule,
				    PUCHAR Mac)
{
	UINT i = 0;
	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);

	if (pstClassifierRule->ucEthCSSrcMACLen == 0)
		return TRUE;
	BCM_DEBUG_PRINT(Adapter,
			DBG_TYPE_TX,
			IPV4_DBG,
			DBG_LVL_ALL,
			"%s\n", __func__);
	for (i = 0; i < MAC_ADDRESS_SIZE; i++) {
		BCM_DEBUG_PRINT(Adapter,
				DBG_TYPE_TX,
				IPV4_DBG,
				DBG_LVL_ALL,
				"SRC MAC[%x] = %x ClassifierRuleSrcMAC = %x Mask : %x\n",
				i,
				Mac[i],
				pstClassifierRule->au8EThCSSrcMAC[i],
				pstClassifierRule->au8EThCSSrcMACMask[i]);
		if ((pstClassifierRule->au8EThCSSrcMAC[i] &
					pstClassifierRule->au8EThCSSrcMACMask[i]) !=
				(Mac[i] & pstClassifierRule->au8EThCSSrcMACMask[i]))
			return false;
	}
	return TRUE;
}

static bool EthCSMatchDestMACAddress(struct bcm_classifier_rule *pstClassifierRule,
				     PUCHAR Mac)
{
	UINT i = 0;
	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);

	if (pstClassifierRule->ucEthCSDestMACLen == 0)
		return TRUE;
	BCM_DEBUG_PRINT(Adapter,
			DBG_TYPE_TX,
			IPV4_DBG,
			DBG_LVL_ALL,
			"%s\n",
			__func__);
	for (i = 0; i < MAC_ADDRESS_SIZE; i++) {
		BCM_DEBUG_PRINT(Adapter,
				DBG_TYPE_TX,
				IPV4_DBG,
				DBG_LVL_ALL,
				"SRC MAC[%x] = %x ClassifierRuleSrcMAC = %x Mask : %x\n",
				i,
				Mac[i],
				pstClassifierRule->au8EThCSDestMAC[i],
				pstClassifierRule->au8EThCSDestMACMask[i]);
		if ((pstClassifierRule->au8EThCSDestMAC[i] &
					pstClassifierRule->au8EThCSDestMACMask[i]) !=
				(Mac[i] & pstClassifierRule->au8EThCSDestMACMask[i]))
			return false;
	}
	return TRUE;
}

static bool EthCSMatchEThTypeSAP(struct bcm_classifier_rule *pstClassifierRule,
				 struct sk_buff *skb,
				 struct bcm_eth_packet_info *pstEthCsPktInfo)
{
	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);

	if ((pstClassifierRule->ucEtherTypeLen == 0) ||
		(pstClassifierRule->au8EthCSEtherType[0] == 0))
		return TRUE;

	BCM_DEBUG_PRINT(Adapter,
			DBG_TYPE_TX,
			IPV4_DBG,
			DBG_LVL_ALL,
			"%s SrcEtherType:%x CLS EtherType[0]:%x\n",
			__func__,
			pstEthCsPktInfo->usEtherType,
			pstClassifierRule->au8EthCSEtherType[0]);
	if (pstClassifierRule->au8EthCSEtherType[0] == 1) {
		BCM_DEBUG_PRINT(Adapter,
				DBG_TYPE_TX,
				IPV4_DBG,
				DBG_LVL_ALL,
				"%s  CLS EtherType[1]:%x EtherType[2]:%x\n",
				__func__,
				pstClassifierRule->au8EthCSEtherType[1],
				pstClassifierRule->au8EthCSEtherType[2]);

		if (memcmp(&pstEthCsPktInfo->usEtherType,
			   &pstClassifierRule->au8EthCSEtherType[1],
			   2) == 0)
			return TRUE;
		else
			return false;
	}

	if (pstClassifierRule->au8EthCSEtherType[0] == 2) {
		if (eEth802LLCFrame != pstEthCsPktInfo->eNwpktEthFrameType)
			return false;

		BCM_DEBUG_PRINT(Adapter,
				DBG_TYPE_TX,
				IPV4_DBG,
				DBG_LVL_ALL,
				"%s  EthCS DSAP:%x EtherType[2]:%x\n",
				__func__,
				pstEthCsPktInfo->ucDSAP,
				pstClassifierRule->au8EthCSEtherType[2]);
		if (pstEthCsPktInfo->ucDSAP ==
				pstClassifierRule->au8EthCSEtherType[2])
			return TRUE;
		else
			return false;

	}

	return false;

}

static bool EthCSMatchVLANRules(struct bcm_classifier_rule *pstClassifierRule,
				struct sk_buff *skb,
				struct bcm_eth_packet_info *pstEthCsPktInfo)
{
	bool bClassificationSucceed = false;
	USHORT usVLANID;
	B_UINT8 uPriority = 0;
	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);

	BCM_DEBUG_PRINT(Adapter,
			DBG_TYPE_TX,
			IPV4_DBG,
			DBG_LVL_ALL,
			"%s  CLS UserPrio:%x CLS VLANID:%x\n",
			__func__,
			ntohs(*((USHORT *)pstClassifierRule->usUserPriority)),
			pstClassifierRule->usVLANID);

	/*
	 * In case FW didn't receive the TLV,
	 * the priority field should be ignored
	 */
	if (pstClassifierRule->usValidityBitMap &
			(1<<PKT_CLASSIFICATION_USER_PRIORITY_VALID)) {
		if (pstEthCsPktInfo->eNwpktEthFrameType != eEth802QVLANFrame)
				return false;

		uPriority = (ntohs(*(USHORT *)(skb->data +
				   sizeof(struct bcm_eth_header))) &
				   0xF000) >> 13;

		if ((uPriority >= pstClassifierRule->usUserPriority[0]) &&
				(uPriority <=
				 pstClassifierRule->usUserPriority[1]))
			bClassificationSucceed = TRUE;

		if (!bClassificationSucceed)
			return false;
	}

	BCM_DEBUG_PRINT(Adapter,
			DBG_TYPE_TX,
			IPV4_DBG,
			DBG_LVL_ALL,
			"ETH CS 802.1 D  User Priority Rule Matched\n");

	bClassificationSucceed = false;

	if (pstClassifierRule->usValidityBitMap &
			(1<<PKT_CLASSIFICATION_VLANID_VALID)) {
		if (pstEthCsPktInfo->eNwpktEthFrameType != eEth802QVLANFrame)
				return false;

		usVLANID = ntohs(*(USHORT *)(skb->data +
					sizeof(struct bcm_eth_header))) & 0xFFF;

		BCM_DEBUG_PRINT(Adapter,
				DBG_TYPE_TX,
				IPV4_DBG,
				DBG_LVL_ALL,
				"%s  Pkt VLANID %x Priority: %d\n",
				__func__,
				usVLANID,
				uPriority);

		if (usVLANID == ((pstClassifierRule->usVLANID & 0xFFF0) >> 4))
			bClassificationSucceed = TRUE;

		if (!bClassificationSucceed)
			return false;
	}

	BCM_DEBUG_PRINT(Adapter,
			DBG_TYPE_TX,
			IPV4_DBG,
			DBG_LVL_ALL,
			"ETH CS 802.1 Q VLAN ID Rule Matched\n");

	return TRUE;
}


static bool EThCSClassifyPkt(struct bcm_mini_adapter *Adapter,
			     struct sk_buff *skb,
			     struct bcm_eth_packet_info *pstEthCsPktInfo,
			     struct bcm_classifier_rule *pstClassifierRule,
			     B_UINT8 EthCSCupport)
{
	bool bClassificationSucceed = false;

	bClassificationSucceed = EthCSMatchSrcMACAddress(pstClassifierRule,
			((struct bcm_eth_header *)(skb->data))->au8SourceAddress);
	if (!bClassificationSucceed)
		return false;
	BCM_DEBUG_PRINT(Adapter,
			DBG_TYPE_TX,
			IPV4_DBG,
			DBG_LVL_ALL,
			"ETH CS SrcMAC Matched\n");

	bClassificationSucceed = EthCSMatchDestMACAddress(pstClassifierRule,
			((struct bcm_eth_header *)(skb->data))->au8DestinationAddress);
	if (!bClassificationSucceed)
		return false;
	BCM_DEBUG_PRINT(Adapter,
			DBG_TYPE_TX,
			IPV4_DBG,
			DBG_LVL_ALL,
			"ETH CS DestMAC Matched\n");

	/* classify on ETHType/802.2SAP TLV */
	bClassificationSucceed = EthCSMatchEThTypeSAP(pstClassifierRule,
						      skb,
						      pstEthCsPktInfo);
	if (!bClassificationSucceed)
		return false;

	BCM_DEBUG_PRINT(Adapter,
			DBG_TYPE_TX,
			IPV4_DBG,
			DBG_LVL_ALL,
			"ETH CS EthType/802.2SAP Matched\n");

	/* classify on 802.1VLAN Header Parameters */
	bClassificationSucceed = EthCSMatchVLANRules(pstClassifierRule,
						     skb,
						     pstEthCsPktInfo);
	if (!bClassificationSucceed)
		return false;
	BCM_DEBUG_PRINT(Adapter,
			DBG_TYPE_TX,
			IPV4_DBG,
			DBG_LVL_ALL,
			"ETH CS 802.1 VLAN Rules Matched\n");

	return bClassificationSucceed;
}

static void EThCSGetPktInfo(struct bcm_mini_adapter *Adapter,
			    PVOID pvEthPayload,
			    struct bcm_eth_packet_info *pstEthCsPktInfo)
{
	USHORT u16Etype = ntohs(
			((struct bcm_eth_header *)pvEthPayload)->u16Etype);

	BCM_DEBUG_PRINT(Adapter,
			DBG_TYPE_TX,
			IPV4_DBG,
			DBG_LVL_ALL,
			"EthCSGetPktInfo : Eth Hdr Type : %X\n",
			u16Etype);
	if (u16Etype > 0x5dc) {
		BCM_DEBUG_PRINT(Adapter,
				DBG_TYPE_TX,
				IPV4_DBG,
				DBG_LVL_ALL,
				"EthCSGetPktInfo : ETH2 Frame\n");
		/* ETH2 Frame */
		if (u16Etype == ETHERNET_FRAMETYPE_802QVLAN) {
			/* 802.1Q VLAN Header */
			pstEthCsPktInfo->eNwpktEthFrameType = eEth802QVLANFrame;
			u16Etype = ((struct bcm_eth_q_frame *)pvEthPayload)->EthType;
			/* ((ETH_CS_802_Q_FRAME*)pvEthPayload)->UserPriority */
		} else {
			pstEthCsPktInfo->eNwpktEthFrameType = eEthOtherFrame;
			u16Etype = ntohs(u16Etype);
		}
	} else {
		/* 802.2 LLC */
		BCM_DEBUG_PRINT(Adapter,
				DBG_TYPE_TX,
				IPV4_DBG,
				DBG_LVL_ALL,
				"802.2 LLC Frame\n");
		pstEthCsPktInfo->eNwpktEthFrameType = eEth802LLCFrame;
		pstEthCsPktInfo->ucDSAP =
			((struct bcm_eth_llc_frame *)pvEthPayload)->DSAP;
		if (pstEthCsPktInfo->ucDSAP == 0xAA && ((struct bcm_eth_llc_frame *)pvEthPayload)->SSAP == 0xAA) {
			/* SNAP Frame */
			pstEthCsPktInfo->eNwpktEthFrameType = eEth802LLCSNAPFrame;
			u16Etype = ((struct bcm_eth_llc_snap_frame *)pvEthPayload)->usEtherType;
		}
	}
	if (u16Etype == ETHERNET_FRAMETYPE_IPV4)
		pstEthCsPktInfo->eNwpktIPFrameType = eIPv4Packet;
	else if (u16Etype == ETHERNET_FRAMETYPE_IPV6)
		pstEthCsPktInfo->eNwpktIPFrameType = eIPv6Packet;
	else
		pstEthCsPktInfo->eNwpktIPFrameType = eNonIPPacket;

	pstEthCsPktInfo->usEtherType = ((struct bcm_eth_header *)pvEthPayload)->u16Etype;
	BCM_DEBUG_PRINT(Adapter,
			DBG_TYPE_TX,
			IPV4_DBG,
			DBG_LVL_ALL,
			"EthCsPktInfo->eNwpktIPFrameType : %x\n",
			pstEthCsPktInfo->eNwpktIPFrameType);
	BCM_DEBUG_PRINT(Adapter,
			DBG_TYPE_TX,
			IPV4_DBG,
			DBG_LVL_ALL,
			"EthCsPktInfo->eNwpktEthFrameType : %x\n",
			pstEthCsPktInfo->eNwpktEthFrameType);
	BCM_DEBUG_PRINT(Adapter,
			DBG_TYPE_TX,
			IPV4_DBG,
			DBG_LVL_ALL,
			"EthCsPktInfo->usEtherType : %x\n",
			pstEthCsPktInfo->usEtherType);
}