summaryrefslogtreecommitdiffstats
path: root/NetworkPkg/MnpDxe/MnpVlan.c
blob: cc3637a6ed4101aa13c095c71702044327d83848 (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
/** @file
  VLAN Config Protocol implementation and VLAN packet process routine.

Copyright (c) 2009 - 2018, Intel Corporation. All rights reserved.<BR>
SPDX-License-Identifier: BSD-2-Clause-Patent

**/

#include "MnpImpl.h"
#include "MnpVlan.h"

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

EFI_VLAN_CONFIG_PROTOCOL  mVlanConfigProtocolTemplate = {
  VlanConfigSet,
  VlanConfigFind,
  VlanConfigRemove
};

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

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

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

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

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

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

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

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

  return ChildHandle;
}

/**
  Remove VLAN tag from a packet.

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

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

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

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

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

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

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

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

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

  return TRUE;
}

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

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

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

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

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

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

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

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

  @param[in]      MnpDeviceData      Pointer to the MNP device context data.
  @param[in]      Buffer             Pointer to the buffer contains the array of VLAN_TCI.
  @param[in]      NumberOfVlan       Pointer to number of VLAN.
  @param[out]     NewNumberOfVlan    Pointer to number of unique VLAN.

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

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

  Count              = 0;
  FoundDuplicateItem = FALSE;
  Status             = EFI_SUCCESS;

  for (Index = 0; Index < NumberOfVlan; Index++) {
    for (Index2 = Index + 1; Index2 < NumberOfVlan; Index2++) {
      if (Buffer[Index].Bits.Vid == Buffer[Index2].Bits.Vid) {
        FoundDuplicateItem = TRUE;
        Count++;
        break;
      }
    }

    if (FoundDuplicateItem) {
      for (Index2 = Index +1; Index2 < NumberOfVlan; Index++, Index2++) {
        CopyMem (Buffer + Index, Buffer + Index2, sizeof (VLAN_TCI));
      }
    }

    FoundDuplicateItem = FALSE;
  }

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

  return Status;
}

/**
  Get VLAN configuration variable.

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

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

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

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

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

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

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

  return Status;
}

/**
  Set VLAN configuration variable.

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

    ASSERT (Index < NumberOfVlan);

    NewVariable = OldVariable;
    OldVariable = NULL;
  }

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

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

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

  return Status;
}

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

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

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

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

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

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

  *NumberOfVlan = 0;
  *Entries      = NULL;

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

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

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

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

    return EFI_SUCCESS;
  }

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

  VlanData = AllocateZeroPool (sizeof (EFI_VLAN_FIND_DATA));
  if (VlanData == NULL) {
    return EFI_OUT_OF_RESOURCES;
  }

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

  *NumberOfVlan = 1;
  *Entries      = VlanData;

  return EFI_SUCCESS;
}

/**
  Remove the configured VLAN device.

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

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

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

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

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

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

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

  MnpDeviceData->NumberOfVlan--;

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

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

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

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

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

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

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

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

  return Status;
}