[mirror_edk2.git] / MdeModulePkg / Library / DxeNetLib / NetBuffer.c

/** @file\r
  Network library.\r
  \r
Copyright (c) 2005 - 2006, Intel Corporation.<BR>\r
All rights reserved. This program and the accompanying materials\r
are licensed and made available under the terms and conditions of the BSD License\r
which accompanies this distribution.  The full text of the license may be found at\r
http://opensource.org/licenses/bsd-license.php\r
\r
THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,\r
WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.\r
**/\r
\r
#include <Uefi.h>\r
\r
#include <Library/NetLib.h>\r
#include <Library/BaseLib.h>\r
#include <Library/DebugLib.h>\r
#include <Library/BaseMemoryLib.h>\r
#include <Library/UefiBootServicesTableLib.h>\r
#include <Library/MemoryAllocationLib.h>\r
\r
\r
/**\r
  Allocate and build up the sketch for a NET_BUF. The net buffer allocated\r
  has the BlockOpNum's NET_BLOCK_OP, and its associated NET_VECTOR has the\r
  BlockNum's NET_BLOCK.\r
\r
  @param[in]  BlockNum              The number of NET_BLOCK in the Vector of net buffer\r
  @param[in]  BlockOpNum            The number of NET_BLOCK_OP in the net buffer\r
\r
  @return                       Pointer to the allocated NET_BUF. If NULL, the\r
                                allocation failed due to resource limit.\r
\r
**/\r
NET_BUF *\r
NetbufAllocStruct (\r
  IN UINT32                 BlockNum,\r
  IN UINT32                 BlockOpNum\r
  )\r
{\r
  NET_BUF                   *Nbuf;\r
  NET_VECTOR                *Vector;\r
\r
  ASSERT (BlockOpNum >= 1);\r
\r
  //\r
  // Allocate three memory blocks.\r
  //\r
  Nbuf = AllocateZeroPool (NET_BUF_SIZE (BlockOpNum));\r
\r
  if (Nbuf == NULL) {\r
    return NULL;\r
  }\r
\r
  Nbuf->Signature           = NET_BUF_SIGNATURE;\r
  Nbuf->RefCnt              = 1;\r
  Nbuf->BlockOpNum          = BlockOpNum;\r
  InitializeListHead (&Nbuf->List);\r
\r
  if (BlockNum != 0) {\r
    Vector = AllocateZeroPool (NET_VECTOR_SIZE (BlockNum));\r
\r
    if (Vector == NULL) {\r
      goto FreeNbuf;\r
    }\r
\r
    Vector->Signature = NET_VECTOR_SIGNATURE;\r
    Vector->RefCnt    = 1;\r
    Vector->BlockNum  = BlockNum;\r
    Nbuf->Vector      = Vector;\r
  }\r
\r
  return Nbuf;\r
\r
FreeNbuf:\r
\r
  gBS->FreePool (Nbuf);\r
  return NULL;\r
}\r
\r
\r
/**\r
  Allocate a single block NET_BUF. Upon allocation, all the\r
  free space is in the tail room.\r
\r
  @param[in]  Len              The length of the block.\r
\r
  @return                       Pointer to the allocated NET_BUF. If NULL  the\r
                                allocation failed due to resource limit.\r
\r
**/\r
NET_BUF  *\r
EFIAPI\r
NetbufAlloc (\r
  IN UINT32                 Len\r
  )\r
{\r
  NET_BUF                   *Nbuf;\r
  NET_VECTOR                *Vector;\r
  UINT8                     *Bulk;\r
\r
  ASSERT (Len > 0);\r
\r
  Nbuf = NetbufAllocStruct (1, 1);\r
\r
  if (Nbuf == NULL) {\r
    return NULL;\r
  }\r
\r
  Bulk = AllocatePool (Len);\r
\r
  if (Bulk == NULL) {\r
    goto FreeNBuf;\r
  }\r
\r
  Vector = Nbuf->Vector;\r
  Vector->Len                 = Len;\r
\r
  Vector->Block[0].Bulk       = Bulk;\r
  Vector->Block[0].Len        = Len;\r
\r
  Nbuf->BlockOp[0].BlockHead  = Bulk;\r
  Nbuf->BlockOp[0].BlockTail  = Bulk + Len;\r
\r
  Nbuf->BlockOp[0].Head       = Bulk;\r
  Nbuf->BlockOp[0].Tail       = Bulk;\r
  Nbuf->BlockOp[0].Size       = 0;\r
\r
  return Nbuf;\r
\r
FreeNBuf:\r
  gBS->FreePool (Nbuf);\r
  return NULL;\r
}\r
\r
\r
/**\r
  Free the vector.\r
\r
  @param[in]  Vector                Pointer to the NET_VECTOR to be freed.\r
\r
**/\r
VOID\r
NetbufFreeVector (\r
  IN NET_VECTOR             *Vector\r
  )\r
{\r
  UINT32                    Index;\r
\r
  NET_CHECK_SIGNATURE (Vector, NET_VECTOR_SIGNATURE);\r
  ASSERT (Vector->RefCnt > 0);\r
\r
  Vector->RefCnt--;\r
\r
  if (Vector->RefCnt > 0) {\r
    return;\r
  }\r
\r
  if (Vector->Free != NULL) {\r
    //\r
    // Call external free function to free the vector if it\r
    // isn't NULL. If NET_VECTOR_OWN_FIRST is set, release the\r
    // first block since it is allocated by us\r
    //\r
    if (Vector->Flag & NET_VECTOR_OWN_FIRST) {\r
      gBS->FreePool (Vector->Block[0].Bulk);\r
    }\r
\r
    Vector->Free (Vector->Arg);\r
\r
  } else {\r
    //\r
    // Free each memory block associated with the Vector\r
    //\r
    for (Index = 0; Index < Vector->BlockNum; Index++) {\r
      gBS->FreePool (Vector->Block[Index].Bulk);\r
    }\r
  }\r
\r
  gBS->FreePool (Vector);\r
}\r
\r
\r
/**\r
  Free the buffer and its associated NET_VECTOR.\r
\r
  @param[in]  Nbuf                  Pointer to the NET_BUF to be freed.\r
\r
**/\r
VOID\r
EFIAPI\r
NetbufFree (\r
  IN NET_BUF                *Nbuf\r
  )\r
{\r
  NET_CHECK_SIGNATURE (Nbuf, NET_BUF_SIGNATURE);\r
  ASSERT (Nbuf->RefCnt > 0);\r
\r
  Nbuf->RefCnt--;\r
\r
  if (Nbuf->RefCnt == 0) {\r
    //\r
    // Update Vector only when NBuf is to be released. That is,\r
    // all the sharing of Nbuf increse Vector's RefCnt by one\r
    //\r
    NetbufFreeVector (Nbuf->Vector);\r
    gBS->FreePool (Nbuf);\r
  }\r
}\r
\r
\r
/**\r
  Create a copy of NET_BUF that share the associated NET_DATA.\r
\r
  @param[in]  Nbuf              Pointer to the net buffer to be cloned.\r
\r
  @return                       Pointer to the cloned net buffer.If NULL, the\r
                                allocation failed due to resource limit.\r
\r
**/\r
NET_BUF  *\r
EFIAPI\r
NetbufClone (\r
  IN NET_BUF                *Nbuf\r
  )\r
{\r
  NET_BUF                   *Clone;\r
\r
  NET_CHECK_SIGNATURE (Nbuf, NET_BUF_SIGNATURE);\r
\r
  Clone = AllocatePool (NET_BUF_SIZE (Nbuf->BlockOpNum));\r
\r
  if (Clone == NULL) {\r
    return NULL;\r
  }\r
\r
  Clone->Signature  = NET_BUF_SIGNATURE;\r
  Clone->RefCnt     = 1;\r
  InitializeListHead (&Clone->List);\r
\r
  Clone->Ip   = Nbuf->Ip;\r
  Clone->Tcp  = Nbuf->Tcp;\r
\r
  CopyMem (Clone->ProtoData, Nbuf->ProtoData, NET_PROTO_DATA);\r
\r
  NET_GET_REF (Nbuf->Vector);\r
\r
  Clone->Vector     = Nbuf->Vector;\r
  Clone->BlockOpNum = Nbuf->BlockOpNum;\r
  Clone->TotalSize  = Nbuf->TotalSize;\r
  CopyMem (Clone->BlockOp, Nbuf->BlockOp, sizeof (NET_BLOCK_OP) * Nbuf->BlockOpNum);\r
\r
  return Clone;\r
}\r
\r
\r
/**\r
  Create a duplicated copy of Nbuf, data is copied. Also leave some\r
  head space before the data.\r
\r
  @param[in]       Nbuf                  Pointer to the net buffer to be cloned.\r
  @param[in, out]  Duplicate             Pointer to the net buffer to duplicate to, if NULL\r
                                         a new net  buffer is allocated.\r
  @param[in]      HeadSpace              Length of the head space to reserve.\r
\r
  @return                       Pointer to the duplicated net buffer.If NULL, the\r
                                allocation failed due to resource limit.\r
\r
**/\r
NET_BUF  *\r
EFIAPI\r
NetbufDuplicate (\r
  IN NET_BUF                *Nbuf,\r
  IN OUT NET_BUF            *Duplicate        OPTIONAL,\r
  IN UINT32                 HeadSpace\r
  )\r
{\r
  UINT8                     *Dst;\r
\r
  NET_CHECK_SIGNATURE (Nbuf, NET_BUF_SIGNATURE);\r
\r
  if (Duplicate == NULL) {\r
    Duplicate = NetbufAlloc (Nbuf->TotalSize + HeadSpace);\r
  }\r
\r
  if (Duplicate == NULL) {\r
    return NULL;\r
  }\r
\r
  //\r
  // Don't set the IP and TCP head point, since it is most\r
  // like that they are pointing to the memory of Nbuf.\r
  //\r
  CopyMem (Duplicate->ProtoData, Nbuf->ProtoData, NET_PROTO_DATA);\r
  NetbufReserve (Duplicate, HeadSpace);\r
\r
  Dst = NetbufAllocSpace (Duplicate, Nbuf->TotalSize, NET_BUF_TAIL);\r
  NetbufCopy (Nbuf, 0, Nbuf->TotalSize, Dst);\r
\r
  return Duplicate;\r
}\r
\r
\r
/**\r
  Free a list of net buffers.\r
\r
  @param[in, out]  Head              Pointer to the head of linked net buffers.\r
\r
**/\r
VOID\r
EFIAPI\r
NetbufFreeList (\r
  IN OUT LIST_ENTRY         *Head\r
  )\r
{\r
  LIST_ENTRY                *Entry;\r
  LIST_ENTRY                *Next;\r
  NET_BUF                   *Nbuf;\r
\r
  Entry = Head->ForwardLink;\r
\r
  NET_LIST_FOR_EACH_SAFE (Entry, Next, Head) {\r
    Nbuf = NET_LIST_USER_STRUCT (Entry, NET_BUF, List);\r
    NET_CHECK_SIGNATURE (Nbuf, NET_BUF_SIGNATURE);\r
\r
    RemoveEntryList (Entry);\r
    NetbufFree (Nbuf);\r
  }\r
\r
  ASSERT (IsListEmpty (Head));\r
}\r
\r
\r
/**\r
  Get the position of some byte in the net buffer. This can be used\r
  to, for example, retrieve the IP header in the packet. It also\r
  returns the fragment that contains the byte which is used mainly by\r
  the buffer implementation itself.\r
\r
  @param[in]   Nbuf                  Pointer to the net buffer.\r
  @param[in]   Offset                The index or offset of the byte.\r
  @param[out]  Index                 Index of the fragment that contains the block.\r
\r
  @return *                     Pointer to the nth byte of data in the net buffer.\r
                                If NULL, there is no such data in the net buffer.\r
\r
**/\r
UINT8  *\r
EFIAPI\r
NetbufGetByte (\r
  IN  NET_BUF               *Nbuf,\r
  IN  UINT32                Offset,\r
  OUT UINT32                *Index  OPTIONAL\r
  )\r
{\r
  NET_BLOCK_OP              *BlockOp;\r
  UINT32                    Loop;\r
  UINT32                    Len;\r
\r
  NET_CHECK_SIGNATURE (Nbuf, NET_BUF_SIGNATURE);\r
\r
  if (Offset >= Nbuf->TotalSize) {\r
    return NULL;\r
  }\r
\r
  BlockOp = Nbuf->BlockOp;\r
  Len     = 0;\r
\r
  for (Loop = 0; Loop < Nbuf->BlockOpNum; Loop++) {\r
\r
    if (Len + BlockOp[Loop].Size <= Offset) {\r
      Len += BlockOp[Loop].Size;\r
      continue;\r
    }\r
\r
    if (Index != NULL) {\r
      *Index = Loop;\r
    }\r
\r
    return BlockOp[Loop].Head + (Offset - Len);\r
  }\r
\r
  return NULL;\r
}\r
\r
\r
\r
/**\r
  Set the NET_BLOCK and corresponding NET_BLOCK_OP in\r
  the buffer. All the pointers in NET_BLOCK and NET_BLOCK_OP\r
  are set to the bulk's head and tail respectively. So, this\r
  function alone can't be used by NetbufAlloc.\r
\r
  @param[in, out]  Nbuf                  Pointer to the net buffer.\r
  @param[in]       Bulk                  Pointer to the data.\r
  @param[in]       Len                   Length of the bulk data.\r
  @param[in]       Index                 The data block index in the net buffer the bulk\r
                                         data should belong to.\r
                                \r
**/\r
VOID\r
NetbufSetBlock (\r
  IN OUT NET_BUF            *Nbuf,\r
  IN UINT8                  *Bulk,\r
  IN UINT32                 Len,\r
  IN UINT32                 Index\r
  )\r
{\r
  NET_BLOCK_OP              *BlockOp;\r
  NET_BLOCK                 *Block;\r
\r
  NET_CHECK_SIGNATURE (Nbuf, NET_BUF_SIGNATURE);\r
  NET_CHECK_SIGNATURE (Nbuf->Vector, NET_VECTOR_SIGNATURE);\r
  ASSERT (Index < Nbuf->BlockOpNum);\r
\r
  Block               = &(Nbuf->Vector->Block[Index]);\r
  BlockOp             = &(Nbuf->BlockOp[Index]);\r
  Block->Len          = Len;\r
  Block->Bulk         = Bulk;\r
  BlockOp->BlockHead  = Bulk;\r
  BlockOp->BlockTail  = Bulk + Len;\r
  BlockOp->Head       = Bulk;\r
  BlockOp->Tail       = Bulk + Len;\r
  BlockOp->Size       = Len;\r
}\r
\r
\r
\r
/**\r
  Set the NET_BLOCK_OP in the buffer. The corresponding NET_BLOCK\r
  structure is left untouched. Some times, there is no 1:1 relationship\r
  between NET_BLOCK and NET_BLOCK_OP. For example, that in NetbufGetFragment.\r
\r
  @param[in, out]  Nbuf                  Pointer to the net buffer.\r
  @param[in]       Bulk                  Pointer to the data.\r
  @param[in]       Len                   Length of the bulk data.\r
  @param[in]       Index                 The data block index in the net buffer the bulk\r
                                         data should belong to.\r
\r
**/\r
VOID\r
NetbufSetBlockOp (\r
  IN OUT NET_BUF            *Nbuf,\r
  IN UINT8                  *Bulk,\r
  IN UINT32                 Len,\r
  IN UINT32                 Index\r
  )\r
{\r
  NET_BLOCK_OP              *BlockOp;\r
\r
  NET_CHECK_SIGNATURE (Nbuf, NET_BUF_SIGNATURE);\r
  ASSERT (Index < Nbuf->BlockOpNum);\r
\r
  BlockOp             = &(Nbuf->BlockOp[Index]);\r
  BlockOp->BlockHead  = Bulk;\r
  BlockOp->BlockTail  = Bulk + Len;\r
  BlockOp->Head       = Bulk;\r
  BlockOp->Tail       = Bulk + Len;\r
  BlockOp->Size       = Len;\r
}\r
\r
\r
/**\r
  Helper function for NetbufClone. It is necessary because NetbufGetFragment\r
  may allocate the first block to accomodate the HeadSpace and HeadLen. So, it\r
  need to create a new NET_VECTOR. But, we want to avoid data copy by sharing\r
  the old NET_VECTOR.\r
\r
  @param[in]  Arg                   Point to the old NET_VECTOR.\r
\r
**/\r
VOID\r
NetbufGetFragmentFree (\r
  IN VOID                   *Arg\r
  )\r
{\r
  NET_VECTOR                *Vector;\r
\r
  Vector = (NET_VECTOR *)Arg;\r
  NetbufFreeVector (Vector);\r
}\r
\r
\r
\r
/**\r
  Create a NET_BUF structure which contains Len byte data of\r
  Nbuf starting from Offset. A new NET_BUF structure will be\r
  created but the associated data in NET_VECTOR is shared.\r
  This function exists to do IP packet fragmentation.\r
\r
  @param[in]  Nbuf                  Pointer to the net buffer to be cloned.\r
  @param[in]  Offset                Starting point of the data to be included in new\r
                                    buffer.\r
  @param[in]  Len                   How many data to include in new data.\r
  @param[in]  HeadSpace             How many bytes of head space to reserve for\r
                                    protocol header.\r
\r
  @return                       Pointer to the cloned net buffer.If NULL, the\r
                                allocation failed due to resource limit.\r
\r
**/\r
NET_BUF  *\r
EFIAPI\r
NetbufGetFragment (\r
  IN NET_BUF                *Nbuf,\r
  IN UINT32                 Offset,\r
  IN UINT32                 Len,\r
  IN UINT32                 HeadSpace\r
  )\r
{\r
  NET_BUF                   *Child;\r
  NET_VECTOR                *Vector;\r
  NET_BLOCK_OP              *BlockOp;\r
  UINT32                    CurBlockOp;\r
  UINT32                    BlockOpNum;\r
  UINT8                     *FirstBulk;\r
  UINT32                    Index;\r
  UINT32                    First;\r
  UINT32                    Last;\r
  UINT32                    FirstSkip;\r
  UINT32                    FirstLen;\r
  UINT32                    LastLen;\r
  UINT32                    Cur;\r
\r
  NET_CHECK_SIGNATURE (Nbuf, NET_BUF_SIGNATURE);\r
\r
  if ((Len == 0) || (Offset + Len > Nbuf->TotalSize)) {\r
    return NULL;\r
  }\r
\r
  //\r
  // First find the first and last BlockOp that contains\r
  // the valid data, and compute the offset of the first\r
  // BlockOp and length of the last BlockOp\r
  //\r
  BlockOp = Nbuf->BlockOp;\r
  Cur     = 0;\r
\r
  for (Index = 0; Index < Nbuf->BlockOpNum; Index++) {\r
    if (Offset < Cur + BlockOp[Index].Size) {\r
      break;\r
    }\r
\r
    Cur += BlockOp[Index].Size;\r
  }\r
\r
  //\r
  // First is the index of the first BlockOp, FirstSkip is\r
  // the offset of the first byte in the first BlockOp.\r
  //\r
  First     = Index;\r
  FirstSkip = Offset - Cur;\r
  FirstLen  = BlockOp[Index].Size - FirstSkip;\r
\r
  //\r
  //redundant assignment to make compiler happy.\r
  //\r
  Last      = 0;\r
  LastLen   = 0;\r
\r
  if (Len > FirstLen) {\r
    Cur += BlockOp[Index].Size;\r
    Index++;\r
\r
    for (; Index < Nbuf->BlockOpNum; Index++) {\r
      if (Offset + Len <= Cur + BlockOp[Index].Size) {\r
        Last    = Index;\r
        LastLen = Offset + Len - Cur;\r
        break;\r
      }\r
\r
      Cur += BlockOp[Index].Size;\r
    }\r
\r
  } else {\r
    Last     = First;\r
    LastLen  = Len;\r
    FirstLen = Len;\r
  }\r
\r
  BlockOpNum = Last - First + 1;\r
  CurBlockOp = 0;\r
\r
  if (HeadSpace != 0) {\r
    //\r
    // Allocate an extra block to accomdate the head space.\r
    //\r
    BlockOpNum++;\r
\r
    Child = NetbufAllocStruct (1, BlockOpNum);\r
\r
    if (Child == NULL) {\r
      return NULL;\r
    }\r
\r
    FirstBulk = AllocatePool (HeadSpace);\r
\r
    if (FirstBulk == NULL) {\r
      goto FreeChild;\r
    }\r
\r
    Vector        = Child->Vector;\r
    Vector->Free  = NetbufGetFragmentFree;\r
    Vector->Arg   = Nbuf->Vector;\r
    Vector->Flag  = NET_VECTOR_OWN_FIRST;\r
    Vector->Len   = HeadSpace;\r
\r
    //\r
    //Reserve the head space in the first block\r
    //\r
    NetbufSetBlock (Child, FirstBulk, HeadSpace, 0);\r
    Child->BlockOp[0].Head += HeadSpace;\r
    Child->BlockOp[0].Size =  0;\r
    CurBlockOp++;\r
\r
  }else {\r
    Child = NetbufAllocStruct (0, BlockOpNum);\r
\r
    if (Child == NULL) {\r
      return NULL;\r
    }\r
\r
    Child->Vector = Nbuf->Vector;\r
  }\r
\r
  NET_GET_REF (Nbuf->Vector);\r
  Child->TotalSize = Len;\r
\r
  //\r
  // Set all the BlockOp up, the first and last one are special\r
  // and need special process.\r
  //\r
  NetbufSetBlockOp (\r
    Child,\r
    Nbuf->BlockOp[First].Head + FirstSkip,\r
    FirstLen,\r
    CurBlockOp++\r
    );\r
\r
  for (Index = First + 1; Index <= Last - 1 ; Index++) {\r
    NetbufSetBlockOp (\r
      Child,\r
      BlockOp[Index].Head,\r
      BlockOp[Index].Size,\r
      CurBlockOp++\r
      );\r
  }\r
\r
  if (First != Last) {\r
    NetbufSetBlockOp (\r
      Child,\r
      BlockOp[Last].Head,\r
      LastLen,\r
      CurBlockOp\r
      );\r
  }\r
\r
  CopyMem (Child->ProtoData, Nbuf->ProtoData, NET_PROTO_DATA);\r
  return Child;\r
\r
FreeChild:\r
\r
  gBS->FreePool (Child);\r
  return NULL;\r
}\r
\r
\r
\r
/**\r
  Build a NET_BUF from external blocks.\r
\r
  @param[in]  ExtFragment           Pointer to the data block.\r
  @param[in]  ExtNum                The number of the data block.\r
  @param[in]  HeadSpace             The head space to be reserved.\r
  @param[in]  HeadLen               The length of the protocol header, This function\r
                                    will pull that number of data into a linear block.\r
  @param[in]  ExtFree               Pointer to the caller provided free function.\r
  @param[in]  Arg                   The argument passed to ExtFree when ExtFree is\r
                                    called.\r
\r
  @return                    Pointer to the net buffer built from the data blocks.\r
                             If NULL, the allocation failed due to resource limit.\r
\r
**/\r
NET_BUF  *\r
EFIAPI\r
NetbufFromExt (\r
  IN NET_FRAGMENT           *ExtFragment,\r
  IN UINT32                 ExtNum,\r
  IN UINT32                 HeadSpace,\r
  IN UINT32                 HeadLen,\r
  IN NET_VECTOR_EXT_FREE    ExtFree,\r
  IN VOID                   *Arg          OPTIONAL\r
  )\r
{\r
  NET_BUF                   *Nbuf;\r
  NET_VECTOR                *Vector;\r
  NET_FRAGMENT              SavedFragment;\r
  UINT32                    SavedIndex;\r
  UINT32                    TotalLen;\r
  UINT32                    BlockNum;\r
  UINT8                     *FirstBlock;\r
  UINT32                    FirstBlockLen;\r
  UINT8                     *Header;\r
  UINT32                    CurBlock;\r
  UINT32                    Index;\r
  UINT32                    Len;\r
  UINT32                    Copied;\r
\r
  ASSERT ((ExtFragment != NULL) && (ExtNum > 0) && (ExtFree != NULL));\r
\r
  SavedFragment.Bulk = NULL;\r
  SavedFragment.Len  = 0;\r
\r
  FirstBlockLen  = 0;\r
  FirstBlock     = NULL;\r
  BlockNum       = ExtNum;\r
  Index          = 0;\r
  TotalLen       = 0;\r
  SavedIndex     = 0;\r
  Len            = 0;\r
  Copied         = 0;\r
\r
  //\r
  // No need to consolidate the header if the first block is\r
  // longer than the header length or there is only one block.\r
  //\r
  if ((ExtFragment[0].Len >= HeadLen) || (ExtNum == 1)) {\r
    HeadLen = 0;\r
  }\r
\r
  //\r
  // Allocate an extra block if we need to:\r
  //  1. Allocate some header space\r
  //  2. aggreate the packet header\r
  //\r
  if ((HeadSpace != 0) || (HeadLen != 0)) {\r
    FirstBlockLen = HeadLen + HeadSpace;\r
    FirstBlock    = AllocatePool (FirstBlockLen);\r
\r
    if (FirstBlock == NULL) {\r
      return NULL;\r
    }\r
\r
    BlockNum++;\r
  }\r
\r
  //\r
  // Copy the header to the first block, reduce the NET_BLOCK\r
  // to allocate by one for each block that is completely covered\r
  // by the first bulk.\r
  //\r
  if (HeadLen != 0) {\r
    Len    = HeadLen;\r
    Header = FirstBlock + HeadSpace;\r
\r
    for (Index = 0; Index < ExtNum; Index++) {\r
      if (Len >= ExtFragment[Index].Len) {\r
        CopyMem (Header, ExtFragment[Index].Bulk, ExtFragment[Index].Len);\r
\r
        Copied    += ExtFragment[Index].Len;\r
        Len       -= ExtFragment[Index].Len;\r
        Header    += ExtFragment[Index].Len;\r
        TotalLen  += ExtFragment[Index].Len;\r
        BlockNum--;\r
\r
        if (Len == 0) {\r
          //\r
          // Increament the index number to point to the next\r
          // non-empty fragment.\r
          //\r
          Index++;\r
          break;\r
        }\r
\r
      } else {\r
        CopyMem (Header, ExtFragment[Index].Bulk, Len);\r
\r
        Copied    += Len;\r
        TotalLen  += Len;\r
\r
        //\r
        // Adjust the block structure to exclude the data copied,\r
        // So, the left-over block can be processed as other blocks.\r
        // But it must be recovered later. (SavedIndex > 0) always\r
        // holds since we don't aggreate the header if the first block\r
        // is bigger enough that the header is continuous\r
        //\r
        SavedIndex    = Index;\r
        SavedFragment = ExtFragment[Index];\r
        ExtFragment[Index].Bulk += Len;\r
        ExtFragment[Index].Len  -= Len;\r
        break;\r
      }\r
    }\r
  }\r
\r
  Nbuf = NetbufAllocStruct (BlockNum, BlockNum);\r
\r
  if (Nbuf == NULL) {\r
    goto FreeFirstBlock;\r
  }\r
\r
  Vector       = Nbuf->Vector;\r
  Vector->Free = ExtFree;\r
  Vector->Arg  = Arg;\r
  Vector->Flag = ((FirstBlockLen != 0) ? NET_VECTOR_OWN_FIRST : 0);\r
\r
  //\r
  // Set the first block up which may contain\r
  // some head space and aggregated header\r
  //\r
  CurBlock = 0;\r
\r
  if (FirstBlockLen != 0) {\r
    NetbufSetBlock (Nbuf, FirstBlock, HeadSpace + Copied, 0);\r
    Nbuf->BlockOp[0].Head += HeadSpace;\r
    Nbuf->BlockOp[0].Size =  Copied;\r
\r
    CurBlock++;\r
  }\r
\r
  for (; Index < ExtNum; Index++) {\r
    NetbufSetBlock (Nbuf, ExtFragment[Index].Bulk, ExtFragment[Index].Len, CurBlock);\r
    TotalLen += ExtFragment[Index].Len;\r
    CurBlock++;\r
  }\r
\r
  Vector->Len     = TotalLen + HeadSpace;\r
  Nbuf->TotalSize = TotalLen;\r
\r
  if (SavedIndex != 0) {\r
    ExtFragment[SavedIndex] = SavedFragment;\r
  }\r
\r
  return Nbuf;\r
\r
FreeFirstBlock:\r
  gBS->FreePool (FirstBlock);\r
  return NULL;\r
}\r
\r
\r
/**\r
  Build a fragment table to contain the fragments in the\r
  buffer. This is the opposite of the NetbufFromExt.\r
\r
  @param[in]       Nbuf                  Point to the net buffer.\r
  @param[in, out]  ExtFragment           Pointer to the data block.\r
  @param[in, out]  ExtNum                The number of the data block.\r
\r
  @retval EFI_BUFFER_TOO_SMALL  The number of non-empty block is bigger than ExtNum.\r
  @retval EFI_SUCCESS           Fragment table built.\r
\r
**/\r
EFI_STATUS\r
EFIAPI\r
NetbufBuildExt (\r
  IN NET_BUF                *Nbuf,\r
  IN OUT NET_FRAGMENT       *ExtFragment,\r
  IN OUT UINT32             *ExtNum\r
  )\r
{\r
  UINT32                    Index;\r
  UINT32                    Current;\r
\r
  Current = 0;\r
\r
  for (Index = 0; (Index < Nbuf->BlockOpNum); Index++) {\r
    if (Nbuf->BlockOp[Index].Size == 0) {\r
      continue;\r
    }\r
\r
    if (Current < *ExtNum) {\r
      ExtFragment[Current].Bulk = Nbuf->BlockOp[Index].Head;\r
      ExtFragment[Current].Len  = Nbuf->BlockOp[Index].Size;\r
      Current++;\r
    } else {\r
      return EFI_BUFFER_TOO_SMALL;\r
    }\r
  }\r
\r
  *ExtNum = Current;\r
  return EFI_SUCCESS;\r
}\r
\r
\r
/**\r
  Build a NET_BUF from a list of NET_BUF.\r
\r
  @param[in]   BufList               A List of NET_BUF.\r
  @param[in]   HeadSpace             The head space to be reserved.\r
  @param[in]   HeaderLen             The length of the protocol header, This function\r
                                     will pull that number of data into a linear block.\r
  @param[in]   ExtFree               Pointer to the caller provided free function.\r
  @param[in]   Arg                   The argument passed to ExtFree when ExtFree is\r
                                     called.\r
\r
  @return                       Pointer to the net buffer built from the data\r
                                blocks.\r
\r
**/\r
NET_BUF  *\r
EFIAPI\r
NetbufFromBufList (\r
  IN LIST_ENTRY             *BufList,\r
  IN UINT32                 HeadSpace,\r
  IN UINT32                 HeaderLen,\r
  IN NET_VECTOR_EXT_FREE    ExtFree,\r
  IN VOID                   *Arg              OPTIONAL\r
  )\r
{\r
  NET_FRAGMENT              *Fragment;\r
  UINT32                    FragmentNum;\r
  LIST_ENTRY                *Entry;\r
  NET_BUF                   *Nbuf;\r
  UINT32                    Index;\r
  UINT32                    Current;\r
\r
  //\r
  //Compute how many blocks are there\r
  //\r
  FragmentNum = 0;\r
\r
  NET_LIST_FOR_EACH (Entry, BufList) {\r
    Nbuf = NET_LIST_USER_STRUCT (Entry, NET_BUF, List);\r
    NET_CHECK_SIGNATURE (Nbuf, NET_BUF_SIGNATURE);\r
    FragmentNum += Nbuf->BlockOpNum;\r
  }\r
\r
  //\r
  //Allocate and copy block points\r
  //\r
  Fragment = AllocatePool (sizeof (NET_FRAGMENT) * FragmentNum);\r
\r
  if (Fragment == NULL) {\r
    return NULL;\r
  }\r
\r
  Current = 0;\r
\r
  NET_LIST_FOR_EACH (Entry, BufList) {\r
    Nbuf = NET_LIST_USER_STRUCT (Entry, NET_BUF, List);\r
    NET_CHECK_SIGNATURE (Nbuf, NET_BUF_SIGNATURE);\r
\r
    for (Index = 0; Index < Nbuf->BlockOpNum; Index++) {\r
      if (Nbuf->BlockOp[Index].Size) {\r
        Fragment[Current].Bulk = Nbuf->BlockOp[Index].Head;\r
        Fragment[Current].Len  = Nbuf->BlockOp[Index].Size;\r
        Current++;\r
      }\r
    }\r
  }\r
\r
  Nbuf = NetbufFromExt (Fragment, Current, HeadSpace, HeaderLen, ExtFree, Arg);\r
  gBS->FreePool (Fragment);\r
\r
  return Nbuf;\r
}\r
\r
\r
/**\r
  Reserve some space in the header room of the buffer.\r
  Upon allocation, all the space are in the tail room\r
  of the buffer. Call this function to move some space\r
  to the header room. This function is quite limited in\r
  that it can only reserver space from the first block\r
  of an empty NET_BUF not built from the external. But\r
  it should be enough for the network stack.\r
\r
  @param[in, out]  Nbuf                  Pointer to the net buffer.\r
  @param[in]       Len                   The length of buffer to be reserverd.\r
\r
**/\r
VOID\r
EFIAPI\r
NetbufReserve (\r
  IN OUT NET_BUF            *Nbuf,\r
  IN UINT32                 Len\r
  )\r
{\r
  NET_CHECK_SIGNATURE (Nbuf, NET_BUF_SIGNATURE);\r
  NET_CHECK_SIGNATURE (Nbuf->Vector, NET_VECTOR_SIGNATURE);\r
\r
  ASSERT ((Nbuf->BlockOpNum == 1) && (Nbuf->TotalSize == 0));\r
  ASSERT ((Nbuf->Vector->Free == NULL) && (Nbuf->Vector->Len >= Len));\r
\r
  Nbuf->BlockOp[0].Head += Len;\r
  Nbuf->BlockOp[0].Tail += Len;\r
\r
  ASSERT (Nbuf->BlockOp[0].Tail <= Nbuf->BlockOp[0].BlockTail);\r
}\r
\r
\r
/**\r
  Allocate some space from the header or tail of the buffer.\r
\r
  @param[in, out]  Nbuf                  Pointer to the net buffer.\r
  @param[in]       Len                   The length of the buffer to be allocated.\r
  @param [in]      FromHead              The flag to indicate whether reserve the data from\r
                                         head or tail. TRUE for from head, and FALSE for\r
                                         from tail.\r
\r
  @return                       Pointer to the first byte of the allocated buffer.\r
\r
**/\r
UINT8  *\r
EFIAPI\r
NetbufAllocSpace (\r
  IN OUT NET_BUF            *Nbuf,\r
  IN UINT32                 Len,\r
  IN BOOLEAN                FromHead\r
  )\r
{\r
  NET_BLOCK_OP              *BlockOp;\r
  UINT32                    Index;\r
  UINT8                     *SavedTail;\r
\r
  NET_CHECK_SIGNATURE (Nbuf, NET_BUF_SIGNATURE);\r
  NET_CHECK_SIGNATURE (Nbuf->Vector, NET_VECTOR_SIGNATURE);\r
\r
  ASSERT (Len > 0);\r
\r
  if (FromHead) {\r
    //\r
    // Allocate some space from head. If the buffer is empty,\r
    // allocate from the first block. If it isn't, allocate\r
    // from the first non-empty block, or the block before that.\r
    //\r
    if (Nbuf->TotalSize == 0) {\r
      Index = 0;\r
    } else {\r
      NetbufGetByte (Nbuf, 0, &Index);\r
\r
      if ((NET_HEADSPACE(&(Nbuf->BlockOp[Index])) < Len) && (Index > 0)) {\r
        Index--;\r
      }\r
    }\r
\r
    BlockOp = &(Nbuf->BlockOp[Index]);\r
\r
    if (NET_HEADSPACE (BlockOp) < Len) {\r
      return NULL;\r
    }\r
\r
    BlockOp->Head   -= Len;\r
    BlockOp->Size   += Len;\r
    Nbuf->TotalSize += Len;\r
\r
    return BlockOp->Head;\r
\r
  } else {\r
    //\r
    // Allocate some space from the tail. If the buffer is empty,\r
    // allocate from the first block. If it isn't, allocate\r
    // from the last non-empty block, or the block after that.\r
    //\r
    if (Nbuf->TotalSize == 0) {\r
      Index = 0;\r
    } else {\r
      NetbufGetByte (Nbuf, Nbuf->TotalSize - 1, &Index);\r
\r
      if ((NET_TAILSPACE(&(Nbuf->BlockOp[Index])) < Len) &&\r
          (Index < Nbuf->BlockOpNum - 1)) {\r
\r
        Index++;\r
      }\r
    }\r
\r
    BlockOp = &(Nbuf->BlockOp[Index]);\r
\r
    if (NET_TAILSPACE (BlockOp) < Len) {\r
      return NULL;\r
    }\r
\r
    SavedTail       = BlockOp->Tail;\r
\r
    BlockOp->Tail   += Len;\r
    BlockOp->Size   += Len;\r
    Nbuf->TotalSize += Len;\r
\r
    return SavedTail;\r
  }\r
}\r
\r
\r
/**\r
  Trim a single NET_BLOCK.\r
\r
  @param[in, out]  BlockOp               Pointer to the NET_BLOCK.\r
  @param[in]       Len                   The length of the data to be trimmed.\r
  @param[in]       FromHead              The flag to indicate whether trim data from head or\r
                                         tail. TRUE for from head, and FALSE for from tail.\r
\r
**/\r
VOID\r
NetblockTrim (\r
  IN OUT NET_BLOCK_OP       *BlockOp,\r
  IN UINT32                 Len,\r
  IN BOOLEAN                FromHead\r
  )\r
{\r
  ASSERT (BlockOp && (BlockOp->Size >= Len));\r
\r
  BlockOp->Size -= Len;\r
\r
  if (FromHead) {\r
    BlockOp->Head += Len;\r
  } else {\r
    BlockOp->Tail -= Len;\r
  }\r
}\r
\r
\r
/**\r
  Trim some data from the header or tail of the buffer.\r
\r
  @param[in, out]  Nbuf                  Pointer to the net buffer.\r
  @param[in]       Len                   The length of the data to be trimmed.\r
  @param[in]      FromHead               The flag to indicate whether trim data from head or\r
                                         tail. TRUE for from head, and FALSE for from tail.\r
\r
  @return    Length of the actually trimmed data.\r
\r
**/\r
UINT32\r
EFIAPI\r
NetbufTrim (\r
  IN OUT NET_BUF            *Nbuf,\r
  IN UINT32                 Len,\r
  IN BOOLEAN                FromHead\r
  )\r
{\r
  NET_BLOCK_OP              *BlockOp;\r
  UINT32                    Index;\r
  UINT32                    Trimmed;\r
\r
  NET_CHECK_SIGNATURE (Nbuf, NET_BUF_SIGNATURE);\r
\r
  if (Len > Nbuf->TotalSize) {\r
    Len = Nbuf->TotalSize;\r
  }\r
\r
  //\r
  // If FromTail is true, iterate backward. That\r
  // is, init Index to NBuf->BlockNum - 1, and\r
  // decrease it by 1 during each loop. Otherwise,\r
  // iterate forward. That is, init Index to 0, and\r
  // increase it by 1 during each loop.\r
  //\r
  Trimmed          = 0;\r
  Nbuf->TotalSize -= Len;\r
\r
  Index   = (FromHead ? 0 : Nbuf->BlockOpNum - 1);\r
  BlockOp = Nbuf->BlockOp;\r
\r
  for (;;) {\r
    if (BlockOp[Index].Size == 0) {\r
      Index += (FromHead ? 1 : -1);\r
      continue;\r
    }\r
\r
    if (Len > BlockOp[Index].Size) {\r
      Len     -= BlockOp[Index].Size;\r
      Trimmed += BlockOp[Index].Size;\r
      NetblockTrim (&BlockOp[Index], BlockOp[Index].Size, FromHead);\r
    } else {\r
      Trimmed += Len;\r
      NetblockTrim (&BlockOp[Index], Len, FromHead);\r
      break;\r
    }\r
\r
    Index += (FromHead ? 1 : -1);\r
  }\r
\r
  return Trimmed;\r
}\r
\r
\r
/**\r
  Copy the data from the specific offset to the destination.\r
\r
  @param[in]   Nbuf                  Pointer to the net buffer.\r
  @param[in]   Offset                The sequence number of the first byte to copy.\r
  @param[in]   Len                   Length of the data to copy.\r
  @param[in]   Dest                  The destination of the data to copy to.\r
\r
  @retval UINTN                 The length of the copied data.\r
\r
**/\r
UINT32\r
EFIAPI\r
NetbufCopy (\r
  IN NET_BUF                *Nbuf,\r
  IN UINT32                 Offset,\r
  IN UINT32                 Len,\r
  IN UINT8                  *Dest\r
  )\r
{\r
  NET_BLOCK_OP              *BlockOp;\r
  UINT32                    Skip;\r
  UINT32                    Left;\r
  UINT32                    Copied;\r
  UINT32                    Index;\r
  UINT32                    Cur;\r
\r
  NET_CHECK_SIGNATURE (Nbuf, NET_BUF_SIGNATURE);\r
  ASSERT (Dest);\r
\r
  if ((Len == 0) || (Nbuf->TotalSize <= Offset)) {\r
    return 0;\r
  }\r
\r
  if (Nbuf->TotalSize - Offset < Len) {\r
    Len = Nbuf->TotalSize - Offset;\r
  }\r
\r
  BlockOp = Nbuf->BlockOp;\r
\r
  //\r
  // Skip to the offset. Don't make "Offset-By-One" error here.\r
  // Cur + BLOCK.SIZE is the first sequence number of next block.\r
  // So, (Offset < Cur + BLOCK.SIZE) means that the  first byte\r
  // is in the current block. if (Offset == Cur + BLOCK.SIZE), the\r
  // first byte is the next block's first byte.\r
  //\r
  Cur = 0;\r
\r
  for (Index = 0; Index < Nbuf->BlockOpNum; Index++) {\r
    if (BlockOp[Index].Size == 0) {\r
      continue;\r
    }\r
\r
    if (Offset < Cur + BlockOp[Index].Size) {\r
      break;\r
    }\r
\r
    Cur += BlockOp[Index].Size;\r
  }\r
\r
  //\r
  // Cur is the sequence number of the first byte in the block\r
  // Offset - Cur is the number of bytes before first byte to\r
  // to copy in the current block.\r
  //\r
  Skip  = Offset - Cur;\r
  Left  = BlockOp[Index].Size - Skip;\r
\r
  if (Len <= Left) {\r
    CopyMem (Dest, BlockOp[Index].Head + Skip, Len);\r
    return Len;\r
  }\r
\r
  CopyMem (Dest, BlockOp[Index].Head + Skip, Left);\r
\r
  Dest  += Left;\r
  Len   -= Left;\r
  Copied = Left;\r
\r
  Index++;\r
\r
  for (; Index < Nbuf->BlockOpNum; Index++) {\r
    if (Len > BlockOp[Index].Size) {\r
      Len    -= BlockOp[Index].Size;\r
      Copied += BlockOp[Index].Size;\r
\r
      CopyMem (Dest, BlockOp[Index].Head, BlockOp[Index].Size);\r
      Dest   += BlockOp[Index].Size;\r
    } else {\r
      Copied += Len;\r
      CopyMem (Dest, BlockOp[Index].Head, Len);\r
      break;\r
    }\r
  }\r
\r
  return Copied;\r
}\r
\r
\r
/**\r
  Initiate the net buffer queue.\r
\r
  @param[in, out]  NbufQue               Pointer to the net buffer queue to be initiated.\r
\r
**/\r
VOID\r
EFIAPI\r
NetbufQueInit (\r
  IN OUT NET_BUF_QUEUE          *NbufQue\r
  )\r
{\r
  NbufQue->Signature  = NET_QUE_SIGNATURE;\r
  NbufQue->RefCnt     = 1;\r
  InitializeListHead (&NbufQue->List);\r
\r
  InitializeListHead (&NbufQue->BufList);\r
  NbufQue->BufSize  = 0;\r
  NbufQue->BufNum   = 0;\r
}\r
\r
\r
/**\r
  Allocate an initialized net buffer queue.\r
\r
  @return                       Pointer to the allocated net buffer queue.If NULL, the\r
                                allocation failed due to resource limit.\r
\r
**/\r
NET_BUF_QUEUE  *\r
EFIAPI\r
NetbufQueAlloc (\r
  VOID\r
  )\r
{\r
  NET_BUF_QUEUE             *NbufQue;\r
\r
  NbufQue = AllocatePool (sizeof (NET_BUF_QUEUE));\r
  if (NbufQue == NULL) {\r
    return NULL;\r
  }\r
\r
  NetbufQueInit (NbufQue);\r
\r
  return NbufQue;\r
}\r
\r
\r
/**\r
  Free a net buffer queue.\r
\r
  @param[in]  NbufQue               Poitner to the net buffer queue to be freed.\r
\r
**/\r
VOID\r
EFIAPI\r
NetbufQueFree (\r
  IN NET_BUF_QUEUE          *NbufQue\r
  )\r
{\r
  NET_CHECK_SIGNATURE (NbufQue, NET_QUE_SIGNATURE);\r
\r
  NbufQue->RefCnt--;\r
\r
  if (NbufQue->RefCnt == 0) {\r
    NetbufQueFlush (NbufQue);\r
    gBS->FreePool (NbufQue);\r
  }\r
}\r
\r
\r
/**\r
  Append a buffer to the end of the queue.\r
\r
  @param[in, out]  NbufQue               Pointer to the net buffer queue.\r
  @param[in, out]  Nbuf                  Pointer to the net buffer to be appended.\r
\r
**/\r
VOID\r
EFIAPI\r
NetbufQueAppend (\r
  IN OUT NET_BUF_QUEUE          *NbufQue,\r
  IN OUT NET_BUF                *Nbuf\r
  )\r
{\r
  NET_CHECK_SIGNATURE (NbufQue, NET_QUE_SIGNATURE);\r
  NET_CHECK_SIGNATURE (Nbuf, NET_BUF_SIGNATURE);\r
\r
  InsertTailList (&NbufQue->BufList, &Nbuf->List);\r
\r
  NbufQue->BufSize += Nbuf->TotalSize;\r
  NbufQue->BufNum++;\r
}\r
\r
\r
/**\r
  Remove a net buffer from head in the specific queue.\r
\r
  @param[in, out]  NbufQue               Pointer to the net buffer queue.\r
\r
  @return                       Pointer to the net buffer removed from the specific\r
                                queue. If NULL, there is no net buffer in the specific\r
                                queue.\r
\r
**/\r
NET_BUF  *\r
EFIAPI\r
NetbufQueRemove (\r
  IN OUT NET_BUF_QUEUE          *NbufQue\r
  )\r
{\r
  NET_BUF                   *First;\r
\r
  NET_CHECK_SIGNATURE (NbufQue, NET_QUE_SIGNATURE);\r
\r
  if (NbufQue->BufNum == 0) {\r
    return NULL;\r
  }\r
\r
  First = NET_LIST_USER_STRUCT (NbufQue->BufList.ForwardLink, NET_BUF, List);\r
\r
  NetListRemoveHead (&NbufQue->BufList);\r
\r
  NbufQue->BufSize -= First->TotalSize;\r
  NbufQue->BufNum--;\r
  return First;\r
}\r
\r
\r
/**\r
  Copy some data from the buffer queue to the destination.\r
\r
  @param[in]   NbufQue               Pointer to the net buffer queue.\r
  @param[in]   Offset                The sequence number of the first byte to copy.\r
  @param[in]   Len                   Length of the data to copy.\r
  @param[out]  Dest                  The destination of the data to copy to.\r
\r
  @return       The length of the copied data. If 0, then the length is zero or offset \r
                suppress the total size of net buffer.\r
\r
**/\r
UINT32\r
EFIAPI\r
NetbufQueCopy (\r
  IN NET_BUF_QUEUE          *NbufQue,\r
  IN UINT32                 Offset,\r
  IN UINT32                 Len,\r
  OUT UINT8                 *Dest\r
  )\r
{\r
  LIST_ENTRY                *Entry;\r
  NET_BUF                   *Nbuf;\r
  UINT32                    Skip;\r
  UINT32                    Left;\r
  UINT32                    Cur;\r
  UINT32                    Copied;\r
\r
  NET_CHECK_SIGNATURE (NbufQue, NET_QUE_SIGNATURE);\r
  ASSERT (Dest != NULL);\r
\r
  if ((Len == 0) || (NbufQue->BufSize <= Offset)) {\r
    return 0;\r
  }\r
\r
  if (NbufQue->BufSize - Offset < Len) {\r
    Len = NbufQue->BufSize - Offset;\r
  }\r
\r
  //\r
  // skip to the Offset\r
  //\r
  Cur   = 0;\r
  Nbuf  = NULL;\r
\r
  NET_LIST_FOR_EACH (Entry, &NbufQue->BufList) {\r
    Nbuf = NET_LIST_USER_STRUCT (Entry, NET_BUF, List);\r
\r
    if (Offset < Cur + Nbuf->TotalSize) {\r
      break;\r
    }\r
\r
    Cur += Nbuf->TotalSize;\r
  }\r
\r
  //\r
  // Copy the data in the first buffer.\r
  //\r
  Skip  = Offset - Cur;\r
  Left  = Nbuf->TotalSize - Skip;\r
\r
  if (Len < Left) {\r
    return NetbufCopy (Nbuf, Skip, Len, Dest);\r
  }\r
\r
  NetbufCopy (Nbuf, Skip, Left, Dest);\r
  Dest  += Left;\r
  Len   -= Left;\r
  Copied = Left;\r
\r
  //\r
  // Iterate over the others\r
  //\r
  Entry = Entry->ForwardLink;\r
\r
  while ((Len > 0) && (Entry != &NbufQue->BufList)) {\r
    Nbuf = NET_LIST_USER_STRUCT (Entry, NET_BUF, List);\r
\r
    if (Len > Nbuf->TotalSize) {\r
      Len -= Nbuf->TotalSize;\r
      Copied += Nbuf->TotalSize;\r
\r
      NetbufCopy (Nbuf, 0, Nbuf->TotalSize, Dest);\r
      Dest += Nbuf->TotalSize;\r
\r
    } else {\r
      NetbufCopy (Nbuf, 0, Len, Dest);\r
      Copied += Len;\r
      break;\r
    }\r
\r
    Entry = Entry->ForwardLink;\r
  }\r
\r
  return Copied;\r
}\r
\r
\r
/**\r
  Trim some data from the queue header, release the buffer if\r
  whole buffer is trimmed.\r
\r
  @param[in, out]  NbufQue               Pointer to the net buffer queue.\r
  @param[in]       Len                   Length of the data to trim.\r
\r
  @return   The length of the data trimmed, or 0 if length of the data to trim is zero.\r
\r
**/\r
UINT32\r
EFIAPI\r
NetbufQueTrim (\r
  IN OUT NET_BUF_QUEUE      *NbufQue,\r
  IN UINT32                 Len\r
  )\r
{\r
  LIST_ENTRY                *Entry;\r
  LIST_ENTRY                *Next;\r
  NET_BUF                   *Nbuf;\r
  UINT32                    Trimmed;\r
\r
  NET_CHECK_SIGNATURE (NbufQue, NET_QUE_SIGNATURE);\r
\r
  if (Len == 0) {\r
    return 0;\r
  }\r
\r
  if (Len > NbufQue->BufSize) {\r
    Len = NbufQue->BufSize;\r
  }\r
\r
  NbufQue->BufSize -= Len;\r
  Trimmed = 0;\r
\r
  NET_LIST_FOR_EACH_SAFE (Entry, Next, &NbufQue->BufList) {\r
    Nbuf = NET_LIST_USER_STRUCT (Entry, NET_BUF, List);\r
\r
    if (Len >= Nbuf->TotalSize) {\r
      Trimmed += Nbuf->TotalSize;\r
      Len -= Nbuf->TotalSize;\r
\r
      RemoveEntryList (Entry);\r
      NetbufFree (Nbuf);\r
\r
      NbufQue->BufNum--;\r
\r
      if (Len == 0) {\r
        break;\r
      }\r
\r
    } else {\r
      Trimmed += NetbufTrim (Nbuf, Len, NET_BUF_HEAD);\r
      break;\r
    }\r
  }\r
\r
  return Trimmed;\r
}\r
\r
\r
/**\r
  Flush the net buffer queue.\r
\r
  @param[in, out]  NbufQue               Pointer to the queue to be flushed.\r
\r
**/\r
VOID\r
EFIAPI\r
NetbufQueFlush (\r
  IN OUT NET_BUF_QUEUE          *NbufQue\r
  )\r
{\r
  NET_CHECK_SIGNATURE (NbufQue, NET_QUE_SIGNATURE);\r
\r
  NetbufFreeList (&NbufQue->BufList);\r
\r
  NbufQue->BufNum   = 0;\r
  NbufQue->BufSize  = 0;\r
}\r
\r
\r
/**\r
  Compute checksum for a bulk of data.\r
\r
  @param[in]   Bulk                  Pointer to the data.\r
  @param[in]   Len                   Length of the data, in bytes.\r
\r
  @return    The computed checksum.\r
\r
**/\r
UINT16\r
EFIAPI\r
NetblockChecksum (\r
  IN UINT8                  *Bulk,\r
  IN UINT32                 Len\r
  )\r
{\r
  register UINT32           Sum;\r
\r
  Sum = 0;\r
\r
  while (Len > 1) {\r
    Sum += *(UINT16 *) Bulk;\r
    Bulk += 2;\r
    Len -= 2;\r
  }\r
\r
  //\r
  // Add left-over byte, if any\r
  //\r
  if (Len > 0) {\r
    Sum += *(UINT8 *) Bulk;\r
  }\r
\r
  //\r
  // Fold 32-bit sum to 16 bits\r
  //\r
  while ((Sum >> 16) != 0) {\r
    Sum = (Sum & 0xffff) + (Sum >> 16);\r
\r
  }\r
\r
  return (UINT16) Sum;\r
}\r
\r
\r
/**\r
  Add two checksums.\r
\r
  @param[in]   Checksum1             The first checksum to be added.\r
  @param[in]   Checksum2             The second checksum to be added.\r
\r
  @return         The new checksum.\r
\r
**/\r
UINT16\r
EFIAPI\r
NetAddChecksum (\r
  IN UINT16                 Checksum1,\r
  IN UINT16                 Checksum2\r
  )\r
{\r
  UINT32                    Sum;\r
\r
  Sum = Checksum1 + Checksum2;\r
\r
  //\r
  // two UINT16 can only add up to a carry of 1.\r
  //\r
  if ((Sum >> 16) != 0) {\r
    Sum = (Sum & 0xffff) + 1;\r
\r
  }\r
\r
  return (UINT16) Sum;\r
}\r
\r
\r
/**\r
  Compute the checksum for a NET_BUF.\r
\r
  @param[in]   Nbuf                  Pointer to the net buffer.\r
\r
  @return    The computed checksum.\r
\r
**/\r
UINT16\r
EFIAPI\r
NetbufChecksum (\r
  IN NET_BUF                *Nbuf\r
  )\r
{\r
  NET_BLOCK_OP              *BlockOp;\r
  UINT32                    Offset;\r
  UINT16                    TotalSum;\r
  UINT16                    BlockSum;\r
  UINT32                    Index;\r
\r
  NET_CHECK_SIGNATURE (Nbuf, NET_BUF_SIGNATURE);\r
\r
  TotalSum  = 0;\r
  Offset    = 0;\r
  BlockOp   = Nbuf->BlockOp;\r
\r
  for (Index = 0; Index < Nbuf->BlockOpNum; Index++) {\r
    if (BlockOp[Index].Size == 0) {\r
      continue;\r
    }\r
\r
    BlockSum = NetblockChecksum (BlockOp[Index].Head, BlockOp[Index].Size);\r
\r
    if ((Offset & 0x01) != 0) {\r
      //\r
      // The checksum starts with an odd byte, swap\r
      // the checksum before added to total checksum\r
      //\r
      BlockSum = (UINT16) NET_SWAP_SHORT (BlockSum);\r
    }\r
\r
    TotalSum = NetAddChecksum (BlockSum, TotalSum);\r
    Offset  += BlockOp[Index].Size;\r
  }\r
\r
  return TotalSum;\r
}\r
\r
\r
/**\r
  Compute the checksum for TCP/UDP pseudo header.\r
  Src, Dst are in network byte order. and Len is\r
  in host byte order.\r
\r
  @param[in]   Src                   The source address of the packet.\r
  @param[in]   Dst                   The destination address of the packet.\r
  @param[in]   Proto                 The protocol type of the packet.\r
  @param[in]   Len                   The length of the packet.\r
\r
  @return   The computed checksum.\r
\r
**/\r
UINT16\r
EFIAPI\r
NetPseudoHeadChecksum (\r
  IN IP4_ADDR               Src,\r
  IN IP4_ADDR               Dst,\r
  IN UINT8                  Proto,\r
  IN UINT16                 Len\r
  )\r
{\r
  NET_PSEUDO_HDR            Hdr;\r
\r
  //\r
  // Zero the memory to relieve align problems\r
  //\r
  ZeroMem (&Hdr, sizeof (Hdr));\r
\r
  Hdr.SrcIp     = Src;\r
  Hdr.DstIp     = Dst;\r
  Hdr.Protocol  = Proto;\r
  Hdr.Len       = HTONS (Len);\r
\r
  return NetblockChecksum ((UINT8 *) &Hdr, sizeof (Hdr));\r
}\r