NetBuffer.c

Go to the documentation of this file.

 
#include <Uefi.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>
 
NET_BUF *
NetbufAllocStruct (
  IN UINT32  BlockNum,
  IN UINT32  BlockOpNum
  )
{
  NET_BUF     *Nbuf;
  NET_VECTOR  *Vector;
 
  ASSERT (BlockOpNum >= 1);
 
  //
  // Allocate three memory blocks.
  //
  Nbuf = AllocateZeroPool (NET_BUF_SIZE (BlockOpNum));
 
  if (Nbuf == NULL) {
    return NULL;
  }
 
  Nbuf->Signature  = NET_BUF_SIGNATURE;
  Nbuf->RefCnt     = 1;
  Nbuf->BlockOpNum = BlockOpNum;
  InitializeListHead (&Nbuf->List);
 
  if (BlockNum != 0) {
    Vector = AllocateZeroPool (NET_VECTOR_SIZE (BlockNum));
 
    if (Vector == NULL) {
      goto FreeNbuf;
    }
 
    Vector->Signature = NET_VECTOR_SIGNATURE;
    Vector->RefCnt    = 1;
    Vector->BlockNum  = BlockNum;
    Nbuf->Vector      = Vector;
  }
 
  return Nbuf;
 
FreeNbuf:
 
  FreePool (Nbuf);
  return NULL;
}
 
NET_BUF  *
EFIAPI
NetbufAlloc (
  IN UINT32  Len
  )
{
  NET_BUF     *Nbuf;
  NET_VECTOR  *Vector;
  UINT8       *Bulk;
 
  ASSERT (Len > 0);
 
  Nbuf = NetbufAllocStruct (1, 1);
 
  if (Nbuf == NULL) {
    return NULL;
  }
 
  Bulk = AllocatePool (Len);
 
  if (Bulk == NULL) {
    goto FreeNBuf;
  }
 
  Vector      = Nbuf->Vector;
  Vector->Len = Len;
 
  Vector->Block[0].Bulk = Bulk;
  Vector->Block[0].Len  = Len;
 
  Nbuf->BlockOp[0].BlockHead = Bulk;
  Nbuf->BlockOp[0].BlockTail = Bulk + Len;
 
  Nbuf->BlockOp[0].Head = Bulk;
  Nbuf->BlockOp[0].Tail = Bulk;
  Nbuf->BlockOp[0].Size = 0;
 
  return Nbuf;
 
FreeNBuf:
  FreePool (Nbuf);
  return NULL;
}
 
VOID
NetbufFreeVector (
  IN NET_VECTOR  *Vector
  )
{
  UINT32  Index;
 
  ASSERT (Vector != NULL);
  NET_CHECK_SIGNATURE (Vector, NET_VECTOR_SIGNATURE);
  ASSERT (Vector->RefCnt > 0);
 
  Vector->RefCnt--;
 
  if (Vector->RefCnt > 0) {
    return;
  }
 
  if (Vector->Free != NULL) {
    //
    // Call external free function to free the vector if it
    // isn't NULL. If NET_VECTOR_OWN_FIRST is set, release the
    // first block since it is allocated by us
    //
    if ((Vector->Flag & NET_VECTOR_OWN_FIRST) != 0) {
      gBS->FreePool (Vector->Block[0].Bulk);
    }
 
    Vector->Free (Vector->Arg);
  } else {
    //
    // Free each memory block associated with the Vector
    //
    for (Index = 0; Index < Vector->BlockNum; Index++) {
      gBS->FreePool (Vector->Block[Index].Bulk);
    }
  }
 
  FreePool (Vector);
}
 
VOID
EFIAPI
NetbufFree (
  IN NET_BUF  *Nbuf
  )
{
  ASSERT (Nbuf != NULL);
  NET_CHECK_SIGNATURE (Nbuf, NET_BUF_SIGNATURE);
  ASSERT (Nbuf->RefCnt > 0);
 
  Nbuf->RefCnt--;
 
  if (Nbuf->RefCnt == 0) {
    //
    // Update Vector only when NBuf is to be released. That is,
    // all the sharing of Nbuf increse Vector's RefCnt by one
    //
    NetbufFreeVector (Nbuf->Vector);
    FreePool (Nbuf);
  }
}
 
NET_BUF *
EFIAPI
NetbufClone (
  IN NET_BUF  *Nbuf
  )
{
  NET_BUF  *Clone;
 
  NET_CHECK_SIGNATURE (Nbuf, NET_BUF_SIGNATURE);
 
  Clone = AllocatePool (NET_BUF_SIZE (Nbuf->BlockOpNum));
 
  if (Clone == NULL) {
    return NULL;
  }
 
  Clone->Signature = NET_BUF_SIGNATURE;
  Clone->RefCnt    = 1;
  InitializeListHead (&Clone->List);
 
  Clone->Ip  = Nbuf->Ip;
  Clone->Tcp = Nbuf->Tcp;
 
  CopyMem (Clone->ProtoData, Nbuf->ProtoData, NET_PROTO_DATA);
 
  NET_GET_REF (Nbuf->Vector);
 
  Clone->Vector     = Nbuf->Vector;
  Clone->BlockOpNum = Nbuf->BlockOpNum;
  Clone->TotalSize  = Nbuf->TotalSize;
  CopyMem (Clone->BlockOp, Nbuf->BlockOp, sizeof (NET_BLOCK_OP) * Nbuf->BlockOpNum);
 
  return Clone;
}
 
