drivers/staging/bcm/IPv6Protocol.c

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