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