NET_BUF  *
EFIAPI
NetbufDuplicate (
  IN NET_BUF      *Nbuf,
  IN OUT NET_BUF  *Duplicate        OPTIONAL,
  IN UINT32       HeadSpace
  )
{
  UINT8  *Dst;
 
  NET_CHECK_SIGNATURE (Nbuf, NET_BUF_SIGNATURE);
 
  if (Duplicate == NULL) {
    Duplicate = NetbufAlloc (Nbuf->TotalSize + HeadSpace);
  }
 
  if (Duplicate == NULL) {
    return NULL;
  }
 
  //
  // Don't set the IP and TCP head point, since it is most
  // like that they are pointing to the memory of Nbuf.
  //
  CopyMem (Duplicate->ProtoData, Nbuf->ProtoData, NET_PROTO_DATA);
  NetbufReserve (Duplicate, HeadSpace);
 
  Dst = NetbufAllocSpace (Duplicate, Nbuf->TotalSize, NET_BUF_TAIL);
  NetbufCopy (Nbuf, 0, Nbuf->TotalSize, Dst);
 
  return Duplicate;
}
 
VOID
EFIAPI
NetbufFreeList (
  IN OUT LIST_ENTRY  *Head
  )
{
  LIST_ENTRY  *Entry;
  LIST_ENTRY  *Next;
  NET_BUF     *Nbuf;
 
  Entry = Head->ForwardLink;
 
  NET_LIST_FOR_EACH_SAFE (Entry, Next, Head) {
    Nbuf = NET_LIST_USER_STRUCT (Entry, NET_BUF, List);
    NET_CHECK_SIGNATURE (Nbuf, NET_BUF_SIGNATURE);
 
    RemoveEntryList (Entry);
    NetbufFree (Nbuf);
  }
 
  ASSERT (IsListEmpty (Head));
}
 
UINT8  *
EFIAPI
NetbufGetByte (
  IN  NET_BUF  *Nbuf,
  IN  UINT32   Offset,
  OUT UINT32   *Index  OPTIONAL
  )
{
  NET_BLOCK_OP  *BlockOp;
  UINT32        Loop;
  UINT32        Len;
 
  NET_CHECK_SIGNATURE (Nbuf, NET_BUF_SIGNATURE);
 
  if (Offset >= Nbuf->TotalSize) {
    return NULL;
  }
 
  BlockOp = Nbuf->BlockOp;
  Len     = 0;
 
  for (Loop = 0; Loop < Nbuf->BlockOpNum; Loop++) {
    if (Len + BlockOp[Loop].Size <= Offset) {
      Len += BlockOp[Loop].Size;
      continue;
    }
 
    if (Index != NULL) {
      *Index = Loop;
    }
 
    return BlockOp[Loop].Head + (Offset - Len);
  }
 
  return NULL;
}
 
VOID
NetbufSetBlock (
  IN OUT NET_BUF  *Nbuf,
  IN UINT8        *Bulk,
  IN UINT32       Len,
  IN UINT32       Index
  )
{
  NET_BLOCK_OP  *BlockOp;
  NET_BLOCK     *Block;
 
  NET_CHECK_SIGNATURE (Nbuf, NET_BUF_SIGNATURE);
  NET_CHECK_SIGNATURE (Nbuf->Vector, NET_VECTOR_SIGNATURE);
  ASSERT (Index < Nbuf->BlockOpNum);
 
  Block              = &(Nbuf->Vector->Block[Index]);
  BlockOp            = &(Nbuf->BlockOp[Index]);
  Block->Len         = Len;
  Block->Bulk        = Bulk;
  BlockOp->BlockHead = Bulk;
  BlockOp->BlockTail = Bulk + Len;
  BlockOp->Head      = Bulk;
  BlockOp->Tail      = Bulk + Len;
  BlockOp->Size      = Len;
}
 
VOID
NetbufSetBlockOp (
  IN OUT NET_BUF  *Nbuf,
  IN UINT8        *Bulk,
  IN UINT32       Len,
  IN UINT32       Index
  )
{
  NET_BLOCK_OP  *BlockOp;
 
  NET_CHECK_SIGNATURE (Nbuf, NET_BUF_SIGNATURE);
  ASSERT (Index < Nbuf->BlockOpNum);
 
  BlockOp            = &(Nbuf->BlockOp[Index]);
  BlockOp->BlockHead = Bulk;
  BlockOp->BlockTail = Bulk + Len;
  BlockOp->Head      = Bulk;
  BlockOp->Tail      = Bulk + Len;
  BlockOp->Size      = Len;
}
 
VOID
EFIAPI
NetbufGetFragmentFree (
  IN VOID  *Arg
  )
{
  NET_VECTOR  *Vector;
 
  Vector = (NET_VECTOR *)Arg;
  NetbufFreeVector (Vector);
}
 
