/** @file
  VLAN Config Protocol implementation and VLAN packet process routine.

Copyright (c) 2009 - 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"

VLAN_DEVICE_PATH          mVlanDevicePathTemplate = {
  {
    MESSAGING_DEVICE_PATH,
    MSG_VLAN_DP,
    {
      (UINT8) (sizeof (VLAN_DEVICE_PATH)),
      (UINT8) ((sizeof (VLAN_DEVICE_PATH)) >> 8)
    }
  },
  0
};

EFI_VLAN_CONFIG_PROTOCOL  mVlanConfigProtocolTemplate = {
  VlanConfigSet,
  VlanConfigFind,
  VlanConfigRemove
};


/**
  Create a child handle for the VLAN ID.

  @param[in]       ImageHandle        The driver image handle.
  @param[in]       ControllerHandle   Handle of device to bind driver to.
  @param[in]       VlanId             The VLAN ID.
  @param[out]      Devicepath         Pointer to returned device path for child handle.

  @return The handle of VLAN child or NULL if failed to create VLAN child.

**/
EFI_HANDLE
MnpCreateVlanChild (
  IN     EFI_HANDLE                  ImageHandle,
  IN     EFI_HANDLE                  ControllerHandle,
  IN     UINT16                      VlanId,
     OUT EFI_DEVICE_PATH_PROTOCOL    **Devicepath OPTIONAL
  )
{
  EFI_HANDLE                ChildHandle;
  VLAN_DEVICE_PATH          VlanNode;
  EFI_DEVICE_PATH_PROTOCOL  *ParentDevicePath;
  EFI_DEVICE_PATH_PROTOCOL  *VlanDevicePath;
  EFI_STATUS                Status;

  //
  // Try to get parent device path
  //
  Status = gBS->OpenProtocol (
                  ControllerHandle,
                  &gEfiDevicePathProtocolGuid,
                  (VOID **) &ParentDevicePath,
                  ImageHandle,
                  ControllerHandle,
                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
                  );
  if (EFI_ERROR (Status)) {
    return NULL;
  }

  //
  // Construct device path for child handle: MAC + VLAN
  //
  CopyMem (&VlanNode, &mVlanDevicePathTemplate, sizeof (VLAN_DEVICE_PATH));
  VlanNode.VlanId = VlanId;
  VlanDevicePath = AppendDevicePathNode (
                     ParentDevicePath,
                     (EFI_DEVICE_PATH_PROTOCOL *) &VlanNode
                     );
  if (VlanDevicePath == NULL) {
    return NULL;
  }

  //
  // Create child VLAN handle by installing DevicePath protocol
  //
  ChildHandle = NULL;
  Status = gBS->InstallMultipleProtocolInterfaces (
                  &ChildHandle,
                  &gEfiDevicePathProtocolGuid,
                  VlanDevicePath,
                  NULL
                  );
  if (EFI_ERROR (Status)) {
    FreePool (VlanDevicePath);
    return NULL;
  }

  if (Devicepath != NULL) {
    *Devicepath = VlanDevicePath;
  }

  return ChildHandle;
}

/**
  Remove VLAN tag from a packet.

  @param[in, out]  MnpDeviceData      Pointer to the mnp device context data.
  @param[in, out]  Nbuf               Pointer to the NET_BUF to remove VLAN tag.
  @param[out]      VlanId             Pointer to the returned VLAN ID.

  @retval TRUE             VLAN tag is removed from this packet.
  @retval FALSE            There is no VLAN tag in this packet.

**/
BOOLEAN
MnpRemoveVlanTag (
  IN OUT MNP_DEVICE_DATA   *MnpDeviceData,
  IN OUT NET_BUF           *Nbuf,
     OUT UINT16            *VlanId
  )
{
  UINT8     *Packet;
  UINTN     ProtocolOffset;
  UINT16    ProtocolType;
  VLAN_TCI  VlanTag;

  ProtocolOffset = MnpDeviceData->Snp->Mode->HwAddressSize * 2;

  //
  // Get the packet buffer.
  //
  Packet = NetbufGetByte (Nbuf, 0, NULL);
  ASSERT (Packet != NULL);

  //
  // Check whether this is VLAN tagged frame by Ether Type
  //
  *VlanId      = 0;
  ProtocolType = NTOHS (*(UINT16 *) (Packet + ProtocolOffset));
  if (ProtocolType != ETHER_TYPE_VLAN) {
    //
    // Not a VLAN tagged frame
    //
    return FALSE;
  }

  VlanTag.Uint16 = NTOHS (*(UINT16 *) (Packet + ProtocolOffset + sizeof (ProtocolType)));
  *VlanId = VlanTag.Bits.Vid;

  //
  // Move hardware address (DA + SA) 4 bytes right to override VLAN tag
  //
  CopyMem (Packet + NET_VLAN_TAG_LEN, Packet, ProtocolOffset);

  //
  // Remove VLAN tag from the Nbuf
  //
  NetbufTrim (Nbuf, NET_VLAN_TAG_LEN, NET_BUF_HEAD);

  return TRUE;
}


