/** @file
  Implementation of Managed Network Protocol private services.

Copyright (c) 2005 - 2016, Intel Corporation. All rights reserved.<BR>
This program and the accompanying materials
are licensed and made available under the terms and conditions
of the BSD License which accompanies this distribution.  The full
text of the license may be found at<BR>
http://opensource.org/licenses/bsd-license.php

THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.

**/

#include "MnpImpl.h"
#include "MnpVlan.h"

EFI_SERVICE_BINDING_PROTOCOL    mMnpServiceBindingProtocol = {
  MnpServiceBindingCreateChild,
  MnpServiceBindingDestroyChild
};

EFI_MANAGED_NETWORK_PROTOCOL    mMnpProtocolTemplate = {
  MnpGetModeData,
  MnpConfigure,
  MnpMcastIpToMac,
  MnpGroups,
  MnpTransmit,
  MnpReceive,
  MnpCancel,
  MnpPoll
};

EFI_MANAGED_NETWORK_CONFIG_DATA mMnpDefaultConfigData = {
  10000000,
  10000000,
  0,
  FALSE,
  FALSE,
  FALSE,
  FALSE,
  FALSE,
  FALSE,
  FALSE
};

/**
  Add Count of net buffers to MnpDeviceData->FreeNbufQue. The length of the net
  buffer is specified by MnpDeviceData->BufferLength.

  @param[in, out]  MnpDeviceData         Pointer to the MNP_DEVICE_DATA.
  @param[in]       Count                 Number of NET_BUFFERs to add.

  @retval EFI_SUCCESS           The specified amount of NET_BUFs are allocated
                                and added to MnpDeviceData->FreeNbufQue.
  @retval EFI_OUT_OF_RESOURCES  Failed to allocate a NET_BUF structure.

**/
EFI_STATUS
MnpAddFreeNbuf (
  IN OUT MNP_DEVICE_DATA   *MnpDeviceData,
  IN     UINTN             Count
  )
{
  EFI_STATUS  Status;
  UINTN       Index;
  NET_BUF     *Nbuf;

  NET_CHECK_SIGNATURE (MnpDeviceData, MNP_DEVICE_DATA_SIGNATURE);
  ASSERT ((Count > 0) && (MnpDeviceData->BufferLength > 0));

  Status = EFI_SUCCESS;
  for (Index = 0; Index < Count; Index++) {
    Nbuf = NetbufAlloc (MnpDeviceData->BufferLength + MnpDeviceData->PaddingSize);
    if (Nbuf == NULL) {
      DEBUG ((EFI_D_ERROR, "MnpAddFreeNbuf: NetBufAlloc failed.\n"));

      Status = EFI_OUT_OF_RESOURCES;
      break;
    }

    if (MnpDeviceData->PaddingSize > 0) {
      //
      // Pad padding bytes before the media header
      //
      NetbufAllocSpace (Nbuf, MnpDeviceData->PaddingSize, NET_BUF_TAIL);
      NetbufTrim (Nbuf, MnpDeviceData->PaddingSize, NET_BUF_HEAD);
    }

    NetbufQueAppend (&MnpDeviceData->FreeNbufQue, Nbuf);
  }

  MnpDeviceData->NbufCnt += Index;
  return Status;
}


/**
  Allocate a free NET_BUF from MnpDeviceData->FreeNbufQue. If there is none
  in the queue, first try to allocate some and add them into the queue, then
  fetch the NET_BUF from the updated FreeNbufQue.

  @param[in, out]  MnpDeviceData        Pointer to the MNP_DEVICE_DATA.

  @return     Pointer to the allocated free NET_BUF structure, if NULL the
              operation is failed.

**/
NET_BUF *
MnpAllocNbuf (
  IN OUT MNP_DEVICE_DATA   *MnpDeviceData
  )
{
  EFI_STATUS    Status;
  NET_BUF_QUEUE *FreeNbufQue;
  NET_BUF       *Nbuf;
  EFI_TPL       OldTpl;

  NET_CHECK_SIGNATURE (MnpDeviceData, MNP_DEVICE_DATA_SIGNATURE);

  FreeNbufQue = &MnpDeviceData->FreeNbufQue;
  OldTpl      = gBS->RaiseTPL (TPL_NOTIFY);

  //
  // Check whether there are available buffers, or else try to add some.
  //
  if (FreeNbufQue->BufNum == 0) {
    if ((MnpDeviceData->NbufCnt + MNP_NET_BUFFER_INCREASEMENT) > MNP_MAX_NET_BUFFER_NUM) {
      DEBUG (
        (EFI_D_ERROR,
        "MnpAllocNbuf: The maximum NET_BUF size is reached for MNP driver instance %p.\n",
        MnpDeviceData)
        );

      Nbuf = NULL;
      goto ON_EXIT;
    }

    Status = MnpAddFreeNbuf (MnpDeviceData, MNP_NET_BUFFER_INCREASEMENT);
    if (EFI_ERROR (Status)) {
      DEBUG (
        (EFI_D_ERROR,
        "MnpAllocNbuf: Failed to add NET_BUFs into the FreeNbufQue, %r.\n",
        Status)
        );

      //
      // Don't return NULL, perhaps MnpAddFreeNbuf does add some NET_BUFs but
      // the amount is less than MNP_NET_BUFFER_INCREASEMENT.
      //
    }
  }

  Nbuf = NetbufQueRemove (FreeNbufQue);

  //
  // Increase the RefCnt.
  //
  if (Nbuf != NULL) {
    NET_GET_REF (Nbuf);
  }

ON_EXIT:
  gBS->RestoreTPL (OldTpl);

  return Nbuf;
}


/**
  Try to reclaim the Nbuf into the buffer pool.

  @param[in, out]  MnpDeviceData         Pointer to the mnp device context data.
  @param[in, out]  Nbuf                  Pointer to the NET_BUF to free.

**/
VOID
MnpFreeNbuf (
  IN OUT MNP_DEVICE_DATA   *MnpDeviceData,
  IN OUT NET_BUF           *Nbuf
  )
{
  EFI_TPL  OldTpl;

  NET_CHECK_SIGNATURE (MnpDeviceData, MNP_DEVICE_DATA_SIGNATURE);
  ASSERT (Nbuf->RefCnt > 1);

  OldTpl = gBS->RaiseTPL (TPL_NOTIFY);

  NET_PUT_REF (Nbuf);

  if (Nbuf->RefCnt == 1) {
    //
    // Trim all buffer contained in the Nbuf, then append it to the NbufQue.
    //
    NetbufTrim (Nbuf, Nbuf->TotalSize, NET_BUF_TAIL);

    if (NetbufAllocSpace (Nbuf, NET_VLAN_TAG_LEN, NET_BUF_HEAD) != NULL) {
      //
      // There is space reserved for vlan tag in the head, reclaim it
      //
      NetbufTrim (Nbuf, NET_VLAN_TAG_LEN, NET_BUF_TAIL);
    }

    NetbufQueAppend (&MnpDeviceData->FreeNbufQue, Nbuf);
  }

  gBS->RestoreTPL (OldTpl);
}