NET_BUF  *
EFIAPI
NetbufGetFragment (
  IN NET_BUF  *Nbuf,
  IN UINT32   Offset,
  IN UINT32   Len,
  IN UINT32   HeadSpace
  )
{
  NET_BUF       *Child;
  NET_VECTOR    *Vector;
  NET_BLOCK_OP  *BlockOp;
  UINT32        CurBlockOp;
  UINT32        BlockOpNum;
  UINT8         *FirstBulk;
  UINT32        Index;
  UINT32        First;
  UINT32        Last;
  UINT32        FirstSkip;
  UINT32        FirstLen;
  UINT32        LastLen;
  UINT32        Cur;
 
  NET_CHECK_SIGNATURE (Nbuf, NET_BUF_SIGNATURE);
 
  if ((Len == 0) || (Offset + Len > Nbuf->TotalSize)) {
    return NULL;
  }
 
  //
  // First find the first and last BlockOp that contains
  // the valid data, and compute the offset of the first
  // BlockOp and length of the last BlockOp
  //
  BlockOp = Nbuf->BlockOp;
  Cur     = 0;
 
  for (Index = 0; Index < Nbuf->BlockOpNum; Index++) {
    if (Offset < Cur + BlockOp[Index].Size) {
      break;
    }
 
    Cur += BlockOp[Index].Size;
  }
 
  //
  // First is the index of the first BlockOp, FirstSkip is
  // the offset of the first byte in the first BlockOp.
  //
  First     = Index;
  FirstSkip = Offset - Cur;
  FirstLen  = BlockOp[Index].Size - FirstSkip;
 
  Last    = 0;
  LastLen = 0;
 
  if (Len > FirstLen) {
    Cur += BlockOp[Index].Size;
    Index++;
 
    for ( ; Index < Nbuf->BlockOpNum; Index++) {
      if (Offset + Len <= Cur + BlockOp[Index].Size) {
        Last    = Index;
        LastLen = Offset + Len - Cur;
        break;
      }
 
      Cur += BlockOp[Index].Size;
    }
  } else {
    Last     = First;
    LastLen  = Len;
    FirstLen = Len;
  }
 
  ASSERT (Last >= First);
  BlockOpNum = Last - First + 1;
  CurBlockOp = 0;
 
  if (HeadSpace != 0) {
    //
    // Allocate an extra block to accommodate the head space.
    //
    BlockOpNum++;
 
    Child = NetbufAllocStruct (1, BlockOpNum);
 
    if (Child == NULL) {
      return NULL;
    }
 
    FirstBulk = AllocatePool (HeadSpace);
 
    if (FirstBulk == NULL) {
      goto FreeChild;
    }
 
    Vector       = Child->Vector;
    Vector->Free = NetbufGetFragmentFree;
    Vector->Arg  = Nbuf->Vector;
    Vector->Flag = NET_VECTOR_OWN_FIRST;
    Vector->Len  = HeadSpace;
 
    //
    // Reserve the head space in the first block
    //
    NetbufSetBlock (Child, FirstBulk, HeadSpace, 0);
    Child->BlockOp[0].Head += HeadSpace;
    Child->BlockOp[0].Size  =  0;
    CurBlockOp++;
  } else {
    Child = NetbufAllocStruct (0, BlockOpNum);
 
    if (Child == NULL) {
      return NULL;
    }
 
    Child->Vector = Nbuf->Vector;
  }
 
  NET_GET_REF (Nbuf->Vector);
  Child->TotalSize = Len;
 
  //
  // Set all the BlockOp up, the first and last one are special
  // and need special process.
  //
  NetbufSetBlockOp (
    Child,
    Nbuf->BlockOp[First].Head + FirstSkip,
    FirstLen,
    CurBlockOp++
    );
 
  for (Index = First + 1; Index < Last; Index++) {
    NetbufSetBlockOp (
      Child,
      BlockOp[Index].Head,
      BlockOp[Index].Size,
      CurBlockOp++
      );
  }
 
  if (First != Last) {
    NetbufSetBlockOp (
      Child,
      BlockOp[Last].Head,
      LastLen,
      CurBlockOp
      );
  }
 
  CopyMem (Child->ProtoData, Nbuf->ProtoData, NET_PROTO_DATA);
  return Child;
 
FreeChild:
 
  FreePool (Child);
  return NULL;
}
 
