MdeModulePkg/Universal/Network/MnpDxe/MnpVlan.c

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