/**
  Add Count of TX buffers to MnpDeviceData->AllTxBufList and MnpDeviceData->FreeTxBufList.
  The length of the buffer is specified by MnpDeviceData->BufferLength.

  @param[in, out]  MnpDeviceData         Pointer to the MNP_DEVICE_DATA.
  @param[in]       Count                 Number of TX buffers to add.

  @retval EFI_SUCCESS           The specified amount of TX buffers are allocated.
  @retval EFI_OUT_OF_RESOURCES  Failed to allocate a TX buffer.

**/
EFI_STATUS
MnpAddFreeTxBuf (
  IN OUT MNP_DEVICE_DATA   *MnpDeviceData,
  IN     UINTN             Count
  )
{
  EFI_STATUS        Status;
  UINT32            Index;
  MNP_TX_BUF_WRAP   *TxBufWrap;

  NET_CHECK_SIGNATURE (MnpDeviceData, MNP_DEVICE_DATA_SIGNATURE);
  ASSERT ((Count > 0) && (MnpDeviceData->BufferLength > 0));

  Status = EFI_SUCCESS;
  for (Index = 0; Index < Count; Index++) {
    TxBufWrap = (MNP_TX_BUF_WRAP*) AllocatePool (sizeof (MNP_TX_BUF_WRAP) + MnpDeviceData->BufferLength - 1);
    if (TxBufWrap == NULL) {
      DEBUG ((EFI_D_ERROR, "MnpAddFreeTxBuf: TxBuf Alloc failed.\n"));

      Status = EFI_OUT_OF_RESOURCES;
      break;
    }
    DEBUG ((EFI_D_INFO, "MnpAddFreeTxBuf: Add TxBufWrap %p, TxBuf %p\n", TxBufWrap, TxBufWrap->TxBuf));
    TxBufWrap->Signature = MNP_TX_BUF_WRAP_SIGNATURE;
    TxBufWrap->InUse     = FALSE;
    InsertTailList (&MnpDeviceData->FreeTxBufList, &TxBufWrap->WrapEntry);
    InsertTailList (&MnpDeviceData->AllTxBufList, &TxBufWrap->AllEntry);
  }

  MnpDeviceData->TxBufCount += Index;
  return Status;
}

/**
  Allocate a free TX buffer from MnpDeviceData->FreeTxBufList. If there is none
  in the queue, first try to recycle some from SNP, then try to allocate some and add 
  them into the queue, then fetch the NET_BUF from the updated FreeTxBufList.

  @param[in, out]  MnpDeviceData        Pointer to the MNP_DEVICE_DATA.

  @return     Pointer to the allocated free NET_BUF structure, if NULL the
              operation is failed.

**/
UINT8 *
MnpAllocTxBuf (
  IN OUT MNP_DEVICE_DATA   *MnpDeviceData
  )
{
  EFI_TPL           OldTpl;
  UINT8             *TxBuf;
  EFI_STATUS        Status;
  LIST_ENTRY        *Entry;
  MNP_TX_BUF_WRAP   *TxBufWrap;
  
  NET_CHECK_SIGNATURE (MnpDeviceData, MNP_DEVICE_DATA_SIGNATURE);

  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);

  if (IsListEmpty (&MnpDeviceData->FreeTxBufList)) {
    //
    // First try to recycle some TX buffer from SNP
    //
    Status = MnpRecycleTxBuf (MnpDeviceData);
    if (EFI_ERROR (Status)) {
      TxBuf = NULL;
      goto ON_EXIT;
    }

    //
    // If still no free TX buffer, allocate more.
    //
    if (IsListEmpty (&MnpDeviceData->FreeTxBufList)) {
      if ((MnpDeviceData->TxBufCount + MNP_TX_BUFFER_INCREASEMENT) > MNP_MAX_TX_BUFFER_NUM) {
        DEBUG (
          (EFI_D_ERROR,
          "MnpAllocTxBuf: The maximum TxBuf size is reached for MNP driver instance %p.\n",
          MnpDeviceData)
          );

        TxBuf = NULL;
        goto ON_EXIT;
      }

      Status = MnpAddFreeTxBuf (MnpDeviceData, MNP_TX_BUFFER_INCREASEMENT);
      if (IsListEmpty (&MnpDeviceData->FreeTxBufList)) {
        DEBUG (
          (EFI_D_ERROR,
          "MnpAllocNbuf: Failed to add TxBuf into the FreeTxBufList, %r.\n",
          Status)
          );

        TxBuf = NULL;
        goto ON_EXIT;
      }
    }
  }

  ASSERT (!IsListEmpty (&MnpDeviceData->FreeTxBufList));
  Entry = MnpDeviceData->FreeTxBufList.ForwardLink;
  RemoveEntryList (MnpDeviceData->FreeTxBufList.ForwardLink);
  TxBufWrap = NET_LIST_USER_STRUCT_S (Entry, MNP_TX_BUF_WRAP, WrapEntry, MNP_TX_BUF_WRAP_SIGNATURE);
  TxBufWrap->InUse = TRUE;
  TxBuf = TxBufWrap->TxBuf;

ON_EXIT:
  gBS->RestoreTPL (OldTpl);

  return TxBuf;
}

/**
  Try to reclaim the TX buffer into the buffer pool.

  @param[in, out]  MnpDeviceData         Pointer to the mnp device context data.
  @param[in, out]  TxBuf                 Pointer to the TX buffer to free.

**/
VOID
MnpFreeTxBuf (
  IN OUT MNP_DEVICE_DATA   *MnpDeviceData,
  IN OUT UINT8             *TxBuf
  )
{
  MNP_TX_BUF_WRAP   *TxBufWrap;
  EFI_TPL           OldTpl;

  NET_CHECK_SIGNATURE (MnpDeviceData, MNP_DEVICE_DATA_SIGNATURE);

  if (TxBuf == NULL) {
    return;
  }

  TxBufWrap = NET_LIST_USER_STRUCT (TxBuf, MNP_TX_BUF_WRAP, TxBuf);
  if (TxBufWrap->Signature != MNP_TX_BUF_WRAP_SIGNATURE) {
    DEBUG (
      (EFI_D_ERROR,
      "MnpFreeTxBuf: Signature check failed in MnpFreeTxBuf.\n")
      );
    return;
  }

  if (!TxBufWrap->InUse) {
    DEBUG (
      (EFI_D_WARN,
      "MnpFreeTxBuf: Duplicated recycle report from SNP.\n")
      );
    return;
  }
  
  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
  InsertTailList (&MnpDeviceData->FreeTxBufList, &TxBufWrap->WrapEntry);
  TxBufWrap->InUse = FALSE;
  gBS->RestoreTPL (OldTpl);
}

/**
  Try to recycle all the transmitted buffer address from SNP.

  @param[in, out]  MnpDeviceData     Pointer to the mnp device context data.

  @retval EFI_SUCCESS             Successed to recyclethe transmitted buffer address.
  @retval Others                  Failed to recyclethe transmitted buffer address.

**/
EFI_STATUS
MnpRecycleTxBuf (
  IN OUT MNP_DEVICE_DATA   *MnpDeviceData
  )
{
  UINT8                         *TxBuf;
  EFI_SIMPLE_NETWORK_PROTOCOL   *Snp;
  EFI_STATUS                    Status;

  Snp = MnpDeviceData->Snp;
  ASSERT (Snp != NULL);

  do {
    TxBuf = NULL;
    Status = Snp->GetStatus (Snp, NULL, (VOID **) &TxBuf);
    if (EFI_ERROR (Status)) {
      return Status;
    }

    if (TxBuf != NULL) {
      MnpFreeTxBuf (MnpDeviceData, TxBuf);
    }
  } while (TxBuf != NULL);

  return EFI_SUCCESS;
}

