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