Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Application/Cpuid/Cpuid.c
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Application/Cpuid/Cpuid.c	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Application/Cpuid/Cpuid.c	(revision 101291)
@@ -2,5 +2,5 @@
   UEFI Application to display CPUID leaf information.
 
-  Copyright (c) 2016 - 2019, Intel Corporation. All rights reserved.<BR>
+  Copyright (c) 2016 - 2023, Intel Corporation. All rights reserved.<BR>
   SPDX-License-Identifier: BSD-2-Clause-Patent
 
@@ -218,10 +218,10 @@
   DisplayFamily = Eax.Bits.FamilyId;
   if (Eax.Bits.FamilyId == 0x0F) {
-    DisplayFamily |= (Eax.Bits.ExtendedFamilyId << 4);
+    DisplayFamily += Eax.Bits.ExtendedFamilyId;
   }
 
   DisplayModel = Eax.Bits.Model;
   if ((Eax.Bits.FamilyId == 0x06) || (Eax.Bits.FamilyId == 0x0f)) {
-    DisplayModel |= (Eax.Bits.ExtendedModelId << 4);
+    DisplayModel += (Eax.Bits.ExtendedModelId << 4);
   }
 
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/CpuDxe/CpuDxe.c
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/CpuDxe/CpuDxe.c	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/CpuDxe/CpuDxe.c	(revision 101291)
@@ -2,5 +2,5 @@
   CPU DXE Module to produce CPU ARCH Protocol.
 
-  Copyright (c) 2008 - 2022, Intel Corporation. All rights reserved.<BR>
+  Copyright (c) 2008 - 2023, Intel Corporation. All rights reserved.<BR>
   SPDX-License-Identifier: BSD-2-Clause-Patent
 
@@ -506,18 +506,36 @@
   )
 {
-  UINT32  RegEax;
-  UINT8   PhysicalAddressBits;
-
-  AsmCpuid (0x80000000, &RegEax, NULL, NULL, NULL);
-
-  if (RegEax >= 0x80000008) {
-    AsmCpuid (0x80000008, &RegEax, NULL, NULL, NULL);
-
-    PhysicalAddressBits = (UINT8)RegEax;
+  UINT32                                       MaxExtendedFunction;
+  CPUID_VIR_PHY_ADDRESS_SIZE_EAX               VirPhyAddressSize;
+  UINT32                                       MaxFunction;
+  CPUID_STRUCTURED_EXTENDED_FEATURE_FLAGS_ECX  ExtendedFeatureFlagsEcx;
+  MSR_IA32_TME_ACTIVATE_REGISTER               TmeActivate;
+
+  AsmCpuid (CPUID_EXTENDED_FUNCTION, &MaxExtendedFunction, NULL, NULL, NULL);
+
+  if (MaxExtendedFunction >= CPUID_VIR_PHY_ADDRESS_SIZE) {
+    AsmCpuid (CPUID_VIR_PHY_ADDRESS_SIZE, &VirPhyAddressSize.Uint32, NULL, NULL, NULL);
   } else {
-    PhysicalAddressBits = 36;
-  }
-
-  mValidMtrrBitsMask    = LShiftU64 (1, PhysicalAddressBits) - 1;
+    VirPhyAddressSize.Bits.PhysicalAddressBits = 36;
+  }
+
+  //
+  // CPUID enumeration of MAX_PA is unaffected by TME-MK activation and will continue
+  // to report the maximum physical address bits available for software to use,
+  // irrespective of the number of KeyID bits.
+  // So, we need to check if TME is enabled and adjust the PA size accordingly.
+  //
+  AsmCpuid (CPUID_SIGNATURE, &MaxFunction, NULL, NULL, NULL);
+  if (MaxFunction >= CPUID_STRUCTURED_EXTENDED_FEATURE_FLAGS) {
+    AsmCpuidEx (CPUID_STRUCTURED_EXTENDED_FEATURE_FLAGS, 0, NULL, NULL, &ExtendedFeatureFlagsEcx.Uint32, NULL);
+    if (ExtendedFeatureFlagsEcx.Bits.TME_EN == 1) {
+      TmeActivate.Uint64 = AsmReadMsr64 (MSR_IA32_TME_ACTIVATE);
+      if (TmeActivate.Bits.TmeEnable == 1) {
+        VirPhyAddressSize.Bits.PhysicalAddressBits -= TmeActivate.Bits.MkTmeKeyidBits;
+      }
+    }
+  }
+
+  mValidMtrrBitsMask    = LShiftU64 (1, VirPhyAddressSize.Bits.PhysicalAddressBits) - 1;
   mValidMtrrAddressMask = mValidMtrrBitsMask & 0xfffffffffffff000ULL;
 }
@@ -1061,5 +1079,5 @@
       "%a: %a: add [%Lx, %Lx): %r\n",
       gEfiCallerBaseName,
-      __FUNCTION__,
+      __func__,
       IntersectionBase,
       IntersectionEnd,
@@ -1074,5 +1092,5 @@
     "with aperture [%Lx, %Lx) cap %Lx\n",
     gEfiCallerBaseName,
-    __FUNCTION__,
+    __func__,
     Descriptor->BaseAddress,
     Descriptor->BaseAddress + Descriptor->Length,
@@ -1115,5 +1133,5 @@
       "%a: %a: GetMemorySpaceMap(): %r\n",
       gEfiCallerBaseName,
-      __FUNCTION__,
+      __func__,
       Status
       ));
@@ -1198,5 +1216,5 @@
       "%a: %a: AllocateMemorySpace() Status - %r\n",
       gEfiCallerBaseName,
-      __FUNCTION__,
+      __func__,
       Status
       ));
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/CpuDxe/CpuDxe.h
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/CpuDxe/CpuDxe.h	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/CpuDxe/CpuDxe.h	(revision 101291)
@@ -2,5 +2,5 @@
   CPU DXE Module to produce CPU ARCH Protocol and CPU MP Protocol.
 
-  Copyright (c) 2008 - 2019, Intel Corporation. All rights reserved.<BR>
+  Copyright (c) 2008 - 2023, Intel Corporation. All rights reserved.<BR>
   SPDX-License-Identifier: BSD-2-Clause-Patent
 
@@ -14,4 +14,5 @@
 #include <Protocol/Cpu.h>
 #include <Protocol/MpService.h>
+#include <Register/Intel/Cpuid.h>
 #include <Register/Intel/Msr.h>
 
@@ -29,5 +30,4 @@
 #include <Library/MtrrLib.h>
 #include <Library/LocalApicLib.h>
-#include <Library/UefiCpuLib.h>
 #include <Library/UefiLib.h>
 #include <Library/CpuExceptionHandlerLib.h>
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/CpuDxe/CpuDxe.inf
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/CpuDxe/CpuDxe.inf	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/CpuDxe/CpuDxe.inf	(revision 101291)
@@ -34,5 +34,4 @@
   UefiDriverEntryPoint
   LocalApicLib
-  UefiCpuLib
   UefiLib
   CpuExceptionHandlerLib
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/CpuDxeRiscV64/CpuDxe.c
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/CpuDxeRiscV64/CpuDxe.c	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/CpuDxeRiscV64/CpuDxe.c	(revision 101291)
@@ -297,6 +297,5 @@
   )
 {
-  DEBUG ((DEBUG_INFO, "%a: Set memory attributes not supported yet\n", __FUNCTION__));
-  return EFI_SUCCESS;
+  return RiscVSetMemoryAttributes (BaseAddress, Length, Attributes);
 }
 
@@ -329,8 +328,8 @@
   }
 
-  DEBUG ((DEBUG_INFO, " %a: Firmware Context is at 0x%x.\n", __FUNCTION__, FirmwareContext));
+  DEBUG ((DEBUG_INFO, " %a: Firmware Context is at 0x%x.\n", __func__, FirmwareContext));
 
   mBootHartId = FirmwareContext->BootHartId;
-  DEBUG ((DEBUG_INFO, " %a: mBootHartId = 0x%x.\n", __FUNCTION__, mBootHartId));
+  DEBUG ((DEBUG_INFO, " %a: mBootHartId = 0x%x.\n", __func__, mBootHartId));
 
   InitializeCpuExceptionHandlers (NULL);
@@ -340,4 +339,10 @@
   //
   DisableInterrupts ();
+
+  //
+  // Enable MMU
+  //
+  Status = RiscVConfigureMmu ();
+  ASSERT_EFI_ERROR (Status);
 
   //
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/CpuDxeRiscV64/CpuDxe.h
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/CpuDxeRiscV64/CpuDxe.h	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/CpuDxeRiscV64/CpuDxe.h	(revision 101291)
@@ -16,4 +16,5 @@
 #include <Protocol/RiscVBootProtocol.h>
 #include <Library/BaseRiscVSbiLib.h>
+#include <Library/BaseRiscVMmuLib.h>
 #include <Library/BaseLib.h>
 #include <Library/CpuExceptionHandlerLib.h>
@@ -21,4 +22,5 @@
 #include <Library/UefiBootServicesTableLib.h>
 #include <Library/UefiDriverEntryPoint.h>
+#include <Register/RiscV64/RiscVEncoding.h>
 
 /**
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/CpuDxeRiscV64/CpuDxeRiscV64.inf
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/CpuDxeRiscV64/CpuDxeRiscV64.inf	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/CpuDxeRiscV64/CpuDxeRiscV64.inf	(revision 101291)
@@ -38,4 +38,6 @@
   PeCoffGetEntryPointLib
   RiscVSbiLib
+  RiscVMmuLib
+  CacheMaintenanceLib
 
 [Sources]
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/CpuMpPei/CpuBist.c
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/CpuMpPei/CpuBist.c	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/CpuMpPei/CpuBist.c	(revision 101291)
@@ -176,5 +176,11 @@
   EFI_SEC_PLATFORM_INFORMATION_CPU      *CpuInstanceInHob;
 
-  MpInitLibGetNumberOfProcessors (&NumberOfProcessors, &NumberOfEnabledProcessors);
+  Status = MpInitLibGetNumberOfProcessors (&NumberOfProcessors, &NumberOfEnabledProcessors);
+  ASSERT_EFI_ERROR (Status);
+
+  if (EFI_ERROR (Status)) {
+    NumberOfProcessors        = 1;
+    NumberOfEnabledProcessors = 1;
+  }
 
   BistInformationSize = sizeof (EFI_SEC_PLATFORM_INFORMATION_RECORD2) +
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/CpuMpPei/CpuMpPei.c
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/CpuMpPei/CpuMpPei.c	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/CpuMpPei/CpuMpPei.c	(revision 101291)
@@ -474,5 +474,11 @@
   }
 
-  MpInitLibGetNumberOfProcessors (&NumberOfProcessors, NULL);
+  Status = MpInitLibGetNumberOfProcessors (&NumberOfProcessors, NULL);
+  ASSERT_EFI_ERROR (Status);
+
+  if (EFI_ERROR (Status)) {
+    NumberOfProcessors = 1;
+  }
+
   SwitchStackData = AllocatePages (EFI_SIZE_TO_PAGES (NumberOfProcessors * sizeof (EXCEPTION_STACK_SWITCH_CONTEXT)));
   ASSERT (SwitchStackData != NULL);
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/CpuMpPei/CpuMpPei.h
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/CpuMpPei/CpuMpPei.h	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/CpuMpPei/CpuMpPei.h	(revision 101291)
@@ -29,4 +29,5 @@
 #include <Library/BaseMemoryLib.h>
 #include <Library/MemoryAllocationLib.h>
+#include <Library/CpuPageTableLib.h>
 
 extern EFI_PEI_PPI_DESCRIPTOR  mPeiCpuMpPpiDesc;
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/CpuMpPei/CpuMpPei.inf
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/CpuMpPei/CpuMpPei.inf	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/CpuMpPei/CpuMpPei.inf	(revision 101291)
@@ -47,4 +47,5 @@
   CpuLib
   MemoryAllocationLib
+  CpuPageTableLib
 
 [Guids]
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/CpuMpPei/CpuPaging.c
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/CpuMpPei/CpuPaging.c	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/CpuMpPei/CpuPaging.c	(revision 101291)
@@ -147,36 +147,4 @@
 
 /**
-  Get the address width supported by current processor.
-
-  @retval 32      If processor is in 32-bit mode.
-  @retval 36-48   If processor is in 64-bit mode.
-
-**/
-UINTN
-GetPhysicalAddressWidth (
-  VOID
-  )
-{
-  UINT32  RegEax;
-
-  if (sizeof (UINTN) == 4) {
-    return 32;
-  }
-
-  AsmCpuid (CPUID_EXTENDED_FUNCTION, &RegEax, NULL, NULL, NULL);
-  if (RegEax >= CPUID_VIR_PHY_ADDRESS_SIZE) {
-    AsmCpuid (CPUID_VIR_PHY_ADDRESS_SIZE, &RegEax, NULL, NULL, NULL);
-    RegEax &= 0xFF;
-    if (RegEax > 48) {
-      return 48;
-    }
-
-    return (UINTN)RegEax;
-  }
-
-  return 36;
-}
-
-/**
   Get the type of top level page table.
 
@@ -445,114 +413,78 @@
 
 /**
-  Get maximum size of page memory supported by current processor.
-
-  @param[in]   TopLevelType     The type of top level page entry.
-
-  @retval Page1G     If processor supports 1G page and PML4.
-  @retval Page2M     For all other situations.
-
-**/
-PAGE_ATTRIBUTE
-GetMaxMemoryPage (
-  IN  PAGE_ATTRIBUTE  TopLevelType
-  )
-{
-  UINT32  RegEax;
-  UINT32  RegEdx;
-
-  if (TopLevelType == Page512G) {
-    AsmCpuid (CPUID_EXTENDED_FUNCTION, &RegEax, NULL, NULL, NULL);
-    if (RegEax >= CPUID_EXTENDED_CPU_SIG) {
-      AsmCpuid (CPUID_EXTENDED_CPU_SIG, NULL, NULL, NULL, &RegEdx);
-      if ((RegEdx & BIT26) != 0) {
-        return Page1G;
-      }
-    }
-  }
-
-  return Page2M;
-}
-
-/**
-  Create PML4 or PAE page table.
-
-  @return The address of page table.
-
-**/
-UINTN
-CreatePageTable (
+  Enable PAE Page Table.
+
+  @retval   EFI_SUCCESS           The PAE Page Table was enabled successfully.
+  @retval   EFI_OUT_OF_RESOURCES  The PAE Page Table could not be enabled due to lack of available memory.
+
+**/
+EFI_STATUS
+EnablePaePageTable (
   VOID
   )
 {
-  RETURN_STATUS         Status;
-  UINTN                 PhysicalAddressBits;
-  UINTN                 NumberOfEntries;
-  PAGE_ATTRIBUTE        TopLevelPageAttr;
-  UINTN                 PageTable;
-  PAGE_ATTRIBUTE        MaxMemoryPage;
-  UINTN                 Index;
-  UINT64                AddressEncMask;
-  UINT64                *PageEntry;
-  EFI_PHYSICAL_ADDRESS  PhysicalAddress;
-
-  TopLevelPageAttr    = (PAGE_ATTRIBUTE)GetPageTableTopLevelType ();
-  PhysicalAddressBits = GetPhysicalAddressWidth ();
-  NumberOfEntries     = (UINTN)1 << (PhysicalAddressBits -
-                                     mPageAttributeTable[TopLevelPageAttr].AddressBitOffset);
-
-  PageTable = (UINTN)AllocatePageTableMemory (1);
-  if (PageTable == 0) {
-    return 0;
-  }
-
-  AddressEncMask  = PcdGet64 (PcdPteMemoryEncryptionAddressOrMask);
-  AddressEncMask &= mPageAttributeTable[TopLevelPageAttr].AddressMask;
-  MaxMemoryPage   = GetMaxMemoryPage (TopLevelPageAttr);
-  PageEntry       = (UINT64 *)PageTable;
-
-  PhysicalAddress = 0;
-  for (Index = 0; Index < NumberOfEntries; ++Index) {
-    *PageEntry = PhysicalAddress | AddressEncMask | PAGE_ATTRIBUTE_BITS;
-
-    //
-    // Split the top page table down to the maximum page size supported
-    //
-    if (MaxMemoryPage < TopLevelPageAttr) {
-      Status = SplitPage (PageEntry, TopLevelPageAttr, MaxMemoryPage, TRUE);
-      ASSERT_EFI_ERROR (Status);
-    }
-
-    if (TopLevelPageAttr == Page1G) {
-      //
-      // PDPTE[2:1] (PAE Paging) must be 0. SplitPage() might change them to 1.
-      //
-      *PageEntry &= ~(UINT64)(IA32_PG_RW | IA32_PG_U);
-    }
-
-    PageEntry       += 1;
-    PhysicalAddress += mPageAttributeTable[TopLevelPageAttr].Length;
-  }
-
-  return PageTable;
-}
-
-/**
-  Setup page tables and make them work.
-
-**/
-VOID
-EnablePaging (
-  VOID
-  )
-{
-  UINTN  PageTable;
-
-  PageTable = CreatePageTable ();
-  ASSERT (PageTable != 0);
-  if (PageTable != 0) {
-    AsmWriteCr3 (PageTable);
-    AsmWriteCr4 (AsmReadCr4 () | BIT5);   // CR4.PAE
-    AsmWriteCr0 (AsmReadCr0 () | BIT31);  // CR0.PG
-  }
+  EFI_STATUS  Status;
+
+  UINTN               PageTable;
+  VOID                *Buffer;
+  UINTN               BufferSize;
+  IA32_MAP_ATTRIBUTE  MapAttribute;
+  IA32_MAP_ATTRIBUTE  MapMask;
+
+  PageTable                   = 0;
+  Buffer                      = NULL;
+  BufferSize                  = 0;
+  MapAttribute.Uint64         = 0;
+  MapMask.Uint64              = MAX_UINT64;
+  MapAttribute.Bits.Present   = 1;
+  MapAttribute.Bits.ReadWrite = 1;
+
+  //
+  // 1:1 map 4GB in 32bit mode
+  //
+  Status = PageTableMap (&PageTable, PagingPae, 0, &BufferSize, 0, SIZE_4GB, &MapAttribute, &MapMask, NULL);
+  ASSERT (Status == EFI_BUFFER_TOO_SMALL);
+  if (Status != EFI_BUFFER_TOO_SMALL) {
+    return Status;
+  }
+
+  //
+  // Allocate required Buffer.
+  //
+  Buffer = AllocatePageTableMemory (EFI_SIZE_TO_PAGES (BufferSize));
+  ASSERT (Buffer != NULL);
+  if (Buffer == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Status = PageTableMap (&PageTable, PagingPae, Buffer, &BufferSize, 0, SIZE_4GB, &MapAttribute, &MapMask, NULL);
+  ASSERT_EFI_ERROR (Status);
+  if (EFI_ERROR (Status) || (PageTable == 0)) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  // Write the Pagetable to CR3.
+  //
+  AsmWriteCr3 (PageTable);
+
+  //
+  // Enable CR4.PAE
+  //
+  AsmWriteCr4 (AsmReadCr4 () | BIT5);
+
+  //
+  // Enable CR0.PG
+  //
+  AsmWriteCr0 (AsmReadCr0 () | BIT31);
+
+  DEBUG ((
+    DEBUG_INFO,
+    "EnablePaePageTable: Created PageTable = 0x%x, BufferSize = %x\n",
+    PageTable,
+    BufferSize
+    ));
+
+  return Status;
 }
 
@@ -600,4 +532,5 @@
   UINTN                 Bsp;
   UINTN                 Index;
+  EFI_STATUS            Status;
 
   //
@@ -609,5 +542,11 @@
   }
 
-  MpInitLibGetNumberOfProcessors (&NumberOfProcessors, NULL);
+  Status = MpInitLibGetNumberOfProcessors (&NumberOfProcessors, NULL);
+  ASSERT_EFI_ERROR (Status);
+
+  if (EFI_ERROR (Status)) {
+    NumberOfProcessors = 1;
+  }
+
   MpInitLibWhoAmI (&Bsp);
   for (Index = 0; Index < NumberOfProcessors; ++Index) {
@@ -679,4 +618,5 @@
   EDKII_MIGRATED_FV_INFO  *MigratedFvInfo;
   EFI_PEI_HOB_POINTERS    Hob;
+  IA32_CR0                Cr0;
 
   //
@@ -692,6 +632,17 @@
   }
 
-  if (InitStackGuard || (Hob.Raw != NULL)) {
-    EnablePaging ();
+  //
+  // Some security features depend on the page table enabling. So, here
+  // is to enable paging if it is not enabled (only in 32bit mode).
+  //
+  Cr0.UintN = AsmReadCr0 ();
+  if ((Cr0.Bits.PG == 0) && (InitStackGuard || (Hob.Raw != NULL))) {
+    ASSERT (sizeof (UINTN) == sizeof (UINT32));
+
+    Status = EnablePaePageTable ();
+    if (EFI_ERROR (Status)) {
+      DEBUG ((DEBUG_ERROR, "MemoryDiscoveredPpiNotifyCallback: Failed to enable PAE page table: %r.\n", Status));
+      CpuDeadLoop ();
+    }
   }
 
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/CpuS3DataDxe/CpuS3Data.c
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/CpuS3DataDxe/CpuS3Data.c	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/CpuS3DataDxe/CpuS3Data.c	(revision 101291)
@@ -128,5 +128,5 @@
   ASSERT_EFI_ERROR (Status);
 
-  DEBUG ((DEBUG_VERBOSE, "%a\n", __FUNCTION__));
+  DEBUG ((DEBUG_VERBOSE, "%a\n", __func__));
   MtrrGetAllMtrrs (&AcpiCpuDataEx->MtrrTable);
 
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/CpuTimerDxeRiscV64/CpuTimerDxeRiscV64.inf
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/CpuTimerDxeRiscV64/CpuTimerDxeRiscV64.inf	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/CpuTimerDxeRiscV64/CpuTimerDxeRiscV64.inf	(revision 101291)
@@ -41,4 +41,7 @@
   Timer.c
 
+[Pcd]
+  gUefiCpuPkgTokenSpaceGuid.PcdCpuCoreCrystalClockFrequency  ## CONSUMES
+
 [Protocols]
   gEfiCpuArchProtocolGuid       ## CONSUMES
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/CpuTimerDxeRiscV64/Timer.c
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/CpuTimerDxeRiscV64/Timer.c	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/CpuTimerDxeRiscV64/Timer.c	(revision 101291)
@@ -10,4 +10,5 @@
 #include <Library/BaseLib.h>
 #include <Library/BaseRiscVSbiLib.h>
+#include <Library/UefiLib.h>
 #include "Timer.h"
 
@@ -41,5 +42,6 @@
 // The current period of the timer interrupt
 //
-STATIC UINT64  mTimerPeriod = 0;
+STATIC UINT64  mTimerPeriod     = 0;
+STATIC UINT64  mLastPeriodStart = 0;
 
 /**
@@ -57,23 +59,43 @@
 {
   EFI_TPL  OriginalTPL;
-  UINT64   RiscvTimer;
+  UINT64   PeriodStart;
+
+  PeriodStart = RiscVReadTimer ();
 
   OriginalTPL = gBS->RaiseTPL (TPL_HIGH_LEVEL);
   if (mTimerNotifyFunction != NULL) {
-    mTimerNotifyFunction (mTimerPeriod);
+    //
+    // For any number of reasons, the ticks could be coming
+    // in slower than mTimerPeriod. For example, some code
+    // is doing a *lot* of stuff inside an EFI_TPL_HIGH
+    // critical section, and this should not cause the EFI
+    // time to increment slower. So when we take an interrupt,
+    // account for the actual time passed.
+    //
+    mTimerNotifyFunction (
+      DivU64x32 (
+        EFI_TIMER_PERIOD_SECONDS (PeriodStart - mLastPeriodStart),
+        PcdGet64 (PcdCpuCoreCrystalClockFrequency)
+        )
+      );
   }
 
-  RiscVDisableTimerInterrupt (); // Disable SMode timer int
-  RiscVClearPendingTimerInterrupt ();
   if (mTimerPeriod == 0) {
+    RiscVDisableTimerInterrupt ();
     gBS->RestoreTPL (OriginalTPL);
-    RiscVDisableTimerInterrupt (); // Disable SMode timer int
     return;
   }
 
-  RiscvTimer               = RiscVReadTimer ();
-  SbiSetTimer (RiscvTimer += mTimerPeriod);
+  mLastPeriodStart = PeriodStart;
+  PeriodStart     += DivU64x32 (
+                       MultU64x32 (
+                         mTimerPeriod,
+                         PcdGet64 (PcdCpuCoreCrystalClockFrequency)
+                         ),
+                       1000000u
+                       );  // convert to tick
+  SbiSetTimer (PeriodStart);
+  RiscVEnableTimerInterrupt (); // enable SMode timer int
   gBS->RestoreTPL (OriginalTPL);
-  RiscVEnableTimerInterrupt (); // enable SMode timer int
 }
 
@@ -155,5 +177,5 @@
   )
 {
-  UINT64  RiscvTimer;
+  UINT64  PeriodStart;
 
   DEBUG ((DEBUG_INFO, "TimerDriverSetTimerPeriod(0x%lx)\n", TimerPeriod));
@@ -165,7 +187,16 @@
   }
 
-  mTimerPeriod = TimerPeriod / 10; // convert unit from 100ns to 1us
-  RiscvTimer   = RiscVReadTimer ();
-  SbiSetTimer (RiscvTimer + mTimerPeriod);
+  mTimerPeriod = TimerPeriod / 10;     // convert unit from 100ns to 1us
+
+  mLastPeriodStart = RiscVReadTimer ();
+  PeriodStart      = mLastPeriodStart;
+  PeriodStart     += DivU64x32 (
+                       MultU64x32 (
+                         mTimerPeriod,
+                         PcdGet64 (PcdCpuCoreCrystalClockFrequency)
+                         ),
+                       1000000u
+                       ); // convert to tick
+  SbiSetTimer (PeriodStart);
 
   mCpu->EnableInterrupt (mCpu);
@@ -272,5 +303,9 @@
   // Install interrupt handler for RISC-V Timer.
   //
-  Status = mCpu->RegisterInterruptHandler (mCpu, EXCEPT_RISCV_TIMER_INT, TimerInterruptHandler);
+  Status = mCpu->RegisterInterruptHandler (
+                   mCpu,
+                   EXCEPT_RISCV_IRQ_TIMER_FROM_SMODE,
+                   TimerInterruptHandler
+                   );
   ASSERT_EFI_ERROR (Status);
 
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/CpuTimerDxeRiscV64/Timer.h
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/CpuTimerDxeRiscV64/Timer.h	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/CpuTimerDxeRiscV64/Timer.h	(revision 101291)
@@ -22,5 +22,5 @@
 
 //
-// RISC-V use 100us timer.
+// RISC-V use 100ns timer.
 // The default timer tick duration is set to 10 ms = 10 * 1000 * 10 100 ns units
 //
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Include/Guid/SmmBaseHob.h
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Include/Guid/SmmBaseHob.h	(revision 101291)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Include/Guid/SmmBaseHob.h	(revision 101291)
@@ -0,0 +1,75 @@
+/** @file
+  The Smm Base HOB is used to store the information of:
+  * The relocated SmBase address in array for each processor.
+
+  The default Smbase for the x86 processor is 0x30000. When SMI happens, processor
+  runs the SMI handler at Smbase+0x8000. Also, the SMM save state area is within
+  Smbase+0x10000. Since it's the start address to store the processor save state
+  and code for the SMI entry point, those info are tiled within an SMRAM allocated
+  or reserved buffer. This tile size shall be enough to cover 3 parts:
+  1. Processor SMRAM Save State Map starts at Smbase + 0xfc00
+  2. Extra processor specific context start starts at Smbase + 0xfb00
+  3. SMI entry point starts at Smbase + 0x8000.
+  Besides, This size should be rounded up to nearest power of 2. The Smm Base HOB
+  producer should be responsible for reserving enough size.
+
+  One of the SMM initialization from processor perspective is to relocate and program
+  the new Smbase (in TSEG range) for each processor thread. When the Smbase relocation
+  happens in a PEI module, the PEI module shall produce the SMM_BASE_HOB in HOB database
+  which tells the PiSmmCpuDxeSmm driver (which runs at a later phase) about the new
+  Smbase for each processor. PiSmmCpuDxeSmm driver installs the SMI handler at the
+  SMM_BASE_HOB.Smbase[Index]+0x8000 for processor index. When the HOB doesn't exist,
+  PiSmmCpuDxeSmm driver shall relocate and program the new Smbase itself.
+
+  Note:
+  1. Smbase relocation process needs to program the vender specific hardware
+  interface to set Smbase, it might be in the thread scope. It's doable to
+  program the hardware interface using DXE MP service protocol in PiSmmCpuDxeSmm
+  entry point. But, considering the standalone MM environment where the CpuMm
+  driver runs in a isolated environment and it cannot invoke any DXE or PEI MP
+  service, we recommend to put the hardware interface programming in a separate
+  PEI module instead of in the PiSmmCpuDxeSmm driver.
+
+  2. There is the hard requirement that SMI Entry Size <= 0x1000, data Size <=
+  0x1000 in PiSmmCpuDxeSmm. So, this require the allocated or reserved buffer in
+  SMRAM should be >= 0x2000.
+
+  Copyright (c) 2023, Intel Corporation. All rights reserved.<BR>
+  SPDX-License-Identifier: BSD-2-Clause-Patent
+
+**/
+
+#ifndef SMM_BASE_HOB_H_
+#define SMM_BASE_HOB_H_
+
+#define SMM_BASE_HOB_DATA_GUID \
+  { \
+    0xc2217ba7, 0x03bb, 0x4f63, {0xa6, 0x47, 0x7c, 0x25, 0xc5, 0xfc, 0x9d, 0x73}  \
+  }
+
+#pragma pack(1)
+typedef struct {
+  ///
+  /// ProcessorIndex tells which processor range this specific HOB instance described.
+  /// If ProcessorIndex is set to 0, it indicats the HOB describes the processor from
+  /// 0 to NumberOfProcessors - 1. The HOB list may contains multiple this HOB
+  /// instances. Each HOB instances describe the information for processor from
+  /// ProcessorIndex to ProcessorIndex + NumberOfProcessors - 1. The instance order in
+  /// the HOB list is random so consumer can not assume the ProcessorIndex of first
+  /// instance is 0.
+  ///
+  UINT32    ProcessorIndex;
+  ///
+  /// Describes the Number of all max supported processors.
+  ///
+  UINT32    NumberOfProcessors;
+  ///
+  /// Pointer to SmBase address for each processor.
+  ///
+  UINT64    SmBase[];
+} SMM_BASE_HOB_DATA;
+#pragma pack()
+
+extern EFI_GUID  gSmmBaseHobGuid;
+
+#endif
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Include/Library/BaseRiscVMmuLib.h
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Include/Library/BaseRiscVMmuLib.h	(revision 101291)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Include/Library/BaseRiscVMmuLib.h	(revision 101291)
@@ -0,0 +1,68 @@
+/** @file
+
+  Copyright (c) 2015 - 2016, Linaro Ltd. All rights reserved.<BR>
+  Copyright (c) 2023, Ventana Micro Systems Inc. All Rights Reserved.<BR>
+
+  SPDX-License-Identifier: BSD-2-Clause-Patent
+
+**/
+
+#ifndef BASE_RISCV_MMU_LIB_H_
+#define BASE_RISCV_MMU_LIB_H_
+
+/**
+  The API to flush all local TLBs.
+
+**/
+VOID
+EFIAPI
+RiscVLocalTlbFlushAll (
+  VOID
+  );
+
+/**
+  The API to flush local TLB at a virtual address.
+
+  @param  VirtAddr  The virtual address.
+
+**/
+VOID
+EFIAPI
+RiscVLocalTlbFlush (
+  UINTN  VirtAddr
+  );
+
+/**
+  The API to set a GCD attribute on an memory region.
+
+  @param  BaseAddress             The base address of the region.
+  @param  Length                  The length of the region.
+  @param  Attributes              The GCD attributes.
+
+  @retval EFI_INVALID_PARAMETER   The BaseAddress or Length was not valid.
+  @retval EFI_OUT_OF_RESOURCES    Not enough resource.
+  @retval EFI_SUCCESS             The operation succesfully.
+
+**/
+EFI_STATUS
+EFIAPI
+RiscVSetMemoryAttributes (
+  IN EFI_PHYSICAL_ADDRESS  BaseAddress,
+  IN UINT64                Length,
+  IN UINT64                Attributes
+  );
+
+/**
+  The API to configure and enable RISC-V MMU with the highest mode supported.
+
+  @retval EFI_OUT_OF_RESOURCES    Not enough resource.
+  @retval EFI_SUCCESS             The operation succesfully.
+
+**/
+EFI_STATUS
+EFIAPI
+RiscVConfigureMmu (
+  VOID
+  );
+
+#endif /* BASE_RISCV_MMU_LIB_H_ */
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Include/Library/CpuPageTableLib.h
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Include/Library/CpuPageTableLib.h	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Include/Library/CpuPageTableLib.h	(revision 101291)
@@ -2,5 +2,5 @@
   Public include file for PageTableLib library.
 
-  Copyright (c) 2022, Intel Corporation. All rights reserved.<BR>
+  Copyright (c) 2022 - 2023, Intel Corporation. All rights reserved.<BR>
   SPDX-License-Identifier: BSD-2-Clause-Patent
 
@@ -12,20 +12,20 @@
 typedef union {
   struct {
-    UINT64    Present              : 1; // 0 = Not present in memory, 1 = Present in memory
-    UINT64    ReadWrite            : 1; // 0 = Read-Only, 1= Read/Write
-    UINT64    UserSupervisor       : 1; // 0 = Supervisor, 1=User
-    UINT64    WriteThrough         : 1; // 0 = Write-Back caching, 1=Write-Through caching
-    UINT64    CacheDisabled        : 1; // 0 = Cached, 1=Non-Cached
-    UINT64    Accessed             : 1; // 0 = Not accessed, 1 = Accessed (set by CPU)
-    UINT64    Dirty                : 1; // 0 = Not dirty, 1 = Dirty (set by CPU)
-    UINT64    Pat                  : 1; // PAT
+    UINT32    Present                  : 1;  // 0 = Not present in memory, 1 = Present in memory
+    UINT32    ReadWrite                : 1;  // 0 = Read-Only, 1= Read/Write
+    UINT32    UserSupervisor           : 1;  // 0 = Supervisor, 1=User
+    UINT32    WriteThrough             : 1;  // 0 = Write-Back caching, 1=Write-Through caching
+    UINT32    CacheDisabled            : 1;  // 0 = Cached, 1=Non-Cached
+    UINT32    Accessed                 : 1;  // 0 = Not accessed, 1 = Accessed (set by CPU)
+    UINT32    Dirty                    : 1;  // 0 = Not dirty, 1 = Dirty (set by CPU)
+    UINT32    Pat                      : 1;  // PAT
+    UINT32    Global                   : 1;  // 0 = Not global, 1 = Global (if CR4.PGE = 1)
+    UINT32    Reserved1                : 3;  // Ignored
+    UINT32    PageTableBaseAddressLow  : 20; // Page Table Base Address Low
 
-    UINT64    Global               : 1; // 0 = Not global, 1 = Global (if CR4.PGE = 1)
-    UINT64    Reserved1            : 3; // Ignored
-
-    UINT64    PageTableBaseAddress : 40; // Page Table Base Address
-    UINT64    Reserved2            : 7;  // Ignored
-    UINT64    ProtectionKey        : 4;  // Protection key
-    UINT64    Nx                   : 1;  // No Execute bit
+    UINT32    PageTableBaseAddressHigh : 20; // Page Table Base Address High
+    UINT32    Reserved2                : 7;  // Ignored
+    UINT32    ProtectionKey            : 4;  // Protection key
+    UINT32    Nx                       : 1;  // No Execute bit
   } Bits;
   UINT64    Uint64;
@@ -75,12 +75,17 @@
                                  when a new physical base address is set.
   @param[in]      Mask           The mask used for attribute. The corresponding field in Attribute is ignored if that in Mask is 0.
+  @param[out]     IsModified     TRUE means page table is modified. FALSE means page table is not modified.
 
   @retval RETURN_UNSUPPORTED        PagingMode is not supported.
   @retval RETURN_INVALID_PARAMETER  PageTable, BufferSize, Attribute or Mask is NULL.
+  @retval RETURN_INVALID_PARAMETER  For non-present range, Mask->Bits.Present is 0 but some other attributes are provided.
+  @retval RETURN_INVALID_PARAMETER  For non-present range, Mask->Bits.Present is 1, Attribute->Bits.Present is 1 but some other attributes are not provided.
+  @retval RETURN_INVALID_PARAMETER  For non-present range, Mask->Bits.Present is 1, Attribute->Bits.Present is 0 but some other attributes are provided.
+  @retval RETURN_INVALID_PARAMETER  For present range, Mask->Bits.Present is 1, Attribute->Bits.Present is 0 but some other attributes are provided.
   @retval RETURN_INVALID_PARAMETER  *BufferSize is not multiple of 4KB.
   @retval RETURN_BUFFER_TOO_SMALL   The buffer is too small for page table creation/updating.
                                     BufferSize is updated to indicate the expected buffer size.
                                     Caller may still get RETURN_BUFFER_TOO_SMALL with the new BufferSize.
-  @retval RETURN_SUCCESS            PageTable is created/updated successfully.
+  @retval RETURN_SUCCESS            PageTable is created/updated successfully or the input Length is 0.
 **/
 RETURN_STATUS
@@ -94,5 +99,6 @@
   IN     UINT64              Length,
   IN     IA32_MAP_ATTRIBUTE  *Attribute,
-  IN     IA32_MAP_ATTRIBUTE  *Mask
+  IN     IA32_MAP_ATTRIBUTE  *Mask,
+  OUT    BOOLEAN             *IsModified   OPTIONAL
   );
 
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Include/Library/LocalApicLib.h
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Include/Library/LocalApicLib.h	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Include/Library/LocalApicLib.h	(revision 101291)
@@ -5,5 +5,5 @@
   handles cases where local APIC is disabled.
 
-  Copyright (c) 2010 - 2019, Intel Corporation. All rights reserved.<BR>
+  Copyright (c) 2010 - 2023, Intel Corporation. All rights reserved.<BR>
   SPDX-License-Identifier: BSD-2-Clause-Patent
 
@@ -184,4 +184,19 @@
 SendInitIpiAllExcludingSelf (
   VOID
+  );
+
+/**
+  Send a Start-up IPI to all processors excluding self.
+  This function returns after the IPI has been accepted by the target processors.
+  if StartupRoutine >= 1M, then ASSERT.
+  if StartupRoutine is not multiple of 4K, then ASSERT.
+  @param  StartupRoutine  Points to a start-up routine which is below 1M physical
+                          address and 4K aligned.
+**/
+
+VOID
+EFIAPI
+SendStartupIpiAllExcludingSelf (
+  IN UINT32  StartupRoutine
   );
 
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Include/Library/MmSaveStateLib.h
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Include/Library/MmSaveStateLib.h	(revision 101291)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Include/Library/MmSaveStateLib.h	(revision 101291)
@@ -0,0 +1,74 @@
+/** @file
+Library that provides service to read/write CPU specific smram save state registers.
+
+Copyright (c) 2010 - 2019, Intel Corporation. All rights reserved.<BR>
+Copyright (C) 2023 Advanced Micro Devices, Inc. All rights reserved.<BR>
+
+@note
+  SaveState(Read/Write) of EFI_SMM_SAVE_STATE_REGISTER_PROCESSOR_ID/EFI_MM_SAVE_STATE_REGISTER_PROCESSOR_ID
+  is handled by PiSmmCpuDxeSmm driver.
+
+SPDX-License-Identifier: BSD-2-Clause-Patent
+
+**/
+
+#ifndef MM_SAVE_STATE_LIB_H_
+#define MM_SAVE_STATE_LIB_H_
+
+#include <Protocol/MmCpu.h>
+#include <Uefi/UefiBaseType.h>
+
+/**
+  Read a save state register on the target processor.  If this function
+  returns EFI_UNSUPPORTED, then the caller is responsible for reading the
+  MM Save State register.
+
+  @param[in]  CpuIndex  The index of the CPU to read the Save State register.
+                        The value must be between 0 and the NumberOfCpus field in
+                        the System Management System Table (SMST).
+  @param[in]  Register  The MM Save State register to read.
+  @param[in]  Width     The number of bytes to read from the CPU save state.
+  @param[out] Buffer    Upon return, this holds the CPU register value read
+                        from the save state.
+
+  @retval EFI_SUCCESS           The register was read from Save State.
+  @retval EFI_INVALID_PARAMTER  Buffer is NULL.
+  @retval EFI_UNSUPPORTED       This function does not support reading Register.
+  @retval EFI_NOT_FOUND         If desired Register not found.
+**/
+EFI_STATUS
+EFIAPI
+MmSaveStateReadRegister (
+  IN  UINTN                       CpuIndex,
+  IN  EFI_MM_SAVE_STATE_REGISTER  Register,
+  IN  UINTN                       Width,
+  OUT VOID                        *Buffer
+  );
+
+/**
+  Writes a save state register on the target processor.  If this function
+  returns EFI_UNSUPPORTED, then the caller is responsible for writing the
+  MM save state register.
+
+  @param[in] CpuIndex  The index of the CPU to write the MM Save State.  The
+                       value must be between 0 and the NumberOfCpus field in
+                       the System Management System Table (SMST).
+  @param[in] Register  The MM Save State register to write.
+  @param[in] Width     The number of bytes to write to the CPU save state.
+  @param[in] Buffer    Upon entry, this holds the new CPU register value.
+
+  @retval EFI_SUCCESS           The register was written to Save State.
+  @retval EFI_INVALID_PARAMTER  Buffer is NULL.
+  @retval EFI_UNSUPPORTED       This function does not support writing Register.
+  @retval EFI_NOT_FOUND         If desired Register not found.
+**/
+EFI_STATUS
+EFIAPI
+MmSaveStateWriteRegister (
+  IN UINTN                       CpuIndex,
+  IN EFI_MM_SAVE_STATE_REGISTER  Register,
+  IN UINTN                       Width,
+  IN CONST VOID                  *Buffer
+  );
+
+#endif
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Include/Library/SmmCpuFeaturesLib.h
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Include/Library/SmmCpuFeaturesLib.h	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Include/Library/SmmCpuFeaturesLib.h	(revision 101291)
@@ -326,56 +326,4 @@
 
 /**
-  Read an SMM Save State register on the target processor.  If this function
-  returns EFI_UNSUPPORTED, then the caller is responsible for reading the
-  SMM Save Sate register.
-
-  @param[in]  CpuIndex  The index of the CPU to read the SMM Save State.  The
-                        value must be between 0 and the NumberOfCpus field in
-                        the System Management System Table (SMST).
-  @param[in]  Register  The SMM Save State register to read.
-  @param[in]  Width     The number of bytes to read from the CPU save state.
-  @param[out] Buffer    Upon return, this holds the CPU register value read
-                        from the save state.
-
-  @retval EFI_SUCCESS           The register was read from Save State.
-  @retval EFI_INVALID_PARAMETER  Buffer is NULL.
-  @retval EFI_UNSUPPORTED       This function does not support reading Register.
-
-**/
-EFI_STATUS
-EFIAPI
-SmmCpuFeaturesReadSaveStateRegister (
-  IN  UINTN                        CpuIndex,
-  IN  EFI_SMM_SAVE_STATE_REGISTER  Register,
-  IN  UINTN                        Width,
-  OUT VOID                         *Buffer
-  );
-
-/**
-  Writes an SMM Save State register on the target processor.  If this function
-  returns EFI_UNSUPPORTED, then the caller is responsible for writing the
-  SMM Save Sate register.
-
-  @param[in] CpuIndex  The index of the CPU to write the SMM Save State.  The
-                       value must be between 0 and the NumberOfCpus field in
-                       the System Management System Table (SMST).
-  @param[in] Register  The SMM Save State register to write.
-  @param[in] Width     The number of bytes to write to the CPU save state.
-  @param[in] Buffer    Upon entry, this holds the new CPU register value.
-
-  @retval EFI_SUCCESS           The register was written to Save State.
-  @retval EFI_INVALID_PARAMETER  Buffer is NULL.
-  @retval EFI_UNSUPPORTED       This function does not support writing Register.
-**/
-EFI_STATUS
-EFIAPI
-SmmCpuFeaturesWriteSaveStateRegister (
-  IN UINTN                        CpuIndex,
-  IN EFI_SMM_SAVE_STATE_REGISTER  Register,
-  IN UINTN                        Width,
-  IN CONST VOID                   *Buffer
-  );
-
-/**
   This function is hook point called after the gEfiSmmReadyToLockProtocolGuid
   notification is completely processed.
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Include/Library/UefiCpuLib.h
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Include/Library/UefiCpuLib.h	(revision 101283)
+++ 	(revision )
@@ -1,65 +1,0 @@
-/** @file
-  Public header file for UEFI CPU library class.
-
-  This library class defines some routines that are generic for IA32 family CPU
-  to be UEFI specification compliant.
-
-  Copyright (c) 2009 - 2021, Intel Corporation. All rights reserved.<BR>
-  Copyright (c) 2020, AMD Inc. All rights reserved.<BR>
-  SPDX-License-Identifier: BSD-2-Clause-Patent
-
-**/
-
-#ifndef __UEFI_CPU_LIB_H__
-#define __UEFI_CPU_LIB_H__
-
-/**
-  Initializes floating point units for requirement of UEFI specification.
-
-  This function initializes floating-point control word to 0x027F (all exceptions
-  masked,double-precision, round-to-nearest) and multimedia-extensions control word
-  (if supported) to 0x1F80 (all exceptions masked, round-to-nearest, flush to zero
-  for masked underflow).
-
-**/
-VOID
-EFIAPI
-InitializeFloatingPointUnits (
-  VOID
-  );
-
-/**
-  Determine if the standard CPU signature is "AuthenticAMD".
-
-  @retval TRUE  The CPU signature matches.
-  @retval FALSE The CPU signature does not match.
-
-**/
-BOOLEAN
-EFIAPI
-StandardSignatureIsAuthenticAMD (
-  VOID
-  );
-
-/**
-  Return the 32bit CPU family and model value.
-
-  @return CPUID[01h].EAX with Processor Type and Stepping ID cleared.
-**/
-UINT32
-EFIAPI
-GetCpuFamilyModel (
-  VOID
-  );
-
-/**
-  Return the CPU stepping ID.
-  @return CPU stepping ID value in CPUID[01h].EAX.
-**/
-UINT8
-EFIAPI
-GetCpuSteppingId (
-  VOID
-  );
-
-#endif
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/BaseRiscV64CpuExceptionHandlerLib/CpuExceptionHandlerLib.c
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/BaseRiscV64CpuExceptionHandlerLib/CpuExceptionHandlerLib.c	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/BaseRiscV64CpuExceptionHandlerLib/CpuExceptionHandlerLib.c	(revision 101291)
@@ -12,9 +12,166 @@
 #include <Library/DebugLib.h>
 #include <Library/BaseLib.h>
+#include <Library/SerialPortLib.h>
+#include <Library/PrintLib.h>
 #include <Register/RiscV64/RiscVEncoding.h>
-
 #include "CpuExceptionHandlerLib.h"
 
-STATIC EFI_CPU_INTERRUPT_HANDLER  mInterruptHandlers[2];
+//
+// Define the maximum message length
+//
+#define MAX_DEBUG_MESSAGE_LENGTH  0x100
+
+STATIC EFI_CPU_INTERRUPT_HANDLER  mExceptionHandlers[EXCEPT_RISCV_MAX_EXCEPTIONS + 1];
+STATIC EFI_CPU_INTERRUPT_HANDLER  mIrqHandlers[EXCEPT_RISCV_MAX_IRQS + 1];
+
+STATIC CONST CHAR8  mExceptionOrIrqUnknown[]                            = "Unknown";
+STATIC CONST CHAR8  *mExceptionNameStr[EXCEPT_RISCV_MAX_EXCEPTIONS + 1] = {
+  "EXCEPT_RISCV_INST_MISALIGNED",
+  "EXCEPT_RISCV_INST_ACCESS_FAULT",
+  "EXCEPT_RISCV_ILLEGAL_INST",
+  "EXCEPT_RISCV_BREAKPOINT",
+  "EXCEPT_RISCV_LOAD_ADDRESS_MISALIGNED",
+  "EXCEPT_RISCV_LOAD_ACCESS_FAULT",
+  "EXCEPT_RISCV_STORE_AMO_ADDRESS_MISALIGNED",
+  "EXCEPT_RISCV_STORE_AMO_ACCESS_FAULT",
+  "EXCEPT_RISCV_ENV_CALL_FROM_UMODE",
+  "EXCEPT_RISCV_ENV_CALL_FROM_SMODE",
+  "EXCEPT_RISCV_ENV_CALL_FROM_VS_MODE",
+  "EXCEPT_RISCV_ENV_CALL_FROM_MMODE",
+  "EXCEPT_RISCV_INST_ACCESS_PAGE_FAULT",
+  "EXCEPT_RISCV_LOAD_ACCESS_PAGE_FAULT",
+  "EXCEPT_RISCV_14",
+  "EXCEPT_RISCV_STORE_ACCESS_PAGE_FAULT",
+  "EXCEPT_RISCV_16",
+  "EXCEPT_RISCV_17",
+  "EXCEPT_RISCV_18",
+  "EXCEPT_RISCV_19",
+  "EXCEPT_RISCV_INST_GUEST_PAGE_FAULT",
+  "EXCEPT_RISCV_LOAD_GUEST_PAGE_FAULT",
+  "EXCEPT_RISCV_VIRTUAL_INSTRUCTION",
+  "EXCEPT_RISCV_STORE_GUEST_PAGE_FAULT"
+};
+
+STATIC CONST CHAR8  *mIrqNameStr[EXCEPT_RISCV_MAX_IRQS + 1] = {
+  "EXCEPT_RISCV_IRQ_0",
+  "EXCEPT_RISCV_IRQ_SOFT_FROM_SMODE",
+  "EXCEPT_RISCV_IRQ_SOFT_FROM_VSMODE",
+  "EXCEPT_RISCV_IRQ_SOFT_FROM_MMODE",
+  "EXCEPT_RISCV_IRQ_4",
+  "EXCEPT_RISCV_IRQ_TIMER_FROM_SMODE",
+};
+
+/**
+  Prints a message to the serial port.
+
+  @param  Format      Format string for the message to print.
+  @param  ...         Variable argument list whose contents are accessed
+                      based on the format string specified by Format.
+
+**/
+STATIC
+VOID
+EFIAPI
+InternalPrintMessage (
+  IN  CONST CHAR8  *Format,
+  ...
+  )
+{
+  CHAR8    Buffer[MAX_DEBUG_MESSAGE_LENGTH];
+  VA_LIST  Marker;
+
+  //
+  // Convert the message to an ASCII String
+  //
+  VA_START (Marker, Format);
+  AsciiVSPrint (Buffer, sizeof (Buffer), Format, Marker);
+  VA_END (Marker);
+
+  //
+  // Send the print string to a Serial Port
+  //
+  SerialPortWrite ((UINT8 *)Buffer, AsciiStrLen (Buffer));
+}
+
+/**
+  Get ASCII format string exception name by exception type.
+
+  @param ExceptionType  Exception type.
+
+  @return  ASCII format string exception name.
+**/
+STATIC
+CONST CHAR8 *
+GetExceptionNameStr (
+  IN EFI_EXCEPTION_TYPE  ExceptionType
+  )
+{
+  if (EXCEPT_RISCV_IS_IRQ (ExceptionType)) {
+    if (EXCEPT_RISCV_IRQ_INDEX (ExceptionType) > EXCEPT_RISCV_MAX_IRQS) {
+      return mExceptionOrIrqUnknown;
+    }
+
+    return mIrqNameStr[EXCEPT_RISCV_IRQ_INDEX (ExceptionType)];
+  }
+
+  if (ExceptionType > EXCEPT_RISCV_MAX_EXCEPTIONS) {
+    return mExceptionOrIrqUnknown;
+  }
+
+  return mExceptionNameStr[ExceptionType];
+}
+
+/**
+  Display CPU information. This can be called by 3rd-party handlers
+  set by RegisterCpuInterruptHandler.
+
+  @param ExceptionType  Exception type.
+  @param SystemContext  Pointer to EFI_SYSTEM_CONTEXT.
+**/
+VOID
+EFIAPI
+DumpCpuContext (
+  IN EFI_EXCEPTION_TYPE  ExceptionType,
+  IN EFI_SYSTEM_CONTEXT  SystemContext
+  )
+{
+  UINTN                 Printed;
+  SMODE_TRAP_REGISTERS  *Regs;
+
+  Printed = 0;
+  Regs    = (SMODE_TRAP_REGISTERS *)SystemContext.SystemContextRiscV64;
+
+  InternalPrintMessage (
+    "!!!! RISCV64 Exception Type - %016x(%a) !!!!\n",
+    ExceptionType,
+    GetExceptionNameStr (ExceptionType)
+    );
+
+  DEBUG_CODE_BEGIN ();
+
+  #define REGS()                                                          \
+  REG (t0); REG (t1); REG (t2); REG (t3); REG (t4); REG (t5); REG (t6); \
+  REG (s0); REG (s1); REG (s2); REG (s3); REG (s4); REG (s5); REG (s6); \
+  REG (s7); REG (s8); REG (s9); REG (s10); REG (s11);                   \
+  REG (a0); REG (a1); REG (a2); REG (a3); REG (a4); REG (a5); REG (a6); \
+  REG (a7);                                                             \
+  REG (zero); REG (ra); REG (sp); REG (gp); REG (tp);                   \
+  REG (sepc); REG (sstatus); REG (stval);
+
+  #define REG(x)  do {                                      \
+    InternalPrintMessage ("%7a = 0x%017lx%c", #x, Regs->x,  \
+                          (++Printed % 2) ? L'\t' : L'\n'); \
+  } while (0);
+
+  REGS ();
+  if (Printed % 2 != 0) {
+    InternalPrintMessage ("\n");
+  }
+
+  #undef REG
+  #undef REGS
+
+  DEBUG_CODE_END ();
+}
 
 /**
@@ -48,11 +205,11 @@
 
   This function registers and enables the handler specified by InterruptHandler for a processor
-  interrupt or exception type specified by InterruptType. If InterruptHandler is NULL, then the
-  handler for the processor interrupt or exception type specified by InterruptType is uninstalled.
+  interrupt or exception type specified by ExceptionType. If InterruptHandler is NULL, then the
+  handler for the processor interrupt or exception type specified by ExceptionType is uninstalled.
   The installed handler is called once for each processor interrupt or exception.
   NOTE: This function should be invoked after InitializeCpuExceptionHandlers() or
   InitializeCpuInterruptHandlers() invoked, otherwise EFI_UNSUPPORTED returned.
 
-  @param[in]  InterruptType     Defines which interrupt or exception to hook.
+  @param[in]  ExceptionType     Defines which interrupt or exception to hook.
   @param[in]  InterruptHandler  A pointer to a function of type EFI_CPU_INTERRUPT_HANDLER that is called
                                 when a processor interrupt occurs. If this parameter is NULL, then the handler
@@ -60,9 +217,9 @@
 
   @retval EFI_SUCCESS           The handler for the processor interrupt was successfully installed or uninstalled.
-  @retval EFI_ALREADY_STARTED   InterruptHandler is not NULL, and a handler for InterruptType was
+  @retval EFI_ALREADY_STARTED   InterruptHandler is not NULL, and a handler for ExceptionType was
                                 previously installed.
-  @retval EFI_INVALID_PARAMETER InterruptHandler is NULL, and a handler for InterruptType was not
+  @retval EFI_INVALID_PARAMETER InterruptHandler is NULL, and a handler for ExceptionType was not
                                 previously installed.
-  @retval EFI_UNSUPPORTED       The interrupt specified by InterruptType is not supported,
+  @retval EFI_UNSUPPORTED       The interrupt specified by ExceptionType is not supported,
                                 or this function is not supported.
 **/
@@ -70,10 +227,35 @@
 EFIAPI
 RegisterCpuInterruptHandler (
-  IN EFI_EXCEPTION_TYPE         InterruptType,
+  IN EFI_EXCEPTION_TYPE         ExceptionType,
   IN EFI_CPU_INTERRUPT_HANDLER  InterruptHandler
   )
 {
-  DEBUG ((DEBUG_INFO, "%a: Type:%x Handler: %x\n", __FUNCTION__, InterruptType, InterruptHandler));
-  mInterruptHandlers[InterruptType] = InterruptHandler;
+  DEBUG ((DEBUG_INFO, "%a: Type:%x Handler: %x\n", __func__, ExceptionType, InterruptHandler));
+  if (EXCEPT_RISCV_IS_IRQ (ExceptionType)) {
+    if (EXCEPT_RISCV_IRQ_INDEX (ExceptionType) > EXCEPT_RISCV_MAX_IRQS) {
+      return EFI_UNSUPPORTED;
+    }
+
+    if (mIrqHandlers[EXCEPT_RISCV_IRQ_INDEX (ExceptionType)] != NULL) {
+      return EFI_ALREADY_STARTED;
+    } else if (InterruptHandler == NULL) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    mIrqHandlers[EXCEPT_RISCV_IRQ_INDEX (ExceptionType)] = InterruptHandler;
+  } else {
+    if (ExceptionType > EXCEPT_RISCV_MAX_EXCEPTIONS) {
+      return EFI_UNSUPPORTED;
+    }
+
+    if (mExceptionHandlers[ExceptionType] != NULL) {
+      return EFI_ALREADY_STARTED;
+    } else if (InterruptHandler == NULL) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    mExceptionHandlers[ExceptionType] = InterruptHandler;
+  }
+
   return EFI_SUCCESS;
 }
@@ -114,20 +296,30 @@
   )
 {
-  UINTN               SCause;
+  EFI_EXCEPTION_TYPE  ExceptionType;
   EFI_SYSTEM_CONTEXT  RiscVSystemContext;
+  UINTN               IrqIndex;
 
   RiscVSystemContext.SystemContextRiscV64 = (EFI_SYSTEM_CONTEXT_RISCV64 *)SmodeTrapReg;
-  //
-  // Check scasue register.
-  //
-  SCause = (UINTN)RiscVGetSupervisorTrapCause ();
-  if ((SCause & (1UL << (sizeof (UINTN) * 8- 1))) != 0) {
-    //
-    // This is interrupt event.
-    //
-    SCause &= ~(1UL << (sizeof (UINTN) * 8- 1));
-    if ((SCause == IRQ_S_TIMER) && (mInterruptHandlers[EXCEPT_RISCV_TIMER_INT] != NULL)) {
-      mInterruptHandlers[EXCEPT_RISCV_TIMER_INT](EXCEPT_RISCV_TIMER_INT, RiscVSystemContext);
-    }
-  }
-}
+  ExceptionType                           = (UINTN)RiscVGetSupervisorTrapCause ();
+
+  if (EXCEPT_RISCV_IS_IRQ (ExceptionType)) {
+    IrqIndex = EXCEPT_RISCV_IRQ_INDEX (ExceptionType);
+
+    if ((IrqIndex <= EXCEPT_RISCV_MAX_IRQS) &&
+        (mIrqHandlers[IrqIndex] != NULL))
+    {
+      mIrqHandlers[IrqIndex](ExceptionType, RiscVSystemContext);
+      return;
+    }
+  } else {
+    if ((ExceptionType <= EXCEPT_RISCV_MAX_EXCEPTIONS) &&
+        (mExceptionHandlers[ExceptionType] != 0))
+    {
+      mExceptionHandlers[ExceptionType](ExceptionType, RiscVSystemContext);
+      return;
+    }
+  }
+
+  DumpCpuContext (ExceptionType, RiscVSystemContext);
+  CpuDeadLoop ();
+}
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/BaseRiscV64CpuExceptionHandlerLib/CpuExceptionHandlerLib.h
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/BaseRiscV64CpuExceptionHandlerLib/CpuExceptionHandlerLib.h	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/BaseRiscV64CpuExceptionHandlerLib/CpuExceptionHandlerLib.h	(revision 101291)
@@ -60,5 +60,5 @@
 #define SMODE_TRAP_REGS_sepc     32
 #define SMODE_TRAP_REGS_sstatus  33
-#define SMODE_TRAP_REGS_sie      34
+#define SMODE_TRAP_REGS_stval    34
 #define SMODE_TRAP_REGS_last     35
 
@@ -69,5 +69,5 @@
 typedef struct {
   //
-  // Below are follow the format of EFI_SYSTEM_CONTEXT
+  // Below follow the format of EFI_SYSTEM_CONTEXT.
   //
   UINT64    zero;
@@ -103,12 +103,7 @@
   UINT64    t5;
   UINT64    t6;
-  //
-  // Below are the additional information to
-  // EFI_SYSTEM_CONTEXT, private to supervisor mode trap
-  // and not public to EFI environment.
-  //
   UINT64    sepc;
   UINT64    sstatus;
-  UINT64    sie;
+  UINT64    stval;
 } SMODE_TRAP_REGISTERS;
 #pragma pack()
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/BaseRiscV64CpuExceptionHandlerLib/SupervisorTrapHandler.S
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/BaseRiscV64CpuExceptionHandlerLib/SupervisorTrapHandler.S	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/BaseRiscV64CpuExceptionHandlerLib/SupervisorTrapHandler.S	(revision 101291)
@@ -21,12 +21,12 @@
 
   csrr  t0, CSR_SSTATUS
-  and   t0, t0, (SSTATUS_SIE | SSTATUS_SPIE)
   sd    t0, SMODE_TRAP_REGS_OFFSET(sstatus)(sp)
   csrr  t0, CSR_SEPC
   sd    t0, SMODE_TRAP_REGS_OFFSET(sepc)(sp)
-  csrr  t0, CSR_SIE
-  sd    t0, SMODE_TRAP_REGS_OFFSET(sie)(sp)
+  csrr  t0, CSR_STVAL
+  sd    t0, SMODE_TRAP_REGS_OFFSET(stval)(sp)
   ld    t0, SMODE_TRAP_REGS_OFFSET(t0)(sp)
 
+  sd    zero, SMODE_TRAP_REGS_OFFSET(zero)(sp)
   sd    ra, SMODE_TRAP_REGS_OFFSET(ra)(sp)
   sd    gp, SMODE_TRAP_REGS_OFFSET(gp)(sp)
@@ -60,4 +60,5 @@
 
   /* Call to Supervisor mode trap handler in CpuExceptionHandlerLib.c */
+  mv    a0, sp
   call  RiscVSupervisorModeTrapHandler
 
@@ -67,4 +68,5 @@
   ld    tp, SMODE_TRAP_REGS_OFFSET(tp)(sp)
   ld    t2, SMODE_TRAP_REGS_OFFSET(t2)(sp)
+  ld    t1, SMODE_TRAP_REGS_OFFSET(t1)(sp)
   ld    s0, SMODE_TRAP_REGS_OFFSET(s0)(sp)
   ld    s1, SMODE_TRAP_REGS_OFFSET(s1)(sp)
@@ -94,11 +96,8 @@
   ld    t0, SMODE_TRAP_REGS_OFFSET(sepc)(sp)
   csrw  CSR_SEPC, t0
-  ld    t0, SMODE_TRAP_REGS_OFFSET(sie)(sp)
-  csrw  CSR_SIE, t0
-  csrr  t0, CSR_SSTATUS
-  ld    t1, SMODE_TRAP_REGS_OFFSET(sstatus)(sp)
-  or    t0, t0, t1
+  ld    t0, SMODE_TRAP_REGS_OFFSET(sstatus)(sp)
   csrw  CSR_SSTATUS, t0
-  ld    t1, SMODE_TRAP_REGS_OFFSET(t1)(sp)
+  ld    t0, SMODE_TRAP_REGS_OFFSET(stval)(sp)
+  csrw  CSR_STVAL, t0
   ld    t0, SMODE_TRAP_REGS_OFFSET(t0)(sp)
   addi  sp, sp, SMODE_TRAP_REGS_SIZE
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/BaseRiscV64CpuTimerLib/CpuTimerLib.c
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/BaseRiscV64CpuTimerLib/CpuTimerLib.c	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/BaseRiscV64CpuTimerLib/CpuTimerLib.c	(revision 101291)
@@ -23,24 +23,17 @@
 
 **/
+STATIC
 VOID
 InternalRiscVTimerDelay (
-  IN UINT32  Delay
+  IN UINT64  Delay
   )
 {
-  UINT32  Ticks;
-  UINT32  Times;
+  UINT64  Ticks;
 
-  Times  = Delay >> (RISCV_TIMER_COMPARE_BITS - 2);
-  Delay &= ((1 << (RISCV_TIMER_COMPARE_BITS - 2)) - 1);
-  do {
-    //
-    // The target timer count is calculated here
-    //
-    Ticks = RiscVReadTimer () + Delay;
-    Delay = 1 << (RISCV_TIMER_COMPARE_BITS - 2);
-    while (((Ticks - RiscVReadTimer ()) & (1 << (RISCV_TIMER_COMPARE_BITS - 1))) == 0) {
-      CpuPause ();
-    }
-  } while (Times-- > 0);
+  Ticks = RiscVReadTimer () + Delay;
+
+  while (RiscVReadTimer () <= Ticks) {
+    CpuPause ();
+  }
 }
 
@@ -62,11 +55,11 @@
 {
   InternalRiscVTimerDelay (
-    (UINT32)DivU64x32 (
-              MultU64x32 (
-                MicroSeconds,
-                PcdGet64 (PcdCpuCoreCrystalClockFrequency)
-                ),
-              1000000u
-              )
+    DivU64x32 (
+      MultU64x32 (
+        MicroSeconds,
+        PcdGet64 (PcdCpuCoreCrystalClockFrequency)
+        ),
+      1000000u
+      )
     );
   return MicroSeconds;
@@ -90,11 +83,11 @@
 {
   InternalRiscVTimerDelay (
-    (UINT32)DivU64x32 (
-              MultU64x32 (
-                NanoSeconds,
-                PcdGet64 (PcdCpuCoreCrystalClockFrequency)
-                ),
-              1000000000u
-              )
+    DivU64x32 (
+      MultU64x32 (
+        NanoSeconds,
+        PcdGet64 (PcdCpuCoreCrystalClockFrequency)
+        ),
+      1000000000u
+      )
     );
   return NanoSeconds;
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/BaseRiscVMmuLib/BaseRiscVMmuLib.c
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/BaseRiscVMmuLib/BaseRiscVMmuLib.c	(revision 101291)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/BaseRiscVMmuLib/BaseRiscVMmuLib.c	(revision 101291)
@@ -0,0 +1,730 @@
+/** @file
+  MMU library for RISC-V.
+
+  Copyright (c) 2011-2020, ARM Limited. All rights reserved.
+  Copyright (c) 2016, Linaro Limited. All rights reserved.
+  Copyright (c) 2017, Intel Corporation. All rights reserved.<BR>
+  Copyright (c) 2023, Ventana Micro Systems Inc. All Rights Reserved.<BR>
+
+  SPDX-License-Identifier: BSD-2-Clause-Patent
+
+**/
+
+#include <PiDxe.h>
+#include <Uefi.h>
+#include <Library/BaseLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/BaseRiscVMmuLib.h>
+#include <Library/CacheMaintenanceLib.h>
+#include <Library/DebugLib.h>
+#include <Library/DxeServicesTableLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/PcdLib.h>
+#include <Register/RiscV64/RiscVEncoding.h>
+
+#define RISCV_PG_V           BIT0
+#define RISCV_PG_R           BIT1
+#define RISCV_PG_W           BIT2
+#define RISCV_PG_X           BIT3
+#define RISCV_PG_G           BIT5
+#define RISCV_PG_A           BIT6
+#define RISCV_PG_D           BIT7
+#define PTE_ATTRIBUTES_MASK  0xE
+
+#define PTE_PPN_MASK          0x3FFFFFFFFFFC00ULL
+#define PTE_PPN_SHIFT         10
+#define RISCV_MMU_PAGE_SHIFT  12
+
+STATIC UINTN  mModeSupport[] = { SATP_MODE_SV57, SATP_MODE_SV48, SATP_MODE_SV39 };
+STATIC UINTN  mMaxRootTableLevel;
+STATIC UINTN  mBitPerLevel;
+STATIC UINTN  mTableEntryCount;
+
+/**
+  Determine if the MMU enabled or not.
+
+  @retval TRUE  The MMU already enabled.
+  @retval FALSE The MMU not enabled.
+
+**/
+STATIC
+BOOLEAN
+RiscVMmuEnabled (
+  VOID
+  )
+{
+  return ((RiscVGetSupervisorAddressTranslationRegister () &
+           SATP64_MODE) != (SATP_MODE_OFF << SATP64_MODE_SHIFT));
+}
+
+/**
+  Retrieve the root translate table.
+
+  @return The root translate table.
+
+**/
+STATIC
+UINTN
+RiscVGetRootTranslateTable (
+  VOID
+  )
+{
+  return (RiscVGetSupervisorAddressTranslationRegister () & SATP64_PPN) <<
+         RISCV_MMU_PAGE_SHIFT;
+}
+
+/**
+  Determine if an entry is valid pte.
+
+  @param    Entry   The entry value.
+
+  @retval   TRUE    The entry is a valid pte.
+  @retval   FALSE   The entry is not a valid pte.
+
+**/
+STATIC
+BOOLEAN
+IsValidPte (
+  IN  UINTN  Entry
+  )
+{
+  if (((Entry & RISCV_PG_V) == 0) ||
+      (((Entry & (RISCV_PG_R | RISCV_PG_W)) == RISCV_PG_W)))
+  {
+    return FALSE;
+  }
+
+  return TRUE;
+}
+
+/**
+  Set an entry to be a valid pte.
+
+  @param  Entry   The entry value.
+
+  @return         The entry value.
+
+**/
+STATIC
+UINTN
+SetValidPte (
+  IN  UINTN  Entry
+  )
+{
+  /* Set Valid and Global mapping bits */
+  return Entry | RISCV_PG_G | RISCV_PG_V;
+}
+
+/**
+  Determine if an entry is a block pte.
+
+  @param    Entry   The entry value.
+
+  @retval   TRUE    The entry is a block pte.
+  @retval   FALSE   The entry is not a block pte.
+
+**/
+STATIC
+BOOLEAN
+IsBlockEntry (
+  IN  UINTN  Entry
+  )
+{
+  return IsValidPte (Entry) &&
+         (Entry & (RISCV_PG_X | RISCV_PG_R));
+}
+
+/**
+  Determine if an entry is a table pte.
+
+  @param    Entry   The entry value.
+
+  @retval   TRUE    The entry is a table pte.
+  @retval   FALSE   The entry is not a table pte.
+
+**/
+STATIC
+BOOLEAN
+IsTableEntry (
+  IN  UINTN  Entry
+  )
+{
+  return IsValidPte (Entry) &&
+         !IsBlockEntry (Entry);
+}
+
+/**
+  Set an entry to be a table pte.
+
+  @param  Entry   The entry value.
+
+  @return         The entry value.
+
+**/
+STATIC
+UINTN
+SetTableEntry (
+  IN  UINTN  Entry
+  )
+{
+  Entry  = SetValidPte (Entry);
+  Entry &= ~(RISCV_PG_X | RISCV_PG_W | RISCV_PG_R);
+
+  return Entry;
+}
+
+/**
+  Replace an existing entry with new value.
+
+  @param  Entry               The entry pointer.
+  @param  Value               The new entry value.
+  @param  RegionStart         The start of region that new value affects.
+  @param  IsLiveBlockMapping  TRUE if this is live update, FALSE otherwise.
+
+**/
+STATIC
+VOID
+ReplaceTableEntry (
+  IN  UINTN    *Entry,
+  IN  UINTN    Value,
+  IN  UINTN    RegionStart,
+  IN  BOOLEAN  IsLiveBlockMapping
+  )
+{
+  *Entry = Value;
+
+  if (IsLiveBlockMapping && RiscVMmuEnabled ()) {
+    RiscVLocalTlbFlush (RegionStart);
+  }
+}
+
+/**
+  Get an ppn value from an entry.
+
+  @param  Entry   The entry value.
+
+  @return         The ppn value.
+
+**/
+STATIC
+UINTN
+GetPpnfromPte (
+  IN UINTN  Entry
+  )
+{
+  return ((Entry & PTE_PPN_MASK) >> PTE_PPN_SHIFT);
+}
+
+/**
+  Set an ppn value to a entry.
+
+  @param  Entry   The entry value.
+  @param  Address The address.
+
+  @return The new entry value.
+
+**/
+STATIC
+UINTN
+SetPpnToPte (
+  UINTN  Entry,
+  UINTN  Address
+  )
+{
+  UINTN  Ppn;
+
+  Ppn = ((Address >> RISCV_MMU_PAGE_SHIFT) << PTE_PPN_SHIFT);
+  ASSERT (~(Ppn & ~PTE_PPN_MASK));
+  Entry &= ~PTE_PPN_MASK;
+  return Entry | Ppn;
+}
+
+/**
+  Free resources of translation table recursively.
+
+  @param  TranslationTable  The pointer of table.
+  @param  Level             The current level.
+
+**/
+STATIC
+VOID
+FreePageTablesRecursive (
+  IN  UINTN  *TranslationTable,
+  IN  UINTN  Level
+  )
+{
+  UINTN  Index;
+
+  if (Level < mMaxRootTableLevel - 1) {
+    for (Index = 0; Index < mTableEntryCount; Index++) {
+      if (IsTableEntry (TranslationTable[Index])) {
+        FreePageTablesRecursive (
+          (UINTN *)(GetPpnfromPte ((TranslationTable[Index])) <<
+                    RISCV_MMU_PAGE_SHIFT),
+          Level + 1
+          );
+      }
+    }
+  }
+
+  FreePages (TranslationTable, 1);
+}
+
+/**
+  Update region mapping recursively.
+
+  @param  RegionStart           The start address of the region.
+  @param  RegionEnd             The end address of the region.
+  @param  AttributeSetMask      The attribute mask to be set.
+  @param  AttributeClearMask    The attribute mask to be clear.
+  @param  PageTable             The pointer of current page table.
+  @param  Level                 The current level.
+  @param  TableIsLive           TRUE if this is live update, FALSE otherwise.
+
+  @retval EFI_OUT_OF_RESOURCES  Not enough resource.
+  @retval EFI_SUCCESS           The operation succesfully.
+
+**/
+STATIC
+EFI_STATUS
+UpdateRegionMappingRecursive (
+  IN  UINTN    RegionStart,
+  IN  UINTN    RegionEnd,
+  IN  UINTN    AttributeSetMask,
+  IN  UINTN    AttributeClearMask,
+  IN  UINTN    *PageTable,
+  IN  UINTN    Level,
+  IN  BOOLEAN  TableIsLive
+  )
+{
+  EFI_STATUS  Status;
+  UINTN       BlockShift;
+  UINTN       BlockMask;
+  UINTN       BlockEnd;
+  UINTN       *Entry;
+  UINTN       EntryValue;
+  UINTN       *TranslationTable;
+  BOOLEAN     NextTableIsLive;
+
+  ASSERT (Level < mMaxRootTableLevel);
+  ASSERT (((RegionStart | RegionEnd) & EFI_PAGE_MASK) == 0);
+
+  BlockShift = (mMaxRootTableLevel - Level - 1) * mBitPerLevel + RISCV_MMU_PAGE_SHIFT;
+  BlockMask  = MAX_ADDRESS >> (64 - BlockShift);
+
+  DEBUG (
+    (
+     DEBUG_VERBOSE,
+     "%a(%d): %llx - %llx set %lx clr %lx\n",
+     __func__,
+     Level,
+     RegionStart,
+     RegionEnd,
+     AttributeSetMask,
+     AttributeClearMask
+    )
+    );
+
+  for ( ; RegionStart < RegionEnd; RegionStart = BlockEnd) {
+    BlockEnd = MIN (RegionEnd, (RegionStart | BlockMask) + 1);
+    Entry    = &PageTable[(RegionStart >> BlockShift) & (mTableEntryCount - 1)];
+
+    //
+    // If RegionStart or BlockEnd is not aligned to the block size at this
+    // level, we will have to create a table mapping in order to map less
+    // than a block, and recurse to create the block or page entries at
+    // the next level. No block mappings are allowed at all at level 0,
+    // so in that case, we have to recurse unconditionally.
+    //
+    if ((Level == 0) ||
+        (((RegionStart | BlockEnd) & BlockMask) != 0) || IsTableEntry (*Entry))
+    {
+      ASSERT (Level < mMaxRootTableLevel - 1);
+      if (!IsTableEntry (*Entry)) {
+        //
+        // No table entry exists yet, so we need to allocate a page table
+        // for the next level.
+        //
+        TranslationTable = AllocatePages (1);
+        if (TranslationTable == NULL) {
+          return EFI_OUT_OF_RESOURCES;
+        }
+
+        ZeroMem (TranslationTable, EFI_PAGE_SIZE);
+
+        if (IsBlockEntry (*Entry)) {
+          //
+          // We are splitting an existing block entry, so we have to populate
+          // the new table with the attributes of the block entry it replaces.
+          //
+          Status = UpdateRegionMappingRecursive (
+                     RegionStart & ~BlockMask,
+                     (RegionStart | BlockMask) + 1,
+                     *Entry & PTE_ATTRIBUTES_MASK,
+                     PTE_ATTRIBUTES_MASK,
+                     TranslationTable,
+                     Level + 1,
+                     FALSE
+                     );
+          if (EFI_ERROR (Status)) {
+            //
+            // The range we passed to UpdateRegionMappingRecursive () is block
+            // aligned, so it is guaranteed that no further pages were allocated
+            // by it, and so we only have to free the page we allocated here.
+            //
+            FreePages (TranslationTable, 1);
+            return Status;
+          }
+        }
+
+        NextTableIsLive = FALSE;
+      } else {
+        TranslationTable = (UINTN *)(GetPpnfromPte (*Entry) << RISCV_MMU_PAGE_SHIFT);
+        NextTableIsLive  = TableIsLive;
+      }
+
+      //
+      // Recurse to the next level
+      //
+      Status = UpdateRegionMappingRecursive (
+                 RegionStart,
+                 BlockEnd,
+                 AttributeSetMask,
+                 AttributeClearMask,
+                 TranslationTable,
+                 Level + 1,
+                 NextTableIsLive
+                 );
+      if (EFI_ERROR (Status)) {
+        if (!IsTableEntry (*Entry)) {
+          //
+          // We are creating a new table entry, so on failure, we can free all
+          // allocations we made recursively, given that the whole subhierarchy
+          // has not been wired into the live page tables yet. (This is not
+          // possible for existing table entries, since we cannot revert the
+          // modifications we made to the subhierarchy it represents.)
+          //
+          FreePageTablesRecursive (TranslationTable, Level + 1);
+        }
+
+        return Status;
+      }
+
+      if (!IsTableEntry (*Entry)) {
+        EntryValue = SetPpnToPte (0, (UINTN)TranslationTable);
+        EntryValue = SetTableEntry (EntryValue);
+        ReplaceTableEntry (
+          Entry,
+          EntryValue,
+          RegionStart,
+          TableIsLive
+          );
+      }
+    } else {
+      EntryValue = (*Entry & ~AttributeClearMask) | AttributeSetMask;
+      //
+      // We don't have page fault exception handler when a virtual page is accessed and
+      // the A bit is clear, or is written and the D bit is clear.
+      // So just set A for read and D for write permission.
+      //
+      if ((AttributeSetMask & RISCV_PG_R) != 0) {
+        EntryValue |= RISCV_PG_A;
+      }
+
+      if ((AttributeSetMask & RISCV_PG_W) != 0) {
+        EntryValue |= RISCV_PG_D;
+      }
+
+      EntryValue = SetPpnToPte (EntryValue, RegionStart);
+      EntryValue = SetValidPte (EntryValue);
+      ReplaceTableEntry (Entry, EntryValue, RegionStart, TableIsLive);
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Update region mapping at root table.
+
+  @param  RegionStart           The start address of the region.
+  @param  RegionLength          The length of the region.
+  @param  AttributeSetMask      The attribute mask to be set.
+  @param  AttributeClearMask    The attribute mask to be clear.
+  @param  RootTable             The pointer of root table.
+  @param  TableIsLive           TRUE if this is live update, FALSE otherwise.
+
+  @retval EFI_INVALID_PARAMETER The RegionStart or RegionLength was not valid.
+  @retval EFI_OUT_OF_RESOURCES  Not enough resource.
+  @retval EFI_SUCCESS           The operation succesfully.
+
+**/
+STATIC
+EFI_STATUS
+UpdateRegionMapping (
+  IN  UINTN    RegionStart,
+  IN  UINTN    RegionLength,
+  IN  UINTN    AttributeSetMask,
+  IN  UINTN    AttributeClearMask,
+  IN  UINTN    *RootTable,
+  IN  BOOLEAN  TableIsLive
+  )
+{
+  if (((RegionStart | RegionLength) & EFI_PAGE_MASK) != 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  return UpdateRegionMappingRecursive (
+           RegionStart,
+           RegionStart + RegionLength,
+           AttributeSetMask,
+           AttributeClearMask,
+           RootTable,
+           0,
+           TableIsLive
+           );
+}
+
+/**
+  Convert GCD attribute to RISC-V page attribute.
+
+  @param  GcdAttributes The GCD attribute.
+
+  @return               The RISC-V page attribute.
+
+**/
+STATIC
+UINTN
+GcdAttributeToPageAttribute (
+  IN UINTN  GcdAttributes
+  )
+{
+  UINTN  RiscVAttributes;
+
+  RiscVAttributes = RISCV_PG_R | RISCV_PG_W | RISCV_PG_X;
+
+  // Determine protection attributes
+  if ((GcdAttributes & EFI_MEMORY_RO) != 0) {
+    RiscVAttributes &= ~(RISCV_PG_W);
+  }
+
+  // Process eXecute Never attribute
+  if ((GcdAttributes & EFI_MEMORY_XP) != 0) {
+    RiscVAttributes &= ~RISCV_PG_X;
+  }
+
+  return RiscVAttributes;
+}
+
+/**
+  The API to set a GCD attribute on an memory region.
+
+  @param  BaseAddress             The base address of the region.
+  @param  Length                  The length of the region.
+  @param  Attributes              The GCD attributes.
+
+  @retval EFI_INVALID_PARAMETER   The BaseAddress or Length was not valid.
+  @retval EFI_OUT_OF_RESOURCES    Not enough resource.
+  @retval EFI_SUCCESS             The operation succesfully.
+
+**/
+EFI_STATUS
+EFIAPI
+RiscVSetMemoryAttributes (
+  IN EFI_PHYSICAL_ADDRESS  BaseAddress,
+  IN UINTN                 Length,
+  IN UINTN                 Attributes
+  )
+{
+  UINTN  PageAttributesSet;
+
+  PageAttributesSet = GcdAttributeToPageAttribute (Attributes);
+
+  if (!RiscVMmuEnabled ()) {
+    return EFI_SUCCESS;
+  }
+
+  DEBUG (
+    (
+     DEBUG_VERBOSE,
+     "%a: Set %llX page attribute 0x%X\n",
+     __func__,
+     BaseAddress,
+     PageAttributesSet
+    )
+    );
+
+  return UpdateRegionMapping (
+           BaseAddress,
+           Length,
+           PageAttributesSet,
+           PTE_ATTRIBUTES_MASK,
+           (UINTN *)RiscVGetRootTranslateTable (),
+           TRUE
+           );
+}
+
+/**
+  Set SATP mode.
+
+  @param  SatpMode  The SATP mode to be set.
+
+  @retval EFI_INVALID_PARAMETER   The SATP mode was not valid.
+  @retval EFI_OUT_OF_RESOURCES    Not enough resource.
+  @retval EFI_DEVICE_ERROR        The SATP mode not supported by HW.
+  @retval EFI_SUCCESS             The operation succesfully.
+
+**/
+STATIC
+EFI_STATUS
+RiscVMmuSetSatpMode  (
+  UINTN  SatpMode
+  )
+{
+  VOID                             *TranslationTable;
+  UINTN                            SatpReg;
+  UINTN                            Ppn;
+  EFI_GCD_MEMORY_SPACE_DESCRIPTOR  *MemoryMap;
+  UINTN                            NumberOfDescriptors;
+  UINTN                            Index;
+  EFI_STATUS                       Status;
+
+  switch (SatpMode) {
+    case SATP_MODE_OFF:
+      return EFI_SUCCESS;
+    case SATP_MODE_SV39:
+      mMaxRootTableLevel = 3;
+      mBitPerLevel       = 9;
+      mTableEntryCount   = 512;
+      break;
+    case SATP_MODE_SV48:
+      mMaxRootTableLevel = 4;
+      mBitPerLevel       = 9;
+      mTableEntryCount   = 512;
+      break;
+    case SATP_MODE_SV57:
+      mMaxRootTableLevel = 5;
+      mBitPerLevel       = 9;
+      mTableEntryCount   = 512;
+      break;
+    default:
+      return EFI_INVALID_PARAMETER;
+  }
+
+  // Allocate pages for translation table
+  TranslationTable = AllocatePages (1);
+  if (TranslationTable == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  ZeroMem (TranslationTable, mTableEntryCount * sizeof (UINTN));
+
+  NumberOfDescriptors = 0;
+  MemoryMap           = NULL;
+  Status              = gDS->GetMemorySpaceMap (
+                               &NumberOfDescriptors,
+                               &MemoryMap
+                               );
+  ASSERT_EFI_ERROR (Status);
+
+  for (Index = 0; Index < NumberOfDescriptors; Index++) {
+    if (MemoryMap[Index].GcdMemoryType == EfiGcdMemoryTypeMemoryMappedIo) {
+      // Default Read/Write attribute for memory mapped IO
+      UpdateRegionMapping (
+        MemoryMap[Index].BaseAddress,
+        MemoryMap[Index].Length,
+        RISCV_PG_R | RISCV_PG_W,
+        PTE_ATTRIBUTES_MASK,
+        TranslationTable,
+        FALSE
+        );
+    } else if (MemoryMap[Index].GcdMemoryType == EfiGcdMemoryTypeSystemMemory) {
+      // Default Read/Write/Execute attribute for system memory
+      UpdateRegionMapping (
+        MemoryMap[Index].BaseAddress,
+        MemoryMap[Index].Length,
+        RISCV_PG_R | RISCV_PG_W | RISCV_PG_X,
+        PTE_ATTRIBUTES_MASK,
+        TranslationTable,
+        FALSE
+        );
+    }
+  }
+
+  FreePool ((VOID *)MemoryMap);
+
+  if (GetInterruptState ()) {
+    DisableInterrupts ();
+  }
+
+  Ppn = (UINTN)TranslationTable >> RISCV_MMU_PAGE_SHIFT;
+  ASSERT (!(Ppn & ~(SATP64_PPN)));
+
+  SatpReg  = Ppn;
+  SatpReg |= (SatpMode <<
+              SATP64_MODE_SHIFT) & SATP64_MODE;
+  RiscVSetSupervisorAddressTranslationRegister (SatpReg);
+  /* Check if HW support the setup satp mode */
+  if (SatpReg != RiscVGetSupervisorAddressTranslationRegister ()) {
+    DEBUG (
+      (
+       DEBUG_VERBOSE,
+       "%a: HW does not support SATP mode:%d\n",
+       __func__,
+       SatpMode
+      )
+      );
+    FreePageTablesRecursive (TranslationTable, 0);
+    return EFI_DEVICE_ERROR;
+  }
+
+  RiscVLocalTlbFlushAll ();
+
+  if (GetInterruptState ()) {
+    EnableInterrupts ();
+  }
+
+  return Status;
+}
+
+/**
+  The API to configure and enable RISC-V MMU with the highest mode supported.
+
+  @retval EFI_OUT_OF_RESOURCES    Not enough resource.
+  @retval EFI_SUCCESS             The operation succesfully.
+
+**/
+EFI_STATUS
+EFIAPI
+RiscVConfigureMmu (
+  VOID
+  )
+{
+  EFI_STATUS  Status;
+  INTN        Idx;
+
+  Status = EFI_SUCCESS;
+
+  /* Try to setup MMU with highest mode as possible */
+  for (Idx = 0; Idx < ARRAY_SIZE (mModeSupport); Idx++) {
+    Status = RiscVMmuSetSatpMode (mModeSupport[Idx]);
+    if (Status == EFI_DEVICE_ERROR) {
+      continue;
+    } else if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    DEBUG (
+      (
+       DEBUG_INFO,
+       "%a: SATP mode %d successfully configured\n",
+       __func__,
+       mModeSupport[Idx]
+      )
+      );
+    break;
+  }
+
+  return Status;
+}
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/BaseRiscVMmuLib/BaseRiscVMmuLib.inf
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/BaseRiscVMmuLib/BaseRiscVMmuLib.inf	(revision 101291)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/BaseRiscVMmuLib/BaseRiscVMmuLib.inf	(revision 101291)
@@ -0,0 +1,27 @@
+## @file
+#  RISC-V MMU library.
+#
+#  Copyright (c) 2023, Ventana Micro Systems Inc. All Rights Reserved.<BR>
+#
+#  SPDX-License-Identifier: BSD-2-Clause-Patent
+#
+##
+
+[Defines]
+  INF_VERSION         = 0x0001001b
+  BASE_NAME           = BaseRiscVMmuLib
+  FILE_GUID           = d3bc42ee-c9eb-4339-ba11-06747083d3ae
+  MODULE_TYPE         = BASE
+  VERSION_STRING      = 1.0
+  LIBRARY_CLASS       = RiscVMmuLib
+
+[Sources]
+  BaseRiscVMmuLib.c
+  RiscVMmuCore.S
+
+[Packages]
+  MdePkg/MdePkg.dec
+  UefiCpuPkg/UefiCpuPkg.dec
+
+[LibraryClasses]
+  BaseLib
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/BaseRiscVMmuLib/RiscVMmuCore.S
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/BaseRiscVMmuLib/RiscVMmuCore.S	(revision 101291)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/BaseRiscVMmuLib/RiscVMmuCore.S	(revision 101291)
@@ -0,0 +1,31 @@
+/** @file
+*
+*  Copyright (c) 2023, Ventana Micro Systems Inc. All Rights Reserved.<BR>
+*
+*  SPDX-License-Identifier: BSD-2-Clause-Patent
+*
+**/
+
+#include <Base.h>
+#include <Register/RiscV64/RiscVImpl.h>
+
+.text
+  .align 3
+
+//
+// Local tlb flush all.
+//
+//
+ASM_FUNC (RiscVLocalTlbFlushAll)
+sfence.vma
+ret
+
+//
+// Local tlb flush at a virtual address
+// @retval a0 : virtual address.
+//
+ASM_FUNC (
+  RiscVLocalTlbFlush
+  )
+sfence.vma a0
+ret
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/BaseXApicLib/BaseXApicLib.c
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/BaseXApicLib/BaseXApicLib.c	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/BaseXApicLib/BaseXApicLib.c	(revision 101291)
@@ -4,5 +4,5 @@
   This local APIC library instance supports xAPIC mode only.
 
-  Copyright (c) 2010 - 2019, Intel Corporation. All rights reserved.<BR>
+  Copyright (c) 2010 - 2023, Intel Corporation. All rights reserved.<BR>
   Copyright (c) 2017 - 2020, AMD Inc. All rights reserved.<BR>
 
@@ -23,5 +23,4 @@
 #include <Library/PcdLib.h>
 #include <Library/CpuLib.h>
-#include <Library/UefiCpuLib.h>
 
 //
@@ -494,4 +493,31 @@
   IcrLow.Uint32                    = 0;
   IcrLow.Bits.DeliveryMode         = LOCAL_APIC_DELIVERY_MODE_INIT;
+  IcrLow.Bits.Level                = 1;
+  IcrLow.Bits.DestinationShorthand = LOCAL_APIC_DESTINATION_SHORTHAND_ALL_EXCLUDING_SELF;
+  SendIpi (IcrLow.Uint32, 0);
+}
+
+/**
+  Send a Start-up IPI to all processors excluding self.
+  This function returns after the IPI has been accepted by the target processors.
+  if StartupRoutine >= 1M, then ASSERT.
+  if StartupRoutine is not multiple of 4K, then ASSERT.
+  @param  StartupRoutine  Points to a start-up routine which is below 1M physical
+                          address and 4K aligned.
+**/
+VOID
+EFIAPI
+SendStartupIpiAllExcludingSelf (
+  IN UINT32  StartupRoutine
+  )
+{
+  LOCAL_APIC_ICR_LOW  IcrLow;
+
+  ASSERT (StartupRoutine < 0x100000);
+  ASSERT ((StartupRoutine & 0xfff) == 0);
+
+  IcrLow.Uint32                    = 0;
+  IcrLow.Bits.Vector               = (StartupRoutine >> 12);
+  IcrLow.Bits.DeliveryMode         = LOCAL_APIC_DELIVERY_MODE_STARTUP;
   IcrLow.Bits.Level                = 1;
   IcrLow.Bits.DestinationShorthand = LOCAL_APIC_DESTINATION_SHORTHAND_ALL_EXCLUDING_SELF;
@@ -553,20 +579,10 @@
   )
 {
-  LOCAL_APIC_ICR_LOW  IcrLow;
-
-  ASSERT (StartupRoutine < 0x100000);
-  ASSERT ((StartupRoutine & 0xfff) == 0);
-
   SendInitIpiAllExcludingSelf ();
   MicroSecondDelay (PcdGet32 (PcdCpuInitIpiDelayInMicroSeconds));
-  IcrLow.Uint32                    = 0;
-  IcrLow.Bits.Vector               = (StartupRoutine >> 12);
-  IcrLow.Bits.DeliveryMode         = LOCAL_APIC_DELIVERY_MODE_STARTUP;
-  IcrLow.Bits.Level                = 1;
-  IcrLow.Bits.DestinationShorthand = LOCAL_APIC_DESTINATION_SHORTHAND_ALL_EXCLUDING_SELF;
-  SendIpi (IcrLow.Uint32, 0);
+  SendStartupIpiAllExcludingSelf (StartupRoutine);
   if (!StandardSignatureIsAuthenticAMD ()) {
     MicroSecondDelay (200);
-    SendIpi (IcrLow.Uint32, 0);
+    SendStartupIpiAllExcludingSelf (StartupRoutine);
   }
 }
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/BaseXApicLib/BaseXApicLib.inf
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/BaseXApicLib/BaseXApicLib.inf	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/BaseXApicLib/BaseXApicLib.inf	(revision 101291)
@@ -40,5 +40,4 @@
   PcdLib
   CpuLib
-  UefiCpuLib
 
 [Pcd]
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/BaseXApicX2ApicLib/BaseXApicX2ApicLib.c
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/BaseXApicX2ApicLib/BaseXApicX2ApicLib.c	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/BaseXApicX2ApicLib/BaseXApicX2ApicLib.c	(revision 101291)
@@ -5,5 +5,5 @@
   which have xAPIC and x2APIC modes.
 
-  Copyright (c) 2010 - 2019, Intel Corporation. All rights reserved.<BR>
+  Copyright (c) 2010 - 2023, Intel Corporation. All rights reserved.<BR>
   Copyright (c) 2017 - 2020, AMD Inc. All rights reserved.<BR>
 
@@ -24,5 +24,4 @@
 #include <Library/PcdLib.h>
 #include <Library/CpuLib.h>
-#include <Library/UefiCpuLib.h>
 #include <IndustryStandard/Tdx.h>
 
@@ -739,4 +738,31 @@
 
 /**
+  Send a Start-up IPI to all processors excluding self.
+  This function returns after the IPI has been accepted by the target processors.
+  if StartupRoutine >= 1M, then ASSERT.
+  if StartupRoutine is not multiple of 4K, then ASSERT.
+  @param  StartupRoutine  Points to a start-up routine which is below 1M physical
+                          address and 4K aligned.
+**/
+VOID
+EFIAPI
+SendStartupIpiAllExcludingSelf (
+  IN UINT32  StartupRoutine
+  )
+{
+  LOCAL_APIC_ICR_LOW  IcrLow;
+
+  ASSERT (StartupRoutine < 0x100000);
+  ASSERT ((StartupRoutine & 0xfff) == 0);
+
+  IcrLow.Uint32                    = 0;
+  IcrLow.Bits.Vector               = (StartupRoutine >> 12);
+  IcrLow.Bits.DeliveryMode         = LOCAL_APIC_DELIVERY_MODE_STARTUP;
+  IcrLow.Bits.Level                = 1;
+  IcrLow.Bits.DestinationShorthand = LOCAL_APIC_DESTINATION_SHORTHAND_ALL_EXCLUDING_SELF;
+  SendIpi (IcrLow.Uint32, 0);
+}
+
+/**
   Send an INIT-Start-up-Start-up IPI sequence to a specified target processor.
 
@@ -792,20 +818,10 @@
   )
 {
-  LOCAL_APIC_ICR_LOW  IcrLow;
-
-  ASSERT (StartupRoutine < 0x100000);
-  ASSERT ((StartupRoutine & 0xfff) == 0);
-
   SendInitIpiAllExcludingSelf ();
   MicroSecondDelay (PcdGet32 (PcdCpuInitIpiDelayInMicroSeconds));
-  IcrLow.Uint32                    = 0;
-  IcrLow.Bits.Vector               = (StartupRoutine >> 12);
-  IcrLow.Bits.DeliveryMode         = LOCAL_APIC_DELIVERY_MODE_STARTUP;
-  IcrLow.Bits.Level                = 1;
-  IcrLow.Bits.DestinationShorthand = LOCAL_APIC_DESTINATION_SHORTHAND_ALL_EXCLUDING_SELF;
-  SendIpi (IcrLow.Uint32, 0);
+  SendStartupIpiAllExcludingSelf (StartupRoutine);
   if (!StandardSignatureIsAuthenticAMD ()) {
     MicroSecondDelay (200);
-    SendIpi (IcrLow.Uint32, 0);
+    SendStartupIpiAllExcludingSelf (StartupRoutine);
   }
 }
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/BaseXApicX2ApicLib/BaseXApicX2ApicLib.inf
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/BaseXApicX2ApicLib/BaseXApicX2ApicLib.inf	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/BaseXApicX2ApicLib/BaseXApicX2ApicLib.inf	(revision 101291)
@@ -40,5 +40,4 @@
   PcdLib
   CpuLib
-  UefiCpuLib
 
 [Pcd]
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/CpuCommonFeaturesLib/CpuCommonFeaturesLib.inf
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/CpuCommonFeaturesLib/CpuCommonFeaturesLib.inf	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/CpuCommonFeaturesLib/CpuCommonFeaturesLib.inf	(revision 101291)
@@ -5,5 +5,5 @@
 #  Architectures Software Developer's Manual.
 #
-# Copyright (c) 2017 - 2019, Intel Corporation. All rights reserved.<BR>
+# Copyright (c) 2017 - 2023, Intel Corporation. All rights reserved.<BR>
 #
 #  SPDX-License-Identifier: BSD-2-Clause-Patent
@@ -59,6 +59,8 @@
 
 [Pcd]
-  gUefiCpuPkgTokenSpaceGuid.PcdCpuClockModulationDutyCycle   ## SOMETIMES_CONSUMES
-  gUefiCpuPkgTokenSpaceGuid.PcdIsPowerOnReset                ## SOMETIMES_CONSUMES
-  gUefiCpuPkgTokenSpaceGuid.PcdCpuProcTraceOutputScheme      ## SOMETIMES_CONSUMES
-  gUefiCpuPkgTokenSpaceGuid.PcdCpuProcTraceMemSize           ## SOMETIMES_CONSUMES
+  gUefiCpuPkgTokenSpaceGuid.PcdCpuClockModulationDutyCycle       ## SOMETIMES_CONSUMES
+  gUefiCpuPkgTokenSpaceGuid.PcdIsPowerOnReset                    ## SOMETIMES_CONSUMES
+  gUefiCpuPkgTokenSpaceGuid.PcdCpuProcTraceOutputScheme          ## SOMETIMES_CONSUMES
+  gUefiCpuPkgTokenSpaceGuid.PcdCpuProcTraceMemSize               ## SOMETIMES_CONSUMES
+  gUefiCpuPkgTokenSpaceGuid.PcdCpuProcTraceBspOnly               ## SOMETIMES_CONSUMES
+  gUefiCpuPkgTokenSpaceGuid.PcdCpuProcTracePerformanceCollecting ## SOMETIMES_CONSUMES
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/CpuCommonFeaturesLib/ProcTrace.c
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/CpuCommonFeaturesLib/ProcTrace.c	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/CpuCommonFeaturesLib/ProcTrace.c	(revision 101291)
@@ -2,5 +2,5 @@
   Intel Processor Trace feature.
 
-  Copyright (c) 2017 - 2018, Intel Corporation. All rights reserved.<BR>
+  Copyright (c) 2017 - 2023, Intel Corporation. All rights reserved.<BR>
   SPDX-License-Identifier: BSD-2-Clause-Patent
 
@@ -34,4 +34,5 @@
   MSR_IA32_RTIT_OUTPUT_BASE_REGISTER         RtitOutputBase;
   MSR_IA32_RTIT_OUTPUT_MASK_PTRS_REGISTER    RtitOutputMaskPtrs;
+  BOOLEAN                                    CycPacketSupported;
 } PROC_TRACE_PROCESSOR_DATA;
 
@@ -46,4 +47,7 @@
 
   UINTN                        *TopaMemArray;
+
+  BOOLEAN                      EnableOnBspOnly;
+  BOOLEAN                      EnablePerformanceCollecting;
 
   PROC_TRACE_PROCESSOR_DATA    *ProcessorData;
@@ -75,7 +79,9 @@
   ConfigData->ProcessorData = (PROC_TRACE_PROCESSOR_DATA *)((UINT8 *)ConfigData + sizeof (PROC_TRACE_DATA));
 
-  ConfigData->NumberOfProcessors    = (UINT32)NumberOfProcessors;
-  ConfigData->ProcTraceMemSize      = PcdGet32 (PcdCpuProcTraceMemSize);
-  ConfigData->ProcTraceOutputScheme = PcdGet8 (PcdCpuProcTraceOutputScheme);
+  ConfigData->NumberOfProcessors          = (UINT32)NumberOfProcessors;
+  ConfigData->ProcTraceMemSize            = PcdGet32 (PcdCpuProcTraceMemSize);
+  ConfigData->ProcTraceOutputScheme       = PcdGet8 (PcdCpuProcTraceOutputScheme);
+  ConfigData->EnableOnBspOnly             = PcdGetBool (PcdCpuProcTraceBspOnly);
+  ConfigData->EnablePerformanceCollecting = PcdGetBool (PcdCpuProcTracePerformanceCollecting);
 
   return ConfigData;
@@ -109,5 +115,6 @@
   PROC_TRACE_DATA                              *ProcTraceData;
   CPUID_STRUCTURED_EXTENDED_FEATURE_FLAGS_EBX  Ebx;
-  CPUID_INTEL_PROCESSOR_TRACE_MAIN_LEAF_ECX    Ecx;
+  CPUID_INTEL_PROCESSOR_TRACE_MAIN_LEAF_ECX    ProcTraceEcx;
+  CPUID_INTEL_PROCESSOR_TRACE_MAIN_LEAF_EBX    ProcTraceEbx;
 
   //
@@ -130,7 +137,7 @@
   }
 
-  AsmCpuidEx (CPUID_INTEL_PROCESSOR_TRACE, CPUID_INTEL_PROCESSOR_TRACE_MAIN_LEAF, NULL, NULL, &Ecx.Uint32, NULL);
-  ProcTraceData->ProcessorData[ProcessorNumber].TopaSupported        = (BOOLEAN)(Ecx.Bits.RTIT == 1);
-  ProcTraceData->ProcessorData[ProcessorNumber].SingleRangeSupported = (BOOLEAN)(Ecx.Bits.SingleRangeOutput == 1);
+  AsmCpuidEx (CPUID_INTEL_PROCESSOR_TRACE, CPUID_INTEL_PROCESSOR_TRACE_MAIN_LEAF, NULL, &ProcTraceEbx.Uint32, &ProcTraceEcx.Uint32, NULL);
+  ProcTraceData->ProcessorData[ProcessorNumber].TopaSupported        = (BOOLEAN)(ProcTraceEcx.Bits.RTIT == 1);
+  ProcTraceData->ProcessorData[ProcessorNumber].SingleRangeSupported = (BOOLEAN)(ProcTraceEcx.Bits.SingleRangeOutput == 1);
   if ((ProcTraceData->ProcessorData[ProcessorNumber].TopaSupported && (ProcTraceData->ProcTraceOutputScheme == RtitOutputSchemeToPA)) ||
       (ProcTraceData->ProcessorData[ProcessorNumber].SingleRangeSupported && (ProcTraceData->ProcTraceOutputScheme == RtitOutputSchemeSingleRange)))
@@ -139,4 +146,6 @@
     ProcTraceData->ProcessorData[ProcessorNumber].RtitOutputBase.Uint64     = AsmReadMsr64 (MSR_IA32_RTIT_OUTPUT_BASE);
     ProcTraceData->ProcessorData[ProcessorNumber].RtitOutputMaskPtrs.Uint64 = AsmReadMsr64 (MSR_IA32_RTIT_OUTPUT_MASK_PTRS);
+    ProcTraceData->ProcessorData[ProcessorNumber].CycPacketSupported        = (BOOLEAN)(ProcTraceEbx.Bits.ConfigurablePsb == 1);
+
     return TRUE;
   }
@@ -189,4 +198,5 @@
   MSR_IA32_RTIT_OUTPUT_MASK_PTRS_REGISTER  OutputMaskPtrsReg;
   RTIT_TOPA_TABLE_ENTRY                    *TopaEntryPtr;
+  BOOLEAN                                  IsBsp;
 
   //
@@ -237,4 +247,10 @@
   }
 
+  IsBsp = (CpuInfo->ProcessorInfo.StatusFlag & PROCESSOR_AS_BSP_BIT) ? TRUE : FALSE;
+
+  if (ProcTraceData->EnableOnBspOnly && !IsBsp) {
+    return RETURN_SUCCESS;
+  }
+
   MemRegionBaseAddr = 0;
   FirstIn           = FALSE;
@@ -261,41 +277,60 @@
     //   aligned based on their size, not just 4K. Thus a 2M region must have bits 20:12 cleared.
     //
-    ThreadMemRegionTable = (UINTN *)AllocatePool (ProcTraceData->NumberOfProcessors * sizeof (UINTN *));
-    if (ThreadMemRegionTable == NULL) {
-      DEBUG ((DEBUG_ERROR, "Allocate ProcTrace ThreadMemRegionTable Failed\n"));
-      return RETURN_OUT_OF_RESOURCES;
-    }
-
-    ProcTraceData->ThreadMemRegionTable = ThreadMemRegionTable;
-
-    for (Index = 0; Index < ProcTraceData->NumberOfProcessors; Index++, ProcTraceData->AllocatedThreads++) {
-      Pages          = EFI_SIZE_TO_PAGES (MemRegionSize);
-      Alignment      = MemRegionSize;
-      AlignedAddress = (UINTN)AllocateAlignedReservedPages (Pages, Alignment);
-      if (AlignedAddress == 0) {
-        DEBUG ((DEBUG_ERROR, "ProcTrace: Out of mem, allocated only for %d threads\n", ProcTraceData->AllocatedThreads));
-        if (Index == 0) {
-          //
-          // Could not allocate for BSP even
-          //
-          FreePool ((VOID *)ThreadMemRegionTable);
-          ThreadMemRegionTable = NULL;
-          return RETURN_OUT_OF_RESOURCES;
+
+    Pages     = EFI_SIZE_TO_PAGES (MemRegionSize);
+    Alignment = MemRegionSize;
+    if (ProcTraceData->EnableOnBspOnly) {
+      //
+      // When only enable ProcTrace on BSP, this is the first and only time ProcTraceInitialize() runs.
+      //
+      MemRegionBaseAddr = (UINTN)AllocateAlignedReservedPages (Pages, Alignment);
+      if (MemRegionBaseAddr == 0) {
+        //
+        // Could not allocate for BSP even
+        //
+        DEBUG ((DEBUG_ERROR, "ProcTrace: Out of mem, failed to allocate buffer for BSP\n"));
+        return RETURN_OUT_OF_RESOURCES;
+      }
+
+      DEBUG ((DEBUG_INFO, "ProcTrace: Allocated PT MemRegionBaseAddr(aligned) for BSP only: 0x%llX.\n", (UINT64)MemRegionBaseAddr));
+    } else {
+      ThreadMemRegionTable = (UINTN *)AllocatePool (ProcTraceData->NumberOfProcessors * sizeof (UINTN *));
+      if (ThreadMemRegionTable == NULL) {
+        DEBUG ((DEBUG_ERROR, "Allocate ProcTrace ThreadMemRegionTable Failed\n"));
+        return RETURN_OUT_OF_RESOURCES;
+      }
+
+      ProcTraceData->ThreadMemRegionTable = ThreadMemRegionTable;
+
+      for (Index = 0; Index < ProcTraceData->NumberOfProcessors; Index++, ProcTraceData->AllocatedThreads++) {
+        AlignedAddress = (UINTN)AllocateAlignedReservedPages (Pages, Alignment);
+        if (AlignedAddress == 0) {
+          DEBUG ((DEBUG_ERROR, "ProcTrace: Out of mem, allocated only for %d threads\n", ProcTraceData->AllocatedThreads));
+          if (Index == 0) {
+            //
+            // Could not allocate for BSP even
+            //
+            FreePool ((VOID *)ThreadMemRegionTable);
+            ThreadMemRegionTable = NULL;
+            return RETURN_OUT_OF_RESOURCES;
+          }
+
+          break;
         }
 
-        break;
+        ThreadMemRegionTable[Index] = AlignedAddress;
+        DEBUG ((DEBUG_INFO, "ProcTrace: PT MemRegionBaseAddr(aligned) for thread %d: 0x%llX \n", Index, (UINT64)ThreadMemRegionTable[Index]));
       }
 
-      ThreadMemRegionTable[Index] = AlignedAddress;
-      DEBUG ((DEBUG_INFO, "ProcTrace: PT MemRegionBaseAddr(aligned) for thread %d: 0x%llX \n", Index, (UINT64)ThreadMemRegionTable[Index]));
-    }
-
-    DEBUG ((DEBUG_INFO, "ProcTrace: Allocated PT mem for %d thread \n", ProcTraceData->AllocatedThreads));
-  }
-
-  if (ProcessorNumber < ProcTraceData->AllocatedThreads) {
-    MemRegionBaseAddr = ProcTraceData->ThreadMemRegionTable[ProcessorNumber];
-  } else {
-    return RETURN_SUCCESS;
+      DEBUG ((DEBUG_INFO, "ProcTrace: Allocated PT mem for %d thread \n", ProcTraceData->AllocatedThreads));
+    }
+  }
+
+  if (!ProcTraceData->EnableOnBspOnly) {
+    if (ProcessorNumber < ProcTraceData->AllocatedThreads) {
+      MemRegionBaseAddr = ProcTraceData->ThreadMemRegionTable[ProcessorNumber];
+    } else {
+      return RETURN_SUCCESS;
+    }
   }
 
@@ -368,48 +403,65 @@
     if (FirstIn) {
       DEBUG ((DEBUG_INFO, "ProcTrace: Enabling ToPA scheme \n"));
-      //
-      // Let BSP allocate ToPA table mem for all threads
-      //
-      TopaMemArray = (UINTN *)AllocatePool (ProcTraceData->AllocatedThreads * sizeof (UINTN *));
-      if (TopaMemArray == NULL) {
-        DEBUG ((DEBUG_ERROR, "ProcTrace: Allocate mem for ToPA Failed\n"));
-        return RETURN_OUT_OF_RESOURCES;
+
+      Pages     = EFI_SIZE_TO_PAGES (sizeof (PROC_TRACE_TOPA_TABLE));
+      Alignment = 0x1000;
+
+      if (ProcTraceData->EnableOnBspOnly) {
+        //
+        // When only enable ProcTrace on BSP, this is the first and only time ProcTraceInitialize() runs.
+        //
+        TopaTableBaseAddr = (UINTN)AllocateAlignedReservedPages (Pages, Alignment);
+        if (TopaTableBaseAddr == 0) {
+          DEBUG ((DEBUG_ERROR, "ProcTrace: Out of mem, failed to allocate ToPA mem for BSP"));
+          return RETURN_OUT_OF_RESOURCES;
+        }
+
+        DEBUG ((DEBUG_INFO, "ProcTrace: Topa table address(aligned) for BSP only: 0x%llX \n", (UINT64)TopaTableBaseAddr));
+      } else {
+        //
+        // Let BSP allocate ToPA table mem for all threads
+        //
+        TopaMemArray = (UINTN *)AllocatePool (ProcTraceData->AllocatedThreads * sizeof (UINTN *));
+        if (TopaMemArray == NULL) {
+          DEBUG ((DEBUG_ERROR, "ProcTrace: Allocate mem for ToPA Failed\n"));
+          return RETURN_OUT_OF_RESOURCES;
+        }
+
+        ProcTraceData->TopaMemArray = TopaMemArray;
+
+        for (Index = 0; Index < ProcTraceData->AllocatedThreads; Index++) {
+          AlignedAddress = (UINTN)AllocateAlignedReservedPages (Pages, Alignment);
+          if (AlignedAddress == 0) {
+            if (Index < ProcTraceData->AllocatedThreads) {
+              ProcTraceData->AllocatedThreads = Index;
+            }
+
+            DEBUG ((DEBUG_ERROR, "ProcTrace:  Out of mem, allocated ToPA mem only for %d threads\n", ProcTraceData->AllocatedThreads));
+            if (Index == 0) {
+              //
+              // Could not allocate for BSP even
+              //
+              FreePool ((VOID *)TopaMemArray);
+              TopaMemArray = NULL;
+              return RETURN_OUT_OF_RESOURCES;
+            }
+
+            break;
+          }
+
+          TopaMemArray[Index] = AlignedAddress;
+          DEBUG ((DEBUG_INFO, "ProcTrace: Topa table address(aligned) for thread %d is 0x%llX \n", Index, (UINT64)TopaMemArray[Index]));
+        }
+
+        DEBUG ((DEBUG_INFO, "ProcTrace: Allocated ToPA mem for %d thread \n", ProcTraceData->AllocatedThreads));
       }
-
-      ProcTraceData->TopaMemArray = TopaMemArray;
-
-      for (Index = 0; Index < ProcTraceData->AllocatedThreads; Index++) {
-        Pages          = EFI_SIZE_TO_PAGES (sizeof (PROC_TRACE_TOPA_TABLE));
-        Alignment      = 0x1000;
-        AlignedAddress = (UINTN)AllocateAlignedReservedPages (Pages, Alignment);
-        if (AlignedAddress == 0) {
-          if (Index < ProcTraceData->AllocatedThreads) {
-            ProcTraceData->AllocatedThreads = Index;
-          }
-
-          DEBUG ((DEBUG_ERROR, "ProcTrace:  Out of mem, allocated ToPA mem only for %d threads\n", ProcTraceData->AllocatedThreads));
-          if (Index == 0) {
-            //
-            // Could not allocate for BSP even
-            //
-            FreePool ((VOID *)TopaMemArray);
-            TopaMemArray = NULL;
-            return RETURN_OUT_OF_RESOURCES;
-          }
-
-          break;
-        }
-
-        TopaMemArray[Index] = AlignedAddress;
-        DEBUG ((DEBUG_INFO, "ProcTrace: Topa table address(aligned) for thread %d is 0x%llX \n", Index, (UINT64)TopaMemArray[Index]));
+    }
+
+    if (!ProcTraceData->EnableOnBspOnly) {
+      if (ProcessorNumber < ProcTraceData->AllocatedThreads) {
+        TopaTableBaseAddr = ProcTraceData->TopaMemArray[ProcessorNumber];
+      } else {
+        return RETURN_SUCCESS;
       }
-
-      DEBUG ((DEBUG_INFO, "ProcTrace: Allocated ToPA mem for %d thread \n", ProcTraceData->AllocatedThreads));
-    }
-
-    if (ProcessorNumber < ProcTraceData->AllocatedThreads) {
-      TopaTableBaseAddr = ProcTraceData->TopaMemArray[ProcessorNumber];
-    } else {
-      return RETURN_SUCCESS;
     }
 
@@ -472,4 +524,20 @@
   CtrlReg.Bits.BranchEn = 1;
   CtrlReg.Bits.TraceEn  = 1;
+
+  //
+  // Generate CYC/TSC timing packets to collect performance data.
+  //
+  if (ProcTraceData->EnablePerformanceCollecting) {
+    if (ProcTraceData->ProcessorData[ProcessorNumber].CycPacketSupported) {
+      CtrlReg.Bits.CYCEn     = 1;
+      CtrlReg.Bits.CYCThresh = 5;
+    }
+
+    //
+    // Write to TSCEn is always supported
+    //
+    CtrlReg.Bits.TSCEn = 1;
+  }
+
   CPU_REGISTER_TABLE_WRITE64 (
     ProcessorNumber,
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/CpuExceptionHandlerLib/DxeCpuExceptionHandlerLib.inf
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/CpuExceptionHandlerLib/DxeCpuExceptionHandlerLib.inf	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/CpuExceptionHandlerLib/DxeCpuExceptionHandlerLib.inf	(revision 101291)
@@ -29,5 +29,5 @@
 
 [Sources.X64]
-  X64/Xcode5ExceptionHandlerAsm.nasm
+  X64/ExceptionHandlerAsm.nasm
   X64/ArchExceptionHandler.c
   X64/ArchInterruptDefs.h
@@ -62,2 +62,5 @@
   DebugLib
   CcExitLib
+
+[BuildOptions]
+  XCODE:*_*_X64_NASM_FLAGS = -D NO_ABSOLUTE_RELOCS_IN_TEXT
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/CpuExceptionHandlerLib/PeiCpuExceptionHandlerLib.inf
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/CpuExceptionHandlerLib/PeiCpuExceptionHandlerLib.inf	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/CpuExceptionHandlerLib/PeiCpuExceptionHandlerLib.inf	(revision 101291)
@@ -29,5 +29,5 @@
 
 [Sources.X64]
-  X64/Xcode5ExceptionHandlerAsm.nasm
+  X64/SecPeiExceptionHandlerAsm.nasm
   X64/ArchExceptionHandler.c
   X64/ArchInterruptDefs.h
@@ -63,2 +63,4 @@
   gUefiCpuPkgTokenSpaceGuid.PcdCpuSmmStackGuard                    ## CONSUMES
 
+[BuildOptions]
+  XCODE:*_*_X64_NASM_FLAGS = -D NO_ABSOLUTE_RELOCS_IN_TEXT
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/CpuExceptionHandlerLib/SecPeiCpuExceptionHandlerLib.inf
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/CpuExceptionHandlerLib/SecPeiCpuExceptionHandlerLib.inf	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/CpuExceptionHandlerLib/SecPeiCpuExceptionHandlerLib.inf	(revision 101291)
@@ -29,5 +29,5 @@
 
 [Sources.X64]
-  X64/ExceptionHandlerAsm.nasm
+  X64/SecPeiExceptionHandlerAsm.nasm
   X64/ArchExceptionHandler.c
   X64/ArchInterruptDefs.h
@@ -59,2 +59,4 @@
   gUefiCpuPkgTokenSpaceGuid.PcdCpuSmmStackGuard                    ## CONSUMES
 
+[BuildOptions]
+  XCODE:*_*_X64_NASM_FLAGS = -D NO_ABSOLUTE_RELOCS_IN_TEXT
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/CpuExceptionHandlerLib/SmmCpuExceptionHandlerLib.inf
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/CpuExceptionHandlerLib/SmmCpuExceptionHandlerLib.inf	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/CpuExceptionHandlerLib/SmmCpuExceptionHandlerLib.inf	(revision 101291)
@@ -29,5 +29,5 @@
 
 [Sources.X64]
-  X64/Xcode5ExceptionHandlerAsm.nasm
+  X64/ExceptionHandlerAsm.nasm
   X64/ArchExceptionHandler.c
   X64/ArchInterruptDefs.h
@@ -62,2 +62,4 @@
   gUefiCpuPkgTokenSpaceGuid.PcdCpuSmmStackGuard                    ## CONSUMES
 
+[BuildOptions]
+  XCODE:*_*_X64_NASM_FLAGS = -D NO_ABSOLUTE_RELOCS_IN_TEXT
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/CpuExceptionHandlerLib/UnitTest/CpuExceptionHandlerTestCommon.c
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/CpuExceptionHandlerLib/UnitTest/CpuExceptionHandlerTestCommon.c	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/CpuExceptionHandlerLib/UnitTest/CpuExceptionHandlerTestCommon.c	(revision 101291)
@@ -293,5 +293,5 @@
         DEBUG_INFO,
         "%a: Bsp StackBase = 0x%016lx  StackSize = 0x%016lx\n",
-        __FUNCTION__,
+        __func__,
         MemoryHob->AllocDescriptor.MemoryBaseAddress,
         MemoryHob->AllocDescriptor.MemoryLength
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/CpuExceptionHandlerLib/X64/ExceptionHandlerAsm.nasm
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/CpuExceptionHandlerLib/X64/ExceptionHandlerAsm.nasm	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/CpuExceptionHandlerLib/X64/ExceptionHandlerAsm.nasm	(revision 101291)
@@ -14,4 +14,26 @@
 ;
 ;------------------------------------------------------------------------------
+%include "Nasm.inc"
+
+;
+; Equivalent NASM structure of IA32_DESCRIPTOR
+;
+struc IA32_DESCRIPTOR
+  .Limit                         CTYPE_UINT16 1
+  .Base                          CTYPE_UINTN  1
+endstruc
+
+;
+; Equivalent NASM structure of IA32_IDT_GATE_DESCRIPTOR
+;
+struc IA32_IDT_GATE_DESCRIPTOR
+  .OffsetLow                     CTYPE_UINT16 1
+  .Selector                      CTYPE_UINT16 1
+  .Reserved_0                    CTYPE_UINT8 1
+  .GateType                      CTYPE_UINT8 1
+  .OffsetHigh                    CTYPE_UINT16 1
+  .OffsetUpper                   CTYPE_UINT32 1
+  .Reserved_1                    CTYPE_UINT32 1
+endstruc
 
 ;
@@ -32,12 +54,15 @@
 ALIGN   8
 
-; Generate 32 IDT vectors.
-; 32 IDT vectors are enough because interrupts (32+) are not enabled in SEC and PEI phase.
+; Generate 256 IDT vectors.
 AsmIdtVectorBegin:
 %assign Vector 0
-%rep  32
-    push    byte %[Vector]
-    push    rax
+%rep  256
+    push    strict dword %[Vector] ; This instruction pushes sign-extended 8-byte value on stack
+    push    rax
+%ifdef NO_ABSOLUTE_RELOCS_IN_TEXT
+    mov     rax, strict qword 0    ; mov     rax, ASM_PFX(CommonInterruptEntry)
+%else
     mov     rax, ASM_PFX(CommonInterruptEntry)
+%endif
     jmp     rax
 %assign Vector Vector+1
@@ -46,9 +71,13 @@
 
 HookAfterStubHeaderBegin:
-    db      0x6a        ; push
-@VectorNum:
-    db      0          ; 0 will be fixed
-    push    rax
+    push    strict dword 0      ; 0 will be fixed
+VectorNum:
+    push    rax
+%ifdef NO_ABSOLUTE_RELOCS_IN_TEXT
+    mov     rax, strict qword 0 ;     mov     rax, HookAfterStubHeaderEnd
+JmpAbsoluteAddress:
+%else
     mov     rax, HookAfterStubHeaderEnd
+%endif
     jmp     rax
 HookAfterStubHeaderEnd:
@@ -277,7 +306,53 @@
     ;
     sub     rsp, 4 * 8 + 8
-    mov     rax, ASM_PFX(CommonExceptionHandler)
-    call    rax
+    call    ASM_PFX(CommonExceptionHandler)
     add     rsp, 4 * 8 + 8
+
+    ; The follow algorithm is used for clear shadow stack token busy bit.
+    ; The comment is based on the sample shadow stack.
+    ; Shadow stack is 32 bytes aligned.
+    ; The sample shadow stack layout :
+    ; Address | Context
+    ;         +-------------------------+
+    ;  0xFB8  |   FREE                  | It is 0xFC0|0x02|(LMA & CS.L), after SAVEPREVSSP.
+    ;         +-------------------------+
+    ;  0xFC0  |  Prev SSP               |
+    ;         +-------------------------+
+    ;  0xFC8  |   RIP                   |
+    ;         +-------------------------+
+    ;  0xFD0  |   CS                    |
+    ;         +-------------------------+
+    ;  0xFD8  |  0xFD8 | BUSY           | BUSY flag cleared after CLRSSBSY
+    ;         +-------------------------+
+    ;  0xFE0  | 0xFC0|0x02|(LMA & CS.L) |
+    ;         +-------------------------+
+    ; Instructions for Intel Control Flow Enforcement Technology (CET) are supported since NASM version 2.15.01.
+    cmp     qword [ASM_PFX(mDoFarReturnFlag)], 0
+    jz      CetDone
+    mov     rax, cr4
+    and     rax, 0x800000       ; Check if CET is enabled
+    jz      CetDone
+    sub     rsp, 0x10
+    sidt    [rsp]
+    mov     rcx, qword [rsp + IA32_DESCRIPTOR.Base]; Get IDT base address
+    add     rsp, 0x10
+    mov     rax, qword [rbp + 8]; Get exception number
+    sal     rax, 0x04           ; Get IDT offset
+    add     rax, rcx            ; Get IDT gate descriptor address
+    mov     al, byte [rax + IA32_IDT_GATE_DESCRIPTOR.Reserved_0]
+    and     rax, 0x01           ; Check IST field
+    jz      CetDone
+                                ; SSP should be 0xFC0 at this point
+    mov     rax, 0x04           ; advance past cs:lip:prevssp;supervisor shadow stack token
+    incsspq rax                 ; After this SSP should be 0xFE0
+    saveprevssp                 ; now the shadow stack restore token will be created at 0xFB8
+    rdsspq  rax                 ; Read new SSP, SSP should be 0xFE8
+    sub     rax, 0x10
+    clrssbsy [rax]              ; Clear token at 0xFD8, SSP should be 0 after this
+    sub     rax, 0x20
+    rstorssp [rax]              ; Restore to token at 0xFB8, new SSP will be 0xFB8
+    mov     rax, 0x01           ; Pop off the new save token created
+    incsspq rax                 ; SSP should be 0xFC0 now
+CetDone:
 
     cli
@@ -385,9 +460,24 @@
 global ASM_PFX(AsmGetTemplateAddressMap)
 ASM_PFX(AsmGetTemplateAddressMap):
-    mov     rax, AsmIdtVectorBegin
+    lea     rax, [AsmIdtVectorBegin]
     mov     qword [rcx], rax
-    mov     qword [rcx + 0x8],  (AsmIdtVectorEnd - AsmIdtVectorBegin) / 32
-    mov     rax, HookAfterStubHeaderBegin
+    mov     qword [rcx + 0x8],  (AsmIdtVectorEnd - AsmIdtVectorBegin) / 256
+    lea     rax, [HookAfterStubHeaderBegin]
     mov     qword [rcx + 0x10], rax
+
+%ifdef NO_ABSOLUTE_RELOCS_IN_TEXT
+; Fix up CommonInterruptEntry address
+    lea    rax, [ASM_PFX(CommonInterruptEntry)]
+    lea    rcx, [AsmIdtVectorBegin]
+%rep  256
+    mov    qword [rcx + (JmpAbsoluteAddress - 8 - HookAfterStubHeaderBegin)], rax
+    add    rcx, (AsmIdtVectorEnd - AsmIdtVectorBegin) / 256
+%endrep
+; Fix up HookAfterStubHeaderEnd
+    lea    rax, [HookAfterStubHeaderEnd]
+    lea    rcx, [JmpAbsoluteAddress]
+    mov    qword [rcx - 8], rax
+%endif
+
     ret
 
@@ -398,5 +488,5 @@
 ASM_PFX(AsmVectorNumFixup):
     mov     rax, rdx
-    mov     [rcx + (@VectorNum - HookAfterStubHeaderBegin)], al
+    mov     [rcx + (VectorNum - 4 - HookAfterStubHeaderBegin)], al
     ret
 
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/CpuExceptionHandlerLib/X64/SecPeiExceptionHandlerAsm.nasm
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/CpuExceptionHandlerLib/X64/SecPeiExceptionHandlerAsm.nasm	(revision 101291)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/CpuExceptionHandlerLib/X64/SecPeiExceptionHandlerAsm.nasm	(revision 101291)
@@ -0,0 +1,406 @@
+;------------------------------------------------------------------------------ ;
+; Copyright (c) 2012 - 2022, Intel Corporation. All rights reserved.<BR>
+; SPDX-License-Identifier: BSD-2-Clause-Patent
+;
+; Module Name:
+;
+;   ExceptionHandlerAsm.Asm
+;
+; Abstract:
+;
+;   x64 CPU Exception Handler
+;
+; Notes:
+;
+;------------------------------------------------------------------------------
+
+;
+; CommonExceptionHandler()
+;
+
+%define VC_EXCEPTION 29
+
+extern ASM_PFX(mErrorCodeFlag)    ; Error code flags for exceptions
+extern ASM_PFX(mDoFarReturnFlag)  ; Do far return flag
+extern ASM_PFX(CommonExceptionHandler)
+
+SECTION .data
+
+DEFAULT REL
+%ifndef NO_ABSOLUTE_RELOCS_IN_TEXT
+SECTION .text
+%endif
+
+ALIGN   8
+
+; Generate 32 IDT vectors.
+; 32 IDT vectors are enough because interrupts (32+) are not enabled in SEC and PEI phase.
+AsmIdtVectorBegin:
+%assign Vector 0
+%rep  32
+    push    byte %[Vector]
+    push    rax
+    mov     rax, ASM_PFX(CommonInterruptEntry)
+    jmp     rax
+%assign Vector Vector+1
+%endrep
+AsmIdtVectorEnd:
+
+HookAfterStubHeaderBegin:
+    db      0x6a        ; push
+@VectorNum:
+    db      0          ; 0 will be fixed
+    push    rax
+    mov     rax, HookAfterStubHeaderEnd
+    jmp     rax
+
+SECTION .text
+
+HookAfterStubHeaderEnd:
+    mov     rax, rsp
+    and     sp,  0xfff0        ; make sure 16-byte aligned for exception context
+    sub     rsp, 0x18           ; reserve room for filling exception data later
+    push    rcx
+    mov     rcx, [rax + 8]
+    bt      [ASM_PFX(mErrorCodeFlag)], ecx
+    jnc     .0
+    push    qword [rsp]             ; push additional rcx to make stack alignment
+.0:
+    xchg    rcx, [rsp]        ; restore rcx, save Exception Number in stack
+    push    qword [rax]             ; push rax into stack to keep code consistence
+
+;---------------------------------------;
+; CommonInterruptEntry                  ;
+;---------------------------------------;
+; The follow algorithm is used for the common interrupt routine.
+; Entry from each interrupt with a push eax and eax=interrupt number
+; Stack frame would be as follows as specified in IA32 manuals:
+;
+; +---------------------+ <-- 16-byte aligned ensured by processor
+; +    Old SS           +
+; +---------------------+
+; +    Old RSP          +
+; +---------------------+
+; +    RFlags           +
+; +---------------------+
+; +    CS               +
+; +---------------------+
+; +    RIP              +
+; +---------------------+
+; +    Error Code       +
+; +---------------------+
+; +   Vector Number     +
+; +---------------------+
+; +    RBP              +
+; +---------------------+ <-- RBP, 16-byte aligned
+; The follow algorithm is used for the common interrupt routine.
+global ASM_PFX(CommonInterruptEntry)
+ASM_PFX(CommonInterruptEntry):
+    cli
+    pop     rax
+    ;
+    ; All interrupt handlers are invoked through interrupt gates, so
+    ; IF flag automatically cleared at the entry point
+    ;
+    xchg    rcx, [rsp]      ; Save rcx into stack and save vector number into rcx
+    and     rcx, 0xFF
+    cmp     ecx, 32         ; Intel reserved vector for exceptions?
+    jae     NoErrorCode
+    bt      [ASM_PFX(mErrorCodeFlag)], ecx
+    jc      HasErrorCode
+
+NoErrorCode:
+
+    ;
+    ; Push a dummy error code on the stack
+    ; to maintain coherent stack map
+    ;
+    push    qword [rsp]
+    mov     qword [rsp + 8], 0
+HasErrorCode:
+    push    rbp
+    mov     rbp, rsp
+    push    0             ; clear EXCEPTION_HANDLER_CONTEXT.OldIdtHandler
+    push    0             ; clear EXCEPTION_HANDLER_CONTEXT.ExceptionDataFlag
+
+    ;
+    ; Stack:
+    ; +---------------------+ <-- 16-byte aligned ensured by processor
+    ; +    Old SS           +
+    ; +---------------------+
+    ; +    Old RSP          +
+    ; +---------------------+
+    ; +    RFlags           +
+    ; +---------------------+
+    ; +    CS               +
+    ; +---------------------+
+    ; +    RIP              +
+    ; +---------------------+
+    ; +    Error Code       +
+    ; +---------------------+
+    ; + RCX / Vector Number +
+    ; +---------------------+
+    ; +    RBP              +
+    ; +---------------------+ <-- RBP, 16-byte aligned
+    ;
+
+    ;
+    ; Since here the stack pointer is 16-byte aligned, so
+    ; EFI_FX_SAVE_STATE_X64 of EFI_SYSTEM_CONTEXT_x64
+    ; is 16-byte aligned
+    ;
+
+;; UINT64  Rdi, Rsi, Rbp, Rsp, Rbx, Rdx, Rcx, Rax;
+;; UINT64  R8, R9, R10, R11, R12, R13, R14, R15;
+    push r15
+    push r14
+    push r13
+    push r12
+    push r11
+    push r10
+    push r9
+    push r8
+    push rax
+    push qword [rbp + 8]   ; RCX
+    push rdx
+    push rbx
+    push qword [rbp + 48]  ; RSP
+    push qword [rbp]       ; RBP
+    push rsi
+    push rdi
+
+;; UINT64  Gs, Fs, Es, Ds, Cs, Ss;  insure high 16 bits of each is zero
+    movzx   rax, word [rbp + 56]
+    push    rax                      ; for ss
+    movzx   rax, word [rbp + 32]
+    push    rax                      ; for cs
+    mov     rax, ds
+    push    rax
+    mov     rax, es
+    push    rax
+    mov     rax, fs
+    push    rax
+    mov     rax, gs
+    push    rax
+
+    mov     [rbp + 8], rcx               ; save vector number
+
+;; UINT64  Rip;
+    push    qword [rbp + 24]
+
+;; UINT64  Gdtr[2], Idtr[2];
+    xor     rax, rax
+    push    rax
+    push    rax
+    sidt    [rsp]
+    mov     bx, word [rsp]
+    mov     rax, qword [rsp + 2]
+    mov     qword [rsp], rax
+    mov     word [rsp + 8], bx
+
+    xor     rax, rax
+    push    rax
+    push    rax
+    sgdt    [rsp]
+    mov     bx, word [rsp]
+    mov     rax, qword [rsp + 2]
+    mov     qword [rsp], rax
+    mov     word [rsp + 8], bx
+
+;; UINT64  Ldtr, Tr;
+    xor     rax, rax
+    str     ax
+    push    rax
+    sldt    ax
+    push    rax
+
+;; UINT64  RFlags;
+    push    qword [rbp + 40]
+
+;; UINT64  Cr0, Cr1, Cr2, Cr3, Cr4, Cr8;
+    mov     rax, cr8
+    push    rax
+    mov     rax, cr4
+    or      rax, 0x208
+    mov     cr4, rax
+    push    rax
+    mov     rax, cr3
+    push    rax
+    mov     rax, cr2
+    push    rax
+    xor     rax, rax
+    push    rax
+    mov     rax, cr0
+    push    rax
+
+;; UINT64  Dr0, Dr1, Dr2, Dr3, Dr6, Dr7;
+    cmp     qword [rbp + 8], VC_EXCEPTION
+    je      VcDebugRegs          ; For SEV-ES (#VC) Debug registers ignored
+
+    mov     rax, dr7
+    push    rax
+    mov     rax, dr6
+    push    rax
+    mov     rax, dr3
+    push    rax
+    mov     rax, dr2
+    push    rax
+    mov     rax, dr1
+    push    rax
+    mov     rax, dr0
+    push    rax
+    jmp     DrFinish
+
+VcDebugRegs:
+;; UINT64  Dr0, Dr1, Dr2, Dr3, Dr6, Dr7 are skipped for #VC to avoid exception recursion
+    xor     rax, rax
+    push    rax
+    push    rax
+    push    rax
+    push    rax
+    push    rax
+    push    rax
+
+DrFinish:
+;; FX_SAVE_STATE_X64 FxSaveState;
+    sub rsp, 512
+    mov rdi, rsp
+    fxsave [rdi]
+
+;; UEFI calling convention for x64 requires that Direction flag in EFLAGs is clear
+    cld
+
+;; UINT32  ExceptionData;
+    push    qword [rbp + 16]
+
+;; Prepare parameter and call
+    mov     rcx, [rbp + 8]
+    mov     rdx, rsp
+    ;
+    ; Per X64 calling convention, allocate maximum parameter stack space
+    ; and make sure RSP is 16-byte aligned
+    ;
+    sub     rsp, 4 * 8 + 8
+    call    ASM_PFX(CommonExceptionHandler)
+    add     rsp, 4 * 8 + 8
+
+    cli
+;; UINT64  ExceptionData;
+    add     rsp, 8
+
+;; FX_SAVE_STATE_X64 FxSaveState;
+
+    mov rsi, rsp
+    fxrstor [rsi]
+    add rsp, 512
+
+;; UINT64  Dr0, Dr1, Dr2, Dr3, Dr6, Dr7;
+;; Skip restoration of DRx registers to support in-circuit emualators
+;; or debuggers set breakpoint in interrupt/exception context
+    add     rsp, 8 * 6
+
+;; UINT64  Cr0, Cr1, Cr2, Cr3, Cr4, Cr8;
+    pop     rax
+    mov     cr0, rax
+    add     rsp, 8   ; not for Cr1
+    pop     rax
+    mov     cr2, rax
+    pop     rax
+    mov     cr3, rax
+    pop     rax
+    mov     cr4, rax
+    pop     rax
+    mov     cr8, rax
+
+;; UINT64  RFlags;
+    pop     qword [rbp + 40]
+
+;; UINT64  Ldtr, Tr;
+;; UINT64  Gdtr[2], Idtr[2];
+;; Best not let anyone mess with these particular registers...
+    add     rsp, 48
+
+;; UINT64  Rip;
+    pop     qword [rbp + 24]
+
+;; UINT64  Gs, Fs, Es, Ds, Cs, Ss;
+    pop     rax
+    ; mov     gs, rax ; not for gs
+    pop     rax
+    ; mov     fs, rax ; not for fs
+    ; (X64 will not use fs and gs, so we do not restore it)
+    pop     rax
+    mov     es, rax
+    pop     rax
+    mov     ds, rax
+    pop     qword [rbp + 32]  ; for cs
+    pop     qword [rbp + 56]  ; for ss
+
+;; UINT64  Rdi, Rsi, Rbp, Rsp, Rbx, Rdx, Rcx, Rax;
+;; UINT64  R8, R9, R10, R11, R12, R13, R14, R15;
+    pop     rdi
+    pop     rsi
+    add     rsp, 8               ; not for rbp
+    pop     qword [rbp + 48] ; for rsp
+    pop     rbx
+    pop     rdx
+    pop     rcx
+    pop     rax
+    pop     r8
+    pop     r9
+    pop     r10
+    pop     r11
+    pop     r12
+    pop     r13
+    pop     r14
+    pop     r15
+
+    mov     rsp, rbp
+    pop     rbp
+    add     rsp, 16
+    cmp     qword [rsp - 32], 0  ; check EXCEPTION_HANDLER_CONTEXT.OldIdtHandler
+    jz      DoReturn
+    cmp     qword [rsp - 40], 1  ; check EXCEPTION_HANDLER_CONTEXT.ExceptionDataFlag
+    jz      ErrorCode
+    jmp     qword [rsp - 32]
+ErrorCode:
+    sub     rsp, 8
+    jmp     qword [rsp - 24]
+
+DoReturn:
+    cmp     qword [ASM_PFX(mDoFarReturnFlag)], 0   ; Check if need to do far return instead of IRET
+    jz      DoIret
+    push    rax
+    mov     rax, rsp          ; save old RSP to rax
+    mov     rsp, [rsp + 0x20]
+    push    qword [rax + 0x10]       ; save CS in new location
+    push    qword [rax + 0x8]        ; save EIP in new location
+    push    qword [rax + 0x18]       ; save EFLAGS in new location
+    mov     rax, [rax]        ; restore rax
+    popfq                     ; restore EFLAGS
+    retfq
+DoIret:
+    iretq
+
+;-------------------------------------------------------------------------------------
+;  GetTemplateAddressMap (&AddressMap);
+;-------------------------------------------------------------------------------------
+; comments here for definition of address map
+global ASM_PFX(AsmGetTemplateAddressMap)
+ASM_PFX(AsmGetTemplateAddressMap):
+    lea     rax, [AsmIdtVectorBegin]
+    mov     qword [rcx], rax
+    mov     qword [rcx + 0x8],  (AsmIdtVectorEnd - AsmIdtVectorBegin) / 32
+    lea     rax, [HookAfterStubHeaderBegin]
+    mov     qword [rcx + 0x10], rax
+    ret
+
+;-------------------------------------------------------------------------------------
+;  AsmVectorNumFixup (*NewVectorAddr, VectorNum, *OldVectorAddr);
+;-------------------------------------------------------------------------------------
+global ASM_PFX(AsmVectorNumFixup)
+ASM_PFX(AsmVectorNumFixup):
+    mov     rax, rdx
+    mov     [rcx + (@VectorNum - HookAfterStubHeaderBegin)], al
+    ret
+
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/CpuExceptionHandlerLib/X64/Xcode5ExceptionHandlerAsm.nasm
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/CpuExceptionHandlerLib/X64/Xcode5ExceptionHandlerAsm.nasm	(revision 101283)
+++ 	(revision )
@@ -1,482 +1,0 @@
-;------------------------------------------------------------------------------ ;
-; Copyright (c) 2012 - 2022, Intel Corporation. All rights reserved.<BR>
-; SPDX-License-Identifier: BSD-2-Clause-Patent
-;
-; Module Name:
-;
-;   ExceptionHandlerAsm.Asm
-;
-; Abstract:
-;
-;   x64 CPU Exception Handler
-;
-; Notes:
-;
-;------------------------------------------------------------------------------
-%include "Nasm.inc"
-
-;
-; Equivalent NASM structure of IA32_DESCRIPTOR
-;
-struc IA32_DESCRIPTOR
-  .Limit                         CTYPE_UINT16 1
-  .Base                          CTYPE_UINTN  1
-endstruc
-
-;
-; Equivalent NASM structure of IA32_IDT_GATE_DESCRIPTOR
-;
-struc IA32_IDT_GATE_DESCRIPTOR
-  .OffsetLow                     CTYPE_UINT16 1
-  .Selector                      CTYPE_UINT16 1
-  .Reserved_0                    CTYPE_UINT8 1
-  .GateType                      CTYPE_UINT8 1
-  .OffsetHigh                    CTYPE_UINT16 1
-  .OffsetUpper                   CTYPE_UINT32 1
-  .Reserved_1                    CTYPE_UINT32 1
-endstruc
-
-;
-; CommonExceptionHandler()
-;
-
-%define VC_EXCEPTION 29
-
-extern ASM_PFX(mErrorCodeFlag)    ; Error code flags for exceptions
-extern ASM_PFX(mDoFarReturnFlag)  ; Do far return flag
-extern ASM_PFX(CommonExceptionHandler)
-
-SECTION .data
-
-DEFAULT REL
-SECTION .text
-
-ALIGN   8
-
-; Generate 256 IDT vectors.
-AsmIdtVectorBegin:
-%assign Vector 0
-%rep  256
-    push    strict dword %[Vector] ; This instruction pushes sign-extended 8-byte value on stack
-    push    rax
-    mov     rax, strict qword 0    ; mov     rax, ASM_PFX(CommonInterruptEntry)
-    jmp     rax
-%assign Vector Vector+1
-%endrep
-AsmIdtVectorEnd:
-
-HookAfterStubHeaderBegin:
-    push    strict dword 0      ; 0 will be fixed
-VectorNum:
-    push    rax
-    mov     rax, strict qword 0 ;     mov     rax, HookAfterStubHeaderEnd
-JmpAbsoluteAddress:
-    jmp     rax
-HookAfterStubHeaderEnd:
-    mov     rax, rsp
-    and     sp,  0xfff0        ; make sure 16-byte aligned for exception context
-    sub     rsp, 0x18           ; reserve room for filling exception data later
-    push    rcx
-    mov     rcx, [rax + 8]
-    bt      [ASM_PFX(mErrorCodeFlag)], ecx
-    jnc     .0
-    push    qword [rsp]             ; push additional rcx to make stack alignment
-.0:
-    xchg    rcx, [rsp]        ; restore rcx, save Exception Number in stack
-    push    qword [rax]             ; push rax into stack to keep code consistence
-
-;---------------------------------------;
-; CommonInterruptEntry                  ;
-;---------------------------------------;
-; The follow algorithm is used for the common interrupt routine.
-; Entry from each interrupt with a push eax and eax=interrupt number
-; Stack frame would be as follows as specified in IA32 manuals:
-;
-; +---------------------+ <-- 16-byte aligned ensured by processor
-; +    Old SS           +
-; +---------------------+
-; +    Old RSP          +
-; +---------------------+
-; +    RFlags           +
-; +---------------------+
-; +    CS               +
-; +---------------------+
-; +    RIP              +
-; +---------------------+
-; +    Error Code       +
-; +---------------------+
-; +   Vector Number     +
-; +---------------------+
-; +    RBP              +
-; +---------------------+ <-- RBP, 16-byte aligned
-; The follow algorithm is used for the common interrupt routine.
-global ASM_PFX(CommonInterruptEntry)
-ASM_PFX(CommonInterruptEntry):
-    cli
-    pop     rax
-    ;
-    ; All interrupt handlers are invoked through interrupt gates, so
-    ; IF flag automatically cleared at the entry point
-    ;
-    xchg    rcx, [rsp]      ; Save rcx into stack and save vector number into rcx
-    and     rcx, 0xFF
-    cmp     ecx, 32         ; Intel reserved vector for exceptions?
-    jae     NoErrorCode
-    bt      [ASM_PFX(mErrorCodeFlag)], ecx
-    jc      HasErrorCode
-
-NoErrorCode:
-
-    ;
-    ; Push a dummy error code on the stack
-    ; to maintain coherent stack map
-    ;
-    push    qword [rsp]
-    mov     qword [rsp + 8], 0
-HasErrorCode:
-    push    rbp
-    mov     rbp, rsp
-    push    0             ; clear EXCEPTION_HANDLER_CONTEXT.OldIdtHandler
-    push    0             ; clear EXCEPTION_HANDLER_CONTEXT.ExceptionDataFlag
-
-    ;
-    ; Stack:
-    ; +---------------------+ <-- 16-byte aligned ensured by processor
-    ; +    Old SS           +
-    ; +---------------------+
-    ; +    Old RSP          +
-    ; +---------------------+
-    ; +    RFlags           +
-    ; +---------------------+
-    ; +    CS               +
-    ; +---------------------+
-    ; +    RIP              +
-    ; +---------------------+
-    ; +    Error Code       +
-    ; +---------------------+
-    ; + RCX / Vector Number +
-    ; +---------------------+
-    ; +    RBP              +
-    ; +---------------------+ <-- RBP, 16-byte aligned
-    ;
-
-    ;
-    ; Since here the stack pointer is 16-byte aligned, so
-    ; EFI_FX_SAVE_STATE_X64 of EFI_SYSTEM_CONTEXT_x64
-    ; is 16-byte aligned
-    ;
-
-;; UINT64  Rdi, Rsi, Rbp, Rsp, Rbx, Rdx, Rcx, Rax;
-;; UINT64  R8, R9, R10, R11, R12, R13, R14, R15;
-    push r15
-    push r14
-    push r13
-    push r12
-    push r11
-    push r10
-    push r9
-    push r8
-    push rax
-    push qword [rbp + 8]   ; RCX
-    push rdx
-    push rbx
-    push qword [rbp + 48]  ; RSP
-    push qword [rbp]       ; RBP
-    push rsi
-    push rdi
-
-;; UINT64  Gs, Fs, Es, Ds, Cs, Ss;  insure high 16 bits of each is zero
-    movzx   rax, word [rbp + 56]
-    push    rax                      ; for ss
-    movzx   rax, word [rbp + 32]
-    push    rax                      ; for cs
-    mov     rax, ds
-    push    rax
-    mov     rax, es
-    push    rax
-    mov     rax, fs
-    push    rax
-    mov     rax, gs
-    push    rax
-
-    mov     [rbp + 8], rcx               ; save vector number
-
-;; UINT64  Rip;
-    push    qword [rbp + 24]
-
-;; UINT64  Gdtr[2], Idtr[2];
-    xor     rax, rax
-    push    rax
-    push    rax
-    sidt    [rsp]
-    mov     bx, word [rsp]
-    mov     rax, qword [rsp + 2]
-    mov     qword [rsp], rax
-    mov     word [rsp + 8], bx
-
-    xor     rax, rax
-    push    rax
-    push    rax
-    sgdt    [rsp]
-    mov     bx, word [rsp]
-    mov     rax, qword [rsp + 2]
-    mov     qword [rsp], rax
-    mov     word [rsp + 8], bx
-
-;; UINT64  Ldtr, Tr;
-    xor     rax, rax
-    str     ax
-    push    rax
-    sldt    ax
-    push    rax
-
-;; UINT64  RFlags;
-    push    qword [rbp + 40]
-
-;; UINT64  Cr0, Cr1, Cr2, Cr3, Cr4, Cr8;
-    mov     rax, cr8
-    push    rax
-    mov     rax, cr4
-    or      rax, 0x208
-    mov     cr4, rax
-    push    rax
-    mov     rax, cr3
-    push    rax
-    mov     rax, cr2
-    push    rax
-    xor     rax, rax
-    push    rax
-    mov     rax, cr0
-    push    rax
-
-;; UINT64  Dr0, Dr1, Dr2, Dr3, Dr6, Dr7;
-    cmp     qword [rbp + 8], VC_EXCEPTION
-    je      VcDebugRegs          ; For SEV-ES (#VC) Debug registers ignored
-
-    mov     rax, dr7
-    push    rax
-    mov     rax, dr6
-    push    rax
-    mov     rax, dr3
-    push    rax
-    mov     rax, dr2
-    push    rax
-    mov     rax, dr1
-    push    rax
-    mov     rax, dr0
-    push    rax
-    jmp     DrFinish
-
-VcDebugRegs:
-;; UINT64  Dr0, Dr1, Dr2, Dr3, Dr6, Dr7 are skipped for #VC to avoid exception recursion
-    xor     rax, rax
-    push    rax
-    push    rax
-    push    rax
-    push    rax
-    push    rax
-    push    rax
-
-DrFinish:
-;; FX_SAVE_STATE_X64 FxSaveState;
-    sub rsp, 512
-    mov rdi, rsp
-    fxsave [rdi]
-
-;; UEFI calling convention for x64 requires that Direction flag in EFLAGs is clear
-    cld
-
-;; UINT32  ExceptionData;
-    push    qword [rbp + 16]
-
-;; Prepare parameter and call
-    mov     rcx, [rbp + 8]
-    mov     rdx, rsp
-    ;
-    ; Per X64 calling convention, allocate maximum parameter stack space
-    ; and make sure RSP is 16-byte aligned
-    ;
-    sub     rsp, 4 * 8 + 8
-    call    ASM_PFX(CommonExceptionHandler)
-    add     rsp, 4 * 8 + 8
-
-    ; The follow algorithm is used for clear shadow stack token busy bit.
-    ; The comment is based on the sample shadow stack.
-    ; Shadow stack is 32 bytes aligned.
-    ; The sample shadow stack layout :
-    ; Address | Context
-    ;         +-------------------------+
-    ;  0xFB8  |   FREE                  | It is 0xFC0|0x02|(LMA & CS.L), after SAVEPREVSSP.
-    ;         +-------------------------+
-    ;  0xFC0  |  Prev SSP               |
-    ;         +-------------------------+
-    ;  0xFC8  |   RIP                   |
-    ;         +-------------------------+
-    ;  0xFD0  |   CS                    |
-    ;         +-------------------------+
-    ;  0xFD8  |  0xFD8 | BUSY           | BUSY flag cleared after CLRSSBSY
-    ;         +-------------------------+
-    ;  0xFE0  | 0xFC0|0x02|(LMA & CS.L) |
-    ;         +-------------------------+
-    ; Instructions for Intel Control Flow Enforcement Technology (CET) are supported since NASM version 2.15.01.
-    cmp     qword [ASM_PFX(mDoFarReturnFlag)], 0
-    jz      CetDone
-    mov     rax, cr4
-    and     rax, 0x800000       ; Check if CET is enabled
-    jz      CetDone
-    sub     rsp, 0x10
-    sidt    [rsp]
-    mov     rcx, qword [rsp + IA32_DESCRIPTOR.Base]; Get IDT base address
-    add     rsp, 0x10
-    mov     rax, qword [rbp + 8]; Get exception number
-    sal     rax, 0x04           ; Get IDT offset
-    add     rax, rcx            ; Get IDT gate descriptor address
-    mov     al, byte [rax + IA32_IDT_GATE_DESCRIPTOR.Reserved_0]
-    and     rax, 0x01           ; Check IST field
-    jz      CetDone
-                                ; SSP should be 0xFC0 at this point
-    mov     rax, 0x04           ; advance past cs:lip:prevssp;supervisor shadow stack token
-    incsspq rax                 ; After this SSP should be 0xFE0
-    saveprevssp                 ; now the shadow stack restore token will be created at 0xFB8
-    rdsspq  rax                 ; Read new SSP, SSP should be 0xFE8
-    sub     rax, 0x10
-    clrssbsy [rax]              ; Clear token at 0xFD8, SSP should be 0 after this
-    sub     rax, 0x20
-    rstorssp [rax]              ; Restore to token at 0xFB8, new SSP will be 0xFB8
-    mov     rax, 0x01           ; Pop off the new save token created
-    incsspq rax                 ; SSP should be 0xFC0 now
-CetDone:
-
-    cli
-;; UINT64  ExceptionData;
-    add     rsp, 8
-
-;; FX_SAVE_STATE_X64 FxSaveState;
-
-    mov rsi, rsp
-    fxrstor [rsi]
-    add rsp, 512
-
-;; UINT64  Dr0, Dr1, Dr2, Dr3, Dr6, Dr7;
-;; Skip restoration of DRx registers to support in-circuit emualators
-;; or debuggers set breakpoint in interrupt/exception context
-    add     rsp, 8 * 6
-
-;; UINT64  Cr0, Cr1, Cr2, Cr3, Cr4, Cr8;
-    pop     rax
-    mov     cr0, rax
-    add     rsp, 8   ; not for Cr1
-    pop     rax
-    mov     cr2, rax
-    pop     rax
-    mov     cr3, rax
-    pop     rax
-    mov     cr4, rax
-    pop     rax
-    mov     cr8, rax
-
-;; UINT64  RFlags;
-    pop     qword [rbp + 40]
-
-;; UINT64  Ldtr, Tr;
-;; UINT64  Gdtr[2], Idtr[2];
-;; Best not let anyone mess with these particular registers...
-    add     rsp, 48
-
-;; UINT64  Rip;
-    pop     qword [rbp + 24]
-
-;; UINT64  Gs, Fs, Es, Ds, Cs, Ss;
-    pop     rax
-    ; mov     gs, rax ; not for gs
-    pop     rax
-    ; mov     fs, rax ; not for fs
-    ; (X64 will not use fs and gs, so we do not restore it)
-    pop     rax
-    mov     es, rax
-    pop     rax
-    mov     ds, rax
-    pop     qword [rbp + 32]  ; for cs
-    pop     qword [rbp + 56]  ; for ss
-
-;; UINT64  Rdi, Rsi, Rbp, Rsp, Rbx, Rdx, Rcx, Rax;
-;; UINT64  R8, R9, R10, R11, R12, R13, R14, R15;
-    pop     rdi
-    pop     rsi
-    add     rsp, 8               ; not for rbp
-    pop     qword [rbp + 48] ; for rsp
-    pop     rbx
-    pop     rdx
-    pop     rcx
-    pop     rax
-    pop     r8
-    pop     r9
-    pop     r10
-    pop     r11
-    pop     r12
-    pop     r13
-    pop     r14
-    pop     r15
-
-    mov     rsp, rbp
-    pop     rbp
-    add     rsp, 16
-    cmp     qword [rsp - 32], 0  ; check EXCEPTION_HANDLER_CONTEXT.OldIdtHandler
-    jz      DoReturn
-    cmp     qword [rsp - 40], 1  ; check EXCEPTION_HANDLER_CONTEXT.ExceptionDataFlag
-    jz      ErrorCode
-    jmp     qword [rsp - 32]
-ErrorCode:
-    sub     rsp, 8
-    jmp     qword [rsp - 24]
-
-DoReturn:
-    cmp     qword [ASM_PFX(mDoFarReturnFlag)], 0   ; Check if need to do far return instead of IRET
-    jz      DoIret
-    push    rax
-    mov     rax, rsp          ; save old RSP to rax
-    mov     rsp, [rsp + 0x20]
-    push    qword [rax + 0x10]       ; save CS in new location
-    push    qword [rax + 0x8]        ; save EIP in new location
-    push    qword [rax + 0x18]       ; save EFLAGS in new location
-    mov     rax, [rax]        ; restore rax
-    popfq                     ; restore EFLAGS
-    retfq
-DoIret:
-    iretq
-
-;-------------------------------------------------------------------------------------
-;  GetTemplateAddressMap (&AddressMap);
-;-------------------------------------------------------------------------------------
-; comments here for definition of address map
-global ASM_PFX(AsmGetTemplateAddressMap)
-ASM_PFX(AsmGetTemplateAddressMap):
-    lea     rax, [AsmIdtVectorBegin]
-    mov     qword [rcx], rax
-    mov     qword [rcx + 0x8],  (AsmIdtVectorEnd - AsmIdtVectorBegin) / 256
-    lea     rax, [HookAfterStubHeaderBegin]
-    mov     qword [rcx + 0x10], rax
-
-; Fix up CommonInterruptEntry address
-    lea    rax, [ASM_PFX(CommonInterruptEntry)]
-    lea    rcx, [AsmIdtVectorBegin]
-%rep  256
-    mov    qword [rcx + (JmpAbsoluteAddress - 8 - HookAfterStubHeaderBegin)], rax
-    add    rcx, (AsmIdtVectorEnd - AsmIdtVectorBegin) / 256
-%endrep
-; Fix up HookAfterStubHeaderEnd
-    lea    rax, [HookAfterStubHeaderEnd]
-    lea    rcx, [JmpAbsoluteAddress]
-    mov    qword [rcx - 8], rax
-
-    ret
-
-;-------------------------------------------------------------------------------------
-;  AsmVectorNumFixup (*NewVectorAddr, VectorNum, *OldVectorAddr);
-;-------------------------------------------------------------------------------------
-global ASM_PFX(AsmVectorNumFixup)
-ASM_PFX(AsmVectorNumFixup):
-    mov     rax, rdx
-    mov     [rcx + (VectorNum - 4 - HookAfterStubHeaderBegin)], al
-    ret
-
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/CpuExceptionHandlerLib/Xcode5SecPeiCpuExceptionHandlerLib.inf
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/CpuExceptionHandlerLib/Xcode5SecPeiCpuExceptionHandlerLib.inf	(revision 101283)
+++ 	(revision )
@@ -1,65 +1,0 @@
-## @file
-#  CPU Exception Handler library instance for SEC/PEI modules.
-#
-#  Copyright (C) 2020, Advanced Micro Devices, Inc. All rights reserved.<BR>
-#  Copyright (c) 2012 - 2022, Intel Corporation. All rights reserved.<BR>
-#  SPDX-License-Identifier: BSD-2-Clause-Patent
-#
-#  This is the XCODE5 variant of the SEC/PEI CpuExceptionHandlerLib. This
-#  variant performs binary patching to fix up addresses that allow the
-#  XCODE5 toolchain to be used.
-#
-##
-
-[Defines]
-  INF_VERSION                    = 0x00010005
-  BASE_NAME                      = Xcode5SecPeiCpuExceptionHandlerLib
-  MODULE_UNI_FILE                = Xcode5SecPeiCpuExceptionHandlerLib.uni
-  FILE_GUID                      = 49C481AF-1621-42F3-8FA1-27C64143E304
-  MODULE_TYPE                    = PEIM
-  VERSION_STRING                 = 1.1
-  LIBRARY_CLASS                  = CpuExceptionHandlerLib|SEC PEI_CORE PEIM
-
-#
-# The following information is for reference only and not required by the build tools.
-#
-#  VALID_ARCHITECTURES           = IA32 X64
-#
-
-[Sources.Ia32]
-  Ia32/ExceptionHandlerAsm.nasm
-  Ia32/ExceptionTssEntryAsm.nasm
-  Ia32/ArchExceptionHandler.c
-  Ia32/ArchInterruptDefs.h
-
-[Sources.X64]
-  X64/Xcode5ExceptionHandlerAsm.nasm
-  X64/ArchExceptionHandler.c
-  X64/ArchInterruptDefs.h
-
-[Sources.common]
-  CpuExceptionCommon.h
-  CpuExceptionCommon.c
-  SecPeiCpuException.c
-
-[Packages]
-  MdePkg/MdePkg.dec
-  MdeModulePkg/MdeModulePkg.dec
-  UefiCpuPkg/UefiCpuPkg.dec
-
-[LibraryClasses]
-  BaseLib
-  SerialPortLib
-  PrintLib
-  LocalApicLib
-  PeCoffGetEntryPointLib
-  CcExitLib
-
-[Pcd]
-  gEfiMdeModulePkgTokenSpaceGuid.PcdCpuStackGuard
-  gUefiCpuPkgTokenSpaceGuid.PcdCpuStackSwitchExceptionList
-  gUefiCpuPkgTokenSpaceGuid.PcdCpuKnownGoodStackSize
-
-[FeaturePcd]
-  gUefiCpuPkgTokenSpaceGuid.PcdCpuSmmStackGuard                    ## CONSUMES
-
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/CpuExceptionHandlerLib/Xcode5SecPeiCpuExceptionHandlerLib.uni
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/CpuExceptionHandlerLib/Xcode5SecPeiCpuExceptionHandlerLib.uni	(revision 101283)
+++ 	(revision )
@@ -1,18 +1,0 @@
-// /** @file
-// XCODE5 CPU Exception Handler library instance for SEC/PEI modules.
-//
-// CPU Exception Handler library instance for SEC/PEI modules when built
-// using the XCODE5 toolchain.
-//
-// Copyright (C) 2020, Advanced Micro Devices, Inc. All rights reserved.<BR>
-// Copyright (c) 2012 - 2014, Intel Corporation. All rights reserved.<BR>
-//
-// SPDX-License-Identifier: BSD-2-Clause-Patent
-//
-// **/
-
-
-#string STR_MODULE_ABSTRACT             #language en-US "CPU Exception Handler library instance for SEC/PEI modules with the XCODE5 toolchain."
-
-#string STR_MODULE_DESCRIPTION          #language en-US "CPU Exception Handler library instance for SEC/PEI modules with the XCODE5 toolchain."
-
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/CpuPageTableLib/CpuPageTable.h
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/CpuPageTableLib/CpuPageTable.h	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/CpuPageTableLib/CpuPageTable.h	(revision 101291)
@@ -2,5 +2,5 @@
   Internal header for CpuPageTableLib.
 
-  Copyright (c) 2022, Intel Corporation. All rights reserved.<BR>
+  Copyright (c) 2022 - 2023, Intel Corporation. All rights reserved.<BR>
   SPDX-License-Identifier: BSD-2-Clause-Patent
 
@@ -20,4 +20,6 @@
 
 #define REGION_LENGTH(l)  LShiftU64 (1, (l) * 9 + 3)
+
+#define MAX_PAE_PDPTE_NUM  4
 
 typedef enum {
@@ -30,9 +32,10 @@
 
 typedef struct {
-  UINT64    Present        : 1;       // 0 = Not present in memory, 1 = Present in memory
-  UINT64    ReadWrite      : 1;       // 0 = Read-Only, 1= Read/Write
-  UINT64    UserSupervisor : 1;       // 0 = Supervisor, 1=User
-  UINT64    Reserved       : 58;
-  UINT64    Nx             : 1;        // No Execute bit
+  UINT32    Present        : 1;       // 0 = Not present in memory, 1 = Present in memory
+  UINT32    ReadWrite      : 1;       // 0 = Read-Only, 1= Read/Write
+  UINT32    UserSupervisor : 1;       // 0 = Supervisor, 1=User
+  UINT32    Reserved0      : 29;
+  UINT32    Reserved1      : 31;
+  UINT32    Nx             : 1;        // No Execute bit
 } IA32_PAGE_COMMON_ENTRY;
 
@@ -42,18 +45,18 @@
 typedef union {
   struct {
-    UINT64    Present              : 1; // 0 = Not present in memory, 1 = Present in memory
-    UINT64    ReadWrite            : 1; // 0 = Read-Only, 1= Read/Write
-    UINT64    UserSupervisor       : 1; // 0 = Supervisor, 1=User
-    UINT64    WriteThrough         : 1; // 0 = Write-Back caching, 1=Write-Through caching
-    UINT64    CacheDisabled        : 1; // 0 = Cached, 1=Non-Cached
-    UINT64    Accessed             : 1; // 0 = Not accessed, 1 = Accessed (set by CPU)
-    UINT64    Available0           : 1; // Ignored
-    UINT64    MustBeZero           : 1; // Must Be Zero
-
-    UINT64    Available2           : 4; // Ignored
-
-    UINT64    PageTableBaseAddress : 40; // Page Table Base Address
-    UINT64    Available3           : 11; // Ignored
-    UINT64    Nx                   : 1;  // No Execute bit
+    UINT32    Present                  : 1;  // 0 = Not present in memory, 1 = Present in memory
+    UINT32    ReadWrite                : 1;  // 0 = Read-Only, 1= Read/Write
+    UINT32    UserSupervisor           : 1;  // 0 = Supervisor, 1=User
+    UINT32    WriteThrough             : 1;  // 0 = Write-Back caching, 1=Write-Through caching
+    UINT32    CacheDisabled            : 1;  // 0 = Cached, 1=Non-Cached
+    UINT32    Accessed                 : 1;  // 0 = Not accessed, 1 = Accessed (set by CPU)
+    UINT32    Available0               : 1;  // Ignored
+    UINT32    MustBeZero               : 1;  // Must Be Zero
+    UINT32    Available2               : 4;  // Ignored
+    UINT32    PageTableBaseAddressLow  : 20; // Page Table Base Address Low
+
+    UINT32    PageTableBaseAddressHigh : 20; // Page Table Base Address High
+    UINT32    Available3               : 11; // Ignored
+    UINT32    Nx                       : 1;  // No Execute bit
   } Bits;
   UINT64    Uint64;
@@ -87,21 +90,21 @@
 typedef union {
   struct {
-    UINT64    Present              : 1; // 0 = Not present in memory, 1 = Present in memory
-    UINT64    ReadWrite            : 1; // 0 = Read-Only, 1= Read/Write
-    UINT64    UserSupervisor       : 1; // 0 = Supervisor, 1=User
-    UINT64    WriteThrough         : 1; // 0 = Write-Back caching, 1=Write-Through caching
-    UINT64    CacheDisabled        : 1; // 0 = Cached, 1=Non-Cached
-    UINT64    Accessed             : 1; // 0 = Not accessed, 1 = Accessed (set by CPU)
-    UINT64    Dirty                : 1; // 0 = Not dirty, 1 = Dirty (set by CPU)
-    UINT64    MustBeOne            : 1; // Page Size. Must Be One
-
-    UINT64    Global               : 1; // 0 = Not global, 1 = Global (if CR4.PGE = 1)
-    UINT64    Available1           : 3; // Ignored
-    UINT64    Pat                  : 1; // PAT
-
-    UINT64    PageTableBaseAddress : 39; // Page Table Base Address
-    UINT64    Available3           : 7;  // Ignored
-    UINT64    ProtectionKey        : 4;  // Protection key
-    UINT64    Nx                   : 1;  // No Execute bit
+    UINT32    Present                  : 1;  // 0 = Not present in memory, 1 = Present in memory
+    UINT32    ReadWrite                : 1;  // 0 = Read-Only, 1= Read/Write
+    UINT32    UserSupervisor           : 1;  // 0 = Supervisor, 1=User
+    UINT32    WriteThrough             : 1;  // 0 = Write-Back caching, 1=Write-Through caching
+    UINT32    CacheDisabled            : 1;  // 0 = Cached, 1=Non-Cached
+    UINT32    Accessed                 : 1;  // 0 = Not accessed, 1 = Accessed (set by CPU)
+    UINT32    Dirty                    : 1;  // 0 = Not dirty, 1 = Dirty (set by CPU)
+    UINT32    MustBeOne                : 1;  // Page Size. Must Be One
+    UINT32    Global                   : 1;  // 0 = Not global, 1 = Global (if CR4.PGE = 1)
+    UINT32    Available1               : 3;  // Ignored
+    UINT32    Pat                      : 1;  // PAT
+    UINT32    PageTableBaseAddressLow  : 19; // Page Table Base Address Low
+
+    UINT32    PageTableBaseAddressHigh : 20; // Page Table Base Address High
+    UINT32    Available3               : 7;  // Ignored
+    UINT32    ProtectionKey            : 4;  // Protection key
+    UINT32    Nx                       : 1;  // No Execute bit
   } Bits;
   UINT64    Uint64;
@@ -124,20 +127,20 @@
 typedef union {
   struct {
-    UINT64    Present              : 1; // 0 = Not present in memory, 1 = Present in memory
-    UINT64    ReadWrite            : 1; // 0 = Read-Only, 1= Read/Write
-    UINT64    UserSupervisor       : 1; // 0 = Supervisor, 1=User
-    UINT64    WriteThrough         : 1; // 0 = Write-Back caching, 1=Write-Through caching
-    UINT64    CacheDisabled        : 1; // 0 = Cached, 1=Non-Cached
-    UINT64    Accessed             : 1; // 0 = Not accessed, 1 = Accessed (set by CPU)
-    UINT64    Dirty                : 1; // 0 = Not dirty, 1 = Dirty (set by CPU)
-    UINT64    Pat                  : 1; // PAT
-
-    UINT64    Global               : 1; // 0 = Not global, 1 = Global (if CR4.PGE = 1)
-    UINT64    Available1           : 3; // Ignored
-
-    UINT64    PageTableBaseAddress : 40; // Page Table Base Address
-    UINT64    Available3           : 7;  // Ignored
-    UINT64    ProtectionKey        : 4;  // Protection key
-    UINT64    Nx                   : 1;  // No Execute bit
+    UINT32    Present                  : 1;  // 0 = Not present in memory, 1 = Present in memory
+    UINT32    ReadWrite                : 1;  // 0 = Read-Only, 1= Read/Write
+    UINT32    UserSupervisor           : 1;  // 0 = Supervisor, 1=User
+    UINT32    WriteThrough             : 1;  // 0 = Write-Back caching, 1=Write-Through caching
+    UINT32    CacheDisabled            : 1;  // 0 = Cached, 1=Non-Cached
+    UINT32    Accessed                 : 1;  // 0 = Not accessed, 1 = Accessed (set by CPU)
+    UINT32    Dirty                    : 1;  // 0 = Not dirty, 1 = Dirty (set by CPU)
+    UINT32    Pat                      : 1;  // PAT
+    UINT32    Global                   : 1;  // 0 = Not global, 1 = Global (if CR4.PGE = 1)
+    UINT32    Available1               : 3;  // Ignored
+    UINT32    PageTableBaseAddressLow  : 20; // Page Table Base Address Low
+
+    UINT32    PageTableBaseAddressHigh : 20; // Page Table Base Address High
+    UINT32    Available3               : 7;  // Ignored
+    UINT32    ProtectionKey            : 4;  // Protection key
+    UINT32    Nx                       : 1;  // No Execute bit
   } Bits;
   UINT64    Uint64;
@@ -150,14 +153,14 @@
 typedef union {
   struct {
-    UINT64    Present              : 1; // 0 = Not present in memory, 1 = Present in memory
-    UINT64    MustBeZero           : 2; // Must Be Zero
-    UINT64    WriteThrough         : 1; // 0 = Write-Back caching, 1=Write-Through caching
-    UINT64    CacheDisabled        : 1; // 0 = Cached, 1=Non-Cached
-    UINT64    MustBeZero2          : 4; // Must Be Zero
-
-    UINT64    Available            : 3; // Ignored
-
-    UINT64    PageTableBaseAddress : 40; // Page Table Base Address
-    UINT64    MustBeZero3          : 12; // Must Be Zero
+    UINT32    Present                  : 1;  // 0 = Not present in memory, 1 = Present in memory
+    UINT32    MustBeZero               : 2;  // Must Be Zero
+    UINT32    WriteThrough             : 1;  // 0 = Write-Back caching, 1=Write-Through caching
+    UINT32    CacheDisabled            : 1;  // 0 = Cached, 1=Non-Cached
+    UINT32    MustBeZero2              : 4;  // Must Be Zero
+    UINT32    Available                : 3;  // Ignored
+    UINT32    PageTableBaseAddressLow  : 20; // Page Table Base Address Low
+
+    UINT32    PageTableBaseAddressHigh : 20; // Page Table Base Address High
+    UINT32    MustBeZero3              : 12; // Must Be Zero
   } Bits;
   UINT64    Uint64;
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/CpuPageTableLib/CpuPageTableMap.c
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/CpuPageTableLib/CpuPageTableMap.c	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/CpuPageTableLib/CpuPageTableMap.c	(revision 101291)
@@ -2,5 +2,5 @@
   This library implements CpuPageTableLib that are generic for IA32 family CPU.
 
-  Copyright (c) 2022, Intel Corporation. All rights reserved.<BR>
+  Copyright (c) 2022 - 2023, Intel Corporation. All rights reserved.<BR>
   SPDX-License-Identifier: BSD-2-Clause-Patent
 
@@ -27,5 +27,5 @@
   )
 {
-  if (Mask->Bits.PageTableBaseAddress) {
+  if (Mask->Bits.PageTableBaseAddressLow || Mask->Bits.PageTableBaseAddressHigh) {
     Pte4K->Uint64 = (IA32_MAP_ATTRIBUTE_PAGE_TABLE_BASE_ADDRESS (Attribute) + Offset) | (Pte4K->Uint64 & ~IA32_PE_BASE_ADDRESS_MASK_40);
   }
@@ -94,5 +94,5 @@
   )
 {
-  if (Mask->Bits.PageTableBaseAddress) {
+  if (Mask->Bits.PageTableBaseAddressLow || Mask->Bits.PageTableBaseAddressHigh) {
     PleB->Uint64 = (IA32_MAP_ATTRIBUTE_PAGE_TABLE_BASE_ADDRESS (Attribute) + Offset) | (PleB->Uint64 & ~IA32_PE_BASE_ADDRESS_MASK_39);
   }
@@ -203,5 +203,6 @@
   }
 
-  Pnle->Bits.Accessed = 0;
+  Pnle->Bits.Accessed   = 0;
+  Pnle->Bits.MustBeZero = 0;
 
   //
@@ -213,4 +214,42 @@
   Pnle->Bits.WriteThrough  = 0;
   Pnle->Bits.CacheDisabled = 0;
+}
+
+/**
+  Check if the combination for Attribute and Mask is valid for non-present entry.
+  1.Mask.Present is 0 but some other attributes is provided. This case should be invalid.
+  2.Map non-present range to present. In this case, all attributes should be provided.
+
+  @param[in] Attribute    The attribute of the linear address range.
+  @param[in] Mask         The mask used for attribute to check.
+
+  @retval RETURN_INVALID_PARAMETER  For non-present range, Mask->Bits.Present is 0 but some other attributes are provided.
+  @retval RETURN_INVALID_PARAMETER  For non-present range, Mask->Bits.Present is 1, Attribute->Bits.Present is 1 but some other attributes are not provided.
+  @retval RETURN_SUCCESS            The combination for Attribute and Mask is valid.
+**/
+RETURN_STATUS
+IsAttributesAndMaskValidForNonPresentEntry (
+  IN     IA32_MAP_ATTRIBUTE  *Attribute,
+  IN     IA32_MAP_ATTRIBUTE  *Mask
+  )
+{
+  if ((Mask->Bits.Present == 1) && (Attribute->Bits.Present == 1)) {
+    //
+    // Creating new page table or remapping non-present range to present.
+    //
+    if ((Mask->Bits.ReadWrite == 0) || (Mask->Bits.UserSupervisor == 0) || (Mask->Bits.WriteThrough == 0) || (Mask->Bits.CacheDisabled == 0) ||
+        (Mask->Bits.Accessed == 0) || (Mask->Bits.Dirty == 0) || (Mask->Bits.Pat == 0) || (Mask->Bits.Global == 0) ||
+        ((Mask->Bits.PageTableBaseAddressLow == 0) && (Mask->Bits.PageTableBaseAddressHigh == 0)) || (Mask->Bits.ProtectionKey == 0) || (Mask->Bits.Nx == 0))
+    {
+      return RETURN_INVALID_PARAMETER;
+    }
+  } else if ((Mask->Bits.Present == 0) && (Mask->Uint64 > 1)) {
+    //
+    // Only change other attributes for non-present range is not permitted.
+    //
+    return RETURN_INVALID_PARAMETER;
+  }
+
+  return RETURN_SUCCESS;
 }
 
@@ -236,5 +275,8 @@
                                     when a new physical base address is set.
   @param[in]      Mask              The mask used for attribute. The corresponding field in Attribute is ignored if that in Mask is 0.
-
+  @param[out]     IsModified        TRUE means page table is modified. FALSE means page table is not modified.
+
+  @retval RETURN_INVALID_PARAMETER  For non-present range, Mask->Bits.Present is 0 but some other attributes are provided.
+  @retval RETURN_INVALID_PARAMETER  For non-present range, Mask->Bits.Present is 1, Attribute->Bits.Present is 1 but some other attributes are not provided.
   @retval RETURN_SUCCESS            PageTable is created/updated successfully.
 **/
@@ -252,5 +294,6 @@
   IN     UINT64              Offset,
   IN     IA32_MAP_ATTRIBUTE  *Attribute,
-  IN     IA32_MAP_ATTRIBUTE  *Mask
+  IN     IA32_MAP_ATTRIBUTE  *Mask,
+  OUT    BOOLEAN             *IsModified
   )
 {
@@ -259,4 +302,6 @@
   UINTN               Index;
   IA32_PAGING_ENTRY   *PagingEntry;
+  UINTN               PagingEntryIndex;
+  UINTN               PagingEntryIndexEnd;
   IA32_PAGING_ENTRY   *CurrentPagingEntry;
   UINT64              RegionLength;
@@ -274,4 +319,8 @@
   IA32_MAP_ATTRIBUTE  CurrentMask;
   IA32_MAP_ATTRIBUTE  LocalParentAttribute;
+  UINT64              PhysicalAddrInEntry;
+  UINT64              PhysicalAddrInAttr;
+  IA32_PAGING_ENTRY   OriginalParentPagingEntry;
+  IA32_PAGING_ENTRY   OriginalCurrentPagingEntry;
 
   ASSERT (Level != 0);
@@ -289,14 +338,64 @@
   ParentAttribute             = &LocalParentAttribute;
 
+  OriginalParentPagingEntry.Uint64 = ParentPagingEntry->Uint64;
+
+  //
+  // RegionLength: 256T (1 << 48) 512G (1 << 39), 1G (1 << 30), 2M (1 << 21) or 4K (1 << 12).
+  //
+  BitStart         = 12 + (Level - 1) * 9;
+  PagingEntryIndex = (UINTN)BitFieldRead64 (LinearAddress + Offset, BitStart, BitStart + 9 - 1);
+  RegionLength     = REGION_LENGTH (Level);
+  RegionMask       = RegionLength - 1;
+
   //
   // ParentPagingEntry ONLY is deferenced for checking Present and MustBeOne bits
   // when Modify is FALSE.
   //
-
-  if (ParentPagingEntry->Pce.Present == 0) {
-    //
-    // The parent entry is CR3 or PML5E/PML4E/PDPTE/PDE.
+  if ((ParentPagingEntry->Pce.Present == 0) || IsPle (ParentPagingEntry, Level + 1)) {
+    //
+    // When ParentPagingEntry is non-present, parent entry is CR3 or PML5E/PML4E/PDPTE/PDE.
     // It does NOT point to an existing page directory.
-    //
+    // When ParentPagingEntry is present, parent entry is leaf PDPTE_1G or PDE_2M. Split to 2M or 4K pages.
+    // Note: it's impossible the parent entry is a PTE_4K.
+    //
+    PleBAttribute.Uint64 = PageTableLibGetPleBMapAttribute (&ParentPagingEntry->PleB, ParentAttribute);
+    if (ParentPagingEntry->Pce.Present == 0) {
+      //
+      // [LinearAddress, LinearAddress + Length] contains non-present range.
+      //
+      Status = IsAttributesAndMaskValidForNonPresentEntry (Attribute, Mask);
+      if (RETURN_ERROR (Status)) {
+        return Status;
+      }
+
+      OneOfPagingEntry.Pnle.Uint64 = 0;
+    } else {
+      PageTableLibSetPle (Level, &OneOfPagingEntry, 0, &PleBAttribute, &AllOneMask);
+    }
+
+    //
+    // Check if the attribute, the physical address calculated by ParentPagingEntry is equal to
+    // the attribute, the physical address calculated by input Attribue and Mask.
+    //
+    if ((IA32_MAP_ATTRIBUTE_ATTRIBUTES (&PleBAttribute) & IA32_MAP_ATTRIBUTE_ATTRIBUTES (Mask))
+        == (IA32_MAP_ATTRIBUTE_ATTRIBUTES (Attribute) & IA32_MAP_ATTRIBUTE_ATTRIBUTES (Mask)))
+    {
+      if ((Mask->Bits.PageTableBaseAddressLow == 0) && (Mask->Bits.PageTableBaseAddressHigh == 0)) {
+        return RETURN_SUCCESS;
+      }
+
+      //
+      // Non-present entry won't reach there since:
+      // 1.When map non-present entry to present, the attribute must be different.
+      // 2.When still map non-present entry to non-present, PageTableBaseAddressLow and High in Mask must be 0.
+      //
+      ASSERT (ParentPagingEntry->Pce.Present == 1);
+      PhysicalAddrInEntry = IA32_MAP_ATTRIBUTE_PAGE_TABLE_BASE_ADDRESS (&PleBAttribute) + MultU64x32 (RegionLength, (UINT32)PagingEntryIndex);
+      PhysicalAddrInAttr  = (IA32_MAP_ATTRIBUTE_PAGE_TABLE_BASE_ADDRESS (Attribute) + Offset) & (~RegionMask);
+      if (PhysicalAddrInEntry == PhysicalAddrInAttr) {
+        return RETURN_SUCCESS;
+      }
+    }
+
     ASSERT (Buffer == NULL || *BufferSize >= SIZE_4KB);
     CreateNew    = TRUE;
@@ -304,46 +403,16 @@
 
     if (Modify) {
-      ParentPagingEntry->Uintn = (UINTN)Buffer + *BufferSize;
-      ZeroMem ((VOID *)ParentPagingEntry->Uintn, SIZE_4KB);
-      //
-      // Set default attribute bits for PML5E/PML4E/PDPTE/PDE.
-      //
-      PageTableLibSetPnle (&ParentPagingEntry->Pnle, &NopAttribute, &AllOneMask);
-    } else {
-      //
-      // Just make sure Present and MustBeZero (PageSize) bits are accurate.
-      //
-      OneOfPagingEntry.Pnle.Uint64 = 0;
-    }
-  } else if (IsPle (ParentPagingEntry, Level + 1)) {
-    //
-    // The parent entry is a PDPTE_1G or PDE_2M. Split to 2M or 4K pages.
-    // Note: it's impossible the parent entry is a PTE_4K.
-    //
-    //
-    // Use NOP attributes as the attribute of grand-parents because CPU will consider
-    // the actual attributes of grand-parents when determing the memory type.
-    //
-    PleBAttribute.Uint64 = PageTableLibGetPleBMapAttribute (&ParentPagingEntry->PleB, ParentAttribute);
-    if ((IA32_MAP_ATTRIBUTE_ATTRIBUTES (&PleBAttribute) & IA32_MAP_ATTRIBUTE_ATTRIBUTES (Mask))
-        == (IA32_MAP_ATTRIBUTE_ATTRIBUTES (Attribute) & IA32_MAP_ATTRIBUTE_ATTRIBUTES (Mask)))
-    {
-      //
-      // This function is called when the memory length is less than the region length of the parent level.
-      // No need to split the page when the attributes equal.
-      //
-      return RETURN_SUCCESS;
-    }
-
-    ASSERT (Buffer == NULL || *BufferSize >= SIZE_4KB);
-    CreateNew    = TRUE;
-    *BufferSize -= SIZE_4KB;
-    PageTableLibSetPle (Level, &OneOfPagingEntry, 0, &PleBAttribute, &AllOneMask);
-    if (Modify) {
-      //
-      // Create 512 child-level entries that map to 2M/4K.
-      //
-      ParentPagingEntry->Uintn = (UINTN)Buffer + *BufferSize;
-      ZeroMem ((VOID *)ParentPagingEntry->Uintn, SIZE_4KB);
+      PagingEntry = (IA32_PAGING_ENTRY *)((UINTN)Buffer + *BufferSize);
+      ZeroMem (PagingEntry, SIZE_4KB);
+
+      if (ParentPagingEntry->Pce.Present) {
+        //
+        // Create 512 child-level entries that map to 2M/4K.
+        //
+        for (SubOffset = 0, Index = 0; Index < 512; Index++) {
+          PagingEntry[Index].Uint64 = OneOfPagingEntry.Uint64 + SubOffset;
+          SubOffset                += RegionLength;
+        }
+      }
 
       //
@@ -352,14 +421,32 @@
       //       will make the entire region read-only even the child entries set the RW bit.
       //
+      // Non-leaf entry doesn't have PAT bit. So use ~IA32_PE_BASE_ADDRESS_MASK_40 is to make sure PAT bit
+      // (bit12) in original big-leaf entry is not assigned to PageTableBaseAddress field of non-leaf entry.
+      //
       PageTableLibSetPnle (&ParentPagingEntry->Pnle, &NopAttribute, &AllOneMask);
-
-      RegionLength = REGION_LENGTH (Level);
-      PagingEntry  = (IA32_PAGING_ENTRY *)(UINTN)IA32_PNLE_PAGE_TABLE_BASE_ADDRESS (&ParentPagingEntry->Pnle);
-      for (SubOffset = 0, Index = 0; Index < 512; Index++) {
-        PagingEntry[Index].Uint64 = OneOfPagingEntry.Uint64 + SubOffset;
-        SubOffset                += RegionLength;
-      }
+      ParentPagingEntry->Uint64 = ((UINTN)(VOID *)PagingEntry) | (ParentPagingEntry->Uint64 & (~IA32_PE_BASE_ADDRESS_MASK_40));
     }
   } else {
+    //
+    // If (LinearAddress + Length - 1) is not in the same ParentPagingEntry with (LinearAddress + Offset), then the remaining child PagingEntry
+    // starting from PagingEntryIndex of ParentPagingEntry is all covered by [LinearAddress + Offset, LinearAddress + Length - 1].
+    //
+    PagingEntryIndexEnd = (BitFieldRead64 (LinearAddress + Length - 1, BitStart + 9, 63) != BitFieldRead64 (LinearAddress + Offset, BitStart + 9, 63)) ? 511 :
+                          (UINTN)BitFieldRead64 (LinearAddress + Length - 1, BitStart, BitStart + 9 - 1);
+    PagingEntry = (IA32_PAGING_ENTRY *)(UINTN)IA32_PNLE_PAGE_TABLE_BASE_ADDRESS (&ParentPagingEntry->Pnle);
+    for (Index = PagingEntryIndex; Index <= PagingEntryIndexEnd; Index++) {
+      if (PagingEntry[Index].Pce.Present == 0) {
+        //
+        // [LinearAddress, LinearAddress + Length] contains non-present range.
+        //
+        Status = IsAttributesAndMaskValidForNonPresentEntry (Attribute, Mask);
+        if (RETURN_ERROR (Status)) {
+          return Status;
+        }
+
+        break;
+      }
+    }
+
     //
     // It's a non-leaf entry
@@ -376,13 +463,4 @@
     //       when PDPTE[0].Nx = 1 but caller wants to map [0-2MB] as Nx = 0 (PDT[0].Nx = 0)
     //            we need to change PDPTE[0].Nx = 0 and let all PDE[0-255].Nx = 1 in this step.
-    if ((ParentPagingEntry->Pnle.Bits.Present == 0) && (Mask->Bits.Present == 1) && (Attribute->Bits.Present == 1)) {
-      if (Modify) {
-        ParentPagingEntry->Pnle.Bits.Present = 1;
-      }
-
-      ChildAttribute.Bits.Present = 0;
-      ChildMask.Bits.Present      = 1;
-    }
-
     if ((ParentPagingEntry->Pnle.Bits.ReadWrite == 0) && (Mask->Bits.ReadWrite == 1) && (Attribute->Bits.ReadWrite == 1)) {
       if (Modify) {
@@ -418,5 +496,4 @@
         // e.g.: Set PDE[0-255].ReadWrite = 0
         //
-        PagingEntry = (IA32_PAGING_ENTRY *)(UINTN)IA32_PNLE_PAGE_TABLE_BASE_ADDRESS (&ParentPagingEntry->Pnle);
         for (Index = 0; Index < 512; Index++) {
           if (PagingEntry[Index].Pce.Present == 0) {
@@ -435,13 +512,8 @@
 
   //
-  // RegionLength: 256T (1 << 48) 512G (1 << 39), 1G (1 << 30), 2M (1 << 21) or 4K (1 << 12).
   // RegionStart:  points to the linear address that's aligned on RegionLength and lower than (LinearAddress + Offset).
   //
-  BitStart     = 12 + (Level - 1) * 9;
-  Index        = (UINTN)BitFieldRead64 (LinearAddress + Offset, BitStart, BitStart + 9 - 1);
-  RegionLength = LShiftU64 (1, BitStart);
-  RegionMask   = RegionLength - 1;
-  RegionStart  = (LinearAddress + Offset) & ~RegionMask;
-
+  Index                   = PagingEntryIndex;
+  RegionStart             = (LinearAddress + Offset) & ~RegionMask;
   ParentAttribute->Uint64 = PageTableLibGetPnleMapAttribute (&ParentPagingEntry->Pnle, ParentAttribute);
 
@@ -492,5 +564,13 @@
         }
 
+        //
+        // Check if any leaf PagingEntry is modified.
+        //
+        OriginalCurrentPagingEntry.Uint64 = CurrentPagingEntry->Uint64;
         PageTableLibSetPle (Level, CurrentPagingEntry, Offset, Attribute, &CurrentMask);
+
+        if (OriginalCurrentPagingEntry.Uint64 != CurrentPagingEntry->Uint64) {
+          *IsModified = TRUE;
+        }
       }
     } else {
@@ -515,5 +595,6 @@
                  Offset,
                  Attribute,
-                 Mask
+                 Mask,
+                 IsModified
                  );
       if (RETURN_ERROR (Status)) {
@@ -525,4 +606,12 @@
     RegionStart += RegionLength;
     Index++;
+  }
+
+  //
+  // Check if ParentPagingEntry entry is modified here is enough. Except the changes happen in leaf PagingEntry during
+  // the while loop, if there is any other change happens in page table, the ParentPagingEntry must has been modified.
+  //
+  if (OriginalParentPagingEntry.Uint64 != ParentPagingEntry->Uint64) {
+    *IsModified = TRUE;
   }
 
@@ -547,12 +636,17 @@
                                  when a new physical base address is set.
   @param[in]      Mask           The mask used for attribute. The corresponding field in Attribute is ignored if that in Mask is 0.
+  @param[out]     IsModified     TRUE means page table is modified. FALSE means page table is not modified.
 
   @retval RETURN_UNSUPPORTED        PagingMode is not supported.
   @retval RETURN_INVALID_PARAMETER  PageTable, BufferSize, Attribute or Mask is NULL.
+  @retval RETURN_INVALID_PARAMETER  For non-present range, Mask->Bits.Present is 0 but some other attributes are provided.
+  @retval RETURN_INVALID_PARAMETER  For non-present range, Mask->Bits.Present is 1, Attribute->Bits.Present is 1 but some other attributes are not provided.
+  @retval RETURN_INVALID_PARAMETER  For non-present range, Mask->Bits.Present is 1, Attribute->Bits.Present is 0 but some other attributes are provided.
+  @retval RETURN_INVALID_PARAMETER  For present range, Mask->Bits.Present is 1, Attribute->Bits.Present is 0 but some other attributes are provided.
   @retval RETURN_INVALID_PARAMETER  *BufferSize is not multiple of 4KB.
   @retval RETURN_BUFFER_TOO_SMALL   The buffer is too small for page table creation/updating.
                                     BufferSize is updated to indicate the expected buffer size.
                                     Caller may still get RETURN_BUFFER_TOO_SMALL with the new BufferSize.
-  @retval RETURN_SUCCESS            PageTable is created/updated successfully.
+  @retval RETURN_SUCCESS            PageTable is created/updated successfully or the input Length is 0.
 **/
 RETURN_STATUS
@@ -566,5 +660,6 @@
   IN     UINT64              Length,
   IN     IA32_MAP_ATTRIBUTE  *Attribute,
-  IN     IA32_MAP_ATTRIBUTE  *Mask
+  IN     IA32_MAP_ATTRIBUTE  *Mask,
+  OUT    BOOLEAN             *IsModified   OPTIONAL
   )
 {
@@ -576,9 +671,16 @@
   IA32_PAGE_LEVEL     MaxLeafLevel;
   IA32_MAP_ATTRIBUTE  ParentAttribute;
-
-  if ((PagingMode == Paging32bit) || (PagingMode == PagingPae) || (PagingMode >= PagingModeMax)) {
+  BOOLEAN             LocalIsModified;
+  UINTN               Index;
+  IA32_PAGING_ENTRY   *PagingEntry;
+  UINT8               BufferInStack[SIZE_4KB - 1 + MAX_PAE_PDPTE_NUM * sizeof (IA32_PAGING_ENTRY)];
+
+  if (Length == 0) {
+    return RETURN_SUCCESS;
+  }
+
+  if ((PagingMode == Paging32bit) || (PagingMode >= PagingModeMax)) {
     //
     // 32bit paging is never supported.
-    // PAE paging will be supported later.
     //
     return RETURN_UNSUPPORTED;
@@ -596,5 +698,5 @@
   }
 
-  if ((LinearAddress % SIZE_4KB != 0) || (Length % SIZE_4KB != 0)) {
+  if (((UINTN)LinearAddress % SIZE_4KB != 0) || ((UINTN)Length % SIZE_4KB != 0)) {
     //
     // LinearAddress and Length should be multiple of 4K.
@@ -607,11 +709,19 @@
   }
 
+  //
+  // If to map [LinearAddress, LinearAddress + Length] as non-present,
+  // all attributes except Present should not be provided.
+  //
+  if ((Attribute->Bits.Present == 0) && (Mask->Bits.Present == 1) && (Mask->Uint64 > 1)) {
+    return RETURN_INVALID_PARAMETER;
+  }
+
   MaxLeafLevel     = (IA32_PAGE_LEVEL)(UINT8)PagingMode;
   MaxLevel         = (IA32_PAGE_LEVEL)(UINT8)(PagingMode >> 8);
-  MaxLinearAddress = LShiftU64 (1, 12 + MaxLevel * 9);
+  MaxLinearAddress = (PagingMode == PagingPae) ? LShiftU64 (1, 32) : LShiftU64 (1, 12 + MaxLevel * 9);
 
   if ((LinearAddress > MaxLinearAddress) || (Length > MaxLinearAddress - LinearAddress)) {
     //
-    // Maximum linear address is (1 << 48) or (1 << 57)
+    // Maximum linear address is (1 << 32), (1 << 48) or (1 << 57)
     //
     return RETURN_INVALID_PARAMETER;
@@ -620,4 +730,19 @@
   TopPagingEntry.Uintn = *PageTable;
   if (TopPagingEntry.Uintn != 0) {
+    if (PagingMode == PagingPae) {
+      //
+      // Create 4 temporary PDPTE at a 4k-aligned address.
+      // Copy the original PDPTE content and set ReadWrite, UserSupervisor to 1, set Nx to 0.
+      //
+      TopPagingEntry.Uintn = ALIGN_VALUE ((UINTN)BufferInStack, BASE_4KB);
+      PagingEntry          = (IA32_PAGING_ENTRY *)(TopPagingEntry.Uintn);
+      CopyMem (PagingEntry, (VOID *)(*PageTable), MAX_PAE_PDPTE_NUM * sizeof (IA32_PAGING_ENTRY));
+      for (Index = 0; Index < MAX_PAE_PDPTE_NUM; Index++) {
+        PagingEntry[Index].Pnle.Bits.ReadWrite      = 1;
+        PagingEntry[Index].Pnle.Bits.UserSupervisor = 1;
+        PagingEntry[Index].Pnle.Bits.Nx             = 0;
+      }
+    }
+
     TopPagingEntry.Pce.Present        = 1;
     TopPagingEntry.Pce.ReadWrite      = 1;
@@ -626,10 +751,16 @@
   }
 
-  ParentAttribute.Uint64                    = 0;
-  ParentAttribute.Bits.PageTableBaseAddress = 1;
-  ParentAttribute.Bits.Present              = 1;
-  ParentAttribute.Bits.ReadWrite            = 1;
-  ParentAttribute.Bits.UserSupervisor       = 1;
-  ParentAttribute.Bits.Nx                   = 0;
+  if (IsModified == NULL) {
+    IsModified = &LocalIsModified;
+  }
+
+  *IsModified = FALSE;
+
+  ParentAttribute.Uint64                       = 0;
+  ParentAttribute.Bits.PageTableBaseAddressLow = 1;
+  ParentAttribute.Bits.Present                 = 1;
+  ParentAttribute.Bits.ReadWrite               = 1;
+  ParentAttribute.Bits.UserSupervisor          = 1;
+  ParentAttribute.Bits.Nx                      = 0;
 
   //
@@ -649,6 +780,8 @@
                    0,
                    Attribute,
-                   Mask
+                   Mask,
+                   IsModified
                    );
+  ASSERT (*IsModified == FALSE);
   if (RETURN_ERROR (Status)) {
     return Status;
@@ -681,8 +814,36 @@
              0,
              Attribute,
-             Mask
+             Mask,
+             IsModified
              );
+
   if (!RETURN_ERROR (Status)) {
-    *PageTable = (UINTN)(TopPagingEntry.Uintn & IA32_PE_BASE_ADDRESS_MASK_40);
+    PagingEntry = (IA32_PAGING_ENTRY *)(UINTN)(TopPagingEntry.Uintn & IA32_PE_BASE_ADDRESS_MASK_40);
+
+    if (PagingMode == PagingPae) {
+      //
+      // These MustBeZero fields are treated as RW and other attributes by the common map logic. So they might be set to 1.
+      //
+      for (Index = 0; Index < MAX_PAE_PDPTE_NUM; Index++) {
+        PagingEntry[Index].PdptePae.Bits.MustBeZero  = 0;
+        PagingEntry[Index].PdptePae.Bits.MustBeZero2 = 0;
+        PagingEntry[Index].PdptePae.Bits.MustBeZero3 = 0;
+      }
+
+      if (*PageTable != 0) {
+        //
+        // Copy temp PDPTE to original PDPTE.
+        //
+        CopyMem ((VOID *)(*PageTable), PagingEntry, MAX_PAE_PDPTE_NUM * sizeof (IA32_PAGING_ENTRY));
+      }
+    }
+
+    if (*PageTable == 0) {
+      //
+      // Do not assign the *PageTable when it's an existing page table.
+      // If it's an existing PAE page table, PagingEntry is the temp buffer in stack.
+      //
+      *PageTable = (UINTN)PagingEntry;
+    }
   }
 
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/CpuPageTableLib/CpuPageTableParse.c
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/CpuPageTableLib/CpuPageTableParse.c	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/CpuPageTableLib/CpuPageTableParse.c	(revision 101291)
@@ -2,5 +2,5 @@
   This library implements CpuPageTableLib that are generic for IA32 family CPU.
 
-  Copyright (c) 2022, Intel Corporation. All rights reserved.<BR>
+  Copyright (c) 2022 - 2023, Intel Corporation. All rights reserved.<BR>
   SPDX-License-Identifier: BSD-2-Clause-Patent
 
@@ -159,4 +159,5 @@
   IN     UINT64              PageTableBaseAddress,
   IN     UINTN               Level,
+  IN     UINTN               MaxLevel,
   IN     UINT64              RegionStart,
   IN     IA32_MAP_ATTRIBUTE  *ParentMapAttribute,
@@ -172,11 +173,13 @@
   IA32_MAP_ATTRIBUTE  MapAttribute;
   UINT64              RegionLength;
+  UINTN               PagingEntryNumber;
 
   ASSERT (OneEntry != NULL);
 
-  PagingEntry  = (IA32_PAGING_ENTRY *)(UINTN)PageTableBaseAddress;
-  RegionLength = REGION_LENGTH (Level);
-
-  for (Index = 0; Index < 512; Index++, RegionStart += RegionLength) {
+  PagingEntry       = (IA32_PAGING_ENTRY *)(UINTN)PageTableBaseAddress;
+  RegionLength      = REGION_LENGTH (Level);
+  PagingEntryNumber = ((MaxLevel == 3) && (Level == 3)) ? MAX_PAE_PDPTE_NUM : 512;
+
+  for (Index = 0; Index < PagingEntryNumber; Index++, RegionStart += RegionLength) {
     if (PagingEntry[Index].Pce.Present == 0) {
       continue;
@@ -229,4 +232,5 @@
         IA32_PNLE_PAGE_TABLE_BASE_ADDRESS (&PagingEntry[Index].Pnle),
         Level - 1,
+        MaxLevel,
         RegionStart,
         &MapAttribute,
@@ -270,4 +274,6 @@
   IA32_MAP_ENTRY      OneEntry;
   UINTN               MaxLevel;
+  UINTN               Index;
+  IA32_PAGING_ENTRY   BufferInStack[MAX_PAE_PDPTE_NUM];
 
   if ((PagingMode == Paging32bit) || (PagingMode >= PagingModeMax)) {
@@ -289,4 +295,15 @@
     *MapCount = 0;
     return RETURN_SUCCESS;
+  }
+
+  if (PagingMode == PagingPae) {
+    CopyMem (BufferInStack, (VOID *)PageTable, sizeof (BufferInStack));
+    for (Index = 0; Index < MAX_PAE_PDPTE_NUM; Index++) {
+      BufferInStack[Index].Pnle.Bits.ReadWrite      = 1;
+      BufferInStack[Index].Pnle.Bits.UserSupervisor = 1;
+      BufferInStack[Index].Pnle.Bits.Nx             = 0;
+    }
+
+    PageTable = (UINTN)BufferInStack;
   }
 
@@ -320,5 +337,5 @@
   *MapCount   = 0;
   LastEntry   = NULL;
-  PageTableLibParsePnle ((UINT64)PageTable, MaxLevel, 0, &NopAttribute, Map, MapCount, MapCapacity, &LastEntry, &OneEntry);
+  PageTableLibParsePnle ((UINT64)PageTable, MaxLevel, MaxLevel, 0, &NopAttribute, Map, MapCount, MapCapacity, &LastEntry, &OneEntry);
 
   if (*MapCount > MapCapacity) {
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/CpuPageTableLib/UnitTest/CpuPageTableLibUnitTestHost.c
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/CpuPageTableLib/UnitTest/CpuPageTableLibUnitTestHost.c	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/CpuPageTableLib/UnitTest/CpuPageTableLibUnitTestHost.c	(revision 101291)
@@ -2,5 +2,5 @@
   Unit tests of the CpuPageTableLib instance of the CpuPageTableLib class
 
-  Copyright (c) 2022, Intel Corporation. All rights reserved.<BR>
+  Copyright (c) 2022 - 2023, Intel Corporation. All rights reserved.<BR>
   SPDX-License-Identifier: BSD-2-Clause-Patent
 
@@ -10,8 +10,9 @@
 
 // ----------------------------------------------------------------------- PageMode--TestCount-TestRangeCount---RandomOptions
-static CPU_PAGE_TABLE_LIB_RANDOM_TEST_CONTEXT  mTestContextPaging4Level    = { Paging4Level, 100, 20, ONLY_ONE_ONE_MAPPING|USE_RANDOM_ARRAY };
-static CPU_PAGE_TABLE_LIB_RANDOM_TEST_CONTEXT  mTestContextPaging4Level1GB = { Paging4Level1GB, 100, 20, ONLY_ONE_ONE_MAPPING|USE_RANDOM_ARRAY };
-static CPU_PAGE_TABLE_LIB_RANDOM_TEST_CONTEXT  mTestContextPaging5Level    = { Paging5Level, 100, 20, ONLY_ONE_ONE_MAPPING|USE_RANDOM_ARRAY };
-static CPU_PAGE_TABLE_LIB_RANDOM_TEST_CONTEXT  mTestContextPaging5Level1GB = { Paging5Level1GB, 100, 20, ONLY_ONE_ONE_MAPPING|USE_RANDOM_ARRAY };
+// static CPU_PAGE_TABLE_LIB_RANDOM_TEST_CONTEXT  mTestContextPaging4Level    = { Paging4Level, 30, 20, USE_RANDOM_ARRAY };
+// static CPU_PAGE_TABLE_LIB_RANDOM_TEST_CONTEXT  mTestContextPaging4Level1GB = { Paging4Level1GB, 30, 20, USE_RANDOM_ARRAY };
+// static CPU_PAGE_TABLE_LIB_RANDOM_TEST_CONTEXT  mTestContextPaging5Level    = { Paging5Level, 30, 20, USE_RANDOM_ARRAY };
+// static CPU_PAGE_TABLE_LIB_RANDOM_TEST_CONTEXT  mTestContextPaging5Level1GB = { Paging5Level1GB, 30, 20, USE_RANDOM_ARRAY };
+// static CPU_PAGE_TABLE_LIB_RANDOM_TEST_CONTEXT  mTestContextPagingPae       = { PagingPae, 30, 20, USE_RANDOM_ARRAY };
 
 /**
@@ -52,5 +53,5 @@
   // If the input linear address is not 4K align, it should return invalid parameter
   //
-  UT_ASSERT_EQUAL (PageTableMap (&PageTable, PagingMode, &Buffer, &PageTableBufferSize, 1, SIZE_4KB, &MapAttribute, &MapMask), RETURN_INVALID_PARAMETER);
+  UT_ASSERT_EQUAL (PageTableMap (&PageTable, PagingMode, &Buffer, &PageTableBufferSize, 1, SIZE_4KB, &MapAttribute, &MapMask, NULL), RETURN_INVALID_PARAMETER);
 
   //
@@ -58,5 +59,5 @@
   //
   PageTableBufferSize = 10;
-  UT_ASSERT_EQUAL (PageTableMap (&PageTable, PagingMode, &Buffer, &PageTableBufferSize, 0, SIZE_4KB, &MapAttribute, &MapMask), RETURN_INVALID_PARAMETER);
+  UT_ASSERT_EQUAL (PageTableMap (&PageTable, PagingMode, &Buffer, &PageTableBufferSize, 0, SIZE_4KB, &MapAttribute, &MapMask, NULL), RETURN_INVALID_PARAMETER);
 
   //
@@ -65,5 +66,5 @@
   PageTableBufferSize = 0;
   PagingMode          = Paging32bit;
-  UT_ASSERT_EQUAL (PageTableMap (&PageTable, PagingMode, &Buffer, &PageTableBufferSize, 1, SIZE_4KB, &MapAttribute, &MapMask), RETURN_UNSUPPORTED);
+  UT_ASSERT_EQUAL (PageTableMap (&PageTable, PagingMode, &Buffer, &PageTableBufferSize, 1, SIZE_4KB, &MapAttribute, &MapMask, NULL), RETURN_UNSUPPORTED);
 
   //
@@ -71,5 +72,5 @@
   //
   PagingMode = Paging5Level1GB;
-  UT_ASSERT_EQUAL (PageTableMap (&PageTable, PagingMode, &Buffer, &PageTableBufferSize, 1, SIZE_4KB, &MapAttribute, NULL), RETURN_INVALID_PARAMETER);
+  UT_ASSERT_EQUAL (PageTableMap (&PageTable, PagingMode, &Buffer, &PageTableBufferSize, 1, SIZE_4KB, &MapAttribute, NULL, NULL), RETURN_INVALID_PARAMETER);
 
   return UNIT_TEST_PASSED;
@@ -120,8 +121,8 @@
   // Create page table to cover [0, 10M], it should have 5 PTE
   //
-  Status = PageTableMap (&PageTable, PagingMode, Buffer, &PageTableBufferSize, 0, (UINT64)SIZE_2MB * 5, &MapAttribute, &MapMask);
+  Status = PageTableMap (&PageTable, PagingMode, Buffer, &PageTableBufferSize, 0, (UINT64)SIZE_2MB * 5, &MapAttribute, &MapMask, NULL);
   UT_ASSERT_EQUAL (Status, RETURN_BUFFER_TOO_SMALL);
   Buffer = AllocatePages (EFI_SIZE_TO_PAGES (PageTableBufferSize));
-  Status = PageTableMap (&PageTable, PagingMode, Buffer, &PageTableBufferSize, 0, (UINT64)SIZE_2MB * 5, &MapAttribute, &MapMask);
+  Status = PageTableMap (&PageTable, PagingMode, Buffer, &PageTableBufferSize, 0, (UINT64)SIZE_2MB * 5, &MapAttribute, &MapMask, NULL);
   UT_ASSERT_EQUAL (Status, RETURN_SUCCESS);
   TestStatus = IsPageTableValid (PageTable, PagingMode);
@@ -135,5 +136,5 @@
   //
   Buffer = NULL;
-  Status = PageTableMap (&PageTable, PagingMode, Buffer, &PageTableBufferSize, 0, (UINT64)SIZE_4KB, &MapAttribute, &MapMask);
+  Status = PageTableMap (&PageTable, PagingMode, Buffer, &PageTableBufferSize, 0, (UINT64)SIZE_4KB, &MapAttribute, &MapMask, NULL);
   UT_ASSERT_EQUAL (PageTableBufferSize, 0);
   UT_ASSERT_EQUAL (Status, RETURN_SUCCESS);
@@ -149,5 +150,5 @@
   MapMask.Bits.Nx     = 0;
   PageTableBufferSize = 0;
-  Status              = PageTableMap (&PageTable, PagingMode, NULL, &PageTableBufferSize, 0, (UINT64)SIZE_4KB, &MapAttribute, &MapMask);
+  Status              = PageTableMap (&PageTable, PagingMode, NULL, &PageTableBufferSize, 0, (UINT64)SIZE_4KB, &MapAttribute, &MapMask, NULL);
   UT_ASSERT_EQUAL (Status, RETURN_SUCCESS);
   UT_ASSERT_EQUAL (PageTableBufferSize, 0);
@@ -165,5 +166,5 @@
   MapMask.Bits.Accessed      = 1;
   PageTableBufferSize        = 0;
-  Status                     = PageTableMap (&PageTable, PagingMode, NULL, &PageTableBufferSize, (UINT64)SIZE_2MB, (UINT64)SIZE_2MB, &MapAttribute, &MapMask);
+  Status                     = PageTableMap (&PageTable, PagingMode, NULL, &PageTableBufferSize, (UINT64)SIZE_2MB, (UINT64)SIZE_2MB, &MapAttribute, &MapMask, NULL);
   UT_ASSERT_EQUAL (Status, RETURN_SUCCESS);
   UT_ASSERT_EQUAL (PageTableBufferSize, 0);
@@ -218,8 +219,8 @@
   MapMask.Bits.Present      = 1;
   MapMask.Uint64            = MAX_UINT64;
-  Status                    = PageTableMap (&PageTable, PagingMode, Buffer, &PageTableBufferSize, (UINT64)0, (UINT64)SIZE_1GB, &MapAttribute, &MapMask);
+  Status                    = PageTableMap (&PageTable, PagingMode, Buffer, &PageTableBufferSize, (UINT64)0, (UINT64)SIZE_1GB, &MapAttribute, &MapMask, NULL);
   UT_ASSERT_EQUAL (Status, RETURN_BUFFER_TOO_SMALL);
   Buffer = AllocatePages (EFI_SIZE_TO_PAGES (PageTableBufferSize));
-  Status = PageTableMap (&PageTable, PagingMode, Buffer, &PageTableBufferSize, (UINT64)0, (UINT64)SIZE_1GB, &MapAttribute, &MapMask);
+  Status = PageTableMap (&PageTable, PagingMode, Buffer, &PageTableBufferSize, (UINT64)0, (UINT64)SIZE_1GB, &MapAttribute, &MapMask, NULL);
   UT_ASSERT_EQUAL (Status, RETURN_SUCCESS);
 
@@ -282,9 +283,9 @@
   // Create Page table to cover [0,2G], with ReadWrite = 1
   //
-  Status = PageTableMap (&PageTable, PagingMode, Buffer, &PageTableBufferSize, 0, SIZE_2GB, &MapAttribute, &MapMask);
+  Status = PageTableMap (&PageTable, PagingMode, Buffer, &PageTableBufferSize, 0, SIZE_2GB, &MapAttribute, &MapMask, NULL);
   UT_ASSERT_EQUAL (Status, RETURN_BUFFER_TOO_SMALL);
   BackupPageTableBufferSize = PageTableBufferSize;
   Buffer                    = AllocatePages (EFI_SIZE_TO_PAGES (PageTableBufferSize));
-  Status                    = PageTableMap (&PageTable, PagingMode, Buffer, &PageTableBufferSize, 0, SIZE_2GB, &MapAttribute, &MapMask);
+  Status                    = PageTableMap (&PageTable, PagingMode, Buffer, &PageTableBufferSize, 0, SIZE_2GB, &MapAttribute, &MapMask, NULL);
   UT_ASSERT_EQUAL (Status, RETURN_SUCCESS);
   IsPageTableValid (PageTable, PagingMode);
@@ -332,5 +333,5 @@
   //
   MapAttribute.Bits.ReadWrite = 0;
-  Status                      = PageTableMap (&PageTable, PagingMode, NULL, &PageTableBufferSize, 0, SIZE_2MB, &MapAttribute, &MapMask);
+  Status                      = PageTableMap (&PageTable, PagingMode, NULL, &PageTableBufferSize, 0, SIZE_2MB, &MapAttribute, &MapMask, NULL);
   UT_ASSERT_EQUAL (Status, RETURN_SUCCESS);
   IsPageTableValid (PageTable, PagingMode);
@@ -361,5 +362,5 @@
   MapAttribute.Bits.ReadWrite = 1;
   PageTableBufferSize         = 0;
-  Status                      = PageTableMap (&PageTable, PagingMode, NULL, &PageTableBufferSize, 0, SIZE_2MB, &MapAttribute, &MapMask);
+  Status                      = PageTableMap (&PageTable, PagingMode, NULL, &PageTableBufferSize, 0, SIZE_2MB, &MapAttribute, &MapMask, NULL);
   UT_ASSERT_EQUAL (Status, RETURN_SUCCESS);
   IsPageTableValid (PageTable, PagingMode);
@@ -423,20 +424,19 @@
   IA32_PAGING_ENTRY   *PagingEntry;
 
-  PagingMode                             = Paging4Level;
-  PageTableBufferSize                    = 0;
-  PageTable                              = 0;
-  Buffer                                 = NULL;
-  MapAttribute.Uint64                    = 0;
-  MapMask.Uint64                         = MAX_UINT64;
-  MapAttribute.Bits.Present              = 1;
-  MapAttribute.Bits.ReadWrite            = 1;
-  MapAttribute.Bits.PageTableBaseAddress = (SIZE_2MB - SIZE_4KB) >> 12;
+  PagingMode                  = Paging4Level;
+  PageTableBufferSize         = 0;
+  PageTable                   = 0;
+  Buffer                      = NULL;
+  MapMask.Uint64              = MAX_UINT64;
+  MapAttribute.Uint64         = (SIZE_2MB - SIZE_4KB);
+  MapAttribute.Bits.Present   = 1;
+  MapAttribute.Bits.ReadWrite = 1;
   //
   // Create Page table to cover [2M-4K, 4M], with ReadWrite = 1
   //
-  Status = PageTableMap (&PageTable, PagingMode, Buffer, &PageTableBufferSize, SIZE_2MB - SIZE_4KB, SIZE_4KB + SIZE_2MB, &MapAttribute, &MapMask);
+  Status = PageTableMap (&PageTable, PagingMode, Buffer, &PageTableBufferSize, SIZE_2MB - SIZE_4KB, SIZE_4KB + SIZE_2MB, &MapAttribute, &MapMask, NULL);
   UT_ASSERT_EQUAL (Status, RETURN_BUFFER_TOO_SMALL);
   Buffer = AllocatePages (EFI_SIZE_TO_PAGES (PageTableBufferSize));
-  Status = PageTableMap (&PageTable, PagingMode, Buffer, &PageTableBufferSize, SIZE_2MB - SIZE_4KB, SIZE_4KB + SIZE_2MB, &MapAttribute, &MapMask);
+  Status = PageTableMap (&PageTable, PagingMode, Buffer, &PageTableBufferSize, SIZE_2MB - SIZE_4KB, SIZE_4KB + SIZE_2MB, &MapAttribute, &MapMask, NULL);
   UT_ASSERT_EQUAL (Status, RETURN_SUCCESS);
   IsPageTableValid (PageTable, PagingMode);
@@ -461,7 +461,7 @@
   // [2M   ,4M], R/W = 1
   //
-  PagingEntry         = (IA32_PAGING_ENTRY *)(UINTN)PageTable;                                           // Get 4 level entry
-  PagingEntry         = (IA32_PAGING_ENTRY *)(UINTN)(PagingEntry->Pnle.Bits.PageTableBaseAddress << 12); // Get 3 level entry
-  PagingEntry         = (IA32_PAGING_ENTRY *)(UINTN)(PagingEntry->Pnle.Bits.PageTableBaseAddress << 12); // Get 2 level entry
+  PagingEntry         = (IA32_PAGING_ENTRY *)(UINTN)PageTable;                                       // Get 4 level entry
+  PagingEntry         = (IA32_PAGING_ENTRY *)(UINTN)IA32_PNLE_PAGE_TABLE_BASE_ADDRESS (PagingEntry); // Get 3 level entry
+  PagingEntry         = (IA32_PAGING_ENTRY *)(UINTN)IA32_PNLE_PAGE_TABLE_BASE_ADDRESS (PagingEntry); // Get 2 level entry
   PagingEntry->Uint64 = PagingEntry->Uint64 & (~(UINT64)0x2);
   MapCount            = 0;
@@ -481,7 +481,6 @@
   UT_ASSERT_EQUAL (Map[1].LinearAddress, SIZE_2MB);
   UT_ASSERT_EQUAL (Map[1].Length, SIZE_2MB);
-  ExpectedMapAttribute.Uint64                    = MapAttribute.Uint64;
-  ExpectedMapAttribute.Bits.ReadWrite            = 1;
-  ExpectedMapAttribute.Bits.PageTableBaseAddress = SIZE_2MB >> 12;
+  ExpectedMapAttribute.Uint64         = MapAttribute.Uint64 + SIZE_4KB;
+  ExpectedMapAttribute.Bits.ReadWrite = 1;
   UT_ASSERT_EQUAL (Map[1].Attribute.Uint64, ExpectedMapAttribute.Uint64);
 
@@ -490,9 +489,9 @@
   // Just need to set [2M-4K,2M], won't need extra size, so the status should be success
   //
-  MapAttribute.Bits.Present              = 1;
-  MapAttribute.Bits.ReadWrite            = 1;
-  PageTableBufferSize                    = 0;
-  MapAttribute.Bits.PageTableBaseAddress = (SIZE_2MB - SIZE_4KB) >> 12;
-  Status                                 = PageTableMap (&PageTable, PagingMode, Buffer, &PageTableBufferSize, SIZE_2MB - SIZE_4KB, SIZE_4KB * 2, &MapAttribute, &MapMask);
+  MapAttribute.Uint64         = SIZE_2MB - SIZE_4KB;
+  MapAttribute.Bits.Present   = 1;
+  MapAttribute.Bits.ReadWrite = 1;
+  PageTableBufferSize         = 0;
+  Status                      = PageTableMap (&PageTable, PagingMode, Buffer, &PageTableBufferSize, SIZE_2MB - SIZE_4KB, SIZE_4KB * 2, &MapAttribute, &MapMask, NULL);
   UT_ASSERT_EQUAL (Status, RETURN_SUCCESS);
   return UNIT_TEST_PASSED;
@@ -541,8 +540,8 @@
   // Create Page table to cover [0,4M], and [4M, 1G] is not present
   //
-  Status = PageTableMap (&PageTable, PagingMode, Buffer, &PageTableBufferSize, (UINT64)0, (UINT64)SIZE_2MB * 2, &MapAttribute, &MapMask);
+  Status = PageTableMap (&PageTable, PagingMode, Buffer, &PageTableBufferSize, (UINT64)0, (UINT64)SIZE_2MB * 2, &MapAttribute, &MapMask, NULL);
   UT_ASSERT_EQUAL (Status, RETURN_BUFFER_TOO_SMALL);
   Buffer = AllocatePages (EFI_SIZE_TO_PAGES (PageTableBufferSize));
-  Status = PageTableMap (&PageTable, PagingMode, Buffer, &PageTableBufferSize, (UINT64)0, (UINT64)SIZE_2MB * 2, &MapAttribute, &MapMask);
+  Status = PageTableMap (&PageTable, PagingMode, Buffer, &PageTableBufferSize, (UINT64)0, (UINT64)SIZE_2MB * 2, &MapAttribute, &MapMask, NULL);
   UT_ASSERT_EQUAL (Status, RETURN_SUCCESS);
   TestStatus = IsPageTableValid (PageTable, PagingMode);
@@ -556,5 +555,5 @@
   //
   PageTableBufferSize = 0;
-  Status              = PageTableMap (&PageTable, PagingMode, NULL, &PageTableBufferSize, (UINT64)0, (UINT64)SIZE_1GB, &MapAttribute, &MapMask);
+  Status              = PageTableMap (&PageTable, PagingMode, NULL, &PageTableBufferSize, (UINT64)0, (UINT64)SIZE_1GB, &MapAttribute, &MapMask, NULL);
   UT_ASSERT_EQUAL (Status, RETURN_SUCCESS);
   TestStatus = IsPageTableValid (PageTable, PagingMode);
@@ -565,5 +564,5 @@
   MapAttribute.Bits.Accessed = 1;
   PageTableBufferSize        = 0;
-  Status                     = PageTableMap (&PageTable, PagingMode, NULL, &PageTableBufferSize, (UINT64)0, (UINT64)SIZE_2MB, &MapAttribute, &MapMask);
+  Status                     = PageTableMap (&PageTable, PagingMode, NULL, &PageTableBufferSize, (UINT64)0, (UINT64)SIZE_2MB, &MapAttribute, &MapMask, NULL);
   //
   // If it didn't use a big 1G entry to cover whole range, only change [0,2M] for some attribute won't need extra memory
@@ -620,8 +619,8 @@
   // Create Page table to cover [0,2G], with Nx = 0
   //
-  Status = PageTableMap (&PageTable, PagingMode, Buffer, &PageTableBufferSize, (UINT64)0, (UINT64)SIZE_1GB * 2, &MapAttribute, &MapMask);
+  Status = PageTableMap (&PageTable, PagingMode, Buffer, &PageTableBufferSize, (UINT64)0, (UINT64)SIZE_1GB * 2, &MapAttribute, &MapMask, NULL);
   UT_ASSERT_EQUAL (Status, RETURN_BUFFER_TOO_SMALL);
   Buffer = AllocatePages (EFI_SIZE_TO_PAGES (PageTableBufferSize));
-  Status = PageTableMap (&PageTable, PagingMode, Buffer, &PageTableBufferSize, (UINT64)0, (UINT64)SIZE_1GB * 2, &MapAttribute, &MapMask);
+  Status = PageTableMap (&PageTable, PagingMode, Buffer, &PageTableBufferSize, (UINT64)0, (UINT64)SIZE_1GB * 2, &MapAttribute, &MapMask, NULL);
   UT_ASSERT_EQUAL (Status, RETURN_SUCCESS);
   TestStatus = IsPageTableValid (PageTable, PagingMode);
@@ -667,5 +666,5 @@
   // Call library to change Nx to 0 for [0,1G]
   //
-  Status = PageTableMap (&PageTable, PagingMode, NULL, &PageTableBufferSize, (UINT64)0, (UINT64)SIZE_1GB, &MapAttribute, &MapMask);
+  Status = PageTableMap (&PageTable, PagingMode, NULL, &PageTableBufferSize, (UINT64)0, (UINT64)SIZE_1GB, &MapAttribute, &MapMask, NULL);
   UT_ASSERT_EQUAL (Status, RETURN_SUCCESS);
   TestStatus = IsPageTableValid (PageTable, PagingMode);
@@ -699,4 +698,129 @@
 
 /**
+  Check if the input Mask and Attribute is as expected when creating new page table or
+  updating existing page table.
+
+  @param[in]  Context    [Optional] An optional parameter that enables:
+                         1) test-case reuse with varied parameters and
+                         2) test-case re-entry for Target tests that need a
+                         reboot.  This parameter is a VOID* and it is the
+                         responsibility of the test author to ensure that the
+                         contents are well understood by all test cases that may
+                         consume it.
+
+  @retval  UNIT_TEST_PASSED             The Unit test has completed and the test
+                                        case was successful.
+  @retval  UNIT_TEST_ERROR_TEST_FAILED  A test case assertion has failed.
+**/
+UNIT_TEST_STATUS
+EFIAPI
+TestCaseToCheckMapMaskAndAttr (
+  IN UNIT_TEST_CONTEXT  Context
+  )
+{
+  UINTN               PageTable;
+  PAGING_MODE         PagingMode;
+  VOID                *Buffer;
+  UINTN               PageTableBufferSize;
+  IA32_MAP_ATTRIBUTE  MapAttribute;
+  IA32_MAP_ATTRIBUTE  ExpectedMapAttribute;
+  IA32_MAP_ATTRIBUTE  MapMask;
+  RETURN_STATUS       Status;
+  IA32_MAP_ENTRY      *Map;
+  UINTN               MapCount;
+
+  PagingMode                = Paging4Level;
+  PageTableBufferSize       = 0;
+  PageTable                 = 0;
+  Buffer                    = NULL;
+  MapAttribute.Uint64       = 0;
+  MapAttribute.Bits.Present = 1;
+  MapMask.Uint64            = 0;
+  MapMask.Bits.Present      = 1;
+  //
+  // Create Page table to cover [0, 2G]. All fields of MapMask should be set.
+  //
+  Status = PageTableMap (&PageTable, PagingMode, Buffer, &PageTableBufferSize, 0, SIZE_2GB, &MapAttribute, &MapMask, NULL);
+  UT_ASSERT_EQUAL (Status, RETURN_INVALID_PARAMETER);
+  MapMask.Uint64 = MAX_UINT64;
+  Status         = PageTableMap (&PageTable, PagingMode, Buffer, &PageTableBufferSize, 0, SIZE_2GB, &MapAttribute, &MapMask, NULL);
+  UT_ASSERT_EQUAL (Status, RETURN_BUFFER_TOO_SMALL);
+  Buffer = AllocatePages (EFI_SIZE_TO_PAGES (PageTableBufferSize));
+  Status = PageTableMap (&PageTable, PagingMode, Buffer, &PageTableBufferSize, 0, SIZE_2GB, &MapAttribute, &MapMask, NULL);
+  UT_ASSERT_EQUAL (Status, RETURN_SUCCESS);
+
+  //
+  // Update Page table to set [2G - 8K, 2G] from present to non-present. All fields of MapMask except present should not be set.
+  //
+  PageTableBufferSize    = 0;
+  MapAttribute.Uint64    = 0;
+  MapMask.Uint64         = 0;
+  MapMask.Bits.Present   = 1;
+  MapMask.Bits.ReadWrite = 1;
+  Status                 = PageTableMap (&PageTable, PagingMode, Buffer, &PageTableBufferSize, SIZE_2GB - SIZE_8KB, SIZE_8KB, &MapAttribute, &MapMask, NULL);
+  UT_ASSERT_EQUAL (Status, RETURN_INVALID_PARAMETER);
+  MapMask.Bits.ReadWrite = 0;
+  Status                 = PageTableMap (&PageTable, PagingMode, Buffer, &PageTableBufferSize, SIZE_2GB - SIZE_8KB, SIZE_8KB, &MapAttribute, &MapMask, NULL);
+  UT_ASSERT_EQUAL (Status, RETURN_BUFFER_TOO_SMALL);
+  Buffer = AllocatePages (EFI_SIZE_TO_PAGES (PageTableBufferSize));
+  Status = PageTableMap (&PageTable, PagingMode, Buffer, &PageTableBufferSize, SIZE_2GB - SIZE_8KB, SIZE_8KB, &MapAttribute, &MapMask, NULL);
+  UT_ASSERT_EQUAL (Status, RETURN_SUCCESS);
+
+  //
+  // Still set [2G - 8K, 2G] as not present, this case is permitted. But set [2G - 8K, 2G] as RW is not permitted.
+  //
+  PageTableBufferSize  = 0;
+  MapAttribute.Uint64  = 0;
+  MapMask.Uint64       = 0;
+  MapMask.Bits.Present = 1;
+  Status               = PageTableMap (&PageTable, PagingMode, Buffer, &PageTableBufferSize, SIZE_2GB - SIZE_8KB, SIZE_8KB, &MapAttribute, &MapMask, NULL);
+  UT_ASSERT_EQUAL (Status, RETURN_SUCCESS);
+  MapAttribute.Bits.ReadWrite = 1;
+  MapMask.Bits.ReadWrite      = 1;
+  Status                      = PageTableMap (&PageTable, PagingMode, Buffer, &PageTableBufferSize, SIZE_2GB - SIZE_8KB, SIZE_8KB, &MapAttribute, &MapMask, NULL);
+  UT_ASSERT_EQUAL (Status, RETURN_INVALID_PARAMETER);
+
+  //
+  // Update Page table to set [2G - 8K, 2G] as present and RW. All fields of MapMask should be set.
+  //
+  PageTableBufferSize         = 0;
+  MapAttribute.Uint64         = SIZE_2GB - SIZE_8KB;
+  MapAttribute.Bits.ReadWrite = 1;
+  MapAttribute.Bits.Present   = 1;
+  MapMask.Uint64              = 0;
+  MapMask.Bits.ReadWrite      = 1;
+  MapMask.Bits.Present        = 1;
+  Status                      = PageTableMap (&PageTable, PagingMode, Buffer, &PageTableBufferSize, SIZE_2GB - SIZE_8KB, SIZE_8KB, &MapAttribute, &MapMask, NULL);
+  UT_ASSERT_EQUAL (Status, RETURN_INVALID_PARAMETER);
+  MapMask.Uint64 = MAX_UINT64;
+  Status         = PageTableMap (&PageTable, PagingMode, Buffer, &PageTableBufferSize, SIZE_2GB - SIZE_8KB, SIZE_8KB, &MapAttribute, &MapMask, NULL);
+  UT_ASSERT_EQUAL (Status, RETURN_SUCCESS);
+
+  MapCount = 0;
+  Status   = PageTableParse (PageTable, PagingMode, NULL, &MapCount);
+  UT_ASSERT_EQUAL (Status, RETURN_BUFFER_TOO_SMALL);
+  Map    = AllocatePages (EFI_SIZE_TO_PAGES (MapCount* sizeof (IA32_MAP_ENTRY)));
+  Status = PageTableParse (PageTable, PagingMode, Map, &MapCount);
+  UT_ASSERT_EQUAL (Status, RETURN_SUCCESS);
+
+  //
+  // There should be two ranges [0, 2G-8k] with RW = 0 and [2G-8k, 2G] with RW = 1
+  //
+  UT_ASSERT_EQUAL (MapCount, 2);
+  UT_ASSERT_EQUAL (Map[0].LinearAddress, 0);
+  UT_ASSERT_EQUAL (Map[0].Length, SIZE_2GB - SIZE_8KB);
+  ExpectedMapAttribute.Uint64       =  0;
+  ExpectedMapAttribute.Bits.Present =  1;
+  UT_ASSERT_EQUAL (Map[0].Attribute.Uint64, ExpectedMapAttribute.Uint64);
+  UT_ASSERT_EQUAL (Map[1].LinearAddress, SIZE_2GB - SIZE_8KB);
+  UT_ASSERT_EQUAL (Map[1].Length, SIZE_8KB);
+  ExpectedMapAttribute.Uint64         =   SIZE_2GB - SIZE_8KB;
+  ExpectedMapAttribute.Bits.Present   = 1;
+  ExpectedMapAttribute.Bits.ReadWrite = 1;
+  UT_ASSERT_EQUAL (Map[1].Attribute.Uint64, ExpectedMapAttribute.Uint64);
+  return UNIT_TEST_PASSED;
+}
+
+/**
   Initialize the unit test framework, suite, and unit tests for the
   sample unit tests and run the unit tests.
@@ -747,5 +871,5 @@
   AddTestCase (ManualTestCase, "Check if the parent entry has different Nx attribute", "Manual Test Case6", TestCaseManualChangeNx, NULL, NULL, NULL);
   AddTestCase (ManualTestCase, "Check if the needed size is expected", "Manual Test Case7", TestCaseManualSizeNotMatch, NULL, NULL, NULL);
-
+  AddTestCase (ManualTestCase, "Check MapMask when creating new page table or mapping not-present range", "Manual Test Case8", TestCaseToCheckMapMaskAndAttr, NULL, NULL, NULL);
   //
   // Populate the Random Test Cases.
@@ -758,8 +882,9 @@
   }
 
-  AddTestCase (RandomTestCase, "Random Test for Paging4Level", "Random Test Case1", TestCaseforRandomTest, NULL, NULL, &mTestContextPaging4Level);
-  AddTestCase (RandomTestCase, "Random Test for Paging4Level1G", "Random Test Case2", TestCaseforRandomTest, NULL, NULL, &mTestContextPaging4Level1GB);
-  AddTestCase (RandomTestCase, "Random Test for Paging5Level", "Random Test Case3", TestCaseforRandomTest, NULL, NULL, &mTestContextPaging5Level);
-  AddTestCase (RandomTestCase, "Random Test for Paging5Level1G", "Random Test Case4", TestCaseforRandomTest, NULL, NULL, &mTestContextPaging5Level1GB);
+  // AddTestCase (RandomTestCase, "Random Test for Paging4Level", "Random Test Case1", TestCaseforRandomTest, NULL, NULL, &mTestContextPaging4Level);
+  // AddTestCase (RandomTestCase, "Random Test for Paging4Level1G", "Random Test Case2", TestCaseforRandomTest, NULL, NULL, &mTestContextPaging4Level1GB);
+  // AddTestCase (RandomTestCase, "Random Test for Paging5Level", "Random Test Case3", TestCaseforRandomTest, NULL, NULL, &mTestContextPaging5Level);
+  // AddTestCase (RandomTestCase, "Random Test for Paging5Level1G", "Random Test Case4", TestCaseforRandomTest, NULL, NULL, &mTestContextPaging5Level1GB);
+  // AddTestCase (RandomTestCase, "Random Test for PagingPae", "Random Test Case5", TestCaseforRandomTest, NULL, NULL, &mTestContextPagingPae);
 
   //
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/CpuPageTableLib/UnitTest/RandomTest.c
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/CpuPageTableLib/UnitTest/RandomTest.c	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/CpuPageTableLib/UnitTest/RandomTest.c	(revision 101291)
@@ -2,5 +2,5 @@
   Random test case for Unit tests of the CpuPageTableLib instance of the CpuPageTableLib class
 
-  Copyright (c) 2022, Intel Corporation. All rights reserved.<BR>
+  Copyright (c) 2022 - 2023, Intel Corporation. All rights reserved.<BR>
   SPDX-License-Identifier: BSD-2-Clause-Patent
 
@@ -83,20 +83,4 @@
 
 /**
-  Return a random boolean.
-
-  @return boolean
-**/
-BOOLEAN
-RandomBoolean (
-  VOID
-  )
-{
-  BOOLEAN  Value;
-
-  LocalRandomBytes ((UINT8 *)&Value, sizeof (BOOLEAN));
-  return Value%2;
-}
-
-/**
   Return a 32bit random number.
 
@@ -138,4 +122,19 @@
 
   return (UINT64)(Value % (Limit - Start  + 1)) + Start;
+}
+
+/**
+  Returns true with the percentage of input Probability.
+
+  @param[in]   Probability    The percentage to return true.
+
+  @return boolean
+**/
+BOOLEAN
+RandomBoolean (
+  UINT8  Probability
+  )
+{
+  return ((Probability > ((UINT8)Random64 (0, 100))) ? TRUE : FALSE);
 }
 
@@ -159,5 +158,6 @@
 {
   UINT64             Index;
-  UINT64             TempPhysicalBase;
+  UINT32             PageTableBaseAddressLow;
+  UINT32             PageTableBaseAddressHigh;
   IA32_PAGING_ENTRY  *ChildPageEntry;
   UNIT_TEST_STATUS   Status;
@@ -179,10 +179,12 @@
     }
 
-    if ((RandomNumber < 100) && RandomBoolean ()) {
+    if ((RandomNumber < 100) && RandomBoolean (50)) {
       RandomNumber++;
       if (Level == 1) {
-        TempPhysicalBase = PagingEntry->Pte4K.Bits.PageTableBaseAddress;
+        PageTableBaseAddressLow  = PagingEntry->Pte4K.Bits.PageTableBaseAddressLow;
+        PageTableBaseAddressHigh = PagingEntry->Pte4K.Bits.PageTableBaseAddressHigh;
       } else {
-        TempPhysicalBase = PagingEntry->PleB.Bits.PageTableBaseAddress;
+        PageTableBaseAddressLow  = PagingEntry->PleB.Bits.PageTableBaseAddressLow;
+        PageTableBaseAddressHigh = PagingEntry->PleB.Bits.PageTableBaseAddressHigh;
       }
 
@@ -190,7 +192,9 @@
       PagingEntry->Pte4K.Bits.Present = 1;
       if (Level == 1) {
-        PagingEntry->Pte4K.Bits.PageTableBaseAddress = TempPhysicalBase;
+        PagingEntry->Pte4K.Bits.PageTableBaseAddressLow  = PageTableBaseAddressLow;
+        PagingEntry->Pte4K.Bits.PageTableBaseAddressHigh = PageTableBaseAddressHigh;
       } else {
-        PagingEntry->PleB.Bits.PageTableBaseAddress = TempPhysicalBase;
+        PagingEntry->PleB.Bits.PageTableBaseAddressLow  = PageTableBaseAddressLow;
+        PagingEntry->PleB.Bits.PageTableBaseAddressHigh = PageTableBaseAddressHigh;
       }
 
@@ -212,15 +216,17 @@
   }
 
-  if ((RandomNumber < 100) && RandomBoolean ()) {
+  if ((RandomNumber < 100) && RandomBoolean (50)) {
     RandomNumber++;
-    TempPhysicalBase = PagingEntry->Pnle.Bits.PageTableBaseAddress;
-
-    PagingEntry->Uint64                         = Random64 (0, MAX_UINT64) & mValidMaskNoLeaf[Level].Uint64;
-    PagingEntry->Pnle.Bits.Present              = 1;
-    PagingEntry->Pnle.Bits.PageTableBaseAddress = TempPhysicalBase;
+    PageTableBaseAddressLow  = PagingEntry->PleB.Bits.PageTableBaseAddressLow;
+    PageTableBaseAddressHigh = PagingEntry->PleB.Bits.PageTableBaseAddressHigh;
+
+    PagingEntry->Uint64                             = Random64 (0, MAX_UINT64) & mValidMaskNoLeaf[Level].Uint64;
+    PagingEntry->Pnle.Bits.Present                  = 1;
+    PagingEntry->PleB.Bits.PageTableBaseAddressLow  = PageTableBaseAddressLow;
+    PagingEntry->PleB.Bits.PageTableBaseAddressHigh = PageTableBaseAddressHigh;
     ASSERT ((PagingEntry->Uint64 & mValidMaskLeafFlag[Level].Uint64) != mValidMaskLeafFlag[Level].Uint64);
   }
 
-  ChildPageEntry = (IA32_PAGING_ENTRY  *)(UINTN)((PagingEntry->Pnle.Bits.PageTableBaseAddress) << 12);
+  ChildPageEntry = (IA32_PAGING_ENTRY  *)(UINTN)(IA32_PNLE_PAGE_TABLE_BASE_ADDRESS (&PagingEntry->Pnle));
   for (Index = 0; Index < 512; Index++) {
     Status = ValidateAndRandomeModifyPageTablePageTableEntry (&ChildPageEntry[Index], Level-1, MaxLeafLevel, Address + (Index<<(9*(Level-1) + 3)));
@@ -253,8 +259,7 @@
   IA32_PAGING_ENTRY  *PagingEntry;
 
-  if ((PagingMode == Paging32bit) || (PagingMode == PagingPae) || (PagingMode >= PagingModeMax)) {
+  if ((PagingMode == Paging32bit) || (PagingMode >= PagingModeMax)) {
     //
     // 32bit paging is never supported.
-    // PAE paging will be supported later.
     //
     return UNIT_TEST_ERROR_TEST_FAILED;
@@ -276,4 +281,25 @@
 
 /**
+  Remove the last MAP_ENTRY in MapEntrys.
+
+  @param MapEntrys   Pointer to MapEntrys buffer
+**/
+VOID
+RemoveLastMapEntry (
+  IN OUT MAP_ENTRYS  *MapEntrys
+  )
+{
+  UINTN  MapsIndex;
+
+  if (MapEntrys->Count == 0) {
+    return;
+  }
+
+  MapsIndex = MapEntrys->Count - 1;
+  ZeroMem (&(MapEntrys->Maps[MapsIndex]), sizeof (MAP_ENTRY));
+  MapEntrys->Count = MapsIndex;
+}
+
+/**
   Generate single random map entry.
   The map entry can be the input of function PageTableMap
@@ -300,5 +326,5 @@
   // use AlignedTable to avoid that a random number can be very hard to be 1G or 2M aligned
   //
-  if ((MapsIndex != 0) &&  (RandomBoolean ())) {
+  if ((MapsIndex != 0) &&  (RandomBoolean (50))) {
     FormerLinearAddress = MapEntrys->Maps[Random32 (0, (UINT32)MapsIndex-1)].LinearAddress;
     if (FormerLinearAddress < 2 * (UINT64)SIZE_1GB) {
@@ -324,10 +350,19 @@
   MapEntrys->Maps[MapsIndex].Length = Random64 (0, MIN (MaxAddress - MapEntrys->Maps[MapsIndex].LinearAddress, 10 * (UINT64)SIZE_1GB)) & AlignedTable[Random32 (0, ARRAY_SIZE (AlignedTable) -1)];
 
-  if ((MapsIndex != 0)  && (RandomBoolean ())) {
+  if ((MapsIndex != 0)  && (RandomBoolean (50))) {
     MapEntrys->Maps[MapsIndex].Attribute.Uint64 = MapEntrys->Maps[Random32 (0, (UINT32)MapsIndex-1)].Attribute.Uint64;
     MapEntrys->Maps[MapsIndex].Mask.Uint64      = MapEntrys->Maps[Random32 (0, (UINT32)MapsIndex-1)].Mask.Uint64;
   } else {
     MapEntrys->Maps[MapsIndex].Attribute.Uint64 = Random64 (0, MAX_UINT64) & mSupportedBit.Uint64;
-    MapEntrys->Maps[MapsIndex].Mask.Uint64      = Random64 (0, MAX_UINT64) & mSupportedBit.Uint64;
+    if (RandomBoolean (5)) {
+      //
+      // The probability to get random Mask should be small since all bits of a random number
+      // have a high probability of containing 0, which may be a invalid input.
+      //
+      MapEntrys->Maps[MapsIndex].Mask.Uint64 = Random64 (0, MAX_UINT64) & mSupportedBit.Uint64;
+    } else {
+      MapEntrys->Maps[MapsIndex].Mask.Uint64 = MAX_UINT64;
+    }
+
     if (MapEntrys->Maps[MapsIndex].Mask.Bits.ProtectionKey != 0) {
       MapEntrys->Maps[MapsIndex].Mask.Bits.ProtectionKey = 0xF;
@@ -336,16 +371,10 @@
 
   if (mRandomOption & ONLY_ONE_ONE_MAPPING) {
-    MapEntrys->Maps[MapsIndex].Attribute.Bits.PageTableBaseAddress = MapEntrys->Maps[MapsIndex].LinearAddress >> 12;
-    MapEntrys->Maps[MapsIndex].Mask.Bits.PageTableBaseAddress      = 0xFFFFFFFFFF;
+    MapEntrys->Maps[MapsIndex].Attribute.Uint64 &= (~IA32_MAP_ATTRIBUTE_PAGE_TABLE_BASE_ADDRESS_MASK);
+    MapEntrys->Maps[MapsIndex].Attribute.Uint64 |= MapEntrys->Maps[MapsIndex].LinearAddress;
+    MapEntrys->Maps[MapsIndex].Mask.Uint64      |= IA32_MAP_ATTRIBUTE_PAGE_TABLE_BASE_ADDRESS_MASK;
   } else {
-    //
-    // Todo: If the mask bit for base address is zero, when dump the pagetable, every entry mapping to physical address zeor.
-    //       This means the map count will be a large number, and impossible to finish in proper time.
-    //       Need to avoid such case when remove the Random option ONLY_ONE_ONE_MAPPING
-    //
-    MapEntrys->Maps[MapsIndex].Attribute.Bits.PageTableBaseAddress = (Random64 (0, (((UINT64)1)<<52) - 1) & AlignedTable[Random32 (0, ARRAY_SIZE (AlignedTable) -1)])>> 12;
-    if (RandomBoolean ()) {
-      MapEntrys->Maps[MapsIndex].Mask.Bits.PageTableBaseAddress = 0;
-    }
+    MapEntrys->Maps[MapsIndex].Attribute.Uint64 &= (~IA32_MAP_ATTRIBUTE_PAGE_TABLE_BASE_ADDRESS_MASK);
+    MapEntrys->Maps[MapsIndex].Attribute.Uint64 |= (Random64 (0, (((UINT64)1)<<52) - 1) & AlignedTable[Random32 (0, ARRAY_SIZE (AlignedTable) -1)]);
   }
 
@@ -394,6 +423,7 @@
   for (Index = 0; Index < MapCount; Index++) {
     if ((Address >= Map[Index].LinearAddress) && (Address < (Map[Index].LinearAddress + Map[Index].Length))) {
-      AttributeInMap.Uint64                    = (Map[Index].Attribute.Uint64 & mSupportedBit.Uint64);
-      AttributeInMap.Bits.PageTableBaseAddress = ((Address - Map[Index].LinearAddress) >> 12) + Map[Index].Attribute.Bits.PageTableBaseAddress;
+      AttributeInMap.Uint64  = (Map[Index].Attribute.Uint64 & mSupportedBit.Uint64);
+      AttributeInMap.Uint64 &= (~IA32_MAP_ATTRIBUTE_PAGE_TABLE_BASE_ADDRESS_MASK);
+      AttributeInMap.Uint64 |= (Address - Map[Index].LinearAddress + IA32_MAP_ATTRIBUTE_PAGE_TABLE_BASE_ADDRESS (&Map[Index].Attribute)) & IA32_MAP_ATTRIBUTE_PAGE_TABLE_BASE_ADDRESS_MASK;
       break;
     }
@@ -405,6 +435,8 @@
   for (Index = 0; Index < InitMapCount; Index++) {
     if ((Address >= InitMap[Index].LinearAddress) && (Address < (InitMap[Index].LinearAddress + InitMap[Index].Length))) {
-      AttributeInInitMap.Uint64                    = (InitMap[Index].Attribute.Uint64 & mSupportedBit.Uint64);
-      AttributeInInitMap.Bits.PageTableBaseAddress = ((Address - InitMap[Index].LinearAddress) >> 12) + InitMap[Index].Attribute.Bits.PageTableBaseAddress;
+      AttributeInInitMap.Uint64  = (InitMap[Index].Attribute.Uint64 & mSupportedBit.Uint64);
+      AttributeInInitMap.Uint64 &= (~IA32_MAP_ATTRIBUTE_PAGE_TABLE_BASE_ADDRESS_MASK);
+      AttributeInInitMap.Uint64 |= (Address - InitMap[Index].LinearAddress + IA32_MAP_ATTRIBUTE_PAGE_TABLE_BASE_ADDRESS (&InitMap[Index].Attribute)) & IA32_MAP_ATTRIBUTE_PAGE_TABLE_BASE_ADDRESS_MASK;
+
       break;
     }
@@ -423,6 +455,7 @@
       AttributeInMapEntrys.Uint64 &= (~MapEntrys->Maps[Index].Mask.Uint64);
       AttributeInMapEntrys.Uint64 |=  (MapEntrys->Maps[Index].Attribute.Uint64 & MapEntrys->Maps[Index].Mask.Uint64);
-      if (MapEntrys->Maps[Index].Mask.Bits.PageTableBaseAddress != 0) {
-        AttributeInMapEntrys.Bits.PageTableBaseAddress = ((Address - MapEntrys->Maps[Index].LinearAddress) >> 12) + MapEntrys->Maps[Index].Attribute.Bits.PageTableBaseAddress;
+      if (IA32_MAP_ATTRIBUTE_PAGE_TABLE_BASE_ADDRESS (&MapEntrys->Maps[Index].Mask) != 0) {
+        AttributeInMapEntrys.Uint64 &= (~IA32_MAP_ATTRIBUTE_PAGE_TABLE_BASE_ADDRESS_MASK);
+        AttributeInMapEntrys.Uint64 |= (Address - MapEntrys->Maps[Index].LinearAddress + IA32_MAP_ATTRIBUTE_PAGE_TABLE_BASE_ADDRESS (&MapEntrys->Maps[Index].Attribute)) & IA32_MAP_ATTRIBUTE_PAGE_TABLE_BASE_ADDRESS_MASK;
       }
     }
@@ -438,6 +471,6 @@
     DEBUG ((DEBUG_INFO, "======detailed information begin=====\n"));
     DEBUG ((DEBUG_INFO, "\nError: Detect different attribute on a point with linear address: 0x%lx\n", Address));
-    DEBUG ((DEBUG_INFO, "By parsing page table, the point has Attribute 0x%lx, and map to physical address 0x%lx\n", IA32_MAP_ATTRIBUTE_ATTRIBUTES (&AttributeInMap) & MaskInMapEntrys.Uint64, AttributeInMap.Bits.PageTableBaseAddress));
-    DEBUG ((DEBUG_INFO, "While according to inputs, the point should Attribute 0x%lx, and should map to physical address 0x%lx\n", IA32_MAP_ATTRIBUTE_ATTRIBUTES (&AttributeInMapEntrys) & MaskInMapEntrys.Uint64, AttributeInMapEntrys.Bits.PageTableBaseAddress));
+    DEBUG ((DEBUG_INFO, "By parsing page table, the point has Attribute 0x%lx, and map to physical address 0x%lx\n", IA32_MAP_ATTRIBUTE_ATTRIBUTES (&AttributeInMap) & MaskInMapEntrys.Uint64, IA32_MAP_ATTRIBUTE_PAGE_TABLE_BASE_ADDRESS (&AttributeInMap)));
+    DEBUG ((DEBUG_INFO, "While according to inputs, the point should Attribute 0x%lx, and should map to physical address 0x%lx\n", IA32_MAP_ATTRIBUTE_ATTRIBUTES (&AttributeInMapEntrys) & MaskInMapEntrys.Uint64, IA32_MAP_ATTRIBUTE_PAGE_TABLE_BASE_ADDRESS (&AttributeInMapEntrys)));
     DEBUG ((DEBUG_INFO, "The total Mask is 0x%lx\n", MaskInMapEntrys.Uint64));
 
@@ -610,21 +643,67 @@
   )
 {
-  UINTN             MapsIndex;
-  RETURN_STATUS     Status;
-  UINTN             PageTableBufferSize;
-  VOID              *Buffer;
-  IA32_MAP_ENTRY    *Map;
-  UINTN             MapCount;
-  UINTN             Index;
-  UINTN             KeyPointCount;
-  UINTN             NewKeyPointCount;
-  UINT64            *KeyPointBuffer;
-  UINTN             Level;
-  UINT64            Value;
-  UNIT_TEST_STATUS  TestStatus;
-
-  MapsIndex = MapEntrys->Count;
+  UINTN               MapsIndex;
+  RETURN_STATUS       Status;
+  UINTN               PageTableBufferSize;
+  VOID                *Buffer;
+  IA32_MAP_ENTRY      *Map;
+  UINTN               MapCount;
+  IA32_MAP_ENTRY      *Map2;
+  UINTN               MapCount2;
+  UINTN               Index;
+  UINTN               KeyPointCount;
+  UINTN               NewKeyPointCount;
+  UINT64              *KeyPointBuffer;
+  UINTN               Level;
+  UINT64              Value;
+  UNIT_TEST_STATUS    TestStatus;
+  MAP_ENTRY           *LastMapEntry;
+  IA32_MAP_ATTRIBUTE  *Mask;
+  IA32_MAP_ATTRIBUTE  *Attribute;
+  UINT64              LastNotPresentRegionStart;
+  BOOLEAN             IsNotPresent;
+  BOOLEAN             IsModified;
+
+  MapsIndex                 = MapEntrys->Count;
+  MapCount                  = 0;
+  LastNotPresentRegionStart = 0;
+  IsNotPresent              = FALSE;
+  IsModified                = FALSE;
 
   GenerateSingleRandomMapEntry (MaxAddress, MapEntrys);
+  LastMapEntry = &MapEntrys->Maps[MapsIndex];
+  Status       = PageTableParse (*PageTable, PagingMode, NULL, &MapCount);
+
+  if (MapCount != 0) {
+    UT_ASSERT_EQUAL (Status, RETURN_BUFFER_TOO_SMALL);
+    Map = AllocatePages (EFI_SIZE_TO_PAGES (MapCount * sizeof (IA32_MAP_ENTRY)));
+    ASSERT (Map != NULL);
+    Status = PageTableParse (*PageTable, PagingMode, Map, &MapCount);
+  }
+
+  //
+  // Check if the generated MapEntrys->Maps[MapsIndex] contains not-present range.
+  //
+  if (LastMapEntry->Length > 0) {
+    for (Index = 0; Index < MapCount; Index++) {
+      if ((LastNotPresentRegionStart < Map[Index].LinearAddress) &&
+          (LastMapEntry->LinearAddress < Map[Index].LinearAddress) && (LastMapEntry->LinearAddress + LastMapEntry->Length > LastNotPresentRegionStart))
+      {
+        //
+        // MapEntrys->Maps[MapsIndex] contains not-present range in exsiting page table.
+        //
+        break;
+      }
+
+      LastNotPresentRegionStart = Map[Index].LinearAddress + Map[Index].Length;
+    }
+
+    //
+    // Either LastMapEntry overlaps with the not-present region in the very end
+    // Or it overlaps with one in the middle
+    if (LastNotPresentRegionStart < LastMapEntry->LinearAddress + LastMapEntry->Length) {
+      IsNotPresent = TRUE;
+    }
+  }
 
   PageTableBufferSize = 0;
@@ -634,9 +713,51 @@
                           NULL,
                           &PageTableBufferSize,
-                          MapEntrys->Maps[MapsIndex].LinearAddress,
-                          MapEntrys->Maps[MapsIndex].Length,
-                          &MapEntrys->Maps[MapsIndex].Attribute,
-                          &MapEntrys->Maps[MapsIndex].Mask
+                          LastMapEntry->LinearAddress,
+                          LastMapEntry->Length,
+                          &LastMapEntry->Attribute,
+                          &LastMapEntry->Mask,
+                          &IsModified
                           );
+
+  Attribute = &LastMapEntry->Attribute;
+  Mask      = &LastMapEntry->Mask;
+  //
+  // If set [LinearAddress, LinearAddress+Attribute] to not preset, all
+  // other attributes should not be provided.
+  //
+  if ((LastMapEntry->Length > 0) && (Attribute->Bits.Present == 0) && (Mask->Bits.Present == 1) && (Mask->Uint64 > 1)) {
+    RemoveLastMapEntry (MapEntrys);
+    UT_ASSERT_EQUAL (Status, RETURN_INVALID_PARAMETER);
+    return UNIT_TEST_PASSED;
+  }
+
+  //
+  // Return Status for non-present range also should be InvalidParameter when:
+  // 1. Some of attributes are not provided when mapping non-present range to present.
+  // 2. Set any other attribute without setting the non-present range to Present.
+  //
+  if (IsNotPresent) {
+    if ((Mask->Bits.Present == 1) && (Attribute->Bits.Present == 1)) {
+      //
+      // Creating new page table or remapping non-present range to present.
+      //
+      if ((Mask->Bits.ReadWrite == 0) || (Mask->Bits.UserSupervisor == 0) || (Mask->Bits.WriteThrough == 0) || (Mask->Bits.CacheDisabled == 0) ||
+          (Mask->Bits.Accessed == 0) || (Mask->Bits.Dirty == 0) || (Mask->Bits.Pat == 0) || (Mask->Bits.Global == 0) ||
+          ((Mask->Bits.PageTableBaseAddressLow == 0) && (Mask->Bits.PageTableBaseAddressHigh == 0)) || (Mask->Bits.ProtectionKey == 0) || (Mask->Bits.Nx == 0))
+      {
+        RemoveLastMapEntry (MapEntrys);
+        UT_ASSERT_EQUAL (Status, RETURN_INVALID_PARAMETER);
+        return UNIT_TEST_PASSED;
+      }
+    } else if ((Mask->Bits.Present == 0) && (Mask->Uint64 > 1)) {
+      //
+      // Only change other attributes for non-present range is not permitted.
+      //
+      RemoveLastMapEntry (MapEntrys);
+      UT_ASSERT_EQUAL (Status, RETURN_INVALID_PARAMETER);
+      return UNIT_TEST_PASSED;
+    }
+  }
+
   if (PageTableBufferSize != 0) {
     UT_ASSERT_EQUAL (Status, RETURN_BUFFER_TOO_SMALL);
@@ -653,8 +774,9 @@
                Buffer,
                &PageTableBufferSize,
-               MapEntrys->Maps[MapsIndex].LinearAddress,
-               MapEntrys->Maps[MapsIndex].Length,
-               &MapEntrys->Maps[MapsIndex].Attribute,
-               &MapEntrys->Maps[MapsIndex].Mask
+               LastMapEntry->LinearAddress,
+               LastMapEntry->Length,
+               &LastMapEntry->Attribute,
+               &LastMapEntry->Mask,
+               &IsModified
                );
   }
@@ -670,16 +792,29 @@
   }
 
-  MapCount = 0;
-  Status   = PageTableParse (*PageTable, PagingMode, NULL, &MapCount);
-  if (MapCount != 0) {
+  MapCount2 = 0;
+  Status    = PageTableParse (*PageTable, PagingMode, NULL, &MapCount2);
+  if (MapCount2 != 0) {
     UT_ASSERT_EQUAL (Status, RETURN_BUFFER_TOO_SMALL);
 
     //
-    // Allocate memory for Maps
+    // Allocate memory for Map2
     // Note the memory is only used in this one Single MapEntry Test
     //
-    Map = AllocatePages (EFI_SIZE_TO_PAGES (MapCount * sizeof (IA32_MAP_ENTRY)));
-    ASSERT (Map != NULL);
-    Status = PageTableParse (*PageTable, PagingMode, Map, &MapCount);
+    Map2 = AllocatePages (EFI_SIZE_TO_PAGES (MapCount2 * sizeof (IA32_MAP_ENTRY)));
+    ASSERT (Map2 != NULL);
+    Status = PageTableParse (*PageTable, PagingMode, Map2, &MapCount2);
+  }
+
+  //
+  // Check if PageTable has been modified.
+  //
+  if (MapCount2 != MapCount) {
+    UT_ASSERT_EQUAL (IsModified, TRUE);
+  } else {
+    if (CompareMem (Map, Map2, MapCount2 * sizeof (IA32_MAP_ENTRY)) != 0) {
+      UT_ASSERT_EQUAL (IsModified, TRUE);
+    } else {
+      UT_ASSERT_EQUAL (IsModified, FALSE);
+    }
   }
 
@@ -691,9 +826,9 @@
   //
   KeyPointCount = 0;
-  GetKeyPointList (MapEntrys, Map, MapCount, NULL, &KeyPointCount);
+  GetKeyPointList (MapEntrys, Map2, MapCount2, NULL, &KeyPointCount);
   KeyPointBuffer = AllocatePages (EFI_SIZE_TO_PAGES (KeyPointCount * sizeof (UINT64)));
   ASSERT (KeyPointBuffer != NULL);
   NewKeyPointCount = 0;
-  GetKeyPointList (MapEntrys, Map, MapCount, KeyPointBuffer, &NewKeyPointCount);
+  GetKeyPointList (MapEntrys, Map2, MapCount2, KeyPointBuffer, &NewKeyPointCount);
 
   //
@@ -701,5 +836,5 @@
   //
   for (Index = 0; Index < NewKeyPointCount; Index++) {
-    if (!CompareEntrysforOnePoint (KeyPointBuffer[Index], MapEntrys, Map, MapCount, InitMap, InitMapCount)) {
+    if (!CompareEntrysforOnePoint (KeyPointBuffer[Index], MapEntrys, Map2, MapCount2, InitMap, InitMapCount)) {
       DEBUG ((DEBUG_INFO, "Error happens at below key point\n"));
       DEBUG ((DEBUG_INFO, "Index = %d KeyPointBuffer[Index] = 0x%lx\n", Index, KeyPointBuffer[Index]));
@@ -713,4 +848,8 @@
   if (MapCount != 0) {
     FreePages (Map, EFI_SIZE_TO_PAGES (MapCount * sizeof (IA32_MAP_ENTRY)));
+  }
+
+  if (MapCount2 != 0) {
+    FreePages (Map2, EFI_SIZE_TO_PAGES (MapCount2 * sizeof (IA32_MAP_ENTRY)));
   }
 
@@ -890,19 +1029,16 @@
   UT_ASSERT_TRUE ((Random32 (9, 10) >= 9) & (Random32 (9, 10) <= 10));
   UT_ASSERT_TRUE ((Random64 (9, 10) >= 9) & (Random64 (9, 10) <= 10));
-
-  mSupportedBit.Bits.Present              = 1;
-  mSupportedBit.Bits.ReadWrite            = 1;
-  mSupportedBit.Bits.UserSupervisor       = 1;
-  mSupportedBit.Bits.WriteThrough         = 1;
-  mSupportedBit.Bits.CacheDisabled        = 1;
-  mSupportedBit.Bits.Accessed             = 1;
-  mSupportedBit.Bits.Dirty                = 1;
-  mSupportedBit.Bits.Pat                  = 1;
-  mSupportedBit.Bits.Global               = 1;
-  mSupportedBit.Bits.Reserved1            = 0;
-  mSupportedBit.Bits.PageTableBaseAddress = 0;
-  mSupportedBit.Bits.Reserved2            = 0;
-  mSupportedBit.Bits.ProtectionKey        = 0xF;
-  mSupportedBit.Bits.Nx                   = 1;
+  mSupportedBit.Uint64              = 0;
+  mSupportedBit.Bits.Present        = 1;
+  mSupportedBit.Bits.ReadWrite      = 1;
+  mSupportedBit.Bits.UserSupervisor = 1;
+  mSupportedBit.Bits.WriteThrough   = 1;
+  mSupportedBit.Bits.CacheDisabled  = 1;
+  mSupportedBit.Bits.Accessed       = 1;
+  mSupportedBit.Bits.Dirty          = 1;
+  mSupportedBit.Bits.Pat            = 1;
+  mSupportedBit.Bits.Global         = 1;
+  mSupportedBit.Bits.ProtectionKey  = 0xF;
+  mSupportedBit.Bits.Nx             = 1;
 
   mRandomOption = ((CPU_PAGE_TABLE_LIB_RANDOM_TEST_CONTEXT *)Context)->RandomOption;
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/CpuPageTableLib/UnitTest/TestHelper.c
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/CpuPageTableLib/UnitTest/TestHelper.c	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/CpuPageTableLib/UnitTest/TestHelper.c	(revision 101291)
@@ -2,5 +2,5 @@
   helper file for Unit tests of the CpuPageTableLib instance of the CpuPageTableLib class
 
-  Copyright (c) 2022, Intel Corporation. All rights reserved.<BR>
+  Copyright (c) 2022 - 2023, Intel Corporation. All rights reserved.<BR>
   SPDX-License-Identifier: BSD-2-Clause-Patent
 
@@ -141,5 +141,5 @@
   }
 
-  ChildPageEntry = (IA32_PAGING_ENTRY  *)(UINTN)(((UINTN)(PagingEntry->Pnle.Bits.PageTableBaseAddress)) << 12);
+  ChildPageEntry = (IA32_PAGING_ENTRY  *)(UINTN)(IA32_PNLE_PAGE_TABLE_BASE_ADDRESS (&PagingEntry->Pnle));
   for (Index = 0; Index < 512; Index++) {
     Status = IsPageTableEntryValid (&ChildPageEntry[Index], Level-1, MaxLeafLevel, Address + (Index<<(9*(Level-1) + 3)));
@@ -172,8 +172,11 @@
   IA32_PAGING_ENTRY  *PagingEntry;
 
-  if ((PagingMode == Paging32bit) || (PagingMode == PagingPae) || (PagingMode >= PagingModeMax)) {
+  if (PageTable == 0) {
+    return UNIT_TEST_PASSED;
+  }
+
+  if ((PagingMode == Paging32bit) || (PagingMode >= PagingModeMax)) {
     //
     // 32bit paging is never supported.
-    // PAE paging will be supported later.
     //
     return UNIT_TEST_ERROR_TEST_FAILED;
@@ -184,5 +187,10 @@
 
   PagingEntry = (IA32_PAGING_ENTRY *)(UINTN)PageTable;
-  for (Index = 0; Index < 512; Index++) {
+  for (Index = 0; Index < ((PagingMode == PagingPae) ? 4 : 512); Index++) {
+    if (PagingMode == PagingPae) {
+      UT_ASSERT_EQUAL (PagingEntry[Index].PdptePae.Bits.MustBeZero, 0);
+      UT_ASSERT_EQUAL (PagingEntry[Index].PdptePae.Bits.MustBeZero2, 0);
+    }
+
     Status = IsPageTableEntryValid (&PagingEntry[Index], MaxLevel, MaxLeafLevel, Index << (9 * MaxLevel + 3));
     if (Status != UNIT_TEST_PASSED) {
@@ -230,5 +238,5 @@
   // Not a leaf
   //
-  ChildPageEntry = (IA32_PAGING_ENTRY  *)(UINTN)(((UINTN)(PagingEntry->Pnle.Bits.PageTableBaseAddress)) << 12);
+  ChildPageEntry = (IA32_PAGING_ENTRY  *)(UINTN)(IA32_PNLE_PAGE_TABLE_BASE_ADDRESS (&PagingEntry->Pnle));
   *Level         = *Level -1;
   Index          = Address >> (*Level * 9 + 3);
@@ -261,5 +269,5 @@
   IA32_PAGING_ENTRY  *PagingEntry;
 
-  if ((PagingMode == Paging32bit) || (PagingMode == PagingPae) || (PagingMode >= PagingModeMax)) {
+  if ((PagingMode == Paging32bit) || (PagingMode >= PagingModeMax)) {
     //
     // 32bit paging is never supported.
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MmSaveStateLib/AmdMmSaveState.c
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MmSaveStateLib/AmdMmSaveState.c	(revision 101291)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MmSaveStateLib/AmdMmSaveState.c	(revision 101291)
@@ -0,0 +1,309 @@
+/** @file
+Provides services to access SMRAM Save State Map
+
+Copyright (c) 2010 - 2019, Intel Corporation. All rights reserved.<BR>
+Copyright (C) 2023 Advanced Micro Devices, Inc. All rights reserved.<BR>
+SPDX-License-Identifier: BSD-2-Clause-Patent
+
+**/
+
+#include "MmSaveState.h"
+#include <Register/Amd/SmramSaveStateMap.h>
+#include <Library/BaseLib.h>
+
+// EFER register LMA bit
+#define LMA                                        BIT10
+#define EFER_ADDRESS                               0xC0000080ul
+#define AMD_MM_SAVE_STATE_REGISTER_SMMREVID_INDEX  1
+#define AMD_MM_SAVE_STATE_REGISTER_MAX_INDEX       2
+
+// Macro used to simplify the lookup table entries of type CPU_MM_SAVE_STATE_LOOKUP_ENTRY
+#define MM_CPU_OFFSET(Field)  OFFSET_OF (AMD_SMRAM_SAVE_STATE_MAP, Field)
+
+// Lookup table used to retrieve the widths and offsets associated with each
+// supported EFI_MM_SAVE_STATE_REGISTER value
+CONST CPU_MM_SAVE_STATE_LOOKUP_ENTRY  mCpuWidthOffset[] = {
+  { 0, 0, 0,                            0,                                    FALSE },                                        //  Reserved
+
+  //
+  // Internally defined CPU Save State Registers. Not defined in PI SMM CPU Protocol.
+  //
+  { 4, 4, MM_CPU_OFFSET (x86.SMMRevId), MM_CPU_OFFSET (x64.SMMRevId),         0, FALSE},                                      // AMD_MM_SAVE_STATE_REGISTER_SMMREVID_INDEX  = 1
+
+  //
+  // CPU Save State registers defined in PI SMM CPU Protocol.
+  //
+  { 4, 8, MM_CPU_OFFSET (x86.GDTBase),  MM_CPU_OFFSET (x64._GDTRBaseLoDword), MM_CPU_OFFSET (x64._GDTRBaseHiDword), FALSE},   //  EFI_MM_SAVE_STATE_REGISTER_GDTBASE  = 4
+  { 0, 8, 0,                            MM_CPU_OFFSET (x64._IDTRBaseLoDword), MM_CPU_OFFSET (x64._IDTRBaseLoDword), FALSE},   //  EFI_MM_SAVE_STATE_REGISTER_IDTBASE  = 5
+  { 0, 8, 0,                            MM_CPU_OFFSET (x64._LDTRBaseLoDword), MM_CPU_OFFSET (x64._LDTRBaseLoDword), FALSE},   //  EFI_MM_SAVE_STATE_REGISTER_LDTBASE  = 6
+  { 0, 2, 0,                            MM_CPU_OFFSET (x64._GDTRLimit),       0, FALSE},                                      //  EFI_MM_SAVE_STATE_REGISTER_GDTLIMIT = 7
+  { 0, 2, 0,                            MM_CPU_OFFSET (x64._IDTRLimit),       0, FALSE},                                      //  EFI_MM_SAVE_STATE_REGISTER_IDTLIMIT = 8
+  { 0, 4, 0,                            MM_CPU_OFFSET (x64._LDTRLimit),       0, FALSE},                                      //  EFI_MM_SAVE_STATE_REGISTER_LDTLIMIT = 9
+  { 0, 0, 0,                            0,                                    0, FALSE},                                      //  EFI_MM_SAVE_STATE_REGISTER_LDTINFO  = 10
+  { 4, 2, MM_CPU_OFFSET (x86._ES),      MM_CPU_OFFSET (x64._ES),              0, FALSE},                                      //  EFI_MM_SAVE_STATE_REGISTER_ES       = 20
+  { 4, 2, MM_CPU_OFFSET (x86._CS),      MM_CPU_OFFSET (x64._CS),              0, FALSE},                                      //  EFI_MM_SAVE_STATE_REGISTER_CS       = 21
+  { 4, 2, MM_CPU_OFFSET (x86._SS),      MM_CPU_OFFSET (x64._SS),              0, FALSE},                                      //  EFI_MM_SAVE_STATE_REGISTER_SS       = 22
+  { 4, 2, MM_CPU_OFFSET (x86._DS),      MM_CPU_OFFSET (x64._DS),              0, FALSE},                                      //  EFI_MM_SAVE_STATE_REGISTER_DS       = 23
+  { 4, 2, MM_CPU_OFFSET (x86._FS),      MM_CPU_OFFSET (x64._FS),              0, FALSE},                                      //  EFI_MM_SAVE_STATE_REGISTER_FS       = 24
+  { 4, 2, MM_CPU_OFFSET (x86._GS),      MM_CPU_OFFSET (x64._GS),              0, FALSE},                                      //  EFI_MM_SAVE_STATE_REGISTER_GS       = 25
+  { 0, 2, 0,                            MM_CPU_OFFSET (x64._LDTR),            0, FALSE},                                      //  EFI_MM_SAVE_STATE_REGISTER_LDTR_SEL = 26
+  { 0, 2, 0,                            MM_CPU_OFFSET (x64._TR),              0, FALSE},                                      //  EFI_MM_SAVE_STATE_REGISTER_TR_SEL   = 27
+  { 4, 8, MM_CPU_OFFSET (x86._DR7),     MM_CPU_OFFSET (x64._DR7),             MM_CPU_OFFSET (x64._DR7)         + 4, FALSE},   //  EFI_MM_SAVE_STATE_REGISTER_DR7      = 28
+  { 4, 8, MM_CPU_OFFSET (x86._DR6),     MM_CPU_OFFSET (x64._DR6),             MM_CPU_OFFSET (x64._DR6)         + 4, FALSE},   //  EFI_MM_SAVE_STATE_REGISTER_DR6      = 29
+  { 0, 8, 0,                            MM_CPU_OFFSET (x64._R8),              MM_CPU_OFFSET (x64._R8)          + 4, TRUE},    //  EFI_MM_SAVE_STATE_REGISTER_R8       = 30
+  { 0, 8, 0,                            MM_CPU_OFFSET (x64._R9),              MM_CPU_OFFSET (x64._R9)          + 4, TRUE},    //  EFI_MM_SAVE_STATE_REGISTER_R9       = 31
+  { 0, 8, 0,                            MM_CPU_OFFSET (x64._R10),             MM_CPU_OFFSET (x64._R10)         + 4, TRUE},    //  EFI_MM_SAVE_STATE_REGISTER_R10      = 32
+  { 0, 8, 0,                            MM_CPU_OFFSET (x64._R11),             MM_CPU_OFFSET (x64._R11)         + 4, TRUE},    //  EFI_MM_SAVE_STATE_REGISTER_R11      = 33
+  { 0, 8, 0,                            MM_CPU_OFFSET (x64._R12),             MM_CPU_OFFSET (x64._R12)         + 4, TRUE},    //  EFI_MM_SAVE_STATE_REGISTER_R12      = 34
+  { 0, 8, 0,                            MM_CPU_OFFSET (x64._R13),             MM_CPU_OFFSET (x64._R13)         + 4, TRUE},    //  EFI_MM_SAVE_STATE_REGISTER_R13      = 35
+  { 0, 8, 0,                            MM_CPU_OFFSET (x64._R14),             MM_CPU_OFFSET (x64._R14)         + 4, TRUE},    //  EFI_MM_SAVE_STATE_REGISTER_R14      = 36
+  { 0, 8, 0,                            MM_CPU_OFFSET (x64._R15),             MM_CPU_OFFSET (x64._R15)         + 4, TRUE},    //  EFI_MM_SAVE_STATE_REGISTER_R15      = 37
+  { 4, 8, MM_CPU_OFFSET (x86._EAX),     MM_CPU_OFFSET (x64._RAX),             MM_CPU_OFFSET (x64._RAX)         + 4, TRUE},    //  EFI_MM_SAVE_STATE_REGISTER_RAX      = 38
+  { 4, 8, MM_CPU_OFFSET (x86._EBX),     MM_CPU_OFFSET (x64._RBX),             MM_CPU_OFFSET (x64._RBX)         + 4, TRUE},    //  EFI_MM_SAVE_STATE_REGISTER_RBX      = 39
+  { 4, 8, MM_CPU_OFFSET (x86._ECX),     MM_CPU_OFFSET (x64._RCX),             MM_CPU_OFFSET (x64._RCX)         + 4, TRUE},    //  EFI_MM_SAVE_STATE_REGISTER_RBX      = 39
+  { 4, 8, MM_CPU_OFFSET (x86._EDX),     MM_CPU_OFFSET (x64._RDX),             MM_CPU_OFFSET (x64._RDX)         + 4, TRUE},    //  EFI_MM_SAVE_STATE_REGISTER_RDX      = 41
+  { 4, 8, MM_CPU_OFFSET (x86._ESP),     MM_CPU_OFFSET (x64._RSP),             MM_CPU_OFFSET (x64._RSP)         + 4, TRUE},    //  EFI_MM_SAVE_STATE_REGISTER_RSP      = 42
+  { 4, 8, MM_CPU_OFFSET (x86._EBP),     MM_CPU_OFFSET (x64._RBP),             MM_CPU_OFFSET (x64._RBP)         + 4, TRUE},    //  EFI_MM_SAVE_STATE_REGISTER_RBP      = 43
+  { 4, 8, MM_CPU_OFFSET (x86._ESI),     MM_CPU_OFFSET (x64._RSI),             MM_CPU_OFFSET (x64._RSI)         + 4, TRUE},    //  EFI_MM_SAVE_STATE_REGISTER_RSI      = 44
+  { 4, 8, MM_CPU_OFFSET (x86._EDI),     MM_CPU_OFFSET (x64._RDI),             MM_CPU_OFFSET (x64._RDI)         + 4, TRUE},    //  EFI_MM_SAVE_STATE_REGISTER_RDI      = 45
+  { 4, 8, MM_CPU_OFFSET (x86._EIP),     MM_CPU_OFFSET (x64._RIP),             MM_CPU_OFFSET (x64._RIP)         + 4, TRUE},    //  EFI_MM_SAVE_STATE_REGISTER_RIP      = 46
+
+  { 4, 8, MM_CPU_OFFSET (x86._EFLAGS),  MM_CPU_OFFSET (x64._RFLAGS),          MM_CPU_OFFSET (x64._RFLAGS)      + 4, TRUE},    //  EFI_MM_SAVE_STATE_REGISTER_RFLAGS   = 51
+  { 4, 8, MM_CPU_OFFSET (x86._CR0),     MM_CPU_OFFSET (x64._CR0),             MM_CPU_OFFSET (x64._CR0)         + 4, FALSE},   //  EFI_MM_SAVE_STATE_REGISTER_CR0      = 52
+  { 4, 8, MM_CPU_OFFSET (x86._CR3),     MM_CPU_OFFSET (x64._CR3),             MM_CPU_OFFSET (x64._CR3)         + 4, FALSE},   //  EFI_MM_SAVE_STATE_REGISTER_CR3      = 53
+  { 0, 8, 0,                            MM_CPU_OFFSET (x64._CR4),             MM_CPU_OFFSET (x64._CR4)         + 4, FALSE},   //  EFI_MM_SAVE_STATE_REGISTER_CR4      = 54
+  { 0, 0, 0,                            0,                                    0     }
+};
+
+/**
+  Read a save state register on the target processor.  If this function
+  returns EFI_UNSUPPORTED, then the caller is responsible for reading the
+  MM Save State register.
+
+  @param[in]  CpuIndex  The index of the CPU to read the Save State register.
+                        The value must be between 0 and the NumberOfCpus field in
+                        the System Management System Table (SMST).
+  @param[in]  Register  The MM Save State register to read.
+  @param[in]  Width     The number of bytes to read from the CPU save state.
+  @param[out] Buffer    Upon return, this holds the CPU register value read
+                        from the save state.
+
+  @retval EFI_SUCCESS           The register was read from Save State.
+  @retval EFI_INVALID_PARAMTER  Buffer is NULL.
+  @retval EFI_UNSUPPORTED       This function does not support reading Register.
+  @retval EFI_NOT_FOUND         If desired Register not found.
+**/
+EFI_STATUS
+EFIAPI
+MmSaveStateReadRegister (
+  IN  UINTN                       CpuIndex,
+  IN  EFI_MM_SAVE_STATE_REGISTER  Register,
+  IN  UINTN                       Width,
+  OUT VOID                        *Buffer
+  )
+{
+  UINT32                     SmmRevId;
+  EFI_MM_SAVE_STATE_IO_INFO  *IoInfo;
+  AMD_SMRAM_SAVE_STATE_MAP   *CpuSaveState;
+  UINT8                      DataWidth;
+
+  // Read CPU State
+  CpuSaveState = (AMD_SMRAM_SAVE_STATE_MAP *)gMmst->CpuSaveState[CpuIndex];
+
+  // Check for special EFI_MM_SAVE_STATE_REGISTER_LMA
+  if (Register == EFI_MM_SAVE_STATE_REGISTER_LMA) {
+    // Only byte access is supported for this register
+    if (Width != 1) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    *(UINT8 *)Buffer = MmSaveStateGetRegisterLma ();
+
+    return EFI_SUCCESS;
+  }
+
+  // Check for special EFI_MM_SAVE_STATE_REGISTER_IO
+  if (Register == EFI_MM_SAVE_STATE_REGISTER_IO) {
+    //
+    // Get SMM Revision ID
+    //
+    MmSaveStateReadRegisterByIndex (CpuIndex, AMD_MM_SAVE_STATE_REGISTER_SMMREVID_INDEX, sizeof (SmmRevId), &SmmRevId);
+
+    //
+    // See if the CPU supports the IOMisc register in the save state
+    //
+    if (SmmRevId < AMD_SMM_MIN_REV_ID_X64) {
+      return EFI_NOT_FOUND;
+    }
+
+    // Check if IO Restart Dword [IO Trap] is valid or not using bit 1.
+    if (!(CpuSaveState->x64.IO_DWord & 0x02u)) {
+      return EFI_NOT_FOUND;
+    }
+
+    // Zero the IoInfo structure that will be returned in Buffer
+    IoInfo = (EFI_MM_SAVE_STATE_IO_INFO *)Buffer;
+    ZeroMem (IoInfo, sizeof (EFI_MM_SAVE_STATE_IO_INFO));
+
+    IoInfo->IoPort = (UINT16)(CpuSaveState->x64.IO_DWord >> 16u);
+
+    if (CpuSaveState->x64.IO_DWord & 0x10u) {
+      IoInfo->IoWidth = EFI_MM_SAVE_STATE_IO_WIDTH_UINT8;
+      DataWidth       = 0x01u;
+    } else if (CpuSaveState->x64.IO_DWord & 0x20u) {
+      IoInfo->IoWidth = EFI_MM_SAVE_STATE_IO_WIDTH_UINT16;
+      DataWidth       = 0x02u;
+    } else {
+      IoInfo->IoWidth = EFI_MM_SAVE_STATE_IO_WIDTH_UINT32;
+      DataWidth       = 0x04u;
+    }
+
+    if (CpuSaveState->x64.IO_DWord & 0x01u) {
+      IoInfo->IoType = EFI_MM_SAVE_STATE_IO_TYPE_INPUT;
+    } else {
+      IoInfo->IoType = EFI_MM_SAVE_STATE_IO_TYPE_OUTPUT;
+    }
+
+    if ((IoInfo->IoType == EFI_MM_SAVE_STATE_IO_TYPE_INPUT) || (IoInfo->IoType == EFI_MM_SAVE_STATE_IO_TYPE_OUTPUT)) {
+      MmSaveStateReadRegister (CpuIndex, EFI_MM_SAVE_STATE_REGISTER_RAX, DataWidth, &IoInfo->IoData);
+    }
+
+    return EFI_SUCCESS;
+  }
+
+  // Convert Register to a register lookup table index
+  return MmSaveStateReadRegisterByIndex (CpuIndex, MmSaveStateGetRegisterIndex (Register, AMD_MM_SAVE_STATE_REGISTER_MAX_INDEX), Width, Buffer);
+}
+
+/**
+  Writes a save state register on the target processor.  If this function
+  returns EFI_UNSUPPORTED, then the caller is responsible for writing the
+  MM save state register.
+
+  @param[in] CpuIndex  The index of the CPU to write the MM Save State.  The
+                       value must be between 0 and the NumberOfCpus field in
+                       the System Management System Table (SMST).
+  @param[in] Register  The MM Save State register to write.
+  @param[in] Width     The number of bytes to write to the CPU save state.
+  @param[in] Buffer    Upon entry, this holds the new CPU register value.
+
+  @retval EFI_SUCCESS           The register was written to Save State.
+  @retval EFI_INVALID_PARAMTER  Buffer is NULL.
+  @retval EFI_UNSUPPORTED       This function does not support writing Register.
+  @retval EFI_NOT_FOUND         If desired Register not found.
+**/
+EFI_STATUS
+EFIAPI
+MmSaveStateWriteRegister (
+  IN UINTN                       CpuIndex,
+  IN EFI_MM_SAVE_STATE_REGISTER  Register,
+  IN UINTN                       Width,
+  IN CONST VOID                  *Buffer
+  )
+{
+  UINTN                     RegisterIndex;
+  AMD_SMRAM_SAVE_STATE_MAP  *CpuSaveState;
+
+  //
+  // Writes to EFI_MM_SAVE_STATE_REGISTER_LMA are ignored
+  //
+  if (Register == EFI_MM_SAVE_STATE_REGISTER_LMA) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Writes to EFI_MM_SAVE_STATE_REGISTER_IO are not supported
+  //
+  if (Register == EFI_MM_SAVE_STATE_REGISTER_IO) {
+    return EFI_NOT_FOUND;
+  }
+
+  //
+  // Convert Register to a register lookup table index
+  //
+  RegisterIndex = MmSaveStateGetRegisterIndex (Register, AMD_MM_SAVE_STATE_REGISTER_MAX_INDEX);
+  if (RegisterIndex == 0) {
+    return EFI_NOT_FOUND;
+  }
+
+  CpuSaveState = gMmst->CpuSaveState[CpuIndex];
+
+  //
+  // Do not write non-writable SaveState, because it will cause exception.
+  //
+  if (!mCpuWidthOffset[RegisterIndex].Writeable) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Check CPU mode
+  //
+  if (MmSaveStateGetRegisterLma () == EFI_MM_SAVE_STATE_REGISTER_LMA_32BIT) {
+    //
+    // If 32-bit mode width is zero, then the specified register can not be accessed
+    //
+    if (mCpuWidthOffset[RegisterIndex].Width32 == 0) {
+      return EFI_NOT_FOUND;
+    }
+
+    //
+    // If Width is bigger than the 32-bit mode width, then the specified register can not be accessed
+    //
+    if (Width > mCpuWidthOffset[RegisterIndex].Width32) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    //
+    // Write SMM State register
+    //
+    ASSERT (CpuSaveState != NULL);
+    CopyMem ((UINT8 *)CpuSaveState + mCpuWidthOffset[RegisterIndex].Offset32, Buffer, Width);
+  } else {
+    //
+    // If 64-bit mode width is zero, then the specified register can not be accessed
+    //
+    if (mCpuWidthOffset[RegisterIndex].Width64 == 0) {
+      return EFI_NOT_FOUND;
+    }
+
+    //
+    // If Width is bigger than the 64-bit mode width, then the specified register can not be accessed
+    //
+    if (Width > mCpuWidthOffset[RegisterIndex].Width64) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    //
+    // Write lower 32-bits of SMM State register
+    //
+    CopyMem ((UINT8 *)CpuSaveState + mCpuWidthOffset[RegisterIndex].Offset64Lo, Buffer, MIN (4, Width));
+    if (Width >= 4) {
+      //
+      // Write upper 32-bits of SMM State register
+      //
+      CopyMem ((UINT8 *)CpuSaveState + mCpuWidthOffset[RegisterIndex].Offset64Hi, (UINT8 *)Buffer + 4, Width - 4);
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Returns LMA value of the Processor.
+
+  @retval     UINT8     returns LMA bit value.
+**/
+UINT8
+MmSaveStateGetRegisterLma (
+  VOID
+  )
+{
+  UINT32  LMAValue;
+
+  LMAValue = (UINT32)AsmReadMsr64 (EFER_ADDRESS) & LMA;
+  if (LMAValue) {
+    return EFI_MM_SAVE_STATE_REGISTER_LMA_64BIT;
+  }
+
+  return EFI_MM_SAVE_STATE_REGISTER_LMA_32BIT;
+}
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MmSaveStateLib/AmdMmSaveStateLib.inf
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MmSaveStateLib/AmdMmSaveStateLib.inf	(revision 101291)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MmSaveStateLib/AmdMmSaveStateLib.inf	(revision 101291)
@@ -0,0 +1,34 @@
+## @file
+# MM Smram save state service lib.
+#
+# This is MM Smram save state service lib that provide service to read and
+# save savestate area registers.
+#
+# Copyright (C) 2023 Advanced Micro Devices, Inc. All rights reserved.<BR>
+#
+# SPDX-License-Identifier: BSD-2-Clause-Patent
+#
+##
+
+[Defines]
+  INF_VERSION                    = 1.29
+  BASE_NAME                      = AmdMmSaveStateLib
+  FILE_GUID                      = FB7D0A60-E8D4-4EFA-90AA-B357BC569879
+  MODULE_TYPE                    = DXE_SMM_DRIVER
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = MmSaveStateLib
+
+[Sources]
+  MmSaveState.h
+  MmSaveStateCommon.c
+  AmdMmSaveState.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  UefiCpuPkg/UefiCpuPkg.dec
+
+[LibraryClasses]
+  BaseLib
+  BaseMemoryLib
+  DebugLib
+  MmServicesTableLib
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MmSaveStateLib/IntelMmSaveState.c
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MmSaveStateLib/IntelMmSaveState.c	(revision 101291)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MmSaveStateLib/IntelMmSaveState.c	(revision 101291)
@@ -0,0 +1,410 @@
+/** @file
+Provides services to access SMRAM Save State Map
+
+Copyright (c) 2010 - 2019, Intel Corporation. All rights reserved.<BR>
+Copyright (C) 2023 Advanced Micro Devices, Inc. All rights reserved.<BR>
+SPDX-License-Identifier: BSD-2-Clause-Patent
+
+**/
+
+#include "MmSaveState.h"
+#include <Register/Intel/SmramSaveStateMap.h>
+#include <Register/Intel/Cpuid.h>
+#include <Library/BaseLib.h>
+
+#define INTEL_MM_SAVE_STATE_REGISTER_SMMREVID_INDEX   1
+#define INTEL_MM_SAVE_STATE_REGISTER_IOMISC_INDEX     2
+#define INTEL_MM_SAVE_STATE_REGISTER_IOMEMADDR_INDEX  3
+#define INTEL_MM_SAVE_STATE_REGISTER_MAX_INDEX        4
+
+///
+/// Macro used to simplify the lookup table entries of type CPU_MM_SAVE_STATE_LOOKUP_ENTRY
+///
+#define MM_CPU_OFFSET(Field)  OFFSET_OF (SMRAM_SAVE_STATE_MAP, Field)
+
+///
+/// Lookup table used to retrieve the widths and offsets associated with each
+/// supported EFI_MM_SAVE_STATE_REGISTER value
+///
+CONST CPU_MM_SAVE_STATE_LOOKUP_ENTRY  mCpuWidthOffset[] = {
+  { 0, 0, 0,                             0,                                  0,                                  FALSE }, //  Reserved
+
+  //
+  // Internally defined CPU Save State Registers. Not defined in PI SMM CPU Protocol.
+  //
+  { 4, 4, MM_CPU_OFFSET (x86.SMMRevId),  MM_CPU_OFFSET (x64.SMMRevId),       0,                                  FALSE }, // INTEL_MM_SAVE_STATE_REGISTER_SMMREVID_INDEX  = 1
+  { 4, 4, MM_CPU_OFFSET (x86.IOMisc),    MM_CPU_OFFSET (x64.IOMisc),         0,                                  FALSE }, // INTEL_MM_SAVE_STATE_REGISTER_IOMISC_INDEX    = 2
+  { 4, 8, MM_CPU_OFFSET (x86.IOMemAddr), MM_CPU_OFFSET (x64.IOMemAddr),      MM_CPU_OFFSET (x64.IOMemAddr) + 4,  FALSE }, // INTEL_MM_SAVE_STATE_REGISTER_IOMEMADDR_INDEX = 3
+
+  //
+  // CPU Save State registers defined in PI SMM CPU Protocol.
+  //
+  { 0, 8, 0,                             MM_CPU_OFFSET (x64.GdtBaseLoDword), MM_CPU_OFFSET (x64.GdtBaseHiDword), FALSE }, //  EFI_MM_SAVE_STATE_REGISTER_GDTBASE  = 4
+  { 0, 8, 0,                             MM_CPU_OFFSET (x64.IdtBaseLoDword), MM_CPU_OFFSET (x64.IdtBaseHiDword), FALSE }, //  EFI_MM_SAVE_STATE_REGISTER_IDTBASE  = 5
+  { 0, 8, 0,                             MM_CPU_OFFSET (x64.LdtBaseLoDword), MM_CPU_OFFSET (x64.LdtBaseHiDword), FALSE }, //  EFI_MM_SAVE_STATE_REGISTER_LDTBASE  = 6
+  { 0, 0, 0,                             0,                                  0,                                  FALSE }, //  EFI_MM_SAVE_STATE_REGISTER_GDTLIMIT = 7
+  { 0, 0, 0,                             0,                                  0,                                  FALSE }, //  EFI_MM_SAVE_STATE_REGISTER_IDTLIMIT = 8
+  { 0, 0, 0,                             0,                                  0,                                  FALSE }, //  EFI_MM_SAVE_STATE_REGISTER_LDTLIMIT = 9
+  { 0, 0, 0,                             0,                                  0,                                  FALSE }, //  EFI_MM_SAVE_STATE_REGISTER_LDTINFO  = 10
+
+  { 4, 4, MM_CPU_OFFSET (x86._ES),       MM_CPU_OFFSET (x64._ES),            0,                                  FALSE }, //  EFI_MM_SAVE_STATE_REGISTER_ES       = 20
+  { 4, 4, MM_CPU_OFFSET (x86._CS),       MM_CPU_OFFSET (x64._CS),            0,                                  FALSE }, //  EFI_MM_SAVE_STATE_REGISTER_CS       = 21
+  { 4, 4, MM_CPU_OFFSET (x86._SS),       MM_CPU_OFFSET (x64._SS),            0,                                  FALSE }, //  EFI_MM_SAVE_STATE_REGISTER_SS       = 22
+  { 4, 4, MM_CPU_OFFSET (x86._DS),       MM_CPU_OFFSET (x64._DS),            0,                                  FALSE }, //  EFI_MM_SAVE_STATE_REGISTER_DS       = 23
+  { 4, 4, MM_CPU_OFFSET (x86._FS),       MM_CPU_OFFSET (x64._FS),            0,                                  FALSE }, //  EFI_MM_SAVE_STATE_REGISTER_FS       = 24
+  { 4, 4, MM_CPU_OFFSET (x86._GS),       MM_CPU_OFFSET (x64._GS),            0,                                  FALSE }, //  EFI_MM_SAVE_STATE_REGISTER_GS       = 25
+  { 0, 4, 0,                             MM_CPU_OFFSET (x64._LDTR),          0,                                  FALSE }, //  EFI_MM_SAVE_STATE_REGISTER_LDTR_SEL = 26
+  { 4, 4, MM_CPU_OFFSET (x86._TR),       MM_CPU_OFFSET (x64._TR),            0,                                  FALSE }, //  EFI_MM_SAVE_STATE_REGISTER_TR_SEL   = 27
+  { 4, 8, MM_CPU_OFFSET (x86._DR7),      MM_CPU_OFFSET (x64._DR7),           MM_CPU_OFFSET (x64._DR7)    + 4,    FALSE }, //  EFI_MM_SAVE_STATE_REGISTER_DR7      = 28
+  { 4, 8, MM_CPU_OFFSET (x86._DR6),      MM_CPU_OFFSET (x64._DR6),           MM_CPU_OFFSET (x64._DR6)    + 4,    FALSE }, //  EFI_MM_SAVE_STATE_REGISTER_DR6      = 29
+  { 0, 8, 0,                             MM_CPU_OFFSET (x64._R8),            MM_CPU_OFFSET (x64._R8)     + 4,    TRUE  }, //  EFI_MM_SAVE_STATE_REGISTER_R8       = 30
+  { 0, 8, 0,                             MM_CPU_OFFSET (x64._R9),            MM_CPU_OFFSET (x64._R9)     + 4,    TRUE  }, //  EFI_MM_SAVE_STATE_REGISTER_R9       = 31
+  { 0, 8, 0,                             MM_CPU_OFFSET (x64._R10),           MM_CPU_OFFSET (x64._R10)    + 4,    TRUE  }, //  EFI_MM_SAVE_STATE_REGISTER_R10      = 32
+  { 0, 8, 0,                             MM_CPU_OFFSET (x64._R11),           MM_CPU_OFFSET (x64._R11)    + 4,    TRUE  }, //  EFI_MM_SAVE_STATE_REGISTER_R11      = 33
+  { 0, 8, 0,                             MM_CPU_OFFSET (x64._R12),           MM_CPU_OFFSET (x64._R12)    + 4,    TRUE  }, //  EFI_MM_SAVE_STATE_REGISTER_R12      = 34
+  { 0, 8, 0,                             MM_CPU_OFFSET (x64._R13),           MM_CPU_OFFSET (x64._R13)    + 4,    TRUE  }, //  EFI_MM_SAVE_STATE_REGISTER_R13      = 35
+  { 0, 8, 0,                             MM_CPU_OFFSET (x64._R14),           MM_CPU_OFFSET (x64._R14)    + 4,    TRUE  }, //  EFI_MM_SAVE_STATE_REGISTER_R14      = 36
+  { 0, 8, 0,                             MM_CPU_OFFSET (x64._R15),           MM_CPU_OFFSET (x64._R15)    + 4,    TRUE  }, //  EFI_MM_SAVE_STATE_REGISTER_R15      = 37
+  { 4, 8, MM_CPU_OFFSET (x86._EAX),      MM_CPU_OFFSET (x64._RAX),           MM_CPU_OFFSET (x64._RAX)    + 4,    TRUE  }, //  EFI_MM_SAVE_STATE_REGISTER_RAX      = 38
+  { 4, 8, MM_CPU_OFFSET (x86._EBX),      MM_CPU_OFFSET (x64._RBX),           MM_CPU_OFFSET (x64._RBX)    + 4,    TRUE  }, //  EFI_MM_SAVE_STATE_REGISTER_RBX      = 39
+  { 4, 8, MM_CPU_OFFSET (x86._ECX),      MM_CPU_OFFSET (x64._RCX),           MM_CPU_OFFSET (x64._RCX)    + 4,    TRUE  }, //  EFI_MM_SAVE_STATE_REGISTER_RCX      = 40
+  { 4, 8, MM_CPU_OFFSET (x86._EDX),      MM_CPU_OFFSET (x64._RDX),           MM_CPU_OFFSET (x64._RDX)    + 4,    TRUE  }, //  EFI_MM_SAVE_STATE_REGISTER_RDX      = 41
+  { 4, 8, MM_CPU_OFFSET (x86._ESP),      MM_CPU_OFFSET (x64._RSP),           MM_CPU_OFFSET (x64._RSP)    + 4,    TRUE  }, //  EFI_MM_SAVE_STATE_REGISTER_RSP      = 42
+  { 4, 8, MM_CPU_OFFSET (x86._EBP),      MM_CPU_OFFSET (x64._RBP),           MM_CPU_OFFSET (x64._RBP)    + 4,    TRUE  }, //  EFI_MM_SAVE_STATE_REGISTER_RBP      = 43
+  { 4, 8, MM_CPU_OFFSET (x86._ESI),      MM_CPU_OFFSET (x64._RSI),           MM_CPU_OFFSET (x64._RSI)    + 4,    TRUE  }, //  EFI_MM_SAVE_STATE_REGISTER_RSI      = 44
+  { 4, 8, MM_CPU_OFFSET (x86._EDI),      MM_CPU_OFFSET (x64._RDI),           MM_CPU_OFFSET (x64._RDI)    + 4,    TRUE  }, //  EFI_MM_SAVE_STATE_REGISTER_RDI      = 45
+  { 4, 8, MM_CPU_OFFSET (x86._EIP),      MM_CPU_OFFSET (x64._RIP),           MM_CPU_OFFSET (x64._RIP)    + 4,    TRUE  }, //  EFI_MM_SAVE_STATE_REGISTER_RIP      = 46
+
+  { 4, 8, MM_CPU_OFFSET (x86._EFLAGS),   MM_CPU_OFFSET (x64._RFLAGS),        MM_CPU_OFFSET (x64._RFLAGS) + 4,    TRUE  }, //  EFI_MM_SAVE_STATE_REGISTER_RFLAGS   = 51
+  { 4, 8, MM_CPU_OFFSET (x86._CR0),      MM_CPU_OFFSET (x64._CR0),           MM_CPU_OFFSET (x64._CR0)    + 4,    FALSE }, //  EFI_MM_SAVE_STATE_REGISTER_CR0      = 52
+  { 4, 8, MM_CPU_OFFSET (x86._CR3),      MM_CPU_OFFSET (x64._CR3),           MM_CPU_OFFSET (x64._CR3)    + 4,    FALSE }, //  EFI_MM_SAVE_STATE_REGISTER_CR3      = 53
+  { 0, 4, 0,                             MM_CPU_OFFSET (x64._CR4),           0,                                  FALSE }, //  EFI_MM_SAVE_STATE_REGISTER_CR4      = 54
+};
+
+///
+/// Structure used to build a lookup table for the IOMisc width information
+///
+typedef struct {
+  UINT8                         Width;
+  EFI_MM_SAVE_STATE_IO_WIDTH    IoWidth;
+} CPU_MM_SAVE_STATE_IO_WIDTH;
+
+///
+/// Lookup table for the IOMisc width information
+///
+STATIC CONST CPU_MM_SAVE_STATE_IO_WIDTH  mSmmCpuIoWidth[] = {
+  { 0, EFI_MM_SAVE_STATE_IO_WIDTH_UINT8  },  // Undefined           = 0
+  { 1, EFI_MM_SAVE_STATE_IO_WIDTH_UINT8  },  // SMM_IO_LENGTH_BYTE  = 1
+  { 2, EFI_MM_SAVE_STATE_IO_WIDTH_UINT16 },  // SMM_IO_LENGTH_WORD  = 2
+  { 0, EFI_MM_SAVE_STATE_IO_WIDTH_UINT8  },  // Undefined           = 3
+  { 4, EFI_MM_SAVE_STATE_IO_WIDTH_UINT32 },  // SMM_IO_LENGTH_DWORD = 4
+  { 0, EFI_MM_SAVE_STATE_IO_WIDTH_UINT8  },  // Undefined           = 5
+  { 0, EFI_MM_SAVE_STATE_IO_WIDTH_UINT8  },  // Undefined           = 6
+  { 0, EFI_MM_SAVE_STATE_IO_WIDTH_UINT8  }   // Undefined           = 7
+};
+
+///
+/// Lookup table for the IOMisc type information
+///
+STATIC CONST EFI_MM_SAVE_STATE_IO_TYPE  mSmmCpuIoType[] = {
+  EFI_MM_SAVE_STATE_IO_TYPE_OUTPUT,     // SMM_IO_TYPE_OUT_DX        = 0
+  EFI_MM_SAVE_STATE_IO_TYPE_INPUT,      // SMM_IO_TYPE_IN_DX         = 1
+  EFI_MM_SAVE_STATE_IO_TYPE_STRING,     // SMM_IO_TYPE_OUTS          = 2
+  EFI_MM_SAVE_STATE_IO_TYPE_STRING,     // SMM_IO_TYPE_INS           = 3
+  (EFI_MM_SAVE_STATE_IO_TYPE)0,         // Undefined                 = 4
+  (EFI_MM_SAVE_STATE_IO_TYPE)0,         // Undefined                 = 5
+  EFI_MM_SAVE_STATE_IO_TYPE_REP_PREFIX, // SMM_IO_TYPE_REP_OUTS      = 6
+  EFI_MM_SAVE_STATE_IO_TYPE_REP_PREFIX, // SMM_IO_TYPE_REP_INS       = 7
+  EFI_MM_SAVE_STATE_IO_TYPE_OUTPUT,     // SMM_IO_TYPE_OUT_IMMEDIATE = 8
+  EFI_MM_SAVE_STATE_IO_TYPE_INPUT,      // SMM_IO_TYPE_OUT_IMMEDIATE = 9
+  (EFI_MM_SAVE_STATE_IO_TYPE)0,         // Undefined                 = 10
+  (EFI_MM_SAVE_STATE_IO_TYPE)0,         // Undefined                 = 11
+  (EFI_MM_SAVE_STATE_IO_TYPE)0,         // Undefined                 = 12
+  (EFI_MM_SAVE_STATE_IO_TYPE)0,         // Undefined                 = 13
+  (EFI_MM_SAVE_STATE_IO_TYPE)0,         // Undefined                 = 14
+  (EFI_MM_SAVE_STATE_IO_TYPE)0          // Undefined                 = 15
+};
+
+/**
+  Read an SMM Save State register on the target processor.  If this function
+  returns EFI_UNSUPPORTED, then the caller is responsible for reading the
+  SMM Save Sate register.
+
+  @param[in]  CpuIndex  The index of the CPU to read the SMM Save State.  The
+                        value must be between 0 and the NumberOfCpus field in
+                        the System Management System Table (SMST).
+  @param[in]  Register  The SMM Save State register to read.
+  @param[in]  Width     The number of bytes to read from the CPU save state.
+  @param[out] Buffer    Upon return, this holds the CPU register value read
+                        from the save state.
+
+  @retval EFI_SUCCESS           The register was read from Save State.
+  @retval EFI_INVALID_PARAMTER  Buffer is NULL.
+  @retval EFI_UNSUPPORTED       This function does not support reading Register.
+  @retval EFI_NOT_FOUND         If desired Register not found.
+**/
+EFI_STATUS
+EFIAPI
+MmSaveStateReadRegister (
+  IN  UINTN                       CpuIndex,
+  IN  EFI_MM_SAVE_STATE_REGISTER  Register,
+  IN  UINTN                       Width,
+  OUT VOID                        *Buffer
+  )
+{
+  UINT32                     SmmRevId;
+  SMRAM_SAVE_STATE_IOMISC    IoMisc;
+  EFI_MM_SAVE_STATE_IO_INFO  *IoInfo;
+
+  //
+  // Check for special EFI_MM_SAVE_STATE_REGISTER_LMA
+  //
+  if (Register == EFI_MM_SAVE_STATE_REGISTER_LMA) {
+    //
+    // Only byte access is supported for this register
+    //
+    if (Width != 1) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    *(UINT8 *)Buffer = MmSaveStateGetRegisterLma ();
+
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Check for special EFI_MM_SAVE_STATE_REGISTER_IO
+  //
+  if (Register == EFI_MM_SAVE_STATE_REGISTER_IO) {
+    //
+    // Get SMM Revision ID
+    //
+    MmSaveStateReadRegisterByIndex (CpuIndex, INTEL_MM_SAVE_STATE_REGISTER_SMMREVID_INDEX, sizeof (SmmRevId), &SmmRevId);
+
+    //
+    // See if the CPU supports the IOMisc register in the save state
+    //
+    if (SmmRevId < SMRAM_SAVE_STATE_MIN_REV_ID_IOMISC) {
+      return EFI_NOT_FOUND;
+    }
+
+    //
+    // Get the IOMisc register value
+    //
+    MmSaveStateReadRegisterByIndex (CpuIndex, INTEL_MM_SAVE_STATE_REGISTER_IOMISC_INDEX, sizeof (IoMisc.Uint32), &IoMisc.Uint32);
+
+    //
+    // Check for the SMI_FLAG in IOMisc
+    //
+    if (IoMisc.Bits.SmiFlag == 0) {
+      return EFI_NOT_FOUND;
+    }
+
+    //
+    // Only support IN/OUT, but not INS/OUTS/REP INS/REP OUTS.
+    //
+    if ((mSmmCpuIoType[IoMisc.Bits.Type] != EFI_MM_SAVE_STATE_IO_TYPE_INPUT) &&
+        (mSmmCpuIoType[IoMisc.Bits.Type] != EFI_MM_SAVE_STATE_IO_TYPE_OUTPUT))
+    {
+      return EFI_NOT_FOUND;
+    }
+
+    //
+    // Compute index for the I/O Length and I/O Type lookup tables
+    //
+    if ((mSmmCpuIoWidth[IoMisc.Bits.Length].Width == 0) || (mSmmCpuIoType[IoMisc.Bits.Type] == 0)) {
+      return EFI_NOT_FOUND;
+    }
+
+    //
+    // Make sure the incoming buffer is large enough to hold IoInfo before accessing
+    //
+    if (Width < sizeof (EFI_MM_SAVE_STATE_IO_INFO)) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    //
+    // Zero the IoInfo structure that will be returned in Buffer
+    //
+    IoInfo = (EFI_MM_SAVE_STATE_IO_INFO *)Buffer;
+    ZeroMem (IoInfo, sizeof (EFI_MM_SAVE_STATE_IO_INFO));
+
+    //
+    // Use lookup tables to help fill in all the fields of the IoInfo structure
+    //
+    IoInfo->IoPort  = (UINT16)IoMisc.Bits.Port;
+    IoInfo->IoWidth = mSmmCpuIoWidth[IoMisc.Bits.Length].IoWidth;
+    IoInfo->IoType  = mSmmCpuIoType[IoMisc.Bits.Type];
+    MmSaveStateReadRegister (CpuIndex, EFI_MM_SAVE_STATE_REGISTER_RAX, mSmmCpuIoWidth[IoMisc.Bits.Length].Width, &IoInfo->IoData);
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Convert Register to a register lookup table index
+  //
+  return MmSaveStateReadRegisterByIndex (CpuIndex, MmSaveStateGetRegisterIndex (Register, INTEL_MM_SAVE_STATE_REGISTER_MAX_INDEX), Width, Buffer);
+}
+
+/**
+  Writes an SMM Save State register on the target processor.  If this function
+  returns EFI_UNSUPPORTED, then the caller is responsible for writing the
+  SMM Save Sate register.
+
+  @param[in] CpuIndex  The index of the CPU to write the SMM Save State.  The
+                       value must be between 0 and the NumberOfCpus field in
+                       the System Management System Table (SMST).
+  @param[in] Register  The SMM Save State register to write.
+  @param[in] Width     The number of bytes to write to the CPU save state.
+  @param[in] Buffer    Upon entry, this holds the new CPU register value.
+
+  @retval EFI_SUCCESS           The register was written to Save State.
+  @retval EFI_INVALID_PARAMTER  Buffer is NULL.
+  @retval EFI_UNSUPPORTED       This function does not support writing Register.
+  @retval EFI_NOT_FOUND         If desired Register not found.
+**/
+EFI_STATUS
+EFIAPI
+MmSaveStateWriteRegister (
+  IN UINTN                       CpuIndex,
+  IN EFI_MM_SAVE_STATE_REGISTER  Register,
+  IN UINTN                       Width,
+  IN CONST VOID                  *Buffer
+  )
+{
+  UINTN                 RegisterIndex;
+  SMRAM_SAVE_STATE_MAP  *CpuSaveState;
+
+  //
+  // Writes to EFI_MM_SAVE_STATE_REGISTER_LMA are ignored
+  //
+  if (Register == EFI_MM_SAVE_STATE_REGISTER_LMA) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Writes to EFI_MM_SAVE_STATE_REGISTER_IO are not supported
+  //
+  if (Register == EFI_MM_SAVE_STATE_REGISTER_IO) {
+    return EFI_NOT_FOUND;
+  }
+
+  //
+  // Convert Register to a register lookup table index
+  //
+  RegisterIndex = MmSaveStateGetRegisterIndex (Register, INTEL_MM_SAVE_STATE_REGISTER_MAX_INDEX);
+  if (RegisterIndex == 0) {
+    return EFI_NOT_FOUND;
+  }
+
+  CpuSaveState = gMmst->CpuSaveState[CpuIndex];
+
+  //
+  // Do not write non-writable SaveState, because it will cause exception.
+  //
+  if (!mCpuWidthOffset[RegisterIndex].Writeable) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Check CPU mode
+  //
+  if (MmSaveStateGetRegisterLma ()  == EFI_MM_SAVE_STATE_REGISTER_LMA_32BIT) {
+    //
+    // If 32-bit mode width is zero, then the specified register can not be accessed
+    //
+    if (mCpuWidthOffset[RegisterIndex].Width32 == 0) {
+      return EFI_NOT_FOUND;
+    }
+
+    //
+    // If Width is bigger than the 32-bit mode width, then the specified register can not be accessed
+    //
+    if (Width > mCpuWidthOffset[RegisterIndex].Width32) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    //
+    // Write SMM State register
+    //
+    ASSERT (CpuSaveState != NULL);
+    CopyMem ((UINT8 *)CpuSaveState + mCpuWidthOffset[RegisterIndex].Offset32, Buffer, Width);
+  } else {
+    //
+    // If 64-bit mode width is zero, then the specified register can not be accessed
+    //
+    if (mCpuWidthOffset[RegisterIndex].Width64 == 0) {
+      return EFI_NOT_FOUND;
+    }
+
+    //
+    // If Width is bigger than the 64-bit mode width, then the specified register can not be accessed
+    //
+    if (Width > mCpuWidthOffset[RegisterIndex].Width64) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    //
+    // Write at most 4 of the lower bytes of SMM State register
+    //
+    CopyMem ((UINT8 *)CpuSaveState + mCpuWidthOffset[RegisterIndex].Offset64Lo, Buffer, MIN (4, Width));
+    if (Width > 4) {
+      //
+      // Write at most 4 of the upper bytes of SMM State register
+      //
+      CopyMem ((UINT8 *)CpuSaveState + mCpuWidthOffset[RegisterIndex].Offset64Hi, (UINT8 *)Buffer + 4, Width - 4);
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Returns LMA value of the Processor.
+
+  @retval     UINT8     returns LMA bit value.
+**/
+UINT8
+MmSaveStateGetRegisterLma (
+  VOID
+  )
+{
+  UINT32  RegEax;
+  UINT32  RegEdx;
+  UINTN   FamilyId;
+  UINTN   ModelId;
+  UINT8   SmmSaveStateRegisterLma;
+
+  //
+  // Retrieve CPU Family
+  //
+  AsmCpuid (CPUID_VERSION_INFO, &RegEax, NULL, NULL, NULL);
+  FamilyId = (RegEax >> 8) & 0xf;
+  ModelId  = (RegEax >> 4) & 0xf;
+  if ((FamilyId == 0x06) || (FamilyId == 0x0f)) {
+    ModelId = ModelId | ((RegEax >> 12) & 0xf0);
+  }
+
+  RegEdx = 0;
+  AsmCpuid (CPUID_EXTENDED_FUNCTION, &RegEax, NULL, NULL, NULL);
+  if (RegEax >= CPUID_EXTENDED_CPU_SIG) {
+    AsmCpuid (CPUID_EXTENDED_CPU_SIG, NULL, NULL, NULL, &RegEdx);
+  }
+
+  //
+  // Determine the mode of the CPU at the time an SMI occurs
+  //   Intel(R) 64 and IA-32 Architectures Software Developer's Manual
+  //   Volume 3C, Section 34.4.1.1
+  //
+  SmmSaveStateRegisterLma = EFI_MM_SAVE_STATE_REGISTER_LMA_32BIT;
+  if ((RegEdx & BIT29) != 0) {
+    SmmSaveStateRegisterLma = EFI_MM_SAVE_STATE_REGISTER_LMA_64BIT;
+  }
+
+  if (FamilyId == 0x06) {
+    if ((ModelId == 0x17) || (ModelId == 0x0f) || (ModelId == 0x1c)) {
+      SmmSaveStateRegisterLma = EFI_MM_SAVE_STATE_REGISTER_LMA_64BIT;
+    }
+  }
+
+  return SmmSaveStateRegisterLma;
+}
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MmSaveStateLib/IntelMmSaveStateLib.inf
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MmSaveStateLib/IntelMmSaveStateLib.inf	(revision 101291)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MmSaveStateLib/IntelMmSaveStateLib.inf	(revision 101291)
@@ -0,0 +1,34 @@
+## @file
+# SMM Smram save state service lib.
+#
+# This is SMM Smram save state service lib that provide service to read and
+# save savestate area registers.
+#
+# Copyright (C) 2023 Advanced Micro Devices, Inc. All rights reserved.<BR>
+#
+# SPDX-License-Identifier: BSD-2-Clause-Patent
+#
+##
+
+[Defines]
+  INF_VERSION                    = 1.29
+  BASE_NAME                      = IntelMmSaveStateLib
+  FILE_GUID                      = 37E8137B-9F74-4250-8951-7A970A3C39C0
+  MODULE_TYPE                    = DXE_SMM_DRIVER
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = MmSaveStateLib
+
+[Sources]
+  MmSaveState.h
+  MmSaveStateCommon.c
+  IntelMmSaveState.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  UefiCpuPkg/UefiCpuPkg.dec
+
+[LibraryClasses]
+  BaseLib
+  BaseMemoryLib
+  DebugLib
+  MmServicesTableLib
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MmSaveStateLib/MmSaveState.h
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MmSaveStateLib/MmSaveState.h	(revision 101291)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MmSaveStateLib/MmSaveState.h	(revision 101291)
@@ -0,0 +1,94 @@
+/** @file
+  SMRAM Save State Map header file.
+
+  Copyright (c) 2010 - 2019, Intel Corporation. All rights reserved.<BR>
+  Copyright (C) 2023 Advanced Micro Devices, Inc. All rights reserved.<BR>
+  SPDX-License-Identifier: BSD-2-Clause-Patent
+
+**/
+
+#ifndef MM_SAVESTATE_H_
+#define MM_SAVESTATE_H_
+
+#include <Uefi/UefiBaseType.h>
+#include <Protocol/MmCpu.h>
+#include <Library/DebugLib.h>
+#include <Library/MmSaveStateLib.h>
+#include <Library/MmServicesTableLib.h>
+#include <Library/BaseMemoryLib.h>
+
+// Macro used to simplify the lookup table entries of type CPU_MM_SAVE_STATE_REGISTER_RANGE
+#define MM_REGISTER_RANGE(Start, End)  { Start, End, End - Start + 1 }
+
+// Structure used to describe a range of registers
+typedef struct {
+  EFI_MM_SAVE_STATE_REGISTER    Start;
+  EFI_MM_SAVE_STATE_REGISTER    End;
+  UINTN                         Length;
+} CPU_MM_SAVE_STATE_REGISTER_RANGE;
+
+// Structure used to build a lookup table to retrieve the widths and offsets
+// associated with each supported EFI_MM_SAVE_STATE_REGISTER value
+
+typedef struct {
+  UINT8      Width32;
+  UINT8      Width64;
+  UINT16     Offset32;
+  UINT16     Offset64Lo;
+  UINT16     Offset64Hi;
+  BOOLEAN    Writeable;
+} CPU_MM_SAVE_STATE_LOOKUP_ENTRY;
+
+/**
+  Returns LMA value of the Processor.
+
+  @retval     UINT8     returns LMA bit value.
+**/
+UINT8
+MmSaveStateGetRegisterLma (
+  VOID
+  );
+
+/**
+  Read information from the CPU save state.
+
+  @param  Register  Specifies the CPU register to read form the save state.
+  @param  RegOffset Offset for the next register index.
+
+  @retval 0   Register is not valid
+  @retval >0  Index into mCpuWidthOffset[] associated with Register
+
+**/
+UINTN
+MmSaveStateGetRegisterIndex (
+  IN EFI_MM_SAVE_STATE_REGISTER  Register,
+  IN UINTN                       RegOffset
+  );
+
+/**
+  Read a CPU Save State register on the target processor.
+
+  This function abstracts the differences that whether the CPU Save State register is in the
+  IA32 CPU Save State Map or X64 CPU Save State Map.
+
+  This function supports reading a CPU Save State register in SMBase relocation handler.
+
+  @param[in]  CpuIndex       Specifies the zero-based index of the CPU save state.
+  @param[in]  RegisterIndex  Index into mCpuWidthOffset[] look up table.
+  @param[in]  Width          The number of bytes to read from the CPU save state.
+  @param[out] Buffer         Upon return, this holds the CPU register value read from the save state.
+
+  @retval EFI_SUCCESS           The register was read from Save State.
+  @retval EFI_NOT_FOUND         The register is not defined for the Save State of Processor.
+  @retval EFI_INVALID_PARAMTER  This or Buffer is NULL.
+
+**/
+EFI_STATUS
+MmSaveStateReadRegisterByIndex (
+  IN UINTN  CpuIndex,
+  IN UINTN  RegisterIndex,
+  IN UINTN  Width,
+  OUT VOID  *Buffer
+  );
+
+#endif
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MmSaveStateLib/MmSaveStateCommon.c
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MmSaveStateLib/MmSaveStateCommon.c	(revision 101291)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MmSaveStateLib/MmSaveStateCommon.c	(revision 101291)
@@ -0,0 +1,132 @@
+/** @file
+  Provides common supporting function to access SMRAM Save State Map
+
+  Copyright (c) 2010 - 2019, Intel Corporation. All rights reserved.<BR>
+  Copyright (C) 2023 Advanced Micro Devices, Inc. All rights reserved.<BR>
+
+  SPDX-License-Identifier: BSD-2-Clause-Patent
+
+**/
+
+#include "MmSaveState.h"
+
+// Table used by MmSaveStateGetRegisterIndex() to convert an EFI_MM_SAVE_STATE_REGISTER
+// value to an index into a table of type CPU_MM_SAVE_STATE_LOOKUP_ENTRY
+CONST CPU_MM_SAVE_STATE_REGISTER_RANGE  mCpuRegisterRanges[] = {
+  MM_REGISTER_RANGE (EFI_MM_SAVE_STATE_REGISTER_GDTBASE, EFI_MM_SAVE_STATE_REGISTER_LDTINFO),
+  MM_REGISTER_RANGE (EFI_MM_SAVE_STATE_REGISTER_ES,      EFI_MM_SAVE_STATE_REGISTER_RIP),
+  MM_REGISTER_RANGE (EFI_MM_SAVE_STATE_REGISTER_RFLAGS,  EFI_MM_SAVE_STATE_REGISTER_CR4),
+  { (EFI_MM_SAVE_STATE_REGISTER)0,                       (EFI_MM_SAVE_STATE_REGISTER)0,      0}
+};
+
+extern CONST CPU_MM_SAVE_STATE_LOOKUP_ENTRY  mCpuWidthOffset[];
+
+/**
+  Read information from the CPU save state.
+
+  @param  Register  Specifies the CPU register to read form the save state.
+  @param  RegOffset Offset for the next register index.
+
+  @retval 0   Register is not valid
+  @retval >0  Index into mCpuWidthOffset[] associated with Register
+
+**/
+UINTN
+MmSaveStateGetRegisterIndex (
+  IN EFI_MM_SAVE_STATE_REGISTER  Register,
+  IN UINTN                       RegOffset
+  )
+{
+  UINTN  Index;
+  UINTN  Offset;
+
+  for (Index = 0, Offset = RegOffset; mCpuRegisterRanges[Index].Length != 0; Index++) {
+    if ((Register >= mCpuRegisterRanges[Index].Start) && (Register <= mCpuRegisterRanges[Index].End)) {
+      return Register - mCpuRegisterRanges[Index].Start + Offset;
+    }
+
+    Offset += mCpuRegisterRanges[Index].Length;
+  }
+
+  return 0;
+}
+
+/**
+  Read a CPU Save State register on the target processor.
+
+  This function abstracts the differences that whether the CPU Save State register is in the
+  IA32 CPU Save State Map or X64 CPU Save State Map.
+
+  This function supports reading a CPU Save State register in SMBase relocation handler.
+
+  @param[in]  CpuIndex       Specifies the zero-based index of the CPU save state.
+  @param[in]  RegisterIndex  Index into mCpuWidthOffset[] look up table.
+  @param[in]  Width          The number of bytes to read from the CPU save state.
+  @param[out] Buffer         Upon return, this holds the CPU register value read from the save state.
+
+  @retval EFI_SUCCESS           The register was read from Save State.
+  @retval EFI_NOT_FOUND         The register is not defined for the Save State of Processor.
+  @retval EFI_INVALID_PARAMTER  This or Buffer is NULL.
+
+**/
+EFI_STATUS
+MmSaveStateReadRegisterByIndex (
+  IN UINTN  CpuIndex,
+  IN UINTN  RegisterIndex,
+  IN UINTN  Width,
+  OUT VOID  *Buffer
+  )
+{
+  if (RegisterIndex == 0) {
+    return EFI_NOT_FOUND;
+  }
+
+  if (MmSaveStateGetRegisterLma () == EFI_MM_SAVE_STATE_REGISTER_LMA_32BIT) {
+    //
+    // If 32-bit mode width is zero, then the specified register can not be accessed
+    //
+    if (mCpuWidthOffset[RegisterIndex].Width32 == 0) {
+      return EFI_NOT_FOUND;
+    }
+
+    //
+    // If Width is bigger than the 32-bit mode width, then the specified register can not be accessed
+    //
+    if (Width > mCpuWidthOffset[RegisterIndex].Width32) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    //
+    // Write return buffer
+    //
+    ASSERT (gMmst->CpuSaveState[CpuIndex] != NULL);
+    CopyMem (Buffer, (UINT8 *)gMmst->CpuSaveState[CpuIndex] + mCpuWidthOffset[RegisterIndex].Offset32, Width);
+  } else {
+    //
+    // If 64-bit mode width is zero, then the specified register can not be accessed
+    //
+    if (mCpuWidthOffset[RegisterIndex].Width64 == 0) {
+      return EFI_NOT_FOUND;
+    }
+
+    //
+    // If Width is bigger than the 64-bit mode width, then the specified register can not be accessed
+    //
+    if (Width > mCpuWidthOffset[RegisterIndex].Width64) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    //
+    // Write lower 32-bits of return buffer
+    //
+    CopyMem (Buffer, (UINT8 *)gMmst->CpuSaveState[CpuIndex] + mCpuWidthOffset[RegisterIndex].Offset64Lo, MIN (4, Width));
+    if (Width > 4) {
+      //
+      // Write upper 32-bits of return buffer
+      //
+      CopyMem ((UINT8 *)Buffer + 4, (UINT8 *)gMmst->CpuSaveState[CpuIndex] + mCpuWidthOffset[RegisterIndex].Offset64Hi, Width - 4);
+    }
+  }
+
+  return EFI_SUCCESS;
+}
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MpInitLib/AmdSev.c
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MpInitLib/AmdSev.c	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MpInitLib/AmdSev.c	(revision 101291)
@@ -261,2 +261,19 @@
   }
 }
+
+/**
+  Get pointer to CPU MP Data structure from GUIDed HOB.
+
+  @param[in] CpuMpData  The pointer to CPU MP Data structure.
+**/
+VOID
+AmdSevUpdateCpuMpData (
+  IN CPU_MP_DATA  *CpuMpData
+  )
+{
+  CPU_MP_DATA  *OldCpuMpData;
+
+  OldCpuMpData = GetCpuMpDataFromGuidedHob ();
+
+  OldCpuMpData->NewCpuMpData = CpuMpData;
+}
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MpInitLib/DxeMpInitLib.inf
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MpInitLib/DxeMpInitLib.inf	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MpInitLib/DxeMpInitLib.inf	(revision 101291)
@@ -2,5 +2,5 @@
 #  MP Initialize Library instance for DXE driver.
 #
-#  Copyright (c) 2016 - 2021, Intel Corporation. All rights reserved.<BR>
+#  Copyright (c) 2016 - 2023, Intel Corporation. All rights reserved.<BR>
 #  SPDX-License-Identifier: BSD-2-Clause-Patent
 #
@@ -25,8 +25,10 @@
   Ia32/AmdSev.c
   Ia32/MpFuncs.nasm
+  Ia32/CreatePageTable.c
 
 [Sources.X64]
   X64/AmdSev.c
   X64/MpFuncs.nasm
+  X64/CreatePageTable.c
 
 [Sources.common]
@@ -37,4 +39,5 @@
   MpLib.h
   Microcode.c
+  MpHandOff.h
 
 [Packages]
@@ -50,5 +53,4 @@
   MtrrLib
   CpuLib
-  UefiCpuLib
   UefiBootServicesTableLib
   DebugAgentLib
@@ -57,4 +59,6 @@
   CcExitLib
   MicrocodeLib
+[LibraryClasses.X64]
+  CpuPageTableLib
 
 [Protocols]
@@ -78,4 +82,5 @@
   gUefiCpuPkgTokenSpaceGuid.PcdGhcbHypervisorFeatures                  ## CONSUMES
   gUefiCpuPkgTokenSpaceGuid.PcdSevEsWorkAreaBase                       ## SOMETIMES_CONSUMES
+  gUefiCpuPkgTokenSpaceGuid.PcdFirstTimeWakeUpAPsBySipi                ## CONSUMES
   gEfiMdeModulePkgTokenSpaceGuid.PcdCpuStackGuard                      ## CONSUMES
   gEfiMdeModulePkgTokenSpaceGuid.PcdGhcbBase                           ## CONSUMES
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MpInitLib/DxeMpLib.c
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MpInitLib/DxeMpLib.c	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MpInitLib/DxeMpLib.c	(revision 101291)
@@ -2,5 +2,5 @@
   MP initialize support functions for DXE phase.
 
-  Copyright (c) 2016 - 2020, Intel Corporation. All rights reserved.<BR>
+  Copyright (c) 2016 - 2023, Intel Corporation. All rights reserved.<BR>
   SPDX-License-Identifier: BSD-2-Clause-Patent
 
@@ -21,12 +21,13 @@
 #define  AP_SAFE_STACK_SIZE  128
 
-CPU_MP_DATA       *mCpuMpData                  = NULL;
-EFI_EVENT         mCheckAllApsEvent            = NULL;
-EFI_EVENT         mMpInitExitBootServicesEvent = NULL;
-EFI_EVENT         mLegacyBootEvent             = NULL;
-volatile BOOLEAN  mStopCheckAllApsStatus       = TRUE;
-VOID              *mReservedApLoopFunc         = NULL;
-UINTN             mReservedTopOfApStack;
-volatile UINT32   mNumberToFinish = 0;
+CPU_MP_DATA             *mCpuMpData                  = NULL;
+EFI_EVENT               mCheckAllApsEvent            = NULL;
+EFI_EVENT               mMpInitExitBootServicesEvent = NULL;
+EFI_EVENT               mLegacyBootEvent             = NULL;
+volatile BOOLEAN        mStopCheckAllApsStatus       = TRUE;
+RELOCATE_AP_LOOP_ENTRY  mReservedApLoop;
+UINTN                   mReservedTopOfApStack;
+volatile UINT32         mNumberToFinish = 0;
+UINTN                   mApPageTable;
 
 //
@@ -379,9 +380,8 @@
   )
 {
-  CPU_MP_DATA           *CpuMpData;
-  BOOLEAN               MwaitSupport;
-  ASM_RELOCATE_AP_LOOP  AsmRelocateApLoopFunc;
-  UINTN                 ProcessorNumber;
-  UINTN                 StackStart;
+  CPU_MP_DATA  *CpuMpData;
+  BOOLEAN      MwaitSupport;
+  UINTN        ProcessorNumber;
+  UINTN        StackStart;
 
   MpInitLibWhoAmI (&ProcessorNumber);
@@ -389,20 +389,32 @@
   MwaitSupport = IsMwaitSupport ();
   if (CpuMpData->UseSevEsAPMethod) {
+    //
+    // 64-bit AMD processors with SEV-ES
+    //
     StackStart = CpuMpData->SevEsAPResetStackStart;
+    mReservedApLoop.AmdSevEntry (
+                      MwaitSupport,
+                      CpuMpData->ApTargetCState,
+                      CpuMpData->PmCodeSegment,
+                      StackStart - ProcessorNumber * AP_SAFE_STACK_SIZE,
+                      (UINTN)&mNumberToFinish,
+                      CpuMpData->Pm16CodeSegment,
+                      CpuMpData->SevEsAPBuffer,
+                      CpuMpData->WakeupBuffer
+                      );
   } else {
+    //
+    // Intel processors (32-bit or 64-bit), 32-bit AMD processors, or 64-bit AMD processors without SEV-ES
+    //
     StackStart = mReservedTopOfApStack;
-  }
-
-  AsmRelocateApLoopFunc = (ASM_RELOCATE_AP_LOOP)(UINTN)mReservedApLoopFunc;
-  AsmRelocateApLoopFunc (
-    MwaitSupport,
-    CpuMpData->ApTargetCState,
-    CpuMpData->PmCodeSegment,
-    StackStart - ProcessorNumber * AP_SAFE_STACK_SIZE,
-    (UINTN)&mNumberToFinish,
-    CpuMpData->Pm16CodeSegment,
-    CpuMpData->SevEsAPBuffer,
-    CpuMpData->WakeupBuffer
-    );
+    mReservedApLoop.GenericEntry (
+                      MwaitSupport,
+                      CpuMpData->ApTargetCState,
+                      StackStart - ProcessorNumber * AP_SAFE_STACK_SIZE,
+                      (UINTN)&mNumberToFinish,
+                      mApPageTable
+                      );
+  }
+
   //
   // It should never reach here
@@ -452,5 +464,5 @@
   }
 
-  DEBUG ((DEBUG_INFO, "%a() done!\n", __FUNCTION__));
+  DEBUG ((DEBUG_INFO, "%a() done!\n", __func__));
 }
 
@@ -467,9 +479,13 @@
   EFI_STATUS                       Status;
   EFI_PHYSICAL_ADDRESS             Address;
-  UINTN                            ApSafeBufferSize;
   UINTN                            Index;
   EFI_GCD_MEMORY_SPACE_DESCRIPTOR  MemDesc;
   UINTN                            StackBase;
   CPU_INFO_IN_HOB                  *CpuInfoInHob;
+  MP_ASSEMBLY_ADDRESS_MAP          *AddressMap;
+  UINT8                            *ApLoopFunc;
+  UINTN                            ApLoopFuncSize;
+  UINTN                            StackPages;
+  UINTN                            FuncPages;
 
   SaveCpuMpData (CpuMpData);
@@ -526,4 +542,21 @@
   }
 
+  AddressMap = &CpuMpData->AddressMap;
+  if (CpuMpData->UseSevEsAPMethod) {
+    //
+    // 64-bit AMD processors with SEV-ES
+    //
+    Address        = BASE_4GB - 1;
+    ApLoopFunc     = AddressMap->RelocateApLoopFuncAddressAmdSev;
+    ApLoopFuncSize = AddressMap->RelocateApLoopFuncSizeAmdSev;
+  } else {
+    //
+    // Intel processors (32-bit or 64-bit), 32-bit AMD processors, or 64-bit AMD processors without SEV-ES
+    //
+    Address        = MAX_ADDRESS;
+    ApLoopFunc     = AddressMap->RelocateApLoopFuncAddressGeneric;
+    ApLoopFuncSize = AddressMap->RelocateApLoopFuncSizeGeneric;
+  }
+
   //
   // Avoid APs access invalid buffer data which allocated by BootServices,
@@ -534,20 +567,23 @@
   // Exit Boot Services callback function.
   //
-  ApSafeBufferSize = EFI_PAGES_TO_SIZE (
-                       EFI_SIZE_TO_PAGES (
-                         CpuMpData->AddressMap.RelocateApLoopFuncSize
-                         )
-                       );
-  Address = BASE_4GB - 1;
-  Status  = gBS->AllocatePages (
-                   AllocateMaxAddress,
-                   EfiReservedMemoryType,
-                   EFI_SIZE_TO_PAGES (ApSafeBufferSize),
-                   &Address
-                   );
+  // +------------+ (TopOfApStack)
+  // |  Stack * N |
+  // +------------+ (stack base, 4k aligned)
+  // |  Padding   |
+  // +------------+
+  // |  Ap Loop   |
+  // +------------+ ((low address, 4k-aligned)
+  //
+
+  StackPages = EFI_SIZE_TO_PAGES (CpuMpData->CpuCount * AP_SAFE_STACK_SIZE);
+  FuncPages  = EFI_SIZE_TO_PAGES (ApLoopFuncSize);
+
+  Status = gBS->AllocatePages (
+                  AllocateMaxAddress,
+                  EfiReservedMemoryType,
+                  StackPages + FuncPages,
+                  &Address
+                  );
   ASSERT_EFI_ERROR (Status);
-
-  mReservedApLoopFunc = (VOID *)(UINTN)Address;
-  ASSERT (mReservedApLoopFunc != NULL);
 
   //
@@ -562,30 +598,23 @@
     gDS->SetMemorySpaceAttributes (
            Address,
-           ApSafeBufferSize,
+           EFI_PAGES_TO_SIZE (FuncPages),
            MemDesc.Attributes & (~EFI_MEMORY_XP)
            );
   }
 
-  ApSafeBufferSize = EFI_PAGES_TO_SIZE (
-                       EFI_SIZE_TO_PAGES (
-                         CpuMpData->CpuCount * AP_SAFE_STACK_SIZE
-                         )
-                       );
-  Address = BASE_4GB - 1;
-  Status  = gBS->AllocatePages (
-                   AllocateMaxAddress,
-                   EfiReservedMemoryType,
-                   EFI_SIZE_TO_PAGES (ApSafeBufferSize),
-                   &Address
-                   );
-  ASSERT_EFI_ERROR (Status);
-
-  mReservedTopOfApStack = (UINTN)Address + ApSafeBufferSize;
+  mReservedTopOfApStack = (UINTN)Address + EFI_PAGES_TO_SIZE (StackPages+FuncPages);
   ASSERT ((mReservedTopOfApStack & (UINTN)(CPU_STACK_ALIGNMENT - 1)) == 0);
-  CopyMem (
-    mReservedApLoopFunc,
-    CpuMpData->AddressMap.RelocateApLoopFuncAddress,
-    CpuMpData->AddressMap.RelocateApLoopFuncSize
-    );
+  mReservedApLoop.Data = (VOID *)(UINTN)Address;
+  ASSERT (mReservedApLoop.Data != NULL);
+  CopyMem (mReservedApLoop.Data, ApLoopFunc, ApLoopFuncSize);
+  if (!CpuMpData->UseSevEsAPMethod) {
+    //
+    // processors without SEV-ES
+    //
+    mApPageTable = CreatePageTable (
+                     (UINTN)Address,
+                     EFI_PAGES_TO_SIZE (StackPages+FuncPages)
+                     );
+  }
 
   Status = gBS->CreateEvent (
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MpInitLib/Ia32/CreatePageTable.c
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MpInitLib/Ia32/CreatePageTable.c	(revision 101291)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MpInitLib/Ia32/CreatePageTable.c	(revision 101291)
@@ -0,0 +1,23 @@
+/** @file
+  Function to create page talbe.
+  Only create page table for x64, and leave the CreatePageTable empty for Ia32.
+  Copyright (c) 2023, Intel Corporation. All rights reserved.<BR>
+  SPDX-License-Identifier: BSD-2-Clause-Patent
+**/
+
+#include <Base.h>
+
+/**
+  Only create page table for x64, and leave the CreatePageTable empty for Ia32.
+  @param[in]      LinearAddress  The start of the linear address range.
+  @param[in]      Length         The length of the linear address range.
+  @return The page table to be created.
+**/
+UINTN
+CreatePageTable (
+  IN UINTN  Address,
+  IN UINTN  Length
+  )
+{
+  return 0;
+}
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MpInitLib/Ia32/MpFuncs.nasm
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MpInitLib/Ia32/MpFuncs.nasm	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MpInitLib/Ia32/MpFuncs.nasm	(revision 101291)
@@ -1,4 +1,4 @@
 ;------------------------------------------------------------------------------ ;
-; Copyright (c) 2015 - 2022, Intel Corporation. All rights reserved.<BR>
+; Copyright (c) 2015 - 2023, Intel Corporation. All rights reserved.<BR>
 ; SPDX-License-Identifier: BSD-2-Clause-Patent
 ;
@@ -198,6 +198,6 @@
     push       ebx               ; Push ApIndex
     mov        eax, esi
-    add        eax, MP_CPU_EXCHANGE_INFO_OFFSET
-    push       eax               ; push address of exchange info data buffer
+    add        eax, MP_CPU_EXCHANGE_INFO_FIELD (CpuMpData)
+    push       dword [eax]       ; push address of CpuMpData
 
     mov        edi, esi
@@ -220,12 +220,12 @@
 
 ;-------------------------------------------------------------------------------------
-;  AsmRelocateApLoop (MwaitSupport, ApTargetCState, PmCodeSegment, TopOfApStack, CountTofinish, Pm16CodeSegment, SevEsAPJumpTable, WakeupBuffer);
+;  AsmRelocateApLoopGeneric (MwaitSupport, ApTargetCState, PmCodeSegment, TopOfApStack, CountTofinish, Pm16CodeSegment, SevEsAPJumpTable, WakeupBuffer);
 ;
 ;  The last three parameters (Pm16CodeSegment, SevEsAPJumpTable and WakeupBuffer) are
 ;  specific to SEV-ES support and are not applicable on IA32.
 ;-------------------------------------------------------------------------------------
-AsmRelocateApLoopStart:
+AsmRelocateApLoopGenericStart:
     mov        eax, esp
-    mov        esp, [eax + 16]     ; TopOfApStack
+    mov        esp, [eax + 12]     ; TopOfApStack
     push       dword [eax]         ; push return address for stack trace
     push       ebp
@@ -233,9 +233,9 @@
     mov        ebx, [eax + 8]      ; ApTargetCState
     mov        ecx, [eax + 4]      ; MwaitSupport
-    mov        eax, [eax + 20]     ; CountTofinish
+    mov        eax, [eax + 16]     ; CountTofinish
     lock dec   dword [eax]         ; (*CountTofinish)--
     cmp        cl,  1              ; Check mwait-monitor support
-    jnz        HltLoop
-MwaitLoop:
+    jnz        HltLoopGeneric
+MwaitLoopGeneric:
     cli
     mov        eax, esp
@@ -246,10 +246,10 @@
     shl        eax, 4
     mwait
-    jmp        MwaitLoop
-HltLoop:
+    jmp        MwaitLoopGeneric
+HltLoopGeneric:
     cli
     hlt
-    jmp        HltLoop
-AsmRelocateApLoopEnd:
+    jmp        HltLoopGeneric
+AsmRelocateApLoopGenericEnd:
 
 ;-------------------------------------------------------------------------------------
@@ -265,6 +265,6 @@
     mov        dword [ebx + MP_ASSEMBLY_ADDRESS_MAP.ModeEntryOffset], Flat32Start - RendezvousFunnelProcStart
     mov        dword [ebx + MP_ASSEMBLY_ADDRESS_MAP.RendezvousFunnelSize], RendezvousFunnelProcEnd - RendezvousFunnelProcStart
-    mov        dword [ebx + MP_ASSEMBLY_ADDRESS_MAP.RelocateApLoopFuncAddress], AsmRelocateApLoopStart
-    mov        dword [ebx + MP_ASSEMBLY_ADDRESS_MAP.RelocateApLoopFuncSize], AsmRelocateApLoopEnd - AsmRelocateApLoopStart
+    mov        dword [ebx + MP_ASSEMBLY_ADDRESS_MAP.RelocateApLoopFuncAddressGeneric], AsmRelocateApLoopGenericStart
+    mov        dword [ebx + MP_ASSEMBLY_ADDRESS_MAP.RelocateApLoopFuncSizeGeneric], AsmRelocateApLoopGenericEnd - AsmRelocateApLoopGenericStart
     mov        dword [ebx + MP_ASSEMBLY_ADDRESS_MAP.ModeTransitionOffset], Flat32Start - RendezvousFunnelProcStart
     mov        dword [ebx + MP_ASSEMBLY_ADDRESS_MAP.SwitchToRealNoNxOffset], SwitchToRealProcStart - Flat32Start
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MpInitLib/Microcode.c
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MpInitLib/Microcode.c	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MpInitLib/Microcode.c	(revision 101291)
@@ -171,5 +171,5 @@
     DEBUG_INFO,
     "%a: Required microcode patches have been loaded at 0x%lx, with size 0x%lx.\n",
-    __FUNCTION__,
+    __func__,
     CpuMpData->MicrocodePatchAddress,
     CpuMpData->MicrocodePatchRegionSize
@@ -306,5 +306,5 @@
       DEBUG_INFO,
       "%a: 0x%x microcode patches will be loaded into memory, with size 0x%x.\n",
-      __FUNCTION__,
+      __func__,
       PatchCount,
       TotalLoadSize
@@ -366,5 +366,5 @@
   GuidHob = GetFirstGuidHob (&gEdkiiMicrocodePatchHobGuid);
   if (GuidHob == NULL) {
-    DEBUG ((DEBUG_INFO, "%a: Microcode patch cache HOB is not found.\n", __FUNCTION__));
+    DEBUG ((DEBUG_INFO, "%a: Microcode patch cache HOB is not found.\n", __func__));
     return FALSE;
   }
@@ -378,5 +378,5 @@
     DEBUG_INFO,
     "%a: MicrocodeBase = 0x%lx, MicrocodeSize = 0x%lx\n",
-    __FUNCTION__,
+    __func__,
     *Address,
     *RegionSize
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MpInitLib/MpEqu.inc
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MpInitLib/MpEqu.inc	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MpInitLib/MpEqu.inc	(revision 101291)
@@ -1,4 +1,4 @@
 ;------------------------------------------------------------------------------ ;
-; Copyright (c) 2015 - 2022, Intel Corporation. All rights reserved.<BR>
+; Copyright (c) 2015 - 2023, Intel Corporation. All rights reserved.<BR>
 ; SPDX-License-Identifier: BSD-2-Clause-Patent
 ;
@@ -22,13 +22,15 @@
 ;
 struc MP_ASSEMBLY_ADDRESS_MAP
-  .RendezvousFunnelAddress       CTYPE_UINTN 1
-  .ModeEntryOffset               CTYPE_UINTN 1
-  .RendezvousFunnelSize          CTYPE_UINTN 1
-  .RelocateApLoopFuncAddress     CTYPE_UINTN 1
-  .RelocateApLoopFuncSize        CTYPE_UINTN 1
-  .ModeTransitionOffset          CTYPE_UINTN 1
-  .SwitchToRealNoNxOffset        CTYPE_UINTN 1
-  .SwitchToRealPM16ModeOffset    CTYPE_UINTN 1
-  .SwitchToRealPM16ModeSize      CTYPE_UINTN 1
+  .RendezvousFunnelAddress            CTYPE_UINTN 1
+  .ModeEntryOffset                    CTYPE_UINTN 1
+  .RendezvousFunnelSize               CTYPE_UINTN 1
+  .RelocateApLoopFuncAddressGeneric   CTYPE_UINTN 1
+  .RelocateApLoopFuncSizeGeneric      CTYPE_UINTN 1
+  .RelocateApLoopFuncAddressAmdSev    CTYPE_UINTN 1
+  .RelocateApLoopFuncSizeAmdSev       CTYPE_UINTN 1
+  .ModeTransitionOffset               CTYPE_UINTN 1
+  .SwitchToRealNoNxOffset             CTYPE_UINTN 1
+  .SwitchToRealPM16ModeOffset         CTYPE_UINTN 1
+  .SwitchToRealPM16ModeSize           CTYPE_UINTN 1
 endstruc
 
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MpInitLib/MpHandOff.h
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MpInitLib/MpHandOff.h	(revision 101291)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MpInitLib/MpHandOff.h	(revision 101291)
@@ -0,0 +1,47 @@
+/** @file
+  Defines the HOB GUID used to describe the MSEG memory region allocated in PEI.
+  Copyright (c) 2015 - 2023, Intel Corporation. All rights reserved.<BR>
+  SPDX-License-Identifier: BSD-2-Clause-Patent
+**/
+
+#ifndef MP_HANDOFF_H_
+#define MP_HANDOFF_H_
+
+#define MP_HANDOFF_GUID \
+  { \
+    0x11e2bd88, 0xed38, 0x4abd, {0xa3, 0x99, 0x21, 0xf2, 0x5f, 0xd0, 0x7a, 0x60 } \
+  }
+
+extern EFI_GUID  mMpHandOffGuid;
+
+//
+// The information required to transfer from the PEI phase to the
+// DXE phase is contained within the MP_HAND_OFF and PROCESSOR_HAND_OFF.
+// If the SizeOfPointer (WaitLoopExecutionMode) of both phases are equal,
+// and the APs is not in halt mode,
+// then the APs can be awakened by triggering the start-up
+// signal, rather than using INIT-SIPI-SIPI.
+// To trigger the start-up signal, BSP writes the specified
+// StartupSignalValue to the StartupSignalAddress of each processor.
+// This address is monitored by the APs.
+//
+typedef struct {
+  UINT32    ApicId;
+  UINT32    Health;
+  UINT64    StartupSignalAddress;
+  UINT64    StartupProcedureAddress;
+} PROCESSOR_HAND_OFF;
+
+typedef struct {
+  //
+  // The ProcessorIndex indicates the range of processors. If it is set to 0, it signifies
+  // processors from 0 to CpuCount - 1. Multiple instances in the HOB list describe
+  // processors from ProcessorIndex to ProcessorIndex + CpuCount - 1.
+  //
+  UINT32                ProcessorIndex;
+  UINT32                CpuCount;
+  UINT32                WaitLoopExecutionMode;
+  UINT32                StartupSignalValue;
+  PROCESSOR_HAND_OFF    Info[];
+} MP_HAND_OFF;
+#endif
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MpInitLib/MpLib.c
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MpInitLib/MpLib.c	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MpInitLib/MpLib.c	(revision 101291)
@@ -19,4 +19,5 @@
 
 EFI_GUID  mCpuInitMpLibHobGuid = CPU_INIT_MP_LIB_HOB_GUID;
+EFI_GUID  mMpHandOffGuid       = MP_HANDOFF_GUID;
 
 /**
@@ -669,7 +670,77 @@
 
 /**
+  This function place APs in Halt loop.
+
+  @param[in] CpuMpData        Pointer to CPU MP Data
+**/
+VOID
+PlaceAPInHltLoop (
+  IN CPU_MP_DATA  *CpuMpData
+  )
+{
+  while (TRUE) {
+    DisableInterrupts ();
+    if (CpuMpData->UseSevEsAPMethod) {
+      SevEsPlaceApHlt (CpuMpData);
+    } else {
+      CpuSleep ();
+    }
+
+    CpuPause ();
+  }
+}
+
+/**
+  This function place APs in Mwait or Run loop.
+
+  @param[in] ApLoopMode                   Ap Loop Mode
+  @param[in] ApStartupSignalBuffer        Pointer to Ap Startup Signal Buffer
+  @param[in] ApTargetCState               Ap Target CState
+**/
+VOID
+PlaceAPInMwaitLoopOrRunLoop (
+  IN UINT8            ApLoopMode,
+  IN volatile UINT32  *ApStartupSignalBuffer,
+  IN UINT8            ApTargetCState
+  )
+{
+  while (TRUE) {
+    DisableInterrupts ();
+    if (ApLoopMode == ApInMwaitLoop) {
+      //
+      // Place AP in MWAIT-loop
+      //
+      AsmMonitor ((UINTN)ApStartupSignalBuffer, 0, 0);
+      if ((*ApStartupSignalBuffer != WAKEUP_AP_SIGNAL) && (*ApStartupSignalBuffer != MP_HAND_OFF_SIGNAL)) {
+        //
+        // Check AP start-up signal again.
+        // If AP start-up signal is not set, place AP into
+        // the specified C-state
+        //
+        AsmMwait (ApTargetCState << 4, 0);
+      }
+    } else if (ApLoopMode == ApInRunLoop) {
+      //
+      // Place AP in Run-loop
+      //
+      CpuPause ();
+    } else {
+      ASSERT (FALSE);
+    }
+
+    //
+    // If AP start-up signal is written, AP is waken up
+    // otherwise place AP in loop again
+    //
+    if ((*ApStartupSignalBuffer == WAKEUP_AP_SIGNAL) || (*ApStartupSignalBuffer == MP_HAND_OFF_SIGNAL)) {
+      break;
+    }
+  }
+}
+
+/**
   This function will be called from AP reset code if BSP uses WakeUpAP.
 
-  @param[in] ExchangeInfo     Pointer to the MP exchange info buffer
+  @param[in] CpuMpData        Pointer to CPU MP Data
   @param[in] ApIndex          Number of current executing AP
 **/
@@ -677,9 +748,8 @@
 EFIAPI
 ApWakeupFunction (
-  IN MP_CPU_EXCHANGE_INFO  *ExchangeInfo,
-  IN UINTN                 ApIndex
-  )
-{
-  CPU_MP_DATA       *CpuMpData;
+  IN CPU_MP_DATA  *CpuMpData,
+  IN UINTN        ApIndex
+  )
+{
   UINTN             ProcessorNumber;
   EFI_AP_PROCEDURE  Procedure;
@@ -691,9 +761,5 @@
   UINTN             CurrentApicMode;
   AP_STACK_DATA     *ApStackData;
-
-  //
-  // AP finished assembly code and begin to execute C code
-  //
-  CpuMpData = ExchangeInfo->CpuMpData;
+  UINT32            OriginalValue;
 
   //
@@ -736,4 +802,13 @@
       //
       ApStartupSignalBuffer = CpuMpData->CpuData[ProcessorNumber].StartupApSignal;
+      OriginalValue         = InterlockedCompareExchange32 (
+                                (UINT32 *)ApStartupSignalBuffer,
+                                MP_HAND_OFF_SIGNAL,
+                                0
+                                );
+      if (OriginalValue == MP_HAND_OFF_SIGNAL) {
+        SetApState (&CpuMpData->CpuData[ProcessorNumber], CpuStateReady);
+      }
+
       InterlockedCompareExchange32 (
         (UINT32 *)ApStartupSignalBuffer,
@@ -844,52 +919,38 @@
     //
     if (CpuMpData->ApLoopMode == ApInHltLoop) {
-      //
-      // Place AP in HLT-loop
-      //
-      while (TRUE) {
-        DisableInterrupts ();
-        if (CpuMpData->UseSevEsAPMethod) {
-          SevEsPlaceApHlt (CpuMpData);
-        } else {
-          CpuSleep ();
-        }
-
-        CpuPause ();
-      }
-    }
-
-    while (TRUE) {
-      DisableInterrupts ();
-      if (CpuMpData->ApLoopMode == ApInMwaitLoop) {
-        //
-        // Place AP in MWAIT-loop
-        //
-        AsmMonitor ((UINTN)ApStartupSignalBuffer, 0, 0);
-        if (*ApStartupSignalBuffer != WAKEUP_AP_SIGNAL) {
-          //
-          // Check AP start-up signal again.
-          // If AP start-up signal is not set, place AP into
-          // the specified C-state
-          //
-          AsmMwait (CpuMpData->ApTargetCState << 4, 0);
-        }
-      } else if (CpuMpData->ApLoopMode == ApInRunLoop) {
-        //
-        // Place AP in Run-loop
-        //
-        CpuPause ();
-      } else {
-        ASSERT (FALSE);
-      }
-
-      //
-      // If AP start-up signal is written, AP is waken up
-      // otherwise place AP in loop again
-      //
-      if (*ApStartupSignalBuffer == WAKEUP_AP_SIGNAL) {
-        break;
-      }
-    }
-  }
+      PlaceAPInHltLoop (CpuMpData);
+      //
+      // Never run here
+      //
+    } else {
+      PlaceAPInMwaitLoopOrRunLoop (CpuMpData->ApLoopMode, ApStartupSignalBuffer, CpuMpData->ApTargetCState);
+    }
+  }
+}
+
+/**
+  This function serves as the entry point for APs when
+  they are awakened by the stores in the memory address
+  indicated by the MP_HANDOFF_INFO structure.
+
+  @param[in] CpuMpData        Pointer to PEI CPU MP Data
+**/
+VOID
+EFIAPI
+DxeApEntryPoint (
+  CPU_MP_DATA  *CpuMpData
+  )
+{
+  UINTN  ProcessorNumber;
+
+  GetProcessorNumber (CpuMpData, &ProcessorNumber);
+  InterlockedIncrement ((UINT32 *)&CpuMpData->FinishedCount);
+  RestoreVolatileRegisters (&CpuMpData->CpuData[0].VolatileRegisters, FALSE);
+  PlaceAPInMwaitLoopOrRunLoop (
+    CpuMpData->ApLoopMode,
+    CpuMpData->CpuData[ProcessorNumber].StartupApSignal,
+    CpuMpData->ApTargetCState
+    );
+  ApWakeupFunction (CpuMpData, ProcessorNumber);
 }
 
@@ -1265,5 +1326,12 @@
         SevSnpCreateAP (CpuMpData, -1);
       } else {
-        SendInitSipiSipiAllExcludingSelf ((UINT32)ExchangeInfo->BufferStart);
+        if ((CpuMpData->InitFlag == ApInitConfig) && FixedPcdGetBool (PcdFirstTimeWakeUpAPsBySipi)) {
+          //
+          // SIPI can be used for the first time wake up after reset to reduce boot time.
+          //
+          SendStartupIpiAllExcludingSelf ((UINT32)ExchangeInfo->BufferStart);
+        } else {
+          SendInitSipiSipiAllExcludingSelf ((UINT32)ExchangeInfo->BufferStart);
+        }
       }
     }
@@ -1465,4 +1533,30 @@
 
 /**
+  Switch Context for each AP.
+
+**/
+VOID
+EFIAPI
+SwitchContextPerAp (
+  VOID
+  )
+{
+  UINTN            ProcessorNumber;
+  CPU_MP_DATA      *CpuMpData;
+  CPU_INFO_IN_HOB  *CpuInfoInHob;
+
+  CpuMpData    = GetCpuMpData ();
+  CpuInfoInHob = (CPU_INFO_IN_HOB *)(UINTN)CpuMpData->CpuInfoInHob;
+  GetProcessorNumber (CpuMpData, &ProcessorNumber);
+
+  SwitchStack (
+    (SWITCH_STACK_ENTRY_POINT)(UINTN)DxeApEntryPoint,
+    (VOID *)(UINTN)CpuMpData,
+    NULL,
+    (VOID *)((UINTN)CpuInfoInHob[ProcessorNumber].ApTopOfStack)
+    );
+}
+
+/**
   Checks whether timeout expires.
 
@@ -1569,5 +1663,5 @@
       DEBUG_VERBOSE,
       "%a: reached FinishedApLimit=%u in %Lu microseconds\n",
-      __FUNCTION__,
+      __func__,
       FinishedApLimit,
       DivU64x64Remainder (
@@ -1817,4 +1911,95 @@
 
 /**
+  This function Get BspNumber.
+
+  @param[in] MpHandOff        Pointer to MpHandOff
+  @return                     BspNumber
+**/
+UINT32
+GetBspNumber (
+  IN CONST MP_HAND_OFF  *MpHandOff
+  )
+{
+  UINT32  ApicId;
+  UINT32  BspNumber;
+  UINT32  Index;
+
+  //
+  // Get the processor number for the BSP
+  //
+  BspNumber = MAX_UINT32;
+  ApicId    = GetInitialApicId ();
+  for (Index = 0; Index < MpHandOff->CpuCount; Index++) {
+    if (MpHandOff->Info[Index].ApicId == ApicId) {
+      BspNumber = Index;
+    }
+  }
+
+  ASSERT (BspNumber != MAX_UINT32);
+
+  return BspNumber;
+}
+
+/**
+  This function is intended to be invoked by the BSP in order
+  to wake up the AP. The BSP accomplishes this by triggering a
+  start-up signal, which in turn causes any APs that are
+  currently in a loop on the PEI-prepared memory to awaken and
+  begin running the procedure called SwitchContextPerAp.
+  This procedure allows the AP to switch to another section of
+  memory and continue its loop there.
+
+  @param[in] MpHandOff  Pointer to MP hand-off data structure.
+**/
+VOID
+SwitchApContext (
+  IN MP_HAND_OFF  *MpHandOff
+  )
+{
+  UINTN   Index;
+  UINT32  BspNumber;
+
+  BspNumber = GetBspNumber (MpHandOff);
+
+  for (Index = 0; Index < MpHandOff->CpuCount; Index++) {
+    if (Index != BspNumber) {
+      *(UINTN *)(UINTN)MpHandOff->Info[Index].StartupProcedureAddress = (UINTN)SwitchContextPerAp;
+      *(UINT32 *)(UINTN)MpHandOff->Info[Index].StartupSignalAddress   = MpHandOff->StartupSignalValue;
+    }
+  }
+
+  //
+  // Wait all APs waken up if this is not the 1st broadcast of SIPI
+  //
+  for (Index = 0; Index < MpHandOff->CpuCount; Index++) {
+    if (Index != BspNumber) {
+      WaitApWakeup ((UINT32 *)(UINTN)(MpHandOff->Info[Index].StartupSignalAddress));
+    }
+  }
+}
+
+/**
+  Get pointer to MP_HAND_OFF GUIDed HOB.
+
+  @return  The pointer to MP_HAND_OFF structure.
+**/
+MP_HAND_OFF *
+GetMpHandOffHob (
+  VOID
+  )
+{
+  EFI_HOB_GUID_TYPE  *GuidHob;
+  MP_HAND_OFF        *MpHandOff;
+
+  MpHandOff = NULL;
+  GuidHob   = GetFirstGuidHob (&mMpHandOffGuid);
+  if (GuidHob != NULL) {
+    MpHandOff = (MP_HAND_OFF *)GET_GUID_HOB_DATA (GuidHob);
+  }
+
+  return MpHandOff;
+}
+
+/**
   MP Initialize Library initialization.
 
@@ -1835,5 +2020,5 @@
   )
 {
-  CPU_MP_DATA              *OldCpuMpData;
+  MP_HAND_OFF              *MpHandOff;
   CPU_INFO_IN_HOB          *CpuInfoInHob;
   UINT32                   MaxLogicalProcessorNumber;
@@ -1854,9 +2039,9 @@
   UINTN                    ApIdtBase;
 
-  OldCpuMpData = GetCpuMpDataFromGuidedHob ();
-  if (OldCpuMpData == NULL) {
+  MpHandOff = GetMpHandOffHob ();
+  if (MpHandOff == NULL) {
     MaxLogicalProcessorNumber = PcdGet32 (PcdCpuMaxLogicalProcessorNumber);
   } else {
-    MaxLogicalProcessorNumber = OldCpuMpData->CpuCount;
+    MaxLogicalProcessorNumber = MpHandOff->CpuCount;
   }
 
@@ -2002,5 +2187,5 @@
   ProgramVirtualWireMode ();
 
-  if (OldCpuMpData == NULL) {
+  if (MpHandOff == NULL) {
     if (MaxLogicalProcessorNumber > 1) {
       //
@@ -2014,13 +2199,53 @@
     // from HOB
     //
-    OldCpuMpData->NewCpuMpData = CpuMpData;
-    CpuMpData->CpuCount        = OldCpuMpData->CpuCount;
-    CpuMpData->BspNumber       = OldCpuMpData->BspNumber;
-    CpuMpData->CpuInfoInHob    = OldCpuMpData->CpuInfoInHob;
-    CpuInfoInHob               = (CPU_INFO_IN_HOB *)(UINTN)CpuMpData->CpuInfoInHob;
+    if (CpuMpData->UseSevEsAPMethod) {
+      AmdSevUpdateCpuMpData (CpuMpData);
+    }
+
+    CpuMpData->CpuCount  = MpHandOff->CpuCount;
+    CpuMpData->BspNumber = GetBspNumber (MpHandOff);
+    CpuInfoInHob         = (CPU_INFO_IN_HOB *)(UINTN)CpuMpData->CpuInfoInHob;
     for (Index = 0; Index < CpuMpData->CpuCount; Index++) {
       InitializeSpinLock (&CpuMpData->CpuData[Index].ApLock);
-      CpuMpData->CpuData[Index].CpuHealthy = (CpuInfoInHob[Index].Health == 0) ? TRUE : FALSE;
+      CpuMpData->CpuData[Index].CpuHealthy = (MpHandOff->Info[Index].Health == 0) ? TRUE : FALSE;
       CpuMpData->CpuData[Index].ApFunction = 0;
+      CpuInfoInHob[Index].InitialApicId    = MpHandOff->Info[Index].ApicId;
+      CpuInfoInHob[Index].ApTopOfStack     = CpuMpData->Buffer + (Index + 1) * CpuMpData->CpuApStackSize;
+      CpuInfoInHob[Index].ApicId           = MpHandOff->Info[Index].ApicId;
+      CpuInfoInHob[Index].Health           = MpHandOff->Info[Index].Health;
+    }
+
+    DEBUG ((DEBUG_INFO, "MpHandOff->WaitLoopExecutionMode: %04d, sizeof (VOID *): %04d\n", MpHandOff->WaitLoopExecutionMode, sizeof (VOID *)));
+    if (MpHandOff->WaitLoopExecutionMode == sizeof (VOID *)) {
+      ASSERT (CpuMpData->ApLoopMode != ApInHltLoop);
+
+      CpuMpData->FinishedCount = 0;
+      CpuMpData->InitFlag      = ApInitDone;
+      SaveCpuMpData (CpuMpData);
+      //
+      // In scenarios where both the PEI and DXE phases run in the same
+      // execution mode (32bit or 64bit), the BSP triggers
+      // a start-up signal during the DXE phase to wake up the APs. This causes any
+      // APs that are currently in a loop on the memory prepared during the PEI
+      // phase to awaken and run the SwitchContextPerAp procedure. This procedure
+      // enables the APs to switch to a different memory section and continue their
+      // looping process there.
+      //
+      SwitchApContext (MpHandOff);
+      ASSERT (CpuMpData->FinishedCount == (CpuMpData->CpuCount - 1));
+
+      //
+      // Set Apstate as Idle, otherwise Aps cannot be waken-up again.
+      // If any enabled AP is not idle, return EFI_NOT_READY during waken-up.
+      //
+      for (Index = 0; Index < CpuMpData->CpuCount; Index++) {
+        SetApState (&CpuMpData->CpuData[Index], CpuStateIdle);
+      }
+
+      //
+      // Initialize global data for MP support
+      //
+      InitMpGlobalData (CpuMpData);
+      return EFI_SUCCESS;
     }
   }
@@ -2051,5 +2276,5 @@
   //
   if (CpuMpData->CpuCount > 1) {
-    if (OldCpuMpData != NULL) {
+    if (MpHandOff != NULL) {
       //
       // Only needs to use this flag for DXE phase to update the wake up
@@ -2068,5 +2293,5 @@
     }
 
-    if (OldCpuMpData != NULL) {
+    if (MpHandOff != NULL) {
       CpuMpData->InitFlag = ApInitDone;
     }
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MpInitLib/MpLib.h
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MpInitLib/MpLib.h	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MpInitLib/MpLib.h	(revision 101291)
@@ -2,5 +2,5 @@
   Common header file for MP Initialize Library.
 
-  Copyright (c) 2016 - 2022, Intel Corporation. All rights reserved.<BR>
+  Copyright (c) 2016 - 2023, Intel Corporation. All rights reserved.<BR>
   Copyright (c) 2020, AMD Inc. All rights reserved.<BR>
 
@@ -28,5 +28,4 @@
 #include <Library/LocalApicLib.h>
 #include <Library/CpuLib.h>
-#include <Library/UefiCpuLib.h>
 #include <Library/TimerLib.h>
 #include <Library/SynchronizationLib.h>
@@ -41,6 +40,15 @@
 
 #include <Guid/MicrocodePatchHob.h>
+#include "MpHandOff.h"
 
 #define WAKEUP_AP_SIGNAL  SIGNATURE_32 ('S', 'T', 'A', 'P')
+//
+// To trigger the start-up signal, BSP writes the specified
+// StartupSignalValue to the StartupSignalAddress of each processor.
+// This address is monitored by the APs, and as soon as they receive
+// the value that matches the MP_HAND_OFF_SIGNAL, they will wake up
+// and switch the context from PEI to DXE phase.
+//
+#define MP_HAND_OFF_SIGNAL  SIGNATURE_32 ('M', 'P', 'H', 'O')
 
 #define CPU_INIT_MP_LIB_HOB_GUID \
@@ -178,6 +186,8 @@
   UINTN    ModeEntryOffset;
   UINTN    RendezvousFunnelSize;
-  UINT8    *RelocateApLoopFuncAddress;
-  UINTN    RelocateApLoopFuncSize;
+  UINT8    *RelocateApLoopFuncAddressGeneric;
+  UINTN    RelocateApLoopFuncSizeGeneric;
+  UINT8    *RelocateApLoopFuncAddressAmdSev;
+  UINTN    RelocateApLoopFuncSizeAmdSev;
   UINTN    ModeTransitionOffset;
   UINTN    SwitchToRealNoNxOffset;
@@ -312,4 +322,5 @@
 #define AP_SAFE_STACK_SIZE   128
 #define AP_RESET_STACK_SIZE  AP_SAFE_STACK_SIZE
+STATIC_ASSERT ((AP_SAFE_STACK_SIZE & (CPU_STACK_ALIGNMENT - 1)) == 0, "AP_SAFE_STACK_SIZE is not aligned with CPU_STACK_ALIGNMENT");
 
 #pragma pack(1)
@@ -363,5 +374,28 @@
 typedef
   VOID
-(EFIAPI *ASM_RELOCATE_AP_LOOP)(
+(EFIAPI *ASM_RELOCATE_AP_LOOP_GENERIC)(
+  IN BOOLEAN                 MwaitSupport,
+  IN UINTN                   ApTargetCState,
+  IN UINTN                   TopOfApStack,
+  IN UINTN                   NumberToFinish,
+  IN UINTN                   Cr3
+  );
+
+/**
+  Assembly code to place AP into safe loop mode for Amd processors
+  with Sev enabled.
+  Place AP into targeted C-State if MONITOR is supported, otherwise
+  place AP into hlt state.
+  Place AP in protected mode if the current is long mode. Due to AP maybe
+  wakeup by some hardware event. It could avoid accessing page table that
+  may not available during booting to OS.
+  @param[in] MwaitSupport    TRUE indicates MONITOR is supported.
+                             FALSE indicates MONITOR is not supported.
+  @param[in] ApTargetCState  Target C-State value.
+  @param[in] PmCodeSegment   Protected mode code segment value.
+**/
+typedef
+  VOID
+(EFIAPI *ASM_RELOCATE_AP_LOOP_AMDSEV)(
   IN BOOLEAN                 MwaitSupport,
   IN UINTN                   ApTargetCState,
@@ -402,4 +436,10 @@
   );
 
+typedef union {
+  VOID                            *Data;
+  ASM_RELOCATE_AP_LOOP_AMDSEV     AmdSevEntry;  // 64-bit AMD Sev processors
+  ASM_RELOCATE_AP_LOOP_GENERIC    GenericEntry; // Intel processors (32-bit or 64-bit), 32-bit AMD processors, or AMD non-Sev processors
+} RELOCATE_AP_LOOP_ENTRY;
+
 /**
   Get the pointer to CPU MP Data structure.
@@ -436,4 +476,13 @@
 
 /**
+  Switch Context for each AP.
+
+**/
+VOID
+SwitchApContext (
+  IN MP_HAND_OFF  *MpHandOff
+  );
+
+/**
   Get available EfiBootServicesCode memory below 4GB by specified size.
 
@@ -463,4 +512,16 @@
 GetSevEsAPMemory (
   VOID
+  );
+
+/**
+  Create 1:1 mapping page table in reserved memory to map the specified address range.
+  @param[in]      LinearAddress  The start of the linear address range.
+  @param[in]      Length         The length of the linear address range.
+  @return The page table to be created.
+**/
+UINTN
+CreatePageTable (
+  IN UINTN  Address,
+  IN UINTN  Length
   );
 
@@ -849,3 +910,13 @@
   );
 
+/**
+  Get pointer to CPU MP Data structure from GUIDed HOB.
+
+  @param[in] CpuMpData  The pointer to CPU MP Data structure.
+**/
+VOID
+AmdSevUpdateCpuMpData (
+  IN CPU_MP_DATA  *CpuMpData
+  );
+
 #endif
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MpInitLib/PeiMpInitLib.inf
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MpInitLib/PeiMpInitLib.inf	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MpInitLib/PeiMpInitLib.inf	(revision 101291)
@@ -37,5 +37,5 @@
   MpLib.h
   Microcode.c
-
+  MpHandOff.h
 [Packages]
   MdePkg/MdePkg.dec
@@ -50,5 +50,4 @@
   MtrrLib
   CpuLib
-  UefiCpuLib
   SynchronizationLib
   PeiServicesLib
@@ -68,4 +67,5 @@
   gUefiCpuPkgTokenSpaceGuid.PcdSevEsWorkAreaBase                   ## SOMETIMES_CONSUMES
   gUefiCpuPkgTokenSpaceGuid.PcdGhcbHypervisorFeatures              ## CONSUMES
+  gUefiCpuPkgTokenSpaceGuid.PcdFirstTimeWakeUpAPsBySipi             ## CONSUMES
   gEfiMdeModulePkgTokenSpaceGuid.PcdGhcbBase                       ## CONSUMES
   gEfiMdePkgTokenSpaceGuid.PcdConfidentialComputingGuestAttr       ## CONSUMES
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MpInitLib/PeiMpLib.c
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MpInitLib/PeiMpLib.c	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MpInitLib/PeiMpLib.c	(revision 101291)
@@ -127,5 +127,35 @@
   )
 {
-  UINT64  Data64;
+  UINT64           Data64;
+  UINTN            Index;
+  CPU_INFO_IN_HOB  *CpuInfoInHob;
+  MP_HAND_OFF      *MpHandOff;
+  UINTN            MpHandOffSize;
+
+  //
+  // When APs are in a state that can be waken up by a store operation to a memory address,
+  // report the MP_HAND_OFF data for DXE to use.
+  //
+  CpuInfoInHob  = (CPU_INFO_IN_HOB *)(UINTN)CpuMpData->CpuInfoInHob;
+  MpHandOffSize = sizeof (MP_HAND_OFF) + sizeof (PROCESSOR_HAND_OFF) * CpuMpData->CpuCount;
+  MpHandOff     = (MP_HAND_OFF *)BuildGuidHob (&mMpHandOffGuid, MpHandOffSize);
+  ASSERT (MpHandOff != NULL);
+  ZeroMem (MpHandOff, MpHandOffSize);
+  MpHandOff->ProcessorIndex = 0;
+
+  MpHandOff->CpuCount = CpuMpData->CpuCount;
+  if (CpuMpData->ApLoopMode != ApInHltLoop) {
+    MpHandOff->StartupSignalValue    = MP_HAND_OFF_SIGNAL;
+    MpHandOff->WaitLoopExecutionMode = sizeof (VOID *);
+  }
+
+  for (Index = 0; Index < MpHandOff->CpuCount; Index++) {
+    MpHandOff->Info[Index].ApicId = CpuInfoInHob[Index].ApicId;
+    MpHandOff->Info[Index].Health = CpuInfoInHob[Index].Health;
+    if (CpuMpData->ApLoopMode != ApInHltLoop) {
+      MpHandOff->Info[Index].StartupSignalAddress    = (UINT64)(UINTN)CpuMpData->CpuData[Index].StartupApSignal;
+      MpHandOff->Info[Index].StartupProcedureAddress = (UINT64)(UINTN)&CpuMpData->CpuData[Index].ApFunction;
+    }
+  }
 
   //
@@ -759,5 +789,5 @@
     DEBUG_INFO,
     "%a: Required microcode patches have been loaded at 0x%lx, with size 0x%lx.\n",
-    __FUNCTION__,
+    __func__,
     CpuMpData->MicrocodePatchAddress,
     CpuMpData->MicrocodePatchRegionSize
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MpInitLib/X64/AmdSev.c
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MpInitLib/X64/AmdSev.c	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MpInitLib/X64/AmdSev.c	(revision 101291)
@@ -13,4 +13,100 @@
 #include <Register/Amd/Fam17Msr.h>
 #include <Register/Amd/Ghcb.h>
+
+#define _IS_ALIGNED(x, y)  (ALIGN_POINTER((x), (y)) == (x))
+
+/**
+  Perform the requested AP Creation action.
+
+  @param[in]  SaveArea         Pointer to VM save area (VMSA)
+  @param[in]  ApicId           APIC ID of the vCPU
+  @param[in]  Action           AP action to perform
+
+  @retval  TRUE   Action completed successfully
+  @retval  FALSE  Action did not complete successfully
+**/
+STATIC
+BOOLEAN
+SevSnpPerformApAction (
+  IN SEV_ES_SAVE_AREA  *SaveArea,
+  IN UINT32            ApicId,
+  IN UINTN             Action
+  )
+{
+  MSR_SEV_ES_GHCB_REGISTER  Msr;
+  GHCB                      *Ghcb;
+  BOOLEAN                   InterruptState;
+  UINT64                    ExitInfo1;
+  UINT64                    ExitInfo2;
+  UINT32                    RmpAdjustStatus;
+  UINT64                    VmgExitStatus;
+
+  if (Action == SVM_VMGEXIT_SNP_AP_CREATE) {
+    //
+    // To turn the page into a recognized VMSA page, issue RMPADJUST:
+    //   Target VMPL but numerically higher than current VMPL
+    //   Target PermissionMask is not used
+    //
+    RmpAdjustStatus = SevSnpRmpAdjust (
+                        (EFI_PHYSICAL_ADDRESS)(UINTN)SaveArea,
+                        TRUE
+                        );
+    if (RmpAdjustStatus != 0) {
+      DEBUG ((DEBUG_INFO, "SEV-SNP: RMPADJUST failed for VMSA creation\n"));
+      ASSERT (FALSE);
+
+      return FALSE;
+    }
+  }
+
+  ExitInfo1  = (UINT64)ApicId << 32;
+  ExitInfo1 |= Action;
+  ExitInfo2  = (UINT64)(UINTN)SaveArea;
+
+  Msr.GhcbPhysicalAddress = AsmReadMsr64 (MSR_SEV_ES_GHCB);
+  Ghcb                    = Msr.Ghcb;
+
+  CcExitVmgInit (Ghcb, &InterruptState);
+
+  if (Action == SVM_VMGEXIT_SNP_AP_CREATE) {
+    Ghcb->SaveArea.Rax = SaveArea->SevFeatures;
+    CcExitVmgSetOffsetValid (Ghcb, GhcbRax);
+  }
+
+  VmgExitStatus = CcExitVmgExit (
+                    Ghcb,
+                    SVM_EXIT_SNP_AP_CREATION,
+                    ExitInfo1,
+                    ExitInfo2
+                    );
+
+  CcExitVmgDone (Ghcb, InterruptState);
+
+  if (VmgExitStatus != 0) {
+    DEBUG ((DEBUG_INFO, "SEV-SNP: AP Destroy failed\n"));
+    ASSERT (FALSE);
+
+    return FALSE;
+  }
+
+  if (Action == SVM_VMGEXIT_SNP_AP_DESTROY) {
+    //
+    // Make the current VMSA not runnable and accessible to be
+    // reprogrammed.
+    //
+    RmpAdjustStatus = SevSnpRmpAdjust (
+                        (EFI_PHYSICAL_ADDRESS)(UINTN)SaveArea,
+                        FALSE
+                        );
+    if (RmpAdjustStatus != 0) {
+      DEBUG ((DEBUG_INFO, "SEV-SNP: RMPADJUST failed for VMSA reset\n"));
+      ASSERT (FALSE);
+
+      return FALSE;
+    }
+  }
+
+  return TRUE;
+}
 
 /**
@@ -28,25 +124,49 @@
   )
 {
-  SEV_ES_SAVE_AREA          *SaveArea;
-  IA32_CR0                  ApCr0;
-  IA32_CR0                  ResetCr0;
-  IA32_CR4                  ApCr4;
-  IA32_CR4                  ResetCr4;
-  UINTN                     StartIp;
-  UINT8                     SipiVector;
-  UINT32                    RmpAdjustStatus;
-  UINT64                    VmgExitStatus;
-  MSR_SEV_ES_GHCB_REGISTER  Msr;
-  GHCB                      *Ghcb;
-  BOOLEAN                   InterruptState;
-  UINT64                    ExitInfo1;
-  UINT64                    ExitInfo2;
-
-  //
-  // Allocate a single page for the SEV-ES Save Area and initialize it.
-  //
-  SaveArea = AllocateReservedPages (1);
-  if (!SaveArea) {
-    return;
+  UINT8             *Pages;
+  SEV_ES_SAVE_AREA  *SaveArea;
+  IA32_CR0          ApCr0;
+  IA32_CR0          ResetCr0;
+  IA32_CR4          ApCr4;
+  IA32_CR4          ResetCr4;
+  UINTN             StartIp;
+  UINT8             SipiVector;
+
+  if (CpuData->SevEsSaveArea == NULL) {
+    //
+    // Allocate a page for the SEV-ES Save Area and initialize it. Due to AMD
+    // erratum #1467 (VMSA cannot be on a 2MB boundary), allocate an extra page
+    // to choose from to work around the issue.
+    //
+    Pages = AllocateReservedPages (2);
+    if (!Pages) {
+      return;
+    }
+
+    //
+    // Since page allocation works by allocating downward in the address space,
+    // try to always free the first (lower address) page to limit possible holes
+    // in the memory map. So, if the address of the second page is 2MB aligned,
+    // then use the first page and free the second page. Otherwise, free the
+    // first page and use the second page.
+    //
+    if (_IS_ALIGNED (Pages + EFI_PAGE_SIZE, SIZE_2MB)) {
+      SaveArea = (SEV_ES_SAVE_AREA *)Pages;
+      FreePages (Pages + EFI_PAGE_SIZE, 1);
+    } else {
+      SaveArea = (SEV_ES_SAVE_AREA *)(Pages + EFI_PAGE_SIZE);
+      FreePages (Pages, 1);
+    }
+
+    CpuData->SevEsSaveArea = SaveArea;
+  } else {
+    SaveArea = CpuData->SevEsSaveArea;
+
+    //
+    // Tell the hypervisor to not use the current VMSA
+    //
+    if (!SevSnpPerformApAction (SaveArea, ApicId, SVM_VMGEXIT_SNP_AP_DESTROY)) {
+      return;
+    }
   }
 
@@ -133,61 +253,5 @@
   SaveArea->SevFeatures = AsmReadMsr64 (MSR_SEV_STATUS) >> 2;
 
-  //
-  // To turn the page into a recognized VMSA page, issue RMPADJUST:
-  //   Target VMPL but numerically higher than current VMPL
-  //   Target PermissionMask is not used
-  //
-  RmpAdjustStatus = SevSnpRmpAdjust (
-                      (EFI_PHYSICAL_ADDRESS)(UINTN)SaveArea,
-                      TRUE
-                      );
-  ASSERT (RmpAdjustStatus == 0);
-
-  ExitInfo1  = (UINT64)ApicId << 32;
-  ExitInfo1 |= SVM_VMGEXIT_SNP_AP_CREATE;
-  ExitInfo2  = (UINT64)(UINTN)SaveArea;
-
-  Msr.GhcbPhysicalAddress = AsmReadMsr64 (MSR_SEV_ES_GHCB);
-  Ghcb                    = Msr.Ghcb;
-
-  CcExitVmgInit (Ghcb, &InterruptState);
-  Ghcb->SaveArea.Rax = SaveArea->SevFeatures;
-  CcExitVmgSetOffsetValid (Ghcb, GhcbRax);
-  VmgExitStatus = CcExitVmgExit (
-                    Ghcb,
-                    SVM_EXIT_SNP_AP_CREATION,
-                    ExitInfo1,
-                    ExitInfo2
-                    );
-  CcExitVmgDone (Ghcb, InterruptState);
-
-  ASSERT (VmgExitStatus == 0);
-  if (VmgExitStatus != 0) {
-    RmpAdjustStatus = SevSnpRmpAdjust (
-                        (EFI_PHYSICAL_ADDRESS)(UINTN)SaveArea,
-                        FALSE
-                        );
-    if (RmpAdjustStatus == 0) {
-      FreePages (SaveArea, 1);
-    } else {
-      DEBUG ((DEBUG_INFO, "SEV-SNP: RMPADJUST failed, leaking VMSA page\n"));
-    }
-
-    SaveArea = NULL;
-  }
-
-  if (CpuData->SevEsSaveArea) {
-    RmpAdjustStatus = SevSnpRmpAdjust (
-                        (EFI_PHYSICAL_ADDRESS)(UINTN)CpuData->SevEsSaveArea,
-                        FALSE
-                        );
-    if (RmpAdjustStatus == 0) {
-      FreePages (CpuData->SevEsSaveArea, 1);
-    } else {
-      DEBUG ((DEBUG_INFO, "SEV-SNP: RMPADJUST failed, leaking VMSA page\n"));
-    }
-  }
-
-  CpuData->SevEsSaveArea = SaveArea;
+  SevSnpPerformApAction (SaveArea, ApicId, SVM_VMGEXIT_SNP_AP_CREATE);
 }
 
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MpInitLib/X64/AmdSev.nasm
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MpInitLib/X64/AmdSev.nasm	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MpInitLib/X64/AmdSev.nasm	(revision 101291)
@@ -347,2 +347,172 @@
 
 SwitchToRealProcEnd:
+
+;-------------------------------------------------------------------------------------
+;  AsmRelocateApLoopAmdSev (MwaitSupport, ApTargetCState, PmCodeSegment, TopOfApStack, CountTofinish, Pm16CodeSegment, SevEsAPJumpTable, WakeupBuffer);
+;-------------------------------------------------------------------------------------
+
+AsmRelocateApLoopAmdSevStart:
+BITS 64
+    cmp        qword [rsp + 56], 0  ; SevEsAPJumpTable
+    je         NoSevEsAmdSev
+
+    ;
+    ; Perform some SEV-ES related setup before leaving 64-bit mode
+    ;
+    push       rcx
+    push       rdx
+
+    ;
+    ; Get the RDX reset value using CPUID
+    ;
+    mov        rax, 1
+    cpuid
+    mov        rsi, rax          ; Save off the reset value for RDX
+
+    ;
+    ; Prepare the GHCB for the AP_HLT_LOOP VMGEXIT call
+    ;   - Must be done while in 64-bit long mode so that writes to
+    ;     the GHCB memory will be unencrypted.
+    ;   - No NAE events can be generated once this is set otherwise
+    ;     the AP_RESET_HOLD SW_EXITCODE will be overwritten.
+    ;
+    mov        rcx, 0xc0010130
+    rdmsr                        ; Retrieve current GHCB address
+    shl        rdx, 32
+    or         rdx, rax
+
+    mov        rdi, rdx
+    xor        rax, rax
+    mov        rcx, 0x800
+    shr        rcx, 3
+    rep stosq                    ; Clear the GHCB
+
+    mov        rax, 0x80000004   ; VMGEXIT AP_RESET_HOLD
+    mov        [rdx + 0x390], rax
+    mov        rax, 114          ; Set SwExitCode valid bit
+    bts        [rdx + 0x3f0], rax
+    inc        rax               ; Set SwExitInfo1 valid bit
+    bts        [rdx + 0x3f0], rax
+    inc        rax               ; Set SwExitInfo2 valid bit
+    bts        [rdx + 0x3f0], rax
+
+    pop        rdx
+    pop        rcx
+
+NoSevEsAmdSev:
+    cli                          ; Disable interrupt before switching to 32-bit mode
+    mov        rax, [rsp + 40]   ; CountTofinish
+    lock dec   dword [rax]       ; (*CountTofinish)--
+
+    mov        r10, [rsp + 48]   ; Pm16CodeSegment
+    mov        rax, [rsp + 56]   ; SevEsAPJumpTable
+    mov        rbx, [rsp + 64]   ; WakeupBuffer
+    mov        rsp, r9           ; TopOfApStack
+
+    push       rax               ; Save SevEsAPJumpTable
+    push       rbx               ; Save WakeupBuffer
+    push       r10               ; Save Pm16CodeSegment
+    push       rcx               ; Save MwaitSupport
+    push       rdx               ; Save ApTargetCState
+
+    lea        rax, [PmEntryAmdSev]    ; rax <- The start address of transition code
+
+    push       r8
+    push       rax
+
+    ;
+    ; Clear R8 - R15, for reset, before going into 32-bit mode
+    ;
+    xor        r8, r8
+    xor        r9, r9
+    xor        r10, r10
+    xor        r11, r11
+    xor        r12, r12
+    xor        r13, r13
+    xor        r14, r14
+    xor        r15, r15
+
+    ;
+    ; Far return into 32-bit mode
+    ;
+o64 retf
+
+BITS 32
+PmEntryAmdSev:
+    mov        eax, cr0
+    btr        eax, 31           ; Clear CR0.PG
+    mov        cr0, eax          ; Disable paging and caches
+
+    mov        ecx, 0xc0000080
+    rdmsr
+    and        ah, ~ 1           ; Clear LME
+    wrmsr
+    mov        eax, cr4
+    and        al, ~ (1 << 5)    ; Clear PAE
+    mov        cr4, eax
+
+    pop        edx
+    add        esp, 4
+    pop        ecx,
+    add        esp, 4
+
+MwaitCheckAmdSev:
+    cmp        cl, 1              ; Check mwait-monitor support
+    jnz        HltLoopAmdSev
+    mov        ebx, edx           ; Save C-State to ebx
+MwaitLoopAmdSev:
+    cli
+    mov        eax, esp           ; Set Monitor Address
+    xor        ecx, ecx           ; ecx = 0
+    xor        edx, edx           ; edx = 0
+    monitor
+    mov        eax, ebx           ; Mwait Cx, Target C-State per eax[7:4]
+    shl        eax, 4
+    mwait
+    jmp        MwaitLoopAmdSev
+
+HltLoopAmdSev:
+    pop        edx                ; PM16CodeSegment
+    add        esp, 4
+    pop        ebx                ; WakeupBuffer
+    add        esp, 4
+    pop        eax                ; SevEsAPJumpTable
+    add        esp, 4
+    cmp        eax, 0             ; Check for SEV-ES
+    je         DoHltAmdSev
+
+    cli
+    ;
+    ; SEV-ES is enabled, use VMGEXIT (GHCB information already
+    ; set by caller)
+    ;
+BITS 64
+    rep        vmmcall
+BITS 32
+
+    ;
+    ; Back from VMGEXIT AP_HLT_LOOP
+    ;   Push the FLAGS/CS/IP values to use
+    ;
+    push       word 0x0002        ; EFLAGS
+    xor        ecx, ecx
+    mov        cx, [eax + 2]      ; CS
+    push       cx
+    mov        cx, [eax]          ; IP
+    push       cx
+    push       word 0x0000        ; For alignment, will be discarded
+
+    push       edx
+    push       ebx
+
+    mov        edx, esi           ; Restore RDX reset value
+
+    retf
+
+DoHltAmdSev:
+    cli
+    hlt
+    jmp        DoHltAmdSev
+
+BITS 64
+AsmRelocateApLoopAmdSevEnd:
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MpInitLib/X64/CreatePageTable.c
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MpInitLib/X64/CreatePageTable.c	(revision 101291)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MpInitLib/X64/CreatePageTable.c	(revision 101291)
@@ -0,0 +1,81 @@
+/** @file
+  Function to create page talbe.
+  Only create page table for x64, and leave the CreatePageTable empty for Ia32.
+  Copyright (c) 2023, Intel Corporation. All rights reserved.<BR>
+  SPDX-License-Identifier: BSD-2-Clause-Patent
+**/
+#include <Library/CpuPageTableLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Base.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/DebugLib.h>
+#include <Library/BaseLib.h>
+
+/**
+  Create 1:1 mapping page table in reserved memory to map the specified address range.
+  @param[in]      LinearAddress  The start of the linear address range.
+  @param[in]      Length         The length of the linear address range.
+  @return The page table to be created.
+**/
+UINTN
+CreatePageTable (
+  IN UINTN  Address,
+  IN UINTN  Length
+  )
+{
+  EFI_STATUS   Status;
+  VOID         *PageTableBuffer;
+  UINTN        PageTableBufferSize;
+  UINTN        PageTable;
+  PAGING_MODE  PagingMode;
+  IA32_CR4     Cr4;
+
+  IA32_MAP_ATTRIBUTE  MapAttribute;
+  IA32_MAP_ATTRIBUTE  MapMask;
+
+  MapAttribute.Uint64         = Address;
+  MapAttribute.Bits.Present   = 1;
+  MapAttribute.Bits.ReadWrite = 1;
+  MapMask.Uint64              = MAX_UINT64;
+
+  PageTable           = 0;
+  PageTableBufferSize = 0;
+
+  Cr4.UintN = AsmReadCr4 ();
+
+  if (Cr4.Bits.LA57 == 1) {
+    PagingMode = Paging5Level;
+  } else {
+    PagingMode = Paging4Level;
+  }
+
+  Status = PageTableMap (
+             &PageTable,
+             PagingMode,
+             NULL,
+             &PageTableBufferSize,
+             Address,
+             Length,
+             &MapAttribute,
+             &MapMask,
+             NULL
+             );
+  ASSERT (Status == EFI_BUFFER_TOO_SMALL);
+  DEBUG ((DEBUG_INFO, "AP Page Table Buffer Size = %x\n", PageTableBufferSize));
+
+  PageTableBuffer = AllocateReservedPages (EFI_SIZE_TO_PAGES (PageTableBufferSize));
+  ASSERT (PageTableBuffer != NULL);
+  Status = PageTableMap (
+             &PageTable,
+             PagingMode,
+             PageTableBuffer,
+             &PageTableBufferSize,
+             Address,
+             Length,
+             &MapAttribute,
+             &MapMask,
+             NULL
+             );
+  ASSERT_EFI_ERROR (Status);
+  return PageTable;
+}
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MpInitLib/X64/MpFuncs.nasm
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MpInitLib/X64/MpFuncs.nasm	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MpInitLib/X64/MpFuncs.nasm	(revision 101291)
@@ -1,4 +1,4 @@
 ;------------------------------------------------------------------------------ ;
-; Copyright (c) 2015 - 2022, Intel Corporation. All rights reserved.<BR>
+; Copyright (c) 2015 - 2023, Intel Corporation. All rights reserved.<BR>
 ; SPDX-License-Identifier: BSD-2-Clause-Patent
 ;
@@ -260,6 +260,5 @@
 
     mov        edx, ebx          ; edx is ApIndex
-    mov        ecx, esi
-    add        ecx, MP_CPU_EXCHANGE_INFO_OFFSET ; rcx is address of exchange info data buffer
+    mov        rcx, qword [esi + MP_CPU_EXCHANGE_INFO_FIELD (CpuMpData)]
 
     mov        edi, esi
@@ -280,171 +279,55 @@
 
 ;-------------------------------------------------------------------------------------
-;  AsmRelocateApLoop (MwaitSupport, ApTargetCState, PmCodeSegment, TopOfApStack, CountTofinish, Pm16CodeSegment, SevEsAPJumpTable, WakeupBuffer);
-;-------------------------------------------------------------------------------------
-AsmRelocateApLoopStart:
-BITS 64
-    cmp        qword [rsp + 56], 0  ; SevEsAPJumpTable
-    je         NoSevEs
-
-    ;
-    ; Perform some SEV-ES related setup before leaving 64-bit mode
-    ;
-    push       rcx
-    push       rdx
-
-    ;
-    ; Get the RDX reset value using CPUID
-    ;
-    mov        rax, 1
-    cpuid
-    mov        rsi, rax          ; Save off the reset value for RDX
-
-    ;
-    ; Prepare the GHCB for the AP_HLT_LOOP VMGEXIT call
-    ;   - Must be done while in 64-bit long mode so that writes to
-    ;     the GHCB memory will be unencrypted.
-    ;   - No NAE events can be generated once this is set otherwise
-    ;     the AP_RESET_HOLD SW_EXITCODE will be overwritten.
-    ;
-    mov        rcx, 0xc0010130
-    rdmsr                        ; Retrieve current GHCB address
-    shl        rdx, 32
-    or         rdx, rax
-
-    mov        rdi, rdx
-    xor        rax, rax
-    mov        rcx, 0x800
-    shr        rcx, 3
-    rep stosq                    ; Clear the GHCB
-
-    mov        rax, 0x80000004   ; VMGEXIT AP_RESET_HOLD
-    mov        [rdx + 0x390], rax
-    mov        rax, 114          ; Set SwExitCode valid bit
-    bts        [rdx + 0x3f0], rax
-    inc        rax               ; Set SwExitInfo1 valid bit
-    bts        [rdx + 0x3f0], rax
-    inc        rax               ; Set SwExitInfo2 valid bit
-    bts        [rdx + 0x3f0], rax
-
-    pop        rdx
-    pop        rcx
-
-NoSevEs:
-    cli                          ; Disable interrupt before switching to 32-bit mode
-    mov        rax, [rsp + 40]   ; CountTofinish
+;  AsmRelocateApLoop (MwaitSupport, ApTargetCState, TopOfApStack, CountTofinish, Cr3);
+;  This function is called during the finalizaiton of Mp initialization before booting
+;  to OS, and aim to put Aps either in Mwait or HLT.
+;-------------------------------------------------------------------------------------
+; +----------------+
+; | Cr3            |  rsp+40
+; +----------------+
+; | CountTofinish  |  r9
+; +----------------+
+; | TopOfApStack   |  r8
+; +----------------+
+; | ApTargetCState |  rdx
+; +----------------+
+; | MwaitSupport   |  rcx
+; +----------------+
+; | the return     |
+; +----------------+ low address
+
+AsmRelocateApLoopGenericStart:
+    mov        rax, r9           ; CountTofinish
     lock dec   dword [rax]       ; (*CountTofinish)--
 
-    mov        r10, [rsp + 48]   ; Pm16CodeSegment
-    mov        rax, [rsp + 56]   ; SevEsAPJumpTable
-    mov        rbx, [rsp + 64]   ; WakeupBuffer
-    mov        rsp, r9           ; TopOfApStack
-
-    push       rax               ; Save SevEsAPJumpTable
-    push       rbx               ; Save WakeupBuffer
-    push       r10               ; Save Pm16CodeSegment
-    push       rcx               ; Save MwaitSupport
-    push       rdx               ; Save ApTargetCState
-
-    lea        rax, [PmEntry]    ; rax <- The start address of transition code
-
-    push       r8
-    push       rax
-
-    ;
-    ; Clear R8 - R15, for reset, before going into 32-bit mode
-    ;
-    xor        r8, r8
-    xor        r9, r9
-    xor        r10, r10
-    xor        r11, r11
-    xor        r12, r12
-    xor        r13, r13
-    xor        r14, r14
-    xor        r15, r15
-
-    ;
-    ; Far return into 32-bit mode
-    ;
-    retfq
-
-BITS 32
-PmEntry:
-    mov        eax, cr0
-    btr        eax, 31           ; Clear CR0.PG
-    mov        cr0, eax          ; Disable paging and caches
-
-    mov        ecx, 0xc0000080
-    rdmsr
-    and        ah, ~ 1           ; Clear LME
-    wrmsr
-    mov        eax, cr4
-    and        al, ~ (1 << 5)    ; Clear PAE
-    mov        cr4, eax
-
-    pop        edx
-    add        esp, 4
-    pop        ecx,
-    add        esp, 4
-
-MwaitCheck:
+    mov        rax, [rsp + 40]    ; Cr3
+    ; Do not push on old stack, since old stack is not mapped
+    ; in the page table pointed by cr3
+    mov        cr3, rax
+    mov        rsp, r8            ; TopOfApStack
+
+MwaitCheckGeneric:
     cmp        cl, 1              ; Check mwait-monitor support
-    jnz        HltLoop
-    mov        ebx, edx           ; Save C-State to ebx
-MwaitLoop:
+    jnz        HltLoopGeneric
+    mov        rbx, rdx           ; Save C-State to ebx
+
+MwaitLoopGeneric:
     cli
-    mov        eax, esp           ; Set Monitor Address
+    mov        rax, rsp           ; Set Monitor Address
+    sub        eax, 8             ; To ensure the monitor address is in the page table
     xor        ecx, ecx           ; ecx = 0
     xor        edx, edx           ; edx = 0
     monitor
-    mov        eax, ebx           ; Mwait Cx, Target C-State per eax[7:4]
+    mov        rax, rbx           ; Mwait Cx, Target C-State per eax[7:4]
     shl        eax, 4
     mwait
-    jmp        MwaitLoop
-
-HltLoop:
-    pop        edx                ; PM16CodeSegment
-    add        esp, 4
-    pop        ebx                ; WakeupBuffer
-    add        esp, 4
-    pop        eax                ; SevEsAPJumpTable
-    add        esp, 4
-    cmp        eax, 0             ; Check for SEV-ES
-    je         DoHlt
-
-    cli
-    ;
-    ; SEV-ES is enabled, use VMGEXIT (GHCB information already
-    ; set by caller)
-    ;
-BITS 64
-    rep        vmmcall
-BITS 32
-
-    ;
-    ; Back from VMGEXIT AP_HLT_LOOP
-    ;   Push the FLAGS/CS/IP values to use
-    ;
-    push       word 0x0002        ; EFLAGS
-    xor        ecx, ecx
-    mov        cx, [eax + 2]      ; CS
-    push       cx
-    mov        cx, [eax]          ; IP
-    push       cx
-    push       word 0x0000        ; For alignment, will be discarded
-
-    push       edx
-    push       ebx
-
-    mov        edx, esi           ; Restore RDX reset value
-
-    retf
-
-DoHlt:
+    jmp        MwaitLoopGeneric
+
+HltLoopGeneric:
     cli
     hlt
-    jmp        DoHlt
-
-BITS 64
-AsmRelocateApLoopEnd:
+    jmp        HltLoopGeneric
+
+AsmRelocateApLoopGenericEnd:
 
 ;-------------------------------------------------------------------------------------
@@ -457,7 +340,10 @@
     mov        qword [rcx + MP_ASSEMBLY_ADDRESS_MAP.ModeEntryOffset], LongModeStart - RendezvousFunnelProcStart
     mov        qword [rcx + MP_ASSEMBLY_ADDRESS_MAP.RendezvousFunnelSize], RendezvousFunnelProcEnd - RendezvousFunnelProcStart
-    lea        rax, [AsmRelocateApLoopStart]
-    mov        qword [rcx + MP_ASSEMBLY_ADDRESS_MAP.RelocateApLoopFuncAddress], rax
-    mov        qword [rcx + MP_ASSEMBLY_ADDRESS_MAP.RelocateApLoopFuncSize], AsmRelocateApLoopEnd - AsmRelocateApLoopStart
+lea        rax, [AsmRelocateApLoopGenericStart]
+    mov        qword [rcx + MP_ASSEMBLY_ADDRESS_MAP.RelocateApLoopFuncAddressGeneric], rax
+    mov        qword [rcx + MP_ASSEMBLY_ADDRESS_MAP.RelocateApLoopFuncSizeGeneric], AsmRelocateApLoopGenericEnd - AsmRelocateApLoopGenericStart
+    lea        rax, [AsmRelocateApLoopAmdSevStart]
+    mov        qword [rcx + MP_ASSEMBLY_ADDRESS_MAP.RelocateApLoopFuncAddressAmdSev], rax
+    mov        qword [rcx + MP_ASSEMBLY_ADDRESS_MAP.RelocateApLoopFuncSizeAmdSev], AsmRelocateApLoopAmdSevEnd - AsmRelocateApLoopAmdSevStart
     mov        qword [rcx + MP_ASSEMBLY_ADDRESS_MAP.ModeTransitionOffset], Flat32Start - RendezvousFunnelProcStart
     mov        qword [rcx + MP_ASSEMBLY_ADDRESS_MAP.SwitchToRealNoNxOffset], SwitchToRealProcStart - Flat32Start
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MtrrLib/MtrrLib.c
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MtrrLib/MtrrLib.c	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MtrrLib/MtrrLib.c	(revision 101291)
@@ -757,6 +757,9 @@
   )
 {
-  UINT32                          MaxExtendedFunction;
-  CPUID_VIR_PHY_ADDRESS_SIZE_EAX  VirPhyAddressSize;
+  UINT32                                       MaxExtendedFunction;
+  CPUID_VIR_PHY_ADDRESS_SIZE_EAX               VirPhyAddressSize;
+  UINT32                                       MaxFunction;
+  CPUID_STRUCTURED_EXTENDED_FEATURE_FLAGS_ECX  ExtendedFeatureFlagsEcx;
+  MSR_IA32_TME_ACTIVATE_REGISTER               TmeActivate;
 
   AsmCpuid (CPUID_EXTENDED_FUNCTION, &MaxExtendedFunction, NULL, NULL, NULL);
@@ -766,4 +769,21 @@
   } else {
     VirPhyAddressSize.Bits.PhysicalAddressBits = 36;
+  }
+
+  //
+  // CPUID enumeration of MAX_PA is unaffected by TME-MK activation and will continue
+  // to report the maximum physical address bits available for software to use,
+  // irrespective of the number of KeyID bits.
+  // So, we need to check if TME is enabled and adjust the PA size accordingly.
+  //
+  AsmCpuid (CPUID_SIGNATURE, &MaxFunction, NULL, NULL, NULL);
+  if (MaxFunction >= CPUID_STRUCTURED_EXTENDED_FEATURE_FLAGS) {
+    AsmCpuidEx (CPUID_STRUCTURED_EXTENDED_FEATURE_FLAGS, 0, NULL, NULL, &ExtendedFeatureFlagsEcx.Uint32, NULL);
+    if (ExtendedFeatureFlagsEcx.Bits.TME_EN == 1) {
+      TmeActivate.Uint64 = AsmReadMsr64 (MSR_IA32_TME_ACTIVATE);
+      if (TmeActivate.Bits.TmeEnable == 1) {
+        VirPhyAddressSize.Bits.PhysicalAddressBits -= TmeActivate.Bits.MkTmeKeyidBits;
+      }
+    }
   }
 
@@ -1252,19 +1272,4 @@
 
   return (MTRR_MEMORY_CACHE_TYPE)Type;
-}
-
-/**
-  Return TRUE when the Operand is exactly power of 2.
-
-  @retval TRUE  Operand is exactly power of 2.
-  @retval FALSE Operand is not power of 2.
-**/
-BOOLEAN
-MtrrLibIsPowerOfTwo (
-  IN     UINT64  Operand
-  )
-{
-  ASSERT (Operand != 0);
-  return (BOOLEAN)((Operand & (Operand - 1)) == 0);
 }
 
@@ -1639,5 +1644,5 @@
         }
 
-        if ((Weight[M (Start, Stop)] == MAX_WEIGHT) && MtrrLibIsPowerOfTwo (Length)) {
+        if ((Weight[M (Start, Stop)] == MAX_WEIGHT) && IS_POW2 (Length)) {
           if (MtrrLibGetNumberOfTypes (
                 Ranges,
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MtrrLib/UnitTest/MtrrLibUnitTest.c
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MtrrLib/UnitTest/MtrrLibUnitTest.c	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MtrrLib/UnitTest/MtrrLibUnitTest.c	(revision 101291)
@@ -2,5 +2,5 @@
   Unit tests of the MtrrLib instance of the MtrrLib class
 
-  Copyright (c) 2020, Intel Corporation. All rights reserved.<BR>
+  Copyright (c) 2020 - 2023, Intel Corporation. All rights reserved.<BR>
   SPDX-License-Identifier: BSD-2-Clause-Patent
 
@@ -31,4 +31,6 @@
   { 48, TRUE, TRUE, CacheWriteProtected, 12 },
   { 48, TRUE, TRUE, CacheWriteCombining, 12 },
+
+  { 48, TRUE, TRUE, CacheWriteBack,      12, 7}, // 7 bits for MKTME
 };
 
@@ -220,5 +222,5 @@
     );
   GenerateValidAndConfigurableMtrrPairs (
-    SystemParameter->PhysicalAddressBits,
+    SystemParameter->PhysicalAddressBits - SystemParameter->MkTmeKeyidBits,
     RawMtrrRange,
     UcCount,
@@ -233,5 +235,5 @@
   GetEffectiveMemoryRanges (
     SystemParameter->DefaultCacheType,
-    SystemParameter->PhysicalAddressBits,
+    SystemParameter->PhysicalAddressBits - SystemParameter->MkTmeKeyidBits,
     RawMtrrRange,
     ExpectedVariableMtrrUsage,
@@ -279,5 +281,5 @@
     CollectTestResult (
       SystemParameter->DefaultCacheType,
-      SystemParameter->PhysicalAddressBits,
+      SystemParameter->PhysicalAddressBits - SystemParameter->MkTmeKeyidBits,
       SystemParameter->VariableMtrrCount,
       &LocalMtrrs,
@@ -326,5 +328,5 @@
 
   RangeCount = Random32 (1, ARRAY_SIZE (Ranges));
-  MaxAddress = 1ull << SystemParameter->PhysicalAddressBits;
+  MaxAddress = 1ull << (SystemParameter->PhysicalAddressBits - SystemParameter->MkTmeKeyidBits);
 
   for (Index = 0; Index < RangeCount; Index++) {
@@ -968,5 +970,5 @@
     );
   GenerateValidAndConfigurableMtrrPairs (
-    SystemParameter->PhysicalAddressBits,
+    SystemParameter->PhysicalAddressBits - SystemParameter->MkTmeKeyidBits,
     RawMtrrRange,
     UcCount,
@@ -981,5 +983,5 @@
   GetEffectiveMemoryRanges (
     SystemParameter->DefaultCacheType,
-    SystemParameter->PhysicalAddressBits,
+    SystemParameter->PhysicalAddressBits - SystemParameter->MkTmeKeyidBits,
     RawMtrrRange,
     ExpectedVariableMtrrUsage,
@@ -1020,5 +1022,5 @@
     CollectTestResult (
       SystemParameter->DefaultCacheType,
-      SystemParameter->PhysicalAddressBits,
+      SystemParameter->PhysicalAddressBits - SystemParameter->MkTmeKeyidBits,
       SystemParameter->VariableMtrrCount,
       &LocalMtrrs,
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MtrrLib/UnitTest/MtrrLibUnitTest.h
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MtrrLib/UnitTest/MtrrLibUnitTest.h	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MtrrLib/UnitTest/MtrrLibUnitTest.h	(revision 101291)
@@ -1,5 +1,5 @@
 /** @file
 
-  Copyright (c) 2020, Intel Corporation. All rights reserved.<BR>
+  Copyright (c) 2020 - 2023, Intel Corporation. All rights reserved.<BR>
   SPDX-License-Identifier: BSD-2-Clause-Patent
 
@@ -41,4 +41,5 @@
   MTRR_MEMORY_CACHE_TYPE    DefaultCacheType;
   UINT32                    VariableMtrrCount;
+  UINT8                     MkTmeKeyidBits;
 } MTRR_LIB_SYSTEM_PARAMETER;
 
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MtrrLib/UnitTest/Support.c
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MtrrLib/UnitTest/Support.c	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/MtrrLib/UnitTest/Support.c	(revision 101291)
@@ -2,5 +2,5 @@
   Unit tests of the MtrrLib instance of the MtrrLib class
 
-  Copyright (c) 2018 - 2020, Intel Corporation. All rights reserved.<BR>
+  Copyright (c) 2018 - 2023, Intel Corporation. All rights reserved.<BR>
   SPDX-License-Identifier: BSD-2-Clause-Patent
 
@@ -13,11 +13,13 @@
 };
 
-UINT64                           mFixedMtrrsValue[MTRR_NUMBER_OF_FIXED_MTRR];
-MSR_IA32_MTRR_PHYSBASE_REGISTER  mVariableMtrrsPhysBase[MTRR_NUMBER_OF_VARIABLE_MTRR];
-MSR_IA32_MTRR_PHYSMASK_REGISTER  mVariableMtrrsPhysMask[MTRR_NUMBER_OF_VARIABLE_MTRR];
-MSR_IA32_MTRR_DEF_TYPE_REGISTER  mDefTypeMsr;
-MSR_IA32_MTRRCAP_REGISTER        mMtrrCapMsr;
-CPUID_VERSION_INFO_EDX           mCpuidVersionInfoEdx;
-CPUID_VIR_PHY_ADDRESS_SIZE_EAX   mCpuidVirPhyAddressSizeEax;
+UINT64                                       mFixedMtrrsValue[MTRR_NUMBER_OF_FIXED_MTRR];
+MSR_IA32_MTRR_PHYSBASE_REGISTER              mVariableMtrrsPhysBase[MTRR_NUMBER_OF_VARIABLE_MTRR];
+MSR_IA32_MTRR_PHYSMASK_REGISTER              mVariableMtrrsPhysMask[MTRR_NUMBER_OF_VARIABLE_MTRR];
+MSR_IA32_MTRR_DEF_TYPE_REGISTER              mDefTypeMsr;
+MSR_IA32_MTRRCAP_REGISTER                    mMtrrCapMsr;
+MSR_IA32_TME_ACTIVATE_REGISTER               mTmeActivateMsr;
+CPUID_VERSION_INFO_EDX                       mCpuidVersionInfoEdx;
+CPUID_STRUCTURED_EXTENDED_FEATURE_FLAGS_ECX  mCpuidExtendedFeatureFlagsEcx;
+CPUID_VIR_PHY_ADDRESS_SIZE_EAX               mCpuidVirPhyAddressSizeEax;
 
 BOOLEAN       mRandomInput;
@@ -88,4 +90,92 @@
 
 /**
+  Retrieves CPUID information using an extended leaf identifier.
+
+  Executes the CPUID instruction with EAX set to the value specified by Index
+  and ECX set to the value specified by SubIndex. This function always returns
+  Index. This function is only available on IA-32 and x64.
+
+  If Eax is not NULL, then the value of EAX after CPUID is returned in Eax.
+  If Ebx is not NULL, then the value of EBX after CPUID is returned in Ebx.
+  If Ecx is not NULL, then the value of ECX after CPUID is returned in Ecx.
+  If Edx is not NULL, then the value of EDX after CPUID is returned in Edx.
+
+  @param  Index     The 32-bit value to load into EAX prior to invoking the
+                    CPUID instruction.
+  @param  SubIndex  The 32-bit value to load into ECX prior to invoking the
+                    CPUID instruction.
+  @param  Eax       The pointer to the 32-bit EAX value returned by the CPUID
+                    instruction. This is an optional parameter that may be
+                    NULL.
+  @param  Ebx       The pointer to the 32-bit EBX value returned by the CPUID
+                    instruction. This is an optional parameter that may be
+                    NULL.
+  @param  Ecx       The pointer to the 32-bit ECX value returned by the CPUID
+                    instruction. This is an optional parameter that may be
+                    NULL.
+  @param  Edx       The pointer to the 32-bit EDX value returned by the CPUID
+                    instruction. This is an optional parameter that may be
+                    NULL.
+
+  @return Index.
+
+**/
+UINT32
+EFIAPI
+UnitTestMtrrLibAsmCpuidEx (
+  IN      UINT32  Index,
+  IN      UINT32  SubIndex,
+  OUT     UINT32  *Eax   OPTIONAL,
+  OUT     UINT32  *Ebx   OPTIONAL,
+  OUT     UINT32  *Ecx   OPTIONAL,
+  OUT     UINT32  *Edx   OPTIONAL
+  )
+{
+  switch (Index) {
+    case CPUID_SIGNATURE:
+      if (Eax != NULL) {
+        *Eax = CPUID_STRUCTURED_EXTENDED_FEATURE_FLAGS;
+      }
+
+      return Index;
+      break;
+    case CPUID_VERSION_INFO:
+      if (Edx != NULL) {
+        *Edx = mCpuidVersionInfoEdx.Uint32;
+      }
+
+      return Index;
+      break;
+    case CPUID_STRUCTURED_EXTENDED_FEATURE_FLAGS:
+      if (Ecx != NULL) {
+        *Ecx = mCpuidExtendedFeatureFlagsEcx.Uint32;
+      }
+
+      return Index;
+      break;
+    case CPUID_EXTENDED_FUNCTION:
+      if (Eax != NULL) {
+        *Eax = CPUID_VIR_PHY_ADDRESS_SIZE;
+      }
+
+      return Index;
+      break;
+    case CPUID_VIR_PHY_ADDRESS_SIZE:
+      if (Eax != NULL) {
+        *Eax = mCpuidVirPhyAddressSizeEax.Uint32;
+      }
+
+      return Index;
+      break;
+  }
+
+  //
+  // Should never fall through to here
+  //
+  ASSERT (FALSE);
+  return Index;
+}
+
+/**
   Retrieves CPUID information.
 
@@ -122,33 +212,5 @@
   )
 {
-  switch (Index) {
-    case CPUID_VERSION_INFO:
-      if (Edx != NULL) {
-        *Edx = mCpuidVersionInfoEdx.Uint32;
-      }
-
-      return Index;
-      break;
-    case CPUID_EXTENDED_FUNCTION:
-      if (Eax != NULL) {
-        *Eax = CPUID_VIR_PHY_ADDRESS_SIZE;
-      }
-
-      return Index;
-      break;
-    case CPUID_VIR_PHY_ADDRESS_SIZE:
-      if (Eax != NULL) {
-        *Eax = mCpuidVirPhyAddressSizeEax.Uint32;
-      }
-
-      return Index;
-      break;
-  }
-
-  //
-  // Should never fall through to here
-  //
-  ASSERT (FALSE);
-  return Index;
+  return UnitTestMtrrLibAsmCpuidEx (Index, 0, Eax, Ebx, Ecx, Edx);
 }
 
@@ -199,4 +261,8 @@
   if (MsrIndex == MSR_IA32_MTRRCAP) {
     return mMtrrCapMsr.Uint64;
+  }
+
+  if (MsrIndex == MSR_IA32_TME_ACTIVATE) {
+    return mTmeActivateMsr.Uint64;
   }
 
@@ -318,7 +384,19 @@
   // Hook BaseLib functions used by MtrrLib that require some emulation.
   //
-  gUnitTestHostBaseLib.X86->AsmCpuid      = UnitTestMtrrLibAsmCpuid;
+  gUnitTestHostBaseLib.X86->AsmCpuid   = UnitTestMtrrLibAsmCpuid;
+  gUnitTestHostBaseLib.X86->AsmCpuidEx = UnitTestMtrrLibAsmCpuidEx;
+
   gUnitTestHostBaseLib.X86->AsmReadMsr64  = UnitTestMtrrLibAsmReadMsr64;
   gUnitTestHostBaseLib.X86->AsmWriteMsr64 = UnitTestMtrrLibAsmWriteMsr64;
+
+  if (SystemParameter->MkTmeKeyidBits != 0) {
+    mCpuidExtendedFeatureFlagsEcx.Bits.TME_EN = 1;
+    mTmeActivateMsr.Bits.TmeEnable            = 1;
+    mTmeActivateMsr.Bits.MkTmeKeyidBits       = SystemParameter->MkTmeKeyidBits;
+  } else {
+    mCpuidExtendedFeatureFlagsEcx.Bits.TME_EN = 0;
+    mTmeActivateMsr.Bits.TmeEnable            = 0;
+    mTmeActivateMsr.Bits.MkTmeKeyidBits       = 0;
+  }
 
   return UNIT_TEST_PASSED;
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/RegisterCpuFeaturesLib/CpuFeaturesInitialize.c
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/RegisterCpuFeaturesLib/CpuFeaturesInitialize.c	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/RegisterCpuFeaturesLib/CpuFeaturesInitialize.c	(revision 101291)
@@ -2,5 +2,5 @@
   CPU Features Initialize functions.
 
-  Copyright (c) 2017 - 2021, Intel Corporation. All rights reserved.<BR>
+  Copyright (c) 2017 - 2023, Intel Corporation. All rights reserved.<BR>
   SPDX-License-Identifier: BSD-2-Clause-Patent
 
@@ -68,10 +68,10 @@
   DisplayedFamily = Eax.Bits.FamilyId;
   if (Eax.Bits.FamilyId == 0x0F) {
-    DisplayedFamily |= (Eax.Bits.ExtendedFamilyId << 4);
+    DisplayedFamily += Eax.Bits.ExtendedFamilyId;
   }
 
   DisplayedModel = Eax.Bits.Model;
   if ((Eax.Bits.FamilyId == 0x06) || (Eax.Bits.FamilyId == 0x0f)) {
-    DisplayedModel |= (Eax.Bits.ExtendedModelId << 4);
+    DisplayedModel += (Eax.Bits.ExtendedModelId << 4);
   }
 
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/SmmCpuFeaturesLib/AmdSmmCpuFeaturesLib.c
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/SmmCpuFeaturesLib/AmdSmmCpuFeaturesLib.c	(revision 101291)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/SmmCpuFeaturesLib/AmdSmmCpuFeaturesLib.c	(revision 101291)
@@ -0,0 +1,387 @@
+/** @file
+Implementation specific to the SmmCpuFeatureLib library instance
+for AMD based platforms.
+
+Copyright (c) 2010 - 2019, Intel Corporation. All rights reserved.<BR>
+Copyright (c) Microsoft Corporation.<BR>
+Copyright (C) 2023 Advanced Micro Devices, Inc. All rights reserved.<BR>
+SPDX-License-Identifier: BSD-2-Clause-Patent
+
+**/
+
+#include <Library/SmmCpuFeaturesLib.h>
+#include <Uefi/UefiBaseType.h>
+#include <Register/Amd/SmramSaveStateMap.h>
+#include <Library/BaseLib.h>
+#include <Library/DebugLib.h>
+#include <Library/MmSaveStateLib.h>
+
+// EFER register LMA bit
+#define LMA  BIT10
+
+// Machine Specific Registers (MSRs)
+#define SMMADDR_ADDRESS  0xC0010112ul
+#define SMMMASK_ADDRESS  0xC0010113ul
+#define EFER_ADDRESS     0XC0000080ul
+
+// The mode of the CPU at the time an SMI occurs
+STATIC UINT8  mSmmSaveStateRegisterLma;
+
+/**
+  Performs library initialization.
+
+  This initialization function contains common functionality shared betwen all
+  library instance constructors.
+
+**/
+VOID
+CpuFeaturesLibInitialization (
+  VOID
+  )
+{
+  UINT32  LMAValue;
+
+  LMAValue                 = (UINT32)AsmReadMsr64 (EFER_ADDRESS) & LMA;
+  mSmmSaveStateRegisterLma = EFI_SMM_SAVE_STATE_REGISTER_LMA_32BIT;
+  if (LMAValue) {
+    mSmmSaveStateRegisterLma = EFI_SMM_SAVE_STATE_REGISTER_LMA_64BIT;
+  }
+}
+
+/**
+  Called during the very first SMI into System Management Mode to initialize
+  CPU features, including SMBASE, for the currently executing CPU.  Since this
+  is the first SMI, the SMRAM Save State Map is at the default address of
+  AMD_SMM_DEFAULT_SMBASE + SMRAM_SAVE_STATE_MAP_OFFSET.  The currently executing
+  CPU is specified by CpuIndex and CpuIndex can be used to access information
+  about the currently executing CPU in the ProcessorInfo array and the
+  HotPlugCpuData data structure.
+
+  @param[in] CpuIndex        The index of the CPU to initialize.  The value
+                             must be between 0 and the NumberOfCpus field in
+                             the System Management System Table (SMST).
+  @param[in] IsMonarch       TRUE if the CpuIndex is the index of the CPU that
+                             was elected as monarch during System Management
+                             Mode initialization.
+                             FALSE if the CpuIndex is not the index of the CPU
+                             that was elected as monarch during System
+                             Management Mode initialization.
+  @param[in] ProcessorInfo   Pointer to an array of EFI_PROCESSOR_INFORMATION
+                             structures.  ProcessorInfo[CpuIndex] contains the
+                             information for the currently executing CPU.
+  @param[in] CpuHotPlugData  Pointer to the CPU_HOT_PLUG_DATA structure that
+                             contains the ApidId and SmBase arrays.
+**/
+VOID
+EFIAPI
+SmmCpuFeaturesInitializeProcessor (
+  IN UINTN                      CpuIndex,
+  IN BOOLEAN                    IsMonarch,
+  IN EFI_PROCESSOR_INFORMATION  *ProcessorInfo,
+  IN CPU_HOT_PLUG_DATA          *CpuHotPlugData
+  )
+{
+  AMD_SMRAM_SAVE_STATE_MAP  *CpuState;
+  UINT32                    LMAValue;
+
+  //
+  // Configure SMBASE.
+  //
+  CpuState             = (AMD_SMRAM_SAVE_STATE_MAP *)(UINTN)(SMM_DEFAULT_SMBASE + SMRAM_SAVE_STATE_MAP_OFFSET);
+  CpuState->x64.SMBASE = (UINT32)CpuHotPlugData->SmBase[CpuIndex];
+
+  // Re-initialize the value of mSmmSaveStateRegisterLma flag which might have been changed in PiCpuSmmDxeSmm Driver
+  // Entry point, to make sure correct value on AMD platform is assigned to be used by SmmCpuFeaturesLib.
+  LMAValue                 = (UINT32)AsmReadMsr64 (EFER_ADDRESS) & LMA;
+  mSmmSaveStateRegisterLma = EFI_SMM_SAVE_STATE_REGISTER_LMA_32BIT;
+  if (LMAValue) {
+    mSmmSaveStateRegisterLma = EFI_SMM_SAVE_STATE_REGISTER_LMA_64BIT;
+  }
+
+  //
+  // If SMRR is supported, then program SMRR base/mask MSRs.
+  // The EFI_MSR_SMRR_PHYS_MASK_VALID bit is not set until the first normal SMI.
+  // The code that initializes SMM environment is running in normal mode
+  // from SMRAM region.  If SMRR is enabled here, then the SMRAM region
+  // is protected and the normal mode code execution will fail.
+  //
+  if (FeaturePcdGet (PcdSmrrEnable)) {
+    //
+    // SMRR size cannot be less than 4-KBytes
+    // SMRR size must be of length 2^n
+    // SMRR base alignment cannot be less than SMRR length
+    //
+    if ((CpuHotPlugData->SmrrSize < SIZE_4KB) ||
+        (CpuHotPlugData->SmrrSize != GetPowerOfTwo32 (CpuHotPlugData->SmrrSize)) ||
+        ((CpuHotPlugData->SmrrBase & ~(CpuHotPlugData->SmrrSize - 1)) != CpuHotPlugData->SmrrBase))
+    {
+      //
+      // Print message and halt if CPU is Monarch
+      //
+      if (IsMonarch) {
+        DEBUG ((DEBUG_ERROR, "SMM Base/Size does not meet alignment/size requirement!\n"));
+        CpuDeadLoop ();
+      }
+    } else {
+      AsmWriteMsr64 (SMMADDR_ADDRESS, CpuHotPlugData->SmrrBase);
+      AsmWriteMsr64 (SMMMASK_ADDRESS, ((~(UINT64)(CpuHotPlugData->SmrrSize - 1)) | 0x6600));
+    }
+  }
+}
+
+/**
+  This function updates the SMRAM save state on the currently executing CPU
+  to resume execution at a specific address after an RSM instruction.  This
+  function must evaluate the SMRAM save state to determine the execution mode
+  the RSM instruction resumes and update the resume execution address with
+  either NewInstructionPointer32 or NewInstructionPoint.  The auto HALT restart
+  flag in the SMRAM save state must always be cleared.  This function returns
+  the value of the instruction pointer from the SMRAM save state that was
+  replaced.  If this function returns 0, then the SMRAM save state was not
+  modified.
+
+  This function is called during the very first SMI on each CPU after
+  SmmCpuFeaturesInitializeProcessor() to set a flag in normal execution mode
+  to signal that the SMBASE of each CPU has been updated before the default
+  SMBASE address is used for the first SMI to the next CPU.
+
+  @param[in] CpuIndex                 The index of the CPU to hook.  The value
+                                      must be between 0 and the NumberOfCpus
+                                      field in the System Management System Table
+                                      (SMST).
+  @param[in] CpuState                 Pointer to SMRAM Save State Map for the
+                                      currently executing CPU.
+  @param[in] NewInstructionPointer32  Instruction pointer to use if resuming to
+                                      32-bit execution mode from 64-bit SMM.
+  @param[in] NewInstructionPointer    Instruction pointer to use if resuming to
+                                      same execution mode as SMM.
+
+  @retval 0    This function did modify the SMRAM save state.
+  @retval > 0  The original instruction pointer value from the SMRAM save state
+               before it was replaced.
+**/
+UINT64
+EFIAPI
+SmmCpuFeaturesHookReturnFromSmm (
+  IN UINTN                 CpuIndex,
+  IN SMRAM_SAVE_STATE_MAP  *CpuState,
+  IN UINT64                NewInstructionPointer32,
+  IN UINT64                NewInstructionPointer
+  )
+{
+  UINT64                    OriginalInstructionPointer;
+  AMD_SMRAM_SAVE_STATE_MAP  *AmdCpuState;
+
+  AmdCpuState = (AMD_SMRAM_SAVE_STATE_MAP *)CpuState;
+
+  if (mSmmSaveStateRegisterLma == EFI_SMM_SAVE_STATE_REGISTER_LMA_32BIT) {
+    OriginalInstructionPointer = (UINT64)AmdCpuState->x86._EIP;
+    AmdCpuState->x86._EIP      = (UINT32)NewInstructionPointer;
+    //
+    // Clear the auto HALT restart flag so the RSM instruction returns
+    // program control to the instruction following the HLT instruction.
+    //
+    if ((AmdCpuState->x86.AutoHALTRestart & BIT0) != 0) {
+      AmdCpuState->x86.AutoHALTRestart &= ~BIT0;
+    }
+  } else {
+    OriginalInstructionPointer = AmdCpuState->x64._RIP;
+    if ((AmdCpuState->x64.EFER & LMA) == 0) {
+      AmdCpuState->x64._RIP = (UINT32)NewInstructionPointer32;
+    } else {
+      AmdCpuState->x64._RIP = (UINT32)NewInstructionPointer;
+    }
+
+    //
+    // Clear the auto HALT restart flag so the RSM instruction returns
+    // program control to the instruction following the HLT instruction.
+    //
+    if ((AmdCpuState->x64.AutoHALTRestart & BIT0) != 0) {
+      AmdCpuState->x64.AutoHALTRestart &= ~BIT0;
+    }
+  }
+
+  return OriginalInstructionPointer;
+}
+
+/**
+  Return the size, in bytes, of a custom SMI Handler in bytes.  If 0 is
+  returned, then a custom SMI handler is not provided by this library,
+  and the default SMI handler must be used.
+
+  @retval 0    Use the default SMI handler.
+  @retval > 0  Use the SMI handler installed by SmmCpuFeaturesInstallSmiHandler()
+               The caller is required to allocate enough SMRAM for each CPU to
+               support the size of the custom SMI handler.
+**/
+UINTN
+EFIAPI
+SmmCpuFeaturesGetSmiHandlerSize (
+  VOID
+  )
+{
+  return 0;
+}
+
+/**
+  Install a custom SMI handler for the CPU specified by CpuIndex.  This function
+  is only called if SmmCpuFeaturesGetSmiHandlerSize() returns a size is greater
+  than zero and is called by the CPU that was elected as monarch during System
+  Management Mode initialization.
+
+  @param[in] CpuIndex   The index of the CPU to install the custom SMI handler.
+                        The value must be between 0 and the NumberOfCpus field
+                        in the System Management System Table (SMST).
+  @param[in] SmBase     The SMBASE address for the CPU specified by CpuIndex.
+  @param[in] SmiStack   The stack to use when an SMI is processed by the
+                        the CPU specified by CpuIndex.
+  @param[in] StackSize  The size, in bytes, if the stack used when an SMI is
+                        processed by the CPU specified by CpuIndex.
+  @param[in] GdtBase    The base address of the GDT to use when an SMI is
+                        processed by the CPU specified by CpuIndex.
+  @param[in] GdtSize    The size, in bytes, of the GDT used when an SMI is
+                        processed by the CPU specified by CpuIndex.
+  @param[in] IdtBase    The base address of the IDT to use when an SMI is
+                        processed by the CPU specified by CpuIndex.
+  @param[in] IdtSize    The size, in bytes, of the IDT used when an SMI is
+                        processed by the CPU specified by CpuIndex.
+  @param[in] Cr3        The base address of the page tables to use when an SMI
+                        is processed by the CPU specified by CpuIndex.
+**/
+VOID
+EFIAPI
+SmmCpuFeaturesInstallSmiHandler (
+  IN UINTN   CpuIndex,
+  IN UINT32  SmBase,
+  IN VOID    *SmiStack,
+  IN UINTN   StackSize,
+  IN UINTN   GdtBase,
+  IN UINTN   GdtSize,
+  IN UINTN   IdtBase,
+  IN UINTN   IdtSize,
+  IN UINT32  Cr3
+  )
+{
+}
+
+/**
+  Determines if MTRR registers must be configured to set SMRAM cache-ability
+  when executing in System Management Mode.
+
+  @retval TRUE   MTRR registers must be configured to set SMRAM cache-ability.
+  @retval FALSE  MTRR registers do not need to be configured to set SMRAM
+                 cache-ability.
+**/
+BOOLEAN
+EFIAPI
+SmmCpuFeaturesNeedConfigureMtrrs (
+  VOID
+  )
+{
+  return FALSE;
+}
+
+/**
+  Disable SMRR register if SMRR is supported and SmmCpuFeaturesNeedConfigureMtrrs()
+  returns TRUE.
+**/
+VOID
+EFIAPI
+SmmCpuFeaturesDisableSmrr (
+  VOID
+  )
+{
+}
+
+/**
+  Enable SMRR register if SMRR is supported and SmmCpuFeaturesNeedConfigureMtrrs()
+  returns TRUE.
+**/
+VOID
+EFIAPI
+SmmCpuFeaturesReenableSmrr (
+  VOID
+  )
+{
+}
+
+/**
+  Processor specific hook point each time a CPU enters System Management Mode.
+
+  @param[in] CpuIndex  The index of the CPU that has entered SMM.  The value
+                       must be between 0 and the NumberOfCpus field in the
+                       System Management System Table (SMST).
+**/
+VOID
+EFIAPI
+SmmCpuFeaturesRendezvousEntry (
+  IN UINTN  CpuIndex
+  )
+{
+}
+
+/**
+  Returns the current value of the SMM register for the specified CPU.
+  If the SMM register is not supported, then 0 is returned.
+
+  @param[in] CpuIndex  The index of the CPU to read the SMM register.  The
+                       value must be between 0 and the NumberOfCpus field in
+                       the System Management System Table (SMST).
+  @param[in] RegName   Identifies the SMM register to read.
+
+  @return  The value of the SMM register specified by RegName from the CPU
+           specified by CpuIndex.
+**/
+UINT64
+EFIAPI
+SmmCpuFeaturesGetSmmRegister (
+  IN UINTN         CpuIndex,
+  IN SMM_REG_NAME  RegName
+  )
+{
+  return 0;
+}
+
+/**
+  Sets the value of an SMM register on a specified CPU.
+  If the SMM register is not supported, then no action is performed.
+
+  @param[in] CpuIndex  The index of the CPU to write the SMM register.  The
+                       value must be between 0 and the NumberOfCpus field in
+                       the System Management System Table (SMST).
+  @param[in] RegName   Identifies the SMM register to write.
+                       registers are read-only.
+  @param[in] Value     The value to write to the SMM register.
+**/
+VOID
+EFIAPI
+SmmCpuFeaturesSetSmmRegister (
+  IN UINTN         CpuIndex,
+  IN SMM_REG_NAME  RegName,
+  IN UINT64        Value
+  )
+{
+}
+
+/**
+  Check to see if an SMM register is supported by a specified CPU.
+
+  @param[in] CpuIndex  The index of the CPU to check for SMM register support.
+                       The value must be between 0 and the NumberOfCpus field
+                       in the System Management System Table (SMST).
+  @param[in] RegName   Identifies the SMM register to check for support.
+
+  @retval TRUE   The SMM register specified by RegName is supported by the CPU
+                 specified by CpuIndex.
+  @retval FALSE  The SMM register specified by RegName is not supported by the
+                 CPU specified by CpuIndex.
+**/
+BOOLEAN
+EFIAPI
+SmmCpuFeaturesIsSmmRegisterSupported (
+  IN UINTN         CpuIndex,
+  IN SMM_REG_NAME  RegName
+  )
+{
+  return FALSE;
+}
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/SmmCpuFeaturesLib/AmdSmmCpuFeaturesLib.inf
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/SmmCpuFeaturesLib/AmdSmmCpuFeaturesLib.inf	(revision 101291)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/SmmCpuFeaturesLib/AmdSmmCpuFeaturesLib.inf	(revision 101291)
@@ -0,0 +1,38 @@
+## @file
+#  The CPU specific programming for PiSmmCpuDxeSmm module.
+#
+#  Copyright (c) 2009 - 2016, Intel Corporation. All rights reserved.<BR>
+#  Copyright (C) 2023 Advanced Micro Devices, Inc. All rights reserved.<BR>
+#  SPDX-License-Identifier: BSD-2-Clause-Patent
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = AmdSmmCpuFeaturesLib
+  MODULE_UNI_FILE                = SmmCpuFeaturesLib.uni
+  FILE_GUID                      = 5849E964-78EC-428E-8CBD-848A7E359134
+  MODULE_TYPE                    = DXE_SMM_DRIVER
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = SmmCpuFeaturesLib
+  CONSTRUCTOR                    = SmmCpuFeaturesLibConstructor
+
+[Sources]
+  SmmCpuFeaturesLib.c
+  SmmCpuFeaturesLibCommon.c
+  AmdSmmCpuFeaturesLib.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  UefiCpuPkg/UefiCpuPkg.dec
+
+[LibraryClasses]
+  BaseLib
+  PcdLib
+  MemoryAllocationLib
+  DebugLib
+  MmSaveStateLib
+
+[FeaturePcd]
+  gUefiCpuPkgTokenSpaceGuid.PcdSmrrEnable               ## CONSUMES
+  gUefiCpuPkgTokenSpaceGuid.PcdSmmFeatureControlEnable  ## CONSUMES
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/SmmCpuFeaturesLib/CpuFeaturesLib.h
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/SmmCpuFeaturesLib/CpuFeaturesLib.h	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/SmmCpuFeaturesLib/CpuFeaturesLib.h	(revision 101291)
@@ -10,4 +10,5 @@
 #define CPU_FEATURES_LIB_H_
 
+#include <Guid/SmmBaseHob.h>
 #include <Library/SmmCpuFeaturesLib.h>
 #include <Library/BaseLib.h>
@@ -15,4 +16,5 @@
 #include <Library/MemoryAllocationLib.h>
 #include <Library/DebugLib.h>
+#include <Library/HobLib.h>
 
 /**
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/SmmCpuFeaturesLib/IntelSmmCpuFeaturesLib.c
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/SmmCpuFeaturesLib/IntelSmmCpuFeaturesLib.c	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/SmmCpuFeaturesLib/IntelSmmCpuFeaturesLib.c	(revision 101291)
@@ -2,5 +2,5 @@
 Implementation shared across all library instances.
 
-Copyright (c) 2010 - 2019, Intel Corporation. All rights reserved.<BR>
+Copyright (c) 2010 - 2023, Intel Corporation. All rights reserved.<BR>
 Copyright (c) Microsoft Corporation.<BR>
 SPDX-License-Identifier: BSD-2-Clause-Patent
@@ -40,4 +40,10 @@
 
 //
+// Indicate SmBase for each Processors has been relocated or not. If TRUE,
+// means no need to do the relocation in SmmCpuFeaturesInitializeProcessor().
+//
+BOOLEAN  mSmmCpuFeaturesSmmRelocated;
+
+//
 // Set default value to assume MTRRs need to be configured on each SMI
 //
@@ -145,4 +151,10 @@
   mSmrrEnabled = (BOOLEAN *)AllocatePool (sizeof (BOOLEAN) * GetCpuMaxLogicalProcessorNumber ());
   ASSERT (mSmrrEnabled != NULL);
+
+  //
+  // If gSmmBaseHobGuid found, means SmBase info has been relocated and recorded
+  // in the SmBase array.
+  //
+  mSmmCpuFeaturesSmmRelocated = (BOOLEAN)(GetFirstGuidHob (&gSmmBaseHobGuid) != NULL);
 }
 
@@ -188,8 +200,13 @@
 
   //
-  // Configure SMBASE.
-  //
-  CpuState             = (SMRAM_SAVE_STATE_MAP *)(UINTN)(SMM_DEFAULT_SMBASE + SMRAM_SAVE_STATE_MAP_OFFSET);
-  CpuState->x86.SMBASE = (UINT32)CpuHotPlugData->SmBase[CpuIndex];
+  // No need to configure SMBASE if SmBase relocation has been done.
+  //
+  if (!mSmmCpuFeaturesSmmRelocated) {
+    //
+    // Configure SMBASE.
+    //
+    CpuState             = (SMRAM_SAVE_STATE_MAP *)(UINTN)(SMM_DEFAULT_SMBASE + SMRAM_SAVE_STATE_MAP_OFFSET);
+    CpuState->x86.SMBASE = (UINT32)CpuHotPlugData->SmBase[CpuIndex];
+  }
 
   //
@@ -401,2 +418,72 @@
   }
 }
+
+/**
+  This function updates the SMRAM save state on the currently executing CPU
+  to resume execution at a specific address after an RSM instruction.  This
+  function must evaluate the SMRAM save state to determine the execution mode
+  the RSM instruction resumes and update the resume execution address with
+  either NewInstructionPointer32 or NewInstructionPoint.  The auto HALT restart
+  flag in the SMRAM save state must always be cleared.  This function returns
+  the value of the instruction pointer from the SMRAM save state that was
+  replaced.  If this function returns 0, then the SMRAM save state was not
+  modified.
+
+  This function is called during the very first SMI on each CPU after
+  SmmCpuFeaturesInitializeProcessor() to set a flag in normal execution mode
+  to signal that the SMBASE of each CPU has been updated before the default
+  SMBASE address is used for the first SMI to the next CPU.
+
+  @param[in] CpuIndex                 The index of the CPU to hook.  The value
+                                      must be between 0 and the NumberOfCpus
+                                      field in the System Management System Table
+                                      (SMST).
+  @param[in] CpuState                 Pointer to SMRAM Save State Map for the
+                                      currently executing CPU.
+  @param[in] NewInstructionPointer32  Instruction pointer to use if resuming to
+                                      32-bit execution mode from 64-bit SMM.
+  @param[in] NewInstructionPointer    Instruction pointer to use if resuming to
+                                      same execution mode as SMM.
+
+  @retval 0    This function did modify the SMRAM save state.
+  @retval > 0  The original instruction pointer value from the SMRAM save state
+               before it was replaced.
+**/
+UINT64
+EFIAPI
+SmmCpuFeaturesHookReturnFromSmm (
+  IN UINTN                 CpuIndex,
+  IN SMRAM_SAVE_STATE_MAP  *CpuState,
+  IN UINT64                NewInstructionPointer32,
+  IN UINT64                NewInstructionPointer
+  )
+{
+  return 0;
+}
+
+/**
+  Check to see if an SMM register is supported by a specified CPU.
+
+  @param[in] CpuIndex  The index of the CPU to check for SMM register support.
+                       The value must be between 0 and the NumberOfCpus field
+                       in the System Management System Table (SMST).
+  @param[in] RegName   Identifies the SMM register to check for support.
+
+  @retval TRUE   The SMM register specified by RegName is supported by the CPU
+                 specified by CpuIndex.
+  @retval FALSE  The SMM register specified by RegName is not supported by the
+                 CPU specified by CpuIndex.
+**/
+BOOLEAN
+EFIAPI
+SmmCpuFeaturesIsSmmRegisterSupported (
+  IN UINTN         CpuIndex,
+  IN SMM_REG_NAME  RegName
+  )
+{
+  if (FeaturePcdGet (PcdSmmFeatureControlEnable) && (RegName == SmmRegFeatureControl)) {
+    return TRUE;
+  }
+
+  return FALSE;
+}
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/SmmCpuFeaturesLib/SmmCpuFeaturesLib.inf
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/SmmCpuFeaturesLib/SmmCpuFeaturesLib.inf	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/SmmCpuFeaturesLib/SmmCpuFeaturesLib.inf	(revision 101291)
@@ -2,5 +2,5 @@
 #  The CPU specific programming for PiSmmCpuDxeSmm module.
 #
-#  Copyright (c) 2009 - 2016, Intel Corporation. All rights reserved.<BR>
+#  Copyright (c) 2009 - 2023, Intel Corporation. All rights reserved.<BR>
 #  SPDX-License-Identifier: BSD-2-Clause-Patent
 #
@@ -34,4 +34,8 @@
   MemoryAllocationLib
   DebugLib
+  HobLib
+
+[Guids]
+  gSmmBaseHobGuid                ## CONSUMES
 
 [Pcd]
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/SmmCpuFeaturesLib/SmmCpuFeaturesLibCommon.c
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/SmmCpuFeaturesLib/SmmCpuFeaturesLibCommon.c	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/SmmCpuFeaturesLib/SmmCpuFeaturesLibCommon.c	(revision 101291)
@@ -17,47 +17,4 @@
 
 #include "CpuFeaturesLib.h"
-
-/**
-  This function updates the SMRAM save state on the currently executing CPU
-  to resume execution at a specific address after an RSM instruction.  This
-  function must evaluate the SMRAM save state to determine the execution mode
-  the RSM instruction resumes and update the resume execution address with
-  either NewInstructionPointer32 or NewInstructionPoint.  The auto HALT restart
-  flag in the SMRAM save state must always be cleared.  This function returns
-  the value of the instruction pointer from the SMRAM save state that was
-  replaced.  If this function returns 0, then the SMRAM save state was not
-  modified.
-
-  This function is called during the very first SMI on each CPU after
-  SmmCpuFeaturesInitializeProcessor() to set a flag in normal execution mode
-  to signal that the SMBASE of each CPU has been updated before the default
-  SMBASE address is used for the first SMI to the next CPU.
-
-  @param[in] CpuIndex                 The index of the CPU to hook.  The value
-                                      must be between 0 and the NumberOfCpus
-                                      field in the System Management System Table
-                                      (SMST).
-  @param[in] CpuState                 Pointer to SMRAM Save State Map for the
-                                      currently executing CPU.
-  @param[in] NewInstructionPointer32  Instruction pointer to use if resuming to
-                                      32-bit execution mode from 64-bit SMM.
-  @param[in] NewInstructionPointer    Instruction pointer to use if resuming to
-                                      same execution mode as SMM.
-
-  @retval 0    This function did modify the SMRAM save state.
-  @retval > 0  The original instruction pointer value from the SMRAM save state
-               before it was replaced.
-**/
-UINT64
-EFIAPI
-SmmCpuFeaturesHookReturnFromSmm (
-  IN UINTN                 CpuIndex,
-  IN SMRAM_SAVE_STATE_MAP  *CpuState,
-  IN UINT64                NewInstructionPointer32,
-  IN UINT64                NewInstructionPointer
-  )
-{
-  return 0;
-}
 
 /**
@@ -92,89 +49,4 @@
 
 /**
-  Check to see if an SMM register is supported by a specified CPU.
-
-  @param[in] CpuIndex  The index of the CPU to check for SMM register support.
-                       The value must be between 0 and the NumberOfCpus field
-                       in the System Management System Table (SMST).
-  @param[in] RegName   Identifies the SMM register to check for support.
-
-  @retval TRUE   The SMM register specified by RegName is supported by the CPU
-                 specified by CpuIndex.
-  @retval FALSE  The SMM register specified by RegName is not supported by the
-                 CPU specified by CpuIndex.
-**/
-BOOLEAN
-EFIAPI
-SmmCpuFeaturesIsSmmRegisterSupported (
-  IN UINTN         CpuIndex,
-  IN SMM_REG_NAME  RegName
-  )
-{
-  if (FeaturePcdGet (PcdSmmFeatureControlEnable) && (RegName == SmmRegFeatureControl)) {
-    return TRUE;
-  }
-
-  return FALSE;
-}
-
-/**
-  Read an SMM Save State register on the target processor.  If this function
-  returns EFI_UNSUPPORTED, then the caller is responsible for reading the
-  SMM Save Sate register.
-
-  @param[in]  CpuIndex  The index of the CPU to read the SMM Save State.  The
-                        value must be between 0 and the NumberOfCpus field in
-                        the System Management System Table (SMST).
-  @param[in]  Register  The SMM Save State register to read.
-  @param[in]  Width     The number of bytes to read from the CPU save state.
-  @param[out] Buffer    Upon return, this holds the CPU register value read
-                        from the save state.
-
-  @retval EFI_SUCCESS           The register was read from Save State.
-  @retval EFI_INVALID_PARAMETER  Buffer is NULL.
-  @retval EFI_UNSUPPORTED       This function does not support reading Register.
-
-**/
-EFI_STATUS
-EFIAPI
-SmmCpuFeaturesReadSaveStateRegister (
-  IN  UINTN                        CpuIndex,
-  IN  EFI_SMM_SAVE_STATE_REGISTER  Register,
-  IN  UINTN                        Width,
-  OUT VOID                         *Buffer
-  )
-{
-  return EFI_UNSUPPORTED;
-}
-
-/**
-  Writes an SMM Save State register on the target processor.  If this function
-  returns EFI_UNSUPPORTED, then the caller is responsible for writing the
-  SMM Save Sate register.
-
-  @param[in] CpuIndex  The index of the CPU to write the SMM Save State.  The
-                       value must be between 0 and the NumberOfCpus field in
-                       the System Management System Table (SMST).
-  @param[in] Register  The SMM Save State register to write.
-  @param[in] Width     The number of bytes to write to the CPU save state.
-  @param[in] Buffer    Upon entry, this holds the new CPU register value.
-
-  @retval EFI_SUCCESS           The register was written to Save State.
-  @retval EFI_INVALID_PARAMETER  Buffer is NULL.
-  @retval EFI_UNSUPPORTED       This function does not support writing Register.
-**/
-EFI_STATUS
-EFIAPI
-SmmCpuFeaturesWriteSaveStateRegister (
-  IN UINTN                        CpuIndex,
-  IN EFI_SMM_SAVE_STATE_REGISTER  Register,
-  IN UINTN                        Width,
-  IN CONST VOID                   *Buffer
-  )
-{
-  return EFI_UNSUPPORTED;
-}
-
-/**
   This function is hook point called after the gEfiSmmReadyToLockProtocolGuid
   notification is completely processed.
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/SmmCpuFeaturesLib/SmmCpuFeaturesLibStm.inf
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/SmmCpuFeaturesLib/SmmCpuFeaturesLibStm.inf	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/SmmCpuFeaturesLib/SmmCpuFeaturesLibStm.inf	(revision 101291)
@@ -3,5 +3,5 @@
 #  is included.
 #
-#  Copyright (c) 2009 - 2018, Intel Corporation. All rights reserved.<BR>
+#  Copyright (c) 2009 - 2023, Intel Corporation. All rights reserved.<BR>
 #  SPDX-License-Identifier: BSD-2-Clause-Patent
 #
@@ -65,4 +65,5 @@
   gEfiAcpi20TableGuid                      ## SOMETIMES_CONSUMES ## SystemTable
   gEfiAcpi10TableGuid                      ## SOMETIMES_CONSUMES ## SystemTable
+  gSmmBaseHobGuid                          ## CONSUMES
 
 [Pcd]
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/SmmCpuFeaturesLib/SmmStm.c
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/SmmCpuFeaturesLib/SmmStm.c	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/SmmCpuFeaturesLib/SmmStm.c	(revision 101291)
@@ -2,5 +2,5 @@
   SMM STM support functions
 
-  Copyright (c) 2015 - 2018, Intel Corporation. All rights reserved.<BR>
+  Copyright (c) 2015 - 2023, Intel Corporation. All rights reserved.<BR>
   SPDX-License-Identifier: BSD-2-Clause-Patent
 
@@ -9,5 +9,4 @@
 #include <PiMm.h>
 #include <Library/BaseMemoryLib.h>
-#include <Library/HobLib.h>
 #include <Library/UefiBootServicesTableLib.h>
 #include <Library/SmmServicesTableLib.h>
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/SmmCpuFeaturesLib/StandaloneMmCpuFeaturesLib.inf
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/SmmCpuFeaturesLib/StandaloneMmCpuFeaturesLib.inf	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Library/SmmCpuFeaturesLib/StandaloneMmCpuFeaturesLib.inf	(revision 101291)
@@ -2,5 +2,5 @@
 #  Standalone MM CPU specific programming.
 #
-#  Copyright (c) 2009 - 2016, Intel Corporation. All rights reserved.<BR>
+#  Copyright (c) 2009 - 2023, Intel Corporation. All rights reserved.<BR>
 #  Copyright (c) Microsoft Corporation.<BR>
 #  SPDX-License-Identifier: BSD-2-Clause-Patent
@@ -35,4 +35,8 @@
   MemoryAllocationLib
   PcdLib
+  HobLib
+
+[Guids]
+  gSmmBaseHobGuid                ## CONSUMES
 
 [FixedPcd]
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/MicrocodeMeasurementDxe/MicrocodeMeasurementDxe.c
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/MicrocodeMeasurementDxe/MicrocodeMeasurementDxe.c	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/MicrocodeMeasurementDxe/MicrocodeMeasurementDxe.c	(revision 101291)
@@ -239,5 +239,5 @@
       );
   } else {
-    DEBUG ((DEBUG_ERROR, "ERROR: TpmMeasureAndLogData failed with status %a!\n", Status));
+    DEBUG ((DEBUG_ERROR, "ERROR: TpmMeasureAndLogData failed with status %r!\n", Status));
   }
 
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/PiSmmCpuDxeSmm/CpuS3.c
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/PiSmmCpuDxeSmm/CpuS3.c	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/PiSmmCpuDxeSmm/CpuS3.c	(revision 101291)
@@ -2,5 +2,5 @@
 Code for Processor S3 restoration
 
-Copyright (c) 2006 - 2022, Intel Corporation. All rights reserved.<BR>
+Copyright (c) 2006 - 2023, Intel Corporation. All rights reserved.<BR>
 SPDX-License-Identifier: BSD-2-Clause-Patent
 
@@ -824,8 +824,21 @@
   }
 
-  //
-  // Restore SMBASE for BSP and all APs
-  //
-  SmmRelocateBases ();
+  DEBUG ((DEBUG_INFO, "SmmRestoreCpu: mSmmRelocated is %d\n", mSmmRelocated));
+
+  //
+  // Check whether Smm Relocation is done or not.
+  // If not, will do the SmmBases Relocation here!!!
+  //
+  if (!mSmmRelocated) {
+    //
+    // Restore SMBASE for BSP and all APs
+    //
+    SmmRelocateBases ();
+  } else {
+    //
+    // Issue SMI IPI (All Excluding  Self SMM IPI + BSP SMM IPI) to execute first SMI init.
+    //
+    ExecuteFirstSmiInit ();
+  }
 
   //
@@ -920,5 +933,5 @@
       DEBUG_ERROR,
       "ERROR:%a(): HOB(gEfiAcpiVariableGuid=%g) needed by S3 resume doesn't exist!\n",
-      __FUNCTION__,
+      __func__,
       &gEfiAcpiVariableGuid
       ));
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/PiSmmCpuDxeSmm/CpuService.c
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/PiSmmCpuDxeSmm/CpuService.c	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/PiSmmCpuDxeSmm/CpuService.c	(revision 101291)
@@ -2,5 +2,5 @@
 Implementation of SMM CPU Services Protocol.
 
-Copyright (c) 2011 - 2022, Intel Corporation. All rights reserved.<BR>
+Copyright (c) 2011 - 2023, Intel Corporation. All rights reserved.<BR>
 SPDX-License-Identifier: BSD-2-Clause-Patent
 
@@ -422,9 +422,16 @@
   }
 
-  //
-  // There are some APs outside SMM, Wait for all avaiable APs to arrive.
-  //
-  SmmWaitForApArrival ();
-  Status = mSmmMpSyncData->AllApArrivedWithException ? EFI_SUCCESS : EFI_TIMEOUT;
+  if ((mSmmMpSyncData->EffectiveSyncMode != SmmCpuSyncModeTradition) && !SmmCpuFeaturesNeedConfigureMtrrs ()) {
+    //
+    // There are some APs outside SMM, Wait for all avaiable APs to arrive.
+    //
+    SmmWaitForApArrival ();
+    Status = mSmmMpSyncData->AllApArrivedWithException ? EFI_SUCCESS : EFI_TIMEOUT;
+  } else {
+    //
+    // BSP has already waitted for APs to arrive SMM if SmmCpuSyncMode selected or need config MTRR.
+    //
+    Status = EFI_TIMEOUT;
+  }
 
 ON_EXIT:
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/PiSmmCpuDxeSmm/Ia32/PageTbl.c
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/PiSmmCpuDxeSmm/Ia32/PageTbl.c	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/PiSmmCpuDxeSmm/Ia32/PageTbl.c	(revision 101291)
@@ -2,5 +2,5 @@
 Page table manipulation functions for IA-32 processors
 
-Copyright (c) 2009 - 2019, Intel Corporation. All rights reserved.<BR>
+Copyright (c) 2009 - 2023, Intel Corporation. All rights reserved.<BR>
 Copyright (c) 2017, AMD Incorporated. All rights reserved.<BR>
 
@@ -32,4 +32,5 @@
 
   mPhysicalAddressBits = 32;
+  mPagingMode          = PagingPae;
 
   if (FeaturePcdGet (PcdCpuSmmProfileEnable) ||
@@ -63,5 +64,5 @@
   }
 
-  return Gen4GPageTable (TRUE);
+  return GenSmmPageTable (PagingPae, mPhysicalAddressBits);
 }
 
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/PiSmmCpuDxeSmm/Ia32/SmmFuncsArch.c
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/PiSmmCpuDxeSmm/Ia32/SmmFuncsArch.c	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/PiSmmCpuDxeSmm/Ia32/SmmFuncsArch.c	(revision 101291)
@@ -2,5 +2,5 @@
   SMM CPU misc functions for Ia32 arch specific.
 
-Copyright (c) 2015 - 2019, Intel Corporation. All rights reserved.<BR>
+Copyright (c) 2015 - 2023, Intel Corporation. All rights reserved.<BR>
 SPDX-License-Identifier: BSD-2-Clause-Patent
 
@@ -15,5 +15,4 @@
 
 extern BOOLEAN  mCetSupported;
-extern UINTN    mSmmShadowStackSize;
 
 X86_ASSEMBLY_PATCH_LABEL  mPatchCetPl0Ssp;
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/PiSmmCpuDxeSmm/Ia32/SmmProfileArch.c
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/PiSmmCpuDxeSmm/Ia32/SmmProfileArch.c	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/PiSmmCpuDxeSmm/Ia32/SmmProfileArch.c	(revision 101291)
@@ -19,5 +19,5 @@
   )
 {
-  mSmmS3ResumeState->SmmS3Cr3 = Gen4GPageTable (TRUE);
+  mSmmS3ResumeState->SmmS3Cr3 = GenSmmPageTable (PagingPae, mPhysicalAddressBits);
 
   return;
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/PiSmmCpuDxeSmm/MpService.c
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/PiSmmCpuDxeSmm/MpService.c	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/PiSmmCpuDxeSmm/MpService.c	(revision 101291)
@@ -2,5 +2,5 @@
 SMM MP service implementation
 
-Copyright (c) 2009 - 2022, Intel Corporation. All rights reserved.<BR>
+Copyright (c) 2009 - 2023, Intel Corporation. All rights reserved.<BR>
 Copyright (c) 2017, AMD Incorporated. All rights reserved.<BR>
 
@@ -30,6 +30,4 @@
 UINT32  *mPackageFirstThreadIndex = NULL;
 
-extern UINTN  mSmmShadowStackSize;
-
 /**
   Performs an atomic compare exchange operation to get semaphore.
@@ -351,4 +349,6 @@
   UINT32   DelayedCount;
   UINT32   BlockedCount;
+
+  PERF_FUNCTION_BEGIN ();
 
   DelayedCount = 0;
@@ -440,5 +440,5 @@
   }
 
-  return;
+  PERF_FUNCTION_END ();
 }
 
@@ -578,4 +578,6 @@
   ApCount = 0;
 
+  PERF_FUNCTION_BEGIN ();
+
   //
   // Flag BSP's presence
@@ -776,4 +778,13 @@
 
   //
+  // At this point, all APs should have exited from APHandler().
+  // Migrate the SMM MP performance logging to standard SMM performance logging.
+  // Any SMM MP performance logging after this point will be migrated in next SMI.
+  //
+  PERF_CODE (
+    MigrateMpPerf (gSmmCpuPrivate->SmmCoreEntryContext.NumberOfCpus, CpuIndex);
+    );
+
+  //
   // Reset the tokens buffer.
   //
@@ -793,4 +804,6 @@
   *mSmmMpSyncData->AllCpusInSync            = FALSE;
   mSmmMpSyncData->AllApArrivedWithException = FALSE;
+
+  PERF_FUNCTION_END ();
 }
 
@@ -1000,134 +1013,4 @@
   //
   ReleaseSemaphore (mSmmMpSyncData->CpuData[BspIndex].Run);
-}
-
-/**
-  Create 4G PageTable in SMRAM.
-
-  @param[in]      Is32BitPageTable Whether the page table is 32-bit PAE
-  @return         PageTable Address
-
-**/
-UINT32
-Gen4GPageTable (
-  IN      BOOLEAN  Is32BitPageTable
-  )
-{
-  VOID    *PageTable;
-  UINTN   Index;
-  UINT64  *Pte;
-  UINTN   PagesNeeded;
-  UINTN   Low2MBoundary;
-  UINTN   High2MBoundary;
-  UINTN   Pages;
-  UINTN   GuardPage;
-  UINT64  *Pdpte;
-  UINTN   PageIndex;
-  UINTN   PageAddress;
-
-  Low2MBoundary  = 0;
-  High2MBoundary = 0;
-  PagesNeeded    = 0;
-  if (FeaturePcdGet (PcdCpuSmmStackGuard)) {
-    //
-    // Add one more page for known good stack, then find the lower 2MB aligned address.
-    //
-    Low2MBoundary = (mSmmStackArrayBase + EFI_PAGE_SIZE) & ~(SIZE_2MB-1);
-    //
-    // Add two more pages for known good stack and stack guard page,
-    // then find the lower 2MB aligned address.
-    //
-    High2MBoundary = (mSmmStackArrayEnd - mSmmStackSize - mSmmShadowStackSize + EFI_PAGE_SIZE * 2) & ~(SIZE_2MB-1);
-    PagesNeeded    = ((High2MBoundary - Low2MBoundary) / SIZE_2MB) + 1;
-  }
-
-  //
-  // Allocate the page table
-  //
-  PageTable = AllocatePageTableMemory (5 + PagesNeeded);
-  ASSERT (PageTable != NULL);
-
-  PageTable = (VOID *)((UINTN)PageTable);
-  Pte       = (UINT64 *)PageTable;
-
-  //
-  // Zero out all page table entries first
-  //
-  ZeroMem (Pte, EFI_PAGES_TO_SIZE (1));
-
-  //
-  // Set Page Directory Pointers
-  //
-  for (Index = 0; Index < 4; Index++) {
-    Pte[Index] = ((UINTN)PageTable + EFI_PAGE_SIZE * (Index + 1)) | mAddressEncMask |
-                 (Is32BitPageTable ? IA32_PAE_PDPTE_ATTRIBUTE_BITS : PAGE_ATTRIBUTE_BITS);
-  }
-
-  Pte += EFI_PAGE_SIZE / sizeof (*Pte);
-
-  //
-  // Fill in Page Directory Entries
-  //
-  for (Index = 0; Index < EFI_PAGE_SIZE * 4 / sizeof (*Pte); Index++) {
-    Pte[Index] = (Index << 21) | mAddressEncMask | IA32_PG_PS | PAGE_ATTRIBUTE_BITS;
-  }
-
-  Pdpte = (UINT64 *)PageTable;
-  if (FeaturePcdGet (PcdCpuSmmStackGuard)) {
-    Pages     = (UINTN)PageTable + EFI_PAGES_TO_SIZE (5);
-    GuardPage = mSmmStackArrayBase + EFI_PAGE_SIZE;
-    for (PageIndex = Low2MBoundary; PageIndex <= High2MBoundary; PageIndex += SIZE_2MB) {
-      Pte                                             = (UINT64 *)(UINTN)(Pdpte[BitFieldRead32 ((UINT32)PageIndex, 30, 31)] & ~mAddressEncMask & ~(EFI_PAGE_SIZE - 1));
-      Pte[BitFieldRead32 ((UINT32)PageIndex, 21, 29)] = (UINT64)Pages | mAddressEncMask | PAGE_ATTRIBUTE_BITS;
-      //
-      // Fill in Page Table Entries
-      //
-      Pte         = (UINT64 *)Pages;
-      PageAddress = PageIndex;
-      for (Index = 0; Index < EFI_PAGE_SIZE / sizeof (*Pte); Index++) {
-        if (PageAddress == GuardPage) {
-          //
-          // Mark the guard page as non-present
-          //
-          Pte[Index] = PageAddress | mAddressEncMask;
-          GuardPage += (mSmmStackSize + mSmmShadowStackSize);
-          if (GuardPage > mSmmStackArrayEnd) {
-            GuardPage = 0;
-          }
-        } else {
-          Pte[Index] = PageAddress | mAddressEncMask | PAGE_ATTRIBUTE_BITS;
-        }
-
-        PageAddress += EFI_PAGE_SIZE;
-      }
-
-      Pages += EFI_PAGE_SIZE;
-    }
-  }
-
-  if ((PcdGet8 (PcdNullPointerDetectionPropertyMask) & BIT1) != 0) {
-    Pte = (UINT64 *)(UINTN)(Pdpte[0] & ~mAddressEncMask & ~(EFI_PAGE_SIZE - 1));
-    if ((Pte[0] & IA32_PG_PS) == 0) {
-      // 4K-page entries are already mapped. Just hide the first one anyway.
-      Pte     = (UINT64 *)(UINTN)(Pte[0] & ~mAddressEncMask & ~(EFI_PAGE_SIZE - 1));
-      Pte[0] &= ~(UINT64)IA32_PG_P; // Hide page 0
-    } else {
-      // Create 4K-page entries
-      Pages = (UINTN)AllocatePageTableMemory (1);
-      ASSERT (Pages != 0);
-
-      Pte[0] = (UINT64)(Pages | mAddressEncMask | PAGE_ATTRIBUTE_BITS);
-
-      Pte         = (UINT64 *)Pages;
-      PageAddress = 0;
-      Pte[0]      = PageAddress | mAddressEncMask; // Hide page 0 but present left
-      for (Index = 1; Index < EFI_PAGE_SIZE / sizeof (*Pte); Index++) {
-        PageAddress += EFI_PAGE_SIZE;
-        Pte[Index]   = PageAddress | mAddressEncMask | PAGE_ATTRIBUTE_BITS;
-      }
-    }
-  }
-
-  return (UINT32)(UINTN)PageTable;
 }
 
@@ -1724,4 +1607,8 @@
   ASSERT (CpuIndex < mMaxNumberOfCpus);
 
+  if (mSmmRelocated) {
+    ASSERT (mSmmInitialized != NULL);
+  }
+
   //
   // Save Cr2 because Page Fault exception in SMM may override its value,
@@ -1731,4 +1618,23 @@
   SaveCr2 (&Cr2);
 
+  if (mSmmRelocated && !mSmmInitialized[CpuIndex]) {
+    //
+    // Perform SmmInitHandler for CpuIndex
+    //
+    SmmInitHandler ();
+
+    //
+    // Restore Cr2
+    //
+    RestoreCr2 (Cr2);
+
+    //
+    // Mark the first SMI init for CpuIndex has been done so as to avoid the reentry.
+    //
+    mSmmInitialized[CpuIndex] = TRUE;
+
+    return;
+  }
+
   //
   // Call the user register Startup function first.
@@ -1741,10 +1647,22 @@
   // Perform CPU specific entry hooks
   //
+  PERF_CODE (
+    MpPerfBegin (CpuIndex, SMM_MP_PERF_PROCEDURE_ID (SmmRendezvousEntry));
+    );
   SmmCpuFeaturesRendezvousEntry (CpuIndex);
+  PERF_CODE (
+    MpPerfEnd (CpuIndex, SMM_MP_PERF_PROCEDURE_ID (SmmRendezvousEntry));
+    );
 
   //
   // Determine if this is a valid SMI
   //
+  PERF_CODE (
+    MpPerfBegin (CpuIndex, SMM_MP_PERF_PROCEDURE_ID (PlatformValidSmi));
+    );
   ValidSmi = PlatformValidSmi ();
+  PERF_CODE (
+    MpPerfEnd (CpuIndex, SMM_MP_PERF_PROCEDURE_ID (PlatformValidSmi));
+    );
 
   //
@@ -1876,5 +1794,18 @@
 
 Exit:
+  //
+  // Note: SmmRendezvousExit perf-logging entry is the only one that will be
+  //       migrated to standard perf-logging database in next SMI by BSPHandler().
+  //       Hence, the number of SmmRendezvousEntry entries will be larger than
+  //       the number of SmmRendezvousExit entries. Delta equals to the number
+  //       of CPU threads.
+  //
+  PERF_CODE (
+    MpPerfBegin (CpuIndex, SMM_MP_PERF_PROCEDURE_ID (SmmRendezvousExit));
+    );
   SmmCpuFeaturesRendezvousExit (CpuIndex);
+  PERF_CODE (
+    MpPerfEnd (CpuIndex, SMM_MP_PERF_PROCEDURE_ID (SmmRendezvousExit));
+    );
 
   //
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/PiSmmCpuDxeSmm/PiSmmCpuDxeSmm.c
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/PiSmmCpuDxeSmm/PiSmmCpuDxeSmm.c	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/PiSmmCpuDxeSmm/PiSmmCpuDxeSmm.c	(revision 101291)
@@ -2,6 +2,7 @@
 Agent Module to load other modules to deploy SMM Entry Vector for X86 CPU.
 
-Copyright (c) 2009 - 2019, Intel Corporation. All rights reserved.<BR>
+Copyright (c) 2009 - 2023, Intel Corporation. All rights reserved.<BR>
 Copyright (c) 2017, AMD Incorporated. All rights reserved.<BR>
+Copyright (C) 2023 Advanced Micro Devices, Inc. All rights reserved.<BR>
 
 SPDX-License-Identifier: BSD-2-Clause-Patent
@@ -60,5 +61,4 @@
 //
 volatile BOOLEAN  *mRebased;
-volatile BOOLEAN  mIsBsp;
 
 ///
@@ -85,4 +85,8 @@
 
 EFI_CPU_INTERRUPT_HANDLER  mExternalVectorTable[EXCEPTION_VECTOR_NUMBER];
+
+BOOLEAN           mSmmRelocated    = FALSE;
+volatile BOOLEAN  *mSmmInitialized = NULL;
+UINT32            mBspApicId       = 0;
 
 //
@@ -274,8 +278,5 @@
   }
 
-  Status = SmmCpuFeaturesReadSaveStateRegister (CpuIndex, Register, Width, Buffer);
-  if (Status == EFI_UNSUPPORTED) {
-    Status = ReadSaveStateRegister (CpuIndex, Register, Width, Buffer);
-  }
+  Status = MmSaveStateReadRegister (CpuIndex, Register, Width, Buffer);
 
   return Status;
@@ -326,8 +327,5 @@
   }
 
-  Status = SmmCpuFeaturesWriteSaveStateRegister (CpuIndex, Register, Width, Buffer);
-  if (Status == EFI_UNSUPPORTED) {
-    Status = WriteSaveStateRegister (CpuIndex, Register, Width, Buffer);
-  }
+  Status = MmSaveStateWriteRegister (CpuIndex, Register, Width, Buffer);
 
   return Status;
@@ -344,6 +342,7 @@
   )
 {
-  UINT32  ApicId;
-  UINTN   Index;
+  UINT32   ApicId;
+  UINTN    Index;
+  BOOLEAN  IsBsp;
 
   //
@@ -353,8 +352,13 @@
   ApicId = GetApicId ();
 
+  IsBsp = (BOOLEAN)(mBspApicId == ApicId);
+
   ASSERT (mNumberOfCpus <= mMaxNumberOfCpus);
 
   for (Index = 0; Index < mNumberOfCpus; Index++) {
     if (ApicId == (UINT32)gSmmCpuPrivate->ProcessorInfo[Index].ProcessorId) {
+      PERF_CODE (
+        MpPerfBegin (Index, SMM_MP_PERF_PROCEDURE_ID (SmmInitHandler));
+        );
       //
       // Initialize SMM specific features on the currently executing CPU
@@ -362,5 +366,5 @@
       SmmCpuFeaturesInitializeProcessor (
         Index,
-        mIsBsp,
+        IsBsp,
         gSmmCpuPrivate->ProcessorInfo,
         &mCpuHotPlugData
@@ -372,7 +376,5 @@
         //
         CheckFeatureSupported ();
-      }
-
-      if (mIsBsp) {
+      } else if (IsBsp) {
         //
         // BSP rebase is already done above.
@@ -382,8 +384,14 @@
       }
 
-      //
-      // Hook return after RSM to set SMM re-based flag
-      //
-      SemaphoreHook (Index, &mRebased[Index]);
+      if (!mSmmRelocated) {
+        //
+        // Hook return after RSM to set SMM re-based flag
+        //
+        SemaphoreHook (Index, &mRebased[Index]);
+      }
+
+      PERF_CODE (
+        MpPerfEnd (Index, SMM_MP_PERF_PROCEDURE_ID (SmmInitHandler));
+        );
 
       return;
@@ -392,4 +400,54 @@
 
   ASSERT (FALSE);
+}
+
+/**
+  Issue SMI IPI (All Excluding Self SMM IPI + BSP SMM IPI) to execute first SMI init.
+
+**/
+VOID
+ExecuteFirstSmiInit (
+  VOID
+  )
+{
+  UINTN  Index;
+
+  PERF_FUNCTION_BEGIN ();
+
+  if (mSmmInitialized == NULL) {
+    mSmmInitialized = (BOOLEAN *)AllocatePool (sizeof (BOOLEAN) * mMaxNumberOfCpus);
+  }
+
+  ASSERT (mSmmInitialized != NULL);
+  if (mSmmInitialized == NULL) {
+    PERF_FUNCTION_END ();
+    return;
+  }
+
+  //
+  // Reset the mSmmInitialized to false.
+  //
+  ZeroMem ((VOID *)mSmmInitialized, sizeof (BOOLEAN) * mMaxNumberOfCpus);
+
+  //
+  // Get the BSP ApicId.
+  //
+  mBspApicId = GetApicId ();
+
+  //
+  // Issue SMI IPI (All Excluding Self SMM IPI + BSP SMM IPI) for SMM init
+  //
+  SendSmiIpi (mBspApicId);
+  SendSmiIpiAllExcludingSelf ();
+
+  //
+  // Wait for all processors to finish its 1st SMI
+  //
+  for (Index = 0; Index < mNumberOfCpus; Index++) {
+    while (!(BOOLEAN)mSmmInitialized[Index]) {
+    }
+  }
+
+  PERF_FUNCTION_END ();
 }
 
@@ -410,7 +468,8 @@
   SMRAM_SAVE_STATE_MAP  *CpuStatePtr;
   UINT8                 *U8Ptr;
-  UINT32                ApicId;
   UINTN                 Index;
   UINTN                 BspIndex;
+
+  PERF_FUNCTION_BEGIN ();
 
   //
@@ -451,5 +510,5 @@
   // Retrieve the local APIC ID of current processor
   //
-  ApicId = GetApicId ();
+  mBspApicId = GetApicId ();
 
   //
@@ -457,9 +516,8 @@
   // This is APs' 1st SMI - rebase will be done here, and APs' default SMI handler will be overridden by gcSmmInitTemplate
   //
-  mIsBsp   = FALSE;
   BspIndex = (UINTN)-1;
   for (Index = 0; Index < mNumberOfCpus; Index++) {
     mRebased[Index] = FALSE;
-    if (ApicId != (UINT32)gSmmCpuPrivate->ProcessorInfo[Index].ProcessorId) {
+    if (mBspApicId != (UINT32)gSmmCpuPrivate->ProcessorInfo[Index].ProcessorId) {
       SendSmiIpi ((UINT32)gSmmCpuPrivate->ProcessorInfo[Index].ProcessorId);
       //
@@ -480,6 +538,5 @@
   //
   ASSERT (BspIndex != (UINTN)-1);
-  mIsBsp = TRUE;
-  SendSmiIpi (ApicId);
+  SendSmiIpi (mBspApicId);
   //
   // Wait for the BSP to finish its 1st SMI
@@ -493,4 +550,5 @@
   CopyMem (CpuStatePtr, &BakBuf2, sizeof (BakBuf2));
   CopyMem (U8Ptr, BakBuf, sizeof (BakBuf));
+  PERF_FUNCTION_END ();
 }
 
@@ -564,4 +622,11 @@
   UINTN                     ModelId;
   UINT32                    Cr3;
+  EFI_HOB_GUID_TYPE         *GuidHob;
+  SMM_BASE_HOB_DATA         *SmmBaseHobData;
+
+  GuidHob        = NULL;
+  SmmBaseHobData = NULL;
+
+  PERF_FUNCTION_BEGIN ();
 
   //
@@ -636,4 +701,8 @@
 
   gSmmCpuPrivate->SmmCoreEntryContext.NumberOfCpus = mMaxNumberOfCpus;
+
+  PERF_CODE (
+    InitializeMpPerf (gSmmCpuPrivate->SmmCoreEntryContext.NumberOfCpus);
+    );
 
   //
@@ -792,24 +861,48 @@
 
   //
-  // Allocate buffer for all of the tiles.
-  //
-  // Intel(R) 64 and IA-32 Architectures Software Developer's Manual
-  // Volume 3C, Section 34.11 SMBASE Relocation
-  //   For Pentium and Intel486 processors, the SMBASE values must be
-  //   aligned on a 32-KByte boundary or the processor will enter shutdown
-  //   state during the execution of a RSM instruction.
-  //
-  // Intel486 processors: FamilyId is 4
-  // Pentium processors : FamilyId is 5
-  //
-  BufferPages = EFI_SIZE_TO_PAGES (SIZE_32KB + TileSize * (mMaxNumberOfCpus - 1));
-  if ((FamilyId == 4) || (FamilyId == 5)) {
-    Buffer = AllocateAlignedCodePages (BufferPages, SIZE_32KB);
+  // Retrive the allocated SmmBase from gSmmBaseHobGuid. If found,
+  // means the SmBase relocation has been done.
+  //
+  GuidHob = GetFirstGuidHob (&gSmmBaseHobGuid);
+  if (GuidHob != NULL) {
+    //
+    // Check whether the Required TileSize is enough.
+    //
+    if (TileSize > SIZE_8KB) {
+      DEBUG ((DEBUG_ERROR, "The Range of Smbase in SMRAM is not enough -- Required TileSize = 0x%08x, Actual TileSize = 0x%08x\n", TileSize, SIZE_8KB));
+      CpuDeadLoop ();
+      return RETURN_BUFFER_TOO_SMALL;
+    }
+
+    SmmBaseHobData = GET_GUID_HOB_DATA (GuidHob);
+
+    //
+    // Assume single instance of HOB produced, expect the HOB.NumberOfProcessors equals to the mMaxNumberOfCpus.
+    //
+    ASSERT (SmmBaseHobData->NumberOfProcessors == (UINT32)mMaxNumberOfCpus && SmmBaseHobData->ProcessorIndex == 0);
+    mSmmRelocated = TRUE;
   } else {
-    Buffer = AllocateAlignedCodePages (BufferPages, SIZE_4KB);
-  }
-
-  ASSERT (Buffer != NULL);
-  DEBUG ((DEBUG_INFO, "SMRAM SaveState Buffer (0x%08x, 0x%08x)\n", Buffer, EFI_PAGES_TO_SIZE (BufferPages)));
+    //
+    // When the HOB doesn't exist, allocate new SMBASE itself.
+    //
+    DEBUG ((DEBUG_INFO, "PiCpuSmmEntry: gSmmBaseHobGuid not found!\n"));
+    //
+    // very old processors (i486 + pentium) need 32k not 4k alignment, exclude them.
+    //
+    ASSERT (FamilyId >= 6);
+    //
+    // Allocate buffer for all of the tiles.
+    //
+    BufferPages = EFI_SIZE_TO_PAGES (SIZE_32KB + TileSize * (mMaxNumberOfCpus - 1));
+    Buffer      = AllocateAlignedCodePages (BufferPages, SIZE_4KB);
+    if (Buffer == NULL) {
+      DEBUG ((DEBUG_ERROR, "Failed to allocate %Lu pages.\n", (UINT64)BufferPages));
+      CpuDeadLoop ();
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    ASSERT (Buffer != NULL);
+    DEBUG ((DEBUG_INFO, "New Allcoated SMRAM SaveState Buffer (0x%08x, 0x%08x)\n", Buffer, EFI_PAGES_TO_SIZE (BufferPages)));
+  }
 
   //
@@ -846,5 +939,6 @@
   //
   for (Index = 0; Index < mMaxNumberOfCpus; Index++) {
-    mCpuHotPlugData.SmBase[Index]           = (UINTN)Buffer + Index * TileSize - SMM_HANDLER_OFFSET;
+    mCpuHotPlugData.SmBase[Index] = mSmmRelocated ? (UINTN)SmmBaseHobData->SmBase[Index] : (UINTN)Buffer + Index * TileSize - SMM_HANDLER_OFFSET;
+
     gSmmCpuPrivate->CpuSaveStateSize[Index] = sizeof (SMRAM_SAVE_STATE_MAP);
     gSmmCpuPrivate->CpuSaveState[Index]     = (VOID *)(mCpuHotPlugData.SmBase[Index] + SMRAM_SAVE_STATE_MAP_OFFSET);
@@ -852,5 +946,5 @@
 
     if (Index < mNumberOfCpus) {
-      Status = MpServices->GetProcessorInfo (MpServices, Index, &gSmmCpuPrivate->ProcessorInfo[Index]);
+      Status = MpServices->GetProcessorInfo (MpServices, Index | CPU_V2_EXTENDED_TOPOLOGY, &gSmmCpuPrivate->ProcessorInfo[Index]);
       ASSERT_EFI_ERROR (Status);
       mCpuHotPlugData.ApicId[Index] = gSmmCpuPrivate->ProcessorInfo[Index].ProcessorId;
@@ -959,15 +1053,21 @@
 
   //
-  // Relocate SMM Base addresses to the ones allocated from SMRAM
-  //
-  mRebased = (BOOLEAN *)AllocateZeroPool (sizeof (BOOLEAN) * mMaxNumberOfCpus);
-  ASSERT (mRebased != NULL);
-  SmmRelocateBases ();
-
-  //
-  // Call hook for BSP to perform extra actions in normal mode after all
-  // SMM base addresses have been relocated on all CPUs
-  //
-  SmmCpuFeaturesSmmRelocationComplete ();
+  // Check whether Smm Relocation is done or not.
+  // If not, will do the SmmBases Relocation here!!!
+  //
+  if (!mSmmRelocated) {
+    //
+    // Relocate SMM Base addresses to the ones allocated from SMRAM
+    //
+    mRebased = (BOOLEAN *)AllocateZeroPool (sizeof (BOOLEAN) * mMaxNumberOfCpus);
+    ASSERT (mRebased != NULL);
+    SmmRelocateBases ();
+
+    //
+    // Call hook for BSP to perform extra actions in normal mode after all
+    // SMM base addresses have been relocated on all CPUs
+    //
+    SmmCpuFeaturesSmmRelocationComplete ();
+  }
 
   DEBUG ((DEBUG_INFO, "mXdSupported - 0x%x\n", mXdSupported));
@@ -991,11 +1091,30 @@
         );
       if (FeaturePcdGet (PcdCpuSmmStackGuard)) {
-        SetNotPresentPage (
+        ConvertMemoryPageAttributes (
           Cr3,
+          mPagingMode,
           (EFI_PHYSICAL_ADDRESS)(UINTN)Stacks + mSmmStackSize + EFI_PAGES_TO_SIZE (1) + (mSmmStackSize + mSmmShadowStackSize) * Index,
-          EFI_PAGES_TO_SIZE (1)
+          EFI_PAGES_TO_SIZE (1),
+          EFI_MEMORY_RP,
+          TRUE,
+          NULL
           );
       }
     }
+  }
+
+  //
+  // For relocated SMBASE, some MSRs & CSRs are still required to be configured in SMM Mode for SMM Initialization.
+  // Those MSRs & CSRs must be configured before normal SMI sources happen.
+  // So, here is to issue SMI IPI (All Excluding  Self SMM IPI + BSP SMM IPI) to execute first SMI init.
+  //
+  if (mSmmRelocated) {
+    ExecuteFirstSmiInit ();
+
+    //
+    // Call hook for BSP to perform extra actions in normal mode after all
+    // SMM base addresses have been relocated on all CPUs
+    //
+    SmmCpuFeaturesSmmRelocationComplete ();
   }
 
@@ -1096,5 +1215,32 @@
   DEBUG ((DEBUG_INFO, "SMM CPU Module exit from SMRAM with EFI_SUCCESS\n"));
 
+  PERF_FUNCTION_END ();
   return EFI_SUCCESS;
+}
+
+/**
+  Function to compare 2 EFI_SMRAM_DESCRIPTOR based on CpuStart.
+
+  @param[in] Buffer1            pointer to Device Path poiner to compare
+  @param[in] Buffer2            pointer to second DevicePath pointer to compare
+
+  @retval 0                     Buffer1 equal to Buffer2
+  @retval <0                    Buffer1 is less than Buffer2
+  @retval >0                    Buffer1 is greater than Buffer2
+**/
+INTN
+EFIAPI
+CpuSmramRangeCompare (
+  IN  CONST VOID  *Buffer1,
+  IN  CONST VOID  *Buffer2
+  )
+{
+  if (((EFI_SMRAM_DESCRIPTOR *)Buffer1)->CpuStart > ((EFI_SMRAM_DESCRIPTOR *)Buffer2)->CpuStart) {
+    return 1;
+  } else if (((EFI_SMRAM_DESCRIPTOR *)Buffer1)->CpuStart < ((EFI_SMRAM_DESCRIPTOR *)Buffer2)->CpuStart) {
+    return -1;
+  }
+
+  return 0;
 }
 
@@ -1120,4 +1266,5 @@
   UINT64                    MaxSize;
   BOOLEAN                   Found;
+  EFI_SMRAM_DESCRIPTOR      SmramDescriptor;
 
   //
@@ -1141,4 +1288,9 @@
 
   mSmmCpuSmramRangeCount = Size / sizeof (EFI_SMRAM_DESCRIPTOR);
+
+  //
+  // Sort the mSmmCpuSmramRanges
+  //
+  QuickSort (mSmmCpuSmramRanges, mSmmCpuSmramRangeCount, sizeof (EFI_SMRAM_DESCRIPTOR), (BASE_SORT_COMPARE)CpuSmramRangeCompare, &SmramDescriptor);
 
   //
@@ -1250,4 +1402,6 @@
   EFI_STATUS  Status;
 
+  PERF_FUNCTION_BEGIN ();
+
   //
   // Check to see if the Feature Control MSR is supported on this CPU
@@ -1256,4 +1410,5 @@
   if (!SmmCpuFeaturesIsSmmRegisterSupported (Index, SmmRegFeatureControl)) {
     mSmmCodeAccessCheckEnable = FALSE;
+    PERF_FUNCTION_END ();
     return;
   }
@@ -1265,4 +1420,5 @@
   if ((AsmReadMsr64 (EFI_MSR_SMM_MCA_CAP) & SMM_CODE_ACCESS_CHK_BIT) == 0) {
     mSmmCodeAccessCheckEnable = FALSE;
+    PERF_FUNCTION_END ();
     return;
   }
@@ -1321,4 +1477,6 @@
     }
   }
+
+  PERF_FUNCTION_END ();
 }
 
@@ -1442,4 +1600,19 @@
 {
   if (mSmmReadyToLock) {
+    PERF_FUNCTION_BEGIN ();
+
+    //
+    // Check if all Aps enter SMM. In Relaxed-AP Sync Mode, BSP will not wait for
+    // all Aps arrive. However,PerformRemainingTasks() needs to wait all Aps arrive before calling
+    // SetMemMapAttributes() and ConfigSmmCodeAccessCheck() when mSmmReadyToLock
+    // is true. In SetMemMapAttributes(), SmmSetMemoryAttributesEx() will call
+    // FlushTlbForAll() that need to start up the aps. So it need to let all
+    // aps arrive. Same as SetMemMapAttributes(), ConfigSmmCodeAccessCheck()
+    // also will start up the aps.
+    //
+    if (EFI_ERROR (SmmCpuRendezvous (NULL, TRUE))) {
+      DEBUG ((DEBUG_ERROR, "PerformRemainingTasks: fail to wait for all AP check in SMM!\n"));
+    }
+
     //
     // Start SMM Profile feature
@@ -1476,5 +1649,11 @@
     ConfigSmmCodeAccessCheck ();
 
+    //
+    // Measure performance of SmmCpuFeaturesCompleteSmmReadyToLock() from caller side
+    // as the implementation is provided by platform.
+    //
+    PERF_START (NULL, "SmmCompleteReadyToLock", NULL, 0);
     SmmCpuFeaturesCompleteSmmReadyToLock ();
+    PERF_END (NULL, "SmmCompleteReadyToLock", NULL, 0);
 
     //
@@ -1482,4 +1661,6 @@
     //
     mSmmReadyToLock = FALSE;
+
+    PERF_FUNCTION_END ();
   }
 }
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/PiSmmCpuDxeSmm/PiSmmCpuDxeSmm.h
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/PiSmmCpuDxeSmm/PiSmmCpuDxeSmm.h	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/PiSmmCpuDxeSmm/PiSmmCpuDxeSmm.h	(revision 101291)
@@ -2,6 +2,7 @@
 Agent Module to load other modules to deploy SMM Entry Vector for X86 CPU.
 
-Copyright (c) 2009 - 2022, Intel Corporation. All rights reserved.<BR>
+Copyright (c) 2009 - 2023, Intel Corporation. All rights reserved.<BR>
 Copyright (c) 2017, AMD Incorporated. All rights reserved.<BR>
+Copyright (C) 2023 Advanced Micro Devices, Inc. All rights reserved.<BR>
 
 SPDX-License-Identifier: BSD-2-Clause-Patent
@@ -26,4 +27,5 @@
 #include <Guid/MemoryAttributesTable.h>
 #include <Guid/PiSmmMemoryAttributesTable.h>
+#include <Guid/SmmBaseHob.h>
 
 #include <Library/BaseLib.h>
@@ -45,5 +47,4 @@
 #include <Library/LocalApicLib.h>
 #include <Library/CpuLib.h>
-#include <Library/UefiCpuLib.h>
 #include <Library/CpuExceptionHandlerLib.h>
 #include <Library/ReportStatusCodeLib.h>
@@ -51,4 +52,7 @@
 #include <Library/PeCoffGetEntryPointLib.h>
 #include <Library/RegisterCpuFeaturesLib.h>
+#include <Library/PerformanceLib.h>
+#include <Library/CpuPageTableLib.h>
+#include <Library/MmSaveStateLib.h>
 
 #include <AcpiCpuData.h>
@@ -60,4 +64,5 @@
 #include "CpuService.h"
 #include "SmmProfile.h"
+#include "SmmMpPerf.h"
 
 //
@@ -261,4 +266,6 @@
 extern EFI_MM_MP_PROTOCOL    mSmmMp;
 extern BOOLEAN               m5LevelPagingNeeded;
+extern PAGING_MODE           mPagingMode;
+extern UINTN                 mSmmShadowStackSize;
 
 ///
@@ -350,56 +357,25 @@
 
 /**
-Read a CPU Save State register on the target processor.
-
-This function abstracts the differences that whether the CPU Save State register is in the
-IA32 CPU Save State Map or X64 CPU Save State Map.
-
-This function supports reading a CPU Save State register in SMBase relocation handler.
-
-@param[in]  CpuIndex       Specifies the zero-based index of the CPU save state.
-@param[in]  RegisterIndex  Index into mSmmCpuWidthOffset[] look up table.
-@param[in]  Width          The number of bytes to read from the CPU save state.
-@param[out] Buffer         Upon return, this holds the CPU register value read from the save state.
-
-@retval EFI_SUCCESS           The register was read from Save State.
-@retval EFI_NOT_FOUND         The register is not defined for the Save State of Processor.
-@retval EFI_INVALID_PARAMETER Buffer is NULL, or Width does not meet requirement per Register type.
-
-**/
-EFI_STATUS
-EFIAPI
-ReadSaveStateRegister (
-  IN UINTN                        CpuIndex,
-  IN EFI_SMM_SAVE_STATE_REGISTER  Register,
-  IN UINTN                        Width,
-  OUT VOID                        *Buffer
-  );
-
-/**
-Write value to a CPU Save State register on the target processor.
-
-This function abstracts the differences that whether the CPU Save State register is in the
-IA32 CPU Save State Map or X64 CPU Save State Map.
-
-This function supports writing a CPU Save State register in SMBase relocation handler.
-
-@param[in] CpuIndex       Specifies the zero-based index of the CPU save state.
-@param[in] RegisterIndex  Index into mSmmCpuWidthOffset[] look up table.
-@param[in] Width          The number of bytes to read from the CPU save state.
-@param[in] Buffer         Upon entry, this holds the new CPU register value.
-
-@retval EFI_SUCCESS           The register was written to Save State.
-@retval EFI_NOT_FOUND         The register is not defined for the Save State of Processor.
-@retval EFI_INVALID_PARAMETER  ProcessorIndex or Width is not correct.
-
-**/
-EFI_STATUS
-EFIAPI
-WriteSaveStateRegister (
-  IN UINTN                        CpuIndex,
-  IN EFI_SMM_SAVE_STATE_REGISTER  Register,
-  IN UINTN                        Width,
-  IN CONST VOID                   *Buffer
-  );
+  C function for SMI handler. To change all processor's SMMBase Register.
+
+**/
+VOID
+EFIAPI
+SmmInitHandler (
+  VOID
+  );
+
+/**
+  Issue SMI IPI (All Excluding Self SMM IPI + BSP SMM IPI) to execute first SMI init.
+
+**/
+VOID
+ExecuteFirstSmiInit (
+  VOID
+  );
+
+extern BOOLEAN            mSmmRelocated;
+extern volatile  BOOLEAN  *mSmmInitialized;
+extern UINT32             mBspApicId;
 
 extern CONST UINT8        gcSmmInitTemplate[];
@@ -529,4 +505,19 @@
 
 /**
+  Create page table based on input PagingMode and PhysicalAddressBits in smm.
+
+  @param[in]      PagingMode           The paging mode.
+  @param[in]      PhysicalAddressBits  The bits of physical address to map.
+
+  @retval         PageTable Address
+
+**/
+UINTN
+GenSmmPageTable (
+  IN PAGING_MODE  PagingMode,
+  IN UINT8        PhysicalAddressBits
+  );
+
+/**
   Initialize global data for MP synchronization.
 
@@ -683,4 +674,41 @@
   IN      UINTN             CpuIndex,
   IN OUT  VOID              *ProcArguments OPTIONAL
+  );
+
+/**
+  This function modifies the page attributes for the memory region specified by BaseAddress and
+  Length from their current attributes to the attributes specified by Attributes.
+
+  Caller should make sure BaseAddress and Length is at page boundary.
+
+  @param[in]   PageTableBase    The page table base.
+  @param[in]   BaseAddress      The physical address that is the start address of a memory region.
+  @param[in]   Length           The size in bytes of the memory region.
+  @param[in]   Attributes       The bit mask of attributes to modify for the memory region.
+  @param[in]   IsSet            TRUE means to set attributes. FALSE means to clear attributes.
+  @param[out]  IsModified       TRUE means page table modified. FALSE means page table not modified.
+
+  @retval RETURN_SUCCESS           The attributes were modified for the memory region.
+  @retval RETURN_ACCESS_DENIED     The attributes for the memory resource range specified by
+                                   BaseAddress and Length cannot be modified.
+  @retval RETURN_INVALID_PARAMETER Length is zero.
+                                   Attributes specified an illegal combination of attributes that
+                                   cannot be set together.
+  @retval RETURN_OUT_OF_RESOURCES  There are not enough system resources to modify the attributes of
+                                   the memory resource range.
+  @retval RETURN_UNSUPPORTED       The processor does not support one or more bytes of the memory
+                                   resource range specified by BaseAddress and Length.
+                                   The bit mask of attributes is not support for the memory resource
+                                   range specified by BaseAddress and Length.
+**/
+RETURN_STATUS
+ConvertMemoryPageAttributes (
+  IN  UINTN             PageTableBase,
+  IN  PAGING_MODE       PagingMode,
+  IN  PHYSICAL_ADDRESS  BaseAddress,
+  IN  UINT64            Length,
+  IN  UINT64            Attributes,
+  IN  BOOLEAN           IsSet,
+  OUT BOOLEAN           *IsModified   OPTIONAL
   );
 
@@ -986,9 +1014,8 @@
 
   @param[in]   PageTableBase    The page table base.
-  @param[in]   EnablePML5Paging If PML5 paging is enabled.
+  @param[in]   PagingMode       The paging mode.
   @param[in]   BaseAddress      The physical address that is the start address of a memory region.
   @param[in]   Length           The size in bytes of the memory region.
   @param[in]   Attributes       The bit mask of attributes to set for the memory region.
-  @param[out]  IsSplitted       TRUE means page table splitted. FALSE means page table not splitted.
 
   @retval EFI_SUCCESS           The attributes were set for the memory region.
@@ -1008,10 +1035,9 @@
 EFI_STATUS
 SmmSetMemoryAttributesEx (
-  IN  UINTN                 PageTableBase,
-  IN  BOOLEAN               EnablePML5Paging,
-  IN  EFI_PHYSICAL_ADDRESS  BaseAddress,
-  IN  UINT64                Length,
-  IN  UINT64                Attributes,
-  OUT BOOLEAN               *IsSplitted  OPTIONAL
+  IN  UINTN             PageTableBase,
+  IN  PAGING_MODE       PagingMode,
+  IN  PHYSICAL_ADDRESS  BaseAddress,
+  IN  UINT64            Length,
+  IN  UINT64            Attributes
   );
 
@@ -1021,9 +1047,8 @@
 
   @param[in]   PageTableBase    The page table base.
-  @param[in]   EnablePML5Paging If PML5 paging is enabled.
+  @param[in]   PagingMode       The paging mode.
   @param[in]   BaseAddress      The physical address that is the start address of a memory region.
   @param[in]   Length           The size in bytes of the memory region.
   @param[in]   Attributes       The bit mask of attributes to clear for the memory region.
-  @param[out]  IsSplitted       TRUE means page table splitted. FALSE means page table not splitted.
 
   @retval EFI_SUCCESS           The attributes were cleared for the memory region.
@@ -1032,10 +1057,10 @@
   @retval EFI_INVALID_PARAMETER Length is zero.
                                 Attributes specified an illegal combination of attributes that
-                                cannot be set together.
+                                cannot be cleared together.
   @retval EFI_OUT_OF_RESOURCES  There are not enough system resources to modify the attributes of
                                 the memory resource range.
   @retval EFI_UNSUPPORTED       The processor does not support one or more bytes of the memory
                                 resource range specified by BaseAddress and Length.
-                                The bit mask of attributes is not support for the memory resource
+                                The bit mask of attributes is not supported for the memory resource
                                 range specified by BaseAddress and Length.
 
@@ -1044,9 +1069,8 @@
 SmmClearMemoryAttributesEx (
   IN  UINTN                 PageTableBase,
-  IN  BOOLEAN               EnablePML5Paging,
+  IN  PAGING_MODE           PagingMode,
   IN  EFI_PHYSICAL_ADDRESS  BaseAddress,
   IN  UINT64                Length,
-  IN  UINT64                Attributes,
-  OUT BOOLEAN               *IsSplitted  OPTIONAL
+  IN  UINT64                Attributes
   );
 
@@ -1169,20 +1193,4 @@
 EFI_STATUS
 SetShadowStack (
-  IN  UINTN                 Cr3,
-  IN  EFI_PHYSICAL_ADDRESS  BaseAddress,
-  IN  UINT64                Length
-  );
-
-/**
-  Set not present memory.
-
-  @param[in]  Cr3              The page table base address.
-  @param[in]  BaseAddress      The physical address that is the start address of a memory region.
-  @param[in]  Length           The size in bytes of the memory region.
-
-  @retval EFI_SUCCESS           The not present memory is set.
-**/
-EFI_STATUS
-SetNotPresentPage (
   IN  UINTN                 Cr3,
   IN  EFI_PHYSICAL_ADDRESS  BaseAddress,
@@ -1545,3 +1553,27 @@
   );
 
+/**
+  Disable Write Protect on pages marked as read-only if Cr0.Bits.WP is 1.
+
+  @param[out]  WpEnabled      If Cr0.WP is enabled.
+  @param[out]  CetEnabled     If CET is enabled.
+**/
+VOID
+DisableReadOnlyPageWriteProtect (
+  OUT BOOLEAN  *WpEnabled,
+  OUT BOOLEAN  *CetEnabled
+  );
+
+/**
+  Enable Write Protect on pages marked as read-only.
+
+  @param[out]  WpEnabled      If Cr0.WP should be enabled.
+  @param[out]  CetEnabled     If CET should be enabled.
+**/
+VOID
+EnableReadOnlyPageWriteProtect (
+  BOOLEAN  WpEnabled,
+  BOOLEAN  CetEnabled
+  );
+
 #endif
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/PiSmmCpuDxeSmm/PiSmmCpuDxeSmm.inf
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/PiSmmCpuDxeSmm/PiSmmCpuDxeSmm.inf	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/PiSmmCpuDxeSmm/PiSmmCpuDxeSmm.inf	(revision 101291)
@@ -5,6 +5,7 @@
 # provides CPU specific services in SMM.
 #
-# Copyright (c) 2009 - 2022, Intel Corporation. All rights reserved.<BR>
+# Copyright (c) 2009 - 2023, Intel Corporation. All rights reserved.<BR>
 # Copyright (c) 2017, AMD Incorporated. All rights reserved.<BR>
+# Copyright (C) 2023 Advanced Micro Devices, Inc. All rights reserved.<BR>
 #
 # SPDX-License-Identifier: BSD-2-Clause-Patent
@@ -43,4 +44,6 @@
   SmmMp.h
   SmmMp.c
+  SmmMpPerf.h
+  SmmMpPerf.c
 
 [Sources.Ia32]
@@ -90,5 +93,4 @@
   PciLib
   LocalApicLib
-  UefiCpuLib
   SmmCpuPlatformHookLib
   CpuExceptionHandlerLib
@@ -99,4 +101,7 @@
   SmmCpuFeaturesLib
   PeCoffGetEntryPointLib
+  PerformanceLib
+  CpuPageTableLib
+  MmSaveStateLib
 
 [Protocols]
@@ -115,4 +120,5 @@
   gEdkiiPiSmmMemoryAttributesTableGuid     ## CONSUMES ## SystemTable
   gEfiMemoryAttributesTableGuid            ## CONSUMES ## SystemTable
+  gSmmBaseHobGuid                          ## CONSUMES
 
 [FeaturePcd]
@@ -126,4 +132,5 @@
   gUefiCpuPkgTokenSpaceGuid.PcdCpuSmmFeatureControlMsrLock         ## CONSUMES
   gEfiMdeModulePkgTokenSpaceGuid.PcdDxeIplSwitchToLongMode         ## CONSUMES
+  gUefiCpuPkgTokenSpaceGuid.PcdSmmApPerfLogEnable                  ## CONSUMES
 
 [Pcd]
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/PiSmmCpuDxeSmm/SmmCpuMemoryManagement.c
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/PiSmmCpuDxeSmm/SmmCpuMemoryManagement.c	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/PiSmmCpuDxeSmm/SmmCpuMemoryManagement.c	(revision 101291)
@@ -1,5 +1,5 @@
 /** @file
 
-Copyright (c) 2016 - 2019, Intel Corporation. All rights reserved.<BR>
+Copyright (c) 2016 - 2023, Intel Corporation. All rights reserved.<BR>
 SPDX-License-Identifier: BSD-2-Clause-Patent
 
@@ -27,12 +27,7 @@
 EFI_MEMORY_ATTRIBUTES_TABLE  *mUefiMemoryAttributesTable = NULL;
 
-PAGE_ATTRIBUTE_TABLE  mPageAttributeTable[] = {
-  { Page4K, SIZE_4KB, PAGING_4K_ADDRESS_MASK_64 },
-  { Page2M, SIZE_2MB, PAGING_2M_ADDRESS_MASK_64 },
-  { Page1G, SIZE_1GB, PAGING_1G_ADDRESS_MASK_64 },
-};
-
-BOOLEAN  mIsShadowStack      = FALSE;
-BOOLEAN  m5LevelPagingNeeded = FALSE;
+BOOLEAN      mIsShadowStack      = FALSE;
+BOOLEAN      m5LevelPagingNeeded = FALSE;
+PAGING_MODE  mPagingMode         = PagingModeMax;
 
 //
@@ -47,4 +42,62 @@
 
 /**
+  Disable Write Protect on pages marked as read-only if Cr0.Bits.WP is 1.
+
+  @param[out]  WpEnabled      If Cr0.WP is enabled.
+  @param[out]  CetEnabled     If CET is enabled.
+**/
+VOID
+DisableReadOnlyPageWriteProtect (
+  OUT BOOLEAN  *WpEnabled,
+  OUT BOOLEAN  *CetEnabled
+  )
+{
+  IA32_CR0  Cr0;
+
+  *CetEnabled = ((AsmReadCr4 () & CR4_CET_ENABLE) != 0) ? TRUE : FALSE;
+  Cr0.UintN   = AsmReadCr0 ();
+  *WpEnabled  = (Cr0.Bits.WP != 0) ? TRUE : FALSE;
+  if (*WpEnabled) {
+    if (*CetEnabled) {
+      //
+      // CET must be disabled if WP is disabled. Disable CET before clearing CR0.WP.
+      //
+      DisableCet ();
+    }
+
+    Cr0.Bits.WP = 0;
+    AsmWriteCr0 (Cr0.UintN);
+  }
+}
+
+/**
+  Enable Write Protect on pages marked as read-only.
+
+  @param[out]  WpEnabled      If Cr0.WP should be enabled.
+  @param[out]  CetEnabled     If CET should be enabled.
+**/
+VOID
+EnableReadOnlyPageWriteProtect (
+  BOOLEAN  WpEnabled,
+  BOOLEAN  CetEnabled
+  )
+{
+  IA32_CR0  Cr0;
+
+  if (WpEnabled) {
+    Cr0.UintN   = AsmReadCr0 ();
+    Cr0.Bits.WP = 1;
+    AsmWriteCr0 (Cr0.UintN);
+
+    if (CetEnabled) {
+      //
+      // re-enable CET.
+      //
+      EnableCet ();
+    }
+  }
+}
+
+/**
   Initialize a buffer pool for page table use only.
 
@@ -68,8 +121,7 @@
   )
 {
-  VOID      *Buffer;
-  BOOLEAN   CetEnabled;
-  BOOLEAN   WpEnabled;
-  IA32_CR0  Cr0;
+  VOID     *Buffer;
+  BOOLEAN  WpEnabled;
+  BOOLEAN  CetEnabled;
 
   //
@@ -108,32 +160,7 @@
   //
   if (mIsReadOnlyPageTable) {
-    CetEnabled = ((AsmReadCr4 () & CR4_CET_ENABLE) != 0) ? TRUE : FALSE;
-    Cr0.UintN  = AsmReadCr0 ();
-    WpEnabled  = (Cr0.Bits.WP != 0) ? TRUE : FALSE;
-    if (WpEnabled) {
-      if (CetEnabled) {
-        //
-        // CET must be disabled if WP is disabled. Disable CET before clearing CR0.WP.
-        //
-        DisableCet ();
-      }
-
-      Cr0.Bits.WP = 0;
-      AsmWriteCr0 (Cr0.UintN);
-    }
-
+    DisableReadOnlyPageWriteProtect (&WpEnabled, &CetEnabled);
     SmmSetMemoryAttributes ((EFI_PHYSICAL_ADDRESS)(UINTN)Buffer, EFI_PAGES_TO_SIZE (PoolPages), EFI_MEMORY_RO);
-    if (WpEnabled) {
-      Cr0.UintN   = AsmReadCr0 ();
-      Cr0.Bits.WP = 1;
-      AsmWriteCr0 (Cr0.UintN);
-
-      if (CetEnabled) {
-        //
-        // re-enable CET.
-        //
-        EnableCet ();
-      }
-    }
+    EnableReadOnlyPageWriteProtect (WpEnabled, CetEnabled);
   }
 
@@ -184,50 +211,4 @@
 
   return Buffer;
-}
-
-/**
-  Return length according to page attributes.
-
-  @param[in]  PageAttributes   The page attribute of the page entry.
-
-  @return The length of page entry.
-**/
-UINTN
-PageAttributeToLength (
-  IN PAGE_ATTRIBUTE  PageAttribute
-  )
-{
-  UINTN  Index;
-
-  for (Index = 0; Index < sizeof (mPageAttributeTable)/sizeof (mPageAttributeTable[0]); Index++) {
-    if (PageAttribute == mPageAttributeTable[Index].Attribute) {
-      return (UINTN)mPageAttributeTable[Index].Length;
-    }
-  }
-
-  return 0;
-}
-
-/**
-  Return address mask according to page attributes.
-
-  @param[in]  PageAttributes   The page attribute of the page entry.
-
-  @return The address mask of page entry.
-**/
-UINTN
-PageAttributeToMask (
-  IN PAGE_ATTRIBUTE  PageAttribute
-  )
-{
-  UINTN  Index;
-
-  for (Index = 0; Index < sizeof (mPageAttributeTable)/sizeof (mPageAttributeTable[0]); Index++) {
-    if (PageAttribute == mPageAttributeTable[Index].Attribute) {
-      return (UINTN)mPageAttributeTable[Index].AddressMask;
-    }
-  }
-
-  return 0;
 }
 
@@ -355,179 +336,4 @@
 
 /**
-  Modify memory attributes of page entry.
-
-  @param[in]   PageEntry        The page entry.
-  @param[in]   Attributes       The bit mask of attributes to modify for the memory region.
-  @param[in]   IsSet            TRUE means to set attributes. FALSE means to clear attributes.
-  @param[out]  IsModified       TRUE means page table modified. FALSE means page table not modified.
-**/
-VOID
-ConvertPageEntryAttribute (
-  IN  UINT64   *PageEntry,
-  IN  UINT64   Attributes,
-  IN  BOOLEAN  IsSet,
-  OUT BOOLEAN  *IsModified
-  )
-{
-  UINT64  CurrentPageEntry;
-  UINT64  NewPageEntry;
-
-  CurrentPageEntry = *PageEntry;
-  NewPageEntry     = CurrentPageEntry;
-  if ((Attributes & EFI_MEMORY_RP) != 0) {
-    if (IsSet) {
-      NewPageEntry &= ~(UINT64)IA32_PG_P;
-    } else {
-      NewPageEntry |= IA32_PG_P;
-    }
-  }
-
-  if ((Attributes & EFI_MEMORY_RO) != 0) {
-    if (IsSet) {
-      NewPageEntry &= ~(UINT64)IA32_PG_RW;
-      if (mIsShadowStack) {
-        // Environment setup
-        // ReadOnly page need set Dirty bit for shadow stack
-        NewPageEntry |= IA32_PG_D;
-        // Clear user bit for supervisor shadow stack
-        NewPageEntry &= ~(UINT64)IA32_PG_U;
-      } else {
-        // Runtime update
-        // Clear dirty bit for non shadow stack, to protect RO page.
-        NewPageEntry &= ~(UINT64)IA32_PG_D;
-      }
-    } else {
-      NewPageEntry |= IA32_PG_RW;
-    }
-  }
-
-  if ((Attributes & EFI_MEMORY_XP) != 0) {
-    if (mXdSupported) {
-      if (IsSet) {
-        NewPageEntry |= IA32_PG_NX;
-      } else {
-        NewPageEntry &= ~IA32_PG_NX;
-      }
-    }
-  }
-
-  *PageEntry = NewPageEntry;
-  if (CurrentPageEntry != NewPageEntry) {
-    *IsModified = TRUE;
-    DEBUG ((DEBUG_VERBOSE, "ConvertPageEntryAttribute 0x%lx", CurrentPageEntry));
-    DEBUG ((DEBUG_VERBOSE, "->0x%lx\n", NewPageEntry));
-  } else {
-    *IsModified = FALSE;
-  }
-}
-
-/**
-  This function returns if there is need to split page entry.
-
-  @param[in]  BaseAddress      The base address to be checked.
-  @param[in]  Length           The length to be checked.
-  @param[in]  PageEntry        The page entry to be checked.
-  @param[in]  PageAttribute    The page attribute of the page entry.
-
-  @retval SplitAttributes on if there is need to split page entry.
-**/
-PAGE_ATTRIBUTE
-NeedSplitPage (
-  IN  PHYSICAL_ADDRESS  BaseAddress,
-  IN  UINT64            Length,
-  IN  UINT64            *PageEntry,
-  IN  PAGE_ATTRIBUTE    PageAttribute
-  )
-{
-  UINT64  PageEntryLength;
-
-  PageEntryLength = PageAttributeToLength (PageAttribute);
-
-  if (((BaseAddress & (PageEntryLength - 1)) == 0) && (Length >= PageEntryLength)) {
-    return PageNone;
-  }
-
-  if (((BaseAddress & PAGING_2M_MASK) != 0) || (Length < SIZE_2MB)) {
-    return Page4K;
-  }
-
-  return Page2M;
-}
-
-/**
-  This function splits one page entry to small page entries.
-
-  @param[in]  PageEntry        The page entry to be splitted.
-  @param[in]  PageAttribute    The page attribute of the page entry.
-  @param[in]  SplitAttribute   How to split the page entry.
-
-  @retval RETURN_SUCCESS            The page entry is splitted.
-  @retval RETURN_UNSUPPORTED        The page entry does not support to be splitted.
-  @retval RETURN_OUT_OF_RESOURCES   No resource to split page entry.
-**/
-RETURN_STATUS
-SplitPage (
-  IN  UINT64          *PageEntry,
-  IN  PAGE_ATTRIBUTE  PageAttribute,
-  IN  PAGE_ATTRIBUTE  SplitAttribute
-  )
-{
-  UINT64  BaseAddress;
-  UINT64  *NewPageEntry;
-  UINTN   Index;
-
-  ASSERT (PageAttribute == Page2M || PageAttribute == Page1G);
-
-  if (PageAttribute == Page2M) {
-    //
-    // Split 2M to 4K
-    //
-    ASSERT (SplitAttribute == Page4K);
-    if (SplitAttribute == Page4K) {
-      NewPageEntry = AllocatePageTableMemory (1);
-      DEBUG ((DEBUG_VERBOSE, "Split - 0x%x\n", NewPageEntry));
-      if (NewPageEntry == NULL) {
-        return RETURN_OUT_OF_RESOURCES;
-      }
-
-      BaseAddress = *PageEntry & PAGING_2M_ADDRESS_MASK_64;
-      for (Index = 0; Index < SIZE_4KB / sizeof (UINT64); Index++) {
-        NewPageEntry[Index] = (BaseAddress + SIZE_4KB * Index) | mAddressEncMask | ((*PageEntry) & PAGE_PROGATE_BITS);
-      }
-
-      (*PageEntry) = (UINT64)(UINTN)NewPageEntry | mAddressEncMask | PAGE_ATTRIBUTE_BITS;
-      return RETURN_SUCCESS;
-    } else {
-      return RETURN_UNSUPPORTED;
-    }
-  } else if (PageAttribute == Page1G) {
-    //
-    // Split 1G to 2M
-    // No need support 1G->4K directly, we should use 1G->2M, then 2M->4K to get more compact page table.
-    //
-    ASSERT (SplitAttribute == Page2M || SplitAttribute == Page4K);
-    if (((SplitAttribute == Page2M) || (SplitAttribute == Page4K))) {
-      NewPageEntry = AllocatePageTableMemory (1);
-      DEBUG ((DEBUG_VERBOSE, "Split - 0x%x\n", NewPageEntry));
-      if (NewPageEntry == NULL) {
-        return RETURN_OUT_OF_RESOURCES;
-      }
-
-      BaseAddress = *PageEntry & PAGING_1G_ADDRESS_MASK_64;
-      for (Index = 0; Index < SIZE_4KB / sizeof (UINT64); Index++) {
-        NewPageEntry[Index] = (BaseAddress + SIZE_2MB * Index) | mAddressEncMask | IA32_PG_PS | ((*PageEntry) & PAGE_PROGATE_BITS);
-      }
-
-      (*PageEntry) = (UINT64)(UINTN)NewPageEntry | mAddressEncMask | PAGE_ATTRIBUTE_BITS;
-      return RETURN_SUCCESS;
-    } else {
-      return RETURN_UNSUPPORTED;
-    }
-  } else {
-    return RETURN_UNSUPPORTED;
-  }
-}
-
-/**
   This function modifies the page attributes for the memory region specified by BaseAddress and
   Length from their current attributes to the attributes specified by Attributes.
@@ -536,10 +342,9 @@
 
   @param[in]   PageTableBase    The page table base.
-  @param[in]   EnablePML5Paging If PML5 paging is enabled.
+  @param[in]   PagingMode       The paging mode.
   @param[in]   BaseAddress      The physical address that is the start address of a memory region.
   @param[in]   Length           The size in bytes of the memory region.
   @param[in]   Attributes       The bit mask of attributes to modify for the memory region.
   @param[in]   IsSet            TRUE means to set attributes. FALSE means to clear attributes.
-  @param[out]  IsSplitted       TRUE means page table splitted. FALSE means page table not splitted.
   @param[out]  IsModified       TRUE means page table modified. FALSE means page table not modified.
 
@@ -560,20 +365,23 @@
 ConvertMemoryPageAttributes (
   IN  UINTN             PageTableBase,
-  IN  BOOLEAN           EnablePML5Paging,
+  IN  PAGING_MODE       PagingMode,
   IN  PHYSICAL_ADDRESS  BaseAddress,
   IN  UINT64            Length,
   IN  UINT64            Attributes,
   IN  BOOLEAN           IsSet,
-  OUT BOOLEAN           *IsSplitted   OPTIONAL,
   OUT BOOLEAN           *IsModified   OPTIONAL
   )
 {
-  UINT64                *PageEntry;
-  PAGE_ATTRIBUTE        PageAttribute;
-  UINTN                 PageEntryLength;
-  PAGE_ATTRIBUTE        SplitAttribute;
   RETURN_STATUS         Status;
-  BOOLEAN               IsEntryModified;
+  IA32_MAP_ATTRIBUTE    PagingAttribute;
+  IA32_MAP_ATTRIBUTE    PagingAttrMask;
+  UINTN                 PageTableBufferSize;
+  VOID                  *PageTableBuffer;
   EFI_PHYSICAL_ADDRESS  MaximumSupportMemAddress;
+  IA32_MAP_ENTRY        *Map;
+  UINTN                 Count;
+  UINTN                 Index;
+  UINT64                OverlappedRangeBase;
+  UINT64                OverlappedRangeLimit;
 
   ASSERT (Attributes != 0);
@@ -582,4 +390,5 @@
   ASSERT ((BaseAddress & (SIZE_4KB - 1)) == 0);
   ASSERT ((Length & (SIZE_4KB - 1)) == 0);
+  ASSERT (PageTableBase != 0);
 
   if (Length == 0) {
@@ -600,58 +409,132 @@
   }
 
-  //  DEBUG ((DEBUG_ERROR, "ConvertMemoryPageAttributes(%x) - %016lx, %016lx, %02lx\n", IsSet, BaseAddress, Length, Attributes));
-
-  if (IsSplitted != NULL) {
-    *IsSplitted = FALSE;
-  }
-
   if (IsModified != NULL) {
     *IsModified = FALSE;
   }
 
-  //
-  // Below logic is to check 2M/4K page to make sure we do not waste memory.
-  //
-  while (Length != 0) {
-    PageEntry = GetPageTableEntry (PageTableBase, EnablePML5Paging, BaseAddress, &PageAttribute);
-    if (PageEntry == NULL) {
-      return RETURN_UNSUPPORTED;
-    }
-
-    PageEntryLength = PageAttributeToLength (PageAttribute);
-    SplitAttribute  = NeedSplitPage (BaseAddress, Length, PageEntry, PageAttribute);
-    if (SplitAttribute == PageNone) {
-      ConvertPageEntryAttribute (PageEntry, Attributes, IsSet, &IsEntryModified);
-      if (IsEntryModified) {
-        if (IsModified != NULL) {
-          *IsModified = TRUE;
+  PagingAttribute.Uint64 = 0;
+  PagingAttribute.Uint64 = mAddressEncMask | BaseAddress;
+  PagingAttrMask.Uint64  = 0;
+
+  if ((Attributes & EFI_MEMORY_RO) != 0) {
+    PagingAttrMask.Bits.ReadWrite = 1;
+    if (IsSet) {
+      PagingAttribute.Bits.ReadWrite = 0;
+      PagingAttrMask.Bits.Dirty      = 1;
+      if (mIsShadowStack) {
+        // Environment setup
+        // ReadOnly page need set Dirty bit for shadow stack
+        PagingAttribute.Bits.Dirty = 1;
+        // Clear user bit for supervisor shadow stack
+        PagingAttribute.Bits.UserSupervisor = 0;
+        PagingAttrMask.Bits.UserSupervisor  = 1;
+      } else {
+        // Runtime update
+        // Clear dirty bit for non shadow stack, to protect RO page.
+        PagingAttribute.Bits.Dirty = 0;
+      }
+    } else {
+      PagingAttribute.Bits.ReadWrite = 1;
+    }
+  }
+
+  if ((Attributes & EFI_MEMORY_XP) != 0) {
+    if (mXdSupported) {
+      PagingAttribute.Bits.Nx = IsSet ? 1 : 0;
+      PagingAttrMask.Bits.Nx  = 1;
+    }
+  }
+
+  if ((Attributes & EFI_MEMORY_RP) != 0) {
+    if (IsSet) {
+      PagingAttribute.Bits.Present = 0;
+      //
+      // When map a range to non-present, all attributes except Present should not be provided.
+      //
+      PagingAttrMask.Uint64       = 0;
+      PagingAttrMask.Bits.Present = 1;
+    } else {
+      //
+      // When map range to present range, provide all attributes.
+      //
+      PagingAttribute.Bits.Present = 1;
+      PagingAttrMask.Uint64        = MAX_UINT64;
+
+      //
+      // By default memory is Ring 3 accessble.
+      //
+      PagingAttribute.Bits.UserSupervisor = 1;
+
+      DEBUG_CODE_BEGIN ();
+      if (((Attributes & EFI_MEMORY_RO) == 0) || (((Attributes & EFI_MEMORY_XP) == 0) && (mXdSupported))) {
+        //
+        // When mapping a range to present and EFI_MEMORY_RO or EFI_MEMORY_XP is not specificed,
+        // check if [BaseAddress, BaseAddress + Length] contains present range.
+        // Existing Present range in [BaseAddress, BaseAddress + Length] is set to NX disable or ReadOnly.
+        //
+        Count  = 0;
+        Map    = NULL;
+        Status = PageTableParse (PageTableBase, mPagingMode, NULL, &Count);
+
+        while (Status == RETURN_BUFFER_TOO_SMALL) {
+          if (Map != NULL) {
+            FreePool (Map);
+          }
+
+          Map = AllocatePool (Count * sizeof (IA32_MAP_ENTRY));
+          ASSERT (Map != NULL);
+          Status = PageTableParse (PageTableBase, mPagingMode, Map, &Count);
         }
+
+        ASSERT_RETURN_ERROR (Status);
+        for (Index = 0; Index < Count; Index++) {
+          if (Map[Index].LinearAddress >= BaseAddress + Length) {
+            break;
+          }
+
+          if ((BaseAddress < Map[Index].LinearAddress + Map[Index].Length) && (BaseAddress + Length > Map[Index].LinearAddress)) {
+            OverlappedRangeBase  = MAX (BaseAddress, Map[Index].LinearAddress);
+            OverlappedRangeLimit = MIN (BaseAddress + Length, Map[Index].LinearAddress + Map[Index].Length);
+
+            if (((Attributes & EFI_MEMORY_RO) == 0) && (Map[Index].Attribute.Bits.ReadWrite == 1)) {
+              DEBUG ((DEBUG_ERROR, "SMM ConvertMemoryPageAttributes: [0x%lx, 0x%lx] is set from ReadWrite to ReadOnly\n", OverlappedRangeBase, OverlappedRangeLimit));
+            }
+
+            if (((Attributes & EFI_MEMORY_XP) == 0) && (mXdSupported) && (Map[Index].Attribute.Bits.Nx == 1)) {
+              DEBUG ((DEBUG_ERROR, "SMM ConvertMemoryPageAttributes: [0x%lx, 0x%lx] is set from NX enabled to NX disabled\n", OverlappedRangeBase, OverlappedRangeLimit));
+            }
+          }
+        }
+
+        FreePool (Map);
       }
 
-      //
-      // Convert success, move to next
-      //
-      BaseAddress += PageEntryLength;
-      Length      -= PageEntryLength;
-    } else {
-      Status = SplitPage (PageEntry, PageAttribute, SplitAttribute);
-      if (RETURN_ERROR (Status)) {
-        return RETURN_UNSUPPORTED;
-      }
-
-      if (IsSplitted != NULL) {
-        *IsSplitted = TRUE;
-      }
-
-      if (IsModified != NULL) {
-        *IsModified = TRUE;
-      }
-
-      //
-      // Just split current page
-      // Convert success in next around
-      //
-    }
-  }
+      DEBUG_CODE_END ();
+    }
+  }
+
+  if (PagingAttrMask.Uint64 == 0) {
+    return RETURN_SUCCESS;
+  }
+
+  PageTableBufferSize = 0;
+  Status              = PageTableMap (&PageTableBase, PagingMode, NULL, &PageTableBufferSize, BaseAddress, Length, &PagingAttribute, &PagingAttrMask, IsModified);
+
+  if (Status == RETURN_BUFFER_TOO_SMALL) {
+    PageTableBuffer = AllocatePageTableMemory (EFI_SIZE_TO_PAGES (PageTableBufferSize));
+    ASSERT (PageTableBuffer != NULL);
+    Status = PageTableMap (&PageTableBase, PagingMode, PageTableBuffer, &PageTableBufferSize, BaseAddress, Length, &PagingAttribute, &PagingAttrMask, IsModified);
+  }
+
+  if (Status == RETURN_INVALID_PARAMETER) {
+    //
+    // The only reason that PageTableMap returns RETURN_INVALID_PARAMETER here is to modify other attributes
+    // of a non-present range but remains the non-present range still as non-present.
+    //
+    DEBUG ((DEBUG_ERROR, "SMM ConvertMemoryPageAttributes: Only change EFI_MEMORY_XP/EFI_MEMORY_RO for non-present range in [0x%lx, 0x%lx] is not permitted\n", BaseAddress, BaseAddress + Length));
+  }
+
+  ASSERT_RETURN_ERROR (Status);
+  ASSERT (PageTableBufferSize == 0);
 
   return RETURN_SUCCESS;
@@ -698,9 +581,8 @@
 
   @param[in]   PageTableBase    The page table base.
-  @param[in]   EnablePML5Paging If PML5 paging is enabled.
+  @param[in]   PagingMode       The paging mode.
   @param[in]   BaseAddress      The physical address that is the start address of a memory region.
   @param[in]   Length           The size in bytes of the memory region.
   @param[in]   Attributes       The bit mask of attributes to set for the memory region.
-  @param[out]  IsSplitted       TRUE means page table splitted. FALSE means page table not splitted.
 
   @retval EFI_SUCCESS           The attributes were set for the memory region.
@@ -721,9 +603,8 @@
 SmmSetMemoryAttributesEx (
   IN  UINTN                 PageTableBase,
-  IN  BOOLEAN               EnablePML5Paging,
+  IN  PAGING_MODE           PagingMode,
   IN  EFI_PHYSICAL_ADDRESS  BaseAddress,
   IN  UINT64                Length,
-  IN  UINT64                Attributes,
-  OUT BOOLEAN               *IsSplitted  OPTIONAL
+  IN  UINT64                Attributes
   )
 {
@@ -731,5 +612,5 @@
   BOOLEAN     IsModified;
 
-  Status = ConvertMemoryPageAttributes (PageTableBase, EnablePML5Paging, BaseAddress, Length, Attributes, TRUE, IsSplitted, &IsModified);
+  Status = ConvertMemoryPageAttributes (PageTableBase, PagingMode, BaseAddress, Length, Attributes, TRUE, &IsModified);
   if (!EFI_ERROR (Status)) {
     if (IsModified) {
@@ -749,9 +630,8 @@
 
   @param[in]   PageTableBase    The page table base.
-  @param[in]   EnablePML5Paging If PML5 paging is enabled.
+  @param[in]   PagingMode       The paging mode.
   @param[in]   BaseAddress      The physical address that is the start address of a memory region.
   @param[in]   Length           The size in bytes of the memory region.
   @param[in]   Attributes       The bit mask of attributes to clear for the memory region.
-  @param[out]  IsSplitted       TRUE means page table splitted. FALSE means page table not splitted.
 
   @retval EFI_SUCCESS           The attributes were cleared for the memory region.
@@ -772,9 +652,8 @@
 SmmClearMemoryAttributesEx (
   IN  UINTN                 PageTableBase,
-  IN  BOOLEAN               EnablePML5Paging,
+  IN  PAGING_MODE           PagingMode,
   IN  EFI_PHYSICAL_ADDRESS  BaseAddress,
   IN  UINT64                Length,
-  IN  UINT64                Attributes,
-  OUT BOOLEAN               *IsSplitted  OPTIONAL
+  IN  UINT64                Attributes
   )
 {
@@ -782,5 +661,5 @@
   BOOLEAN     IsModified;
 
-  Status = ConvertMemoryPageAttributes (PageTableBase, EnablePML5Paging, BaseAddress, Length, Attributes, FALSE, IsSplitted, &IsModified);
+  Status = ConvertMemoryPageAttributes (PageTableBase, PagingMode, BaseAddress, Length, Attributes, FALSE, &IsModified);
   if (!EFI_ERROR (Status)) {
     if (IsModified) {
@@ -824,12 +703,8 @@
   )
 {
-  IA32_CR4  Cr4;
-  UINTN     PageTableBase;
-  BOOLEAN   Enable5LevelPaging;
-
-  PageTableBase      = AsmReadCr3 () & PAGING_4K_ADDRESS_MASK_64;
-  Cr4.UintN          = AsmReadCr4 ();
-  Enable5LevelPaging = (BOOLEAN)(Cr4.Bits.LA57 == 1);
-  return SmmSetMemoryAttributesEx (PageTableBase, Enable5LevelPaging, BaseAddress, Length, Attributes, NULL);
+  UINTN  PageTableBase;
+
+  PageTableBase = AsmReadCr3 () & PAGING_4K_ADDRESS_MASK_64;
+  return SmmSetMemoryAttributesEx (PageTableBase, mPagingMode, BaseAddress, Length, Attributes);
 }
 
@@ -863,12 +738,8 @@
   )
 {
-  IA32_CR4  Cr4;
-  UINTN     PageTableBase;
-  BOOLEAN   Enable5LevelPaging;
-
-  PageTableBase      = AsmReadCr3 () & PAGING_4K_ADDRESS_MASK_64;
-  Cr4.UintN          = AsmReadCr4 ();
-  Enable5LevelPaging = (BOOLEAN)(Cr4.Bits.LA57 == 1);
-  return SmmClearMemoryAttributesEx (PageTableBase, Enable5LevelPaging, BaseAddress, Length, Attributes, NULL);
+  UINTN  PageTableBase;
+
+  PageTableBase = AsmReadCr3 () & PAGING_4K_ADDRESS_MASK_64;
+  return SmmClearMemoryAttributesEx (PageTableBase, mPagingMode, BaseAddress, Length, Attributes);
 }
 
@@ -892,29 +763,7 @@
 
   mIsShadowStack = TRUE;
-  Status         = SmmSetMemoryAttributesEx (Cr3, m5LevelPagingNeeded, BaseAddress, Length, EFI_MEMORY_RO, NULL);
+  Status         = SmmSetMemoryAttributesEx (Cr3, mPagingMode, BaseAddress, Length, EFI_MEMORY_RO);
   mIsShadowStack = FALSE;
 
-  return Status;
-}
-
-/**
-  Set not present memory.
-
-  @param[in]  Cr3              The page table base address.
-  @param[in]  BaseAddress      The physical address that is the start address of a memory region.
-  @param[in]  Length           The size in bytes of the memory region.
-
-  @retval EFI_SUCCESS           The not present memory is set.
-**/
-EFI_STATUS
-SetNotPresentPage (
-  IN  UINTN                 Cr3,
-  IN  EFI_PHYSICAL_ADDRESS  BaseAddress,
-  IN  UINT64                Length
-  )
-{
-  EFI_STATUS  Status;
-
-  Status = SmmSetMemoryAttributesEx (Cr3, m5LevelPagingNeeded, BaseAddress, Length, EFI_MEMORY_RP, NULL);
   return Status;
 }
@@ -1081,4 +930,68 @@
 
 /**
+  This function set [Base, Limit] to the input MemoryAttribute.
+
+  @param  Base        Start address of range.
+  @param  Limit       Limit address of range.
+  @param  Attribute   The bit mask of attributes to modify for the memory region.
+  @param  Map         Pointer to the array of Cr3 IA32_MAP_ENTRY.
+  @param  Count       Count of IA32_MAP_ENTRY in Map.
+**/
+VOID
+SetMemMapWithNonPresentRange (
+  UINT64          Base,
+  UINT64          Limit,
+  UINT64          Attribute,
+  IA32_MAP_ENTRY  *Map,
+  UINTN           Count
+  )
+{
+  UINTN   Index;
+  UINT64  NonPresentRangeStart;
+
+  NonPresentRangeStart = 0;
+  for (Index = 0; Index < Count; Index++) {
+    if ((Map[Index].LinearAddress > NonPresentRangeStart) &&
+        (Base < Map[Index].LinearAddress) && (Limit > NonPresentRangeStart))
+    {
+      //
+      // We should NOT set attributes for non-present ragne.
+      //
+      //
+      // There is a non-present ( [NonPresentStart, Map[Index].LinearAddress] ) range before current Map[Index]
+      // and it is overlapped with [Base, Limit].
+      //
+      if (Base < NonPresentRangeStart) {
+        SmmSetMemoryAttributes (
+          Base,
+          NonPresentRangeStart - Base,
+          Attribute
+          );
+      }
+
+      Base = Map[Index].LinearAddress;
+    }
+
+    NonPresentRangeStart = Map[Index].LinearAddress + Map[Index].Length;
+    if (NonPresentRangeStart >= Limit) {
+      break;
+    }
+  }
+
+  Limit = MIN (NonPresentRangeStart, Limit);
+
+  if (Base < Limit) {
+    //
+    // There is no non-present range in current [Base, Limit] anymore.
+    //
+    SmmSetMemoryAttributes (
+      Base,
+      Limit - Base,
+      Attribute
+      );
+  }
+}
+
+/**
   This function sets memory attribute according to MemoryAttributesTable.
 **/
@@ -1094,4 +1007,11 @@
   UINTN                                 Index;
   EDKII_PI_SMM_MEMORY_ATTRIBUTES_TABLE  *MemoryAttributesTable;
+  UINTN                                 PageTable;
+  EFI_STATUS                            Status;
+  IA32_MAP_ENTRY                        *Map;
+  UINTN                                 Count;
+  UINT64                                MemoryAttribute;
+  BOOLEAN                               WpEnabled;
+  BOOLEAN                               CetEnabled;
 
   SmmGetSystemConfigurationTable (&gEdkiiPiSmmMemoryAttributesTableGuid, (VOID **)&MemoryAttributesTable);
@@ -1100,4 +1020,6 @@
     return;
   }
+
+  PERF_FUNCTION_BEGIN ();
 
   DEBUG ((DEBUG_INFO, "MemoryAttributesTable:\n"));
@@ -1120,38 +1042,57 @@
   }
 
+  Count     = 0;
+  Map       = NULL;
+  PageTable = AsmReadCr3 ();
+  Status    = PageTableParse (PageTable, mPagingMode, NULL, &Count);
+  while (Status == RETURN_BUFFER_TOO_SMALL) {
+    if (Map != NULL) {
+      FreePool (Map);
+    }
+
+    Map = AllocatePool (Count * sizeof (IA32_MAP_ENTRY));
+    ASSERT (Map != NULL);
+    Status = PageTableParse (PageTable, mPagingMode, Map, &Count);
+  }
+
+  ASSERT_RETURN_ERROR (Status);
+
+  DisableReadOnlyPageWriteProtect (&WpEnabled, &CetEnabled);
+
   MemoryMap = MemoryMapStart;
   for (Index = 0; Index < MemoryMapEntryCount; Index++) {
     DEBUG ((DEBUG_VERBOSE, "SetAttribute: Memory Entry - 0x%lx, 0x%x\n", MemoryMap->PhysicalStart, MemoryMap->NumberOfPages));
-    switch (MemoryMap->Type) {
-      case EfiRuntimeServicesCode:
-        SmmSetMemoryAttributes (
-          MemoryMap->PhysicalStart,
-          EFI_PAGES_TO_SIZE ((UINTN)MemoryMap->NumberOfPages),
-          EFI_MEMORY_RO
-          );
-        break;
-      case EfiRuntimeServicesData:
-        SmmSetMemoryAttributes (
-          MemoryMap->PhysicalStart,
-          EFI_PAGES_TO_SIZE ((UINTN)MemoryMap->NumberOfPages),
-          EFI_MEMORY_XP
-          );
-        break;
-      default:
-        SmmSetMemoryAttributes (
-          MemoryMap->PhysicalStart,
-          EFI_PAGES_TO_SIZE ((UINTN)MemoryMap->NumberOfPages),
-          EFI_MEMORY_XP
-          );
-        break;
-    }
+    if (MemoryMap->Type == EfiRuntimeServicesCode) {
+      MemoryAttribute = EFI_MEMORY_RO;
+    } else {
+      ASSERT ((MemoryMap->Type == EfiRuntimeServicesData) || (MemoryMap->Type == EfiConventionalMemory));
+      //
+      // Set other type memory as NX.
+      //
+      MemoryAttribute = EFI_MEMORY_XP;
+    }
+
+    //
+    // There may exist non-present range overlaps with the MemoryMap range.
+    // Do not change other attributes of non-present range while still remaining it as non-present
+    //
+    SetMemMapWithNonPresentRange (
+      MemoryMap->PhysicalStart,
+      MemoryMap->PhysicalStart + EFI_PAGES_TO_SIZE ((UINTN)MemoryMap->NumberOfPages),
+      MemoryAttribute,
+      Map,
+      Count
+      );
 
     MemoryMap = NEXT_MEMORY_DESCRIPTOR (MemoryMap, DescriptorSize);
   }
+
+  EnableReadOnlyPageWriteProtect (WpEnabled, CetEnabled);
+  FreePool (Map);
 
   PatchSmmSaveStateMap ();
   PatchGdtIdtMap ();
 
-  return;
+  PERF_FUNCTION_END ();
 }
 
@@ -1454,6 +1395,12 @@
   UINTN                  Index;
   EFI_MEMORY_DESCRIPTOR  *Entry;
+  BOOLEAN                WpEnabled;
+  BOOLEAN                CetEnabled;
+
+  PERF_FUNCTION_BEGIN ();
 
   DEBUG ((DEBUG_INFO, "SetUefiMemMapAttributes\n"));
+
+  DisableReadOnlyPageWriteProtect (&WpEnabled, &CetEnabled);
 
   if (mUefiMemoryMap != NULL) {
@@ -1535,7 +1482,11 @@
   }
 
+  EnableReadOnlyPageWriteProtect (WpEnabled, CetEnabled);
+
   //
   // Do not free mUefiMemoryAttributesTable, it will be checked in IsSmmCommBufferForbiddenAddress().
   //
+
+  PERF_FUNCTION_END ();
 }
 
@@ -1675,4 +1626,69 @@
 
 /**
+  Create page table based on input PagingMode and PhysicalAddressBits in smm.
+
+  @param[in]      PagingMode           The paging mode.
+  @param[in]      PhysicalAddressBits  The bits of physical address to map.
+
+  @retval         PageTable Address
+
+**/
+UINTN
+GenSmmPageTable (
+  IN PAGING_MODE  PagingMode,
+  IN UINT8        PhysicalAddressBits
+  )
+{
+  UINTN               PageTableBufferSize;
+  UINTN               PageTable;
+  VOID                *PageTableBuffer;
+  IA32_MAP_ATTRIBUTE  MapAttribute;
+  IA32_MAP_ATTRIBUTE  MapMask;
+  RETURN_STATUS       Status;
+  UINTN               GuardPage;
+  UINTN               Index;
+  UINT64              Length;
+
+  Length                           = LShiftU64 (1, PhysicalAddressBits);
+  PageTable                        = 0;
+  PageTableBufferSize              = 0;
+  MapMask.Uint64                   = MAX_UINT64;
+  MapAttribute.Uint64              = mAddressEncMask;
+  MapAttribute.Bits.Present        = 1;
+  MapAttribute.Bits.ReadWrite      = 1;
+  MapAttribute.Bits.UserSupervisor = 1;
+  MapAttribute.Bits.Accessed       = 1;
+  MapAttribute.Bits.Dirty          = 1;
+
+  Status = PageTableMap (&PageTable, PagingMode, NULL, &PageTableBufferSize, 0, Length, &MapAttribute, &MapMask, NULL);
+  ASSERT (Status == RETURN_BUFFER_TOO_SMALL);
+  DEBUG ((DEBUG_INFO, "GenSMMPageTable: 0x%x bytes needed for initial SMM page table\n", PageTableBufferSize));
+  PageTableBuffer = AllocatePageTableMemory (EFI_SIZE_TO_PAGES (PageTableBufferSize));
+  ASSERT (PageTableBuffer != NULL);
+  Status = PageTableMap (&PageTable, PagingMode, PageTableBuffer, &PageTableBufferSize, 0, Length, &MapAttribute, &MapMask, NULL);
+  ASSERT (Status == RETURN_SUCCESS);
+  ASSERT (PageTableBufferSize == 0);
+
+  if (FeaturePcdGet (PcdCpuSmmStackGuard)) {
+    //
+    // Mark the 4KB guard page between known good stack and smm stack as non-present
+    //
+    for (Index = 0; Index < gSmmCpuPrivate->SmmCoreEntryContext.NumberOfCpus; Index++) {
+      GuardPage = mSmmStackArrayBase + EFI_PAGE_SIZE + Index * (mSmmStackSize + mSmmShadowStackSize);
+      Status    = ConvertMemoryPageAttributes (PageTable, PagingMode, GuardPage, SIZE_4KB, EFI_MEMORY_RP, TRUE, NULL);
+    }
+  }
+
+  if ((PcdGet8 (PcdNullPointerDetectionPropertyMask) & BIT1) != 0) {
+    //
+    // Mark [0, 4k] as non-present
+    //
+    Status = ConvertMemoryPageAttributes (PageTable, PagingMode, 0, SIZE_4KB, EFI_MEMORY_RP, TRUE, NULL);
+  }
+
+  return (UINTN)PageTable;
+}
+
+/**
   This function retrieves the attributes of the memory region specified by
   BaseAddress and Length. If different attributes are got from different part
@@ -1800,5 +1816,5 @@
     // Set entire pool including header, used-memory and left free-memory as ReadOnly in SMM page table.
     //
-    ConvertMemoryPageAttributes (PageTableBase, m5LevelPagingNeeded, Address, PoolSize, EFI_MEMORY_RO, TRUE, NULL, NULL);
+    ConvertMemoryPageAttributes (PageTableBase, mPagingMode, Address, PoolSize, EFI_MEMORY_RO, TRUE, NULL);
     Pool = Pool->NextPool;
   } while (Pool != HeadPool);
@@ -1857,4 +1873,5 @@
   )
 {
+  BOOLEAN  WpEnabled;
   BOOLEAN  CetEnabled;
 
@@ -1863,4 +1880,5 @@
   }
 
+  PERF_FUNCTION_BEGIN ();
   DEBUG ((DEBUG_INFO, "SetPageTableAttributes\n"));
 
@@ -1869,13 +1887,5 @@
   // We need *write* page table memory, to mark itself to be *read only*.
   //
-  CetEnabled = ((AsmReadCr4 () & CR4_CET_ENABLE) != 0) ? TRUE : FALSE;
-  if (CetEnabled) {
-    //
-    // CET must be disabled if WP is disabled.
-    //
-    DisableCet ();
-  }
-
-  AsmWriteCr0 (AsmReadCr0 () & ~CR0_WP);
+  DisableReadOnlyPageWriteProtect (&WpEnabled, &CetEnabled);
 
   // Set memory used by page table as Read Only.
@@ -1886,5 +1896,5 @@
   // Enable write protection, after page table attribute updated.
   //
-  AsmWriteCr0 (AsmReadCr0 () | CR0_WP);
+  EnableReadOnlyPageWriteProtect (TRUE, CetEnabled);
   mIsReadOnlyPageTable = TRUE;
 
@@ -1893,12 +1903,4 @@
   //
   FlushTlbForAll ();
-
-  if (CetEnabled) {
-    //
-    // re-enable CET.
-    //
-    EnableCet ();
-  }
-
-  return;
-}
+  PERF_FUNCTION_END ();
+}
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/PiSmmCpuDxeSmm/SmmMpPerf.c
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/PiSmmCpuDxeSmm/SmmMpPerf.c	(revision 101291)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/PiSmmCpuDxeSmm/SmmMpPerf.c	(revision 101291)
@@ -0,0 +1,99 @@
+/** @file
+SMM MP perf-logging implementation
+
+Copyright (c) 2023, Intel Corporation. All rights reserved.<BR>
+
+SPDX-License-Identifier: BSD-2-Clause-Patent
+
+**/
+
+#include "PiSmmCpuDxeSmm.h"
+
+#define  SMM_MP_PERF_PROCEDURE_NAME(procedure)  # procedure
+GLOBAL_REMOVE_IF_UNREFERENCED
+CHAR8  *gSmmMpPerfProcedureName[] = {
+  SMM_MP_PERF_PROCEDURE_LIST (SMM_MP_PERF_PROCEDURE_NAME)
+};
+//
+// Each element holds the performance data for one processor.
+//
+GLOBAL_REMOVE_IF_UNREFERENCED
+SMM_PERF_AP_PROCEDURE_PERFORMANCE  *mSmmMpProcedurePerformance = NULL;
+
+/**
+  Initialize the perf-logging feature for APs.
+
+  @param NumberofCpus    Number of processors in the platform.
+**/
+VOID
+InitializeMpPerf (
+  UINTN  NumberofCpus
+  )
+{
+  mSmmMpProcedurePerformance = AllocateZeroPool (NumberofCpus * sizeof (*mSmmMpProcedurePerformance));
+  ASSERT (mSmmMpProcedurePerformance != NULL);
+}
+
+/**
+  Migrate MP performance data to standardized performance database.
+
+  @param NumberofCpus    Number of processors in the platform.
+  @param BspIndex        The index of the BSP.
+**/
+VOID
+MigrateMpPerf (
+  UINTN  NumberofCpus,
+  UINTN  BspIndex
+  )
+{
+  UINTN  CpuIndex;
+  UINTN  MpProcecureId;
+
+  for (CpuIndex = 0; CpuIndex < NumberofCpus; CpuIndex++) {
+    if ((CpuIndex != BspIndex) && !FeaturePcdGet (PcdSmmApPerfLogEnable)) {
+      //
+      // Skip migrating AP performance data if AP perf-logging is disabled.
+      //
+      continue;
+    }
+
+    for (MpProcecureId = 0; MpProcecureId < SMM_MP_PERF_PROCEDURE_ID (SmmMpProcedureMax); MpProcecureId++) {
+      if (mSmmMpProcedurePerformance[CpuIndex].Begin[MpProcecureId] != 0) {
+        PERF_START (NULL, gSmmMpPerfProcedureName[MpProcecureId], NULL, mSmmMpProcedurePerformance[CpuIndex].Begin[MpProcecureId]);
+        PERF_END (NULL, gSmmMpPerfProcedureName[MpProcecureId], NULL, mSmmMpProcedurePerformance[CpuIndex].End[MpProcecureId]);
+      }
+    }
+  }
+
+  ZeroMem (mSmmMpProcedurePerformance, NumberofCpus * sizeof (*mSmmMpProcedurePerformance));
+}
+
+/**
+  Save the performance counter value before running the MP procedure.
+
+  @param CpuIndex        The index of the CPU.
+  @param MpProcedureId   The ID of the MP procedure.
+**/
+VOID
+MpPerfBegin (
+  IN UINTN  CpuIndex,
+  IN UINTN  MpProcedureId
+  )
+{
+  mSmmMpProcedurePerformance[CpuIndex].Begin[MpProcedureId] = GetPerformanceCounter ();
+}
+
+/**
+  Save the performance counter value after running the MP procedure.
+
+  @param CpuIndex        The index of the CPU.
+  @param MpProcedureId   The ID of the MP procedure.
+**/
+VOID
+MpPerfEnd (
+  IN UINTN  CpuIndex,
+  IN UINTN  MpProcedureId
+  )
+{
+  mSmmMpProcedurePerformance[CpuIndex].End[MpProcedureId] = GetPerformanceCounter ();
+}
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/PiSmmCpuDxeSmm/SmmMpPerf.h
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/PiSmmCpuDxeSmm/SmmMpPerf.h	(revision 101291)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/PiSmmCpuDxeSmm/SmmMpPerf.h	(revision 101291)
@@ -0,0 +1,83 @@
+/** @file
+SMM MP perf-logging implementation
+
+Copyright (c) 2023, Intel Corporation. All rights reserved.<BR>
+
+SPDX-License-Identifier: BSD-2-Clause-Patent
+
+**/
+
+#ifndef MP_PERF_H_
+#define MP_PERF_H_
+
+//
+// The list of all MP procedures that need to be perf-logged.
+//
+#define  SMM_MP_PERF_PROCEDURE_LIST(_) \
+  _(SmmInitHandler), \
+  _(SmmRendezvousEntry), \
+  _(PlatformValidSmi), \
+  _(SmmRendezvousExit), \
+  _(SmmMpProcedureMax) // Add new entries above this line
+
+//
+// To perf-log MP procedures, call MpPerfBegin()/MpPerfEnd() with CpuIndex
+// and SMM_MP_PERF_PROCEDURE_ID with entry name defined in the SMM_MP_PERF_PROCEDURE_LIST.
+//
+#define  SMM_MP_PERF_PROCEDURE_ID(procedure)  SmmMpProcedureId ## procedure
+enum {
+  SMM_MP_PERF_PROCEDURE_LIST (SMM_MP_PERF_PROCEDURE_ID)
+};
+
+typedef struct {
+  UINT64    Begin[SMM_MP_PERF_PROCEDURE_ID (SmmMpProcedureMax)];
+  UINT64    End[SMM_MP_PERF_PROCEDURE_ID (SmmMpProcedureMax)];
+} SMM_PERF_AP_PROCEDURE_PERFORMANCE;
+
+/**
+  Initialize the perf-logging feature for APs.
+
+  @param NumberofCpus    Number of processors in the platform.
+**/
+VOID
+InitializeMpPerf (
+  UINTN  NumberofCpus
+  );
+
+/**
+  Migrate MP performance data to standardized performance database.
+
+  @param NumberofCpus    Number of processors in the platform.
+  @param BspIndex        The index of the BSP.
+**/
+VOID
+MigrateMpPerf (
+  UINTN  NumberofCpus,
+  UINTN  BspIndex
+  );
+
+/**
+  Save the performance counter value before running the MP procedure.
+
+  @param CpuIndex        The index of the CPU.
+  @param MpProcedureId   The ID of the MP procedure.
+**/
+VOID
+MpPerfBegin (
+  IN UINTN  CpuIndex,
+  IN UINTN  MpProcedureId
+  );
+
+/**
+  Save the performance counter value after running the MP procedure.
+
+  @param CpuIndex        The index of the CPU.
+  @param MpProcedureId   The ID of the MP procedure.
+**/
+VOID
+MpPerfEnd (
+  IN UINTN  CpuIndex,
+  IN UINTN  MpProcedureId
+  );
+
+#endif
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/PiSmmCpuDxeSmm/SmmProfile.c
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/PiSmmCpuDxeSmm/SmmProfile.c	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/PiSmmCpuDxeSmm/SmmProfile.c	(revision 101291)
@@ -374,4 +374,30 @@
   //
   return FALSE;
+}
+
+/**
+  Function to compare 2 MEMORY_PROTECTION_RANGE based on range base.
+
+  @param[in] Buffer1            pointer to Device Path poiner to compare
+  @param[in] Buffer2            pointer to second DevicePath pointer to compare
+
+  @retval 0                     Buffer1 equal to Buffer2
+  @retval <0                    Buffer1 is less than Buffer2
+  @retval >0                    Buffer1 is greater than Buffer2
+**/
+INTN
+EFIAPI
+ProtectionRangeCompare (
+  IN  CONST VOID  *Buffer1,
+  IN  CONST VOID  *Buffer2
+  )
+{
+  if (((MEMORY_PROTECTION_RANGE *)Buffer1)->Range.Base > ((MEMORY_PROTECTION_RANGE *)Buffer2)->Range.Base) {
+    return 1;
+  } else if (((MEMORY_PROTECTION_RANGE *)Buffer1)->Range.Base < ((MEMORY_PROTECTION_RANGE *)Buffer2)->Range.Base) {
+    return -1;
+  }
+
+  return 0;
 }
 
@@ -398,4 +424,5 @@
   UINT64                           High4KBPageSize;
   UINT64                           Low4KBPageSize;
+  MEMORY_PROTECTION_RANGE          MemProtectionRange;
 
   NumberOfDescriptors      = 0;
@@ -534,4 +561,9 @@
   mSplitMemRangeCount = NumberOfSpliteRange;
 
+  //
+  // Sort the mProtectionMemRange
+  //
+  QuickSort (mProtectionMemRange, mProtectionMemRangeCount, sizeof (MEMORY_PROTECTION_RANGE), (BASE_SORT_COMPARE)ProtectionRangeCompare, &MemProtectionRange);
+
   DEBUG ((DEBUG_INFO, "SMM Profile Memory Ranges:\n"));
   for (Index = 0; Index < mProtectionMemRangeCount; Index++) {
@@ -555,249 +587,91 @@
   )
 {
-  UINT64    Pml5Entry;
-  UINT64    Pml4Entry;
-  UINT64    *Pml5;
-  UINT64    *Pml4;
-  UINT64    *Pdpt;
-  UINT64    *Pd;
-  UINT64    *Pt;
-  UINTN     Address;
-  UINTN     Pml5Index;
-  UINTN     Pml4Index;
-  UINTN     PdptIndex;
-  UINTN     PdIndex;
-  UINTN     PtIndex;
-  UINTN     NumberOfPdptEntries;
-  UINTN     NumberOfPml4Entries;
-  UINTN     NumberOfPml5Entries;
-  UINTN     SizeOfMemorySpace;
-  BOOLEAN   Nx;
-  IA32_CR4  Cr4;
-  BOOLEAN   Enable5LevelPaging;
-
-  Cr4.UintN          = AsmReadCr4 ();
-  Enable5LevelPaging = (BOOLEAN)(Cr4.Bits.LA57 == 1);
-
-  if (sizeof (UINTN) == sizeof (UINT64)) {
-    if (!Enable5LevelPaging) {
-      Pml5Entry = (UINTN)mSmmProfileCr3 | IA32_PG_P;
-      Pml5      = &Pml5Entry;
-    } else {
-      Pml5 = (UINT64 *)(UINTN)mSmmProfileCr3;
-    }
-
-    SizeOfMemorySpace = HighBitSet64 (gPhyMask) + 1;
-    ASSERT (SizeOfMemorySpace <= 52);
-
-    //
-    // Calculate the table entries of PML5E, PML4E and PDPTE.
-    //
-    NumberOfPml5Entries = 1;
-    if (SizeOfMemorySpace > 48) {
-      if (Enable5LevelPaging) {
-        NumberOfPml5Entries = (UINTN)LShiftU64 (1, SizeOfMemorySpace - 48);
-      }
-
-      SizeOfMemorySpace = 48;
-    }
-
-    NumberOfPml4Entries = 1;
-    if (SizeOfMemorySpace > 39) {
-      NumberOfPml4Entries = (UINTN)LShiftU64 (1, SizeOfMemorySpace - 39);
-      SizeOfMemorySpace   = 39;
-    }
-
-    NumberOfPdptEntries = 1;
-    ASSERT (SizeOfMemorySpace > 30);
-    NumberOfPdptEntries = (UINTN)LShiftU64 (1, SizeOfMemorySpace - 30);
+  RETURN_STATUS  Status;
+  UINTN          Index;
+  UINTN          PageTable;
+  UINT64         Base;
+  UINT64         Length;
+  UINT64         Limit;
+  UINT64         PreviousAddress;
+  UINT64         MemoryAttrMask;
+  BOOLEAN        WpEnabled;
+  BOOLEAN        CetEnabled;
+
+  PERF_FUNCTION_BEGIN ();
+
+  PageTable = AsmReadCr3 ();
+  if (sizeof (UINTN) == sizeof (UINT32)) {
+    Limit = BASE_4GB;
   } else {
-    Pml4Entry           = (UINTN)mSmmProfileCr3 | IA32_PG_P;
-    Pml4                = &Pml4Entry;
-    Pml5Entry           = (UINTN)Pml4 | IA32_PG_P;
-    Pml5                = &Pml5Entry;
-    NumberOfPml5Entries = 1;
-    NumberOfPml4Entries = 1;
-    NumberOfPdptEntries = 4;
-  }
-
-  //
-  // Go through page table and change 2MB-page into 4KB-page.
-  //
-  for (Pml5Index = 0; Pml5Index < NumberOfPml5Entries; Pml5Index++) {
-    if ((Pml5[Pml5Index] & IA32_PG_P) == 0) {
-      //
-      // If PML5 entry does not exist, skip it
-      //
-      continue;
-    }
-
-    Pml4 = (UINT64 *)(UINTN)(Pml5[Pml5Index] & PHYSICAL_ADDRESS_MASK);
-    for (Pml4Index = 0; Pml4Index < NumberOfPml4Entries; Pml4Index++) {
-      if ((Pml4[Pml4Index] & IA32_PG_P) == 0) {
-        //
-        // If PML4 entry does not exist, skip it
-        //
-        continue;
-      }
-
-      Pdpt = (UINT64 *)(UINTN)(Pml4[Pml4Index] & ~mAddressEncMask & PHYSICAL_ADDRESS_MASK);
-      for (PdptIndex = 0; PdptIndex < NumberOfPdptEntries; PdptIndex++, Pdpt++) {
-        if ((*Pdpt & IA32_PG_P) == 0) {
-          //
-          // If PDPT entry does not exist, skip it
-          //
-          continue;
-        }
-
-        if ((*Pdpt & IA32_PG_PS) != 0) {
-          //
-          // This is 1G entry, skip it
-          //
-          continue;
-        }
-
-        Pd = (UINT64 *)(UINTN)(*Pdpt & ~mAddressEncMask & PHYSICAL_ADDRESS_MASK);
-        if (Pd == 0) {
-          continue;
-        }
-
-        for (PdIndex = 0; PdIndex < SIZE_4KB / sizeof (*Pd); PdIndex++, Pd++) {
-          if ((*Pd & IA32_PG_P) == 0) {
-            //
-            // If PD entry does not exist, skip it
-            //
-            continue;
-          }
-
-          Address = (UINTN)LShiftU64 (
-                             LShiftU64 (
-                               LShiftU64 ((Pml5Index << 9) + Pml4Index, 9) + PdptIndex,
-                               9
-                               ) + PdIndex,
-                             21
-                             );
-
-          //
-          // If it is 2M page, check IsAddressSplit()
-          //
-          if (((*Pd & IA32_PG_PS) != 0) && IsAddressSplit (Address)) {
-            //
-            // Based on current page table, create 4KB page table for split area.
-            //
-            ASSERT (Address == (*Pd & PHYSICAL_ADDRESS_MASK));
-
-            Pt = AllocatePageTableMemory (1);
-            ASSERT (Pt != NULL);
-
-            // Split it
-            for (PtIndex = 0; PtIndex < SIZE_4KB / sizeof (*Pt); PtIndex++) {
-              Pt[PtIndex] = Address + ((PtIndex << 12) | mAddressEncMask | PAGE_ATTRIBUTE_BITS);
-            } // end for PT
-
-            *Pd = (UINT64)(UINTN)Pt | mAddressEncMask | PAGE_ATTRIBUTE_BITS;
-          } // end if IsAddressSplit
-        } // end for PD
-      } // end for PDPT
-    } // end for PML4
-  } // end for PML5
-
-  //
-  // Go through page table and set several page table entries to absent or execute-disable.
-  //
+    Limit = (IsRestrictedMemoryAccess ()) ? LShiftU64 (1, mPhysicalAddressBits) : BASE_4GB;
+  }
+
+  DisableReadOnlyPageWriteProtect (&WpEnabled, &CetEnabled);
+  //
+  // [0, 4k] may be non-present.
+  //
+  PreviousAddress = ((PcdGet8 (PcdNullPointerDetectionPropertyMask) & BIT1) != 0) ? BASE_4KB : 0;
+
   DEBUG ((DEBUG_INFO, "Patch page table start ...\n"));
-  for (Pml5Index = 0; Pml5Index < NumberOfPml5Entries; Pml5Index++) {
-    if ((Pml5[Pml5Index] & IA32_PG_P) == 0) {
-      //
-      // If PML5 entry does not exist, skip it
-      //
-      continue;
-    }
-
-    Pml4 = (UINT64 *)(UINTN)(Pml5[Pml5Index] & PHYSICAL_ADDRESS_MASK);
-    for (Pml4Index = 0; Pml4Index < NumberOfPml4Entries; Pml4Index++) {
-      if ((Pml4[Pml4Index] & IA32_PG_P) == 0) {
-        //
-        // If PML4 entry does not exist, skip it
-        //
-        continue;
-      }
-
-      Pdpt = (UINT64 *)(UINTN)(Pml4[Pml4Index] & ~mAddressEncMask & PHYSICAL_ADDRESS_MASK);
-      for (PdptIndex = 0; PdptIndex < NumberOfPdptEntries; PdptIndex++, Pdpt++) {
-        if ((*Pdpt & IA32_PG_P) == 0) {
-          //
-          // If PDPT entry does not exist, skip it
-          //
-          continue;
-        }
-
-        if ((*Pdpt & IA32_PG_PS) != 0) {
-          //
-          // This is 1G entry, set NX bit and skip it
-          //
-          if (mXdSupported) {
-            *Pdpt = *Pdpt | IA32_PG_NX;
-          }
-
-          continue;
-        }
-
-        Pd = (UINT64 *)(UINTN)(*Pdpt & ~mAddressEncMask & PHYSICAL_ADDRESS_MASK);
-        if (Pd == 0) {
-          continue;
-        }
-
-        for (PdIndex = 0; PdIndex < SIZE_4KB / sizeof (*Pd); PdIndex++, Pd++) {
-          if ((*Pd & IA32_PG_P) == 0) {
-            //
-            // If PD entry does not exist, skip it
-            //
-            continue;
-          }
-
-          Address = (UINTN)LShiftU64 (
-                             LShiftU64 (
-                               LShiftU64 ((Pml5Index << 9) + Pml4Index, 9) + PdptIndex,
-                               9
-                               ) + PdIndex,
-                             21
-                             );
-
-          if ((*Pd & IA32_PG_PS) != 0) {
-            // 2MB page
-
-            if (!IsAddressValid (Address, &Nx)) {
-              //
-              // Patch to remove Present flag and RW flag
-              //
-              *Pd = *Pd & (INTN)(INT32)(~PAGE_ATTRIBUTE_BITS);
-            }
-
-            if (Nx && mXdSupported) {
-              *Pd = *Pd | IA32_PG_NX;
-            }
-          } else {
-            // 4KB page
-            Pt = (UINT64 *)(UINTN)(*Pd & ~mAddressEncMask & PHYSICAL_ADDRESS_MASK);
-            if (Pt == 0) {
-              continue;
-            }
-
-            for (PtIndex = 0; PtIndex < SIZE_4KB / sizeof (*Pt); PtIndex++, Pt++) {
-              if (!IsAddressValid (Address, &Nx)) {
-                *Pt = *Pt & (INTN)(INT32)(~PAGE_ATTRIBUTE_BITS);
-              }
-
-              if (Nx && mXdSupported) {
-                *Pt = *Pt | IA32_PG_NX;
-              }
-
-              Address += SIZE_4KB;
-            } // end for PT
-          } // end if PS
-        } // end for PD
-      } // end for PDPT
-    } // end for PML4
-  } // end for PML5
+  if (FeaturePcdGet (PcdCpuSmmProfileEnable)) {
+    for (Index = 0; Index < mProtectionMemRangeCount; Index++) {
+      MemoryAttrMask = 0;
+      if (mProtectionMemRange[Index].Nx == TRUE) {
+        MemoryAttrMask |= EFI_MEMORY_XP;
+      }
+
+      if (mProtectionMemRange[Index].Present == FALSE) {
+        MemoryAttrMask = EFI_MEMORY_RP;
+      }
+
+      Base   = mProtectionMemRange[Index].Range.Base;
+      Length = mProtectionMemRange[Index].Range.Top - Base;
+      if (MemoryAttrMask != 0) {
+        Status = ConvertMemoryPageAttributes (PageTable, mPagingMode, Base, Length, MemoryAttrMask, TRUE, NULL);
+        ASSERT_RETURN_ERROR (Status);
+      }
+
+      if (Base > PreviousAddress) {
+        //
+        // Mark the ranges not in mProtectionMemRange as non-present.
+        //
+        MemoryAttrMask = EFI_MEMORY_RP;
+        Status         = ConvertMemoryPageAttributes (PageTable, mPagingMode, PreviousAddress, Base - PreviousAddress, MemoryAttrMask, TRUE, NULL);
+        ASSERT_RETURN_ERROR (Status);
+      }
+
+      PreviousAddress = Base + Length;
+    }
+
+    //
+    // This assignment is for setting the last remaining range
+    //
+    MemoryAttrMask = EFI_MEMORY_RP;
+  } else {
+    MemoryAttrMask = EFI_MEMORY_XP;
+    for (Index = 0; Index < mSmmCpuSmramRangeCount; Index++) {
+      Base = mSmmCpuSmramRanges[Index].CpuStart;
+      if (Base > PreviousAddress) {
+        //
+        // Mark the ranges not in mSmmCpuSmramRanges as NX.
+        //
+        Status = ConvertMemoryPageAttributes (PageTable, mPagingMode, PreviousAddress, Base - PreviousAddress, MemoryAttrMask, TRUE, NULL);
+        ASSERT_RETURN_ERROR (Status);
+      }
+
+      PreviousAddress = mSmmCpuSmramRanges[Index].CpuStart + mSmmCpuSmramRanges[Index].PhysicalSize;
+    }
+  }
+
+  if (PreviousAddress < Limit) {
+    //
+    // Set the last remaining range to EFI_MEMORY_RP/EFI_MEMORY_XP.
+    // This path applies to both SmmProfile enable/disable case.
+    //
+    Status = ConvertMemoryPageAttributes (PageTable, mPagingMode, PreviousAddress, Limit - PreviousAddress, MemoryAttrMask, TRUE, NULL);
+    ASSERT_RETURN_ERROR (Status);
+  }
+
+  EnableReadOnlyPageWriteProtect (WpEnabled, CetEnabled);
 
   //
@@ -811,5 +685,5 @@
   mXdEnabled = TRUE;
 
-  return;
+  PERF_FUNCTION_END ();
 }
 
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/PiSmmCpuDxeSmm/SmmProfileInternal.h
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/PiSmmCpuDxeSmm/SmmProfileInternal.h	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/PiSmmCpuDxeSmm/SmmProfileInternal.h	(revision 101291)
@@ -15,5 +15,4 @@
 #include <Library/DxeServicesTableLib.h>
 #include <Library/CpuLib.h>
-#include <Library/UefiCpuLib.h>
 #include <IndustryStandard/Acpi.h>
 
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/PiSmmCpuDxeSmm/SmramSaveState.c
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/PiSmmCpuDxeSmm/SmramSaveState.c	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/PiSmmCpuDxeSmm/SmramSaveState.c	(revision 101291)
@@ -3,4 +3,6 @@
 
 Copyright (c) 2010 - 2019, Intel Corporation. All rights reserved.<BR>
+Copyright (C) 2023 Advanced Micro Devices, Inc. All rights reserved.<BR>
+
 SPDX-License-Identifier: BSD-2-Clause-Patent
 
@@ -50,50 +52,4 @@
 
 ///
-/// Macro used to simplify the lookup table entries of type CPU_SMM_SAVE_STATE_LOOKUP_ENTRY
-///
-#define SMM_CPU_OFFSET(Field)  OFFSET_OF (SMRAM_SAVE_STATE_MAP, Field)
-
-///
-/// Macro used to simplify the lookup table entries of type CPU_SMM_SAVE_STATE_REGISTER_RANGE
-///
-#define SMM_REGISTER_RANGE(Start, End)  { Start, End, End - Start + 1 }
-
-///
-/// Structure used to describe a range of registers
-///
-typedef struct {
-  EFI_SMM_SAVE_STATE_REGISTER    Start;
-  EFI_SMM_SAVE_STATE_REGISTER    End;
-  UINTN                          Length;
-} CPU_SMM_SAVE_STATE_REGISTER_RANGE;
-
-///
-/// Structure used to build a lookup table to retrieve the widths and offsets
-/// associated with each supported EFI_SMM_SAVE_STATE_REGISTER value
-///
-
-#define SMM_SAVE_STATE_REGISTER_SMMREVID_INDEX   1
-#define SMM_SAVE_STATE_REGISTER_IOMISC_INDEX     2
-#define SMM_SAVE_STATE_REGISTER_IOMEMADDR_INDEX  3
-#define SMM_SAVE_STATE_REGISTER_MAX_INDEX        4
-
-typedef struct {
-  UINT8      Width32;
-  UINT8      Width64;
-  UINT16     Offset32;
-  UINT16     Offset64Lo;
-  UINT16     Offset64Hi;
-  BOOLEAN    Writeable;
-} CPU_SMM_SAVE_STATE_LOOKUP_ENTRY;
-
-///
-/// Structure used to build a lookup table for the IOMisc width information
-///
-typedef struct {
-  UINT8                          Width;
-  EFI_SMM_SAVE_STATE_IO_WIDTH    IoWidth;
-} CPU_SMM_SAVE_STATE_IO_WIDTH;
-
-///
 /// Variables from SMI Handler
 ///
@@ -110,459 +66,7 @@
 
 ///
-/// Table used by GetRegisterIndex() to convert an EFI_SMM_SAVE_STATE_REGISTER
-/// value to an index into a table of type CPU_SMM_SAVE_STATE_LOOKUP_ENTRY
-///
-CONST CPU_SMM_SAVE_STATE_REGISTER_RANGE  mSmmCpuRegisterRanges[] = {
-  SMM_REGISTER_RANGE (EFI_SMM_SAVE_STATE_REGISTER_GDTBASE, EFI_SMM_SAVE_STATE_REGISTER_LDTINFO),
-  SMM_REGISTER_RANGE (EFI_SMM_SAVE_STATE_REGISTER_ES,      EFI_SMM_SAVE_STATE_REGISTER_RIP),
-  SMM_REGISTER_RANGE (EFI_SMM_SAVE_STATE_REGISTER_RFLAGS,  EFI_SMM_SAVE_STATE_REGISTER_CR4),
-  { (EFI_SMM_SAVE_STATE_REGISTER)0,                        (EFI_SMM_SAVE_STATE_REGISTER)0,      0}
-};
-
-///
-/// Lookup table used to retrieve the widths and offsets associated with each
-/// supported EFI_SMM_SAVE_STATE_REGISTER value
-///
-CONST CPU_SMM_SAVE_STATE_LOOKUP_ENTRY  mSmmCpuWidthOffset[] = {
-  { 0, 0, 0,                              0,                                   0,                                   FALSE },  //  Reserved
-
-  //
-  // Internally defined CPU Save State Registers. Not defined in PI SMM CPU Protocol.
-  //
-  { 4, 4, SMM_CPU_OFFSET (x86.SMMRevId),  SMM_CPU_OFFSET (x64.SMMRevId),       0,                                   FALSE }, // SMM_SAVE_STATE_REGISTER_SMMREVID_INDEX  = 1
-  { 4, 4, SMM_CPU_OFFSET (x86.IOMisc),    SMM_CPU_OFFSET (x64.IOMisc),         0,                                   FALSE }, // SMM_SAVE_STATE_REGISTER_IOMISC_INDEX    = 2
-  { 4, 8, SMM_CPU_OFFSET (x86.IOMemAddr), SMM_CPU_OFFSET (x64.IOMemAddr),      SMM_CPU_OFFSET (x64.IOMemAddr) + 4,  FALSE }, // SMM_SAVE_STATE_REGISTER_IOMEMADDR_INDEX = 3
-
-  //
-  // CPU Save State registers defined in PI SMM CPU Protocol.
-  //
-  { 0, 8, 0,                              SMM_CPU_OFFSET (x64.GdtBaseLoDword), SMM_CPU_OFFSET (x64.GdtBaseHiDword), FALSE }, //  EFI_SMM_SAVE_STATE_REGISTER_GDTBASE  = 4
-  { 0, 8, 0,                              SMM_CPU_OFFSET (x64.IdtBaseLoDword), SMM_CPU_OFFSET (x64.IdtBaseHiDword), FALSE }, //  EFI_SMM_SAVE_STATE_REGISTER_IDTBASE  = 5
-  { 0, 8, 0,                              SMM_CPU_OFFSET (x64.LdtBaseLoDword), SMM_CPU_OFFSET (x64.LdtBaseHiDword), FALSE }, //  EFI_SMM_SAVE_STATE_REGISTER_LDTBASE  = 6
-  { 0, 0, 0,                              0,                                   0,                                   FALSE }, //  EFI_SMM_SAVE_STATE_REGISTER_GDTLIMIT = 7
-  { 0, 0, 0,                              0,                                   0,                                   FALSE }, //  EFI_SMM_SAVE_STATE_REGISTER_IDTLIMIT = 8
-  { 0, 0, 0,                              0,                                   0,                                   FALSE }, //  EFI_SMM_SAVE_STATE_REGISTER_LDTLIMIT = 9
-  { 0, 0, 0,                              0,                                   0,                                   FALSE }, //  EFI_SMM_SAVE_STATE_REGISTER_LDTINFO  = 10
-
-  { 4, 4, SMM_CPU_OFFSET (x86._ES),       SMM_CPU_OFFSET (x64._ES),            0,                                   FALSE }, //  EFI_SMM_SAVE_STATE_REGISTER_ES       = 20
-  { 4, 4, SMM_CPU_OFFSET (x86._CS),       SMM_CPU_OFFSET (x64._CS),            0,                                   FALSE }, //  EFI_SMM_SAVE_STATE_REGISTER_CS       = 21
-  { 4, 4, SMM_CPU_OFFSET (x86._SS),       SMM_CPU_OFFSET (x64._SS),            0,                                   FALSE }, //  EFI_SMM_SAVE_STATE_REGISTER_SS       = 22
-  { 4, 4, SMM_CPU_OFFSET (x86._DS),       SMM_CPU_OFFSET (x64._DS),            0,                                   FALSE }, //  EFI_SMM_SAVE_STATE_REGISTER_DS       = 23
-  { 4, 4, SMM_CPU_OFFSET (x86._FS),       SMM_CPU_OFFSET (x64._FS),            0,                                   FALSE }, //  EFI_SMM_SAVE_STATE_REGISTER_FS       = 24
-  { 4, 4, SMM_CPU_OFFSET (x86._GS),       SMM_CPU_OFFSET (x64._GS),            0,                                   FALSE }, //  EFI_SMM_SAVE_STATE_REGISTER_GS       = 25
-  { 0, 4, 0,                              SMM_CPU_OFFSET (x64._LDTR),          0,                                   FALSE }, //  EFI_SMM_SAVE_STATE_REGISTER_LDTR_SEL = 26
-  { 4, 4, SMM_CPU_OFFSET (x86._TR),       SMM_CPU_OFFSET (x64._TR),            0,                                   FALSE }, //  EFI_SMM_SAVE_STATE_REGISTER_TR_SEL   = 27
-  { 4, 8, SMM_CPU_OFFSET (x86._DR7),      SMM_CPU_OFFSET (x64._DR7),           SMM_CPU_OFFSET (x64._DR7)    + 4,    FALSE }, //  EFI_SMM_SAVE_STATE_REGISTER_DR7      = 28
-  { 4, 8, SMM_CPU_OFFSET (x86._DR6),      SMM_CPU_OFFSET (x64._DR6),           SMM_CPU_OFFSET (x64._DR6)    + 4,    FALSE }, //  EFI_SMM_SAVE_STATE_REGISTER_DR6      = 29
-  { 0, 8, 0,                              SMM_CPU_OFFSET (x64._R8),            SMM_CPU_OFFSET (x64._R8)     + 4,    TRUE  }, //  EFI_SMM_SAVE_STATE_REGISTER_R8       = 30
-  { 0, 8, 0,                              SMM_CPU_OFFSET (x64._R9),            SMM_CPU_OFFSET (x64._R9)     + 4,    TRUE  }, //  EFI_SMM_SAVE_STATE_REGISTER_R9       = 31
-  { 0, 8, 0,                              SMM_CPU_OFFSET (x64._R10),           SMM_CPU_OFFSET (x64._R10)    + 4,    TRUE  }, //  EFI_SMM_SAVE_STATE_REGISTER_R10      = 32
-  { 0, 8, 0,                              SMM_CPU_OFFSET (x64._R11),           SMM_CPU_OFFSET (x64._R11)    + 4,    TRUE  }, //  EFI_SMM_SAVE_STATE_REGISTER_R11      = 33
-  { 0, 8, 0,                              SMM_CPU_OFFSET (x64._R12),           SMM_CPU_OFFSET (x64._R12)    + 4,    TRUE  }, //  EFI_SMM_SAVE_STATE_REGISTER_R12      = 34
-  { 0, 8, 0,                              SMM_CPU_OFFSET (x64._R13),           SMM_CPU_OFFSET (x64._R13)    + 4,    TRUE  }, //  EFI_SMM_SAVE_STATE_REGISTER_R13      = 35
-  { 0, 8, 0,                              SMM_CPU_OFFSET (x64._R14),           SMM_CPU_OFFSET (x64._R14)    + 4,    TRUE  }, //  EFI_SMM_SAVE_STATE_REGISTER_R14      = 36
-  { 0, 8, 0,                              SMM_CPU_OFFSET (x64._R15),           SMM_CPU_OFFSET (x64._R15)    + 4,    TRUE  }, //  EFI_SMM_SAVE_STATE_REGISTER_R15      = 37
-  { 4, 8, SMM_CPU_OFFSET (x86._EAX),      SMM_CPU_OFFSET (x64._RAX),           SMM_CPU_OFFSET (x64._RAX)    + 4,    TRUE  }, //  EFI_SMM_SAVE_STATE_REGISTER_RAX      = 38
-  { 4, 8, SMM_CPU_OFFSET (x86._EBX),      SMM_CPU_OFFSET (x64._RBX),           SMM_CPU_OFFSET (x64._RBX)    + 4,    TRUE  }, //  EFI_SMM_SAVE_STATE_REGISTER_RBX      = 39
-  { 4, 8, SMM_CPU_OFFSET (x86._ECX),      SMM_CPU_OFFSET (x64._RCX),           SMM_CPU_OFFSET (x64._RCX)    + 4,    TRUE  }, //  EFI_SMM_SAVE_STATE_REGISTER_RCX      = 40
-  { 4, 8, SMM_CPU_OFFSET (x86._EDX),      SMM_CPU_OFFSET (x64._RDX),           SMM_CPU_OFFSET (x64._RDX)    + 4,    TRUE  }, //  EFI_SMM_SAVE_STATE_REGISTER_RDX      = 41
-  { 4, 8, SMM_CPU_OFFSET (x86._ESP),      SMM_CPU_OFFSET (x64._RSP),           SMM_CPU_OFFSET (x64._RSP)    + 4,    TRUE  }, //  EFI_SMM_SAVE_STATE_REGISTER_RSP      = 42
-  { 4, 8, SMM_CPU_OFFSET (x86._EBP),      SMM_CPU_OFFSET (x64._RBP),           SMM_CPU_OFFSET (x64._RBP)    + 4,    TRUE  }, //  EFI_SMM_SAVE_STATE_REGISTER_RBP      = 43
-  { 4, 8, SMM_CPU_OFFSET (x86._ESI),      SMM_CPU_OFFSET (x64._RSI),           SMM_CPU_OFFSET (x64._RSI)    + 4,    TRUE  }, //  EFI_SMM_SAVE_STATE_REGISTER_RSI      = 44
-  { 4, 8, SMM_CPU_OFFSET (x86._EDI),      SMM_CPU_OFFSET (x64._RDI),           SMM_CPU_OFFSET (x64._RDI)    + 4,    TRUE  }, //  EFI_SMM_SAVE_STATE_REGISTER_RDI      = 45
-  { 4, 8, SMM_CPU_OFFSET (x86._EIP),      SMM_CPU_OFFSET (x64._RIP),           SMM_CPU_OFFSET (x64._RIP)    + 4,    TRUE  }, //  EFI_SMM_SAVE_STATE_REGISTER_RIP      = 46
-
-  { 4, 8, SMM_CPU_OFFSET (x86._EFLAGS),   SMM_CPU_OFFSET (x64._RFLAGS),        SMM_CPU_OFFSET (x64._RFLAGS) + 4,    TRUE  }, //  EFI_SMM_SAVE_STATE_REGISTER_RFLAGS   = 51
-  { 4, 8, SMM_CPU_OFFSET (x86._CR0),      SMM_CPU_OFFSET (x64._CR0),           SMM_CPU_OFFSET (x64._CR0)    + 4,    FALSE }, //  EFI_SMM_SAVE_STATE_REGISTER_CR0      = 52
-  { 4, 8, SMM_CPU_OFFSET (x86._CR3),      SMM_CPU_OFFSET (x64._CR3),           SMM_CPU_OFFSET (x64._CR3)    + 4,    FALSE }, //  EFI_SMM_SAVE_STATE_REGISTER_CR3      = 53
-  { 0, 4, 0,                              SMM_CPU_OFFSET (x64._CR4),           0,                                   FALSE }, //  EFI_SMM_SAVE_STATE_REGISTER_CR4      = 54
-};
-
-///
-/// Lookup table for the IOMisc width information
-///
-CONST CPU_SMM_SAVE_STATE_IO_WIDTH  mSmmCpuIoWidth[] = {
-  { 0, EFI_SMM_SAVE_STATE_IO_WIDTH_UINT8  },  // Undefined           = 0
-  { 1, EFI_SMM_SAVE_STATE_IO_WIDTH_UINT8  },  // SMM_IO_LENGTH_BYTE  = 1
-  { 2, EFI_SMM_SAVE_STATE_IO_WIDTH_UINT16 },  // SMM_IO_LENGTH_WORD  = 2
-  { 0, EFI_SMM_SAVE_STATE_IO_WIDTH_UINT8  },  // Undefined           = 3
-  { 4, EFI_SMM_SAVE_STATE_IO_WIDTH_UINT32 },  // SMM_IO_LENGTH_DWORD = 4
-  { 0, EFI_SMM_SAVE_STATE_IO_WIDTH_UINT8  },  // Undefined           = 5
-  { 0, EFI_SMM_SAVE_STATE_IO_WIDTH_UINT8  },  // Undefined           = 6
-  { 0, EFI_SMM_SAVE_STATE_IO_WIDTH_UINT8  }   // Undefined           = 7
-};
-
-///
-/// Lookup table for the IOMisc type information
-///
-CONST EFI_SMM_SAVE_STATE_IO_TYPE  mSmmCpuIoType[] = {
-  EFI_SMM_SAVE_STATE_IO_TYPE_OUTPUT,     // SMM_IO_TYPE_OUT_DX        = 0
-  EFI_SMM_SAVE_STATE_IO_TYPE_INPUT,      // SMM_IO_TYPE_IN_DX         = 1
-  EFI_SMM_SAVE_STATE_IO_TYPE_STRING,     // SMM_IO_TYPE_OUTS          = 2
-  EFI_SMM_SAVE_STATE_IO_TYPE_STRING,     // SMM_IO_TYPE_INS           = 3
-  (EFI_SMM_SAVE_STATE_IO_TYPE)0,         // Undefined                 = 4
-  (EFI_SMM_SAVE_STATE_IO_TYPE)0,         // Undefined                 = 5
-  EFI_SMM_SAVE_STATE_IO_TYPE_REP_PREFIX, // SMM_IO_TYPE_REP_OUTS      = 6
-  EFI_SMM_SAVE_STATE_IO_TYPE_REP_PREFIX, // SMM_IO_TYPE_REP_INS       = 7
-  EFI_SMM_SAVE_STATE_IO_TYPE_OUTPUT,     // SMM_IO_TYPE_OUT_IMMEDIATE = 8
-  EFI_SMM_SAVE_STATE_IO_TYPE_INPUT,      // SMM_IO_TYPE_OUT_IMMEDIATE = 9
-  (EFI_SMM_SAVE_STATE_IO_TYPE)0,         // Undefined                 = 10
-  (EFI_SMM_SAVE_STATE_IO_TYPE)0,         // Undefined                 = 11
-  (EFI_SMM_SAVE_STATE_IO_TYPE)0,         // Undefined                 = 12
-  (EFI_SMM_SAVE_STATE_IO_TYPE)0,         // Undefined                 = 13
-  (EFI_SMM_SAVE_STATE_IO_TYPE)0,         // Undefined                 = 14
-  (EFI_SMM_SAVE_STATE_IO_TYPE)0          // Undefined                 = 15
-};
-
-///
 /// The mode of the CPU at the time an SMI occurs
 ///
 UINT8  mSmmSaveStateRegisterLma;
-
-/**
-  Read information from the CPU save state.
-
-  @param  Register  Specifies the CPU register to read form the save state.
-
-  @retval 0   Register is not valid
-  @retval >0  Index into mSmmCpuWidthOffset[] associated with Register
-
-**/
-UINTN
-GetRegisterIndex (
-  IN EFI_SMM_SAVE_STATE_REGISTER  Register
-  )
-{
-  UINTN  Index;
-  UINTN  Offset;
-
-  for (Index = 0, Offset = SMM_SAVE_STATE_REGISTER_MAX_INDEX; mSmmCpuRegisterRanges[Index].Length != 0; Index++) {
-    if ((Register >= mSmmCpuRegisterRanges[Index].Start) && (Register <= mSmmCpuRegisterRanges[Index].End)) {
-      return Register - mSmmCpuRegisterRanges[Index].Start + Offset;
-    }
-
-    Offset += mSmmCpuRegisterRanges[Index].Length;
-  }
-
-  return 0;
-}
-
-/**
-  Read a CPU Save State register on the target processor.
-
-  This function abstracts the differences that whether the CPU Save State register is in the
-  IA32 CPU Save State Map or X64 CPU Save State Map.
-
-  This function supports reading a CPU Save State register in SMBase relocation handler.
-
-  @param[in]  CpuIndex       Specifies the zero-based index of the CPU save state.
-  @param[in]  RegisterIndex  Index into mSmmCpuWidthOffset[] look up table.
-  @param[in]  Width          The number of bytes to read from the CPU save state.
-  @param[out] Buffer         Upon return, this holds the CPU register value read from the save state.
-
-  @retval EFI_SUCCESS           The register was read from Save State.
-  @retval EFI_NOT_FOUND         The register is not defined for the Save State of Processor.
-  @retval EFI_INVALID_PARAMETER  This or Buffer is NULL.
-
-**/
-EFI_STATUS
-ReadSaveStateRegisterByIndex (
-  IN UINTN  CpuIndex,
-  IN UINTN  RegisterIndex,
-  IN UINTN  Width,
-  OUT VOID  *Buffer
-  )
-{
-  SMRAM_SAVE_STATE_MAP  *CpuSaveState;
-
-  if (RegisterIndex == 0) {
-    return EFI_NOT_FOUND;
-  }
-
-  CpuSaveState = gSmst->CpuSaveState[CpuIndex];
-
-  if (mSmmSaveStateRegisterLma == EFI_SMM_SAVE_STATE_REGISTER_LMA_32BIT) {
-    //
-    // If 32-bit mode width is zero, then the specified register can not be accessed
-    //
-    if (mSmmCpuWidthOffset[RegisterIndex].Width32 == 0) {
-      return EFI_NOT_FOUND;
-    }
-
-    //
-    // If Width is bigger than the 32-bit mode width, then the specified register can not be accessed
-    //
-    if (Width > mSmmCpuWidthOffset[RegisterIndex].Width32) {
-      return EFI_INVALID_PARAMETER;
-    }
-
-    //
-    // Write return buffer
-    //
-    ASSERT (CpuSaveState != NULL);
-    CopyMem (Buffer, (UINT8 *)CpuSaveState + mSmmCpuWidthOffset[RegisterIndex].Offset32, Width);
-  } else {
-    //
-    // If 64-bit mode width is zero, then the specified register can not be accessed
-    //
-    if (mSmmCpuWidthOffset[RegisterIndex].Width64 == 0) {
-      return EFI_NOT_FOUND;
-    }
-
-    //
-    // If Width is bigger than the 64-bit mode width, then the specified register can not be accessed
-    //
-    if (Width > mSmmCpuWidthOffset[RegisterIndex].Width64) {
-      return EFI_INVALID_PARAMETER;
-    }
-
-    //
-    // Write at most 4 of the lower bytes of the return buffer
-    //
-    CopyMem (Buffer, (UINT8 *)CpuSaveState + mSmmCpuWidthOffset[RegisterIndex].Offset64Lo, MIN (4, Width));
-    if (Width > 4) {
-      //
-      // Write at most 4 of the upper bytes of the return buffer
-      //
-      CopyMem ((UINT8 *)Buffer + 4, (UINT8 *)CpuSaveState + mSmmCpuWidthOffset[RegisterIndex].Offset64Hi, Width - 4);
-    }
-  }
-
-  return EFI_SUCCESS;
-}
-
-/**
-  Read a CPU Save State register on the target processor.
-
-  This function abstracts the differences that whether the CPU Save State register is in the
-  IA32 CPU Save State Map or X64 CPU Save State Map.
-
-  This function supports reading a CPU Save State register in SMBase relocation handler.
-
-  @param[in]  CpuIndex       Specifies the zero-based index of the CPU save state.
-  @param[in]  RegisterIndex  Index into mSmmCpuWidthOffset[] look up table.
-  @param[in]  Width          The number of bytes to read from the CPU save state.
-  @param[out] Buffer         Upon return, this holds the CPU register value read from the save state.
-
-  @retval EFI_SUCCESS           The register was read from Save State.
-  @retval EFI_NOT_FOUND         The register is not defined for the Save State of Processor.
-  @retval EFI_INVALID_PARAMETER Buffer is NULL, or Width does not meet requirement per Register type.
-
-**/
-EFI_STATUS
-EFIAPI
-ReadSaveStateRegister (
-  IN UINTN                        CpuIndex,
-  IN EFI_SMM_SAVE_STATE_REGISTER  Register,
-  IN UINTN                        Width,
-  OUT VOID                        *Buffer
-  )
-{
-  UINT32                      SmmRevId;
-  SMRAM_SAVE_STATE_IOMISC     IoMisc;
-  EFI_SMM_SAVE_STATE_IO_INFO  *IoInfo;
-
-  //
-  // Check for special EFI_SMM_SAVE_STATE_REGISTER_LMA
-  //
-  if (Register == EFI_SMM_SAVE_STATE_REGISTER_LMA) {
-    //
-    // Only byte access is supported for this register
-    //
-    if (Width != 1) {
-      return EFI_INVALID_PARAMETER;
-    }
-
-    *(UINT8 *)Buffer = mSmmSaveStateRegisterLma;
-
-    return EFI_SUCCESS;
-  }
-
-  //
-  // Check for special EFI_SMM_SAVE_STATE_REGISTER_IO
-  //
-  if (Register == EFI_SMM_SAVE_STATE_REGISTER_IO) {
-    //
-    // Get SMM Revision ID
-    //
-    ReadSaveStateRegisterByIndex (CpuIndex, SMM_SAVE_STATE_REGISTER_SMMREVID_INDEX, sizeof (SmmRevId), &SmmRevId);
-
-    //
-    // See if the CPU supports the IOMisc register in the save state
-    //
-    if (SmmRevId < SMRAM_SAVE_STATE_MIN_REV_ID_IOMISC) {
-      return EFI_NOT_FOUND;
-    }
-
-    //
-    // Get the IOMisc register value
-    //
-    ReadSaveStateRegisterByIndex (CpuIndex, SMM_SAVE_STATE_REGISTER_IOMISC_INDEX, sizeof (IoMisc.Uint32), &IoMisc.Uint32);
-
-    //
-    // Check for the SMI_FLAG in IOMisc
-    //
-    if (IoMisc.Bits.SmiFlag == 0) {
-      return EFI_NOT_FOUND;
-    }
-
-    //
-    // Only support IN/OUT, but not INS/OUTS/REP INS/REP OUTS.
-    //
-    if ((mSmmCpuIoType[IoMisc.Bits.Type] != EFI_SMM_SAVE_STATE_IO_TYPE_INPUT) &&
-        (mSmmCpuIoType[IoMisc.Bits.Type] != EFI_SMM_SAVE_STATE_IO_TYPE_OUTPUT))
-    {
-      return EFI_NOT_FOUND;
-    }
-
-    //
-    // Compute index for the I/O Length and I/O Type lookup tables
-    //
-    if ((mSmmCpuIoWidth[IoMisc.Bits.Length].Width == 0) || (mSmmCpuIoType[IoMisc.Bits.Type] == 0)) {
-      return EFI_NOT_FOUND;
-    }
-
-    //
-    // Make sure the incoming buffer is large enough to hold IoInfo before accessing
-    //
-    if (Width < sizeof (EFI_SMM_SAVE_STATE_IO_INFO)) {
-      return EFI_INVALID_PARAMETER;
-    }
-
-    //
-    // Zero the IoInfo structure that will be returned in Buffer
-    //
-    IoInfo = (EFI_SMM_SAVE_STATE_IO_INFO *)Buffer;
-    ZeroMem (IoInfo, sizeof (EFI_SMM_SAVE_STATE_IO_INFO));
-
-    //
-    // Use lookup tables to help fill in all the fields of the IoInfo structure
-    //
-    IoInfo->IoPort  = (UINT16)IoMisc.Bits.Port;
-    IoInfo->IoWidth = mSmmCpuIoWidth[IoMisc.Bits.Length].IoWidth;
-    IoInfo->IoType  = mSmmCpuIoType[IoMisc.Bits.Type];
-    ReadSaveStateRegister (CpuIndex, EFI_SMM_SAVE_STATE_REGISTER_RAX, mSmmCpuIoWidth[IoMisc.Bits.Length].Width, &IoInfo->IoData);
-    return EFI_SUCCESS;
-  }
-
-  //
-  // Convert Register to a register lookup table index
-  //
-  return ReadSaveStateRegisterByIndex (CpuIndex, GetRegisterIndex (Register), Width, Buffer);
-}
-
-/**
-  Write value to a CPU Save State register on the target processor.
-
-  This function abstracts the differences that whether the CPU Save State register is in the
-  IA32 CPU Save State Map or X64 CPU Save State Map.
-
-  This function supports writing a CPU Save State register in SMBase relocation handler.
-
-  @param[in] CpuIndex       Specifies the zero-based index of the CPU save state.
-  @param[in] RegisterIndex  Index into mSmmCpuWidthOffset[] look up table.
-  @param[in] Width          The number of bytes to read from the CPU save state.
-  @param[in] Buffer         Upon entry, this holds the new CPU register value.
-
-  @retval EFI_SUCCESS           The register was written to Save State.
-  @retval EFI_NOT_FOUND         The register is not defined for the Save State of Processor.
-  @retval EFI_INVALID_PARAMETER  ProcessorIndex or Width is not correct.
-
-**/
-EFI_STATUS
-EFIAPI
-WriteSaveStateRegister (
-  IN UINTN                        CpuIndex,
-  IN EFI_SMM_SAVE_STATE_REGISTER  Register,
-  IN UINTN                        Width,
-  IN CONST VOID                   *Buffer
-  )
-{
-  UINTN                 RegisterIndex;
-  SMRAM_SAVE_STATE_MAP  *CpuSaveState;
-
-  //
-  // Writes to EFI_SMM_SAVE_STATE_REGISTER_LMA are ignored
-  //
-  if (Register == EFI_SMM_SAVE_STATE_REGISTER_LMA) {
-    return EFI_SUCCESS;
-  }
-
-  //
-  // Writes to EFI_SMM_SAVE_STATE_REGISTER_IO are not supported
-  //
-  if (Register == EFI_SMM_SAVE_STATE_REGISTER_IO) {
-    return EFI_NOT_FOUND;
-  }
-
-  //
-  // Convert Register to a register lookup table index
-  //
-  RegisterIndex = GetRegisterIndex (Register);
-  if (RegisterIndex == 0) {
-    return EFI_NOT_FOUND;
-  }
-
-  CpuSaveState = gSmst->CpuSaveState[CpuIndex];
-
-  //
-  // Do not write non-writable SaveState, because it will cause exception.
-  //
-  if (!mSmmCpuWidthOffset[RegisterIndex].Writeable) {
-    return EFI_UNSUPPORTED;
-  }
-
-  //
-  // Check CPU mode
-  //
-  if (mSmmSaveStateRegisterLma == EFI_SMM_SAVE_STATE_REGISTER_LMA_32BIT) {
-    //
-    // If 32-bit mode width is zero, then the specified register can not be accessed
-    //
-    if (mSmmCpuWidthOffset[RegisterIndex].Width32 == 0) {
-      return EFI_NOT_FOUND;
-    }
-
-    //
-    // If Width is bigger than the 32-bit mode width, then the specified register can not be accessed
-    //
-    if (Width > mSmmCpuWidthOffset[RegisterIndex].Width32) {
-      return EFI_INVALID_PARAMETER;
-    }
-
-    //
-    // Write SMM State register
-    //
-    ASSERT (CpuSaveState != NULL);
-    CopyMem ((UINT8 *)CpuSaveState + mSmmCpuWidthOffset[RegisterIndex].Offset32, Buffer, Width);
-  } else {
-    //
-    // If 64-bit mode width is zero, then the specified register can not be accessed
-    //
-    if (mSmmCpuWidthOffset[RegisterIndex].Width64 == 0) {
-      return EFI_NOT_FOUND;
-    }
-
-    //
-    // If Width is bigger than the 64-bit mode width, then the specified register can not be accessed
-    //
-    if (Width > mSmmCpuWidthOffset[RegisterIndex].Width64) {
-      return EFI_INVALID_PARAMETER;
-    }
-
-    //
-    // Write at most 4 of the lower bytes of SMM State register
-    //
-    CopyMem ((UINT8 *)CpuSaveState + mSmmCpuWidthOffset[RegisterIndex].Offset64Lo, Buffer, MIN (4, Width));
-    if (Width > 4) {
-      //
-      // Write at most 4 of the upper bytes of SMM State register
-      //
-      CopyMem ((UINT8 *)CpuSaveState + mSmmCpuWidthOffset[RegisterIndex].Offset64Hi, (UINT8 *)Buffer + 4, Width - 4);
-    }
-  }
-
-  return EFI_SUCCESS;
-}
 
 /**
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/PiSmmCpuDxeSmm/X64/PageTbl.c
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/PiSmmCpuDxeSmm/X64/PageTbl.c	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/PiSmmCpuDxeSmm/X64/PageTbl.c	(revision 101291)
@@ -2,5 +2,5 @@
 Page Fault (#PF) handler for X64 processors
 
-Copyright (c) 2009 - 2022, Intel Corporation. All rights reserved.<BR>
+Copyright (c) 2009 - 2023, Intel Corporation. All rights reserved.<BR>
 Copyright (c) 2017, AMD Incorporated. All rights reserved.<BR>
 
@@ -13,6 +13,4 @@
 #define PAGE_TABLE_PAGES  8
 #define ACC_MAX_BIT       BIT3
-
-extern UINTN  mSmmShadowStackSize;
 
 LIST_ENTRY                mPagePool           = INITIALIZE_LIST_HEAD_VARIABLE (mPagePool);
@@ -171,158 +169,4 @@
 
 /**
-  Set static page table.
-
-  @param[in] PageTable              Address of page table.
-  @param[in] PhysicalAddressBits    The maximum physical address bits supported.
-**/
-VOID
-SetStaticPageTable (
-  IN UINTN  PageTable,
-  IN UINT8  PhysicalAddressBits
-  )
-{
-  UINT64  PageAddress;
-  UINTN   NumberOfPml5EntriesNeeded;
-  UINTN   NumberOfPml4EntriesNeeded;
-  UINTN   NumberOfPdpEntriesNeeded;
-  UINTN   IndexOfPml5Entries;
-  UINTN   IndexOfPml4Entries;
-  UINTN   IndexOfPdpEntries;
-  UINTN   IndexOfPageDirectoryEntries;
-  UINT64  *PageMapLevel5Entry;
-  UINT64  *PageMapLevel4Entry;
-  UINT64  *PageMap;
-  UINT64  *PageDirectoryPointerEntry;
-  UINT64  *PageDirectory1GEntry;
-  UINT64  *PageDirectoryEntry;
-
-  //
-  // IA-32e paging translates 48-bit linear addresses to 52-bit physical addresses
-  //  when 5-Level Paging is disabled.
-  //
-  ASSERT (PhysicalAddressBits <= 52);
-  if (!m5LevelPagingNeeded && (PhysicalAddressBits > 48)) {
-    PhysicalAddressBits = 48;
-  }
-
-  NumberOfPml5EntriesNeeded = 1;
-  if (PhysicalAddressBits > 48) {
-    NumberOfPml5EntriesNeeded = (UINTN)LShiftU64 (1, PhysicalAddressBits - 48);
-    PhysicalAddressBits       = 48;
-  }
-
-  NumberOfPml4EntriesNeeded = 1;
-  if (PhysicalAddressBits > 39) {
-    NumberOfPml4EntriesNeeded = (UINTN)LShiftU64 (1, PhysicalAddressBits - 39);
-    PhysicalAddressBits       = 39;
-  }
-
-  NumberOfPdpEntriesNeeded = 1;
-  ASSERT (PhysicalAddressBits > 30);
-  NumberOfPdpEntriesNeeded = (UINTN)LShiftU64 (1, PhysicalAddressBits - 30);
-
-  //
-  // By architecture only one PageMapLevel4 exists - so lets allocate storage for it.
-  //
-  PageMap = (VOID *)PageTable;
-
-  PageMapLevel4Entry = PageMap;
-  PageMapLevel5Entry = NULL;
-  if (m5LevelPagingNeeded) {
-    //
-    // By architecture only one PageMapLevel5 exists - so lets allocate storage for it.
-    //
-    PageMapLevel5Entry = PageMap;
-  }
-
-  PageAddress = 0;
-
-  for ( IndexOfPml5Entries = 0
-        ; IndexOfPml5Entries < NumberOfPml5EntriesNeeded
-        ; IndexOfPml5Entries++, PageMapLevel5Entry++)
-  {
-    //
-    // Each PML5 entry points to a page of PML4 entires.
-    // So lets allocate space for them and fill them in in the IndexOfPml4Entries loop.
-    // When 5-Level Paging is disabled, below allocation happens only once.
-    //
-    if (m5LevelPagingNeeded) {
-      PageMapLevel4Entry = (UINT64 *)((*PageMapLevel5Entry) & ~mAddressEncMask & gPhyMask);
-      if (PageMapLevel4Entry == NULL) {
-        PageMapLevel4Entry = AllocatePageTableMemory (1);
-        ASSERT (PageMapLevel4Entry != NULL);
-        ZeroMem (PageMapLevel4Entry, EFI_PAGES_TO_SIZE (1));
-
-        *PageMapLevel5Entry = (UINT64)(UINTN)PageMapLevel4Entry | mAddressEncMask | PAGE_ATTRIBUTE_BITS;
-      }
-    }
-
-    for (IndexOfPml4Entries = 0; IndexOfPml4Entries < (NumberOfPml5EntriesNeeded == 1 ? NumberOfPml4EntriesNeeded : 512); IndexOfPml4Entries++, PageMapLevel4Entry++) {
-      //
-      // Each PML4 entry points to a page of Page Directory Pointer entries.
-      //
-      PageDirectoryPointerEntry = (UINT64 *)((*PageMapLevel4Entry) & ~mAddressEncMask & gPhyMask);
-      if (PageDirectoryPointerEntry == NULL) {
-        PageDirectoryPointerEntry = AllocatePageTableMemory (1);
-        ASSERT (PageDirectoryPointerEntry != NULL);
-        ZeroMem (PageDirectoryPointerEntry, EFI_PAGES_TO_SIZE (1));
-
-        *PageMapLevel4Entry = (UINT64)(UINTN)PageDirectoryPointerEntry | mAddressEncMask | PAGE_ATTRIBUTE_BITS;
-      }
-
-      if (m1GPageTableSupport) {
-        PageDirectory1GEntry = PageDirectoryPointerEntry;
-        for (IndexOfPageDirectoryEntries = 0; IndexOfPageDirectoryEntries < 512; IndexOfPageDirectoryEntries++, PageDirectory1GEntry++, PageAddress += SIZE_1GB) {
-          if ((IndexOfPml4Entries == 0) && (IndexOfPageDirectoryEntries < 4)) {
-            //
-            // Skip the < 4G entries
-            //
-            continue;
-          }
-
-          //
-          // Fill in the Page Directory entries
-          //
-          *PageDirectory1GEntry = PageAddress | mAddressEncMask | IA32_PG_PS | PAGE_ATTRIBUTE_BITS;
-        }
-      } else {
-        PageAddress = BASE_4GB;
-        for (IndexOfPdpEntries = 0; IndexOfPdpEntries < (NumberOfPml4EntriesNeeded == 1 ? NumberOfPdpEntriesNeeded : 512); IndexOfPdpEntries++, PageDirectoryPointerEntry++) {
-          if ((IndexOfPml4Entries == 0) && (IndexOfPdpEntries < 4)) {
-            //
-            // Skip the < 4G entries
-            //
-            continue;
-          }
-
-          //
-          // Each Directory Pointer entries points to a page of Page Directory entires.
-          // So allocate space for them and fill them in in the IndexOfPageDirectoryEntries loop.
-          //
-          PageDirectoryEntry = (UINT64 *)((*PageDirectoryPointerEntry) & ~mAddressEncMask & gPhyMask);
-          if (PageDirectoryEntry == NULL) {
-            PageDirectoryEntry = AllocatePageTableMemory (1);
-            ASSERT (PageDirectoryEntry != NULL);
-            ZeroMem (PageDirectoryEntry, EFI_PAGES_TO_SIZE (1));
-
-            //
-            // Fill in a Page Directory Pointer Entries
-            //
-            *PageDirectoryPointerEntry = (UINT64)(UINTN)PageDirectoryEntry | mAddressEncMask | PAGE_ATTRIBUTE_BITS;
-          }
-
-          for (IndexOfPageDirectoryEntries = 0; IndexOfPageDirectoryEntries < 512; IndexOfPageDirectoryEntries++, PageDirectoryEntry++, PageAddress += SIZE_2MB) {
-            //
-            // Fill in the Page Directory entries
-            //
-            *PageDirectoryEntry = PageAddress | mAddressEncMask | IA32_PG_PS | PAGE_ATTRIBUTE_BITS;
-          }
-        }
-      }
-    }
-  }
-}
-
-/**
   Create PageTable for SMM use.
 
@@ -335,6 +179,5 @@
   )
 {
-  EFI_PHYSICAL_ADDRESS      Pages;
-  UINT64                    *PTEntry;
+  UINTN                     PageTable;
   LIST_ENTRY                *FreePage;
   UINTN                     Index;
@@ -342,6 +185,8 @@
   IA32_IDT_GATE_DESCRIPTOR  *IdtEntry;
   EFI_STATUS                Status;
+  UINT64                    *PdptEntry;
   UINT64                    *Pml4Entry;
   UINT64                    *Pml5Entry;
+  UINT8                     PhysicalAddressBits;
 
   //
@@ -355,57 +200,47 @@
   mPhysicalAddressBits          = CalculateMaximumSupportAddress ();
   PatchInstructionX86 (gPatch5LevelPagingNeeded, m5LevelPagingNeeded, 1);
+  if (m5LevelPagingNeeded) {
+    mPagingMode = m1GPageTableSupport ? Paging5Level1GB : Paging5Level;
+  } else {
+    mPagingMode = m1GPageTableSupport ? Paging4Level1GB : Paging4Level;
+  }
+
   DEBUG ((DEBUG_INFO, "5LevelPaging Needed             - %d\n", m5LevelPagingNeeded));
   DEBUG ((DEBUG_INFO, "1GPageTable Support             - %d\n", m1GPageTableSupport));
   DEBUG ((DEBUG_INFO, "PcdCpuSmmRestrictedMemoryAccess - %d\n", mCpuSmmRestrictedMemoryAccess));
   DEBUG ((DEBUG_INFO, "PhysicalAddressBits             - %d\n", mPhysicalAddressBits));
-  //
-  // Generate PAE page table for the first 4GB memory space
-  //
-  Pages = Gen4GPageTable (FALSE);
-
-  //
-  // Set IA32_PG_PMNT bit to mask this entry
-  //
-  PTEntry = (UINT64 *)(UINTN)Pages;
+
+  //
+  // Generate initial SMM page table.
+  // Only map [0, 4G] when PcdCpuSmmRestrictedMemoryAccess is FALSE.
+  //
+  PhysicalAddressBits = mCpuSmmRestrictedMemoryAccess ? mPhysicalAddressBits : 32;
+  PageTable           = GenSmmPageTable (mPagingMode, PhysicalAddressBits);
+
+  if (m5LevelPagingNeeded) {
+    Pml5Entry = (UINT64 *)PageTable;
+    //
+    // Set Pml5Entry sub-entries number for smm PF handler usage.
+    //
+    SetSubEntriesNum (Pml5Entry, 1);
+    Pml4Entry = (UINT64 *)((*Pml5Entry) & ~mAddressEncMask & gPhyMask);
+  } else {
+    Pml4Entry = (UINT64 *)PageTable;
+  }
+
+  //
+  // Set IA32_PG_PMNT bit to mask first 4 PdptEntry.
+  //
+  PdptEntry = (UINT64 *)((*Pml4Entry) & ~mAddressEncMask & gPhyMask);
   for (Index = 0; Index < 4; Index++) {
-    PTEntry[Index] |= IA32_PG_PMNT;
-  }
-
-  //
-  // Fill Page-Table-Level4 (PML4) entry
-  //
-  Pml4Entry = (UINT64 *)AllocatePageTableMemory (1);
-  ASSERT (Pml4Entry != NULL);
-  *Pml4Entry = Pages | mAddressEncMask | PAGE_ATTRIBUTE_BITS;
-  ZeroMem (Pml4Entry + 1, EFI_PAGE_SIZE - sizeof (*Pml4Entry));
-
-  //
-  // Set sub-entries number
-  //
-  SetSubEntriesNum (Pml4Entry, 3);
-  PTEntry = Pml4Entry;
-
-  if (m5LevelPagingNeeded) {
-    //
-    // Fill PML5 entry
-    //
-    Pml5Entry = (UINT64 *)AllocatePageTableMemory (1);
-    ASSERT (Pml5Entry != NULL);
-    *Pml5Entry = (UINTN)Pml4Entry | mAddressEncMask | PAGE_ATTRIBUTE_BITS;
-    ZeroMem (Pml5Entry + 1, EFI_PAGE_SIZE - sizeof (*Pml5Entry));
-    //
-    // Set sub-entries number
-    //
-    SetSubEntriesNum (Pml5Entry, 1);
-    PTEntry = Pml5Entry;
-  }
-
-  if (mCpuSmmRestrictedMemoryAccess) {
-    //
-    // When access to non-SMRAM memory is restricted, create page table
-    // that covers all memory space.
-    //
-    SetStaticPageTable ((UINTN)PTEntry, mPhysicalAddressBits);
-  } else {
+    PdptEntry[Index] |= IA32_PG_PMNT;
+  }
+
+  if (!mCpuSmmRestrictedMemoryAccess) {
+    //
+    // Set Pml4Entry sub-entries number for smm PF handler usage.
+    //
+    SetSubEntriesNum (Pml4Entry, 3);
+
     //
     // Add pages to page pool
@@ -464,5 +299,5 @@
   // Return the address of PML4/PML5 (to set CR3)
   //
-  return (UINT32)(UINTN)PTEntry;
+  return (UINT32)PageTable;
 }
 
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/PiSmmCpuDxeSmm/X64/SmmFuncsArch.c
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/PiSmmCpuDxeSmm/X64/SmmFuncsArch.c	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/PiSmmCpuDxeSmm/X64/SmmFuncsArch.c	(revision 101291)
@@ -2,5 +2,5 @@
   SMM CPU misc functions for x64 arch specific.
 
-Copyright (c) 2015 - 2019, Intel Corporation. All rights reserved.<BR>
+Copyright (c) 2015 - 2023, Intel Corporation. All rights reserved.<BR>
 SPDX-License-Identifier: BSD-2-Clause-Patent
 
@@ -13,5 +13,4 @@
 
 extern BOOLEAN  mCetSupported;
-extern UINTN    mSmmShadowStackSize;
 
 X86_ASSEMBLY_PATCH_LABEL  mPatchCetPl0Ssp;
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/PiSmmCpuDxeSmm/X64/SmmProfileArch.c
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/PiSmmCpuDxeSmm/X64/SmmProfileArch.c	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/PiSmmCpuDxeSmm/X64/SmmProfileArch.c	(revision 101291)
@@ -36,24 +36,12 @@
   )
 {
-  EFI_PHYSICAL_ADDRESS  Pages;
-  UINT64                *PTEntry;
-
-  //
-  // Generate PAE page table for the first 4GB memory space
-  //
-  Pages = Gen4GPageTable (FALSE);
-
-  //
-  // Fill Page-Table-Level4 (PML4) entry
-  //
-  PTEntry = (UINT64 *)AllocatePageTableMemory (1);
-  ASSERT (PTEntry != NULL);
-  *PTEntry = Pages | mAddressEncMask | PAGE_ATTRIBUTE_BITS;
-  ZeroMem (PTEntry + 1, EFI_PAGE_SIZE - sizeof (*PTEntry));
-
-  //
+  //
+  // Generate level4 page table for the first 4GB memory space
   // Return the address of PML4 (to set CR3)
   //
-  mSmmS3ResumeState->SmmS3Cr3 = (UINT32)(UINTN)PTEntry;
+  //
+  // The SmmS3Cr3 is only used by S3Resume PEIM to switch CPU from 32bit to 64bit
+  //
+  mSmmS3ResumeState->SmmS3Cr3 = (UINT32)GenSmmPageTable (Paging4Level, 32);
 
   return;
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/ResetVector/Vtf0/Build.py
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/ResetVector/Vtf0/Build.py	(revision 101283)
+++ 	(revision )
@@ -1,94 +1,0 @@
-## @file
-#  Automate the process of building the various reset vector types
-#
-#  Copyright (c) 2009 - 2021, Intel Corporation. All rights reserved.<BR>
-#
-#  SPDX-License-Identifier: BSD-2-Clause-Patent
-#
-
-import os
-import subprocess
-import sys
-
-# VBox begin
-print 'VBox: We do not use this build script, the reset vector binaries are built by the top-level EFI makefile!';
-sys.exit(1);
-# VBox end
-
-PAGE_TABLE_2M  = 'PageTable2M'
-PAGE_TABLE_1G  = 'PageTable1G'
-FILE_FORMAT    = '.raw'
-ALL_RAW_FORMAT = '*' + FILE_FORMAT
-IA32           = 'IA32'
-X64            = 'X64'
-
-# Pre-Define a Macros for Page Table
-PAGE_TABLES = {
-    PAGE_TABLE_2M : "PAGE_TABLE_2M",
-    PAGE_TABLE_1G : "PAGE_TABLE_1G"
-}
-
-def RunCommand(commandLine):
-    return subprocess.call(commandLine)
-
-# Check for all raw binaries and delete them
-for root, dirs, files in os.walk('Bin'):
-    for file in files:
-        if file.endswith(FILE_FORMAT):
-            os.remove(os.path.join(root, file))
-
-for arch in ('ia32', 'x64'):
-    for debugType in (None, 'port80', 'serial'):
-        for pageTable in PAGE_TABLES.keys():
-            ret = True
-            if arch.lower() == X64.lower():
-                directory = os.path.join('Bin', X64, pageTable)
-            else:
-                directory = os.path.join('Bin', IA32)
-
-            # output raw binary name with arch type
-            fileName = 'ResetVector' + '.' + arch
-
-            if debugType is not None:
-                fileName += '.' + debugType
-            fileName += FILE_FORMAT
-
-            output = os.path.join(directory, fileName)
-
-            # if the directory not exists then create it
-            if not os.path.isdir(directory):
-                os.makedirs(directory)
-
-            # Prepare the command to execute the nasmb
-            commandLine = (
-                'nasm',
-                '-D', 'ARCH_%s' % arch.upper(),
-                '-D', 'DEBUG_%s' % str(debugType).upper(),
-                '-D', PAGE_TABLES[pageTable].upper(),
-                '-o', output,
-                'Vtf0.nasmb',
-                )
-
-            print(f"Command : {' '.join(commandLine)}")
-
-            try:
-                ret = RunCommand(commandLine)
-            except FileNotFoundError:
-                print("NASM not found")
-            except:
-                pass
-
-            if ret != 0:
-                print(f"something went wrong while executing {commandLine[-1]}")
-                sys.exit()
-            print('\tASM\t' + output)
-
-            commandLine = (
-                'python',
-                'Tools/FixupForRawSection.py',
-                output,
-                )
-            print('\tFIXUP\t' + output)
-            ret = RunCommand(commandLine)
-            if ret != 0: sys.exit(ret)
-
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/ResetVector/Vtf0/Ia32/PageTables64.asm
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/ResetVector/Vtf0/Ia32/PageTables64.asm	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/ResetVector/Vtf0/Ia32/PageTables64.asm	(revision 101291)
@@ -3,5 +3,5 @@
 ; Sets the CR3 register for 64-bit paging
 ;
-; Copyright (c) 2008 - 2013, Intel Corporation. All rights reserved.<BR>
+; Copyright (c) 2008 - 2023, Intel Corporation. All rights reserved.<BR>
 ; SPDX-License-Identifier: BSD-2-Clause-Patent
 ;
@@ -18,6 +18,26 @@
     ; These pages are built into the ROM image in X64/PageTables.asm
     ;
-    mov     eax, ADDR_OF(TopLevelPageDirectory)
+%ifdef USE_5_LEVEL_PAGE_TABLE
+    mov     eax, 0
+    cpuid
+    cmp     eax, 07h                    ; check if basic CPUID leaf contains leaf 07
+    jb      NotSupport5LevelPaging      ; 5level paging not support, downgrade to 4level paging
+    mov     eax, 07h                    ; check cpuid leaf 7, subleaf 0
+    mov     ecx, 0
+    cpuid
+    bt      ecx, 16                     ; [Bits 16] Supports 5-level paging if 1.
+    jnc     NotSupport5LevelPaging      ; 5level paging not support, downgrade to 4level paging
+    mov     eax, ADDR_OF(Pml5)
     mov     cr3, eax
+    mov     eax, cr4
+    bts     eax, 12                     ; Set LA57=1.
+    mov     cr4, eax
+    jmp     SetCr3Done
+NotSupport5LevelPaging:
+%endif
+
+    mov     eax, ADDR_OF(Pml4)
+    mov     cr3, eax
+SetCr3Done:
 
     OneTimeCallRet SetCr3ForPageTables64
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/ResetVector/Vtf0/ReadMe.txt
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/ResetVector/Vtf0/ReadMe.txt	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/ResetVector/Vtf0/ReadMe.txt	(revision 101291)
@@ -1,14 +1,15 @@
 
 === HOW TO USE VTF0 ===
+Add this line to your DSC [Components.IA32] or [Components.X64] section:
+  UefiCpuPkg/ResetVector/Vtf0/ResetVector.inf
 
 Add this line to your FDF FV section:
-INF  RuleOverride=RESET_VECTOR USE = IA32 UefiCpuPkg/ResetVector/Vtf0/Bin/ResetVector.inf
-(For X64 SEC/PEI change IA32 to X64 => 'USE = X64')
+  INF  RuleOverride=RESET_VECTOR UefiCpuPkg/ResetVector/Vtf0/ResetVector.inf
 
 In your FDF FFS file rules sections add:
-[Rule.Common.SEC.RESET_VECTOR]
-  FILE RAW = $(NAMED_GUID) {
-    RAW RAW                |.raw
-  }
+  [Rule.Common.SEC.RESET_VECTOR]
+    FILE RAW = $(NAMED_GUID) {
+      RAW BIN   |.bin
+    }
 
 === VTF0 Boot Flow ===
@@ -26,16 +27,2 @@
 DI      - 'BP': boot-strap processor, or 'AP': application processor
 EBP/RBP - Pointer to the start of the Boot Firmware Volume
-
-=== HOW TO BUILD VTF0 ===
-
-Dependencies:
-* Python 3 or newer
-* Nasm 2.03 or newer
-
-To rebuild the VTF0 binaries:
-1. Change to VTF0 source dir: UefiCpuPkg/ResetVector/Vtf0
-2. nasm and python should be in executable path
-3. Run this command:
-   python Build.py
-4. Binaries output will be in UefiCpuPkg/ResetVector/Vtf0/Bin
-
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/ResetVector/Vtf0/Vtf0.inf
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/ResetVector/Vtf0/Vtf0.inf	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/ResetVector/Vtf0/Vtf0.inf	(revision 101291)
@@ -2,5 +2,22 @@
 #  Reset Vector
 #
-#  Copyright (c) 2006 - 2014, Intel Corporation. All rights reserved.<BR>
+#  Note:
+#    Conf/build_rule.txt contains the build rule [Nasm-to-Binary-Code-File]
+#    to generate .bin file from .nasmb source.
+#
+#    The platform FDF MUST have a FDF rule as follows to build the .bin
+#    file as ResetVector .ffs file:
+#
+#          [Rule.Common.SEC.RESET_VECTOR]
+#            FILE RAW = $(NAMED_GUID) {
+#              RAW BIN   |.bin
+#            }
+#
+#    Following line in FDF forces to use the above rule for the ResetVector:
+#
+#      INF  RuleOverride=RESET_VECTOR UefiCpuPkg/ResetVector/Vtf0/Vtf0.inf
+#
+#
+#  Copyright (c) 2006 - 2023, Intel Corporation. All rights reserved.<BR>
 #
 #  SPDX-License-Identifier: BSD-2-Clause-Patent
@@ -30,2 +47,11 @@
 [UserExtensions.TianoCore."ExtraFiles"]
   ResetVectorExtra.uni
+
+[BuildOptions]
+# Different build options can be specified:
+#   * for different architectures:
+#       -DARCH_X64, -DARCH_IA32
+#   * for using 1G page table:
+#       -DPAGE_TABLE_1G
+#   * for different debug channels:
+#       -DDEBUG_SERIAL, -DDEBUG_PORT80, or not specify any debug channel
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/ResetVector/Vtf0/Vtf0.nasmb
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/ResetVector/Vtf0/Vtf0.nasmb	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/ResetVector/Vtf0/Vtf0.nasmb	(revision 101291)
@@ -3,5 +3,5 @@
 ; This file includes all other code files to assemble the reset vector code
 ;
-; Copyright (c) 2008 - 2013, Intel Corporation. All rights reserved.<BR>
+; Copyright (c) 2008 - 2023, Intel Corporation. All rights reserved.<BR>
 ; SPDX-License-Identifier: BSD-2-Clause-Patent
 ;
@@ -39,9 +39,5 @@
 
 %ifdef ARCH_X64
-  %ifdef PAGE_TABLE_1G
-    %include "X64/PageTables1G.asm"
-  %else
-    %include "X64/PageTables2M.asm"
-  %endif
+  %include "X64/PageTables.asm"
 %endif
 
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/ResetVector/Vtf0/X64/PageTables.asm
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/ResetVector/Vtf0/X64/PageTables.asm	(revision 101291)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/ResetVector/Vtf0/X64/PageTables.asm	(revision 101291)
@@ -0,0 +1,93 @@
+;------------------------------------------------------------------------------
+; @file
+; Emits Page Tables for 1:1 mapping.
+; If using 1G page table, map addresses 0 - 0x8000000000 (512GB),
+; else, map addresses 0 - 0x100000000 (4GB)
+;
+; Copyright (c) 2021 - 2023, Intel Corporation. All rights reserved.<BR>
+; SPDX-License-Identifier: BSD-2-Clause-Patent
+;
+;------------------------------------------------------------------------------
+
+BITS    64
+
+%define ALIGN_TOP_TO_4K_FOR_PAGING
+
+;
+; Page table non-leaf entry attribute
+;
+%define PAGE_NLE_ATTR (PAGE_ACCESSED + \
+                        PAGE_READ_WRITE + \
+                        PAGE_PRESENT)
+
+;
+; Page table big leaf entry attribute:
+; PDPTE 1GB entry or PDE 2MB entry
+;
+%define PAGE_BLE_ATTR (PAGE_ACCESSED + \
+                        PAGE_READ_WRITE + \
+                        PAGE_DIRTY + \
+                        PAGE_PRESENT + \
+                        PAGE_SIZE)
+
+;
+; Page table non-leaf entry
+;
+%define PAGE_NLE(address) (ADDR_OF(address) + \
+                    PAGE_NLE_ATTR)
+
+%define PAGE_PDPTE_1GB(x) ((x << 30) + PAGE_BLE_ATTR)
+%define PAGE_PDE_2MB(x) ((x << 21) + PAGE_BLE_ATTR)
+
+ALIGN 16
+
+%ifdef PAGE_TABLE_1G
+Pdp:
+    ;
+    ; Page-directory pointer table (512 * 1GB entries => 512GB)
+    ;
+    %assign i 0
+    %rep      512
+        DQ    PAGE_PDPTE_1GB(i)
+        %assign i i+1
+    %endrep
+%else
+
+Pd:
+    ;
+    ; Page-Directory (2048 * 2MB entries => 4GB)
+    ; Four pages below, each is pointed by one entry in Pdp.
+    ;
+    %assign i 0
+    %rep    0x800
+        DQ      PAGE_PDE_2MB(i)
+        %assign i i+1
+    %endrep
+Pdp:
+    ;
+    ; Page-directory pointer table (4 * 1GB entries => 4GB)
+    ;
+    DQ      PAGE_NLE(Pd)
+    DQ      PAGE_NLE(Pd + 0x1000)
+    DQ      PAGE_NLE(Pd + 0x2000)
+    DQ      PAGE_NLE(Pd + 0x3000)
+    TIMES   0x1000 - ($ - Pdp) DB 0
+
+%endif
+
+Pml4:
+    ;
+    ; PML4 (1 * 512GB entry)
+    ;
+    DQ      PAGE_NLE(Pdp)
+    TIMES   0x1000 - ($ - Pml4) DB 0
+
+%ifdef USE_5_LEVEL_PAGE_TABLE
+Pml5:
+    ;
+    ; Pml5 table (only first entry is present, pointing to Pml4)
+    ;
+    DQ      PAGE_NLE(Pml4)
+    TIMES   0x1000 - ($ - Pml5) DB 0
+%endif
+EndOfPageTables:
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/ResetVector/Vtf0/X64/PageTables1G.asm
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/ResetVector/Vtf0/X64/PageTables1G.asm	(revision 101283)
+++ 	(revision )
@@ -1,53 +1,0 @@
-;------------------------------------------------------------------------------
-; @file
-; Emits Page Tables for 1:1 mapping of the addresses 0 - 0x8000000000 (512GB)
-;
-; Copyright (c) 2021, Intel Corporation. All rights reserved.<BR>
-; SPDX-License-Identifier: BSD-2-Clause-Patent
-; Linear-Address Translation to a 1-GByte Page
-;
-;------------------------------------------------------------------------------
-
-BITS    64
-
-%define ALIGN_TOP_TO_4K_FOR_PAGING
-
-%define PAGE_PDP_ATTR (PAGE_ACCESSED + \
-                        PAGE_READ_WRITE + \
-                        PAGE_PRESENT)
-
-%define PAGE_PDP_1G_ATTR (PAGE_ACCESSED + \
-                        PAGE_READ_WRITE + \
-                        PAGE_DIRTY + \
-                        PAGE_PRESENT + \
-                        PAGE_SIZE)
-
-%define PGTBLS_OFFSET(x) ((x) - TopLevelPageDirectory)
-%define PGTBLS_ADDR(x) (ADDR_OF(TopLevelPageDirectory) + (x))
-
-%define PDP(offset) (ADDR_OF(TopLevelPageDirectory) + (offset) + \
-                    PAGE_PDP_ATTR)
-
-%define PDP_1G(x) ((x << 30) + PAGE_PDP_1G_ATTR)
-
-ALIGN 16
-
-TopLevelPageDirectory:
-
-    ;
-    ; Top level Page Directory Pointers (1 * 512GB entry)
-    ;
-    DQ      PDP(0x1000)
-
-    TIMES 0x1000-PGTBLS_OFFSET($) DB 0
-    ;
-    ; Next level Page Directory Pointers (512 * 1GB entries => 512GB)
-    ;
-%assign i 0
-%rep      512
-    DQ    PDP_1G(i)
-    %assign i i+1
-%endrep
-    TIMES 0x2000-PGTBLS_OFFSET($) DB 0
-
-EndOfPageTables:
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/ResetVector/Vtf0/X64/PageTables2M.asm
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/ResetVector/Vtf0/X64/PageTables2M.asm	(revision 101283)
+++ 	(revision )
@@ -1,60 +1,0 @@
-;------------------------------------------------------------------------------
-; @file
-; Emits Page Tables for 1:1 mapping of the addresses 0 - 0x100000000 (4GB)
-;
-; Copyright (c) 2008 - 2014, Intel Corporation. All rights reserved.<BR>
-; SPDX-License-Identifier: BSD-2-Clause-Patent
-;
-;------------------------------------------------------------------------------
-
-BITS    64
-
-%define ALIGN_TOP_TO_4K_FOR_PAGING
-
-%define PAGE_2M_PDE_ATTR (PAGE_SIZE + \
-                          PAGE_ACCESSED + \
-                          PAGE_DIRTY + \
-                          PAGE_READ_WRITE + \
-                          PAGE_PRESENT)
-
-%define PAGE_PDP_ATTR (PAGE_ACCESSED + \
-                       PAGE_READ_WRITE + \
-                       PAGE_PRESENT)
-
-%define PGTBLS_OFFSET(x) ((x) - TopLevelPageDirectory)
-%define PGTBLS_ADDR(x) (ADDR_OF(TopLevelPageDirectory) + (x))
-
-%define PDP(offset) (ADDR_OF(TopLevelPageDirectory) + (offset) + \
-                     PAGE_PDP_ATTR)
-%define PTE_2MB(x) ((x << 21) + PAGE_2M_PDE_ATTR)
-
-TopLevelPageDirectory:
-
-    ;
-    ; Top level Page Directory Pointers (1 * 512GB entry)
-    ;
-    DQ      PDP(0x1000)
-
-
-    ;
-    ; Next level Page Directory Pointers (4 * 1GB entries => 4GB)
-    ;
-    TIMES 0x1000-PGTBLS_OFFSET($) DB 0
-
-    DQ      PDP(0x2000)
-    DQ      PDP(0x3000)
-    DQ      PDP(0x4000)
-    DQ      PDP(0x5000)
-
-    ;
-    ; Page Table Entries (2048 * 2MB entries => 4GB)
-    ;
-    TIMES 0x2000-PGTBLS_OFFSET($) DB 0
-
-%assign i 0
-%rep    0x800
-    DQ      PTE_2MB(i)
-    %assign i i+1
-%endrep
-
-EndOfPageTables:
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/SecCore/SecCore.inf
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/SecCore/SecCore.inf	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/SecCore/SecCore.inf	(revision 101291)
@@ -49,5 +49,4 @@
   DebugAgentLib
   CpuLib
-  UefiCpuLib
   PeCoffGetEntryPointLib
   PeCoffExtraActionLib
@@ -57,4 +56,5 @@
   PeiServicesTablePointerLib
   HobLib
+  CpuPageTableLib
 
 [Ppis]
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/SecCore/SecCoreNative.inf
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/SecCore/SecCoreNative.inf	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/SecCore/SecCoreNative.inf	(revision 101291)
@@ -46,5 +46,4 @@
   DebugAgentLib
   CpuLib
-  UefiCpuLib
   PeCoffGetEntryPointLib
   PeCoffExtraActionLib
@@ -54,4 +53,5 @@
   PeiServicesTablePointerLib
   HobLib
+  CpuPageTableLib
 
 [Ppis]
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/SecCore/SecMain.c
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/SecCore/SecMain.c	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/SecCore/SecMain.c	(revision 101291)
@@ -71,4 +71,133 @@
 
   return EFI_SUCCESS;
+}
+
+/**
+  Migrate page table to permanent memory mapping entire physical address space.
+
+  @retval   EFI_SUCCESS           The PageTable was migrated successfully.
+  @retval   EFI_UNSUPPORTED       Unsupport to migrate page table to permanent memory if IA-32e Mode not actived.
+  @retval   EFI_OUT_OF_RESOURCES  The PageTable could not be migrated due to lack of available memory.
+
+**/
+EFI_STATUS
+MigratePageTable (
+  VOID
+  )
+{
+  EFI_STATUS                      Status;
+  IA32_CR4                        Cr4;
+  BOOLEAN                         Page5LevelSupport;
+  UINT32                          RegEax;
+  CPUID_EXTENDED_CPU_SIG_EDX      RegEdx;
+  BOOLEAN                         Page1GSupport;
+  PAGING_MODE                     PagingMode;
+  CPUID_VIR_PHY_ADDRESS_SIZE_EAX  VirPhyAddressSize;
+  UINT32                          MaxExtendedFunctionId;
+  UINTN                           PageTable;
+  EFI_PHYSICAL_ADDRESS            Buffer;
+  UINTN                           BufferSize;
+  IA32_MAP_ATTRIBUTE              MapAttribute;
+  IA32_MAP_ATTRIBUTE              MapMask;
+
+  VirPhyAddressSize.Uint32    = 0;
+  PageTable                   = 0;
+  BufferSize                  = 0;
+  MapAttribute.Uint64         = 0;
+  MapMask.Uint64              = MAX_UINT64;
+  MapAttribute.Bits.Present   = 1;
+  MapAttribute.Bits.ReadWrite = 1;
+
+  //
+  // Check Page5Level Support or not.
+  //
+  Cr4.UintN         = AsmReadCr4 ();
+  Page5LevelSupport = (Cr4.Bits.LA57 ? TRUE : FALSE);
+
+  //
+  // Check Page1G Support or not.
+  //
+  Page1GSupport = FALSE;
+  AsmCpuid (CPUID_EXTENDED_FUNCTION, &RegEax, NULL, NULL, NULL);
+  if (RegEax >= CPUID_EXTENDED_CPU_SIG) {
+    AsmCpuid (CPUID_EXTENDED_CPU_SIG, NULL, NULL, NULL, &RegEdx.Uint32);
+    if (RegEdx.Bits.Page1GB != 0) {
+      Page1GSupport = TRUE;
+    }
+  }
+
+  //
+  // Decide Paging Mode according Page5LevelSupport & Page1GSupport.
+  //
+  if (Page5LevelSupport) {
+    PagingMode = Page1GSupport ? Paging5Level1GB : Paging5Level;
+  } else {
+    PagingMode = Page1GSupport ? Paging4Level1GB : Paging4Level;
+  }
+
+  //
+  // Get Maximum Physical Address Bits
+  // Get the number of address lines; Maximum Physical Address is 2^PhysicalAddressBits - 1.
+  // If CPUID does not supported, then use a max value of 36 as per SDM 3A, 4.1.4.
+  //
+  AsmCpuid (CPUID_EXTENDED_FUNCTION, &MaxExtendedFunctionId, NULL, NULL, NULL);
+  if (MaxExtendedFunctionId >= CPUID_VIR_PHY_ADDRESS_SIZE) {
+    AsmCpuid (CPUID_VIR_PHY_ADDRESS_SIZE, &VirPhyAddressSize.Uint32, NULL, NULL, NULL);
+  } else {
+    VirPhyAddressSize.Bits.PhysicalAddressBits = 36;
+  }
+
+  if ((PagingMode == Paging4Level1GB) || (PagingMode == Paging4Level)) {
+    //
+    // The max lineaddress bits is 48 for 4 level page table.
+    //
+    VirPhyAddressSize.Bits.PhysicalAddressBits = MIN (VirPhyAddressSize.Bits.PhysicalAddressBits, 48);
+  }
+
+  //
+  // Get required buffer size for the pagetable that will be created.
+  //
+  Status = PageTableMap (&PageTable, PagingMode, 0, &BufferSize, 0, LShiftU64 (1, VirPhyAddressSize.Bits.PhysicalAddressBits), &MapAttribute, &MapMask, NULL);
+  ASSERT (Status == EFI_BUFFER_TOO_SMALL);
+  if (Status != EFI_BUFFER_TOO_SMALL) {
+    return Status;
+  }
+
+  //
+  // Allocate required Buffer.
+  //
+  Status = PeiServicesAllocatePages (
+             EfiBootServicesData,
+             EFI_SIZE_TO_PAGES (BufferSize),
+             &Buffer
+             );
+  if (EFI_ERROR (Status)) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  // Create PageTable in permanent memory.
+  //
+  Status = PageTableMap (&PageTable, PagingMode, (VOID *)(UINTN)Buffer, &BufferSize, 0, LShiftU64 (1, VirPhyAddressSize.Bits.PhysicalAddressBits), &MapAttribute, &MapMask, NULL);
+  ASSERT_EFI_ERROR (Status);
+  if (EFI_ERROR (Status) || (PageTable == 0)) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  // Write the Pagetable to CR3.
+  //
+  AsmWriteCr3 (PageTable);
+
+  DEBUG ((
+    DEBUG_INFO,
+    "MigratePageTable: Created PageTable = 0x%lx, BufferSize = %x, PagingMode = 0x%lx, Support Max Physical Address Bits = %d\n",
+    PageTable,
+    BufferSize,
+    (UINTN)PagingMode,
+    VirPhyAddressSize.Bits.PhysicalAddressBits
+    ));
+
+  return Status;
 }
 
@@ -171,5 +300,5 @@
     DEBUG_INFO,
     "%a() TempRAM Base: 0x%x, TempRAM Size: 0x%x, BootFirmwareVolume 0x%x\n",
-    __FUNCTION__,
+    __func__,
     TempRamBase,
     SizeOfRam,
@@ -243,5 +372,5 @@
     DEBUG_INFO,
     "%a() BFV Base: 0x%x, BFV Size: 0x%x, TempRAM Base: 0x%x, TempRAM Size: 0x%x, PeiTempRamBase: 0x%x, PeiTempRamSize: 0x%x, StackBase: 0x%x, StackSize: 0x%x\n",
-    __FUNCTION__,
+    __func__,
     SecCoreData.BootFirmwareVolumeBase,
     SecCoreData.BootFirmwareVolumeSize,
@@ -346,5 +475,5 @@
     DEBUG_INFO,
     "%a() PeiCoreEntryPoint: 0x%x\n",
-    __FUNCTION__,
+    __func__,
     PeiCoreEntryPoint
     ));
@@ -395,5 +524,5 @@
       DEBUG_INFO,
       "%a() PeiTemporaryRamBase: 0x%x, PeiTemporaryRamSize: 0x%x\n",
-      __FUNCTION__,
+      __func__,
       SecCoreData->PeiTemporaryRamBase,
       SecCoreData->PeiTemporaryRamSize
@@ -409,5 +538,5 @@
     DEBUG_INFO,
     "%a() Stack Base: 0x%p, Stack Size: 0x%x\n",
-    __FUNCTION__,
+    __func__,
     SecCoreData->StackBase,
     (UINT32)SecCoreData->StackSize
@@ -454,4 +583,5 @@
   EFI_PEI_PPI_DESCRIPTOR  *PeiPpiDescriptor;
   REPUBLISH_SEC_PPI_PPI   *RepublishSecPpiPpi;
+  IA32_CR0                Cr0;
 
   //
@@ -496,4 +626,21 @@
 
   //
+  // Migrate page table to permanent memory mapping entire physical address space if CR0.PG is set.
+  //
+  Cr0.UintN = AsmReadCr0 ();
+  if (Cr0.Bits.PG != 0) {
+    //
+    // Assume CPU runs in 64bit mode if paging is enabled.
+    //
+    ASSERT (sizeof (UINTN) == sizeof (UINT64));
+
+    Status = MigratePageTable ();
+    if (EFI_ERROR (Status)) {
+      DEBUG ((DEBUG_ERROR, "SecTemporaryRamDone: Failed to migrate page table to permanent memory: %r.\n", Status));
+      CpuDeadLoop ();
+    }
+  }
+
+  //
   // Disable Temporary RAM after Stack and Heap have been migrated at this point.
   //
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/SecCore/SecMain.h
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/SecCore/SecMain.h	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/SecCore/SecMain.h	(revision 101291)
@@ -20,4 +20,5 @@
 #include <Guid/FirmwarePerformance.h>
 
+#include <Library/BaseLib.h>
 #include <Library/DebugLib.h>
 #include <Library/PcdLib.h>
@@ -25,5 +26,4 @@
 #include <Library/PlatformSecLib.h>
 #include <Library/CpuLib.h>
-#include <Library/UefiCpuLib.h>
 #include <Library/PeCoffGetEntryPointLib.h>
 #include <Library/PeCoffExtraActionLib.h>
@@ -34,4 +34,7 @@
 #include <Library/HobLib.h>
 #include <Library/PeiServicesLib.h>
+#include <Library/CpuPageTableLib.h>
+#include <Register/Intel/Cpuid.h>
+#include <Register/Intel/Msr.h>
 
 #define SEC_IDT_ENTRY_COUNT  34
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Test/UnitTest/EfiMpServicesPpiProtocol/EfiMpServicesUnitTestCommom.c
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Test/UnitTest/EfiMpServicesPpiProtocol/EfiMpServicesUnitTestCommom.c	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Test/UnitTest/EfiMpServicesPpiProtocol/EfiMpServicesUnitTestCommom.c	(revision 101291)
@@ -46,5 +46,5 @@
   Status = MpServicesUnitTestWhoAmI (LocalContext->MpServices, &BspNumber);
   UT_ASSERT_NOT_EFI_ERROR (Status);
-  DEBUG ((DEBUG_INFO, "%a: BspNumber = 0x%x\n", __FUNCTION__, BspNumber));
+  DEBUG ((DEBUG_INFO, "%a: BspNumber = 0x%x\n", __func__, BspNumber));
 
   Status = MpServicesUnitTestGetNumberOfProcessors (
@@ -57,5 +57,5 @@
     DEBUG_INFO,
     "%a: NumberOfProcessors = 0x%x, NumberOfEnabledProcessors = 0x%x\n",
-    __FUNCTION__,
+    __func__,
     NumberOfProcessors,
     NumberOfEnabledProcessors
@@ -90,5 +90,5 @@
         IndexOfDisabledAPs++;
 
-        DEBUG ((DEBUG_INFO, "%a: AP(0x%x) is disabled and temporarily enable it.\n", __FUNCTION__, ProcessorNumber));
+        DEBUG ((DEBUG_INFO, "%a: AP(0x%x) is disabled and temporarily enable it.\n", __func__, ProcessorNumber));
         Status = MpServicesUnitTestEnableDisableAP (
                    LocalContext->MpServices,
@@ -135,5 +135,5 @@
   if (BspNumber != LocalContext->BspNumber) {
     LocalContext->BspNumber = BspNumber;
-    DEBUG ((DEBUG_INFO, "%a: New BspNumber = 0x%x\n", __FUNCTION__, BspNumber));
+    DEBUG ((DEBUG_INFO, "%a: New BspNumber = 0x%x\n", __func__, BspNumber));
   }
 
@@ -149,9 +149,9 @@
   if (NumberOfProcessors != LocalContext->NumberOfProcessors) {
     LocalContext->NumberOfProcessors = NumberOfProcessors;
-    DEBUG ((DEBUG_INFO, "%a: New NumberOfProcessors = 0x%x\n", __FUNCTION__, NumberOfProcessors));
+    DEBUG ((DEBUG_INFO, "%a: New NumberOfProcessors = 0x%x\n", __func__, NumberOfProcessors));
   }
 
   if (NumberOfEnabledProcessors != LocalContext->NumberOfProcessors) {
-    DEBUG ((DEBUG_INFO, "%a: New NumberOfEnabledProcessors = 0x%x\n", __FUNCTION__, NumberOfEnabledProcessors));
+    DEBUG ((DEBUG_INFO, "%a: New NumberOfEnabledProcessors = 0x%x\n", __func__, NumberOfEnabledProcessors));
 
     for (ProcessorNumber = 0; ProcessorNumber < LocalContext->NumberOfProcessors; ProcessorNumber++) {
@@ -164,5 +164,5 @@
 
       if (!(ProcessorInfoBuffer.StatusFlag & PROCESSOR_ENABLED_BIT)) {
-        DEBUG ((DEBUG_INFO, "%a: AP(0x%x) is disabled unexpectedly and reenable it.\n", __FUNCTION__, ProcessorNumber));
+        DEBUG ((DEBUG_INFO, "%a: AP(0x%x) is disabled unexpectedly and reenable it.\n", __func__, ProcessorNumber));
         Status = MpServicesUnitTestEnableDisableAP (
                    LocalContext->MpServices,
@@ -205,5 +205,5 @@
         DEBUG_INFO,
         "%a: Disable AP(0x%x) to restore its state.\n",
-        __FUNCTION__,
+        __func__,
         LocalContext->DisabledApNumber[IndexOfDisabledAPs]
         ));
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/UefiCpuPkg.dec
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/UefiCpuPkg.dec	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/UefiCpuPkg.dec	(revision 101291)
@@ -2,5 +2,6 @@
 # This Package provides UEFI compatible CPU modules and libraries.
 #
-# Copyright (c) 2007 - 2022, Intel Corporation. All rights reserved.<BR>
+# Copyright (c) 2007 - 2023, Intel Corporation. All rights reserved.<BR>
+# Copyright (C) 2023 Advanced Micro Devices, Inc. All rights reserved.<BR>
 #
 # SPDX-License-Identifier: BSD-2-Clause-Patent
@@ -19,9 +20,4 @@
 
 [LibraryClasses]
-  ##  @libraryclass  Defines some routines that are generic for IA32 family CPU
-  ##                 to be UEFI specification compliant.
-  ##
-  UefiCpuLib|Include/Library/UefiCpuLib.h
-
   ##  @libraryclass  Defines some routines that are used to register/manage/program
   ##                 CPU features.
@@ -65,4 +61,12 @@
   ##  @libraryclass  Provides function for manipulating x86 paging structures.
   CpuPageTableLib|Include/Library/CpuPageTableLib.h
+
+  ## @libraryclass   Provides functions for manipulating smram savestate registers.
+  MmSaveStateLib|Include/Library/MmSaveStateLib.h
+
+[LibraryClasses.RISCV64]
+  ##  @libraryclass  Provides functions to manage MMU features on RISCV64 CPUs.
+  ##
+  RiscVMmuLib|Include/Library/BaseRiscVMmuLib.h
 
 [Guids]
@@ -78,4 +82,7 @@
   ## Include/Guid/MicrocodePatchHob.h
   gEdkiiMicrocodePatchHobGuid    = { 0xd178f11d, 0x8716, 0x418e, { 0xa1, 0x31, 0x96, 0x7d, 0x2a, 0xc4, 0x28, 0x43 }}
+
+  ## Include/Guid/SmmBaseHob.h
+  gSmmBaseHobGuid      = { 0xc2217ba7, 0x03bb, 0x4f63, {0xa6, 0x47, 0x7c, 0x25, 0xc5, 0xfc, 0x9d, 0x73 }}
 
 [Protocols]
@@ -178,4 +185,10 @@
   gUefiCpuPkgTokenSpaceGuid.PcdSmmFeatureControlEnable|TRUE|BOOLEAN|0x32132110
 
+  ## Indicates if SMM perf logging in APs will be enabled.<BR><BR>
+  #   TRUE  - SMM perf logging in APs will be enabled.<BR>
+  #   FALSE - SMM perf logging in APs will not be enabled.<BR>
+  # @Prompt Enable SMM perf logging in APs.
+  gUefiCpuPkgTokenSpaceGuid.PcdSmmApPerfLogEnable|TRUE|BOOLEAN|0x32132114
+
 [PcdsFixedAtBuild]
   ## List of exception vectors which need switching stack.
@@ -201,4 +214,15 @@
   # @Prompt Configure the SEV-ES work area base
   gUefiCpuPkgTokenSpaceGuid.PcdSevEsWorkAreaSize|0x0|UINT32|0x30002006
+
+  ## Determining APs' first-time wakeup by SIPI or INIT-SIPI-SIPI.
+  # Following a power-up or RESET of an MP system, The APs complete a
+  # minimal self-configuration, then wait for a startup signal (a SIPI
+  # message) from the BSP processor.
+  #
+  #   TRUE  - Broadcast SIPI.
+  #   FALSE - Broadcast INIT-SIPI-SIPI.
+  #
+  # @Prompt BSP Broadcast Method for the first-time wakeup of APs
+  gUefiCpuPkgTokenSpaceGuid.PcdFirstTimeWakeUpAPsBySipi|TRUE|BOOLEAN|0x30002007
 
 [PcdsFixedAtBuild, PcdsPatchableInModule]
@@ -341,4 +365,19 @@
   gUefiCpuPkgTokenSpaceGuid.PcdIsPowerOnReset|FALSE|BOOLEAN|0x0000001B
 
+  ## This PCD indicates whether CPU processor trace is enabled on BSP only when CPU processor trace is enabled.<BR><BR>
+  #  This PCD is ignored if CPU processor trace is disabled.<BR><BR>
+  #  TRUE  - CPU processor trace is enabled on BSP only.<BR>
+  #  FASLE - CPU processor trace is enabled on all CPU.<BR>
+  # @Prompt Enable CPU processor trace only on BSP.
+  gUefiCpuPkgTokenSpaceGuid.PcdCpuProcTraceBspOnly|FALSE|BOOLEAN|0x60000019
+
+  ## This PCD indicates if enable performance collecting when CPU processor trace is enabled.<BR><BR>
+  #  CYC/TSC timing packets will be generated to collect performance data if this PCD is TRUE.
+  #  This PCD is ignored if CPU processor trace is disabled.<BR><BR>
+  #  TRUE  - Performance collecting will be enabled in processor trace.<BR>
+  #  FASLE - Performance collecting will be disabled in processor trace.<BR>
+  # @Prompt Enable performance collecting when processor trace is enabled.
+  gUefiCpuPkgTokenSpaceGuid.PcdCpuProcTracePerformanceCollecting|FALSE|BOOLEAN|0x60000020
+
 [PcdsFixedAtBuild.X64, PcdsPatchableInModule.X64, PcdsDynamic.X64, PcdsDynamicEx.X64]
   ## Indicate access to non-SMRAM memory is restricted to reserved, runtime and ACPI NVS type after SmmReadyToLock.
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/UefiCpuPkg.dsc
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/UefiCpuPkg.dsc	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/UefiCpuPkg.dsc	(revision 101291)
@@ -2,5 +2,6 @@
 #  UefiCpuPkg Package
 #
-#  Copyright (c) 2007 - 2022, Intel Corporation. All rights reserved.<BR>
+#  Copyright (c) 2007 - 2023, Intel Corporation. All rights reserved.<BR>
+#  Copyright (C) 2023 Advanced Micro Devices, Inc. All rights reserved.<BR>
 #
 #  SPDX-License-Identifier: BSD-2-Clause-Patent
@@ -32,5 +33,4 @@
   DebugPrintErrorLevelLib|MdePkg/Library/BaseDebugPrintErrorLevelLib/BaseDebugPrintErrorLevelLib.inf
   DevicePathLib|MdePkg/Library/UefiDevicePathLib/UefiDevicePathLib.inf
-  UefiCpuLib|UefiCpuPkg/Library/BaseUefiCpuLib/BaseUefiCpuLib.inf
   IoLib|MdePkg/Library/BaseIoLibIntrinsic/BaseIoLibIntrinsic.inf
   MtrrLib|UefiCpuPkg/Library/MtrrLib/MtrrLib.inf
@@ -70,9 +70,5 @@
 [LibraryClasses.common.SEC]
   PlatformSecLib|UefiCpuPkg/Library/PlatformSecLibNull/PlatformSecLibNull.inf
-!if $(TOOL_CHAIN_TAG) == "XCODE5"
-  CpuExceptionHandlerLib|UefiCpuPkg/Library/CpuExceptionHandlerLib/Xcode5SecPeiCpuExceptionHandlerLib.inf
-!else
   CpuExceptionHandlerLib|UefiCpuPkg/Library/CpuExceptionHandlerLib/SecPeiCpuExceptionHandlerLib.inf
-!endif
   HobLib|MdePkg/Library/PeiHobLib/PeiHobLib.inf
   PeiServicesTablePointerLib|MdePkg/Library/PeiServicesTablePointerLibIdt/PeiServicesTablePointerLibIdt.inf
@@ -105,4 +101,5 @@
   HobLib|MdePkg/Library/DxeHobLib/DxeHobLib.inf
   CpuExceptionHandlerLib|UefiCpuPkg/Library/CpuExceptionHandlerLib/SmmCpuExceptionHandlerLib.inf
+  MmSaveStateLib|UefiCpuPkg/Library/MmSaveStateLib/IntelMmSaveStateLib.inf
 
 [LibraryClasses.common.MM_STANDALONE]
@@ -141,15 +138,11 @@
   UefiCpuPkg/CpuMpPei/CpuMpPei.inf
   UefiCpuPkg/CpuS3DataDxe/CpuS3DataDxe.inf
-  UefiCpuPkg/Library/BaseUefiCpuLib/BaseUefiCpuLib.inf
   UefiCpuPkg/Library/BaseXApicLib/BaseXApicLib.inf
   UefiCpuPkg/Library/BaseXApicX2ApicLib/BaseXApicX2ApicLib.inf
   UefiCpuPkg/Library/CpuCommonFeaturesLib/CpuCommonFeaturesLib.inf
   UefiCpuPkg/Library/CpuExceptionHandlerLib/DxeCpuExceptionHandlerLib.inf
-!if $(TOOL_CHAIN_TAG) != "XCODE5"
   UefiCpuPkg/Library/CpuExceptionHandlerLib/SecPeiCpuExceptionHandlerLib.inf
-!endif
   UefiCpuPkg/Library/CpuExceptionHandlerLib/SmmCpuExceptionHandlerLib.inf
   UefiCpuPkg/Library/CpuExceptionHandlerLib/PeiCpuExceptionHandlerLib.inf
-  UefiCpuPkg/Library/CpuExceptionHandlerLib/Xcode5SecPeiCpuExceptionHandlerLib.inf
   UefiCpuPkg/Library/MpInitLib/PeiMpInitLib.inf
   UefiCpuPkg/Library/MpInitLib/DxeMpInitLib.inf
@@ -177,6 +170,13 @@
       SmmCpuFeaturesLib|UefiCpuPkg/Library/SmmCpuFeaturesLib/SmmCpuFeaturesLibStm.inf
   }
+  UefiCpuPkg/PiSmmCpuDxeSmm/PiSmmCpuDxeSmm.inf {
+    <Defines>
+      FILE_GUID = B7242C74-BD21-49EE-84B4-07162E8C080D
+    <LibraryClasses>
+      SmmCpuFeaturesLib|UefiCpuPkg/Library/SmmCpuFeaturesLib/AmdSmmCpuFeaturesLib.inf
+      MmSaveStateLib|UefiCpuPkg/Library/MmSaveStateLib/AmdMmSaveStateLib.inf
+  }
   UefiCpuPkg/Universal/Acpi/S3Resume2Pei/S3Resume2Pei.inf
-  UefiCpuPkg/ResetVector/Vtf0/Bin/ResetVector.inf
+  UefiCpuPkg/ResetVector/Vtf0/Vtf0.inf
   UefiCpuPkg/Library/SmmCpuRendezvousLib/SmmCpuRendezvousLib.inf
   UefiCpuPkg/Library/CpuPageTableLib/CpuPageTableLib.inf
@@ -192,4 +192,7 @@
       UnitTestResultReportLib|UnitTestFrameworkPkg/Library/UnitTestResultReportLib/UnitTestResultReportLibConOut.inf
   }
+  UefiCpuPkg/Library/MmSaveStateLib/AmdMmSaveStateLib.inf
+  UefiCpuPkg/Library/MmSaveStateLib/IntelMmSaveStateLib.inf
+  UefiCpuPkg/Library/SmmCpuFeaturesLib/AmdSmmCpuFeaturesLib.inf
 
 [Components.X64]
@@ -199,4 +202,5 @@
   UefiCpuPkg/Library/BaseRiscV64CpuExceptionHandlerLib/BaseRiscV64CpuExceptionHandlerLib.inf
   UefiCpuPkg/Library/BaseRiscV64CpuTimerLib/BaseRiscV64CpuTimerLib.inf
+  UefiCpuPkg/Library/BaseRiscVMmuLib/BaseRiscVMmuLib.inf
   UefiCpuPkg/CpuTimerDxeRiscV64/CpuTimerDxeRiscV64.inf
   UefiCpuPkg/CpuDxeRiscV64/CpuDxeRiscV64.inf
Index: trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Universal/Acpi/S3Resume2Pei/S3Resume.c
===================================================================
--- trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Universal/Acpi/S3Resume2Pei/S3Resume.c	(revision 101283)
+++ trunk/src/VBox/Devices/EFI/FirmwareNew/UefiCpuPkg/Universal/Acpi/S3Resume2Pei/S3Resume.c	(revision 101291)
@@ -510,5 +510,5 @@
       DEBUG_INFO,
       "%a() Stack Base: 0x%x, Stack Size: 0x%x\n",
-      __FUNCTION__,
+      __func__,
       TempStackTop,
       sizeof (TempStack)
