drivers/staging/bcm/Qos.c

   1 /**
   2 @file Qos.C
   3 This file contains the routines related to Quality of Service.
   4 */
   5 #include "headers.h"
   6
   7 static void EThCSGetPktInfo(struct bcm_mini_adapter *Adapter,
   8                             PVOID pvEthPayload,
   9                             struct bcm_eth_packet_info *pstEthCsPktInfo);
  10
  11 static bool EThCSClassifyPkt(struct bcm_mini_adapter *Adapter,
  12                              struct sk_buff *skb,
  13                              struct bcm_eth_packet_info *pstEthCsPktInfo,
  14                              struct bcm_classifier_rule *pstClassifierRule,
  15                              B_UINT8 EthCSCupport);
  16
  17 static USHORT IpVersion4(struct bcm_mini_adapter *Adapter, struct iphdr *iphd,
  18                          struct bcm_classifier_rule *pstClassifierRule);
  19
  20 static VOID PruneQueue(struct bcm_mini_adapter *Adapter, INT iIndex);
  21
  22
  23 /*******************************************************************
  24 * Function    - MatchSrcIpAddress()
  25 *
  26 * Description - Checks whether the Source IP address from the packet
  27 *                               matches with that of Queue.
  28 *
  29 * Parameters  - pstClassifierRule: Pointer to the packet info structure.
  30 *               - ulSrcIP           : Source IP address from the packet.
  31 *
  32 * Returns     - TRUE(If address matches) else FAIL .
  33 *********************************************************************/
  34 static bool MatchSrcIpAddress(struct bcm_classifier_rule *pstClassifierRule, ULONG ulSrcIP)
  35 {
  36         UCHAR ucLoopIndex = 0;
  37
  38         struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
  39
  40         ulSrcIP = ntohl(ulSrcIP);
  41         if (0 == pstClassifierRule->ucIPSourceAddressLength)
  42                 return TRUE;
  43         for (ucLoopIndex = 0; ucLoopIndex < (pstClassifierRule->ucIPSourceAddressLength); ucLoopIndex++) {
  44                 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)pstClassifierRule->stSrcIpAddress.ulIpv4Mask[ucLoopIndex], (UINT)ulSrcIP, (UINT)pstClassifierRule->stSrcIpAddress.ulIpv6Addr[ucLoopIndex]);
  45                 if ((pstClassifierRule->stSrcIpAddress.ulIpv4Mask[ucLoopIndex] & ulSrcIP) ==
  46                                 (pstClassifierRule->stSrcIpAddress.ulIpv4Addr[ucLoopIndex] & pstClassifierRule->stSrcIpAddress.ulIpv4Mask[ucLoopIndex]))
  47                         return TRUE;
  48         }
  49         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "Src Ip Address Not Matched");
  50         return false;
  51 }
  52
  53
  54 /*******************************************************************
  55 * Function    - MatchDestIpAddress()
  56 *
  57 * Description - Checks whether the Destination IP address from the packet
  58 *                               matches with that of Queue.
  59 *
  60 * Parameters  - pstClassifierRule: Pointer to the packet info structure.
  61 *               - ulDestIP    : Destination IP address from the packet.
  62 *
  63 * Returns     - TRUE(If address matches) else FAIL .
  64 *********************************************************************/
  65 static bool MatchDestIpAddress(struct bcm_classifier_rule *pstClassifierRule, ULONG ulDestIP)
  66 {
  67         UCHAR ucLoopIndex = 0;
  68         struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
  69
  70         ulDestIP = ntohl(ulDestIP);
  71         if (0 == pstClassifierRule->ucIPDestinationAddressLength)
  72                 return TRUE;
  73         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "Destination Ip Address 0x%x 0x%x 0x%x  ", (UINT)ulDestIP, (UINT)pstClassifierRule->stDestIpAddress.ulIpv4Mask[ucLoopIndex], (UINT)pstClassifierRule->stDestIpAddress.ulIpv4Addr[ucLoopIndex]);
  74
  75         for (ucLoopIndex = 0; ucLoopIndex < (pstClassifierRule->ucIPDestinationAddressLength); ucLoopIndex++) {
  76                 if ((pstClassifierRule->stDestIpAddress.ulIpv4Mask[ucLoopIndex] & ulDestIP) ==
  77                                 (pstClassifierRule->stDestIpAddress.ulIpv4Addr[ucLoopIndex] & pstClassifierRule->stDestIpAddress.ulIpv4Mask[ucLoopIndex]))
  78                         return TRUE;
  79         }
  80         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "Destination Ip Address Not Matched");
  81         return false;
  82 }
  83
  84
  85 /************************************************************************
  86 * Function    - MatchTos()
  87 *
  88 * Description - Checks the TOS from the packet matches with that of queue.
  89 *
  90 * Parameters  - pstClassifierRule   : Pointer to the packet info structure.
  91 *               - ucTypeOfService: TOS from the packet.
  92 *
  93 * Returns     - TRUE(If address matches) else FAIL.
  94 **************************************************************************/
  95 static bool MatchTos(struct bcm_classifier_rule *pstClassifierRule, UCHAR ucTypeOfService)
  96 {
  97
  98         struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
  99         if (3 != pstClassifierRule->ucIPTypeOfServiceLength)
 100                 return TRUE;
 101
 102         if (((pstClassifierRule->ucTosMask & ucTypeOfService) <= pstClassifierRule->ucTosHigh) && ((pstClassifierRule->ucTosMask & ucTypeOfService) >= pstClassifierRule->ucTosLow))
 103                 return TRUE;
 104
 105         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "Type Of Service Not Matched");
 106         return false;
 107 }
 108
 109
 110 /***************************************************************************
 111 * Function    - MatchProtocol()
 112 *
 113 * Description - Checks the protocol from the packet matches with that of queue.
 114 *
 115 * Parameters  - pstClassifierRule: Pointer to the packet info structure.
 116 *               - ucProtocol    : Protocol from the packet.
 117 *
 118 * Returns     - TRUE(If address matches) else FAIL.
 119 ****************************************************************************/
 120 bool MatchProtocol(struct bcm_classifier_rule *pstClassifierRule, UCHAR ucProtocol)
 121 {
 122         UCHAR ucLoopIndex = 0;
 123         struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
 124         if (0 == pstClassifierRule->ucProtocolLength)
 125                 return TRUE;
 126         for (ucLoopIndex = 0; ucLoopIndex < pstClassifierRule->ucProtocolLength; ucLoopIndex++) {
 127                 BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "Protocol:0x%X Classification Protocol:0x%X", ucProtocol, pstClassifierRule->ucProtocol[ucLoopIndex]);
 128                 if (pstClassifierRule->ucProtocol[ucLoopIndex] == ucProtocol)
 129                         return TRUE;
 130         }
 131         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "Protocol Not Matched");
 132         return false;
 133 }
 134
 135
 136 /***********************************************************************
 137 * Function    - MatchSrcPort()
 138 *
 139 * Description - Checks, Source port from the packet matches with that of queue.
 140 *
 141 * Parameters  - pstClassifierRule: Pointer to the packet info structure.
 142 *               - ushSrcPort    : Source port from the packet.
 143 *
 144 * Returns     - TRUE(If address matches) else FAIL.
 145 ***************************************************************************/
 146 bool MatchSrcPort(struct bcm_classifier_rule *pstClassifierRule, USHORT ushSrcPort)
 147 {
 148         UCHAR ucLoopIndex = 0;
 149
 150         struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
 151
 152
 153         if (0 == pstClassifierRule->ucSrcPortRangeLength)
 154                 return TRUE;
 155         for (ucLoopIndex = 0; ucLoopIndex < pstClassifierRule->ucSrcPortRangeLength; ucLoopIndex++) {
 156                 if (ushSrcPort <= pstClassifierRule->usSrcPortRangeHi[ucLoopIndex] &&
 157                         ushSrcPort >= pstClassifierRule->usSrcPortRangeLo[ucLoopIndex])
 158                         return TRUE;
 159         }
 160         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "Src Port: %x Not Matched ", ushSrcPort);
 161         return false;
 162 }
 163
 164
 165 /***********************************************************************
 166 * Function    - MatchDestPort()
 167 *
 168 * Description - Checks, Destination port from packet matches with that of queue.
 169 *
 170 * Parameters  - pstClassifierRule: Pointer to the packet info structure.
 171 *               - ushDestPort   : Destination port from the packet.
 172 *
 173 * Returns     - TRUE(If address matches) else FAIL.
 174 ***************************************************************************/
 175 bool MatchDestPort(struct bcm_classifier_rule *pstClassifierRule, USHORT ushDestPort)
 176 {
 177         UCHAR ucLoopIndex = 0;
 178         struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
 179
 180         if (0 == pstClassifierRule->ucDestPortRangeLength)
 181                 return TRUE;
 182
 183         for (ucLoopIndex = 0; ucLoopIndex < pstClassifierRule->ucDestPortRangeLength; ucLoopIndex++) {
 184                 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]);
 185
 186                 if (ushDestPort <= pstClassifierRule->usDestPortRangeHi[ucLoopIndex] &&
 187                         ushDestPort >= pstClassifierRule->usDestPortRangeLo[ucLoopIndex])
 188                         return TRUE;
 189         }
 190         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "Dest Port: %x Not Matched", ushDestPort);
 191         return false;
 192 }
 193 /**
 194 @ingroup tx_functions
 195 Compares IPV4 Ip address and port number
 196 @return Queue Index.
 197 */
 198 static USHORT   IpVersion4(struct bcm_mini_adapter *Adapter,
 199                            struct iphdr *iphd,
 200                            struct bcm_classifier_rule *pstClassifierRule)
 201 {
 202         struct bcm_transport_header *xprt_hdr = NULL;
 203         bool    bClassificationSucceed = false;
 204
 205         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "========>");
 206
 207         xprt_hdr = (struct bcm_transport_header *)((PUCHAR)iphd + sizeof(struct iphdr));
 208
 209         do {
 210                 BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "Trying to see Direction = %d %d",
 211                         pstClassifierRule->ucDirection,
 212                         pstClassifierRule->usVCID_Value);
 213
 214                 //Checking classifier validity
 215                 if (!pstClassifierRule->bUsed || pstClassifierRule->ucDirection == DOWNLINK_DIR)
 216                         break;
 217
 218                 BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "is IPv6 check!");
 219                 if (pstClassifierRule->bIpv6Protocol)
 220                         break;
 221
 222                 //**************Checking IP header parameter**************************//
 223                 BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "Trying to match Source IP Address");
 224                 if (!MatchSrcIpAddress(pstClassifierRule, iphd->saddr))
 225                         break;
 226                 BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "Source IP Address Matched");
 227
 228                 if (!MatchDestIpAddress(pstClassifierRule, iphd->daddr))
 229                         break;
 230                 BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "Destination IP Address Matched");
 231
 232                 if (!MatchTos(pstClassifierRule, iphd->tos)) {
 233                         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "TOS Match failed\n");
 234                         break;
 235                 }
 236                 BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "TOS Matched");
 237
 238                 if (!MatchProtocol(pstClassifierRule, iphd->protocol))
 239                         break;
 240                 BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "Protocol Matched");
 241
 242                 //if protocol is not TCP or UDP then no need of comparing source port and destination port
 243                 if (iphd->protocol != TCP && iphd->protocol != UDP) {
 244                         bClassificationSucceed = TRUE;
 245                         break;
 246                 }
 247                 //******************Checking Transport Layer Header field if present *****************//
 248                 BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "Source Port %04x",
 249                         (iphd->protocol == UDP) ? xprt_hdr->uhdr.source : xprt_hdr->thdr.source);
 250
 251                 if (!MatchSrcPort(pstClassifierRule,
 252                                   ntohs((iphd->protocol == UDP) ?
 253                                   xprt_hdr->uhdr.source : xprt_hdr->thdr.source)))
 254                         break;
 255                 BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "Src Port Matched");
 256
 257                 BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "Destination Port %04x",
 258                         (iphd->protocol == UDP) ? xprt_hdr->uhdr.dest :
 259                         xprt_hdr->thdr.dest);
 260                 if (!MatchDestPort(pstClassifierRule,
 261                                    ntohs((iphd->protocol == UDP) ?
 262                                    xprt_hdr->uhdr.dest : xprt_hdr->thdr.dest)))
 263                         break;
 264                 bClassificationSucceed = TRUE;
 265         } while (0);
 266
 267         if (TRUE == bClassificationSucceed) {
 268                 INT iMatchedSFQueueIndex = 0;
 269                 iMatchedSFQueueIndex = SearchSfid(Adapter, pstClassifierRule->ulSFID);
 270                 if (iMatchedSFQueueIndex >= NO_OF_QUEUES)
 271                         bClassificationSucceed = false;
 272                 else if (false == Adapter->PackInfo[iMatchedSFQueueIndex].bActive)
 273                         bClassificationSucceed = false;
 274         }
 275
 276         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "IpVersion4 <==========");
 277
 278         return bClassificationSucceed;
 279 }
 280
 281 VOID PruneQueueAllSF(struct bcm_mini_adapter *Adapter)
 282 {
 283         UINT iIndex = 0;
 284
 285         for (iIndex = 0; iIndex < HiPriority; iIndex++) {
 286                 if (!Adapter->PackInfo[iIndex].bValid)
 287                         continue;
 288
 289                 PruneQueue(Adapter, iIndex);
 290         }
 291 }
 292
 293
 294 /**
 295 @ingroup tx_functions
 296 This function checks if the max queue size for a queue
 297 is less than number of bytes in the queue. If so -
 298 drops packets from the Head till the number of bytes is
 299 less than or equal to max queue size for the queue.
 300 */
 301 static VOID PruneQueue(struct bcm_mini_adapter *Adapter, INT iIndex)
 302 {
 303         struct sk_buff* PacketToDrop = NULL;
 304         struct net_device_stats *netstats;
 305
 306         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, PRUNE_QUEUE, DBG_LVL_ALL, "=====> Index %d", iIndex);
 307
 308         if (iIndex == HiPriority)
 309                 return;
 310
 311         if (!Adapter || (iIndex < 0) || (iIndex > HiPriority))
 312                 return;
 313
 314         /* To Store the netdevice statistic */
 315         netstats = &Adapter->dev->stats;
 316
 317         spin_lock_bh(&Adapter->PackInfo[iIndex].SFQueueLock);
 318
 319         while (1) {
 320 //      while((UINT)Adapter->PackInfo[iIndex].uiCurrentPacketsOnHost >
 321 //              SF_MAX_ALLOWED_PACKETS_TO_BACKUP) {
 322
 323                 BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, PRUNE_QUEUE, DBG_LVL_ALL, "uiCurrentBytesOnHost:%x uiMaxBucketSize :%x",
 324                 Adapter->PackInfo[iIndex].uiCurrentBytesOnHost,
 325                 Adapter->PackInfo[iIndex].uiMaxBucketSize);
 326
 327                 PacketToDrop = Adapter->PackInfo[iIndex].FirstTxQueue;
 328
 329                 if (PacketToDrop == NULL)
 330                         break;
 331                 if ((Adapter->PackInfo[iIndex].uiCurrentPacketsOnHost < SF_MAX_ALLOWED_PACKETS_TO_BACKUP) &&
 332                         ((1000*(jiffies - *((B_UINT32 *)(PacketToDrop->cb)+SKB_CB_LATENCY_OFFSET))/HZ) <= Adapter->PackInfo[iIndex].uiMaxLatency))
 333                         break;
 334
 335                 if (PacketToDrop) {
 336                         if (netif_msg_tx_err(Adapter))
 337                                 pr_info(PFX "%s: tx queue %d overlimit\n",
 338                                         Adapter->dev->name, iIndex);
 339
 340                         netstats->tx_dropped++;
 341
 342                         DEQUEUEPACKET(Adapter->PackInfo[iIndex].FirstTxQueue,
 343                                                 Adapter->PackInfo[iIndex].LastTxQueue);
 344                         /// update current bytes and packets count
 345                         Adapter->PackInfo[iIndex].uiCurrentBytesOnHost -=
 346                                 PacketToDrop->len;
 347                         Adapter->PackInfo[iIndex].uiCurrentPacketsOnHost--;
 348                         /// update dropped bytes and packets counts
 349                         Adapter->PackInfo[iIndex].uiDroppedCountBytes += PacketToDrop->len;
 350                         Adapter->PackInfo[iIndex].uiDroppedCountPackets++;
 351                         dev_kfree_skb(PacketToDrop);
 352
 353                 }
 354
 355                 BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, PRUNE_QUEUE, DBG_LVL_ALL, "Dropped Bytes:%x Dropped Packets:%x",
 356                         Adapter->PackInfo[iIndex].uiDroppedCountBytes,
 357                         Adapter->PackInfo[iIndex].uiDroppedCountPackets);
 358
 359                 atomic_dec(&Adapter->TotalPacketCount);
 360         }
 361
 362         spin_unlock_bh(&Adapter->PackInfo[iIndex].SFQueueLock);
 363
 364         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, PRUNE_QUEUE, DBG_LVL_ALL, "TotalPacketCount:%x",
 365                 atomic_read(&Adapter->TotalPacketCount));
 366         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, PRUNE_QUEUE, DBG_LVL_ALL, "<=====");
 367 }
 368
 369 VOID flush_all_queues(struct bcm_mini_adapter *Adapter)
 370 {
 371         INT             iQIndex;
 372         UINT    uiTotalPacketLength;
 373         struct sk_buff*                 PacketToDrop = NULL;
 374
 375         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "=====>");
 376
 377 //      down(&Adapter->data_packet_queue_lock);
 378         for (iQIndex = LowPriority; iQIndex < HiPriority; iQIndex++) {
 379                 struct net_device_stats *netstats = &Adapter->dev->stats;
 380
 381                 spin_lock_bh(&Adapter->PackInfo[iQIndex].SFQueueLock);
 382                 while (Adapter->PackInfo[iQIndex].FirstTxQueue) {
 383                         PacketToDrop = Adapter->PackInfo[iQIndex].FirstTxQueue;
 384                         if (PacketToDrop) {
 385                                 uiTotalPacketLength = PacketToDrop->len;
 386                                 netstats->tx_dropped++;
 387                         } else
 388                                 uiTotalPacketLength = 0;
 389
 390                         DEQUEUEPACKET(Adapter->PackInfo[iQIndex].FirstTxQueue,
 391                                                 Adapter->PackInfo[iQIndex].LastTxQueue);
 392
 393                         /* Free the skb */
 394                         dev_kfree_skb(PacketToDrop);
 395
 396                         /// update current bytes and packets count
 397                         Adapter->PackInfo[iQIndex].uiCurrentBytesOnHost -= uiTotalPacketLength;
 398                         Adapter->PackInfo[iQIndex].uiCurrentPacketsOnHost--;
 399
 400                         /// update dropped bytes and packets counts
 401                         Adapter->PackInfo[iQIndex].uiDroppedCountBytes += uiTotalPacketLength;
 402                         Adapter->PackInfo[iQIndex].uiDroppedCountPackets++;
 403
 404                         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "Dropped Bytes:%x Dropped Packets:%x",
 405                                         Adapter->PackInfo[iQIndex].uiDroppedCountBytes,
 406                                         Adapter->PackInfo[iQIndex].uiDroppedCountPackets);
 407                         atomic_dec(&Adapter->TotalPacketCount);
 408                 }
 409                 spin_unlock_bh(&Adapter->PackInfo[iQIndex].SFQueueLock);
 410         }
 411 //      up(&Adapter->data_packet_queue_lock);
 412         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "<=====");
 413 }
 414
 415 USHORT ClassifyPacket(struct bcm_mini_adapter *Adapter, struct sk_buff* skb)
 416 {
 417         INT                     uiLoopIndex = 0;
 418         struct bcm_classifier_rule *pstClassifierRule = NULL;
 419         struct bcm_eth_packet_info stEthCsPktInfo;
 420         PVOID pvEThPayload = NULL;
 421         struct iphdr *pIpHeader = NULL;
 422         INT       uiSfIndex = 0;
 423         USHORT  usIndex = Adapter->usBestEffortQueueIndex;
 424         bool    bFragmentedPkt = false, bClassificationSucceed = false;
 425         USHORT  usCurrFragment = 0;
 426
 427         struct bcm_tcp_header *pTcpHeader;
 428         UCHAR IpHeaderLength;
 429         UCHAR TcpHeaderLength;
 430
 431         pvEThPayload = skb->data;
 432         *((UINT32*) (skb->cb) +SKB_CB_TCPACK_OFFSET) = 0;
 433         EThCSGetPktInfo(Adapter, pvEThPayload, &stEthCsPktInfo);
 434
 435         switch (stEthCsPktInfo.eNwpktEthFrameType) {
 436                 case eEth802LLCFrame:
 437                         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "ClassifyPacket : 802LLCFrame\n");
 438                         pIpHeader = pvEThPayload + sizeof(struct bcm_eth_llc_frame);
 439                         break;
 440                 case eEth802LLCSNAPFrame:
 441                         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "ClassifyPacket : 802LLC SNAP Frame\n");
 442                         pIpHeader = pvEThPayload + sizeof(struct bcm_eth_llc_snap_frame);
 443                         break;
 444                 case eEth802QVLANFrame:
 445                         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "ClassifyPacket : 802.1Q VLANFrame\n");
 446                         pIpHeader = pvEThPayload + sizeof(struct bcm_eth_q_frame);
 447                         break;
 448                 case eEthOtherFrame:
 449                         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "ClassifyPacket : ETH Other Frame\n");
 450                         pIpHeader = pvEThPayload + sizeof(struct bcm_ethernet2_frame);
 451                         break;
 452                 default:
 453                         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "ClassifyPacket : Unrecognized ETH Frame\n");
 454                         pIpHeader = pvEThPayload + sizeof(struct bcm_ethernet2_frame);
 455                         break;
 456         }
 457
 458         if (stEthCsPktInfo.eNwpktIPFrameType == eIPv4Packet) {
 459                 usCurrFragment = (ntohs(pIpHeader->frag_off) & IP_OFFSET);
 460                 if ((ntohs(pIpHeader->frag_off) & IP_MF) || usCurrFragment)
 461                         bFragmentedPkt = TRUE;
 462
 463                 if (bFragmentedPkt) {
 464                                 //Fragmented  Packet. Get Frag Classifier Entry.
 465                         pstClassifierRule = GetFragIPClsEntry(Adapter, pIpHeader->id, pIpHeader->saddr);
 466                         if (pstClassifierRule) {
 467                                         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "It is next Fragmented pkt");
 468                                         bClassificationSucceed = TRUE;
 469                         }
 470                         if (!(ntohs(pIpHeader->frag_off) & IP_MF)) {
 471                                 //Fragmented Last packet . Remove Frag Classifier Entry
 472                                 BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "This is the last fragmented Pkt");
 473                                 DelFragIPClsEntry(Adapter, pIpHeader->id, pIpHeader->saddr);
 474                         }
 475                 }
 476         }
 477
 478         for (uiLoopIndex = MAX_CLASSIFIERS - 1; uiLoopIndex >= 0; uiLoopIndex--) {
 479                 if (bClassificationSucceed)
 480                         break;
 481                 //Iterate through all classifiers which are already in order of priority
 482                 //to classify the packet until match found
 483                 do {
 484                         if (false == Adapter->astClassifierTable[uiLoopIndex].bUsed) {
 485                                 bClassificationSucceed = false;
 486                                 break;
 487                         }
 488                         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "Adapter->PackInfo[%d].bvalid=True\n", uiLoopIndex);
 489
 490                         if (0 == Adapter->astClassifierTable[uiLoopIndex].ucDirection) {
 491                                 bClassificationSucceed = false;//cannot be processed for classification.
 492                                 break;                                          // it is a down link connection
 493                         }
 494
 495                         pstClassifierRule = &Adapter->astClassifierTable[uiLoopIndex];
 496
 497                         uiSfIndex = SearchSfid(Adapter, pstClassifierRule->ulSFID);
 498                         if (uiSfIndex >= NO_OF_QUEUES) {
 499                                 BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "Queue Not Valid. SearchSfid for this classifier Failed\n");
 500                                 break;
 501                         }
 502
 503                         if (Adapter->PackInfo[uiSfIndex].bEthCSSupport) {
 504
 505                                 if (eEthUnsupportedFrame == stEthCsPktInfo.eNwpktEthFrameType) {
 506                                         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, " ClassifyPacket : Packet Not a Valid Supported Ethernet Frame\n");
 507                                         bClassificationSucceed = false;
 508                                         break;
 509                                 }
 510
 511
 512
 513                                 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);
 514                                 bClassificationSucceed = EThCSClassifyPkt(Adapter, skb, &stEthCsPktInfo, pstClassifierRule, Adapter->PackInfo[uiSfIndex].bEthCSSupport);
 515
 516                                 if (!bClassificationSucceed) {
 517                                         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "ClassifyPacket : Ethernet CS Classification Failed\n");
 518                                         break;
 519                                 }
 520                         } else {        // No ETH Supported on this SF
 521                                 if (eEthOtherFrame != stEthCsPktInfo.eNwpktEthFrameType) {
 522                                         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");
 523                                         bClassificationSucceed = false;
 524                                         break;
 525                                 }
 526                         }
 527
 528                         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "Proceeding to IP CS Clasification");
 529
 530                         if (Adapter->PackInfo[uiSfIndex].bIPCSSupport) {
 531
 532                                 if (stEthCsPktInfo.eNwpktIPFrameType == eNonIPPacket) {
 533                                         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, " ClassifyPacket : Packet is Not an IP Packet\n");
 534                                         bClassificationSucceed = false;
 535                                         break;
 536                                 }
 537                                 BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "Dump IP Header :\n");
 538                                 DumpFullPacket((PUCHAR)pIpHeader, 20);
 539
 540                                 if (stEthCsPktInfo.eNwpktIPFrameType == eIPv4Packet)
 541                                         bClassificationSucceed = IpVersion4(Adapter, pIpHeader, pstClassifierRule);
 542                                 else if (stEthCsPktInfo.eNwpktIPFrameType == eIPv6Packet)
 543                                         bClassificationSucceed = IpVersion6(Adapter, pIpHeader, pstClassifierRule);
 544                         }
 545
 546                 } while (0);
 547         }
 548
 549         if (bClassificationSucceed == TRUE) {
 550                 BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "CF id : %d, SF ID is =%lu", pstClassifierRule->uiClassifierRuleIndex, pstClassifierRule->ulSFID);
 551
 552                 //Store The matched Classifier in SKB
 553                 *((UINT32*)(skb->cb)+SKB_CB_CLASSIFICATION_OFFSET) = pstClassifierRule->uiClassifierRuleIndex;
 554                 if ((TCP == pIpHeader->protocol) && !bFragmentedPkt && (ETH_AND_IP_HEADER_LEN + TCP_HEADER_LEN <= skb->len)) {
 555                          IpHeaderLength   = pIpHeader->ihl;
 556                          pTcpHeader = (struct bcm_tcp_header *)(((PUCHAR)pIpHeader)+(IpHeaderLength*4));
 557                          TcpHeaderLength  = GET_TCP_HEADER_LEN(pTcpHeader->HeaderLength);
 558
 559                         if ((pTcpHeader->ucFlags & TCP_ACK) &&
 560                            (ntohs(pIpHeader->tot_len) == (IpHeaderLength*4)+(TcpHeaderLength*4)))
 561                                 *((UINT32*) (skb->cb) + SKB_CB_TCPACK_OFFSET) = TCP_ACK;
 562                 }
 563
 564                 usIndex = SearchSfid(Adapter, pstClassifierRule->ulSFID);
 565                 BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "index is  =%d", usIndex);
 566
 567                 //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.
 568                 if (bFragmentedPkt && (usCurrFragment == 0)) {
 569                         //First Fragment of Fragmented Packet. Create Frag CLS Entry
 570                         struct bcm_fragmented_packet_info stFragPktInfo;
 571                         stFragPktInfo.bUsed = TRUE;
 572                         stFragPktInfo.ulSrcIpAddress = pIpHeader->saddr;
 573                         stFragPktInfo.usIpIdentification = pIpHeader->id;
 574                         stFragPktInfo.pstMatchedClassifierEntry = pstClassifierRule;
 575                         stFragPktInfo.bOutOfOrderFragment = false;
 576                         AddFragIPClsEntry(Adapter, &stFragPktInfo);
 577                 }
 578
 579
 580         }
 581
 582         if (bClassificationSucceed)
 583                 return usIndex;
 584         else
 585                 return INVALID_QUEUE_INDEX;
 586 }
 587
 588 static bool EthCSMatchSrcMACAddress(struct bcm_classifier_rule *pstClassifierRule, PUCHAR Mac)
 589 {
 590         UINT i = 0;
 591         struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
 592         if (pstClassifierRule->ucEthCSSrcMACLen == 0)
 593                 return TRUE;
 594         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "%s\n", __FUNCTION__);
 595         for (i = 0; i < MAC_ADDRESS_SIZE; i++) {
 596                 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]);
 597                 if ((pstClassifierRule->au8EThCSSrcMAC[i] & pstClassifierRule->au8EThCSSrcMACMask[i]) !=
 598                         (Mac[i] & pstClassifierRule->au8EThCSSrcMACMask[i]))
 599                         return false;
 600         }
 601         return TRUE;
 602 }
 603
 604 static bool EthCSMatchDestMACAddress(struct bcm_classifier_rule *pstClassifierRule, PUCHAR Mac)
 605 {
 606         UINT i = 0;
 607         struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
 608         if (pstClassifierRule->ucEthCSDestMACLen == 0)
 609                 return TRUE;
 610         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "%s\n", __FUNCTION__);
 611         for (i = 0; i < MAC_ADDRESS_SIZE; i++) {
 612                 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]);
 613                 if ((pstClassifierRule->au8EThCSDestMAC[i] & pstClassifierRule->au8EThCSDestMACMask[i]) !=
 614                         (Mac[i] & pstClassifierRule->au8EThCSDestMACMask[i]))
 615                         return false;
 616         }
 617         return TRUE;
 618 }
 619
 620 static bool EthCSMatchEThTypeSAP(struct bcm_classifier_rule *pstClassifierRule, struct sk_buff* skb, struct bcm_eth_packet_info *pstEthCsPktInfo)
 621 {
 622         struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
 623         if ((pstClassifierRule->ucEtherTypeLen == 0) ||
 624                 (pstClassifierRule->au8EthCSEtherType[0] == 0))
 625                 return TRUE;
 626
 627         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "%s SrcEtherType:%x CLS EtherType[0]:%x\n", __FUNCTION__, pstEthCsPktInfo->usEtherType, pstClassifierRule->au8EthCSEtherType[0]);
 628         if (pstClassifierRule->au8EthCSEtherType[0] == 1) {
 629                 BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "%s  CLS EtherType[1]:%x EtherType[2]:%x\n", __FUNCTION__, pstClassifierRule->au8EthCSEtherType[1], pstClassifierRule->au8EthCSEtherType[2]);
 630
 631                 if (memcmp(&pstEthCsPktInfo->usEtherType, &pstClassifierRule->au8EthCSEtherType[1], 2) == 0)
 632                         return TRUE;
 633                 else
 634                         return false;
 635         }
 636
 637         if (pstClassifierRule->au8EthCSEtherType[0] == 2) {
 638                 if (eEth802LLCFrame != pstEthCsPktInfo->eNwpktEthFrameType)
 639                         return false;
 640
 641                 BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "%s  EthCS DSAP:%x EtherType[2]:%x\n", __FUNCTION__, pstEthCsPktInfo->ucDSAP, pstClassifierRule->au8EthCSEtherType[2]);
 642                 if (pstEthCsPktInfo->ucDSAP == pstClassifierRule->au8EthCSEtherType[2])
 643                         return TRUE;
 644                 else
 645                         return false;
 646
 647         }
 648
 649         return false;
 650
 651 }
 652
 653 static bool EthCSMatchVLANRules(struct bcm_classifier_rule *pstClassifierRule, struct sk_buff* skb, struct bcm_eth_packet_info *pstEthCsPktInfo)
 654 {
 655         bool bClassificationSucceed = false;
 656         USHORT usVLANID;
 657         B_UINT8 uPriority = 0;
 658         struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
 659
 660         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "%s  CLS UserPrio:%x CLS VLANID:%x\n", __FUNCTION__, ntohs(*((USHORT *)pstClassifierRule->usUserPriority)), pstClassifierRule->usVLANID);
 661
 662         /* In case FW didn't receive the TLV, the priority field should be ignored */
 663         if (pstClassifierRule->usValidityBitMap & (1<<PKT_CLASSIFICATION_USER_PRIORITY_VALID)) {
 664                 if (pstEthCsPktInfo->eNwpktEthFrameType != eEth802QVLANFrame)
 665                                 return false;
 666
 667                 uPriority = (ntohs(*(USHORT *)(skb->data + sizeof(struct bcm_eth_header))) & 0xF000) >> 13;
 668
 669                 if ((uPriority >= pstClassifierRule->usUserPriority[0]) && (uPriority <= pstClassifierRule->usUserPriority[1]))
 670                                 bClassificationSucceed = TRUE;
 671
 672                 if (!bClassificationSucceed)
 673                         return false;
 674         }
 675
 676         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "ETH CS 802.1 D  User Priority Rule Matched\n");
 677
 678         bClassificationSucceed = false;
 679
 680         if (pstClassifierRule->usValidityBitMap & (1<<PKT_CLASSIFICATION_VLANID_VALID)) {
 681                 if (pstEthCsPktInfo->eNwpktEthFrameType != eEth802QVLANFrame)
 682                                 return false;
 683
 684                 usVLANID = ntohs(*(USHORT *)(skb->data + sizeof(struct bcm_eth_header))) & 0xFFF;
 685
 686                 BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "%s  Pkt VLANID %x Priority: %d\n", __FUNCTION__, usVLANID, uPriority);
 687
 688                 if (usVLANID == ((pstClassifierRule->usVLANID & 0xFFF0) >> 4))
 689                         bClassificationSucceed = TRUE;
 690
 691                 if (!bClassificationSucceed)
 692                         return false;
 693         }
 694
 695         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "ETH CS 802.1 Q VLAN ID Rule Matched\n");
 696
 697         return TRUE;
 698 }
 699
 700
 701 static bool EThCSClassifyPkt(struct bcm_mini_adapter *Adapter, struct sk_buff* skb,
 702                                 struct bcm_eth_packet_info *pstEthCsPktInfo,
 703                                 struct bcm_classifier_rule *pstClassifierRule,
 704                                 B_UINT8 EthCSCupport)
 705 {
 706         bool bClassificationSucceed = false;
 707         bClassificationSucceed = EthCSMatchSrcMACAddress(pstClassifierRule, ((struct bcm_eth_header *)(skb->data))->au8SourceAddress);
 708         if (!bClassificationSucceed)
 709                 return false;
 710         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "ETH CS SrcMAC Matched\n");
 711
 712         bClassificationSucceed = EthCSMatchDestMACAddress(pstClassifierRule, ((struct bcm_eth_header *)(skb->data))->au8DestinationAddress);
 713         if (!bClassificationSucceed)
 714                 return false;
 715         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "ETH CS DestMAC Matched\n");
 716
 717         //classify on ETHType/802.2SAP TLV
 718         bClassificationSucceed = EthCSMatchEThTypeSAP(pstClassifierRule, skb, pstEthCsPktInfo);
 719         if (!bClassificationSucceed)
 720                 return false;
 721
 722         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "ETH CS EthType/802.2SAP Matched\n");
 723
 724         //classify on 802.1VLAN Header Parameters
 725
 726         bClassificationSucceed = EthCSMatchVLANRules(pstClassifierRule, skb, pstEthCsPktInfo);
 727         if (!bClassificationSucceed)
 728                 return false;
 729         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "ETH CS 802.1 VLAN Rules Matched\n");
 730
 731         return bClassificationSucceed;
 732 }
 733
 734 static void EThCSGetPktInfo(struct bcm_mini_adapter *Adapter, PVOID pvEthPayload,
 735                             struct bcm_eth_packet_info *pstEthCsPktInfo)
 736 {
 737         USHORT u16Etype = ntohs(((struct bcm_eth_header *)pvEthPayload)->u16Etype);
 738
 739         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "EthCSGetPktInfo : Eth Hdr Type : %X\n", u16Etype);
 740         if (u16Etype > 0x5dc) {
 741                 BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "EthCSGetPktInfo : ETH2 Frame\n");
 742                 //ETH2 Frame
 743                 if (u16Etype == ETHERNET_FRAMETYPE_802QVLAN) {
 744                         //802.1Q VLAN Header
 745                         pstEthCsPktInfo->eNwpktEthFrameType = eEth802QVLANFrame;
 746                         u16Etype = ((struct bcm_eth_q_frame *)pvEthPayload)->EthType;
 747                         //((ETH_CS_802_Q_FRAME*)pvEthPayload)->UserPriority
 748                 } else {
 749                         pstEthCsPktInfo->eNwpktEthFrameType = eEthOtherFrame;
 750                         u16Etype = ntohs(u16Etype);
 751                 }
 752         } else {
 753                 //802.2 LLC
 754                 BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "802.2 LLC Frame\n");
 755                 pstEthCsPktInfo->eNwpktEthFrameType = eEth802LLCFrame;
 756                 pstEthCsPktInfo->ucDSAP = ((struct bcm_eth_llc_frame *)pvEthPayload)->DSAP;
 757                 if (pstEthCsPktInfo->ucDSAP == 0xAA && ((struct bcm_eth_llc_frame *)pvEthPayload)->SSAP == 0xAA) {
 758                         //SNAP Frame
 759                         pstEthCsPktInfo->eNwpktEthFrameType = eEth802LLCSNAPFrame;
 760                         u16Etype = ((struct bcm_eth_llc_snap_frame *)pvEthPayload)->usEtherType;
 761                 }
 762         }
 763         if (u16Etype == ETHERNET_FRAMETYPE_IPV4)
 764                 pstEthCsPktInfo->eNwpktIPFrameType = eIPv4Packet;
 765         else if (u16Etype == ETHERNET_FRAMETYPE_IPV6)
 766                 pstEthCsPktInfo->eNwpktIPFrameType = eIPv6Packet;
 767         else
 768                 pstEthCsPktInfo->eNwpktIPFrameType = eNonIPPacket;
 769
 770         pstEthCsPktInfo->usEtherType = ((struct bcm_eth_header *)pvEthPayload)->u16Etype;
 771         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "EthCsPktInfo->eNwpktIPFrameType : %x\n", pstEthCsPktInfo->eNwpktIPFrameType);
 772         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "EthCsPktInfo->eNwpktEthFrameType : %x\n", pstEthCsPktInfo->eNwpktEthFrameType);
 773         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "EthCsPktInfo->usEtherType : %x\n", pstEthCsPktInfo->usEtherType);
 774 }
 775
 776
 777