NET_BUF  *
EFIAPI
NetbufFromExt (
  IN NET_FRAGMENT         *ExtFragment,
  IN UINT32               ExtNum,
  IN UINT32               HeadSpace,
  IN UINT32               HeadLen,
  IN NET_VECTOR_EXT_FREE  ExtFree,
  IN VOID                 *Arg          OPTIONAL
  )
{
  NET_BUF       *Nbuf;
  NET_VECTOR    *Vector;
  NET_FRAGMENT  SavedFragment;
  UINT32        SavedIndex;
  UINT32        TotalLen;
  UINT32        BlockNum;
  UINT8         *FirstBlock;
  UINT32        FirstBlockLen;
  UINT8         *Header;
  UINT32        CurBlock;
  UINT32        Index;
  UINT32        Len;
  UINT32        Copied;
 
  ASSERT ((ExtFragment != NULL) && (ExtNum > 0) && (ExtFree != NULL));
 
  SavedFragment.Bulk = NULL;
  SavedFragment.Len  = 0;
 
  FirstBlockLen = 0;
  FirstBlock    = NULL;
  BlockNum      = ExtNum;
  Index         = 0;
  TotalLen      = 0;
  SavedIndex    = 0;
  Len           = 0;
  Copied        = 0;
 
  //
  // No need to consolidate the header if the first block is
  // longer than the header length or there is only one block.
  //
  if ((ExtFragment[0].Len >= HeadLen) || (ExtNum == 1)) {
    HeadLen = 0;
  }
 
  //
  // Allocate an extra block if we need to:
  //  1. Allocate some header space
  //  2. aggreate the packet header
  //
  if ((HeadSpace != 0) || (HeadLen != 0)) {
    FirstBlockLen = HeadLen + HeadSpace;
    FirstBlock    = AllocatePool (FirstBlockLen);
 
    if (FirstBlock == NULL) {
      return NULL;
    }
 
    BlockNum++;
  }
 
  //
  // Copy the header to the first block, reduce the NET_BLOCK
  // to allocate by one for each block that is completely covered
  // by the first bulk.
  //
  if (HeadLen != 0) {
    Len    = HeadLen;
    Header = FirstBlock + HeadSpace;
 
    for (Index = 0; Index < ExtNum; Index++) {
      if (Len >= ExtFragment[Index].Len) {
        CopyMem (Header, ExtFragment[Index].Bulk, ExtFragment[Index].Len);
 
        Copied   += ExtFragment[Index].Len;
        Len      -= ExtFragment[Index].Len;
        Header   += ExtFragment[Index].Len;
        TotalLen += ExtFragment[Index].Len;
        BlockNum--;
 
        if (Len == 0) {
          //
          // Increment the index number to point to the next
          // non-empty fragment.
          //
          Index++;
          break;
        }
      } else {
        CopyMem (Header, ExtFragment[Index].Bulk, Len);
 
        Copied   += Len;
        TotalLen += Len;
 
        //
        // Adjust the block structure to exclude the data copied,
        // So, the left-over block can be processed as other blocks.
        // But it must be recovered later. (SavedIndex > 0) always
        // holds since we don't aggreate the header if the first block
        // is bigger enough that the header is continuous
        //
        SavedIndex               = Index;
        SavedFragment            = ExtFragment[Index];
        ExtFragment[Index].Bulk += Len;
        ExtFragment[Index].Len  -= Len;
        break;
      }
    }
  }
 
  Nbuf = NetbufAllocStruct (BlockNum, BlockNum);
 
  if (Nbuf == NULL) {
    goto FreeFirstBlock;
  }
 
  Vector       = Nbuf->Vector;
  Vector->Free = ExtFree;
  Vector->Arg  = Arg;
  Vector->Flag = ((FirstBlockLen != 0) ? NET_VECTOR_OWN_FIRST : 0);
 
  //
  // Set the first block up which may contain
  // some head space and aggregated header
  //
  CurBlock = 0;
 
  if (FirstBlockLen != 0) {
    NetbufSetBlock (Nbuf, FirstBlock, HeadSpace + Copied, 0);
    Nbuf->BlockOp[0].Head += HeadSpace;
    Nbuf->BlockOp[0].Size  =  Copied;
 
    CurBlock++;
  }
 
  for ( ; Index < ExtNum; Index++) {
    NetbufSetBlock (Nbuf, ExtFragment[Index].Bulk, ExtFragment[Index].Len, CurBlock);
    TotalLen += ExtFragment[Index].Len;
    CurBlock++;
  }
 
  Vector->Len     = TotalLen + HeadSpace;
  Nbuf->TotalSize = TotalLen;
 
  if (SavedIndex != 0) {
    ExtFragment[SavedIndex] = SavedFragment;
  }
 
  return Nbuf;
 
FreeFirstBlock:
  if (FirstBlock != NULL) {
    FreePool (FirstBlock);
  }
 
  return NULL;
}
 
EFI_STATUS
EFIAPI
NetbufBuildExt (
  IN NET_BUF           *Nbuf,
  IN OUT NET_FRAGMENT  *ExtFragment,
  IN OUT UINT32        *ExtNum
  )
{
  UINT32  Index;
  UINT32  Current;
 
  Current = 0;
 
  for (Index = 0; (Index < Nbuf->BlockOpNum); Index++) {
    if (Nbuf->BlockOp[Index].Size == 0) {
      continue;
    }
 
    if (Current < *ExtNum) {
      ExtFragment[Current].Bulk = Nbuf->BlockOp[Index].Head;
      ExtFragment[Current].Len  = Nbuf->BlockOp[Index].Size;
      Current++;
    } else {
      return EFI_BUFFER_TOO_SMALL;
    }
  }
 
  *ExtNum = Current;
  return EFI_SUCCESS;
}
 