/**
  Initialize the mnp device context data.

  @param[in, out]  MnpDeviceData      Pointer to the mnp device context data.
  @param[in]       ImageHandle        The driver image handle.
  @param[in]       ControllerHandle   Handle of device to bind driver to.

  @retval EFI_SUCCESS           The mnp service context is initialized.
  @retval EFI_UNSUPPORTED       ControllerHandle does not support Simple Network Protocol.
  @retval Others                Other errors as indicated.

**/
EFI_STATUS
MnpInitializeDeviceData (
  IN OUT MNP_DEVICE_DATA   *MnpDeviceData,
  IN     EFI_HANDLE        ImageHandle,
  IN     EFI_HANDLE        ControllerHandle
  )
{
  EFI_STATUS                  Status;
  EFI_SIMPLE_NETWORK_PROTOCOL *Snp;
  EFI_SIMPLE_NETWORK_MODE     *SnpMode;

  MnpDeviceData->Signature        = MNP_DEVICE_DATA_SIGNATURE;
  MnpDeviceData->ImageHandle      = ImageHandle;
  MnpDeviceData->ControllerHandle = ControllerHandle;

  //
  // Copy the MNP Protocol interfaces from the template.
  //
  CopyMem (&MnpDeviceData->VlanConfig, &mVlanConfigProtocolTemplate, sizeof (EFI_VLAN_CONFIG_PROTOCOL));

  //
  // Open the Simple Network protocol.
  //
  Status = gBS->OpenProtocol (
                  ControllerHandle,
                  &gEfiSimpleNetworkProtocolGuid,
                  (VOID **) &Snp,
                  ImageHandle,
                  ControllerHandle,
                  EFI_OPEN_PROTOCOL_BY_DRIVER
                  );
  if (EFI_ERROR (Status)) {
    return EFI_UNSUPPORTED;
  }

  //
  // Get MTU from Snp.
  //
  SnpMode            = Snp->Mode;
  MnpDeviceData->Snp = Snp;

  //
  // Initialize the lists.
  //
  InitializeListHead (&MnpDeviceData->ServiceList);
  InitializeListHead (&MnpDeviceData->GroupAddressList);

  //
  // Get the buffer length used to allocate NET_BUF to hold data received
  // from SNP. Do this before fill the FreeNetBufQue.
  //
  //
  MnpDeviceData->BufferLength = SnpMode->MediaHeaderSize + NET_VLAN_TAG_LEN + SnpMode->MaxPacketSize + NET_ETHER_FCS_SIZE;

  //
  // Make sure the protocol headers immediately following the media header
  // 4-byte aligned, and also preserve additional space for VLAN tag
  //
  MnpDeviceData->PaddingSize = ((4 - SnpMode->MediaHeaderSize) & 0x3) + NET_VLAN_TAG_LEN;

  //
  // Initialize MAC string which will be used as VLAN configuration variable name
  //
  Status = NetLibGetMacString (ControllerHandle, ImageHandle, &MnpDeviceData->MacString);
  if (EFI_ERROR (Status)) {
    goto ERROR;
  }

  //
  // Initialize the FreeNetBufQue and pre-allocate some NET_BUFs.
  //
  NetbufQueInit (&MnpDeviceData->FreeNbufQue);
  Status = MnpAddFreeNbuf (MnpDeviceData, MNP_INIT_NET_BUFFER_NUM);
  if (EFI_ERROR (Status)) {
    DEBUG ((EFI_D_ERROR, "MnpInitializeDeviceData: MnpAddFreeNbuf failed, %r.\n", Status));

    goto ERROR;
  }

  //
  // Get one NET_BUF from the FreeNbufQue for rx cache.
  //
  MnpDeviceData->RxNbufCache = MnpAllocNbuf (MnpDeviceData);
  NetbufAllocSpace (
    MnpDeviceData->RxNbufCache,
    MnpDeviceData->BufferLength,
    NET_BUF_TAIL
    );

  //
  // Allocate buffer pool for tx.
  //
  InitializeListHead (&MnpDeviceData->FreeTxBufList);
  InitializeListHead (&MnpDeviceData->AllTxBufList);
  MnpDeviceData->TxBufCount = 0;

  //
  // Create the system poll timer.
  //
  Status = gBS->CreateEvent (
                  EVT_NOTIFY_SIGNAL | EVT_TIMER,
                  TPL_CALLBACK,
                  MnpSystemPoll,
                  MnpDeviceData,
                  &MnpDeviceData->PollTimer
                  );
  if (EFI_ERROR (Status)) {
    DEBUG ((EFI_D_ERROR, "MnpInitializeDeviceData: CreateEvent for poll timer failed.\n"));

    goto ERROR;
  }

  //
  // Create the timer for packet timeout check.
  //
  Status = gBS->CreateEvent (
                  EVT_NOTIFY_SIGNAL | EVT_TIMER,
                  TPL_CALLBACK,
                  MnpCheckPacketTimeout,
                  MnpDeviceData,
                  &MnpDeviceData->TimeoutCheckTimer
                  );
  if (EFI_ERROR (Status)) {
    DEBUG ((EFI_D_ERROR, "MnpInitializeDeviceData: CreateEvent for packet timeout check failed.\n"));

    goto ERROR;
  }

  //
  // Create the timer for media detection.
  //
  Status = gBS->CreateEvent (
                  EVT_NOTIFY_SIGNAL | EVT_TIMER,
                  TPL_CALLBACK,
                  MnpCheckMediaStatus,
                  MnpDeviceData,
                  &MnpDeviceData->MediaDetectTimer
                  );
  if (EFI_ERROR (Status)) {
    DEBUG ((EFI_D_ERROR, "MnpInitializeDeviceData: CreateEvent for media detection failed.\n"));

    goto ERROR;
  }

ERROR:
  if (EFI_ERROR (Status)) {
    //
    // Free the dynamic allocated resources if necessary.
    //
    if (MnpDeviceData->MacString != NULL) {
      FreePool (MnpDeviceData->MacString);
    }

    if (MnpDeviceData->TimeoutCheckTimer != NULL) {
      gBS->CloseEvent (MnpDeviceData->TimeoutCheckTimer);
    }

    if (MnpDeviceData->MediaDetectTimer != NULL) {
      gBS->CloseEvent (MnpDeviceData->MediaDetectTimer);
    }

    if (MnpDeviceData->PollTimer != NULL) {
      gBS->CloseEvent (MnpDeviceData->PollTimer);
    }

    if (MnpDeviceData->RxNbufCache != NULL) {
      MnpFreeNbuf (MnpDeviceData, MnpDeviceData->RxNbufCache);
    }

    if (MnpDeviceData->FreeNbufQue.BufNum != 0) {
      NetbufQueFlush (&MnpDeviceData->FreeNbufQue);
    }

    //
    // Close the Simple Network Protocol.
    //
    gBS->CloseProtocol (
          ControllerHandle,
          &gEfiSimpleNetworkProtocolGuid,
          ImageHandle,
          ControllerHandle
          );
  }

  return Status;
}


/**
  Destroy the MNP device context data.

  @param[in, out]  MnpDeviceData      Pointer to the mnp device context data.
  @param[in]       ImageHandle        The driver image handle.

**/
VOID
MnpDestroyDeviceData (
  IN OUT MNP_DEVICE_DATA   *MnpDeviceData,
  IN     EFI_HANDLE        ImageHandle
  )
{
  LIST_ENTRY         *Entry;
  LIST_ENTRY         *NextEntry;
  MNP_TX_BUF_WRAP    *TxBufWrap;

  NET_CHECK_SIGNATURE (MnpDeviceData, MNP_DEVICE_DATA_SIGNATURE);

  //
  // Free Vlan Config variable name string
  //
  if (MnpDeviceData->MacString != NULL) {
    FreePool (MnpDeviceData->MacString);
  }

  //
  // The GroupAddressList must be empty.
  //
  ASSERT (IsListEmpty (&MnpDeviceData->GroupAddressList));

  //
  // Close the event.
  //
  gBS->CloseEvent (MnpDeviceData->TimeoutCheckTimer);
  gBS->CloseEvent (MnpDeviceData->MediaDetectTimer);
  gBS->CloseEvent (MnpDeviceData->PollTimer);

  //
  // Free the Tx buffer pool.
  //
  NET_LIST_FOR_EACH_SAFE(Entry, NextEntry, &MnpDeviceData->AllTxBufList) {
    TxBufWrap = NET_LIST_USER_STRUCT (Entry, MNP_TX_BUF_WRAP, AllEntry);
    RemoveEntryList (Entry);
    FreePool (TxBufWrap);
    MnpDeviceData->TxBufCount--;
  }
  ASSERT (IsListEmpty (&MnpDeviceData->AllTxBufList));
  ASSERT (MnpDeviceData->TxBufCount == 0);

  //
  // Free the RxNbufCache.
  //
  MnpFreeNbuf (MnpDeviceData, MnpDeviceData->RxNbufCache);

  //
  // Flush the FreeNbufQue.
  //
  MnpDeviceData->NbufCnt -= MnpDeviceData->FreeNbufQue.BufNum;
  NetbufQueFlush (&MnpDeviceData->FreeNbufQue);

  //
  // Close the Simple Network Protocol.
  //
  gBS->CloseProtocol (
         MnpDeviceData->ControllerHandle,
         &gEfiSimpleNetworkProtocolGuid,
         ImageHandle,
         MnpDeviceData->ControllerHandle
         );
}


