MdeModulePkg/Universal/Network/MnpDxe/MnpVlan.c

   1 /** @file
   2   VLAN Config Protocol implementation and VLAN packet process routine.
   3
   4 Copyright (c) 2009 - 2013, Intel Corporation. All rights reserved.<BR>
   5 This program and the accompanying materials
   6 are licensed and made available under the terms and conditions
   7 of the BSD License which accompanies this distribution.  The full
   8 text of the license may be found at<BR>
   9 http://opensource.org/licenses/bsd-license.php
  10
  11 THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
  12 WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
  13
  14 **/
  15
  16 #include "MnpImpl.h"
  17 #include "MnpVlan.h"
  18
  19 VLAN_DEVICE_PATH          mVlanDevicePathTemplate = {
  20   {
  21     MESSAGING_DEVICE_PATH,
  22     MSG_VLAN_DP,
  23     {
  24       (UINT8) (sizeof (VLAN_DEVICE_PATH)),
  25       (UINT8) ((sizeof (VLAN_DEVICE_PATH)) >> 8)
  26     }
  27   },
  28   0
  29 };
  30
  31 EFI_VLAN_CONFIG_PROTOCOL  mVlanConfigProtocolTemplate = {
  32   VlanConfigSet,
  33   VlanConfigFind,
  34   VlanConfigRemove
  35 };
  36
  37
  38 /**
  39   Create a child handle for the VLAN ID.
  40
  41   @param[in]       ImageHandle        The driver image handle.
  42   @param[in]       ControllerHandle   Handle of device to bind driver to.
  43   @param[in]       VlanId             The VLAN ID.
  44   @param[out]      Devicepath         Pointer to returned device path for child handle.
  45
  46   @return The handle of VLAN child or NULL if failed to create VLAN child.
  47
  48 **/
  49 EFI_HANDLE
  50 MnpCreateVlanChild (
  51   IN     EFI_HANDLE                  ImageHandle,
  52   IN     EFI_HANDLE                  ControllerHandle,
  53   IN     UINT16                      VlanId,
  54      OUT EFI_DEVICE_PATH_PROTOCOL    **Devicepath OPTIONAL
  55   )
  56 {
  57   EFI_HANDLE                ChildHandle;
  58   VLAN_DEVICE_PATH          VlanNode;
  59   EFI_DEVICE_PATH_PROTOCOL  *ParentDevicePath;
  60   EFI_DEVICE_PATH_PROTOCOL  *VlanDevicePath;
  61   EFI_STATUS                Status;
  62
  63   //
  64   // Try to get parent device path
  65   //
  66   Status = gBS->OpenProtocol (
  67                   ControllerHandle,
  68                   &gEfiDevicePathProtocolGuid,
  69                   (VOID **) &ParentDevicePath,
  70                   ImageHandle,
  71                   ControllerHandle,
  72                   EFI_OPEN_PROTOCOL_GET_PROTOCOL
  73                   );
  74   if (EFI_ERROR (Status)) {
  75     return NULL;
  76   }
  77
  78   //
  79   // Construct device path for child handle: MAC + VLAN
  80   //
  81   CopyMem (&VlanNode, &mVlanDevicePathTemplate, sizeof (VLAN_DEVICE_PATH));
  82   VlanNode.VlanId = VlanId;
  83   VlanDevicePath = AppendDevicePathNode (
  84                      ParentDevicePath,
  85                      (EFI_DEVICE_PATH_PROTOCOL *) &VlanNode
  86                      );
  87   if (VlanDevicePath == NULL) {
  88     return NULL;
  89   }
  90
  91   //
  92   // Create child VLAN handle by installing DevicePath protocol
  93   //
  94   ChildHandle = NULL;
  95   Status = gBS->InstallMultipleProtocolInterfaces (
  96                   &ChildHandle,
  97                   &gEfiDevicePathProtocolGuid,
  98                   VlanDevicePath,
  99                   NULL
 100                   );
 101   if (EFI_ERROR (Status)) {
 102     FreePool (VlanDevicePath);
 103     return NULL;
 104   }
 105
 106   if (Devicepath != NULL) {
 107     *Devicepath = VlanDevicePath;
 108   }
 109
 110   return ChildHandle;
 111 }
 112
 113 /**
 114   Remove VLAN tag from a packet.
 115
 116   @param[in, out]  MnpDeviceData      Pointer to the mnp device context data.
 117   @param[in, out]  Nbuf               Pointer to the NET_BUF to remove VLAN tag.
 118   @param[out]      VlanId             Pointer to the returned VLAN ID.
 119
 120   @retval TRUE             VLAN tag is removed from this packet.
 121   @retval FALSE            There is no VLAN tag in this packet.
 122
 123 **/
 124 BOOLEAN
 125 MnpRemoveVlanTag (
 126   IN OUT MNP_DEVICE_DATA   *MnpDeviceData,
 127   IN OUT NET_BUF           *Nbuf,
 128      OUT UINT16            *VlanId
 129   )
 130 {
 131   UINT8     *Packet;
 132   UINTN     ProtocolOffset;
 133   UINT16    ProtocolType;
 134   VLAN_TCI  VlanTag;
 135
 136   ProtocolOffset = MnpDeviceData->Snp->Mode->HwAddressSize * 2;
 137
 138   //
 139   // Get the packet buffer.
 140   //
 141   Packet = NetbufGetByte (Nbuf, 0, NULL);
 142   ASSERT (Packet != NULL);
 143
 144   //
 145   // Check whether this is VLAN tagged frame by Ether Type
 146   //
 147   *VlanId      = 0;
 148   ProtocolType = NTOHS (*(UINT16 *) (Packet + ProtocolOffset));
 149   if (ProtocolType != ETHER_TYPE_VLAN) {
 150     //
 151     // Not a VLAN tagged frame
 152     //
 153     return FALSE;
 154   }
 155
 156   VlanTag.Uint16 = NTOHS (*(UINT16 *) (Packet + ProtocolOffset + sizeof (ProtocolType)));
 157   *VlanId = VlanTag.Bits.Vid;
 158
 159   //
 160   // Move hardware address (DA + SA) 4 bytes right to override VLAN tag
 161   //
 162   CopyMem (Packet + NET_VLAN_TAG_LEN, Packet, ProtocolOffset);
 163
 164   //
 165   // Remove VLAN tag from the Nbuf
 166   //
 167   NetbufTrim (Nbuf, NET_VLAN_TAG_LEN, NET_BUF_HEAD);
 168
 169   return TRUE;
 170 }
 171
 172
 173 /**
 174   Build the vlan packet to transmit from the TxData passed in.
 175
 176   @param  MnpServiceData         Pointer to the mnp service context data.
 177   @param  TxData                 Pointer to the transmit data containing the
 178                                  information to build the packet.
 179   @param  ProtocolType           Pointer to the Ethernet protocol type.
 180   @param  Packet                 Pointer to record the address of the packet.
 181   @param  Length                 Pointer to a UINT32 variable used to record the
 182                                  packet's length.
 183
 184 **/
 185 VOID
 186 MnpInsertVlanTag (
 187   IN     MNP_SERVICE_DATA                    *MnpServiceData,
 188   IN     EFI_MANAGED_NETWORK_TRANSMIT_DATA   *TxData,
 189      OUT UINT16                              *ProtocolType,
 190   IN OUT UINT8                               **Packet,
 191   IN OUT UINT32                              *Length
 192   )
 193 {
 194   VLAN_TCI                *VlanTci;
 195   UINT16                  *Tpid;
 196   UINT16                  *EtherType;
 197   MNP_DEVICE_DATA         *MnpDeviceData;
 198   EFI_SIMPLE_NETWORK_MODE *SnpMode;
 199
 200   MnpDeviceData = MnpServiceData->MnpDeviceData;
 201   SnpMode       = MnpDeviceData->Snp->Mode;
 202
 203   *ProtocolType = ETHER_TYPE_VLAN;
 204   *Length = *Length + NET_VLAN_TAG_LEN;
 205   *Packet = *Packet - NET_VLAN_TAG_LEN;
 206
 207   Tpid    = (UINT16 *) (*Packet + SnpMode->MediaHeaderSize - sizeof (*ProtocolType));
 208   VlanTci = (VLAN_TCI *) (UINTN) (Tpid + 1);
 209   if (TxData->HeaderLength != 0) {
 210     //
 211     // Media header is in packet, move DA+SA 4 bytes left
 212     //
 213     CopyMem (
 214       *Packet,
 215       *Packet + NET_VLAN_TAG_LEN,
 216       SnpMode->MediaHeaderSize - sizeof (*ProtocolType)
 217       );
 218     *Tpid = HTONS (ETHER_TYPE_VLAN);
 219   } else {
 220     //
 221     // Media header not in packet, VLAN TCI and original protocol type becomes payload
 222     //
 223     EtherType  = (UINT16 *) (UINTN) (VlanTci + 1);
 224     *EtherType = HTONS (TxData->ProtocolType);
 225   }
 226
 227   VlanTci->Bits.Vid      = MnpServiceData->VlanId;
 228   VlanTci->Bits.Cfi      = VLAN_TCI_CFI_CANONICAL_MAC;
 229   VlanTci->Bits.Priority = MnpServiceData->Priority;
 230   VlanTci->Uint16        = HTONS (VlanTci->Uint16);
 231 }
 232
 233
 234 /**
 235   Get VLAN configuration variable.
 236
 237   @param[in]       MnpDeviceData      Pointer to the MNP device context data.
 238   @param[out]      NumberOfVlan       Pointer to number of VLAN to be returned.
 239   @param[out]      VlanVariable       Pointer to the buffer to return requested
 240                                       array of VLAN_TCI.
 241
 242   @retval EFI_SUCCESS            The array of VLAN_TCI was returned in VlanVariable
 243                                  and number of VLAN was returned in NumberOfVlan.
 244   @retval EFI_NOT_FOUND          VLAN configuration variable not found.
 245   @retval EFI_OUT_OF_RESOURCES   There is not enough pool memory to store the configuration.
 246
 247 **/
 248 EFI_STATUS
 249 MnpGetVlanVariable (
 250   IN     MNP_DEVICE_DATA   *MnpDeviceData,
 251      OUT UINTN             *NumberOfVlan,
 252      OUT VLAN_TCI          **VlanVariable
 253   )
 254 {
 255   UINTN       BufferSize;
 256   EFI_STATUS  Status;
 257   VLAN_TCI    *Buffer;
 258
 259   //
 260   // Get VLAN configuration from EFI Variable
 261   //
 262   Buffer = NULL;
 263   BufferSize = 0;
 264   Status = gRT->GetVariable (
 265                   MnpDeviceData->MacString,
 266                   &gEfiVlanConfigProtocolGuid,
 267                   NULL,
 268                   &BufferSize,
 269                   NULL
 270                   );
 271   if (Status != EFI_BUFFER_TOO_SMALL) {
 272     return EFI_NOT_FOUND;
 273   }
 274
 275   //
 276   // Allocate buffer to read the variable
 277   //
 278   Buffer = AllocateZeroPool (BufferSize);
 279   if (Buffer == NULL) {
 280     return EFI_OUT_OF_RESOURCES;
 281   }
 282
 283   Status = gRT->GetVariable (
 284                   MnpDeviceData->MacString,
 285                   &gEfiVlanConfigProtocolGuid,
 286                   NULL,
 287                   &BufferSize,
 288                   Buffer
 289                   );
 290   if (EFI_ERROR (Status)) {
 291     FreePool (Buffer);
 292     return Status;
 293   }
 294
 295   *NumberOfVlan = BufferSize / sizeof (VLAN_TCI);
 296   *VlanVariable = Buffer;
 297
 298   return Status;
 299 }
 300
 301
 302 /**
 303   Set VLAN configuration variable.
 304
 305   @param[in] MnpDeviceData       Pointer to the MNP device context data.
 306   @param[in] NumberOfVlan        Number of VLAN in array VlanVariable.
 307   @param[in] VlanVariable        Pointer to array of VLAN_TCI.
 308
 309   @retval EFI_SUCCESS            The VLAN variable is successfully set.
 310   @retval EFI_OUT_OF_RESOURCES   There is not enough resource to set the configuration.
 311
 312 **/
 313 EFI_STATUS
 314 MnpSetVlanVariable (
 315   IN MNP_DEVICE_DATA             *MnpDeviceData,
 316   IN UINTN                       NumberOfVlan,
 317   IN VLAN_TCI                    *VlanVariable
 318   )
 319 {
 320   return gRT->SetVariable (
 321                 MnpDeviceData->MacString,
 322                 &gEfiVlanConfigProtocolGuid,
 323                 EFI_VARIABLE_NON_VOLATILE | EFI_VARIABLE_BOOTSERVICE_ACCESS | EFI_VARIABLE_RUNTIME_ACCESS,
 324                 NumberOfVlan * sizeof (VLAN_TCI),
 325                 VlanVariable
 326                 );
 327 }
 328
 329
 330 /**
 331   Create a VLAN device or modify the configuration parameter of an
 332   already-configured VLAN.
 333
 334   The Set() function is used to create a new VLAN device or change the VLAN
 335   configuration parameters. If the VlanId hasn't been configured in the
 336   physical Ethernet device, a new VLAN device will be created. If a VLAN with
 337   this VlanId is already configured, then related configuration will be updated
 338   as the input parameters.
 339
 340   If VlanId is zero, the VLAN device will send and receive untagged frames.
 341   Otherwise, the VLAN device will send and receive VLAN-tagged frames containing the VlanId.
 342   If VlanId is out of scope of (0-4094), EFI_INVALID_PARAMETER is returned.
 343   If Priority is out of the scope of (0-7), then EFI_INVALID_PARAMETER is returned.
 344   If there is not enough system memory to perform the registration, then
 345   EFI_OUT_OF_RESOURCES is returned.
 346
 347   @param[in] This                Points to the EFI_VLAN_CONFIG_PROTOCOL.
 348   @param[in] VlanId              A unique identifier (1-4094) of the VLAN which is being created
 349                                  or modified, or zero (0).
 350   @param[in] Priority            3 bit priority in VLAN header. Priority 0 is default value. If
 351                                  VlanId is zero (0), Priority is ignored.
 352
 353   @retval EFI_SUCCESS            The VLAN is successfully configured.
 354   @retval EFI_INVALID_PARAMETER  One or more of following conditions is TRUE:
 355                                  - This is NULL.
 356                                  - VlanId is an invalid VLAN Identifier.
 357                                  - Priority is invalid.
 358   @retval EFI_OUT_OF_RESOURCES   There is not enough system memory to perform the registration.
 359
 360 **/
 361 EFI_STATUS
 362 EFIAPI
 363 VlanConfigSet (
 364   IN EFI_VLAN_CONFIG_PROTOCOL    *This,
 365   IN UINT16                      VlanId,
 366   IN UINT8                       Priority
 367   )
 368 {
 369   EFI_STATUS        Status;
 370   MNP_DEVICE_DATA   *MnpDeviceData;
 371   MNP_SERVICE_DATA  *MnpServiceData;
 372   VLAN_TCI          *OldVariable;
 373   VLAN_TCI          *NewVariable;
 374   UINTN             NumberOfVlan;
 375   UINTN             Index;
 376   BOOLEAN           IsAdd;
 377   LIST_ENTRY        *Entry;
 378
 379   if ((This == NULL) || (VlanId > 4094) || (Priority > 7)) {
 380     return EFI_INVALID_PARAMETER;
 381   }
 382
 383   IsAdd = FALSE;
 384   MnpDeviceData = MNP_DEVICE_DATA_FROM_THIS (This);
 385   if (MnpDeviceData->NumberOfVlan == 0) {
 386     //
 387     // No existing VLAN, this is the first VLAN to add
 388     //
 389     IsAdd = TRUE;
 390     Entry = GetFirstNode (&MnpDeviceData->ServiceList);
 391     MnpServiceData = MNP_SERVICE_DATA_FROM_LINK (Entry);
 392
 393     if (VlanId != 0) {
 394       //
 395       // VlanId is not 0, need destroy the default MNP service data
 396       //
 397       Status = MnpDestroyServiceChild (MnpServiceData);
 398       if (EFI_ERROR (Status)) {
 399         return Status;
 400       }
 401
 402       Status = MnpDestroyServiceData (MnpServiceData);
 403       if (EFI_ERROR (Status)) {
 404         return Status;
 405       }
 406
 407       //
 408       // Create a new MNP service data for this VLAN
 409       //
 410       MnpServiceData = MnpCreateServiceData (MnpDeviceData, VlanId, Priority);
 411       if (MnpServiceData == NULL) {
 412         return EFI_OUT_OF_RESOURCES;
 413       }
 414     }
 415   } else {
 416     //
 417     // Try to find VlanId in existing VLAN list
 418     //
 419     MnpServiceData = MnpFindServiceData (MnpDeviceData, VlanId);
 420     if (MnpServiceData == NULL) {
 421       //
 422       // VlanId not found, create a new MNP service data
 423       //
 424       IsAdd = TRUE;
 425       MnpServiceData = MnpCreateServiceData (MnpDeviceData, VlanId, Priority);
 426       if (MnpServiceData == NULL) {
 427         return EFI_OUT_OF_RESOURCES;
 428       }
 429     }
 430   }
 431
 432   MnpServiceData->VlanId = VlanId;
 433   MnpServiceData->Priority = Priority;
 434   if (IsAdd) {
 435     MnpDeviceData->NumberOfVlan++;
 436   }
 437
 438   //
 439   // Update VLAN configuration variable
 440   //
 441   OldVariable  = NULL;
 442   NewVariable  = NULL;
 443   NumberOfVlan = 0;
 444   MnpGetVlanVariable (MnpDeviceData, &NumberOfVlan, &OldVariable);
 445
 446   if (IsAdd) {
 447     //
 448     // VLAN not exist - add
 449     //
 450     NewVariable = AllocateZeroPool ((NumberOfVlan + 1) * sizeof (VLAN_TCI));
 451     if (NewVariable == NULL) {
 452       Status = EFI_OUT_OF_RESOURCES;
 453       goto Exit;
 454     }
 455
 456     if (OldVariable != NULL) {
 457       CopyMem (NewVariable, OldVariable, NumberOfVlan * sizeof (VLAN_TCI));
 458     }
 459
 460     Index = NumberOfVlan++;
 461   } else {
 462     //
 463     // VLAN already exist - update
 464     //
 465     for (Index = 0; Index < NumberOfVlan; Index++) {
 466       if (OldVariable[Index].Bits.Vid == VlanId) {
 467         break;
 468       }
 469     }
 470     ASSERT (Index < NumberOfVlan);
 471
 472     NewVariable = OldVariable;
 473     OldVariable = NULL;
 474   }
 475
 476   NewVariable[Index].Bits.Vid      = VlanId;
 477   NewVariable[Index].Bits.Priority = Priority;
 478
 479   Status = MnpSetVlanVariable (MnpDeviceData, NumberOfVlan, NewVariable);
 480   FreePool (NewVariable);
 481
 482 Exit:
 483   if (OldVariable != NULL) {
 484     FreePool (OldVariable);
 485   }
 486
 487   return Status;
 488 }
 489
 490
 491 /**
 492   Find configuration information for specified VLAN or all configured VLANs.
 493
 494   The Find() function is used to find the configuration information for matching
 495   VLAN and allocate a buffer into which those entries are copied.
 496
 497   @param[in]  This               Points to the EFI_VLAN_CONFIG_PROTOCOL.
 498   @param[in]  VlanId             Pointer to VLAN identifier. Set to NULL to find all
 499                                  configured VLANs.
 500   @param[out] NumberOfVlan       The number of VLANs which is found by the specified criteria.
 501   @param[out] Entries            The buffer which receive the VLAN configuration.
 502
 503   @retval EFI_SUCCESS            The VLAN is successfully found.
 504   @retval EFI_INVALID_PARAMETER  One or more of following conditions is TRUE:
 505                                  - This is NULL.
 506                                  - Specified VlanId is invalid.
 507   @retval EFI_NOT_FOUND          No matching VLAN is found.
 508
 509 **/
 510 EFI_STATUS
 511 EFIAPI
 512 VlanConfigFind (
 513   IN     EFI_VLAN_CONFIG_PROTOCOL    *This,
 514   IN     UINT16                      *VlanId OPTIONAL,
 515      OUT UINT16                      *NumberOfVlan,
 516      OUT EFI_VLAN_FIND_DATA          **Entries
 517   )
 518 {
 519   MNP_DEVICE_DATA     *MnpDeviceData;
 520   MNP_SERVICE_DATA    *MnpServiceData;
 521   LIST_ENTRY          *Entry;
 522   EFI_VLAN_FIND_DATA  *VlanData;
 523
 524   if ((This == NULL) || (VlanId != NULL && *VlanId > 4094) || (NumberOfVlan == NULL) || (Entries == NULL)) {
 525     return EFI_INVALID_PARAMETER;
 526   }
 527
 528   *NumberOfVlan = 0;
 529   *Entries      = NULL;
 530
 531   MnpDeviceData = MNP_DEVICE_DATA_FROM_THIS (This);
 532   if (MnpDeviceData->NumberOfVlan == 0) {
 533     return EFI_NOT_FOUND;
 534   }
 535
 536   if (VlanId == NULL) {
 537     //
 538     // Return all current VLAN configuration
 539     //
 540     *NumberOfVlan = (UINT16) MnpDeviceData->NumberOfVlan;
 541     VlanData = AllocateZeroPool (*NumberOfVlan * sizeof (EFI_VLAN_FIND_DATA));
 542     if (VlanData == NULL) {
 543       return EFI_OUT_OF_RESOURCES;
 544     }
 545
 546     *Entries = VlanData;
 547     NET_LIST_FOR_EACH (Entry, &MnpDeviceData->ServiceList) {
 548       MnpServiceData = MNP_SERVICE_DATA_FROM_LINK (Entry);
 549
 550       VlanData->VlanId = MnpServiceData->VlanId;
 551       VlanData->Priority = MnpServiceData->Priority;
 552       VlanData++;
 553     }
 554
 555     return EFI_SUCCESS;
 556   }
 557
 558   //
 559   // VlanId is specified, try to find it in current VLAN list
 560   //
 561   MnpServiceData = MnpFindServiceData (MnpDeviceData, *VlanId);
 562   if (MnpServiceData == NULL) {
 563     return EFI_NOT_FOUND;
 564   }
 565
 566   VlanData = AllocateZeroPool (sizeof (EFI_VLAN_FIND_DATA));
 567   if (VlanData == NULL) {
 568     return EFI_OUT_OF_RESOURCES;
 569   }
 570   VlanData->VlanId = MnpServiceData->VlanId;
 571   VlanData->Priority = MnpServiceData->Priority;
 572
 573   *NumberOfVlan = 1;
 574   *Entries = VlanData;
 575
 576   return EFI_SUCCESS;
 577 }
 578
 579
 580 /**
 581   Remove the configured VLAN device.
 582
 583   The Remove() function is used to remove the specified VLAN device.
 584   If the VlanId is out of the scope of (0-4094), EFI_INVALID_PARAMETER is returned.
 585   If specified VLAN hasn't been previously configured, EFI_NOT_FOUND is returned.
 586
 587   @param[in] This                Points to the EFI_VLAN_CONFIG_PROTOCOL.
 588   @param[in] VlanId              Identifier (0-4094) of the VLAN to be removed.
 589
 590   @retval EFI_SUCCESS            The VLAN is successfully removed.
 591   @retval EFI_INVALID_PARAMETER  One or more of following conditions is TRUE:
 592                                  - This is NULL.
 593                                  - VlanId  is an invalid parameter.
 594   @retval EFI_NOT_FOUND          The to-be-removed VLAN does not exist.
 595
 596 **/
 597 EFI_STATUS
 598 EFIAPI
 599 VlanConfigRemove (
 600   IN EFI_VLAN_CONFIG_PROTOCOL    *This,
 601   IN UINT16                      VlanId
 602   )
 603 {
 604   EFI_STATUS        Status;
 605   MNP_DEVICE_DATA   *MnpDeviceData;
 606   MNP_SERVICE_DATA  *MnpServiceData;
 607   LIST_ENTRY        *Entry;
 608   VLAN_TCI          *VlanVariable;
 609   VLAN_TCI          *VlanData;
 610
 611   if ((This == NULL) || (VlanId > 4094)) {
 612     return EFI_INVALID_PARAMETER;
 613   }
 614
 615   MnpDeviceData = MNP_DEVICE_DATA_FROM_THIS (This);
 616   if (MnpDeviceData->NumberOfVlan == 0) {
 617     return EFI_NOT_FOUND;
 618   }
 619
 620   //
 621   // Try to find the VlanId
 622   //
 623   MnpServiceData = MnpFindServiceData (MnpDeviceData, VlanId);
 624   if (MnpServiceData == NULL) {
 625     return EFI_NOT_FOUND;
 626   }
 627
 628   MnpDeviceData->NumberOfVlan--;
 629
 630   if ((VlanId != 0) || (MnpDeviceData->NumberOfVlan != 0)) {
 631     //
 632     // If VlanId is not 0 or VlanId is 0 and it is not the last VLAN to remove,
 633     // destroy its MNP service data
 634     //
 635     Status = MnpDestroyServiceChild (MnpServiceData);
 636     if (EFI_ERROR (Status)) {
 637       return Status;
 638     }
 639
 640     Status = MnpDestroyServiceData (MnpServiceData);
 641     if (EFI_ERROR (Status)) {
 642       return Status;
 643     }
 644   }
 645
 646   if ((VlanId != 0) && (MnpDeviceData->NumberOfVlan == 0)) {
 647     //
 648     // This is the last VLAN to be removed, restore the default MNP service data
 649     //
 650     MnpServiceData = MnpCreateServiceData (MnpDeviceData, 0, 0);
 651     if (MnpServiceData == NULL) {
 652       return EFI_OUT_OF_RESOURCES;
 653     }
 654   }
 655
 656   //
 657   // Update VLAN configuration variable
 658   //
 659   VlanVariable = NULL;
 660   if (MnpDeviceData->NumberOfVlan != 0) {
 661     VlanVariable = AllocatePool (MnpDeviceData->NumberOfVlan * sizeof (VLAN_TCI));
 662     if (VlanVariable == NULL) {
 663       return EFI_OUT_OF_RESOURCES;
 664     }
 665
 666     VlanData = VlanVariable;
 667     NET_LIST_FOR_EACH (Entry, &MnpDeviceData->ServiceList) {
 668       MnpServiceData = MNP_SERVICE_DATA_FROM_LINK (Entry);
 669
 670       VlanData->Bits.Vid      = MnpServiceData->VlanId;
 671       VlanData->Bits.Priority = MnpServiceData->Priority;
 672       VlanData++;
 673     }
 674   }
 675
 676   Status = MnpSetVlanVariable (MnpDeviceData, MnpDeviceData->NumberOfVlan, VlanVariable);
 677
 678   if (VlanVariable != NULL) {
 679     FreePool (VlanVariable);
 680   }
 681
 682   return Status;
 683 }