/**
  Build the vlan packet to transmit from the TxData passed in.

  @param  MnpServiceData         Pointer to the mnp service context data.
  @param  TxData                 Pointer to the transmit data containing the
                                 information to build the packet.
  @param  ProtocolType           Pointer to the Ethernet protocol type.
  @param  Packet                 Pointer to record the address of the packet.
  @param  Length                 Pointer to a UINT32 variable used to record the
                                 packet's length.

**/
VOID
MnpInsertVlanTag (
  IN     MNP_SERVICE_DATA                    *MnpServiceData,
  IN     EFI_MANAGED_NETWORK_TRANSMIT_DATA   *TxData,
     OUT UINT16                              *ProtocolType,
  IN OUT UINT8                               **Packet,
  IN OUT UINT32                              *Length
  )
{
  VLAN_TCI                *VlanTci;
  UINT16                  *Tpid;
  UINT16                  *EtherType;
  MNP_DEVICE_DATA         *MnpDeviceData;
  EFI_SIMPLE_NETWORK_MODE *SnpMode;

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

  *ProtocolType = ETHER_TYPE_VLAN;
  *Length = *Length + NET_VLAN_TAG_LEN;
  *Packet = *Packet - NET_VLAN_TAG_LEN;

  Tpid    = (UINT16 *) (*Packet + SnpMode->MediaHeaderSize - sizeof (*ProtocolType));
  VlanTci = (VLAN_TCI *) (UINTN) (Tpid + 1);
  if (TxData->HeaderLength != 0) {
    //
    // Media header is in packet, move DA+SA 4 bytes left
    //
    CopyMem (
      *Packet,
      *Packet + NET_VLAN_TAG_LEN,
      SnpMode->MediaHeaderSize - sizeof (*ProtocolType)
      );
    *Tpid = HTONS (ETHER_TYPE_VLAN);
  } else {
    //
    // Media header not in packet, VLAN TCI and original protocol type becomes payload
    //
    EtherType  = (UINT16 *) (UINTN) (VlanTci + 1);
    *EtherType = HTONS (TxData->ProtocolType);
  }

  VlanTci->Bits.Vid      = MnpServiceData->VlanId;
  VlanTci->Bits.Cfi      = VLAN_TCI_CFI_CANONICAL_MAC;
  VlanTci->Bits.Priority = MnpServiceData->Priority;
  VlanTci->Uint16        = HTONS (VlanTci->Uint16);
}

