MdeModulePkg/Universal/Network/MnpDxe/MnpVlan.c

   1 /** @file
   2   VLAN Config Protocol implementation and VLAN packet process routine.
   3
   4 Copyright (c) 2009, 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 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   if (MnpServiceData->VlanId == 0) {
 201     *ProtocolType = TxData->ProtocolType;
 202     return ;
 203   }
 204
 205   MnpDeviceData = MnpServiceData->MnpDeviceData;
 206   SnpMode       = MnpDeviceData->Snp->Mode;
 207
 208   *ProtocolType = ETHER_TYPE_VLAN;
 209   *Length = *Length + NET_VLAN_TAG_LEN;
 210   *Packet = *Packet - NET_VLAN_TAG_LEN;
 211
 212   Tpid    = (UINT16 *) (*Packet + SnpMode->MediaHeaderSize - sizeof (*ProtocolType));
 213   VlanTci = (VLAN_TCI *) (UINTN) (Tpid + 1);
 214   if (TxData->HeaderLength != 0) {
 215     //
 216     // Media header is in packet, move DA+SA 4 bytes left
 217     //
 218     CopyMem (
 219       *Packet,
 220       *Packet + NET_VLAN_TAG_LEN,
 221       SnpMode->MediaHeaderSize - sizeof (*ProtocolType)
 222       );
 223     *Tpid = HTONS (ETHER_TYPE_VLAN);
 224   } else {
 225     //
 226     // Media header not in packet, VLAN TCI and original protocol type becomes payload
 227     //
 228     EtherType  = (UINT16 *) (UINTN) (VlanTci + 1);
 229     *EtherType = HTONS (TxData->ProtocolType);
 230   }
 231
 232   VlanTci->Bits.Vid      = MnpServiceData->VlanId;
 233   VlanTci->Bits.Cfi      = VLAN_TCI_CFI_CANONICAL_MAC;
 234   VlanTci->Bits.Priority = MnpServiceData->Priority;
 235   VlanTci->Uint16        = HTONS (VlanTci->Uint16);
 236 }
 237
 238
 239 /**
 240   Get VLAN configuration variable.
 241
 242   @param[in]       MnpDeviceData      Pointer to the MNP device context data.
 243   @param[out]      NumberOfVlan       Pointer to number of VLAN to be returned.
 244   @param[out]      VlanVariable       Pointer to the buffer to return requested
 245                                       array of VLAN_TCI.
 246
 247   @retval EFI_SUCCESS            The array of VLAN_TCI was returned in VlanVariable
 248                                  and number of VLAN was returned in NumberOfVlan.
 249   @retval EFI_NOT_FOUND          VLAN configuration variable not found.
 250   @retval EFI_OUT_OF_RESOURCES   There is not enough pool memory to store the configuration.
 251
 252 **/
 253 EFI_STATUS
 254 MnpGetVlanVariable (
 255   IN     MNP_DEVICE_DATA   *MnpDeviceData,
 256      OUT UINTN             *NumberOfVlan,
 257      OUT VLAN_TCI          **VlanVariable
 258   )
 259 {
 260   UINTN       BufferSize;
 261   EFI_STATUS  Status;
 262   VLAN_TCI    *Buffer;
 263
 264   //
 265   // Get VLAN configuration from EFI Variable
 266   //
 267   Buffer = NULL;
 268   BufferSize = 0;
 269   Status = gRT->GetVariable (
 270                   MnpDeviceData->MacString,
 271                   &gEfiVlanConfigProtocolGuid,
 272                   NULL,
 273                   &BufferSize,
 274                   NULL
 275                   );
 276   if (Status != EFI_BUFFER_TOO_SMALL) {
 277     return EFI_NOT_FOUND;
 278   }
 279
 280   //
 281   // Allocate buffer to read the variable
 282   //
 283   Buffer = AllocateZeroPool (BufferSize);
 284   if (Buffer == NULL) {
 285     return EFI_OUT_OF_RESOURCES;
 286   }
 287
 288   Status = gRT->GetVariable (
 289                   MnpDeviceData->MacString,
 290                   &gEfiVlanConfigProtocolGuid,
 291                   NULL,
 292                   &BufferSize,
 293                   Buffer
 294                   );
 295   if (EFI_ERROR (Status)) {
 296     FreePool (Buffer);
 297     return Status;
 298   }
 299
 300   *NumberOfVlan = BufferSize / sizeof (VLAN_TCI);
 301   *VlanVariable = Buffer;
 302
 303   return Status;
 304 }
 305
 306
 307 /**
 308   Set VLAN configuration variable.
 309
 310   @param[in] MnpDeviceData       Pointer to the MNP device context data.
 311   @param[in] NumberOfVlan        Number of VLAN in array VlanVariable.
 312   @param[in] VlanVariable        Pointer to array of VLAN_TCI.
 313
 314   @retval EFI_SUCCESS            The VLAN variable is successfully set.
 315   @retval EFI_OUT_OF_RESOURCES   There is not enough resource to set the configuration.
 316
 317 **/
 318 EFI_STATUS
 319 MnpSetVlanVariable (
 320   IN MNP_DEVICE_DATA             *MnpDeviceData,
 321   IN UINTN                       NumberOfVlan,
 322   IN VLAN_TCI                    *VlanVariable
 323   )
 324 {
 325   return gRT->SetVariable (
 326                 MnpDeviceData->MacString,
 327                 &gEfiVlanConfigProtocolGuid,
 328                 EFI_VARIABLE_NON_VOLATILE | EFI_VARIABLE_BOOTSERVICE_ACCESS | EFI_VARIABLE_RUNTIME_ACCESS,
 329                 NumberOfVlan * sizeof (VLAN_TCI),
 330                 VlanVariable
 331                 );
 332 }
 333
 334
 335 /**
 336   Create a VLAN device or modify the configuration parameter of an
 337   already-configured VLAN.
 338
 339   The Set() function is used to create a new VLAN device or change the VLAN
 340   configuration parameters. If the VlanId hasn't been configured in the
 341   physical Ethernet device, a new VLAN device will be created. If a VLAN with
 342   this VlanId is already configured, then related configuration will be updated
 343   as the input parameters.
 344
 345   If VlanId is zero, the VLAN device will send and receive untagged frames.
 346   Otherwise, the VLAN device will send and receive VLAN-tagged frames containing the VlanId.
 347   If VlanId is out of scope of (0-4094), EFI_INVALID_PARAMETER is returned.
 348   If Priority is out of the scope of (0-7), then EFI_INVALID_PARAMETER is returned.
 349   If there is not enough system memory to perform the registration, then
 350   EFI_OUT_OF_RESOURCES is returned.
 351
 352   @param[in] This                Points to the EFI_VLAN_CONFIG_PROTOCOL.
 353   @param[in] VlanId              A unique identifier (1-4094) of the VLAN which is being created
 354                                  or modified, or zero (0).
 355   @param[in] Priority            3 bit priority in VLAN header. Priority 0 is default value. If
 356                                  VlanId is zero (0), Priority is ignored.
 357
 358   @retval EFI_SUCCESS            The VLAN is successfully configured.
 359   @retval EFI_INVALID_PARAMETER  One or more of following conditions is TRUE:
 360                                  - This is NULL.
 361                                  - VlanId is an invalid VLAN Identifier.
 362                                  - Priority is invalid.
 363   @retval EFI_OUT_OF_RESOURCES   There is not enough system memory to perform the registration.
 364
 365 **/
 366 EFI_STATUS
 367 EFIAPI
 368 VlanConfigSet (
 369   IN EFI_VLAN_CONFIG_PROTOCOL    *This,
 370   IN UINT16                      VlanId,
 371   IN UINT8                       Priority
 372   )
 373 {
 374   EFI_STATUS        Status;
 375   MNP_DEVICE_DATA   *MnpDeviceData;
 376   MNP_SERVICE_DATA  *MnpServiceData;
 377   VLAN_TCI          *OldVariable;
 378   VLAN_TCI          *NewVariable;
 379   UINTN             NumberOfVlan;
 380   UINTN             Index;
 381   BOOLEAN           IsAdd;
 382   LIST_ENTRY        *Entry;
 383
 384   if ((This == NULL) || (VlanId > 4094) || (Priority > 7)) {
 385     return EFI_INVALID_PARAMETER;
 386   }
 387
 388   IsAdd = FALSE;
 389   MnpDeviceData = MNP_DEVICE_DATA_FROM_THIS (This);
 390   if (MnpDeviceData->NumberOfVlan == 0) {
 391     //
 392     // No existing VLAN, this is the first VLAN to add
 393     //
 394     IsAdd = TRUE;
 395     Entry = GetFirstNode (&MnpDeviceData->ServiceList);
 396     MnpServiceData = MNP_SERVICE_DATA_FROM_LINK (Entry);
 397
 398     if (VlanId != 0) {
 399       //
 400       // VlanId is not 0, need destroy the default MNP service data
 401       //
 402       Status = MnpDestroyServiceChild (MnpServiceData);
 403       if (EFI_ERROR (Status)) {
 404         return Status;
 405       }
 406
 407       Status = MnpDestroyServiceData (MnpServiceData);
 408       if (EFI_ERROR (Status)) {
 409         return Status;
 410       }
 411
 412       //
 413       // Create a new MNP service data for this VLAN
 414       //
 415       MnpServiceData = MnpCreateServiceData (MnpDeviceData, VlanId, Priority);
 416       if (MnpServiceData == NULL) {
 417         return EFI_OUT_OF_RESOURCES;
 418       }
 419     }
 420   } else {
 421     //
 422     // Try to find VlanId in existing VLAN list
 423     //
 424     MnpServiceData = MnpFindServiceData (MnpDeviceData, VlanId);
 425     if (MnpServiceData == NULL) {
 426       //
 427       // VlanId not found, create a new MNP service data
 428       //
 429       IsAdd = TRUE;
 430       MnpServiceData = MnpCreateServiceData (MnpDeviceData, VlanId, Priority);
 431       if (MnpServiceData == NULL) {
 432         return EFI_OUT_OF_RESOURCES;
 433       }
 434     }
 435   }
 436
 437   MnpServiceData->VlanId = VlanId;
 438   MnpServiceData->Priority = Priority;
 439   if (IsAdd) {
 440     MnpDeviceData->NumberOfVlan++;
 441   }
 442
 443   //
 444   // Update VLAN configuration variable
 445   //
 446   OldVariable  = NULL;
 447   NewVariable  = NULL;
 448   NumberOfVlan = 0;
 449   MnpGetVlanVariable (MnpDeviceData, &NumberOfVlan, &OldVariable);
 450
 451   if (IsAdd) {
 452     //
 453     // VLAN not exist - add
 454     //
 455     NewVariable = AllocateZeroPool ((NumberOfVlan + 1) * sizeof (VLAN_TCI));
 456     if (NewVariable == NULL) {
 457       Status = EFI_OUT_OF_RESOURCES;
 458       goto Exit;
 459     }
 460
 461     if (OldVariable != NULL) {
 462       CopyMem (NewVariable, OldVariable, NumberOfVlan * sizeof (VLAN_TCI));
 463     }
 464
 465     Index = NumberOfVlan++;
 466   } else {
 467     //
 468     // VLAN already exist - update
 469     //
 470     for (Index = 0; Index < NumberOfVlan; Index++) {
 471       if (OldVariable[Index].Bits.Vid == VlanId) {
 472         break;
 473       }
 474     }
 475     ASSERT (Index < NumberOfVlan);
 476
 477     NewVariable = OldVariable;
 478     OldVariable = NULL;
 479   }
 480
 481   NewVariable[Index].Bits.Vid      = VlanId;
 482   NewVariable[Index].Bits.Priority = Priority;
 483
 484   Status = MnpSetVlanVariable (MnpDeviceData, NumberOfVlan, NewVariable);
 485   FreePool (NewVariable);
 486
 487 Exit:
 488   if (OldVariable != NULL) {
 489     FreePool (OldVariable);
 490   }
 491
 492   return Status;
 493 }
 494
 495
 496 /**
 497   Find configuration information for specified VLAN or all configured VLANs.
 498
 499   The Find() function is used to find the configuration information for matching
 500   VLAN and allocate a buffer into which those entries are copied.
 501
 502   @param[in]  This               Points to the EFI_VLAN_CONFIG_PROTOCOL.
 503   @param[in]  VlanId             Pointer to VLAN identifier. Set to NULL to find all
 504                                  configured VLANs.
 505   @param[out] NumberOfVlan       The number of VLANs which is found by the specified criteria.
 506   @param[out] Entries            The buffer which receive the VLAN configuration.
 507
 508   @retval EFI_SUCCESS            The VLAN is successfully found.
 509   @retval EFI_INVALID_PARAMETER  One or more of following conditions is TRUE:
 510                                  - This is NULL.
 511                                  - Specified VlanId is invalid.
 512   @retval EFI_NOT_FOUND          No matching VLAN is found.
 513
 514 **/
 515 EFI_STATUS
 516 EFIAPI
 517 VlanConfigFind (
 518   IN     EFI_VLAN_CONFIG_PROTOCOL    *This,
 519   IN     UINT16                      *VlanId OPTIONAL,
 520      OUT UINT16                      *NumberOfVlan,
 521      OUT EFI_VLAN_FIND_DATA          **Entries
 522   )
 523 {
 524   MNP_DEVICE_DATA     *MnpDeviceData;
 525   MNP_SERVICE_DATA    *MnpServiceData;
 526   LIST_ENTRY          *Entry;
 527   EFI_VLAN_FIND_DATA  *VlanData;
 528
 529   if ((This == NULL) || (VlanId != NULL && *VlanId > 4094) || (NumberOfVlan == NULL) || (Entries == NULL)) {
 530     return EFI_INVALID_PARAMETER;
 531   }
 532
 533   *NumberOfVlan = 0;
 534   *Entries      = NULL;
 535
 536   MnpDeviceData = MNP_DEVICE_DATA_FROM_THIS (This);
 537   if (MnpDeviceData->NumberOfVlan == 0) {
 538     return EFI_NOT_FOUND;
 539   }
 540
 541   if (VlanId == NULL) {
 542     //
 543     // Return all current VLAN configuration
 544     //
 545     *NumberOfVlan = (UINT16) MnpDeviceData->NumberOfVlan;
 546     VlanData = AllocateZeroPool (*NumberOfVlan * sizeof (EFI_VLAN_FIND_DATA));
 547     if (VlanData == NULL) {
 548       return EFI_OUT_OF_RESOURCES;
 549     }
 550
 551     *Entries = VlanData;
 552     NET_LIST_FOR_EACH (Entry, &MnpDeviceData->ServiceList) {
 553       MnpServiceData = MNP_SERVICE_DATA_FROM_LINK (Entry);
 554
 555       VlanData->VlanId = MnpServiceData->VlanId;
 556       VlanData->Priority = MnpServiceData->Priority;
 557       VlanData++;
 558     }
 559
 560     return EFI_SUCCESS;
 561   }
 562
 563   //
 564   // VlanId is specified, try to find it in current VLAN list
 565   //
 566   MnpServiceData = MnpFindServiceData (MnpDeviceData, *VlanId);
 567   if (MnpServiceData == NULL) {
 568     return EFI_NOT_FOUND;
 569   }
 570
 571   VlanData = AllocateZeroPool (sizeof (EFI_VLAN_FIND_DATA));
 572   if (VlanData == NULL) {
 573     return EFI_OUT_OF_RESOURCES;
 574   }
 575   VlanData->VlanId = MnpServiceData->VlanId;
 576   VlanData->Priority = MnpServiceData->Priority;
 577
 578   *NumberOfVlan = 1;
 579   *Entries = VlanData;
 580
 581   return EFI_SUCCESS;
 582 }
 583
 584
 585 /**
 586   Remove the configured VLAN device.
 587
 588   The Remove() function is used to remove the specified VLAN device.
 589   If the VlanId is out of the scope of (0-4094), EFI_INVALID_PARAMETER is returned.
 590   If specified VLAN hasn't been previously configured, EFI_NOT_FOUND is returned.
 591
 592   @param[in] This                Points to the EFI_VLAN_CONFIG_PROTOCOL.
 593   @param[in] VlanId              Identifier (0-4094) of the VLAN to be removed.
 594
 595   @retval EFI_SUCCESS            The VLAN is successfully removed.
 596   @retval EFI_INVALID_PARAMETER  One or more of following conditions is TRUE:
 597                                  - This is NULL.
 598                                  - VlanId  is an invalid parameter.
 599   @retval EFI_NOT_FOUND          The to-be-removed VLAN does not exist.
 600
 601 **/
 602 EFI_STATUS
 603 EFIAPI
 604 VlanConfigRemove (
 605   IN EFI_VLAN_CONFIG_PROTOCOL    *This,
 606   IN UINT16                      VlanId
 607   )
 608 {
 609   EFI_STATUS        Status;
 610   MNP_DEVICE_DATA   *MnpDeviceData;
 611   MNP_SERVICE_DATA  *MnpServiceData;
 612   LIST_ENTRY        *Entry;
 613   VLAN_TCI          *VlanVariable;
 614   VLAN_TCI          *VlanData;
 615
 616   if ((This == NULL) || (VlanId > 4094)) {
 617     return EFI_INVALID_PARAMETER;
 618   }
 619
 620   MnpDeviceData = MNP_DEVICE_DATA_FROM_THIS (This);
 621   if (MnpDeviceData->NumberOfVlan == 0) {
 622     return EFI_NOT_FOUND;
 623   }
 624
 625   //
 626   // Try to find the VlanId
 627   //
 628   MnpServiceData = MnpFindServiceData (MnpDeviceData, VlanId);
 629   if (MnpServiceData == NULL) {
 630     return EFI_NOT_FOUND;
 631   }
 632
 633   MnpDeviceData->NumberOfVlan--;
 634
 635   if ((VlanId != 0) || (MnpDeviceData->NumberOfVlan != 0)) {
 636     //
 637     // If VlanId is not 0 or VlanId is 0 and it is not the last VLAN to remove,
 638     // destroy its MNP service data
 639     //
 640     Status = MnpDestroyServiceChild (MnpServiceData);
 641     if (EFI_ERROR (Status)) {
 642       return Status;
 643     }
 644
 645     Status = MnpDestroyServiceData (MnpServiceData);
 646     if (EFI_ERROR (Status)) {
 647       return Status;
 648     }
 649   }
 650
 651   if ((VlanId != 0) && (MnpDeviceData->NumberOfVlan == 0)) {
 652     //
 653     // This is the last VLAN to be removed, restore the default MNP service data
 654     //
 655     MnpServiceData = MnpCreateServiceData (MnpDeviceData, 0, 0);
 656     if (MnpServiceData == NULL) {
 657       return EFI_OUT_OF_RESOURCES;
 658     }
 659   }
 660
 661   //
 662   // Update VLAN configuration variable
 663   //
 664   VlanVariable = NULL;
 665   if (MnpDeviceData->NumberOfVlan != 0) {
 666     VlanVariable = AllocatePool (MnpDeviceData->NumberOfVlan * sizeof (VLAN_TCI));
 667     if (VlanVariable == NULL) {
 668       return EFI_OUT_OF_RESOURCES;
 669     }
 670
 671     VlanData = VlanVariable;
 672     NET_LIST_FOR_EACH (Entry, &MnpDeviceData->ServiceList) {
 673       MnpServiceData = MNP_SERVICE_DATA_FROM_LINK (Entry);
 674
 675       VlanData->Bits.Vid      = MnpServiceData->VlanId;
 676       VlanData->Bits.Priority = MnpServiceData->Priority;
 677       VlanData++;
 678     }
 679   }
 680
 681   Status = MnpSetVlanVariable (MnpDeviceData, MnpDeviceData->NumberOfVlan, VlanVariable);
 682
 683   if (VlanVariable != NULL) {
 684     FreePool (VlanVariable);
 685   }
 686
 687   return Status;
 688 }