/**
  Create mnp service context data.

  @param[in]       MnpDeviceData      Pointer to the mnp device context data.
  @param[in]       VlanId             The VLAN ID.
  @param[in]       Priority           The VLAN priority. If VlanId is 0,
                                      Priority is ignored.

  @return A pointer to MNP_SERVICE_DATA or NULL if failed to create MNP service context.

**/
MNP_SERVICE_DATA *
MnpCreateServiceData (
  IN MNP_DEVICE_DATA     *MnpDeviceData,
  IN UINT16              VlanId,
  IN UINT8                Priority OPTIONAL
  )
{
  EFI_HANDLE                MnpServiceHandle;
  MNP_SERVICE_DATA          *MnpServiceData;
  EFI_STATUS                Status;
  EFI_SIMPLE_NETWORK_MODE   *SnpMode;
  EFI_VLAN_CONFIG_PROTOCOL  *VlanConfig;

  //
  // Initialize the Mnp Service Data.
  //
  MnpServiceData = AllocateZeroPool (sizeof (MNP_SERVICE_DATA));
  if (MnpServiceData == NULL) {
    DEBUG ((EFI_D_ERROR, "MnpCreateServiceData: Faild to allocate memory for the new Mnp Service Data.\n"));

    return NULL;
  }

  //
  // Add to MNP service list
  //
  InsertTailList (&MnpDeviceData->ServiceList, &MnpServiceData->Link);

  MnpServiceData->Signature     = MNP_SERVICE_DATA_SIGNATURE;
  MnpServiceData->MnpDeviceData = MnpDeviceData;

  //
  // Copy the ServiceBinding structure.
  //
  CopyMem (&MnpServiceData->ServiceBinding, &mMnpServiceBindingProtocol, sizeof (EFI_SERVICE_BINDING_PROTOCOL));

  //
  // Initialize the lists.
  //
  InitializeListHead (&MnpServiceData->ChildrenList);

  SnpMode = MnpDeviceData->Snp->Mode;
  if (VlanId != 0) {
    //
    // Create VLAN child handle
    //
    MnpServiceHandle = MnpCreateVlanChild (
                         MnpDeviceData->ImageHandle,
                         MnpDeviceData->ControllerHandle,
                         VlanId,
                         &MnpServiceData->DevicePath
                         );
    if (MnpServiceHandle == NULL) {
      DEBUG ((EFI_D_ERROR, "MnpCreateServiceData: Faild to create child handle.\n"));

      return NULL;
    }

    //
    // Open VLAN Config Protocol by child
    //
    Status = gBS->OpenProtocol (
                    MnpDeviceData->ControllerHandle,
                    &gEfiVlanConfigProtocolGuid,
                    (VOID **) &VlanConfig,
                    MnpDeviceData->ImageHandle,
                    MnpServiceHandle,
                    EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
                    );
    if (EFI_ERROR (Status)) {
      goto Exit;
    }

    //
    // Reduce MTU for VLAN device
    //
    MnpServiceData->Mtu = SnpMode->MaxPacketSize - NET_VLAN_TAG_LEN;
  } else {
    //
    // VlanId set to 0 means rx/tx untagged frame
    //
    MnpServiceHandle    = MnpDeviceData->ControllerHandle;
    MnpServiceData->Mtu = SnpMode->MaxPacketSize;
  }

  MnpServiceData->ServiceHandle = MnpServiceHandle;
  MnpServiceData->VlanId        = VlanId;
  MnpServiceData->Priority      = Priority;

  //
  // Install the MNP Service Binding Protocol
  //
  Status = gBS->InstallMultipleProtocolInterfaces (
                  &MnpServiceHandle,
                  &gEfiManagedNetworkServiceBindingProtocolGuid,
                  &MnpServiceData->ServiceBinding,
                  NULL
                  );

Exit:
  if (EFI_ERROR (Status)) {
    MnpDestroyServiceData (MnpServiceData);
    MnpServiceData = NULL;
  }

  return MnpServiceData;
}

/**
  Destroy the MNP service context data.

  @param[in, out]  MnpServiceData    Pointer to the mnp service context data.

  @retval EFI_SUCCESS           The mnp service context is destroyed.
  @retval Others                Errors as indicated.

**/
EFI_STATUS
MnpDestroyServiceData (
  IN OUT MNP_SERVICE_DATA    *MnpServiceData
  )
{
  EFI_STATUS  Status;

  //
  // Uninstall the MNP Service Binding Protocol
  //
  Status = gBS->UninstallMultipleProtocolInterfaces (
                  MnpServiceData->ServiceHandle,
                  &gEfiManagedNetworkServiceBindingProtocolGuid,
                  &MnpServiceData->ServiceBinding,
                  NULL
                  );
  if (EFI_ERROR (Status)) {
    return Status;
  }

  if (MnpServiceData->VlanId != 0) {
    //
    // Close VlanConfig Protocol opened by VLAN child handle
    //
    Status = gBS->CloseProtocol (
                    MnpServiceData->MnpDeviceData->ControllerHandle,
                    &gEfiVlanConfigProtocolGuid,
                    MnpServiceData->MnpDeviceData->ImageHandle,
                    MnpServiceData->ServiceHandle
                    );
    if (EFI_ERROR (Status)) {
      return Status;
    }

    //
    // Uninstall Device Path Protocol to destroy the VLAN child handle
    //
    Status = gBS->UninstallMultipleProtocolInterfaces (
                    MnpServiceData->ServiceHandle,
                    &gEfiDevicePathProtocolGuid,
                    MnpServiceData->DevicePath,
                    NULL
                    );
    if (EFI_ERROR (Status)) {
      return Status;
    }

    if (MnpServiceData->DevicePath != NULL) {
      FreePool (MnpServiceData->DevicePath);
    }
  }

  //
  // Remove from MnpDeviceData service list
  //
  RemoveEntryList (&MnpServiceData->Link);

  FreePool (MnpServiceData);

  return Status;
}

/**
  Callback function which provided by user to remove one node in NetDestroyLinkList process.
  
  @param[in]    Entry           The entry to be removed.
  @param[in]    Context         Pointer to the callback context corresponds to the Context in NetDestroyLinkList.

  @retval EFI_SUCCESS           The entry has been removed successfully.
  @retval Others                Fail to remove the entry.

**/
EFI_STATUS
EFIAPI
MnpDestoryChildEntry (
  IN LIST_ENTRY         *Entry,
  IN VOID               *Context
  )
{
  MNP_INSTANCE_DATA             *Instance;
  EFI_SERVICE_BINDING_PROTOCOL  *ServiceBinding;

  ServiceBinding = (EFI_SERVICE_BINDING_PROTOCOL *) Context;
  Instance = CR (Entry, MNP_INSTANCE_DATA, InstEntry, MNP_INSTANCE_DATA_SIGNATURE);
  return ServiceBinding->DestroyChild (ServiceBinding, Instance->Handle);
}

/**
  Destroy all child of the MNP service data.

  @param[in, out]  MnpServiceData    Pointer to the mnp service context data.

  @retval EFI_SUCCESS           All child are destroyed.
  @retval Others                Failed to destroy all child.

**/
EFI_STATUS
MnpDestroyServiceChild (
  IN OUT MNP_SERVICE_DATA    *MnpServiceData
  )
{
  LIST_ENTRY                         *List;
  EFI_STATUS                         Status;
  UINTN                              ListLength;
  
  List = &MnpServiceData->ChildrenList;
  
  Status = NetDestroyLinkList (
             List,
             MnpDestoryChildEntry,
             &MnpServiceData->ServiceBinding,
             &ListLength
             );
  if (EFI_ERROR (Status) || ListLength != 0) {
    return EFI_DEVICE_ERROR;
  }

  return EFI_SUCCESS;
}

