Upstream/CloverBootloader
1
0
Fork 0
mirror of https://github.com/CloverHackyColor/CloverBootloader.git synced 2024-12-11 14:28:08 +01:00
Code Issues Packages Projects Releases Wiki Activity
e3a2003995
CloverBootloader/NetworkPkg/MnpDxe/MnpVlan.c

732 lines
22 KiB
C
Raw Blame History

/** @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;
}
Reference in New Issue View Git Blame Copy Permalink