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