/**
  Find the MNP Service Data for given VLAN ID.

  @param[in]  MnpDeviceData      Pointer to the mnp device context data.
  @param[in]  VlanId             The VLAN ID.

  @return A pointer to MNP_SERVICE_DATA or NULL if not found.

**/
MNP_SERVICE_DATA *
MnpFindServiceData (
  IN MNP_DEVICE_DATA     *MnpDeviceData,
  IN UINT16              VlanId
  )
{
  LIST_ENTRY        *Entry;
  MNP_SERVICE_DATA  *MnpServiceData;

  NET_LIST_FOR_EACH (Entry, &MnpDeviceData->ServiceList) {
    //
    // Check VLAN ID of each Mnp Service Data
    //
    MnpServiceData = MNP_SERVICE_DATA_FROM_LINK (Entry);
    if (MnpServiceData->VlanId == VlanId) {
      return MnpServiceData;
    }
  }

  return NULL;
}

/**
  Initialize the mnp instance context data.

  @param[in]       MnpServiceData   Pointer to the mnp service context data.
  @param[in, out]  Instance         Pointer to the mnp instance context data
                                    to initialize.

**/
VOID
MnpInitializeInstanceData (
  IN     MNP_SERVICE_DATA    *MnpServiceData,
  IN OUT MNP_INSTANCE_DATA   *Instance
  )
{
  NET_CHECK_SIGNATURE (MnpServiceData, MNP_SERVICE_DATA_SIGNATURE);
  ASSERT (Instance != NULL);

  //
  // Set the signature.
  //
  Instance->Signature = MNP_INSTANCE_DATA_SIGNATURE;

  //
  // Copy the MNP Protocol interfaces from the template.
  //
  CopyMem (&Instance->ManagedNetwork, &mMnpProtocolTemplate, sizeof (Instance->ManagedNetwork));

  //
  // Copy the default config data.
  //
  CopyMem (&Instance->ConfigData, &mMnpDefaultConfigData, sizeof (Instance->ConfigData));

  //
  // Initialize the lists.
  //
  InitializeListHead (&Instance->GroupCtrlBlkList);
  InitializeListHead (&Instance->RcvdPacketQueue);
  InitializeListHead (&Instance->RxDeliveredPacketQueue);

  //
  // Initialize the RxToken Map.
  //
  NetMapInit (&Instance->RxTokenMap);

  //
  // Save the MnpServiceData info.
  //
  Instance->MnpServiceData = MnpServiceData;
}


/**
  Check whether the token specified by Arg matches the token in Item.

  @param[in]  Map               Pointer to the NET_MAP.
  @param[in]  Item              Pointer to the NET_MAP_ITEM.
  @param[in]  Arg               Pointer to the Arg, it's a pointer to the token to
                                check.

  @retval EFI_SUCCESS           The token specified by Arg is different from the
                                token in Item.
  @retval EFI_ACCESS_DENIED     The token specified by Arg is the same as that in
                                Item.

**/
EFI_STATUS
EFIAPI
MnpTokenExist (
  IN NET_MAP         *Map,
  IN NET_MAP_ITEM    *Item,
  IN VOID            *Arg
  )
{
  EFI_MANAGED_NETWORK_COMPLETION_TOKEN  *Token;
  EFI_MANAGED_NETWORK_COMPLETION_TOKEN  *TokenInItem;

  Token       = (EFI_MANAGED_NETWORK_COMPLETION_TOKEN *) Arg;
  TokenInItem = (EFI_MANAGED_NETWORK_COMPLETION_TOKEN *) Item->Key;

  if ((Token == TokenInItem) || (Token->Event == TokenInItem->Event)) {
    //
    // The token is the same either the two tokens equals or the Events in
    // the two tokens are the same.
    //
    return EFI_ACCESS_DENIED;
  }

  return EFI_SUCCESS;
}

/**
  Cancel the token specified by Arg if it matches the token in Item.

  @param[in, out]  Map               Pointer to the NET_MAP.
  @param[in, out]  Item              Pointer to the NET_MAP_ITEM.
  @param[in]       Arg               Pointer to the Arg, it's a pointer to the
                                     token to cancel.

  @retval EFI_SUCCESS       The Arg is NULL, and the token in Item is cancelled,
                            or the Arg isn't NULL, and the token in Item is
                            different from the Arg.
  @retval EFI_ABORTED       The Arg isn't NULL, the token in Item mathces the
                            Arg, and the token is cancelled.

**/
EFI_STATUS
EFIAPI
MnpCancelTokens (
  IN OUT NET_MAP         *Map,
  IN OUT NET_MAP_ITEM    *Item,
  IN     VOID            *Arg
  )
{
  EFI_MANAGED_NETWORK_COMPLETION_TOKEN  *TokenToCancel;

  if ((Arg != NULL) && (Item->Key != Arg)) {
    //
    // The token in Item is not the token specified by Arg.
    //
    return EFI_SUCCESS;
  }

  TokenToCancel = (EFI_MANAGED_NETWORK_COMPLETION_TOKEN *) Item->Key;

  //
  // Remove the item from the map.
  //
  NetMapRemoveItem (Map, Item, NULL);

  //
  // Cancel this token with status set to EFI_ABORTED.
  //
  TokenToCancel->Status = EFI_ABORTED;
  gBS->SignalEvent (TokenToCancel->Event);

  if (Arg != NULL) {
    //
    // Only abort the token specified by Arg if Arg isn't NULL.
    //
    return EFI_ABORTED;
  }

  return EFI_SUCCESS;
}


/**
  Start and initialize the simple network.

  @param[in]  Snp               Pointer to the simple network protocol.

  @retval EFI_SUCCESS           The simple network protocol is started.
  @retval Others                Other errors as indicated.

**/
EFI_STATUS
MnpStartSnp (
  IN EFI_SIMPLE_NETWORK_PROTOCOL     *Snp
  )
{
  EFI_STATUS  Status;

  ASSERT (Snp != NULL);

  //
  // Start the simple network.
  //
  Status = Snp->Start (Snp);

  if (!EFI_ERROR (Status)) {
    //
    // Initialize the simple network.
    //
    Status = Snp->Initialize (Snp, 0, 0);
  }

  return Status;
}


/**
  Stop the simple network.

  @param[in]  MnpDeviceData     Pointer to the MNP_DEVICE_DATA.

  @retval EFI_SUCCESS           The simple network is stopped.
  @retval Others                Other errors as indicated.

**/
EFI_STATUS
MnpStopSnp (
  IN  MNP_DEVICE_DATA   *MnpDeviceData
  )
{
  EFI_STATUS  Status;
  EFI_SIMPLE_NETWORK_PROTOCOL     *Snp;
  
  Snp = MnpDeviceData->Snp;
  ASSERT (Snp != NULL);

  //
  // Recycle all the transmit buffer from SNP.
  //
  Status = MnpRecycleTxBuf (MnpDeviceData);
  if (EFI_ERROR (Status)) {
    return Status;
  }

  //
  // Shut down the simple network.
  //
  Status  = Snp->Shutdown (Snp);
  if (!EFI_ERROR (Status)) {
    //
    // Stop the simple network.
    //
    Status = Snp->Stop (Snp);
  }

  return Status;
}


