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

/** @file\r
\r
Copyright (c) 2005 - 2006, Intel Corporation\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
Module Name:\r
\r
  NetBuffer.c\r
\r
Abstract:\r
\r
\r
\r
**/\r
\r
#include <PiDxe.h>

#include <Library/NetLib.h>
#include <Library/BaseLib.h>
#include <Library/DebugLib.h>
#include <Library/BaseMemoryLib.h>
#include <Library/UefiBootServicesTableLib.h>
#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  BlockNum              The number of NET_BLOCK in the Vector of net buffer\r
  @param  BlockOpNum            The number of NET_BLOCK_OP in the net buffer\r
\r
  @retval *                     Pointer to the allocated NET_BUF. If NULL  the\r
                                allocation failed due to resource limit.\r
\r
**/\r
STATIC\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 = NetAllocateZeroPool (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
  NetListInit (&Nbuf->List);\r
\r
  if (BlockNum != 0) {\r
    Vector = NetAllocateZeroPool (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
  NetFreePool (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  Len                   The length of the block.\r
\r
  @retval *                     Pointer to the allocated NET_BUF. If NULL  the\r
                                allocation failed due to resource limit.\r
\r
**/\r
NET_BUF  *\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 = NetAllocatePool (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
  NetFreePool (Nbuf);\r
  return NULL;\r
}\r
\r
\r
/**\r
  Free the vector\r
\r
  @param  Vector                Pointer to the NET_VECTOR to be freed.\r
\r
  @return None.\r
\r
**/\r
STATIC\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
      NetFreePool (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
      NetFreePool (Vector->Block[Index].Bulk);\r
    }\r
  }\r
\r
  NetFreePool (Vector);\r
}\r
\r
\r
/**\r
  Free the buffer and its associated NET_VECTOR.\r
\r
  @param  Nbuf                  Pointer to the NET_BUF to be freed.\r
\r
  @return None.\r
\r
**/\r
VOID\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
    NetFreePool (Nbuf);\r
  }\r
}\r
\r
\r
/**\r
  Create a copy of NET_BUF that share the associated NET_DATA.\r
\r
  @param  Nbuf                  Pointer to the net buffer to be cloned.\r
\r
  @retval *                     Pointer to the cloned net buffer.\r
\r
**/\r
NET_BUF  *\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 = NetAllocatePool (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
  NetListInit (&Clone->List);\r
\r
  Clone->Ip   = Nbuf->Ip;\r
  Clone->Tcp  = Nbuf->Tcp;\r
\r
  NetCopyMem (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
  NetCopyMem (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  Nbuf                  Pointer to the net buffer to be cloned.\r
  @param  Duplicate             Pointer to the net buffer to duplicate to, if NULL\r
                                a new net  buffer is allocated.\r
  @param  HeadSpace             Length of the head space to reserve\r
\r
  @retval *                     Pointer to the duplicated net buffer.\r
\r
**/\r
NET_BUF  *\r
NetbufDuplicate (\r
  IN NET_BUF                *Nbuf,\r
  IN 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
  NetCopyMem (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  Head                  Pointer to the head of linked net buffers.\r
\r
  @return None.\r
\r
**/\r
VOID\r
NetbufFreeList (\r
  IN NET_LIST_ENTRY         *Head\r
  )\r
{\r
  NET_LIST_ENTRY            *Entry;\r
  NET_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
    NetListRemoveEntry (Entry);\r
    NetbufFree (Nbuf);\r
  }\r
\r
  ASSERT (NetListIsEmpty (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  Nbuf                  Pointer to the net buffer.\r
  @param  Offset                The index or offset of the byte\r
  @param  Index                 Index of the fragment that contains the block\r
\r
  @retval *                     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
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  Nbuf                  Pointer to the net buffer.\r
  @param  Bulk                  Pointer to the data.\r
  @param  Len                   Length of the bulk data.\r
  @param  Index                 The data block index in the net buffer the bulk\r
                                data should belong to.\r
\r
  @return None.\r
\r
**/\r
STATIC\r
VOID\r
NetbufSetBlock (\r
  IN 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  Nbuf                  Pointer to the net buffer.\r
  @param  Bulk                  Pointer to the data.\r
  @param  Len                   Length of the bulk data.\r
  @param  Index                 The data block index in the net buffer the bulk\r
                                data should belong to.\r
\r
  @return None.\r
\r
**/\r
STATIC\r
VOID\r
NetbufSetBlockOp (\r
  IN 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  Arg                   Point to the old NET_VECTOR\r
\r
  @return NONE\r
\r
**/\r
STATIC\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  Nbuf                  Pointer to the net buffer to be cloned.\r
  @param  Offset                Starting point of the data to be included in new\r
                                buffer.\r
  @param  Len                   How many data to include in new data\r
  @param  HeadSpace             How many bytes of head space to reserve for\r
                                protocol header\r
\r
  @retval *                     Pointer to the cloned net buffer.\r
\r
**/\r
NET_BUF  *\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 = NetAllocatePool (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
  NetCopyMem (Child->ProtoData, Nbuf->ProtoData, NET_PROTO_DATA);\r
  return Child;\r
\r
FreeChild:\r
\r
  NetFreePool (Child);\r
  return NULL;\r
}\r
\r
\r
\r
/**\r
  Build a NET_BUF from external blocks.\r
\r
  @param  ExtFragment           Pointer to the data block.\r
  @param  ExtNum                The number of the data block.\r
  @param  HeadSpace             The head space to be reserved.\r
  @param  HeadLen               The length of the protocol header, This function\r
                                will pull that number of data into a linear block.\r
  @param  ExtFree               Pointer to the caller provided free function.\r
  @param  Arg                   The argument passed to ExtFree when ExtFree is\r
                                called.\r
\r
  @retval *                     Pointer to the net buffer built from the data\r
                                blocks.\r
\r
**/\r
NET_BUF  *\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    = NetAllocatePool (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
        NetCopyMem (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
        NetCopyMem (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 ? 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) {\r
    ExtFragment[SavedIndex] = SavedFragment;\r
  }\r
\r
  return Nbuf;\r
\r
FreeFirstBlock:\r
  NetFreePool (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  Nbuf                  Point to the net buffer\r
  @param  ExtFragment           Pointer to the data block.\r
  @param  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
NetbufBuildExt (\r
  IN NET_BUF                *Nbuf,\r
  IN NET_FRAGMENT           *ExtFragment,\r
  IN 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  BufList               A List of NET_BUF.\r
  @param  HeadSpace             The head space to be reserved.\r
  @param  HeaderLen             The length of the protocol header, This function\r
                                will pull that number of data into a linear block.\r
  @param  ExtFree               Pointer to the caller provided free function.\r
  @param  Arg                   The argument passed to ExtFree when ExtFree is\r
                                called.\r
\r
  @retval *                     Pointer to the net buffer built from the data\r
                                blocks.\r
\r
**/\r
NET_BUF  *\r
NetbufFromBufList (\r
  IN NET_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
  NET_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 = NetAllocatePool (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
  NetFreePool (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  Nbuf                  Pointer to the net buffer.\r
  @param  Len                   The length of buffer to be reserverd.\r
\r
  @return None.\r
\r
**/\r
VOID\r
NetbufReserve (\r
  IN 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  Nbuf                  Pointer to the net buffer.\r
  @param  Len                   The length of the buffer to be allocated.\r
  @param  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
  @retval *                     Pointer to the first byte of the allocated buffer.\r
\r
**/\r
UINT8  *\r
NetbufAllocSpace (\r
  IN 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  BlockOp               Pointer to the NET_BLOCK.\r
  @param  Len                   The length of the data to be trimmed.\r
  @param  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 None.\r
\r
**/\r
STATIC\r
VOID\r
NetblockTrim (\r
  IN 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  Nbuf                  Pointer to the net buffer.\r
  @param  Len                   The length of the data to be trimmed.\r
  @param  FromHead              The flag to indicate whether trim data from head or\r
                                tail. TRUE for from head, and FALSE for from tail.\r
\r
  @retval UINTN                 Length of the actually trimmed data.\r
\r
**/\r
UINT32\r
NetbufTrim (\r
  IN 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  Nbuf                  Pointer to the net buffer.\r
  @param  Offset                The sequence number of the first byte to copy.\r
  @param  Len                   Length of the data to copy.\r
  @param  Dest                  The destination of the data to copy to.\r
\r
  @retval UINTN                 The length of the copied data.\r
\r
**/\r
UINT32\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
    NetCopyMem (Dest, BlockOp[Index].Head + Skip, Len);\r
    return Len;\r
  }\r
\r
  NetCopyMem (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
      NetCopyMem (Dest, BlockOp[Index].Head, BlockOp[Index].Size);\r
      Dest   += BlockOp[Index].Size;\r
    } else {\r
      Copied += Len;\r
      NetCopyMem (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  NbufQue               Pointer to the net buffer queue to be initiated.\r
\r
  @return None.\r
\r
**/\r
VOID\r
NetbufQueInit (\r
  IN NET_BUF_QUEUE          *NbufQue\r
  )\r
{\r
  NbufQue->Signature  = NET_QUE_SIGNATURE;\r
  NbufQue->RefCnt     = 1;\r
  NetListInit (&NbufQue->List);\r
\r
  NetListInit (&NbufQue->BufList);\r
  NbufQue->BufSize  = 0;\r
  NbufQue->BufNum   = 0;\r
}\r
\r
\r
/**\r
  Allocate an initialized net buffer queue.\r
\r
  None.\r
\r
  @retval *                     Pointer to the allocated net buffer queue.\r
\r
**/\r
NET_BUF_QUEUE  *\r
NetbufQueAlloc (\r
  VOID\r
  )\r
{\r
  NET_BUF_QUEUE             *NbufQue;\r
\r
  NbufQue = NetAllocatePool (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  NbufQue               Poitner to the net buffer queue to be freed.\r
\r
  @return None.\r
\r
**/\r
VOID\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
    NetFreePool (NbufQue);\r
  }\r
}\r
\r
\r
/**\r
  Append a buffer to the end of the queue.\r
\r
  @param  NbufQue               Pointer to the net buffer queue.\r
  @param  Nbuf                  Pointer to the net buffer to be appended.\r
\r
  @return None.\r
\r
**/\r
VOID\r
NetbufQueAppend (\r
  IN NET_BUF_QUEUE          *NbufQue,\r
  IN NET_BUF                *Nbuf\r
  )\r
{\r
  NET_CHECK_SIGNATURE (NbufQue, NET_QUE_SIGNATURE);\r
  NET_CHECK_SIGNATURE (Nbuf, NET_BUF_SIGNATURE);\r
\r
  NetListInsertTail (&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  NbufQue               Pointer to the net buffer queue.\r
\r
  @retval *                     Pointer to the net buffer removed from the specific\r
                                queue.\r
\r
**/\r
NET_BUF  *\r
NetbufQueRemove (\r
  IN 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  NbufQue               Pointer to the net buffer queue.\r
  @param  Offset                The sequence number of the first byte to copy.\r
  @param  Len                   Length of the data to copy.\r
  @param  Dest                  The destination of the data to copy to.\r
\r
  @retval UINTN                 The length of the copied data.\r
\r
**/\r
UINT32\r
NetbufQueCopy (\r
  IN NET_BUF_QUEUE          *NbufQue,\r
  IN UINT32                 Offset,\r
  IN UINT32                 Len,\r
  IN UINT8                  *Dest\r
  )\r
{\r
  NET_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  NbufQue               Pointer to the net buffer queue.\r
  @param  Len                   Length of the data to trim.\r
\r
  @retval UINTN                 The length of the data trimmed.\r
\r
**/\r
UINT32\r
NetbufQueTrim (\r
  IN NET_BUF_QUEUE          *NbufQue,\r
  IN UINT32                 Len\r
  )\r
{\r
  NET_LIST_ENTRY            *Entry;\r
  NET_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
      NetListRemoveEntry (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  NbufQue               Pointer to the queue to be flushed.\r
\r
  @return None.\r
\r
**/\r
VOID\r
NetbufQueFlush (\r
  IN 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  Bulk                  Pointer to the data.\r
  @param  Len                   Length of the data, in bytes.\r
\r
  @retval UINT16                The computed checksum.\r
\r
**/\r
UINT16\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) {\r
    Sum = (Sum & 0xffff) + (Sum >> 16);\r
\r
  }\r
\r
  return (UINT16) Sum;\r
}\r
\r
\r
/**\r
  Add two checksums.\r
\r
  @param  Checksum1             The first checksum to be added.\r
  @param  Checksum2             The second checksum to be added.\r
\r
  @retval UINT16                The new checksum.\r
\r
**/\r
UINT16\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) {\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  Nbuf                  Pointer to the net buffer.\r
\r
  @retval UINT16                The computed checksum.\r
\r
**/\r
UINT16\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) {\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  Src                   The source address of the packet.\r
  @param  Dst                   The destination address of the packet.\r
  @param  Proto                 The protocol type of the packet.\r
  @param  Len                   The length of the packet.\r
\r
  @retval UINT16                The computed checksum.\r
\r
**/\r
UINT16\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
  NetZeroMem (&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