123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731 |
- /** @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;
- }
|