/**
  Check VLAN configuration variable and delete the duplicative content if has identical Vlan ID.

  @param[in]      MnpDeviceData      Pointer to the MNP device context data.
  @param[in]      Buffer             Pointer to the buffer contains the array of VLAN_TCI.
  @param[in]      NumberOfVlan       Pointer to number of VLAN.
  @param[out]     NewNumberOfVlan    Pointer to number of unique VLAN.
  
  @retval EFI_SUCCESS            The VLAN variable is successfully checked.
  @retval EFI_OUT_OF_RESOURCES   There is not enough resource to set the configuration.

**/
EFI_STATUS
MnpCheckVlanVariable (
  IN     MNP_DEVICE_DATA   *MnpDeviceData,
  IN     VLAN_TCI          *Buffer,
  IN     UINTN             NumberOfVlan,
     OUT UINTN             *NewNumberOfVlan
  )
{
  UINTN             Index;
  UINTN             Index2;
  UINTN             Count;
  BOOLEAN           FoundDuplicateItem;
  EFI_STATUS        Status;

  Count = 0;
  FoundDuplicateItem  = FALSE;
  Status = EFI_SUCCESS;
  
  for (Index = 0; Index < NumberOfVlan; Index++) {
   for (Index2 = Index + 1; Index2 < NumberOfVlan; Index2++) {
     if (Buffer[Index].Bits.Vid == Buffer[Index2].Bits.Vid) {
       FoundDuplicateItem = TRUE;
       Count++;
       break;
     }
   }
   if (FoundDuplicateItem) {
    for (Index2 = Index +1; Index2 < NumberOfVlan; Index++, Index2++) {
      CopyMem (Buffer + Index, Buffer + Index2, sizeof (VLAN_TCI));
    }
   }
   FoundDuplicateItem = FALSE;
  }

  *NewNumberOfVlan = NumberOfVlan - Count;
  if (Count != 0) {
    Status = MnpSetVlanVariable (MnpDeviceData, *NewNumberOfVlan, Buffer);
  }
  
  return Status;
}

/**
  Get VLAN configuration variable.

  @param[in]       MnpDeviceData      Pointer to the MNP device context data.
  @param[out]      NumberOfVlan       Pointer to number of VLAN to be returned.
  @param[out]      VlanVariable       Pointer to the buffer to return requested
                                      array of VLAN_TCI.

  @retval EFI_SUCCESS            The array of VLAN_TCI was returned in VlanVariable
                                 and number of VLAN was returned in NumberOfVlan.
  @retval EFI_NOT_FOUND          VLAN configuration variable not found.
  @retval EFI_OUT_OF_RESOURCES   There is not enough pool memory to store the configuration.

**/
EFI_STATUS
MnpGetVlanVariable (
  IN     MNP_DEVICE_DATA   *MnpDeviceData,
     OUT UINTN             *NumberOfVlan,
     OUT VLAN_TCI          **VlanVariable
  )
{
  UINTN       BufferSize;
  EFI_STATUS  Status;
  VLAN_TCI    *Buffer;
  UINTN       NewNumberOfVlan;

  //
  // Get VLAN configuration from EFI Variable
  //
  Buffer = NULL;
  BufferSize = 0;
  Status = gRT->GetVariable (
                  MnpDeviceData->MacString,
                  &gEfiVlanConfigProtocolGuid,
                  NULL,
                  &BufferSize,
                  NULL
                  );
  if (Status != EFI_BUFFER_TOO_SMALL) {
    return EFI_NOT_FOUND;
  }

  //
  // Allocate buffer to read the variable
  //
  Buffer = AllocateZeroPool (BufferSize);
  if (Buffer == NULL) {
    return EFI_OUT_OF_RESOURCES;
  }

  Status = gRT->GetVariable (
                  MnpDeviceData->MacString,
                  &gEfiVlanConfigProtocolGuid,
                  NULL,
                  &BufferSize,
                  Buffer
                  );
  if (EFI_ERROR (Status)) {
    FreePool (Buffer);
    return Status;
  }

  Status = MnpCheckVlanVariable (MnpDeviceData, Buffer, BufferSize / sizeof (VLAN_TCI), &NewNumberOfVlan);
  if (!EFI_ERROR (Status)) {
    *NumberOfVlan = NewNumberOfVlan;
    *VlanVariable = Buffer;
  }

  return Status;
}

/**
  Set VLAN configuration variable.

  @param[in] MnpDeviceData       Pointer to the MNP device context data.
  @param[in] NumberOfVlan        Number of VLAN in array VlanVariable.
  @param[in] VlanVariable        Pointer to array of VLAN_TCI.

  @retval EFI_SUCCESS            The VLAN variable is successfully set.
  @retval EFI_OUT_OF_RESOURCES   There is not enough resource to set the configuration.

**/
EFI_STATUS
MnpSetVlanVariable (
  IN MNP_DEVICE_DATA             *MnpDeviceData,
  IN UINTN                       NumberOfVlan,
  IN VLAN_TCI                    *VlanVariable
  )
{
  return gRT->SetVariable (
                MnpDeviceData->MacString,
                &gEfiVlanConfigProtocolGuid,
                EFI_VARIABLE_NON_VOLATILE | EFI_VARIABLE_BOOTSERVICE_ACCESS,
                NumberOfVlan * sizeof (VLAN_TCI),
                VlanVariable
                );
}