NET_BUF  *
EFIAPI
NetbufFromBufList (
  IN LIST_ENTRY           *BufList,
  IN UINT32               HeadSpace,
  IN UINT32               HeaderLen,
  IN NET_VECTOR_EXT_FREE  ExtFree,
  IN VOID                 *Arg              OPTIONAL
  )
{
  NET_FRAGMENT  *Fragment;
  UINT32        FragmentNum;
  LIST_ENTRY    *Entry;
  NET_BUF       *Nbuf;
  UINT32        Index;
  UINT32        Current;
 
  //
  // Compute how many blocks are there
  //
  FragmentNum = 0;
 
  NET_LIST_FOR_EACH (Entry, BufList) {
    Nbuf = NET_LIST_USER_STRUCT (Entry, NET_BUF, List);
    NET_CHECK_SIGNATURE (Nbuf, NET_BUF_SIGNATURE);
    FragmentNum += Nbuf->BlockOpNum;
  }
 
  //
  // Allocate and copy block points
  //
  Fragment = AllocatePool (sizeof (NET_FRAGMENT) * FragmentNum);
 
  if (Fragment == NULL) {
    return NULL;
  }
 
  Current = 0;
 
  NET_LIST_FOR_EACH (Entry, BufList) {
    Nbuf = NET_LIST_USER_STRUCT (Entry, NET_BUF, List);
    NET_CHECK_SIGNATURE (Nbuf, NET_BUF_SIGNATURE);
 
    for (Index = 0; Index < Nbuf->BlockOpNum; Index++) {
      if (Nbuf->BlockOp[Index].Size != 0) {
        Fragment[Current].Bulk = Nbuf->BlockOp[Index].Head;
        Fragment[Current].Len  = Nbuf->BlockOp[Index].Size;
        Current++;
      }
    }
  }
 
  Nbuf = NetbufFromExt (Fragment, Current, HeadSpace, HeaderLen, ExtFree, Arg);
  FreePool (Fragment);
 
  return Nbuf;
}
 
VOID
EFIAPI
NetbufReserve (
  IN OUT NET_BUF  *Nbuf,
  IN UINT32       Len
  )
{
  NET_CHECK_SIGNATURE (Nbuf, NET_BUF_SIGNATURE);
  NET_CHECK_SIGNATURE (Nbuf->Vector, NET_VECTOR_SIGNATURE);
 
  ASSERT ((Nbuf->BlockOpNum == 1) && (Nbuf->TotalSize == 0));
  ASSERT ((Nbuf->Vector->Free == NULL) && (Nbuf->Vector->Len >= Len));
 
  Nbuf->BlockOp[0].Head += Len;
  Nbuf->BlockOp[0].Tail += Len;
 
  ASSERT (Nbuf->BlockOp[0].Tail <= Nbuf->BlockOp[0].BlockTail);
}
 
UINT8 *
EFIAPI
NetbufAllocSpace (
  IN OUT NET_BUF  *Nbuf,
  IN UINT32       Len,
  IN BOOLEAN      FromHead
  )
{
  NET_BLOCK_OP  *BlockOp;
  UINT32        Index;
  UINT8         *SavedTail;
 
  Index = 0;
 
  NET_CHECK_SIGNATURE (Nbuf, NET_BUF_SIGNATURE);
  NET_CHECK_SIGNATURE (Nbuf->Vector, NET_VECTOR_SIGNATURE);
 
  ASSERT (Len > 0);
 
  if (FromHead) {
    //
    // Allocate some space from head. If the buffer is empty,
    // allocate from the first block. If it isn't, allocate
    // from the first non-empty block, or the block before that.
    //
    if (Nbuf->TotalSize == 0) {
      Index = 0;
    } else {
      NetbufGetByte (Nbuf, 0, &Index);
 
      if ((Index != 0) && (NET_HEADSPACE (&(Nbuf->BlockOp[Index])) < Len)) {
        Index--;
      }
    }
 
    BlockOp = &(Nbuf->BlockOp[Index]);
 
    if (NET_HEADSPACE (BlockOp) < Len) {
      return NULL;
    }
 
    BlockOp->Head   -= Len;
    BlockOp->Size   += Len;
    Nbuf->TotalSize += Len;
 
    return BlockOp->Head;
  } else {
    //
    // Allocate some space from the tail. If the buffer is empty,
    // allocate from the first block. If it isn't, allocate
    // from the last non-empty block, or the block after that.
    //
    if (Nbuf->TotalSize == 0) {
      Index = 0;
    } else {
      NetbufGetByte (Nbuf, Nbuf->TotalSize - 1, &Index);
 
      if ((NET_TAILSPACE (&(Nbuf->BlockOp[Index])) < Len) &&
          (Index < Nbuf->BlockOpNum - 1))
      {
        Index++;
      }
    }
 
    BlockOp = &(Nbuf->BlockOp[Index]);
 
    if (NET_TAILSPACE (BlockOp) < Len) {
      return NULL;
    }
 
    SavedTail = BlockOp->Tail;
 
    BlockOp->Tail   += Len;
    BlockOp->Size   += Len;
    Nbuf->TotalSize += Len;
 
    return SavedTail;
  }
}
 
VOID
NetblockTrim (
  IN OUT NET_BLOCK_OP  *BlockOp,
  IN UINT32            Len,
  IN BOOLEAN           FromHead
  )
{
  ASSERT ((BlockOp != NULL) && (BlockOp->Size >= Len));
 
  BlockOp->Size -= Len;
 
  if (FromHead) {
    BlockOp->Head += Len;
  } else {
    BlockOp->Tail -= Len;
  }
}
 
