drivers/staging/bcm/IPv6Protocol.c

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