/**
  Create a VLAN device or modify the configuration parameter of an
  already-configured VLAN.

  The Set() function is used to create a new VLAN device or change the VLAN
  configuration parameters. If the VlanId hasn't been configured in the
  physical Ethernet device, a new VLAN device will be created. If a VLAN with
  this VlanId is already configured, then related configuration will be updated
  as the input parameters.

  If VlanId is zero, the VLAN device will send and receive untagged frames.
  Otherwise, the VLAN device will send and receive VLAN-tagged frames containing the VlanId.
  If VlanId is out of scope of (0-4094), EFI_INVALID_PARAMETER is returned.
  If Priority is out of the scope of (0-7), then EFI_INVALID_PARAMETER is returned.
  If there is not enough system memory to perform the registration, then
  EFI_OUT_OF_RESOURCES is returned.

  @param[in] This                Points to the EFI_VLAN_CONFIG_PROTOCOL.
  @param[in] VlanId              A unique identifier (1-4094) of the VLAN which is being created
                                 or modified, or zero (0).
  @param[in] Priority            3 bit priority in VLAN header. Priority 0 is default value. If
                                 VlanId is zero (0), Priority is ignored.

  @retval EFI_SUCCESS            The VLAN is successfully configured.
  @retval EFI_INVALID_PARAMETER  One or more of following conditions is TRUE:
                                 - This is NULL.
                                 - VlanId is an invalid VLAN Identifier.
                                 - Priority is invalid.
  @retval EFI_OUT_OF_RESOURCES   There is not enough system memory to perform the registration.

**/
EFI_STATUS
EFIAPI
VlanConfigSet (
  IN EFI_VLAN_CONFIG_PROTOCOL    *This,
  IN UINT16                      VlanId,
  IN UINT8                       Priority
  )
{
  EFI_STATUS        Status;
  MNP_DEVICE_DATA   *MnpDeviceData;
  MNP_SERVICE_DATA  *MnpServiceData;
  VLAN_TCI          *OldVariable;
  VLAN_TCI          *NewVariable;
  UINTN             NumberOfVlan;
  UINTN             Index;
  BOOLEAN           IsAdd;
  LIST_ENTRY        *Entry;

  if ((This == NULL) || (VlanId > 4094) || (Priority > 7)) {
    return EFI_INVALID_PARAMETER;
  }

  IsAdd = FALSE;
  MnpDeviceData = MNP_DEVICE_DATA_FROM_THIS (This);
  if (MnpDeviceData->NumberOfVlan == 0) {
    //
    // No existing VLAN, this is the first VLAN to add
    //
    IsAdd = TRUE;
    Entry = GetFirstNode (&MnpDeviceData->ServiceList);
    MnpServiceData = MNP_SERVICE_DATA_FROM_LINK (Entry);

    if (VlanId != 0) {
      //
      // VlanId is not 0, need destroy the default MNP service data
      //
      Status = MnpDestroyServiceChild (MnpServiceData);
      if (EFI_ERROR (Status)) {
        return Status;
      }

      Status = MnpDestroyServiceData (MnpServiceData);
      if (EFI_ERROR (Status)) {
        return Status;
      }

      //
      // Create a new MNP service data for this VLAN
      //
      MnpServiceData = MnpCreateServiceData (MnpDeviceData, VlanId, Priority);
      if (MnpServiceData == NULL) {
        return EFI_OUT_OF_RESOURCES;
      }
    }
  } else {
    //
    // Try to find VlanId in existing VLAN list
    //
    MnpServiceData = MnpFindServiceData (MnpDeviceData, VlanId);
    if (MnpServiceData == NULL) {
      //
      // VlanId not found, create a new MNP service data
      //
      IsAdd = TRUE;
      MnpServiceData = MnpCreateServiceData (MnpDeviceData, VlanId, Priority);
      if (MnpServiceData == NULL) {
        return EFI_OUT_OF_RESOURCES;
      }
    }
  }

  MnpServiceData->VlanId = VlanId;
  MnpServiceData->Priority = Priority;
  if (IsAdd) {
    MnpDeviceData->NumberOfVlan++;
  }

  //
  // Update VLAN configuration variable
  //
  OldVariable  = NULL;
  NewVariable  = NULL;
  NumberOfVlan = 0;
  MnpGetVlanVariable (MnpDeviceData, &NumberOfVlan, &OldVariable);

  if (IsAdd) {
    //
    // VLAN not exist - add
    //
    NewVariable = AllocateZeroPool ((NumberOfVlan + 1) * sizeof (VLAN_TCI));
    if (NewVariable == NULL) {
      Status = EFI_OUT_OF_RESOURCES;
      goto Exit;
    }

    if (OldVariable != NULL) {
      CopyMem (NewVariable, OldVariable, NumberOfVlan * sizeof (VLAN_TCI));
    }

    Index = NumberOfVlan++;
  } else {
    //
    // VLAN already exist - update
    //
    for (Index = 0; Index < NumberOfVlan; Index++) {
      if (OldVariable[Index].Bits.Vid == VlanId) {
        break;
      }
    }
    ASSERT (Index < NumberOfVlan);

    NewVariable = OldVariable;
    OldVariable = NULL;
  }

  NewVariable[Index].Bits.Vid      = VlanId;
  NewVariable[Index].Bits.Priority = Priority;

  Status = MnpSetVlanVariable (MnpDeviceData, NumberOfVlan, NewVariable);
  FreePool (NewVariable);

Exit:
  if (OldVariable != NULL) {
    FreePool (OldVariable);
  }

  return Status;
}