/**
  Start the managed network, this function is called when one instance is configured
  or reconfigured.

  @param[in, out]  MnpServiceData       Pointer to the mnp service context data.
  @param[in]       IsConfigUpdate       The instance is reconfigured or it's the first
                                        time the instanced is configured.
  @param[in]       EnableSystemPoll     Enable the system polling or not.

  @retval EFI_SUCCESS                   The managed network is started and some
                                        configuration is updated.
  @retval Others                        Other errors as indicated.

**/
EFI_STATUS
MnpStart (
  IN OUT MNP_SERVICE_DATA    *MnpServiceData,
  IN     BOOLEAN             IsConfigUpdate,
  IN     BOOLEAN             EnableSystemPoll
  )
{
  EFI_STATUS      Status;
  EFI_TIMER_DELAY TimerOpType;
  MNP_DEVICE_DATA *MnpDeviceData;

  NET_CHECK_SIGNATURE (MnpServiceData, MNP_SERVICE_DATA_SIGNATURE);

  Status        = EFI_SUCCESS;
  MnpDeviceData = MnpServiceData->MnpDeviceData;

  if (!IsConfigUpdate) {
    //
    // If it's not a configuration update, increase the configured children number.
    //
    MnpDeviceData->ConfiguredChildrenNumber++;

    if (MnpDeviceData->ConfiguredChildrenNumber == 1) {
      //
      // It's the first configured child, start the simple network.
      //
      Status = MnpStartSnp (MnpDeviceData->Snp);
      if (EFI_ERROR (Status)) {
        DEBUG ((EFI_D_ERROR, "MnpStart: MnpStartSnp failed, %r.\n", Status));

        goto ErrorExit;
      }

      //
      // Start the timeout timer.
      //
      Status = gBS->SetTimer (
                      MnpDeviceData->TimeoutCheckTimer,
                      TimerPeriodic,
                      MNP_TIMEOUT_CHECK_INTERVAL
                      );
      if (EFI_ERROR (Status)) {
        DEBUG (
          (EFI_D_ERROR,
          "MnpStart, gBS->SetTimer for TimeoutCheckTimer %r.\n",
          Status)
          );

        goto ErrorExit;
      }

      //
      // Start the media detection timer.
      //
      Status = gBS->SetTimer (
                      MnpDeviceData->MediaDetectTimer,
                      TimerPeriodic,
                      MNP_MEDIA_DETECT_INTERVAL
                      );
      if (EFI_ERROR (Status)) {
        DEBUG (
          (EFI_D_ERROR,
          "MnpStart, gBS->SetTimer for MediaDetectTimer %r.\n",
          Status)
          );

        goto ErrorExit;
      }
    }
  }

  if (MnpDeviceData->EnableSystemPoll ^ EnableSystemPoll) {
    //
    // The EnableSystemPoll differs with the current state, disable or enable
    // the system poll.
    //
    TimerOpType = EnableSystemPoll ? TimerPeriodic : TimerCancel;

    Status      = gBS->SetTimer (MnpDeviceData->PollTimer, TimerOpType, MNP_SYS_POLL_INTERVAL);
    if (EFI_ERROR (Status)) {
      DEBUG ((EFI_D_ERROR, "MnpStart: gBS->SetTimer for PollTimer failed, %r.\n", Status));

      goto ErrorExit;
    }

    MnpDeviceData->EnableSystemPoll = EnableSystemPoll;
  }

  //
  // Change the receive filters if need.
  //
  Status = MnpConfigReceiveFilters (MnpDeviceData);

ErrorExit:
  return Status;
}


/**
  Stop the managed network.

  @param[in, out]  MnpServiceData    Pointer to the mnp service context data.

  @retval EFI_SUCCESS                The managed network is stopped.
  @retval Others                     Other errors as indicated.

**/
EFI_STATUS
MnpStop (
  IN OUT MNP_SERVICE_DATA    *MnpServiceData
  )
{
  EFI_STATUS      Status;
  MNP_DEVICE_DATA *MnpDeviceData;

  NET_CHECK_SIGNATURE (MnpServiceData, MNP_SERVICE_DATA_SIGNATURE);
  MnpDeviceData = MnpServiceData->MnpDeviceData;
  ASSERT (MnpDeviceData->ConfiguredChildrenNumber > 0);

  //
  // Configure the receive filters.
  //
  MnpConfigReceiveFilters (MnpDeviceData);

  //
  // Decrease the children number.
  //
  MnpDeviceData->ConfiguredChildrenNumber--;

  if (MnpDeviceData->ConfiguredChildrenNumber > 0) {
    //
    // If there are other configured chilren, return and keep the timers and
    // simple network unchanged.
    //
    return EFI_SUCCESS;
  }

  //
  // No configured children now.
  //
  if (MnpDeviceData->EnableSystemPoll) {
    //
    //  The system poll in on, cancel the poll timer.
    //
    Status  = gBS->SetTimer (MnpDeviceData->PollTimer, TimerCancel, 0);
    MnpDeviceData->EnableSystemPoll = FALSE;
  }

  //
  // Cancel the timeout timer.
  //
  Status = gBS->SetTimer (MnpDeviceData->TimeoutCheckTimer, TimerCancel, 0);

  //
  // Cancel the media detect timer.
  //
  Status = gBS->SetTimer (MnpDeviceData->MediaDetectTimer, TimerCancel, 0);

  //
  // Stop the simple network.
  //
  Status = MnpStopSnp (MnpDeviceData);
  return Status;
}


/**
  Flush the instance's received data.

  @param[in, out]  Instance              Pointer to the mnp instance context data.

**/
VOID
MnpFlushRcvdDataQueue (
  IN OUT MNP_INSTANCE_DATA   *Instance
  )
{
  EFI_TPL         OldTpl;
  MNP_RXDATA_WRAP *RxDataWrap;

  NET_CHECK_SIGNATURE (Instance, MNP_INSTANCE_DATA_SIGNATURE);

  OldTpl = gBS->RaiseTPL (TPL_NOTIFY);

  while (!IsListEmpty (&Instance->RcvdPacketQueue)) {
    //
    // Remove all the Wraps.
    //
    RxDataWrap = NET_LIST_HEAD (&Instance->RcvdPacketQueue, MNP_RXDATA_WRAP, WrapEntry);

    //
    // Recycle the RxDataWrap.
    //
    MnpRecycleRxData (NULL, (VOID *) RxDataWrap);
    Instance->RcvdPacketQueueSize--;
  }

  ASSERT (Instance->RcvdPacketQueueSize == 0);

  gBS->RestoreTPL (OldTpl);
}


/**
  Configure the Instance using ConfigData.

  @param[in, out]  Instance     Pointer to the mnp instance context data.
  @param[in]       ConfigData   Pointer to the configuration data used to configure
                                the isntance.

  @retval EFI_SUCCESS           The Instance is configured.
  @retval EFI_UNSUPPORTED       EnableReceiveTimestamps is on and the
                                implementation doesn't support it.
  @retval Others                Other errors as indicated.

**/
EFI_STATUS
MnpConfigureInstance (
  IN OUT MNP_INSTANCE_DATA                 *Instance,
  IN     EFI_MANAGED_NETWORK_CONFIG_DATA   *ConfigData OPTIONAL
  )
{
  EFI_STATUS                      Status;
  MNP_SERVICE_DATA                *MnpServiceData;
  MNP_DEVICE_DATA                 *MnpDeviceData;
  EFI_MANAGED_NETWORK_CONFIG_DATA *OldConfigData;
  EFI_MANAGED_NETWORK_CONFIG_DATA *NewConfigData;
  BOOLEAN                         IsConfigUpdate;

  NET_CHECK_SIGNATURE (Instance, MNP_INSTANCE_DATA_SIGNATURE);

  if ((ConfigData != NULL) && ConfigData->EnableReceiveTimestamps) {
    //
    // Don't support timestamp.
    //
    return EFI_UNSUPPORTED;
  }

  Status          = EFI_SUCCESS;

  MnpServiceData  = Instance->MnpServiceData;
  MnpDeviceData   = MnpServiceData->MnpDeviceData;
  NET_CHECK_SIGNATURE (MnpDeviceData, MNP_DEVICE_DATA_SIGNATURE);

  IsConfigUpdate  = (BOOLEAN) ((Instance->Configured) && (ConfigData != NULL));

  OldConfigData   = &Instance->ConfigData;
  NewConfigData   = ConfigData;
  if (NewConfigData == NULL) {
    //
    // Restore back the default config data if a reset of this instance
    // is required.
    //
    NewConfigData = &mMnpDefaultConfigData;
  }

  //
  // Reset the instance's receive filter.
  //
  Instance->ReceiveFilter = 0;

  //
  // Clear the receive counters according to the old ConfigData.
  //
  if (OldConfigData->EnableUnicastReceive) {
    MnpDeviceData->UnicastCount--;
  }

  if (OldConfigData->EnableMulticastReceive) {
    MnpDeviceData->MulticastCount--;
  }

  if (OldConfigData->EnableBroadcastReceive) {
    MnpDeviceData->BroadcastCount--;
  }

  if (OldConfigData->EnablePromiscuousReceive) {
    MnpDeviceData->PromiscuousCount--;
  }

  //
  // Set the receive filter counters and the receive filter of the
  // instance according to the new ConfigData.
  //
  if (NewConfigData->EnableUnicastReceive) {
    MnpDeviceData->UnicastCount++;
    Instance->ReceiveFilter |= MNP_RECEIVE_UNICAST;
  }

  if (NewConfigData->EnableMulticastReceive) {
    MnpDeviceData->MulticastCount++;
  }

  if (NewConfigData->EnableBroadcastReceive) {
    MnpDeviceData->BroadcastCount++;
    Instance->ReceiveFilter |= MNP_RECEIVE_BROADCAST;
  }

  if (NewConfigData->EnablePromiscuousReceive) {
    MnpDeviceData->PromiscuousCount++;
  }

  if (OldConfigData->FlushQueuesOnReset) {
    MnpFlushRcvdDataQueue (Instance);
  }

  if (ConfigData == NULL) {
    Instance->ManagedNetwork.Cancel (&Instance->ManagedNetwork, NULL);
  }

  if (!NewConfigData->EnableMulticastReceive) {
    MnpGroupOp (Instance, FALSE, NULL, NULL);
  }

  //
  // Save the new configuration data.
  //
  CopyMem (OldConfigData, NewConfigData, sizeof (*OldConfigData));

  Instance->Configured = (BOOLEAN) (ConfigData != NULL);
  if (Instance->Configured) {
    //
    // The instance is configured, start the Mnp.
    //
    Status = MnpStart (
              MnpServiceData,
              IsConfigUpdate,
              (BOOLEAN) !NewConfigData->DisableBackgroundPolling
              );
  } else {
    //
    // The instance is changed to the unconfigured state, stop the Mnp.
    //
    Status = MnpStop (MnpServiceData);
  }

  return Status;
}

