]> git.proxmox.com Git - mirror_edk2.git/blobdiff - NetworkPkg/Ip6Dxe/Ip6Input.c
Fix a bug in IP driver that the fragment overlap check may be skipped incorrectly.
[mirror_edk2.git] / NetworkPkg / Ip6Dxe / Ip6Input.c
index c18811b611f2576aae08a42ce5832ea756c04827..cf88884e381b20f088b1b791d9540051143ba1f2 100644 (file)
@@ -1,7 +1,7 @@
 /** @file\r
   IP6 internal functions to process the incoming packets.\r
 \r
-  Copyright (c) 2009 - 2010, Intel Corporation. All rights reserved.<BR>\r
+  Copyright (c) 2009 - 2014, Intel Corporation. All rights reserved.<BR>\r
 \r
   This program and the accompanying materials\r
   are licensed and made available under the terms and conditions of the BSD License\r
@@ -243,7 +243,7 @@ Ip6Reassemble (
   // check whether THIS.Start < PREV.End for overlap. If two fragments\r
   // overlaps, trim the overlapped part off THIS fragment.\r
   //\r
-  if ((Cur != ListHead) && ((Prev = Cur->BackLink) != ListHead)) {\r
+  if ((Prev = Cur->BackLink) != ListHead) {\r
     Fragment  = NET_LIST_USER_STRUCT (Prev, NET_BUF, List);\r
     Node      = IP6_GET_CLIP_INFO (Fragment);\r
 \r
@@ -481,11 +481,11 @@ Ip6IpSecFree (
   actions: bypass the packet, discard the packet, or protect the packet.\r
 \r
   @param[in]       IpSb          The IP6 service instance.\r
-  @param[in]       Head          The caller-supplied IP6 header.\r
+  @param[in, out]  Head          The caller-supplied IP6 header.\r
   @param[in, out]  LastHead      The next header field of last IP header.\r
   @param[in, out]  Netbuf        The IP6 packet to be processed by IPsec.\r
-  @param[in]       ExtHdrs       The caller-supplied options.\r
-  @param[in]       ExtHdrsLen    The length of the option.\r
+  @param[in, out]  ExtHdrs       The caller-supplied options.\r
+  @param[in, out]  ExtHdrsLen    The length of the option.\r
   @param[in]       Direction     The directionality in an SPD entry,\r
                                  EfiIPsecInBound, or EfiIPsecOutBound.\r
   @param[in]       Context       The token's wrap.\r
@@ -501,18 +501,20 @@ Ip6IpSecFree (
 **/\r
 EFI_STATUS\r
 Ip6IpSecProcessPacket (\r
-  IN IP6_SERVICE            *IpSb,\r
-  IN EFI_IP6_HEADER         *Head,\r
-  IN OUT UINT8              *LastHead,\r
-  IN OUT NET_BUF            **Netbuf,\r
-  IN VOID                   *ExtHdrs,\r
-  IN UINT32                 ExtHdrsLen,\r
-  IN EFI_IPSEC_TRAFFIC_DIR  Direction,\r
-  IN VOID                   *Context\r
+  IN     IP6_SERVICE            *IpSb,\r
+  IN OUT EFI_IP6_HEADER         **Head,\r
+  IN OUT UINT8                  *LastHead,\r
+  IN OUT NET_BUF                **Netbuf,\r
+  IN OUT UINT8                  **ExtHdrs,\r
+  IN OUT UINT32                 *ExtHdrsLen,\r
+  IN     EFI_IPSEC_TRAFFIC_DIR  Direction,\r
+  IN     VOID                   *Context\r
   )\r
 {\r
   NET_FRAGMENT              *FragmentTable;\r
+  NET_FRAGMENT              *OriginalFragmentTable;\r
   UINT32                    FragmentCount;\r
+  UINT32                    OriginalFragmentCount;\r
   EFI_EVENT                 RecycleEvent;\r
   NET_BUF                   *Packet;\r
   IP6_TXTOKEN_WRAP          *TxWrap;\r
@@ -520,6 +522,7 @@ Ip6IpSecProcessPacket (
   EFI_STATUS                Status;\r
   EFI_IP6_HEADER            *PacketHead;\r
   UINT8                     *Buf;\r
+  EFI_IP6_HEADER            ZeroHead;\r
 \r
   Status        = EFI_SUCCESS;\r
   Packet        = *Netbuf;\r
@@ -530,9 +533,10 @@ Ip6IpSecProcessPacket (
   Buf           = NULL;\r
   TxWrap        = (IP6_TXTOKEN_WRAP *) Context;\r
   FragmentCount = Packet->BlockOpNum;\r
+  ZeroMem (&ZeroHead, sizeof (EFI_IP6_HEADER));\r
 \r
   if (mIpSec == NULL) {\r
-    gBS->LocateProtocol (&gEfiIpSecProtocolGuid, NULL, (VOID **) &mIpSec);\r
+    gBS->LocateProtocol (&gEfiIpSec2ProtocolGuid, NULL, (VOID **) &mIpSec);\r
 \r
     //\r
     // Check whether the ipsec protocol is available.\r
@@ -562,7 +566,7 @@ Ip6IpSecProcessPacket (
   //\r
   // Bypass all multicast inbound or outbound traffic.\r
   //\r
-  if (IP6_IS_MULTICAST (&Head->DestinationAddress) || IP6_IS_MULTICAST (&Head->SourceAddress)) {\r
+  if (IP6_IS_MULTICAST (&(*Head)->DestinationAddress) || IP6_IS_MULTICAST (&(*Head)->SourceAddress)) {\r
     goto ON_EXIT;\r
   }\r
 \r
@@ -577,6 +581,8 @@ Ip6IpSecProcessPacket (
   }\r
 \r
   Status = NetbufBuildExt (Packet, FragmentTable, &FragmentCount);\r
+  OriginalFragmentTable = FragmentTable;\r
+  OriginalFragmentCount = FragmentCount;\r
 \r
   if (EFI_ERROR(Status)) {\r
     FreePool (FragmentTable);\r
@@ -586,16 +592,16 @@ Ip6IpSecProcessPacket (
   //\r
   // Convert host byte order to network byte order\r
   //\r
-  Ip6NtohHead (Head);\r
+  Ip6NtohHead (*Head);\r
 \r
-  Status = mIpSec->Process (\r
+  Status = mIpSec->ProcessExt (\r
                      mIpSec,\r
                      IpSb->Controller,\r
                      IP_VERSION_6,\r
-                     (VOID *) Head,\r
+                     (VOID *) (*Head),\r
                      LastHead,\r
-                     NULL,\r
-                     0,\r
+                     (VOID **) ExtHdrs,\r
+                     ExtHdrsLen,\r
                      (EFI_IPSEC_FRAGMENT_DATA  **) (&FragmentTable),\r
                      &FragmentCount,\r
                      Direction,\r
@@ -604,14 +610,27 @@ Ip6IpSecProcessPacket (
   //\r
   // Convert back to host byte order\r
   //\r
-  Ip6NtohHead (Head);\r
+  Ip6NtohHead (*Head);\r
 \r
   if (EFI_ERROR (Status)) {\r
+    FreePool (OriginalFragmentTable);\r
     goto ON_EXIT;\r
   }\r
 \r
-  if (Direction == EfiIPsecOutBound && TxWrap != NULL) {\r
+  if (OriginalFragmentCount == FragmentCount && OriginalFragmentTable == FragmentTable) {\r
+    //\r
+    // For ByPass Packet\r
+    //\r
+    FreePool (FragmentTable);\r
+    goto ON_EXIT;\r
+  } else {\r
+    //\r
+    // Free the FragmentTable which allocated before calling the IPsec.\r
+    //\r
+    FreePool (OriginalFragmentTable);\r
+  }\r
 \r
+  if (Direction == EfiIPsecOutBound && TxWrap != NULL) {\r
     TxWrap->IpSecRecycleSignal = RecycleEvent;\r
     TxWrap->Packet             = NetbufFromExt (\r
                                    FragmentTable,\r
@@ -622,10 +641,18 @@ Ip6IpSecProcessPacket (
                                    TxWrap\r
                                    );\r
     if (TxWrap->Packet == NULL) {\r
+      TxWrap->Packet = *Netbuf;\r
       Status = EFI_OUT_OF_RESOURCES;\r
       goto ON_EXIT;\r
     }\r
 \r
+    CopyMem (\r
+      IP6_GET_CLIP_INFO (TxWrap->Packet),\r
+      IP6_GET_CLIP_INFO (Packet),\r
+      sizeof (IP6_CLIP_INFO)\r
+      );\r
+    \r
+    NetIpSecNetbufFree(Packet);\r
     *Netbuf = TxWrap->Packet;\r
 \r
   } else {\r
@@ -633,6 +660,8 @@ Ip6IpSecProcessPacket (
     IpSecWrap = AllocateZeroPool (sizeof (IP6_IPSEC_WRAP));\r
 \r
     if (IpSecWrap == NULL) {\r
+      Status = EFI_OUT_OF_RESOURCES;\r
+      gBS->SignalEvent (RecycleEvent);\r
       goto ON_EXIT;\r
     }\r
 \r
@@ -648,38 +677,42 @@ Ip6IpSecProcessPacket (
                                       );\r
 \r
     if (Packet == NULL) {\r
+      Packet = IpSecWrap->Packet;\r
+      gBS->SignalEvent (RecycleEvent);\r
+      FreePool (IpSecWrap);\r
       Status = EFI_OUT_OF_RESOURCES;\r
       goto ON_EXIT;\r
     }\r
 \r
-    if (Direction == EfiIPsecInBound) {\r
+    if (Direction == EfiIPsecInBound && 0 != CompareMem (&ZeroHead, *Head, sizeof (EFI_IP6_HEADER))) {\r
 \r
       PacketHead = (EFI_IP6_HEADER *) NetbufAllocSpace (\r
                                         Packet,\r
-                                        sizeof (EFI_IP6_HEADER) + ExtHdrsLen,\r
+                                        sizeof (EFI_IP6_HEADER) + *ExtHdrsLen,\r
                                         NET_BUF_HEAD\r
                                         );\r
       if (PacketHead == NULL) {\r
-        Status = EFI_OUT_OF_RESOURCES;\r
+        *Netbuf = Packet;\r
+        Status  = EFI_OUT_OF_RESOURCES;\r
         goto ON_EXIT;\r
       }\r
 \r
-      CopyMem (PacketHead, Head, sizeof (EFI_IP6_HEADER));\r
+      CopyMem (PacketHead, *Head, sizeof (EFI_IP6_HEADER));\r
+      *Head = PacketHead;\r
       Packet->Ip.Ip6 = PacketHead;\r
 \r
-      if (ExtHdrs != NULL) {\r
+      if (*ExtHdrs != NULL) {\r
         Buf = (UINT8 *) (PacketHead + 1);\r
-        CopyMem (Buf, ExtHdrs, ExtHdrsLen);\r
+        CopyMem (Buf, *ExtHdrs, *ExtHdrsLen);\r
       }\r
 \r
-      NetbufTrim (Packet, sizeof (EFI_IP6_HEADER) + ExtHdrsLen, TRUE);\r
+      NetbufTrim (Packet, sizeof (EFI_IP6_HEADER) + *ExtHdrsLen, TRUE);\r
       CopyMem (\r
         IP6_GET_CLIP_INFO (Packet),\r
         IP6_GET_CLIP_INFO (IpSecWrap->Packet),\r
         sizeof (IP6_CLIP_INFO)\r
         );\r
     }\r
-\r
     *Netbuf = Packet;\r
   }\r
 \r
@@ -688,73 +721,70 @@ ON_EXIT:
 }\r
 \r
 /**\r
-  The IP6 input routine. It is called by the IP6_INTERFACE when an\r
-  IP6 fragment is received from MNP.\r
-\r
-  @param[in]  Packet             The IP6 packet received.\r
-  @param[in]  IoStatus           The return status of receive request.\r
-  @param[in]  Flag               The link layer flag for the packet received, such\r
-                                 as multicast.\r
-  @param[in]  Context            The IP6 service instance that owns the MNP.\r
+  Pre-process the IPv6 packet. First validates the IPv6 packet, and\r
+  then reassembles packet if it is necessary.\r
+\r
+  @param[in]      IpSb          The IP6 service instance.\r
+  @param[in, out] Packet        The received IP6 packet to be processed.\r
+  @param[in]      Flag          The link layer flag for the packet received, such\r
+                                as multicast.\r
+  @param[out]     Payload       The pointer to the payload of the recieved packet. \r
+                                it starts from the first byte of the extension header.                                 \r
+  @param[out]     LastHead      The pointer of NextHeader of the last extension\r
+                                header processed by IP6.\r
+  @param[out]     ExtHdrsLen    The length of the whole option.\r
+  @param[out]     UnFragmentLen The length of unfragmented length of extension headers.\r
+  @param[out]     Fragmented    Indicate whether the packet is fragmented. \r
+  @param[out]     Head          The pointer to the EFI_IP6_Header.\r
+\r
+  @retval     EFI_SUCCESS              The received packet is well format.\r
+  @retval     EFI_INVALID_PARAMETER    The received packet is malformed.\r
 \r
 **/\r
-VOID\r
-Ip6AcceptFrame (\r
-  IN NET_BUF                *Packet,\r
-  IN EFI_STATUS             IoStatus,\r
-  IN UINT32                 Flag,\r
-  IN VOID                   *Context\r
+EFI_STATUS\r
+Ip6PreProcessPacket (\r
+  IN     IP6_SERVICE     *IpSb,\r
+  IN OUT NET_BUF         **Packet,\r
+  IN     UINT32          Flag,\r
+     OUT UINT8           **Payload,\r
+     OUT UINT8           **LastHead,\r
+     OUT UINT32          *ExtHdrsLen,\r
+     OUT UINT32          *UnFragmentLen,\r
+     OUT BOOLEAN         *Fragmented, \r
+     OUT EFI_IP6_HEADER  **Head\r
   )\r
 {\r
-  IP6_SERVICE               *IpSb;\r
-  IP6_CLIP_INFO             *Info;\r
-  EFI_IP6_HEADER            *Head;\r
   UINT16                    PayloadLen;\r
-  UINT8                     *Payload;\r
   UINT16                    TotalLen;\r
-  UINT8                     *LastHead;\r
   UINT32                    FormerHeadOffset;\r
-  UINT32                    UnFragmentLen;\r
-  UINT32                    ExtHdrsLen;\r
   UINT32                    HeadLen;\r
-  BOOLEAN                   Fragmented;\r
   IP6_FRAGMENT_HEADER       *FragmentHead;\r
   UINT16                    FragmentOffset;\r
-  EFI_STATUS                Status;\r
+  IP6_CLIP_INFO             *Info;\r
   EFI_IPv6_ADDRESS          Loopback;\r
 \r
-  IpSb = (IP6_SERVICE *) Context;\r
-  NET_CHECK_SIGNATURE (IpSb, IP6_SERVICE_SIGNATURE);\r
-\r
-  Payload = NULL;\r
-\r
-  //\r
-  // Check input parameters\r
-  //\r
-  if (EFI_ERROR (IoStatus) || (IpSb->State == IP6_SERVICE_DESTROY)) {\r
-    goto Drop;\r
-  }\r
-\r
+  HeadLen    = 0;\r
+  PayloadLen = 0;\r
   //\r
   // Check whether the input packet is a valid packet\r
   //\r
-  if (Packet->TotalSize < IP6_MIN_HEADLEN) {\r
-    goto Restart;\r
+  if ((*Packet)->TotalSize < IP6_MIN_HEADLEN) {\r
+    return EFI_INVALID_PARAMETER;\r
   }\r
 \r
   //\r
   // Get header information of the packet.\r
   //\r
-  Head = (EFI_IP6_HEADER *) NetbufGetByte (Packet, 0, NULL);\r
-  if (Head == NULL) {\r
-    goto Restart;\r
+  *Head = (EFI_IP6_HEADER *) NetbufGetByte (*Packet, 0, NULL);\r
+  if (*Head == NULL) {\r
+    return EFI_INVALID_PARAMETER;\r
   }\r
 \r
   //\r
   // Multicast addresses must not be used as source addresses in IPv6 packets.\r
   //\r
-  if ((Head->Version != 6) || (IP6_IS_MULTICAST (&Head->SourceAddress))) {\r
-    goto Restart;\r
+  if (((*Head)->Version != 6) || (IP6_IS_MULTICAST (&(*Head)->SourceAddress))) {\r
+    return EFI_INVALID_PARAMETER;\r
   }\r
 \r
   //\r
@@ -762,20 +792,20 @@ Ip6AcceptFrame (
   //\r
   ZeroMem (&Loopback, sizeof (EFI_IPv6_ADDRESS));\r
   Loopback.Addr[15] = 0x1;\r
-  if ((CompareMem (&Loopback, &Head->DestinationAddress, sizeof (EFI_IPv6_ADDRESS)) == 0) ||\r
-      (NetIp6IsUnspecifiedAddr (&Head->DestinationAddress))) {\r
-    goto Restart;\r
+  if ((CompareMem (&Loopback, &(*Head)->DestinationAddress, sizeof (EFI_IPv6_ADDRESS)) == 0) ||\r
+      (NetIp6IsUnspecifiedAddr (&(*Head)->DestinationAddress))) {\r
+    return EFI_INVALID_PARAMETER;\r
   }\r
 \r
   //\r
   // Convert the IP header to host byte order.\r
   //\r
-  Packet->Ip.Ip6 = Ip6NtohHead (Head);\r
+  (*Packet)->Ip.Ip6 = Ip6NtohHead (*Head);\r
 \r
   //\r
   // Get the per packet info.\r
   //\r
-  Info           = IP6_GET_CLIP_INFO (Packet);\r
+  Info           = IP6_GET_CLIP_INFO (*Packet);\r
   Info->LinkFlag = Flag;\r
   Info->CastType = 0;\r
 \r
@@ -783,10 +813,10 @@ Ip6AcceptFrame (
     Info->CastType = Ip6Promiscuous;\r
   }\r
 \r
-  if (Ip6IsOneOfSetAddress (IpSb, &Head->DestinationAddress, NULL, NULL)) {\r
+  if (Ip6IsOneOfSetAddress (IpSb, &(*Head)->DestinationAddress, NULL, NULL)) {\r
     Info->CastType = Ip6Unicast;\r
-  } else if (IP6_IS_MULTICAST (&Head->DestinationAddress)) {\r
-    if (Ip6FindMldEntry (IpSb, &Head->DestinationAddress) != NULL) {\r
+  } else if (IP6_IS_MULTICAST (&(*Head)->DestinationAddress)) {\r
+    if (Ip6FindMldEntry (IpSb, &(*Head)->DestinationAddress) != NULL) {\r
       Info->CastType = Ip6Multicast;\r
     }\r
   }\r
@@ -795,11 +825,11 @@ Ip6AcceptFrame (
   // Drop the packet that is not delivered to us.\r
   //\r
   if (Info->CastType == 0) {\r
-    goto Restart;\r
+    return EFI_INVALID_PARAMETER;\r
   }\r
 \r
 \r
-  PayloadLen = Head->PayloadLength;\r
+  PayloadLen = (*Head)->PayloadLength;\r
 \r
   Info->Start    = 0;\r
   Info->Length   = PayloadLen;\r
@@ -813,52 +843,51 @@ Ip6AcceptFrame (
   //\r
   // Mnp may deliver frame trailer sequence up, trim it off.\r
   //\r
-  if (TotalLen < Packet->TotalSize) {\r
-    NetbufTrim (Packet, Packet->TotalSize - TotalLen, FALSE);\r
+  if (TotalLen < (*Packet)->TotalSize) {\r
+    NetbufTrim (*Packet, (*Packet)->TotalSize - TotalLen, FALSE);\r
   }\r
 \r
-  if (TotalLen != Packet->TotalSize) {\r
-    goto Restart;\r
+  if (TotalLen != (*Packet)->TotalSize) {\r
+    return EFI_INVALID_PARAMETER;\r
   }\r
 \r
   //\r
   // Check the extension headers, if exist validate them\r
   //\r
   if (PayloadLen != 0) {\r
-    Payload = AllocatePool ((UINTN) PayloadLen);\r
-    if (Payload == NULL) {\r
-      goto Restart;\r
+    *Payload = AllocatePool ((UINTN) PayloadLen);\r
+    if (*Payload == NULL) {\r
+      return EFI_INVALID_PARAMETER;\r
     }\r
 \r
-    NetbufCopy (Packet, sizeof (EFI_IP6_HEADER), PayloadLen, Payload);\r
+    NetbufCopy (*Packet, sizeof (EFI_IP6_HEADER), PayloadLen, *Payload);\r
   }\r
 \r
-  LastHead   = NULL;\r
   if (!Ip6IsExtsValid (\r
          IpSb,\r
-         Packet,\r
-         &Head->NextHeader,\r
-         Payload,\r
+         *Packet,\r
+         &(*Head)->NextHeader,\r
+         *Payload,\r
          (UINT32) PayloadLen,\r
          TRUE,\r
          &FormerHeadOffset,\r
-         &LastHead,\r
-         &ExtHdrsLen,\r
-         &UnFragmentLen,\r
-         &Fragmented\r
+         LastHead,\r
+         ExtHdrsLen,\r
+         UnFragmentLen,\r
+         Fragmented\r
          )) {\r
-    goto Restart;\r
+    return EFI_INVALID_PARAMETER;\r
   }\r
 \r
-  HeadLen        = sizeof (EFI_IP6_HEADER) + UnFragmentLen;\r
+  HeadLen        = sizeof (EFI_IP6_HEADER) + *UnFragmentLen;\r
 \r
-  if (Fragmented) {\r
+  if (*Fragmented) {\r
     //\r
     // Get the fragment offset from the Fragment header\r
     //\r
-    FragmentHead = (IP6_FRAGMENT_HEADER *) NetbufGetByte (Packet, HeadLen, NULL);\r
+    FragmentHead = (IP6_FRAGMENT_HEADER *) NetbufGetByte (*Packet, HeadLen, NULL);\r
     if (FragmentHead == NULL) {\r
-      goto Restart;\r
+      return EFI_INVALID_PARAMETER;\r
     }\r
 \r
     FragmentOffset = NTOHS (FragmentHead->FragmentOffset);\r
@@ -888,49 +917,49 @@ Ip6AcceptFrame (
     // Fragments should in the unit of 8 octets long except the last one.\r
     //\r
     if ((Info->LastFrag == 0) && (Info->Length % 8 != 0)) {\r
-      goto Restart;\r
+      return EFI_INVALID_PARAMETER;\r
     }\r
 \r
     //\r
     // Reassemble the packet.\r
     //\r
-    Packet = Ip6Reassemble (&IpSb->Assemble, Packet);\r
-    if (Packet == NULL) {\r
-      goto Restart;\r
+    *Packet = Ip6Reassemble (&IpSb->Assemble, *Packet);\r
+    if (*Packet == NULL) {\r
+      return EFI_INVALID_PARAMETER;\r
     }\r
 \r
     //\r
     // Re-check the assembled packet to get the right values.\r
     //\r
-    Head       = Packet->Ip.Ip6;\r
-    PayloadLen = Head->PayloadLength;\r
+    *Head       = (*Packet)->Ip.Ip6;\r
+    PayloadLen  = (*Head)->PayloadLength;\r
     if (PayloadLen != 0) {\r
-      if (Payload != NULL) {\r
-        FreePool (Payload);\r
+      if (*Payload != NULL) {\r
+        FreePool (*Payload);\r
       }\r
 \r
-      Payload = AllocatePool ((UINTN) PayloadLen);\r
-      if (Payload == NULL) {\r
-        goto Restart;\r
+      *Payload = AllocatePool ((UINTN) PayloadLen);\r
+      if (*Payload == NULL) {\r
+        return EFI_INVALID_PARAMETER;\r
       }\r
 \r
-      NetbufCopy (Packet, sizeof (EFI_IP6_HEADER), PayloadLen, Payload);\r
+      NetbufCopy (*Packet, sizeof (EFI_IP6_HEADER), PayloadLen, *Payload);\r
     }\r
 \r
     if (!Ip6IsExtsValid (\r
            IpSb,\r
-           Packet,\r
-           &Head->NextHeader,\r
-           Payload,\r
+           *Packet,\r
+           &(*Head)->NextHeader,\r
+           *Payload,\r
            (UINT32) PayloadLen,\r
            TRUE,\r
            NULL,\r
-           &LastHead,\r
-           &ExtHdrsLen,\r
-           &UnFragmentLen,\r
-           &Fragmented\r
+           LastHead,\r
+           ExtHdrsLen,\r
+           UnFragmentLen,\r
+           Fragmented\r
            )) {\r
-      goto Restart;\r
+      return EFI_INVALID_PARAMETER;\r
     }\r
   }\r
 \r
@@ -938,31 +967,117 @@ Ip6AcceptFrame (
   // Trim the head off, after this point, the packet is headless.\r
   // and Packet->TotalLen == Info->Length.\r
   //\r
-  NetbufTrim (Packet, sizeof (EFI_IP6_HEADER) + ExtHdrsLen, TRUE);\r
+  NetbufTrim (*Packet, sizeof (EFI_IP6_HEADER) + *ExtHdrsLen, TRUE);\r
+  \r
+  return EFI_SUCCESS;\r
+}\r
+\r
+/**\r
+  The IP6 input routine. It is called by the IP6_INTERFACE when an\r
+  IP6 fragment is received from MNP.\r
+\r
+  @param[in]  Packet             The IP6 packet received.\r
+  @param[in]  IoStatus           The return status of receive request.\r
+  @param[in]  Flag               The link layer flag for the packet received, such\r
+                                 as multicast.\r
+  @param[in]  Context            The IP6 service instance that owns the MNP.\r
+\r
+**/\r
+VOID\r
+Ip6AcceptFrame (\r
+  IN NET_BUF                *Packet,\r
+  IN EFI_STATUS             IoStatus,\r
+  IN UINT32                 Flag,\r
+  IN VOID                   *Context\r
+  )\r
+{\r
+  IP6_SERVICE               *IpSb;\r
+  EFI_IP6_HEADER            *Head;\r
+  UINT8                     *Payload;\r
+  UINT8                     *LastHead;\r
+  UINT32                    UnFragmentLen;\r
+  UINT32                    ExtHdrsLen;\r
+  BOOLEAN                   Fragmented;\r
+  EFI_STATUS                Status;\r
+  EFI_IP6_HEADER            ZeroHead;\r
+\r
+  IpSb = (IP6_SERVICE *) Context;\r
+  NET_CHECK_SIGNATURE (IpSb, IP6_SERVICE_SIGNATURE);\r
 \r
+  Payload  = NULL;\r
+  LastHead = NULL;\r
+\r
+  //\r
+  // Check input parameters\r
+  //\r
+  if (EFI_ERROR (IoStatus) || (IpSb->State == IP6_SERVICE_DESTROY)) {\r
+    goto Drop;\r
+  }\r
+  \r
+  //\r
+  // Pre-Process the Ipv6 Packet and then reassemble if it is necessary.\r
+  //\r
+  Status = Ip6PreProcessPacket (\r
+             IpSb, \r
+             &Packet, \r
+             Flag, \r
+             &Payload, \r
+             &LastHead, \r
+             &ExtHdrsLen, \r
+             &UnFragmentLen, \r
+             &Fragmented,\r
+             &Head\r
+             );\r
+  if (EFI_ERROR (Status)) {\r
+    goto Restart;\r
+  }\r
   //\r
   // After trim off, the packet is a esp/ah/udp/tcp/icmp6 net buffer,\r
   // and no need consider any other ahead ext headers.\r
   //\r
   Status = Ip6IpSecProcessPacket (\r
              IpSb,\r
-             Head,\r
+             &Head,\r
              LastHead, // need get the lasthead value for input\r
              &Packet,\r
-             NULL,\r
-             0,\r
+             &Payload,\r
+             &ExtHdrsLen,\r
              EfiIPsecInBound,\r
              NULL\r
              );\r
 \r
-  if (EFI_ERROR(Status)) {\r
+  if (EFI_ERROR (Status)) {\r
     goto Restart;\r
   }\r
 \r
   //\r
-  // TODO: may check the last head again, the same as the output routine\r
-  //\r
+  // If the packet is protected by IPsec Tunnel Mode, Check the Inner Ip Packet.\r
+  //\r
+  ZeroMem (&ZeroHead, sizeof (EFI_IP6_HEADER));\r
+  if (0 == CompareMem (Head, &ZeroHead, sizeof (EFI_IP6_HEADER))) {\r
+    Status = Ip6PreProcessPacket (\r
+               IpSb, \r
+               &Packet, \r
+               Flag, \r
+               &Payload, \r
+               &LastHead, \r
+               &ExtHdrsLen, \r
+               &UnFragmentLen, \r
+               &Fragmented, \r
+               &Head\r
+               );\r
+    if (EFI_ERROR (Status)) {\r
+      goto Restart;\r
+    }\r
+  }\r
 \r
+  //\r
+  // Check the Packet again.\r
+  //\r
+  if (Packet == NULL) {\r
+    goto Restart;\r
+  }\r
+  \r
   //\r
   // Packet may have been changed. The ownership of the packet\r
   // is transfered to the packet process logic.\r