NetworkPkg/Ip6Dxe/Ip6Option.c

   1 /** @file
   2   IP6 option support functions and routines.
   3
   4   Copyright (c) 2009 - 2010, Intel Corporation. All rights reserved.<BR>
   5
   6   SPDX-License-Identifier: BSD-2-Clause-Patent
   7
   8 **/
   9
  10 #include "Ip6Impl.h"
  11
  12 /**
  13   Validate the IP6 option format for both the packets we received
  14   and that we will transmit. It will compute the ICMPv6 error message fields
  15   if the option is malformatted.
  16
  17   @param[in]  IpSb              The IP6 service data.
  18   @param[in]  Packet            The to be validated packet.
  19   @param[in]  Option            The first byte of the option.
  20   @param[in]  OptionLen         The length of the whole option.
  21   @param[in]  Pointer           Identifies the octet offset within
  22                                 the invoking packet where the error was detected.
  23
  24
  25   @retval TRUE     The option is properly formatted.
  26   @retval FALSE    The option is malformatted.
  27
  28 **/
  29 BOOLEAN
  30 Ip6IsOptionValid (
  31   IN IP6_SERVICE            *IpSb,
  32   IN NET_BUF                *Packet,
  33   IN UINT8                  *Option,
  34   IN UINT8                  OptionLen,
  35   IN UINT32                 Pointer
  36   )
  37 {
  38   UINT8                      Offset;
  39   UINT8                      OptionType;
  40
  41   Offset = 0;
  42
  43   while (Offset < OptionLen) {
  44     OptionType = *(Option + Offset);
  45
  46     switch (OptionType) {
  47     case Ip6OptionPad1:
  48       //
  49       // It is a Pad1 option
  50       //
  51       Offset++;
  52       break;
  53     case Ip6OptionPadN:
  54       //
  55       // It is a PadN option
  56       //
  57       Offset = (UINT8) (Offset + *(Option + Offset + 1) + 2);
  58       break;
  59     case Ip6OptionRouterAlert:
  60       //
  61       // It is a Router Alert Option
  62       //
  63       Offset += 4;
  64       break;
  65     default:
  66       //
  67       // The highest-order two bits specify the action must be taken if
  68       // the processing IPv6 node does not recognize the option type.
  69       //
  70       switch (OptionType & Ip6OptionMask) {
  71       case Ip6OptionSkip:
  72         Offset = (UINT8) (Offset + *(Option + Offset + 1));
  73         break;
  74       case Ip6OptionDiscard:
  75         return FALSE;
  76       case Ip6OptionParameterProblem:
  77         Pointer = Pointer + Offset + sizeof (EFI_IP6_HEADER);
  78         Ip6SendIcmpError (
  79           IpSb,
  80           Packet,
  81           NULL,
  82           &Packet->Ip.Ip6->SourceAddress,
  83           ICMP_V6_PARAMETER_PROBLEM,
  84           2,
  85           &Pointer
  86           );
  87         return FALSE;
  88       case Ip6OptionMask:
  89         if (!IP6_IS_MULTICAST (&Packet->Ip.Ip6->DestinationAddress)) {
  90           Pointer = Pointer + Offset + sizeof (EFI_IP6_HEADER);
  91           Ip6SendIcmpError (
  92             IpSb,
  93             Packet,
  94             NULL,
  95             &Packet->Ip.Ip6->SourceAddress,
  96             ICMP_V6_PARAMETER_PROBLEM,
  97             2,
  98             &Pointer
  99             );
 100         }
 101
 102         return FALSE;
 103         break;
 104       }
 105
 106       break;
 107     }
 108
 109   }
 110
 111   return TRUE;
 112 }
 113
 114 /**
 115   Validate the IP6 option format for both the packets we received
 116   and that we will transmit. It supports the defined options in Neighbor
 117   Discovery messages.
 118
 119   @param[in]  Option            The first byte of the option.
 120   @param[in]  OptionLen         The length of the whole option.
 121
 122   @retval TRUE     The option is properly formatted.
 123   @retval FALSE    The option is malformatted.
 124
 125 **/
 126 BOOLEAN
 127 Ip6IsNDOptionValid (
 128   IN UINT8                  *Option,
 129   IN UINT16                 OptionLen
 130   )
 131 {
 132   UINT32                    Offset;
 133   UINT16                    Length;
 134   IP6_OPTION_HEADER         *OptionHeader;
 135
 136   if (Option == NULL) {
 137     ASSERT (Option != NULL);
 138     return FALSE;
 139   }
 140
 141   Offset = 0;
 142
 143   //
 144   // RFC 4861 states that Neighbor Discovery packet can contain zero or more
 145   // options. Start processing the options if at least Type + Length fields
 146   // fit within the input buffer.
 147   //
 148   while (Offset + sizeof (IP6_OPTION_HEADER) - 1 < OptionLen) {
 149     OptionHeader  = (IP6_OPTION_HEADER*) (Option + Offset);
 150     Length        = (UINT16) OptionHeader->Length * 8;
 151
 152     switch (OptionHeader->Type) {
 153     case Ip6OptionPrefixInfo:
 154       if (Length != 32) {
 155         return FALSE;
 156       }
 157       break;
 158
 159     case Ip6OptionMtu:
 160       if (Length != 8) {
 161         return FALSE;
 162       }
 163       break;
 164
 165     default:
 166       // RFC 4861 states that Length field cannot be 0.
 167       if (Length == 0) {
 168         return FALSE;
 169       }
 170       break;
 171     }
 172
 173     //
 174     // Check whether recognized options are within the input buffer's scope.
 175     //
 176     switch (OptionHeader->Type) {
 177     case Ip6OptionEtherSource:
 178     case Ip6OptionEtherTarget:
 179     case Ip6OptionPrefixInfo:
 180     case Ip6OptionRedirected:
 181     case Ip6OptionMtu:
 182       if (Offset + Length > (UINT32) OptionLen) {
 183         return FALSE;
 184       }
 185       break;
 186
 187     default:
 188       //
 189       // Unrecognized options can be either valid (but unused) or invalid
 190       // (garbage in between or right after valid options). Silently ignore.
 191       //
 192       break;
 193     }
 194
 195     //
 196     // Advance to the next option.
 197     // Length already considers option header's Type + Length.
 198     //
 199     Offset += Length;
 200   }
 201
 202   return TRUE;
 203 }
 204
 205
 206 /**
 207   Validate whether the NextHeader is a known valid protocol or one of the user configured
 208   protocols from the upper layer.
 209
 210   @param[in]  IpSb          The IP6 service instance.
 211   @param[in]  NextHeader    The next header field.
 212
 213   @retval TRUE              The NextHeader is a known valid protocol or user configured.
 214   @retval FALSE             The NextHeader is not a known valid protocol.
 215
 216 **/
 217 BOOLEAN
 218 Ip6IsValidProtocol (
 219   IN IP6_SERVICE            *IpSb,
 220   IN UINT8                  NextHeader
 221   )
 222 {
 223   LIST_ENTRY                *Entry;
 224   IP6_PROTOCOL              *IpInstance;
 225
 226   if (NextHeader == EFI_IP_PROTO_TCP ||
 227       NextHeader == EFI_IP_PROTO_UDP ||
 228       NextHeader == IP6_ICMP ||
 229       NextHeader == IP6_ESP
 230       ) {
 231     return TRUE;
 232   }
 233
 234   if (IpSb == NULL) {
 235     return FALSE;
 236   }
 237
 238   if (IpSb->Signature != IP6_SERVICE_SIGNATURE) {
 239     return FALSE;
 240   }
 241
 242   NET_LIST_FOR_EACH (Entry, &IpSb->Children) {
 243     IpInstance = NET_LIST_USER_STRUCT_S (Entry, IP6_PROTOCOL, Link, IP6_PROTOCOL_SIGNATURE);
 244     if (IpInstance->State == IP6_STATE_CONFIGED) {
 245       if (IpInstance->ConfigData.DefaultProtocol == NextHeader) {
 246         return TRUE;
 247       }
 248     }
 249   }
 250
 251   return FALSE;
 252 }
 253
 254 /**
 255   Validate the IP6 extension header format for both the packets we received
 256   and that we will transmit. It will compute the ICMPv6 error message fields
 257   if the option is mal-formatted.
 258
 259   @param[in]  IpSb          The IP6 service instance. This is an optional parameter.
 260   @param[in]  Packet        The data of the packet. Ignored if NULL.
 261   @param[in]  NextHeader    The next header field in IPv6 basic header.
 262   @param[in]  ExtHdrs       The first byte of the option.
 263   @param[in]  ExtHdrsLen    The length of the whole option.
 264   @param[in]  Rcvd          The option is from the packet we received if TRUE,
 265                             otherwise, the option we want to transmit.
 266   @param[out] FormerHeader  The offset of NextHeader which points to Fragment
 267                             Header when we received, of the ExtHdrs.
 268                             Ignored if we transmit.
 269   @param[out] LastHeader    The pointer of NextHeader of the last extension
 270                             header processed by IP6.
 271   @param[out] RealExtsLen   The length of extension headers processed by IP6 layer.
 272                             This is an optional parameter that may be NULL.
 273   @param[out] UnFragmentLen The length of unfragmented length of extension headers.
 274                             This is an optional parameter that may be NULL.
 275   @param[out] Fragmented    Indicate whether the packet is fragmented.
 276                             This is an optional parameter that may be NULL.
 277
 278   @retval     TRUE          The option is properly formatted.
 279   @retval     FALSE         The option is malformatted.
 280
 281 **/
 282 BOOLEAN
 283 Ip6IsExtsValid (
 284   IN IP6_SERVICE            *IpSb           OPTIONAL,
 285   IN NET_BUF                *Packet         OPTIONAL,
 286   IN UINT8                  *NextHeader,
 287   IN UINT8                  *ExtHdrs,
 288   IN UINT32                 ExtHdrsLen,
 289   IN BOOLEAN                Rcvd,
 290   OUT UINT32                *FormerHeader   OPTIONAL,
 291   OUT UINT8                 **LastHeader,
 292   OUT UINT32                *RealExtsLen    OPTIONAL,
 293   OUT UINT32                *UnFragmentLen  OPTIONAL,
 294   OUT BOOLEAN               *Fragmented     OPTIONAL
 295   )
 296 {
 297   UINT32                     Pointer;
 298   UINT32                     Offset;
 299   UINT8                      *Option;
 300   UINT8                      OptionLen;
 301   BOOLEAN                    Flag;
 302   UINT8                      CountD;
 303   UINT8                      CountA;
 304   IP6_FRAGMENT_HEADER        *FragmentHead;
 305   UINT16                     FragmentOffset;
 306   IP6_ROUTING_HEADER         *RoutingHead;
 307
 308   if (RealExtsLen != NULL) {
 309     *RealExtsLen = 0;
 310   }
 311
 312   if (UnFragmentLen != NULL) {
 313     *UnFragmentLen = 0;
 314   }
 315
 316   if (Fragmented != NULL) {
 317     *Fragmented = FALSE;
 318   }
 319
 320   *LastHeader = NextHeader;
 321
 322   if (ExtHdrs == NULL && ExtHdrsLen == 0) {
 323     return TRUE;
 324   }
 325
 326   if ((ExtHdrs == NULL && ExtHdrsLen != 0) || (ExtHdrs != NULL && ExtHdrsLen == 0)) {
 327     return FALSE;
 328   }
 329
 330   Pointer = 0;
 331   Offset  = 0;
 332   Flag    = FALSE;
 333   CountD  = 0;
 334   CountA  = 0;
 335
 336   while (Offset <= ExtHdrsLen) {
 337
 338     switch (*NextHeader) {
 339     case IP6_HOP_BY_HOP:
 340       if (Offset != 0) {
 341         if (!Rcvd) {
 342           return FALSE;
 343         }
 344         //
 345         // Hop-by-Hop Options header is restricted to appear immediately after an IPv6 header only.
 346         // If not, generate a ICMP parameter problem message with code value of 1.
 347         //
 348         if (Pointer == 0) {
 349           Pointer = sizeof (EFI_IP6_HEADER);
 350         } else {
 351           Pointer = Offset + sizeof (EFI_IP6_HEADER);
 352         }
 353
 354         if ((IpSb != NULL) && (Packet != NULL) &&
 355             !IP6_IS_MULTICAST (&Packet->Ip.Ip6->DestinationAddress)) {
 356           Ip6SendIcmpError (
 357             IpSb,
 358             Packet,
 359             NULL,
 360             &Packet->Ip.Ip6->SourceAddress,
 361             ICMP_V6_PARAMETER_PROBLEM,
 362             1,
 363             &Pointer
 364             );
 365         }
 366         return FALSE;
 367       }
 368
 369       Flag = TRUE;
 370
 371     //
 372     // Fall through
 373     //
 374     case IP6_DESTINATION:
 375       if (*NextHeader == IP6_DESTINATION) {
 376         CountD++;
 377       }
 378
 379       if (CountD > 2) {
 380         return FALSE;
 381       }
 382
 383       NextHeader = ExtHdrs + Offset;
 384       Pointer    = Offset;
 385
 386       Offset++;
 387       Option     = ExtHdrs + Offset;
 388       OptionLen  = (UINT8) ((*Option + 1) * 8 - 2);
 389       Option++;
 390       Offset++;
 391
 392       if (IpSb != NULL && Packet != NULL && !Ip6IsOptionValid (IpSb, Packet, Option, OptionLen, Offset)) {
 393         return FALSE;
 394       }
 395
 396       Offset = Offset + OptionLen;
 397
 398       if (Flag) {
 399         if (UnFragmentLen != NULL) {
 400           *UnFragmentLen = Offset;
 401         }
 402
 403         Flag = FALSE;
 404       }
 405
 406       break;
 407
 408     case IP6_ROUTING:
 409       NextHeader = ExtHdrs + Offset;
 410       RoutingHead = (IP6_ROUTING_HEADER *) NextHeader;
 411
 412       //
 413       // Type 0 routing header is defined in RFC2460 and deprecated in RFC5095.
 414       // Thus all routing types are processed as unrecognized.
 415       //
 416       if (RoutingHead->SegmentsLeft == 0) {
 417         //
 418         // Ignore the routing header and proceed to process the next header.
 419         //
 420         Offset = Offset + (RoutingHead->HeaderLen + 1) * 8;
 421
 422         if (UnFragmentLen != NULL) {
 423           *UnFragmentLen = Offset;
 424         }
 425
 426       } else {
 427         //
 428         // Discard the packet and send an ICMP Parameter Problem, Code 0, message
 429         // to the packet's source address, pointing to the unrecognized routing
 430         // type.
 431         //
 432         Pointer = Offset + 2 + sizeof (EFI_IP6_HEADER);
 433         if ((IpSb != NULL) && (Packet != NULL) &&
 434             !IP6_IS_MULTICAST (&Packet->Ip.Ip6->DestinationAddress)) {
 435           Ip6SendIcmpError (
 436             IpSb,
 437             Packet,
 438             NULL,
 439             &Packet->Ip.Ip6->SourceAddress,
 440             ICMP_V6_PARAMETER_PROBLEM,
 441             0,
 442             &Pointer
 443             );
 444         }
 445
 446         return FALSE;
 447       }
 448
 449       break;
 450
 451     case IP6_FRAGMENT:
 452
 453       //
 454       // RFC2402, AH header should after fragment header.
 455       //
 456       if (CountA > 1) {
 457         return FALSE;
 458       }
 459
 460       //
 461       // RFC2460, ICMP Parameter Problem message with code 0 should be sent
 462       // if the length of a fragment is not a multiple of 8 octets and the M
 463       // flag of that fragment is 1, pointing to the Payload length field of the
 464       // fragment packet.
 465       //
 466       if (IpSb != NULL && Packet != NULL && (ExtHdrsLen % 8) != 0) {
 467         //
 468         // Check whether it is the last fragment.
 469         //
 470         FragmentHead = (IP6_FRAGMENT_HEADER *) (ExtHdrs + Offset);
 471         if (FragmentHead == NULL) {
 472           return FALSE;
 473         }
 474
 475         FragmentOffset = NTOHS (FragmentHead->FragmentOffset);
 476
 477         if (((FragmentOffset & 0x1) == 0x1) &&
 478             !IP6_IS_MULTICAST (&Packet->Ip.Ip6->DestinationAddress)) {
 479           Pointer = sizeof (UINT32);
 480           Ip6SendIcmpError (
 481             IpSb,
 482             Packet,
 483             NULL,
 484             &Packet->Ip.Ip6->SourceAddress,
 485             ICMP_V6_PARAMETER_PROBLEM,
 486             0,
 487             &Pointer
 488             );
 489           return FALSE;
 490         }
 491       }
 492
 493       if (Fragmented != NULL) {
 494         *Fragmented = TRUE;
 495       }
 496
 497       if (Rcvd && FormerHeader != NULL) {
 498         *FormerHeader = (UINT32) (NextHeader - ExtHdrs);
 499       }
 500
 501       NextHeader = ExtHdrs + Offset;
 502       Offset     = Offset + 8;
 503       break;
 504
 505     case IP6_AH:
 506       if (++CountA > 1) {
 507         return FALSE;
 508       }
 509
 510       Option     = ExtHdrs + Offset;
 511       NextHeader = Option;
 512       Option++;
 513       //
 514       // RFC2402, Payload length is specified in 32-bit words, minus "2".
 515       //
 516       OptionLen  = (UINT8) ((*Option + 2) * 4);
 517       Offset     = Offset + OptionLen;
 518       break;
 519
 520     case IP6_NO_NEXT_HEADER:
 521       *LastHeader = NextHeader;
 522       return FALSE;
 523       break;
 524
 525     default:
 526       if (Ip6IsValidProtocol (IpSb, *NextHeader)) {
 527
 528         *LastHeader = NextHeader;
 529
 530         if (RealExtsLen != NULL) {
 531           *RealExtsLen = Offset;
 532         }
 533
 534         return TRUE;
 535       }
 536
 537       //
 538       // The Next Header value is unrecognized by the node, discard the packet and
 539       // send an ICMP parameter problem message with code value of 1.
 540       //
 541       if (Offset == 0) {
 542         //
 543         // The Next Header directly follows IPv6 basic header.
 544         //
 545         Pointer = 6;
 546       } else {
 547         if (Pointer == 0) {
 548           Pointer = sizeof (EFI_IP6_HEADER);
 549         } else {
 550           Pointer = Offset + sizeof (EFI_IP6_HEADER);
 551         }
 552       }
 553
 554       if ((IpSb != NULL) && (Packet != NULL) &&
 555           !IP6_IS_MULTICAST (&Packet->Ip.Ip6->DestinationAddress)) {
 556         Ip6SendIcmpError (
 557           IpSb,
 558           Packet,
 559           NULL,
 560           &Packet->Ip.Ip6->SourceAddress,
 561           ICMP_V6_PARAMETER_PROBLEM,
 562           1,
 563           &Pointer
 564           );
 565       }
 566       return FALSE;
 567     }
 568   }
 569
 570   *LastHeader = NextHeader;
 571
 572   if (RealExtsLen != NULL) {
 573     *RealExtsLen = Offset;
 574   }
 575
 576   return TRUE;
 577 }
 578
 579 /**
 580   Generate an IPv6 router alert option in network order and output it through Buffer.
 581
 582   @param[out]     Buffer         Points to a buffer to record the generated option.
 583   @param[in, out] BufferLen      The length of Buffer, in bytes.
 584   @param[in]      NextHeader     The 8-bit selector indicates the type of header
 585                                  immediately following the Hop-by-Hop Options header.
 586
 587   @retval EFI_BUFFER_TOO_SMALL   The Buffer is too small to contain the generated
 588                                  option. BufferLen is updated for the required size.
 589
 590   @retval EFI_SUCCESS            The option is generated and filled in to Buffer.
 591
 592 **/
 593 EFI_STATUS
 594 Ip6FillHopByHop (
 595   OUT UINT8                  *Buffer,
 596   IN OUT UINTN               *BufferLen,
 597   IN UINT8                   NextHeader
 598   )
 599 {
 600   UINT8                      BufferArray[8];
 601
 602   if (*BufferLen < 8) {
 603     *BufferLen = 8;
 604     return EFI_BUFFER_TOO_SMALL;
 605   }
 606
 607   //
 608   // Form the Hop-By-Hop option in network order.
 609   // NextHeader (1 octet) + HdrExtLen (1 octet) + RouterAlertOption(4 octets) + PadN
 610   // The Hdr Ext Len is the length in 8-octet units, and does not including the first 8 octets.
 611   //
 612   ZeroMem (BufferArray, sizeof (BufferArray));
 613   BufferArray[0] = NextHeader;
 614   BufferArray[2] = 0x5;
 615   BufferArray[3] = 0x2;
 616   BufferArray[6] = 1;
 617
 618   CopyMem (Buffer, BufferArray, sizeof (BufferArray));
 619   return EFI_SUCCESS;
 620 }
 621
 622 /**
 623   Insert a Fragment Header to the Extension headers and output it in UpdatedExtHdrs.
 624
 625   @param[in]  IpSb             The IP6 service instance to transmit the packet.
 626   @param[in]  NextHeader       The extension header type of first extension header.
 627   @param[in]  LastHeader       The extension header type of last extension header.
 628   @param[in]  ExtHdrs          The length of the original extension header.
 629   @param[in]  ExtHdrsLen       The length of the extension headers.
 630   @param[in]  FragmentOffset   The fragment offset of the data following the header.
 631   @param[out] UpdatedExtHdrs   The updated ExtHdrs with Fragment header inserted.
 632                                It's caller's responsibility to free this buffer.
 633
 634   @retval EFI_OUT_OF_RESOURCES Failed to finish the operation due to lake of
 635                                resource.
 636   @retval EFI_UNSUPPORTED      The extension header specified in ExtHdrs is not
 637                                supported currently.
 638   @retval EFI_SUCCESS          The operation performed successfully.
 639
 640 **/
 641 EFI_STATUS
 642 Ip6FillFragmentHeader (
 643   IN  IP6_SERVICE           *IpSb,
 644   IN  UINT8                 NextHeader,
 645   IN  UINT8                 LastHeader,
 646   IN  UINT8                 *ExtHdrs,
 647   IN  UINT32                ExtHdrsLen,
 648   IN  UINT16                FragmentOffset,
 649   OUT UINT8                 **UpdatedExtHdrs
 650   )
 651 {
 652   UINT32                    Length;
 653   UINT8                     *Buffer;
 654   UINT32                    FormerHeader;
 655   UINT32                    Offset;
 656   UINT32                    Part1Len;
 657   UINT32                    HeaderLen;
 658   UINT8                     Current;
 659   IP6_FRAGMENT_HEADER       FragmentHead;
 660
 661   if (UpdatedExtHdrs == NULL) {
 662     return EFI_INVALID_PARAMETER;
 663   }
 664
 665   Length = ExtHdrsLen + sizeof (IP6_FRAGMENT_HEADER);
 666   Buffer = AllocatePool (Length);
 667   if (Buffer == NULL) {
 668     return EFI_OUT_OF_RESOURCES;
 669   }
 670
 671   Offset         = 0;
 672   Part1Len       = 0;
 673   FormerHeader   = 0;
 674   Current        = NextHeader;
 675
 676   while ((ExtHdrs != NULL) && (Offset <= ExtHdrsLen)) {
 677     switch (NextHeader) {
 678     case IP6_ROUTING:
 679     case IP6_HOP_BY_HOP:
 680     case IP6_DESTINATION:
 681       Current      = NextHeader;
 682       NextHeader   = *(ExtHdrs + Offset);
 683
 684       if ((Current == IP6_DESTINATION) && (NextHeader != IP6_ROUTING)) {
 685         //
 686         // Destination Options header should occur at most twice, once before
 687         // a Routing header and once before the upper-layer header. Here we
 688         // find the one before the upper-layer header. Insert the Fragment
 689         // Header before it.
 690         //
 691         CopyMem (Buffer, ExtHdrs, Part1Len);
 692         *(Buffer + FormerHeader) = IP6_FRAGMENT;
 693         //
 694         // Exit the loop.
 695         //
 696         Offset = ExtHdrsLen + 1;
 697         break;
 698       }
 699
 700
 701       FormerHeader = Offset;
 702       HeaderLen    = (*(ExtHdrs + Offset + 1) + 1) * 8;
 703       Part1Len     = Part1Len + HeaderLen;
 704       Offset       = Offset + HeaderLen;
 705       break;
 706
 707     case IP6_FRAGMENT:
 708       Current    = NextHeader;
 709
 710       if (Part1Len != 0) {
 711         CopyMem (Buffer, ExtHdrs, Part1Len);
 712       }
 713
 714       *(Buffer + FormerHeader) = IP6_FRAGMENT;
 715
 716       //
 717       // Exit the loop.
 718       //
 719       Offset = ExtHdrsLen + 1;
 720       break;
 721
 722     case IP6_AH:
 723       Current    = NextHeader;
 724       NextHeader = *(ExtHdrs + Offset);
 725       //
 726       // RFC2402, Payload length is specified in 32-bit words, minus "2".
 727       //
 728       HeaderLen  = (*(ExtHdrs + Offset + 1) + 2) * 4;
 729       Part1Len   = Part1Len + HeaderLen;
 730       Offset     = Offset + HeaderLen;
 731       break;
 732
 733     default:
 734       if (Ip6IsValidProtocol (IpSb, NextHeader)) {
 735         Current = NextHeader;
 736         CopyMem (Buffer, ExtHdrs, Part1Len);
 737         *(Buffer + FormerHeader) = IP6_FRAGMENT;
 738         //
 739         // Exit the loop.
 740         //
 741         Offset = ExtHdrsLen + 1;
 742         break;
 743       }
 744
 745       FreePool (Buffer);
 746       return EFI_UNSUPPORTED;
 747     }
 748   }
 749
 750   //
 751   // Append the Fragment header. If the fragment offset indicates the fragment
 752   // is the first fragment.
 753   //
 754   if ((FragmentOffset & IP6_FRAGMENT_OFFSET_MASK) == 0) {
 755     FragmentHead.NextHeader = Current;
 756   } else {
 757     FragmentHead.NextHeader = LastHeader;
 758   }
 759
 760   FragmentHead.Reserved       = 0;
 761   FragmentHead.FragmentOffset = HTONS (FragmentOffset);
 762   FragmentHead.Identification = mIp6Id;
 763
 764   CopyMem (Buffer + Part1Len, &FragmentHead, sizeof (IP6_FRAGMENT_HEADER));
 765
 766   if ((ExtHdrs != NULL) && (Part1Len < ExtHdrsLen)) {
 767     //
 768     // Append the part2 (fragmentable part) of Extension headers
 769     //
 770     CopyMem (
 771       Buffer + Part1Len + sizeof (IP6_FRAGMENT_HEADER),
 772       ExtHdrs + Part1Len,
 773       ExtHdrsLen - Part1Len
 774       );
 775   }
 776
 777   *UpdatedExtHdrs = Buffer;
 778
 779   return EFI_SUCCESS;
 780 }
 781