UINT32
EFIAPI
NetbufTrim (
  IN OUT NET_BUF  *Nbuf,
  IN UINT32       Len,
  IN BOOLEAN      FromHead
  )
{
  NET_BLOCK_OP  *BlockOp;
  UINT32        Index;
  UINT32        Trimmed;
 
  NET_CHECK_SIGNATURE (Nbuf, NET_BUF_SIGNATURE);
 
  if ((Len == 0) || (Nbuf->TotalSize == 0)) {
    return 0;
  }
 
  if (Len > Nbuf->TotalSize) {
    Len = Nbuf->TotalSize;
  }
 
  //
  // If FromTail is true, iterate backward. That
  // is, init Index to NBuf->BlockNum - 1, and
  // decrease it by 1 during each loop. Otherwise,
  // iterate forward. That is, init Index to 0, and
  // increase it by 1 during each loop.
  //
  Trimmed          = 0;
  Nbuf->TotalSize -= Len;
 
  Index   = (FromHead ? 0 : Nbuf->BlockOpNum - 1);
  BlockOp = Nbuf->BlockOp;
 
  for ( ; ;) {
    if (BlockOp[Index].Size == 0) {
      Index += (FromHead ? 1 : -1);
      continue;
    }
 
    if (Len > BlockOp[Index].Size) {
      Len     -= BlockOp[Index].Size;
      Trimmed += BlockOp[Index].Size;
      NetblockTrim (&BlockOp[Index], BlockOp[Index].Size, FromHead);
    } else {
      Trimmed += Len;
      NetblockTrim (&BlockOp[Index], Len, FromHead);
      break;
    }
 
    Index += (FromHead ? 1 : -1);
  }
 
  return Trimmed;
}
 
UINT32
EFIAPI
NetbufCopy (
  IN NET_BUF  *Nbuf,
  IN UINT32   Offset,
  IN UINT32   Len,
  IN UINT8    *Dest
  )
{
  NET_BLOCK_OP  *BlockOp;
  UINT32        Skip;
  UINT32        Left;
  UINT32        Copied;
  UINT32        Index;
  UINT32        Cur;
 
  NET_CHECK_SIGNATURE (Nbuf, NET_BUF_SIGNATURE);
  ASSERT (Dest);
 
  if ((Len == 0) || (Nbuf->TotalSize <= Offset)) {
    return 0;
  }
 
  if (Nbuf->TotalSize - Offset < Len) {
    Len = Nbuf->TotalSize - Offset;
  }
 
  BlockOp = Nbuf->BlockOp;
 
  //
  // Skip to the offset. Don't make "Offset-By-One" error here.
  // Cur + BLOCK.SIZE is the first sequence number of next block.
  // So, (Offset < Cur + BLOCK.SIZE) means that the  first byte
  // is in the current block. if (Offset == Cur + BLOCK.SIZE), the
  // first byte is the next block's first byte.
  //
  Cur = 0;
 
  for (Index = 0; Index < Nbuf->BlockOpNum; Index++) {
    if (BlockOp[Index].Size == 0) {
      continue;
    }
 
    if (Offset < Cur + BlockOp[Index].Size) {
      break;
    }
 
    Cur += BlockOp[Index].Size;
  }
 
  //
  // Cur is the sequence number of the first byte in the block
  // Offset - Cur is the number of bytes before first byte to
  // to copy in the current block.
  //
  Skip = Offset - Cur;
  Left = BlockOp[Index].Size - Skip;
 
  if (Len <= Left) {
    CopyMem (Dest, BlockOp[Index].Head + Skip, Len);
    return Len;
  }
 
  CopyMem (Dest, BlockOp[Index].Head + Skip, Left);
 
  Dest  += Left;
  Len   -= Left;
  Copied = Left;
 
  Index++;
 
  for ( ; Index < Nbuf->BlockOpNum; Index++) {
    if (Len > BlockOp[Index].Size) {
      Len    -= BlockOp[Index].Size;
      Copied += BlockOp[Index].Size;
 
      CopyMem (Dest, BlockOp[Index].Head, BlockOp[Index].Size);
      Dest += BlockOp[Index].Size;
    } else {
      Copied += Len;
      CopyMem (Dest, BlockOp[Index].Head, Len);
      break;
    }
  }
 
  return Copied;
}
 
VOID
EFIAPI
NetbufQueInit (
  IN OUT NET_BUF_QUEUE  *NbufQue
  )
{
  NbufQue->Signature = NET_QUE_SIGNATURE;
  NbufQue->RefCnt    = 1;
  InitializeListHead (&NbufQue->List);
 
  InitializeListHead (&NbufQue->BufList);
  NbufQue->BufSize = 0;
  NbufQue->BufNum  = 0;
}
 
NET_BUF_QUEUE  *
EFIAPI
NetbufQueAlloc (
  VOID
  )
{
  NET_BUF_QUEUE  *NbufQue;
 
  NbufQue = AllocatePool (sizeof (NET_BUF_QUEUE));
  if (NbufQue == NULL) {
    return NULL;
  }
 
  NetbufQueInit (NbufQue);
 
  return NbufQue;
}
 
VOID
EFIAPI
NetbufQueFree (
  IN NET_BUF_QUEUE  *NbufQue
  )
{
  ASSERT (NbufQue != NULL);
  NET_CHECK_SIGNATURE (NbufQue, NET_QUE_SIGNATURE);
 
  NbufQue->RefCnt--;
 
  if (NbufQue->RefCnt == 0) {
    NetbufQueFlush (NbufQue);
    FreePool (NbufQue);
  }
}
 