/**
  Configure the Snp receive filters according to the instances' receive filter
  settings.

  @param[in]  MnpDeviceData         Pointer to the mnp device context data.

  @retval     EFI_SUCCESS           The receive filters is configured.
  @retval     EFI_OUT_OF_RESOURCES  The receive filters can't be configured due
                                    to lack of memory resource.

**/
EFI_STATUS
MnpConfigReceiveFilters (
  IN MNP_DEVICE_DATA     *MnpDeviceData
  )
{
  EFI_STATUS                  Status;
  EFI_SIMPLE_NETWORK_PROTOCOL *Snp;
  EFI_MAC_ADDRESS             *MCastFilter;
  UINT32                      MCastFilterCnt;
  UINT32                      EnableFilterBits;
  UINT32                      DisableFilterBits;
  BOOLEAN                     ResetMCastFilters;
  LIST_ENTRY                  *Entry;
  UINT32                      Index;
  MNP_GROUP_ADDRESS           *GroupAddress;

  NET_CHECK_SIGNATURE (MnpDeviceData, MNP_DEVICE_DATA_SIGNATURE);

  Snp = MnpDeviceData->Snp;

  //
  // Initialize the enable filter and disable filter.
  //
  EnableFilterBits  = 0;
  DisableFilterBits = Snp->Mode->ReceiveFilterMask;

  if (MnpDeviceData->UnicastCount != 0) {
    //
    // Enable unicast if any instance wants to receive unicast.
    //
    EnableFilterBits |= EFI_SIMPLE_NETWORK_RECEIVE_UNICAST;
  }

  if (MnpDeviceData->BroadcastCount != 0) {
    //
    // Enable broadcast if any instance wants to receive broadcast.
    //
    EnableFilterBits |= EFI_SIMPLE_NETWORK_RECEIVE_BROADCAST;
  }

  MCastFilter       = NULL;
  MCastFilterCnt    = 0;
  ResetMCastFilters = TRUE;

  if ((MnpDeviceData->MulticastCount != 0) && (MnpDeviceData->GroupAddressCount != 0)) {
    //
    // There are instances configured to receive multicast and already some group
    // addresses are joined.
    //

    ResetMCastFilters = FALSE;

    if (MnpDeviceData->GroupAddressCount <= Snp->Mode->MaxMCastFilterCount) {
      //
      // The joind group address is less than simple network's maximum count.
      // Just configure the snp to do the multicast filtering.
      //

      EnableFilterBits |= EFI_SIMPLE_NETWORK_RECEIVE_MULTICAST;

      //
      // Allocate pool for the mulicast addresses.
      //
      MCastFilterCnt  = MnpDeviceData->GroupAddressCount;
      MCastFilter     = AllocatePool (sizeof (EFI_MAC_ADDRESS) * MCastFilterCnt);
      if (MCastFilter == NULL) {
        DEBUG ((EFI_D_ERROR, "MnpConfigReceiveFilters: Failed to allocate memory resource for MCastFilter.\n"));

        return EFI_OUT_OF_RESOURCES;
      }

      //
      // Fill the multicast HW address buffer.
      //
      Index = 0;
      NET_LIST_FOR_EACH (Entry, &MnpDeviceData->GroupAddressList) {

        GroupAddress = NET_LIST_USER_STRUCT (Entry, MNP_GROUP_ADDRESS, AddrEntry);
        CopyMem (MCastFilter + Index, &GroupAddress->Address, sizeof (*(MCastFilter + Index)));
        Index++;

        ASSERT (Index <= MCastFilterCnt);
      }
    } else {
      //
      // The maximum multicast is reached, set the filter to be promiscuous
      // multicast.
      //

      if ((Snp->Mode->ReceiveFilterMask & EFI_SIMPLE_NETWORK_RECEIVE_PROMISCUOUS_MULTICAST) != 0) {
        EnableFilterBits |= EFI_SIMPLE_NETWORK_RECEIVE_PROMISCUOUS_MULTICAST;
      } else {
        //
        // Either MULTICAST or PROMISCUOUS_MULTICAST is not supported by Snp,
        // set the NIC to be promiscuous although this will tremendously degrade
        // the performance.
        //
        EnableFilterBits |= EFI_SIMPLE_NETWORK_RECEIVE_PROMISCUOUS;
      }
    }
  }

  if (MnpDeviceData->PromiscuousCount != 0) {
    //
    // Enable promiscuous if any instance wants to receive promiscuous.
    //
    EnableFilterBits |= EFI_SIMPLE_NETWORK_RECEIVE_PROMISCUOUS;
  }

  //
  // Set the disable filter.
  //
  DisableFilterBits ^= EnableFilterBits;

  //
  // Configure the receive filters of SNP.
  //
  Status = Snp->ReceiveFilters (
                  Snp,
                  EnableFilterBits,
                  DisableFilterBits,
                  ResetMCastFilters,
                  MCastFilterCnt,
                  MCastFilter
                  );
  DEBUG_CODE (
    if (EFI_ERROR (Status)) {
      DEBUG (
        (EFI_D_ERROR,
        "MnpConfigReceiveFilters: Snp->ReceiveFilters failed, %r.\n",
        Status)
        );
    }
  );

  if (MCastFilter != NULL) {
    //
    // Free the buffer used to hold the group addresses.
    //
    FreePool (MCastFilter);
  }

  return Status;
}


