MnpVlan.c

Go to the documentation of this file.

 
#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
};
 
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;
}
 
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;
}
 
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);
}
 
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;
}
 
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;
}
 
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
                );
}
 
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;
}
 
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;
}
 
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;
}