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