/**
  Add a group address control block which controls the MacAddress for
  this instance.

  @param[in, out]  Instance        Pointer to the mnp instance context data.
  @param[in, out]  CtrlBlk         Pointer to the group address control block.
  @param[in, out]  GroupAddress    Pointer to the group adress.
  @param[in]       MacAddress      Pointer to the mac address.
  @param[in]       HwAddressSize   The hardware address size.

  @retval EFI_SUCCESS              The group address control block is added.
  @retval EFI_OUT_OF_RESOURCES     Failed due to lack of memory resources.

**/
EFI_STATUS
MnpGroupOpAddCtrlBlk (
  IN OUT MNP_INSTANCE_DATA         *Instance,
  IN OUT MNP_GROUP_CONTROL_BLOCK   *CtrlBlk,
  IN OUT MNP_GROUP_ADDRESS         *GroupAddress OPTIONAL,
  IN     EFI_MAC_ADDRESS           *MacAddress,
  IN     UINT32                    HwAddressSize
  )
{
  MNP_DEVICE_DATA  *MnpDeviceData;

  NET_CHECK_SIGNATURE (Instance, MNP_INSTANCE_DATA_SIGNATURE);

  MnpDeviceData = Instance->MnpServiceData->MnpDeviceData;
  NET_CHECK_SIGNATURE (MnpDeviceData, MNP_DEVICE_DATA_SIGNATURE);

  if (GroupAddress == NULL) {
    ASSERT (MacAddress != NULL);

    //
    // Allocate a new GroupAddress to be added into MNP's GroupAddressList.
    //
    GroupAddress = AllocatePool (sizeof (MNP_GROUP_ADDRESS));
    if (GroupAddress == NULL) {

      DEBUG ((EFI_D_ERROR, "MnpGroupOpFormCtrlBlk: Failed to allocate memory resource.\n"));

      return EFI_OUT_OF_RESOURCES;
    }

    CopyMem (&GroupAddress->Address, MacAddress, sizeof (GroupAddress->Address));
    GroupAddress->RefCnt = 0;
    InsertTailList (
      &MnpDeviceData->GroupAddressList,
      &GroupAddress->AddrEntry
      );
    MnpDeviceData->GroupAddressCount++;
  }

  //
  // Increase the RefCnt.
  //
  GroupAddress->RefCnt++;

  //
  // Add the CtrlBlk into the instance's GroupCtrlBlkList.
  //
  CtrlBlk->GroupAddress = GroupAddress;
  InsertTailList (&Instance->GroupCtrlBlkList, &CtrlBlk->CtrlBlkEntry);

  return EFI_SUCCESS;
}


/**
  Delete a group control block from the instance. If the controlled group address's
  reference count reaches zero, the group address is removed too.

  @param[in]  Instance              Pointer to the instance context data.
  @param[in]  CtrlBlk               Pointer to the group control block to delete.

  @return The group address controlled by the control block is no longer used or not.

**/
BOOLEAN
MnpGroupOpDelCtrlBlk (
  IN MNP_INSTANCE_DATA           *Instance,
  IN MNP_GROUP_CONTROL_BLOCK     *CtrlBlk
  )
{
  MNP_DEVICE_DATA   *MnpDeviceData;
  MNP_GROUP_ADDRESS *GroupAddress;

  NET_CHECK_SIGNATURE (Instance, MNP_INSTANCE_DATA_SIGNATURE);

  MnpDeviceData = Instance->MnpServiceData->MnpDeviceData;
  NET_CHECK_SIGNATURE (MnpDeviceData, MNP_DEVICE_DATA_SIGNATURE);

  //
  // Remove and free the CtrlBlk.
  //
  GroupAddress = CtrlBlk->GroupAddress;
  RemoveEntryList (&CtrlBlk->CtrlBlkEntry);
  FreePool (CtrlBlk);

  ASSERT (GroupAddress->RefCnt > 0);

  //
  // Count down the RefCnt.
  //
  GroupAddress->RefCnt--;

  if (GroupAddress->RefCnt == 0) {
    //
    // Free this GroupAddress entry if no instance uses it.
    //
    MnpDeviceData->GroupAddressCount--;
    RemoveEntryList (&GroupAddress->AddrEntry);
    FreePool (GroupAddress);

    return TRUE;
  }

  return FALSE;
}


/**
  Do the group operations for this instance.

  @param[in, out]  Instance        Pointer to the instance context data.
  @param[in]       JoinFlag        Set to TRUE to join a group. Set to TRUE to
                                   leave a group/groups.
  @param[in]       MacAddress      Pointer to the group address to join or leave.
  @param[in]       CtrlBlk         Pointer to the group control block if JoinFlag
                                   is FALSE.

  @retval EFI_SUCCESS              The group operation finished.
  @retval EFI_OUT_OF_RESOURCES     Failed due to lack of memory resources.
  @retval Others                   Other errors as indicated.

**/
EFI_STATUS
MnpGroupOp (
  IN OUT MNP_INSTANCE_DATA         *Instance,
  IN     BOOLEAN                   JoinFlag,
  IN     EFI_MAC_ADDRESS           *MacAddress OPTIONAL,
  IN     MNP_GROUP_CONTROL_BLOCK   *CtrlBlk OPTIONAL
  )
{
  MNP_DEVICE_DATA         *MnpDeviceData;
  LIST_ENTRY              *Entry;
  LIST_ENTRY              *NextEntry;
  MNP_GROUP_ADDRESS       *GroupAddress;
  EFI_SIMPLE_NETWORK_MODE *SnpMode;
  MNP_GROUP_CONTROL_BLOCK *NewCtrlBlk;
  EFI_STATUS              Status;
  BOOLEAN                 AddressExist;
  BOOLEAN                 NeedUpdate;

  NET_CHECK_SIGNATURE (Instance, MNP_INSTANCE_DATA_SIGNATURE);

  MnpDeviceData = Instance->MnpServiceData->MnpDeviceData;
  SnpMode       = MnpDeviceData->Snp->Mode;

  if (JoinFlag) {
    //
    // A new gropu address is to be added.
    //
    GroupAddress  = NULL;
    AddressExist  = FALSE;

    //
    // Allocate memory for the control block.
    //
    NewCtrlBlk = AllocatePool (sizeof (MNP_GROUP_CONTROL_BLOCK));
    if (NewCtrlBlk == NULL) {
      DEBUG ((EFI_D_ERROR, "MnpGroupOp: Failed to allocate memory resource.\n"));

      return EFI_OUT_OF_RESOURCES;
    }

    NET_LIST_FOR_EACH (Entry, &MnpDeviceData->GroupAddressList) {
      //
      // Check whether the MacAddress is already joined by other instances.
      //
      GroupAddress = NET_LIST_USER_STRUCT (Entry, MNP_GROUP_ADDRESS, AddrEntry);
      if (CompareMem (MacAddress, &GroupAddress->Address, SnpMode->HwAddressSize) == 0) {
        AddressExist = TRUE;
        break;
      }
    }

    if (!AddressExist) {
      GroupAddress = NULL;
    }

    //
    // Add the GroupAddress for this instance.
    //
    Status = MnpGroupOpAddCtrlBlk (
              Instance,
              NewCtrlBlk,
              GroupAddress,
              MacAddress,
              SnpMode->HwAddressSize
              );
    if (EFI_ERROR (Status)) {
      return Status;
    }

    NeedUpdate = TRUE;
  } else {
    if (MacAddress != NULL) {
      ASSERT (CtrlBlk != NULL);

      //
      // Leave the specific multicast mac address.
      //
      NeedUpdate = MnpGroupOpDelCtrlBlk (Instance, CtrlBlk);
    } else {
      //
      // Leave all multicast mac addresses.
      //
      NeedUpdate = FALSE;

      NET_LIST_FOR_EACH_SAFE (Entry, NextEntry, &Instance->GroupCtrlBlkList) {

        NewCtrlBlk = NET_LIST_USER_STRUCT (
                      Entry,
                      MNP_GROUP_CONTROL_BLOCK,
                      CtrlBlkEntry
                      );
        //
        // Update is required if the group address left is no longer used
        // by other instances.
        //
        NeedUpdate = MnpGroupOpDelCtrlBlk (Instance, NewCtrlBlk);
      }
    }
  }

  Status = EFI_SUCCESS;

  if (NeedUpdate) {
    //
    // Reconfigure the receive filters if necessary.
    //
    Status = MnpConfigReceiveFilters (MnpDeviceData);
  }

  return Status;
}