VOID
EFIAPI
NetbufQueAppend (
  IN OUT NET_BUF_QUEUE  *NbufQue,
  IN OUT NET_BUF        *Nbuf
  )
{
  NET_CHECK_SIGNATURE (NbufQue, NET_QUE_SIGNATURE);
  NET_CHECK_SIGNATURE (Nbuf, NET_BUF_SIGNATURE);
 
  InsertTailList (&NbufQue->BufList, &Nbuf->List);
 
  NbufQue->BufSize += Nbuf->TotalSize;
  NbufQue->BufNum++;
}
 
NET_BUF  *
EFIAPI
NetbufQueRemove (
  IN OUT NET_BUF_QUEUE  *NbufQue
  )
{
  NET_BUF  *First;
 
  NET_CHECK_SIGNATURE (NbufQue, NET_QUE_SIGNATURE);
 
  if (NbufQue->BufNum == 0) {
    return NULL;
  }
 
  First = NET_LIST_USER_STRUCT (NbufQue->BufList.ForwardLink, NET_BUF, List);
 
  NetListRemoveHead (&NbufQue->BufList);
 
  NbufQue->BufSize -= First->TotalSize;
  NbufQue->BufNum--;
  return First;
}
 
UINT32
EFIAPI
NetbufQueCopy (
  IN NET_BUF_QUEUE  *NbufQue,
  IN UINT32         Offset,
  IN UINT32         Len,
  OUT UINT8         *Dest
  )
{
  LIST_ENTRY  *Entry;
  NET_BUF     *Nbuf;
  UINT32      Skip;
  UINT32      Left;
  UINT32      Cur;
  UINT32      Copied;
 
  NET_CHECK_SIGNATURE (NbufQue, NET_QUE_SIGNATURE);
  ASSERT (Dest != NULL);
 
  if ((Len == 0) || (NbufQue->BufSize <= Offset)) {
    return 0;
  }
 
  if (NbufQue->BufSize - Offset < Len) {
    Len = NbufQue->BufSize - Offset;
  }
 
  //
  // skip to the Offset
  //
  Cur  = 0;
  Nbuf = NULL;
 
  NET_LIST_FOR_EACH (Entry, &NbufQue->BufList) {
    Nbuf = NET_LIST_USER_STRUCT (Entry, NET_BUF, List);
 
    if (Offset < Cur + Nbuf->TotalSize) {
      break;
    }
 
    Cur += Nbuf->TotalSize;
  }
 
  ASSERT (Nbuf != NULL);
 
  //
  // Copy the data in the first buffer.
  //
  Skip = Offset - Cur;
  Left = Nbuf->TotalSize - Skip;
 
  if (Len < Left) {
    return NetbufCopy (Nbuf, Skip, Len, Dest);
  }
 
  NetbufCopy (Nbuf, Skip, Left, Dest);
  Dest  += Left;
  Len   -= Left;
  Copied = Left;
 
  //
  // Iterate over the others
  //
  Entry = Entry->ForwardLink;
 
  while ((Len > 0) && (Entry != &NbufQue->BufList)) {
    Nbuf = NET_LIST_USER_STRUCT (Entry, NET_BUF, List);
 
    if (Len > Nbuf->TotalSize) {
      Len    -= Nbuf->TotalSize;
      Copied += Nbuf->TotalSize;
 
      NetbufCopy (Nbuf, 0, Nbuf->TotalSize, Dest);
      Dest += Nbuf->TotalSize;
    } else {
      NetbufCopy (Nbuf, 0, Len, Dest);
      Copied += Len;
      break;
    }
 
    Entry = Entry->ForwardLink;
  }
 
  return Copied;
}
 
UINT32
EFIAPI
NetbufQueTrim (
  IN OUT NET_BUF_QUEUE  *NbufQue,
  IN UINT32             Len
  )
{
  LIST_ENTRY  *Entry;
  LIST_ENTRY  *Next;
  NET_BUF     *Nbuf;
  UINT32      Trimmed;
 
  NET_CHECK_SIGNATURE (NbufQue, NET_QUE_SIGNATURE);
 
  if (Len == 0) {
    return 0;
  }
 
  if (Len > NbufQue->BufSize) {
    Len = NbufQue->BufSize;
  }
 
  NbufQue->BufSize -= Len;
  Trimmed           = 0;
 
  NET_LIST_FOR_EACH_SAFE (Entry, Next, &NbufQue->BufList) {
    Nbuf = NET_LIST_USER_STRUCT (Entry, NET_BUF, List);
 
    if (Len >= Nbuf->TotalSize) {
      Trimmed += Nbuf->TotalSize;
      Len     -= Nbuf->TotalSize;
 
      RemoveEntryList (Entry);
      NetbufFree (Nbuf);
 
      NbufQue->BufNum--;
 
      if (Len == 0) {
        break;
      }
    } else {
      Trimmed += NetbufTrim (Nbuf, Len, NET_BUF_HEAD);
      break;
    }
  }
 
  return Trimmed;
}
 