/**
  Find configuration information for specified VLAN or all configured VLANs.

  The Find() function is used to find the configuration information for matching
  VLAN and allocate a buffer into which those entries are copied.

  @param[in]  This               Points to the EFI_VLAN_CONFIG_PROTOCOL.
  @param[in]  VlanId             Pointer to VLAN identifier. Set to NULL to find all
                                 configured VLANs.
  @param[out] NumberOfVlan       The number of VLANs which is found by the specified criteria.
  @param[out] Entries            The buffer which receive the VLAN configuration.

  @retval EFI_SUCCESS            The VLAN is successfully found.
  @retval EFI_INVALID_PARAMETER  One or more of following conditions is TRUE:
                                 - This is NULL.
                                 - Specified VlanId is invalid.
  @retval EFI_NOT_FOUND          No matching VLAN is found.

**/
EFI_STATUS
EFIAPI
VlanConfigFind (
  IN     EFI_VLAN_CONFIG_PROTOCOL    *This,
  IN     UINT16                      *VlanId OPTIONAL,
     OUT UINT16                      *NumberOfVlan,
     OUT EFI_VLAN_FIND_DATA          **Entries
  )
{
  MNP_DEVICE_DATA     *MnpDeviceData;
  MNP_SERVICE_DATA    *MnpServiceData;
  LIST_ENTRY          *Entry;
  EFI_VLAN_FIND_DATA  *VlanData;

  if ((This == NULL) || (VlanId != NULL && *VlanId > 4094) || (NumberOfVlan == NULL) || (Entries == NULL)) {
    return EFI_INVALID_PARAMETER;
  }

  *NumberOfVlan = 0;
  *Entries      = NULL;

  MnpDeviceData = MNP_DEVICE_DATA_FROM_THIS (This);
  if (MnpDeviceData->NumberOfVlan == 0) {
    return EFI_NOT_FOUND;
  }

  if (VlanId == NULL) {
    //
    // Return all current VLAN configuration
    //
    *NumberOfVlan = (UINT16) MnpDeviceData->NumberOfVlan;
    VlanData = AllocateZeroPool (*NumberOfVlan * sizeof (EFI_VLAN_FIND_DATA));
    if (VlanData == NULL) {
      return EFI_OUT_OF_RESOURCES;
    }

    *Entries = VlanData;
    NET_LIST_FOR_EACH (Entry, &MnpDeviceData->ServiceList) {
      MnpServiceData = MNP_SERVICE_DATA_FROM_LINK (Entry);

      VlanData->VlanId = MnpServiceData->VlanId;
      VlanData->Priority = MnpServiceData->Priority;
      VlanData++;
    }

    return EFI_SUCCESS;
  }

  //
  // VlanId is specified, try to find it in current VLAN list
  //
  MnpServiceData = MnpFindServiceData (MnpDeviceData, *VlanId);
  if (MnpServiceData == NULL) {
    return EFI_NOT_FOUND;
  }

  VlanData = AllocateZeroPool (sizeof (EFI_VLAN_FIND_DATA));
  if (VlanData == NULL) {
    return EFI_OUT_OF_RESOURCES;
  }
  VlanData->VlanId = MnpServiceData->VlanId;
  VlanData->Priority = MnpServiceData->Priority;

  *NumberOfVlan = 1;
  *Entries = VlanData;

  return EFI_SUCCESS;
}


