drivers/staging/bcm/IPv6Protocol.c

   1 #include "headers.h"
   2
   3 static bool MatchSrcIpv6Address(struct bcm_classifier_rule *pstClassifierRule,
   4         struct bcm_ipv6_hdr *pstIpv6Header);
   5 static bool MatchDestIpv6Address(struct bcm_classifier_rule *pstClassifierRule,
   6         struct bcm_ipv6_hdr *pstIpv6Header);
   7 static VOID DumpIpv6Header(struct bcm_ipv6_hdr *pstIpv6Header);
   8
   9 static UCHAR *GetNextIPV6ChainedHeader(UCHAR **ppucPayload,
  10         UCHAR *pucNextHeader, bool *bParseDone, USHORT *pusPayloadLength)
  11 {
  12         UCHAR *pucRetHeaderPtr = NULL;
  13         UCHAR *pucPayloadPtr = NULL;
  14         USHORT  usNextHeaderOffset = 0 ;
  15         struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
  16
  17         if ((ppucPayload == NULL) || (*pusPayloadLength == 0) ||
  18                 (*bParseDone)) {
  19                 *bParseDone = TRUE;
  20                 return NULL;
  21         }
  22
  23         pucRetHeaderPtr = *ppucPayload;
  24         pucPayloadPtr = *ppucPayload;
  25
  26         if (!pucRetHeaderPtr || !pucPayloadPtr) {
  27                 *bParseDone = TRUE;
  28                 return NULL;
  29         }
  30
  31         /* Get the Nextt Header Type */
  32         *bParseDone = false;
  33
  34
  35         switch (*pucNextHeader) {
  36         case IPV6HDR_TYPE_HOPBYHOP:
  37                 {
  38
  39                         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG,
  40                                         DBG_LVL_ALL, "\nIPv6 HopByHop Header");
  41                         usNextHeaderOffset += sizeof(struct bcm_ipv6_options_hdr);
  42                 }
  43                 break;
  44
  45         case IPV6HDR_TYPE_ROUTING:
  46                 {
  47                         struct bcm_ipv6_routing_hdr *pstIpv6RoutingHeader;
  48                         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG,
  49                                         DBG_LVL_ALL, "\nIPv6 Routing Header");
  50                         pstIpv6RoutingHeader = (struct bcm_ipv6_routing_hdr *)pucPayloadPtr;
  51                         usNextHeaderOffset += sizeof(struct bcm_ipv6_routing_hdr);
  52                         usNextHeaderOffset += pstIpv6RoutingHeader->ucNumAddresses * IPV6_ADDRESS_SIZEINBYTES;
  53
  54                 }
  55                 break;
  56         case IPV6HDR_TYPE_FRAGMENTATION:
  57                 {
  58                         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG,
  59                                         DBG_LVL_ALL,
  60                                         "\nIPv6 Fragmentation Header");
  61                         usNextHeaderOffset += sizeof(struct bcm_ipv6_fragment_hdr);
  62
  63                 }
  64                 break;
  65         case IPV6HDR_TYPE_DESTOPTS:
  66                 {
  67                         struct bcm_ipv6_dest_options_hdr *pstIpv6DestOptsHdr = (struct bcm_ipv6_dest_options_hdr *)pucPayloadPtr;
  68                         int nTotalOptions = pstIpv6DestOptsHdr->ucHdrExtLen;
  69                         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG,
  70                                         DBG_LVL_ALL,
  71                                         "\nIPv6 DestOpts Header Header");
  72                         usNextHeaderOffset += sizeof(struct bcm_ipv6_dest_options_hdr);
  73                         usNextHeaderOffset += nTotalOptions * IPV6_DESTOPTS_HDR_OPTIONSIZE ;
  74
  75                 }
  76                 break;
  77         case IPV6HDR_TYPE_AUTHENTICATION:
  78                 {
  79                         struct bcm_ipv6_authentication_hdr *pstIpv6AuthHdr = (struct bcm_ipv6_authentication_hdr *)pucPayloadPtr;
  80                         int nHdrLen = pstIpv6AuthHdr->ucLength;
  81                         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG,
  82                                         DBG_LVL_ALL,
  83                                         "\nIPv6 Authentication Header");
  84                         usNextHeaderOffset += nHdrLen * 4;
  85                 }
  86                 break;
  87         case IPV6HDR_TYPE_ENCRYPTEDSECURITYPAYLOAD:
  88                 {
  89                         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG,
  90                                         DBG_LVL_ALL,
  91                                         "\nIPv6 Encrypted Security Payload Header");
  92                         *bParseDone = TRUE;
  93
  94                 }
  95                 break;
  96         case IPV6_ICMP_HDR_TYPE:
  97                 {
  98                         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG,
  99                                         DBG_LVL_ALL, "\nICMP Header");
 100                         *bParseDone = TRUE;
 101                 }
 102                 break;
 103         case TCP_HEADER_TYPE:
 104                 {
 105                         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG,
 106                                         DBG_LVL_ALL, "\nTCP Header");
 107                         *bParseDone = TRUE;
 108                 }
 109                 break;
 110         case UDP_HEADER_TYPE:
 111                 {
 112                         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG,
 113                                         DBG_LVL_ALL, "\nUDP Header");
 114                         *bParseDone = TRUE;
 115                 }
 116                 break;
 117         default:
 118                 {
 119                         *bParseDone = TRUE;
 120
 121                 }
 122                 break;
 123
 124
 125         }
 126
 127         if (*bParseDone == false) {
 128                 if (*pusPayloadLength <= usNextHeaderOffset) {
 129                         *bParseDone = TRUE;
 130                 } else {
 131                         *pucNextHeader = *pucPayloadPtr;
 132                         pucPayloadPtr += usNextHeaderOffset;
 133                         (*pusPayloadLength) -= usNextHeaderOffset;
 134                 }
 135
 136         }
 137
 138         *ppucPayload = pucPayloadPtr;
 139         return pucRetHeaderPtr;
 140 }
 141
 142
 143 static UCHAR GetIpv6ProtocolPorts(UCHAR *pucPayload, USHORT *pusSrcPort,
 144         USHORT *pusDestPort, USHORT usPayloadLength, UCHAR ucNextHeader)
 145 {
 146         UCHAR *pIpv6HdrScanContext = pucPayload;
 147         bool bDone = false;
 148         UCHAR ucHeaderType = 0;
 149         UCHAR *pucNextHeader = NULL;
 150         struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
 151
 152         if (!pucPayload || (usPayloadLength == 0))
 153                 return 0;
 154
 155         *pusSrcPort = *pusDestPort = 0;
 156         ucHeaderType = ucNextHeader;
 157         while (!bDone) {
 158                 pucNextHeader = GetNextIPV6ChainedHeader(&pIpv6HdrScanContext,
 159                                         &ucHeaderType, &bDone, &usPayloadLength);
 160                 if (bDone) {
 161                         if ((ucHeaderType == TCP_HEADER_TYPE) ||
 162                                 (ucHeaderType == UDP_HEADER_TYPE)) {
 163                                 *pusSrcPort = *((PUSHORT)(pucNextHeader));
 164                                 *pusDestPort = *((PUSHORT)(pucNextHeader+2));
 165                                 BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG,
 166                                                 DBG_LVL_ALL,
 167                                                 "\nProtocol Ports - Src Port :0x%x Dest Port : 0x%x",
 168                                                 ntohs(*pusSrcPort),
 169                                                 ntohs(*pusDestPort));
 170                         }
 171                         break;
 172
 173                 }
 174         }
 175         return ucHeaderType;
 176 }
 177
 178
 179 /*
 180  * Arg 1 struct bcm_mini_adapter *Adapter is a pointer ot the driver contorl structure
 181  * Arg 2 PVOID pcIpHeader is a pointer to the IP header of the packet
 182  */
 183 USHORT  IpVersion6(struct bcm_mini_adapter *Adapter, PVOID pcIpHeader,
 184                                         struct bcm_classifier_rule *pstClassifierRule)
 185 {
 186         USHORT  ushDestPort = 0;
 187         USHORT  ushSrcPort = 0;
 188         UCHAR   ucNextProtocolAboveIP = 0;
 189         struct bcm_ipv6_hdr *pstIpv6Header = NULL;
 190         bool bClassificationSucceed = false;
 191
 192         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG,
 193                         DBG_LVL_ALL, "IpVersion6 ==========>\n");
 194
 195         pstIpv6Header = pcIpHeader;
 196
 197         DumpIpv6Header(pstIpv6Header);
 198
 199         /*
 200          * Try to get the next higher layer protocol
 201          * and the Ports Nos if TCP or UDP
 202          */
 203         ucNextProtocolAboveIP = GetIpv6ProtocolPorts((UCHAR *)(pcIpHeader + sizeof(struct bcm_ipv6_hdr)),
 204                                                         &ushSrcPort,
 205                                                         &ushDestPort,
 206                                                         pstIpv6Header->usPayloadLength,
 207                                                         pstIpv6Header->ucNextHeader);
 208
 209         do {
 210                 if (pstClassifierRule->ucDirection == 0) {
 211                         /*
 212                          * cannot be processed for classification.
 213                          * it is a down link connection
 214                          */
 215                         break;
 216                 }
 217
 218                 if (!pstClassifierRule->bIpv6Protocol) {
 219                         /*
 220                          * We are looking for Ipv6 Classifiers
 221                          * Lets ignore this classifier and try the next one
 222                          */
 223                         break;
 224                 }
 225
 226                 bClassificationSucceed = MatchSrcIpv6Address(pstClassifierRule,
 227                                                                 pstIpv6Header);
 228                 if (!bClassificationSucceed)
 229                         break;
 230
 231                 bClassificationSucceed = MatchDestIpv6Address(pstClassifierRule,
 232                                                                 pstIpv6Header);
 233                 if (!bClassificationSucceed)
 234                         break;
 235
 236                 /*
 237                  * Match the protocol type.
 238                  * For IPv6 the next protocol at end of
 239                  * Chain of IPv6 prot headers
 240                  */
 241                 bClassificationSucceed = MatchProtocol(pstClassifierRule,
 242                                                         ucNextProtocolAboveIP);
 243                 if (!bClassificationSucceed)
 244                         break;
 245
 246                 BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG,
 247                                 DBG_LVL_ALL, "\nIPv6 Protocol Matched");
 248
 249                 if ((ucNextProtocolAboveIP == TCP_HEADER_TYPE) ||
 250                         (ucNextProtocolAboveIP == UDP_HEADER_TYPE)) {
 251                         /* Match Src Port */
 252                         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG,
 253                                         DBG_LVL_ALL, "\nIPv6 Source Port:%x\n",
 254                                         ntohs(ushSrcPort));
 255                         bClassificationSucceed = MatchSrcPort(pstClassifierRule,
 256                                                         ntohs(ushSrcPort));
 257                         if (!bClassificationSucceed)
 258                                 break;
 259
 260                         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG,
 261                                         DBG_LVL_ALL, "\nIPv6 Src Port Matched");
 262
 263                         /* Match Dest Port */
 264                         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG,
 265                                         DBG_LVL_ALL, "\nIPv6 Destination Port:%x\n",
 266                                         ntohs(ushDestPort));
 267                         bClassificationSucceed = MatchDestPort(pstClassifierRule,
 268                                                         ntohs(ushDestPort));
 269                         if (!bClassificationSucceed)
 270                                 break;
 271                         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG,
 272                                         DBG_LVL_ALL, "\nIPv6 Dest Port Matched");
 273                 }
 274         } while (0);
 275
 276         if (bClassificationSucceed == TRUE) {
 277                 INT iMatchedSFQueueIndex = 0;
 278                 iMatchedSFQueueIndex = SearchSfid(Adapter, pstClassifierRule->ulSFID);
 279                 if (iMatchedSFQueueIndex >= NO_OF_QUEUES) {
 280                         bClassificationSucceed = false;
 281                 } else {
 282                         if (Adapter->PackInfo[iMatchedSFQueueIndex].bActive == false)
 283                                 bClassificationSucceed = false;
 284                 }
 285         }
 286
 287         return bClassificationSucceed;
 288 }
 289
 290
 291 static bool MatchSrcIpv6Address(struct bcm_classifier_rule *pstClassifierRule,
 292         struct bcm_ipv6_hdr *pstIpv6Header)
 293 {
 294         UINT uiLoopIndex = 0;
 295         UINT uiIpv6AddIndex = 0;
 296         UINT uiIpv6AddrNoLongWords = 4;
 297         ULONG aulSrcIP[4];
 298         struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
 299         /*
 300          * This is the no. of Src Addresses ie Range of IP Addresses contained
 301          * in the classifier rule for which we need to match
 302          */
 303         UINT  uiCountIPSrcAddresses = (UINT)pstClassifierRule->ucIPSourceAddressLength;
 304
 305
 306         if (uiCountIPSrcAddresses == 0)
 307                 return TRUE;
 308
 309
 310         /* First Convert the Ip Address in the packet to Host Endian order */
 311         for (uiIpv6AddIndex = 0; uiIpv6AddIndex < uiIpv6AddrNoLongWords; uiIpv6AddIndex++)
 312                 aulSrcIP[uiIpv6AddIndex] = ntohl(pstIpv6Header->ulSrcIpAddress[uiIpv6AddIndex]);
 313
 314         for (uiLoopIndex = 0; uiLoopIndex < uiCountIPSrcAddresses; uiLoopIndex += uiIpv6AddrNoLongWords) {
 315                 BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL,
 316                                 "\n Src Ipv6 Address In Received Packet :\n ");
 317                 DumpIpv6Address(aulSrcIP);
 318                 BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL,
 319                                 "\n Src Ipv6 Mask In Classifier Rule:\n");
 320                 DumpIpv6Address(&pstClassifierRule->stSrcIpAddress.ulIpv6Mask[uiLoopIndex]);
 321                 BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL,
 322                                 "\n Src Ipv6 Address In Classifier Rule :\n");
 323                 DumpIpv6Address(&pstClassifierRule->stSrcIpAddress.ulIpv6Addr[uiLoopIndex]);
 324
 325                 for (uiIpv6AddIndex = 0; uiIpv6AddIndex < uiIpv6AddrNoLongWords; uiIpv6AddIndex++) {
 326                         if ((pstClassifierRule->stSrcIpAddress.ulIpv6Mask[uiLoopIndex+uiIpv6AddIndex] & aulSrcIP[uiIpv6AddIndex])
 327                                 != pstClassifierRule->stSrcIpAddress.ulIpv6Addr[uiLoopIndex+uiIpv6AddIndex]) {
 328                                 /*
 329                                  * Match failed for current Ipv6 Address
 330                                  * Try next Ipv6 Address
 331                                  */
 332                                 break;
 333                         }
 334
 335                         if (uiIpv6AddIndex ==  uiIpv6AddrNoLongWords-1) {
 336                                 /* Match Found */
 337                                 BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG,
 338                                                 DBG_LVL_ALL,
 339                                                 "Ipv6 Src Ip Address Matched\n");
 340                                 return TRUE;
 341                         }
 342                 }
 343         }
 344         return false;
 345 }
 346
 347 static bool MatchDestIpv6Address(struct bcm_classifier_rule *pstClassifierRule,
 348         struct bcm_ipv6_hdr *pstIpv6Header)
 349 {
 350         UINT uiLoopIndex = 0;
 351         UINT uiIpv6AddIndex = 0;
 352         UINT uiIpv6AddrNoLongWords = 4;
 353         ULONG aulDestIP[4];
 354         struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
 355         /*
 356          * This is the no. of Destination Addresses
 357          * ie Range of IP Addresses contained in the classifier rule
 358          * for which we need to match
 359          */
 360         UINT  uiCountIPDestinationAddresses = (UINT)pstClassifierRule->ucIPDestinationAddressLength;
 361
 362
 363         if (uiCountIPDestinationAddresses == 0)
 364                 return TRUE;
 365
 366
 367         /* First Convert the Ip Address in the packet to Host Endian order */
 368         for (uiIpv6AddIndex = 0; uiIpv6AddIndex < uiIpv6AddrNoLongWords; uiIpv6AddIndex++)
 369                 aulDestIP[uiIpv6AddIndex] = ntohl(pstIpv6Header->ulDestIpAddress[uiIpv6AddIndex]);
 370
 371         for (uiLoopIndex = 0; uiLoopIndex < uiCountIPDestinationAddresses; uiLoopIndex += uiIpv6AddrNoLongWords) {
 372                 BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL,
 373                                 "\n Destination Ipv6 Address In Received Packet :\n ");
 374                 DumpIpv6Address(aulDestIP);
 375                 BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL,
 376                                 "\n Destination Ipv6 Mask In Classifier Rule :\n");
 377                 DumpIpv6Address(&pstClassifierRule->stDestIpAddress.ulIpv6Mask[uiLoopIndex]);
 378                 BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL,
 379                                 "\n Destination Ipv6 Address In Classifier Rule :\n");
 380                 DumpIpv6Address(&pstClassifierRule->stDestIpAddress.ulIpv6Addr[uiLoopIndex]);
 381
 382                 for (uiIpv6AddIndex = 0; uiIpv6AddIndex < uiIpv6AddrNoLongWords; uiIpv6AddIndex++) {
 383                         if ((pstClassifierRule->stDestIpAddress.ulIpv6Mask[uiLoopIndex+uiIpv6AddIndex] & aulDestIP[uiIpv6AddIndex])
 384                                 != pstClassifierRule->stDestIpAddress.ulIpv6Addr[uiLoopIndex+uiIpv6AddIndex]) {
 385                                 /*
 386                                  * Match failed for current Ipv6 Address.
 387                                  * Try next Ipv6 Address
 388                                  */
 389                                 break;
 390                         }
 391
 392                         if (uiIpv6AddIndex ==  uiIpv6AddrNoLongWords-1) {
 393                                 /* Match Found */
 394                                 BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG,
 395                                                 DBG_LVL_ALL,
 396                                                 "Ipv6 Destination Ip Address Matched\n");
 397                                 return TRUE;
 398                         }
 399                 }
 400         }
 401         return false;
 402
 403 }
 404
 405 VOID DumpIpv6Address(ULONG *puIpv6Address)
 406 {
 407         UINT uiIpv6AddrNoLongWords = 4;
 408         UINT uiIpv6AddIndex = 0;
 409         struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
 410         for (uiIpv6AddIndex = 0; uiIpv6AddIndex < uiIpv6AddrNoLongWords; uiIpv6AddIndex++) {
 411                 BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL,
 412                                 ":%lx", puIpv6Address[uiIpv6AddIndex]);
 413         }
 414
 415 }
 416
 417 static VOID DumpIpv6Header(struct bcm_ipv6_hdr *pstIpv6Header)
 418 {
 419         UCHAR ucVersion;
 420         UCHAR ucPrio;
 421         struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
 422         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL,
 423                         "----Ipv6 Header---");
 424         ucVersion = pstIpv6Header->ucVersionPrio & 0xf0;
 425         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL,
 426                         "Version : %x\n", ucVersion);
 427         ucPrio = pstIpv6Header->ucVersionPrio & 0x0f;
 428         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL,
 429                         "Priority : %x\n", ucPrio);
 430         /*
 431          * BCM_DEBUG_PRINT( Adapter,DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL,
 432          * "Flow Label : %x\n",(pstIpv6Header->ucVersionPrio &0xf0);
 433          */
 434         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL,
 435                         "Payload Length : %x\n",
 436                         ntohs(pstIpv6Header->usPayloadLength));
 437         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL,
 438                         "Next Header : %x\n", pstIpv6Header->ucNextHeader);
 439         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL,
 440                         "Hop Limit : %x\n", pstIpv6Header->ucHopLimit);
 441         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL,
 442                         "Src Address :\n");
 443         DumpIpv6Address(pstIpv6Header->ulSrcIpAddress);
 444         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL,
 445                         "Dest Address :\n");
 446         DumpIpv6Address(pstIpv6Header->ulDestIpAddress);
 447         BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL,
 448                         "----Ipv6 Header End---");
 449
 450
 451 }