VOID
EFIAPI
NetbufQueFlush (
  IN OUT NET_BUF_QUEUE  *NbufQue
  )
{
  NET_CHECK_SIGNATURE (NbufQue, NET_QUE_SIGNATURE);
 
  NetbufFreeList (&NbufQue->BufList);
 
  NbufQue->BufNum  = 0;
  NbufQue->BufSize = 0;
}
 
UINT16
EFIAPI
NetblockChecksum (
  IN UINT8   *Bulk,
  IN UINT32  Len
  )
{
  register UINT32  Sum;
 
  Sum = 0;
 
  //
  // Add left-over byte, if any
  //
  if (Len % 2 != 0) {
    Sum += *(Bulk + Len - 1);
  }
 
  while (Len > 1) {
    Sum  += *(UINT16 *)Bulk;
    Bulk += 2;
    Len  -= 2;
  }
 
  //
  // Fold 32-bit sum to 16 bits
  //
  while ((Sum >> 16) != 0) {
    Sum = (Sum & 0xffff) + (Sum >> 16);
  }
 
  return (UINT16)Sum;
}
 
UINT16
EFIAPI
NetAddChecksum (
  IN UINT16  Checksum1,
  IN UINT16  Checksum2
  )
{
  UINT32  Sum;
 
  Sum = Checksum1 + Checksum2;
 
  //
  // two UINT16 can only add up to a carry of 1.
  //
  if ((Sum >> 16) != 0) {
    Sum = (Sum & 0xffff) + 1;
  }
 
  return (UINT16)Sum;
}
 
UINT16
EFIAPI
NetbufChecksum (
  IN NET_BUF  *Nbuf
  )
{
  NET_BLOCK_OP  *BlockOp;
  UINT32        Offset;
  UINT16        TotalSum;
  UINT16        BlockSum;
  UINT32        Index;
 
  NET_CHECK_SIGNATURE (Nbuf, NET_BUF_SIGNATURE);
 
  TotalSum = 0;
  Offset   = 0;
  BlockOp  = Nbuf->BlockOp;
 
  for (Index = 0; Index < Nbuf->BlockOpNum; Index++) {
    if (BlockOp[Index].Size == 0) {
      continue;
    }
 
    BlockSum = NetblockChecksum (BlockOp[Index].Head, BlockOp[Index].Size);
 
    if ((Offset & 0x01) != 0) {
      //
      // The checksum starts with an odd byte, swap
      // the checksum before added to total checksum
      //
      BlockSum = SwapBytes16 (BlockSum);
    }
 
    TotalSum = NetAddChecksum (BlockSum, TotalSum);
    Offset  += BlockOp[Index].Size;
  }
 
  return TotalSum;
}
 
UINT16
EFIAPI
NetPseudoHeadChecksum (
  IN IP4_ADDR  Src,
  IN IP4_ADDR  Dst,
  IN UINT8     Proto,
  IN UINT16    Len
  )
{
  NET_PSEUDO_HDR  Hdr;
 
  //
  // Zero the memory to relieve align problems
  //
  ZeroMem (&Hdr, sizeof (Hdr));
 
  Hdr.SrcIp    = Src;
  Hdr.DstIp    = Dst;
  Hdr.Protocol = Proto;
  Hdr.Len      = HTONS (Len);
 
  return NetblockChecksum ((UINT8 *)&Hdr, sizeof (Hdr));
}
 
UINT16
EFIAPI
NetIp6PseudoHeadChecksum (
  IN EFI_IPv6_ADDRESS  *Src,
  IN EFI_IPv6_ADDRESS  *Dst,
  IN UINT8             NextHeader,
  IN UINT32            Len
  )
{
  NET_IP6_PSEUDO_HDR  Hdr;
 
  //
  // Zero the memory to relieve align problems
  //
  ZeroMem (&Hdr, sizeof (Hdr));
 
  IP6_COPY_ADDRESS (&Hdr.SrcIp, Src);
  IP6_COPY_ADDRESS (&Hdr.DstIp, Dst);
 
  Hdr.NextHeader = NextHeader;
  Hdr.Len        = HTONL (Len);
 
  return NetblockChecksum ((UINT8 *)&Hdr, sizeof (Hdr));
}
 
VOID
NetIpSecNetbufFree (
  NET_BUF  *Nbuf
  )
{
  NET_CHECK_SIGNATURE (Nbuf, NET_BUF_SIGNATURE);
  ASSERT (Nbuf->RefCnt > 0);
 
  Nbuf->RefCnt--;
 
  if (Nbuf->RefCnt == 0) {
    //
    // Update Vector only when NBuf is to be released. That is,
    // all the sharing of Nbuf increse Vector's RefCnt by one
    //
    NET_CHECK_SIGNATURE (Nbuf->Vector, NET_VECTOR_SIGNATURE);
    ASSERT (Nbuf->Vector->RefCnt > 0);
 
    Nbuf->Vector->RefCnt--;
 
    if (Nbuf->Vector->RefCnt > 0) {
      return;
    }
 
    //
    // If NET_VECTOR_OWN_FIRST is set, release the first block since it is
    // allocated by us
    //
    if ((Nbuf->Vector->Flag & NET_VECTOR_OWN_FIRST) != 0) {
      FreePool (Nbuf->Vector->Block[0].Bulk);
    }
 
    FreePool (Nbuf->Vector);
    FreePool (Nbuf);
  }
}