/**
  Remove the configured VLAN device.

  The Remove() function is used to remove the specified VLAN device.
  If the VlanId is out of the scope of (0-4094), EFI_INVALID_PARAMETER is returned.
  If specified VLAN hasn't been previously configured, EFI_NOT_FOUND is returned.

  @param[in] This                Points to the EFI_VLAN_CONFIG_PROTOCOL.
  @param[in] VlanId              Identifier (0-4094) of the VLAN to be removed.

  @retval EFI_SUCCESS            The VLAN is successfully removed.
  @retval EFI_INVALID_PARAMETER  One or more of following conditions is TRUE:
                                 - This is NULL.
                                 - VlanId  is an invalid parameter.
  @retval EFI_NOT_FOUND          The to-be-removed VLAN does not exist.

**/
EFI_STATUS
EFIAPI
VlanConfigRemove (
  IN EFI_VLAN_CONFIG_PROTOCOL    *This,
  IN UINT16                      VlanId
  )
{
  EFI_STATUS        Status;
  MNP_DEVICE_DATA   *MnpDeviceData;
  MNP_SERVICE_DATA  *MnpServiceData;
  LIST_ENTRY        *Entry;
  VLAN_TCI          *VlanVariable;
  VLAN_TCI          *VlanData;

  if ((This == NULL) || (VlanId > 4094)) {
    return EFI_INVALID_PARAMETER;
  }

  MnpDeviceData = MNP_DEVICE_DATA_FROM_THIS (This);
  if (MnpDeviceData->NumberOfVlan == 0) {
    return EFI_NOT_FOUND;
  }

  //
  // Try to find the VlanId
  //
  MnpServiceData = MnpFindServiceData (MnpDeviceData, VlanId);
  if (MnpServiceData == NULL) {
    return EFI_NOT_FOUND;
  }

  MnpDeviceData->NumberOfVlan--;

  if ((VlanId != 0) || (MnpDeviceData->NumberOfVlan != 0)) {
    //
    // If VlanId is not 0 or VlanId is 0 and it is not the last VLAN to remove,
    // destroy its MNP service data
    //
    Status = MnpDestroyServiceChild (MnpServiceData);
    if (EFI_ERROR (Status)) {
      return Status;
    }

    Status = MnpDestroyServiceData (MnpServiceData);
    if (EFI_ERROR (Status)) {
      return Status;
    }
  }

  if ((VlanId != 0) && (MnpDeviceData->NumberOfVlan == 0)) {
    //
    // This is the last VLAN to be removed, restore the default MNP service data
    //
    MnpServiceData = MnpCreateServiceData (MnpDeviceData, 0, 0);
    if (MnpServiceData == NULL) {
      return EFI_OUT_OF_RESOURCES;
    }
  }

  //
  // Update VLAN configuration variable
  //
  VlanVariable = NULL;
  if (MnpDeviceData->NumberOfVlan != 0) {
    VlanVariable = AllocatePool (MnpDeviceData->NumberOfVlan * sizeof (VLAN_TCI));
    if (VlanVariable == NULL) {
      return EFI_OUT_OF_RESOURCES;
    }

    VlanData = VlanVariable;
    NET_LIST_FOR_EACH (Entry, &MnpDeviceData->ServiceList) {
      MnpServiceData = MNP_SERVICE_DATA_FROM_LINK (Entry);

      VlanData->Bits.Vid      = MnpServiceData->VlanId;
      VlanData->Bits.Priority = MnpServiceData->Priority;
      VlanData++;
    }
  }

  Status = MnpSetVlanVariable (MnpDeviceData, MnpDeviceData->NumberOfVlan, VlanVariable);

  if (VlanVariable != NULL) {
    FreePool (VlanVariable);
  }

  return Status;
}