UefiCpuPkg/PiSmmCpu: Enable 5 level paging when CPU supports

REF:https://bugzilla.tianocore.org/show_bug.cgi?id=1946

The patch changes SMM environment to use 5 level paging when CPU
supports it.

Signed-off-by: Ray Ni <ray.ni@intel.com>
Cc: Eric Dong <eric.dong@intel.com>
Regression-tested-by: Laszlo Ersek <lersek@redhat.com>
Reviewed-by: Eric Dong <eric.dong@intel.com>
(cherry picked from commit 7365eb2c8c)

UefiCpuPkg/PiSmmCpuDxeSmm/SmmCpuMemoryManagement.c

@@ -125,18 +125,36 @@ GetPageTableEntry (
   UINTN                 Index2;
   UINTN                 Index3;
   UINTN                 Index4;
+  UINTN                 Index5;
   UINT64                *L1PageTable;
   UINT64                *L2PageTable;
   UINT64                *L3PageTable;
   UINT64                *L4PageTable;
+  UINT64                *L5PageTable;
+  IA32_CR4              Cr4;
+  BOOLEAN               Enable5LevelPaging;
 
+  Index5 = ((UINTN)RShiftU64 (Address, 48)) & PAGING_PAE_INDEX_MASK;
   Index4 = ((UINTN)RShiftU64 (Address, 39)) & PAGING_PAE_INDEX_MASK;
   Index3 = ((UINTN)Address >> 30) & PAGING_PAE_INDEX_MASK;
   Index2 = ((UINTN)Address >> 21) & PAGING_PAE_INDEX_MASK;
   Index1 = ((UINTN)Address >> 12) & PAGING_PAE_INDEX_MASK;
 
+  Cr4.UintN = AsmReadCr4 ();
+  Enable5LevelPaging = (BOOLEAN) (Cr4.Bits.LA57 == 1);
+
   if (sizeof(UINTN) == sizeof(UINT64)) {
+    if (Enable5LevelPaging) {
+      L5PageTable = (UINT64 *)GetPageTableBase ();
+      if (L5PageTable[Index5] == 0) {
+        *PageAttribute = PageNone;
+        return NULL;
+      }
+
+      L4PageTable = (UINT64 *)(UINTN)(L5PageTable[Index5] & ~mAddressEncMask & PAGING_4K_ADDRESS_MASK_64);
+    } else {
       L4PageTable = (UINT64 *)GetPageTableBase ();
+    }
     if (L4PageTable[Index4] == 0) {
       *PageAttribute = PageNone;
       return NULL;

UefiCpuPkg/PiSmmCpuDxeSmm/SmmProfile.c

@@ -534,34 +534,62 @@ InitPaging (
   VOID
   )
 {
+  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)) {
-    Pml4 = (UINT64*)(UINTN)mSmmProfileCr3;
+    if (!Enable5LevelPaging) {
+      Pml5Entry = (UINTN) mSmmProfileCr3 | IA32_PG_P;
+      Pml5 = &Pml5Entry;
+    } else {
+      Pml5 = (UINT64*) (UINTN) mSmmProfileCr3;
+    }
     SizeOfMemorySpace = HighBitSet64 (gPhyMask) + 1;
     //
     // Calculate the table entries of PML4E and PDPTE.
     //
-    if (SizeOfMemorySpace <= 39 ) {
+    NumberOfPml5Entries = 1;
-      NumberOfPml4Entries = 1;
+    if (SizeOfMemorySpace > 48) {
-      NumberOfPdptEntries = (UINT32)LShiftU64 (1, (SizeOfMemorySpace - 30));
+      NumberOfPml5Entries = (UINTN) LShiftU64 (1, SizeOfMemorySpace - 48);
-    } else {
+      SizeOfMemorySpace = 48;
-      NumberOfPml4Entries = (UINT32)LShiftU64 (1, (SizeOfMemorySpace - 39));
-      NumberOfPdptEntries = 512;
     }
+
+    NumberOfPml4Entries = 1;
+    if (SizeOfMemorySpace > 39) {
+      NumberOfPml4Entries = (UINTN) LShiftU64 (1, SizeOfMemorySpace - 39);
+      SizeOfMemorySpace = 39;
+    }
+
+    NumberOfPdptEntries = 1;
+    ASSERT (SizeOfMemorySpace > 30);
+    NumberOfPdptEntries = (UINTN) LShiftU64 (1, SizeOfMemorySpace - 30);
   } else {
+    Pml4Entry = (UINTN) mSmmProfileCr3 | IA32_PG_P;
+    Pml4 = &Pml4Entry;
+    Pml5Entry = (UINTN) Pml4 | IA32_PG_P;
+    Pml5 = &Pml5Entry;
+    NumberOfPml5Entries  = 1;
     NumberOfPml4Entries  = 1;
     NumberOfPdptEntries  = 4;
   }
@@ -569,8 +597,15 @@ InitPaging (
   //
   // 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 (sizeof (UINTN) == sizeof (UINT64)) {
       if ((Pml4[Pml4Index] & IA32_PG_P) == 0) {
         //
         // If PML4 entry does not exist, skip it
@@ -578,9 +613,6 @@ InitPaging (
         continue;
       }
       Pdpt = (UINT64 *)(UINTN)(Pml4[Pml4Index] & ~mAddressEncMask & PHYSICAL_ADDRESS_MASK);
-    } else {
-      Pdpt = (UINT64*)(UINTN)mSmmProfileCr3;
-    }
       for (PdptIndex = 0; PdptIndex < NumberOfPdptEntries; PdptIndex++, Pdpt++) {
         if ((*Pdpt & IA32_PG_P) == 0) {
           //
@@ -605,7 +637,13 @@ InitPaging (
             //
             continue;
           }
-        Address = (((PdptIndex << 9) + PdIndex) << 21);
+          Address = (UINTN) LShiftU64 (
+                              LShiftU64 (
+                                LShiftU64 ((Pml5Index << 9) + Pml4Index, 9) + PdptIndex,
+                                9
+                                ) + PdIndex,
+                                21
+                              );
 
           //
           // If it is 2M page, check IsAddressSplit()
@@ -619,22 +657,32 @@ InitPaging (
             Pt = AllocatePageTableMemory (1);
             ASSERT (Pt != NULL);
 
+            *Pd = (UINTN) Pt | IA32_PG_RW | IA32_PG_P;
+
             // Split it
-          for (PtIndex = 0; PtIndex < SIZE_4KB / sizeof(*Pt); PtIndex++) {
+            for (PtIndex = 0; PtIndex < SIZE_4KB / sizeof(*Pt); PtIndex++, Pt++) {
-            Pt[PtIndex] = Address + ((PtIndex << 12) | mAddressEncMask | PAGE_ATTRIBUTE_BITS);
+              *Pt = 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.
   //
   DEBUG ((EFI_D_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 (sizeof (UINTN) == sizeof (UINT64)) {
       if ((Pml4[Pml4Index] & IA32_PG_P) == 0) {
         //
         // If PML4 entry does not exist, skip it
@@ -642,9 +690,6 @@ InitPaging (
         continue;
       }
       Pdpt = (UINT64 *)(UINTN)(Pml4[Pml4Index] & ~mAddressEncMask & PHYSICAL_ADDRESS_MASK);
-    } else {
-      Pdpt = (UINT64*)(UINTN)mSmmProfileCr3;
-    }
       for (PdptIndex = 0; PdptIndex < NumberOfPdptEntries; PdptIndex++, Pdpt++) {
         if ((*Pdpt & IA32_PG_P) == 0) {
           //
@@ -672,7 +717,13 @@ InitPaging (
             //
             continue;
           }
-        Address = (((PdptIndex << 9) + PdIndex) << 21);
+          Address = (UINTN) LShiftU64 (
+                              LShiftU64 (
+                                LShiftU64 ((Pml5Index << 9) + Pml4Index, 9) + PdptIndex,
+                                9
+                                ) + PdIndex,
+                                21
+                              );
 
           if ((*Pd & IA32_PG_PS) != 0) {
             // 2MB page
@@ -705,6 +756,7 @@ InitPaging (
         } // end for PD
       } // end for PDPT
     } // end for PML4
+  } // end for PML5
 
   //
   // Flush TLB
@@ -1156,6 +1208,20 @@ RestorePageTableBelow4G (
 {
   UINTN         PTIndex;
   UINTN         PFIndex;
+  IA32_CR4      Cr4;
+  BOOLEAN       Enable5LevelPaging;
+
+  Cr4.UintN = AsmReadCr4 ();
+  Enable5LevelPaging = (BOOLEAN) (Cr4.Bits.LA57 == 1);
+
+  //
+  // PML5
+  //
+  if (Enable5LevelPaging) {
+    PTIndex = (UINTN)BitFieldRead64 (PFAddress, 48, 56);
+    ASSERT (PageTable[PTIndex] != 0);
+    PageTable = (UINT64*)(UINTN)(PageTable[PTIndex] & PHYSICAL_ADDRESS_MASK);
+  }
 
   //
   // PML4

UefiCpuPkg/PiSmmCpuDxeSmm/X64/PageTbl.c

@@ -16,6 +16,8 @@ SPDX-License-Identifier: BSD-2-Clause-Patent
 LIST_ENTRY                          mPagePool = INITIALIZE_LIST_HEAD_VARIABLE (mPagePool);
 BOOLEAN                             m1GPageTableSupport = FALSE;
 BOOLEAN                             mCpuSmmStaticPageTable;
+BOOLEAN                             m5LevelPagingSupport;
+X86_ASSEMBLY_PATCH_LABEL            gPatch5LevelPagingSupport;
 
 /**
   Disable CET.
@@ -60,6 +62,31 @@ Is1GPageSupport (
   return FALSE;
 }
 
+/**
+  Check if 5-level paging is supported by processor or not.
+
+  @retval TRUE   5-level paging is supported.
+  @retval FALSE  5-level paging is not supported.
+
+**/
+BOOLEAN
+Is5LevelPagingSupport (
+  VOID
+  )
+{
+  CPUID_STRUCTURED_EXTENDED_FEATURE_FLAGS_ECX EcxFlags;
+
+  AsmCpuidEx (
+    CPUID_STRUCTURED_EXTENDED_FEATURE_FLAGS,
+    CPUID_STRUCTURED_EXTENDED_FEATURE_FLAGS_SUB_LEAF_INFO,
+    NULL,
+    NULL,
+    &EcxFlags.Uint32,
+    NULL
+    );
+  return (BOOLEAN) (EcxFlags.Bits.FiveLevelPage != 0);
+}
+
 /**
   Set sub-entries number in entry.
 
@@ -130,14 +157,6 @@ CalculateMaximumSupportAddress (
       PhysicalAddressBits = 36;
     }
   }
-
-  //
-  // IA-32e paging translates 48-bit linear addresses to 52-bit physical addresses.
-  //
-  ASSERT (PhysicalAddressBits <= 52);
-  if (PhysicalAddressBits > 48) {
-    PhysicalAddressBits = 48;
-  }
   return PhysicalAddressBits;
 }
 
@@ -152,33 +171,80 @@ SetStaticPageTable (
   )
 {
   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;
 
-  if (mPhysicalAddressBits <= 39 ) {
+  //
-    NumberOfPml4EntriesNeeded = 1;
+  // IA-32e paging translates 48-bit linear addresses to 52-bit physical addresses
-    NumberOfPdpEntriesNeeded = (UINT32)LShiftU64 (1, (mPhysicalAddressBits - 30));
+  //  when 5-Level Paging is disabled.
-  } else {
+  //
-    NumberOfPml4EntriesNeeded = (UINT32)LShiftU64 (1, (mPhysicalAddressBits - 39));
+  ASSERT (mPhysicalAddressBits <= 52);
-    NumberOfPdpEntriesNeeded = 512;
+  if (!m5LevelPagingSupport && mPhysicalAddressBits > 48) {
+    mPhysicalAddressBits = 48;
   }
 
+  NumberOfPml5EntriesNeeded = 1;
+  if (mPhysicalAddressBits > 48) {
+    NumberOfPml5EntriesNeeded = (UINTN) LShiftU64 (1, mPhysicalAddressBits - 48);
+    mPhysicalAddressBits = 48;
+  }
+
+  NumberOfPml4EntriesNeeded = 1;
+  if (mPhysicalAddressBits > 39) {
+    NumberOfPml4EntriesNeeded = (UINTN) LShiftU64 (1, mPhysicalAddressBits - 39);
+    mPhysicalAddressBits = 39;
+  }
+
+  NumberOfPdpEntriesNeeded = 1;
+  ASSERT (mPhysicalAddressBits > 30);
+  NumberOfPdpEntriesNeeded = (UINTN) LShiftU64 (1, mPhysicalAddressBits - 30);
+
   //
   // By architecture only one PageMapLevel4 exists - so lets allocate storage for it.
   //
   PageMap         = (VOID *) PageTable;
 
   PageMapLevel4Entry = PageMap;
+  PageMapLevel5Entry = NULL;
+  if (m5LevelPagingSupport) {
+    //
+    // By architecture only one PageMapLevel5 exists - so lets allocate storage for it.
+    //
+    PageMapLevel5Entry = PageMap;
+  }
   PageAddress        = 0;
-  for (IndexOfPml4Entries = 0; IndexOfPml4Entries < NumberOfPml4EntriesNeeded; IndexOfPml4Entries++, PageMapLevel4Entry++) {
+
+  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 (m5LevelPagingSupport) {
+      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.
       //
@@ -207,7 +273,7 @@ SetStaticPageTable (
         }
       } else {
         PageAddress = BASE_4GB;
-      for (IndexOfPdpEntries = 0; IndexOfPdpEntries < NumberOfPdpEntriesNeeded; IndexOfPdpEntries++, PageDirectoryPointerEntry++) {
+        for (IndexOfPdpEntries = 0; IndexOfPdpEntries < (NumberOfPml4EntriesNeeded == 1 ? NumberOfPdpEntriesNeeded : 512); IndexOfPdpEntries++, PageDirectoryPointerEntry++) {
           if (IndexOfPml4Entries == 0 && IndexOfPdpEntries < 4) {
             //
             // Skip the < 4G entries
@@ -240,6 +306,7 @@ SetStaticPageTable (
       }
     }
   }
+}
 
 /**
   Create PageTable for SMM use.
@@ -259,6 +326,8 @@ SmmInitPageTable (
   UINTN                             PageFaultHandlerHookAddress;
   IA32_IDT_GATE_DESCRIPTOR          *IdtEntry;
   EFI_STATUS                        Status;
+  UINT64                            *Pml4Entry;
+  UINT64                            *Pml5Entry;
 
   //
   // Initialize spin lock
@@ -267,11 +336,13 @@ SmmInitPageTable (
 
   mCpuSmmStaticPageTable = PcdGetBool (PcdCpuSmmStaticPageTable);
   m1GPageTableSupport    = Is1GPageSupport ();
-  DEBUG ((DEBUG_INFO, "1GPageTableSupport - 0x%x\n", m1GPageTableSupport));
+  m5LevelPagingSupport   = Is5LevelPagingSupport ();
-  DEBUG ((DEBUG_INFO, "PcdCpuSmmStaticPageTable - 0x%x\n", mCpuSmmStaticPageTable));
-
   mPhysicalAddressBits   = CalculateMaximumSupportAddress ();
-  DEBUG ((DEBUG_INFO, "PhysicalAddressBits - 0x%x\n", mPhysicalAddressBits));
+  PatchInstructionX86 (gPatch5LevelPagingSupport, m5LevelPagingSupport, 1);
+  DEBUG ((DEBUG_INFO, "5LevelPaging Support     - %d\n", m5LevelPagingSupport));
+  DEBUG ((DEBUG_INFO, "1GPageTable Support      - %d\n", m1GPageTableSupport));
+  DEBUG ((DEBUG_INFO, "PcdCpuSmmStaticPageTable - %d\n", mCpuSmmStaticPageTable));
+  DEBUG ((DEBUG_INFO, "PhysicalAddressBits      - %d\n", mPhysicalAddressBits));
   //
   // Generate PAE page table for the first 4GB memory space
   //
@@ -288,15 +359,30 @@ SmmInitPageTable (
   //
   // Fill Page-Table-Level4 (PML4) entry
   //
-  PTEntry = (UINT64*)AllocatePageTableMemory (1);
+  Pml4Entry = (UINT64*)AllocatePageTableMemory (1);
-  ASSERT (PTEntry != NULL);
+  ASSERT (Pml4Entry != NULL);
-  *PTEntry = Pages | mAddressEncMask | PAGE_ATTRIBUTE_BITS;
+  *Pml4Entry = Pages | mAddressEncMask | PAGE_ATTRIBUTE_BITS;
-  ZeroMem (PTEntry + 1, EFI_PAGE_SIZE - sizeof (*PTEntry));
+  ZeroMem (Pml4Entry + 1, EFI_PAGE_SIZE - sizeof (*Pml4Entry));
 
   //
   // Set sub-entries number
   //
-  SetSubEntriesNum (PTEntry, 3);
+  SetSubEntriesNum (Pml4Entry, 3);
+  PTEntry = Pml4Entry;
+
+  if (m5LevelPagingSupport) {
+    //
+    // Fill PML5 entry
+    //
+    Pml5Entry = (UINT64*)AllocatePageTableMemory (1);
+    *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 (mCpuSmmStaticPageTable) {
     SetStaticPageTable ((UINTN)PTEntry);
@@ -344,7 +430,7 @@ SmmInitPageTable (
   }
 
   //
-  // Return the address of PML4 (to set CR3)
+  // Return the address of PML4/PML5 (to set CR3)
   //
   return (UINT32)(UINTN)PTEntry;
 }
@@ -436,12 +522,16 @@ ReclaimPages (
   VOID
   )
 {
+  UINT64                       Pml5Entry;
+  UINT64                       *Pml5;
   UINT64                       *Pml4;
   UINT64                       *Pdpt;
   UINT64                       *Pdt;
+  UINTN                        Pml5Index;
   UINTN                        Pml4Index;
   UINTN                        PdptIndex;
   UINTN                        PdtIndex;
+  UINTN                        MinPml5;
   UINTN                        MinPml4;
   UINTN                        MinPdpt;
   UINTN                        MinPdt;
@@ -451,21 +541,44 @@ ReclaimPages (
   BOOLEAN                      PML4EIgnore;
   BOOLEAN                      PDPTEIgnore;
   UINT64                       *ReleasePageAddress;
+  IA32_CR4                     Cr4;
+  BOOLEAN                      Enable5LevelPaging;
 
   Pml4 = NULL;
   Pdpt = NULL;
   Pdt  = NULL;
   MinAcc  = (UINT64)-1;
   MinPml4 = (UINTN)-1;
+  MinPml5 = (UINTN)-1;
   MinPdpt = (UINTN)-1;
   MinPdt  = (UINTN)-1;
   Acc     = 0;
   ReleasePageAddress = 0;
 
+  Cr4.UintN = AsmReadCr4 ();
+  Enable5LevelPaging = (BOOLEAN) (Cr4.Bits.LA57 == 1);
+  Pml5 = (UINT64*)(UINTN)(AsmReadCr3 () & gPhyMask);
+
+  if (!Enable5LevelPaging) {
+    //
+    // Create one fake PML5 entry for 4-Level Paging
+    // so that the page table parsing logic only handles 5-Level page structure.
+    //
+    Pml5Entry = (UINTN) Pml5 | IA32_PG_P;
+    Pml5 = &Pml5Entry;
+  }
+
   //
   // First, find the leaf entry has the smallest access record value
   //
-  Pml4 = (UINT64*)(UINTN)(AsmReadCr3 () & gPhyMask);
+  for (Pml5Index = 0; Pml5Index < Enable5LevelPaging ? (EFI_PAGE_SIZE / sizeof (*Pml4)) : 1; Pml5Index++) {
+    if ((Pml5[Pml5Index] & IA32_PG_P) == 0 || (Pml5[Pml5Index] & IA32_PG_PMNT) != 0) {
+      //
+      // If the PML5 entry is not present or is masked, skip it
+      //
+      continue;
+    }
+    Pml4 = (UINT64*)(UINTN)(Pml5[Pml5Index] & gPhyMask);
     for (Pml4Index = 0; Pml4Index < EFI_PAGE_SIZE / sizeof (*Pml4); Pml4Index++) {
       if ((Pml4[Pml4Index] & IA32_PG_P) == 0 || (Pml4[Pml4Index] & IA32_PG_PMNT) != 0) {
         //
@@ -522,6 +635,7 @@ ReclaimPages (
                 // save the Page address to be released
                 //
                 MinAcc  = Acc;
+                MinPml5 = Pml5Index;
                 MinPml4 = Pml4Index;
                 MinPdpt = PdptIndex;
                 MinPdt  = PdtIndex;
@@ -541,6 +655,7 @@ ReclaimPages (
               // save the Page address to be released
               //
               MinAcc  = Acc;
+              MinPml5 = Pml5Index;
               MinPml4 = Pml4Index;
               MinPdpt = PdptIndex;
               MinPdt  = (UINTN)-1;
@@ -561,6 +676,7 @@ ReclaimPages (
           // save the Page address to be released
           //
           MinAcc  = Acc;
+          MinPml5 = Pml5Index;
           MinPml4 = Pml4Index;
           MinPdpt = (UINTN)-1;
           MinPdt  = (UINTN)-1;
@@ -568,6 +684,7 @@ ReclaimPages (
         }
       }
     }
+  }
   //
   // Make sure one PML4/PDPT/PD entry is selected
   //
@@ -588,6 +705,7 @@ ReclaimPages (
       //
       // If 4 KByte Page Table is released, check the PDPT entry
       //
+      Pml4 = (UINT64 *) (UINTN) (Pml5[MinPml5] & gPhyMask);
       Pdpt = (UINT64*)(UINTN)(Pml4[MinPml4] & ~mAddressEncMask & gPhyMask);
       SubEntriesNum = GetSubEntriesNum(Pdpt + MinPdpt);
       if (SubEntriesNum == 0) {
@@ -679,7 +797,7 @@ SmiDefaultPFHandler (
   )
 {
   UINT64                            *PageTable;
-  UINT64                            *Pml4;
+  UINT64                            *PageTableTop;
   UINT64                            PFAddress;
   UINTN                             StartBit;
   UINTN                             EndBit;
@@ -690,6 +808,8 @@ SmiDefaultPFHandler (
   UINTN                             PageAttribute;
   EFI_STATUS                        Status;
   UINT64                            *UpperEntry;
+  BOOLEAN                           Enable5LevelPaging;
+  IA32_CR4                          Cr4;
 
   //
   // Set default SMM page attribute
@@ -699,9 +819,12 @@ SmiDefaultPFHandler (
   PageAttribute = 0;
 
   EndBit = 0;
-  Pml4 = (UINT64*)(AsmReadCr3 () & gPhyMask);
+  PageTableTop = (UINT64*)(AsmReadCr3 () & gPhyMask);
   PFAddress = AsmReadCr2 ();
 
+  Cr4.UintN = AsmReadCr4 ();
+  Enable5LevelPaging = (BOOLEAN) (Cr4.Bits.LA57 != 0);
+
   Status = GetPlatformPageTableAttribute (PFAddress, &PageSize, &NumOfPages, &PageAttribute);
   //
   // If platform not support page table attribute, set default SMM page attribute
@@ -755,9 +878,9 @@ SmiDefaultPFHandler (
   }
 
   for (Index = 0; Index < NumOfPages; Index++) {
-    PageTable  = Pml4;
+    PageTable  = PageTableTop;
     UpperEntry = NULL;
-    for (StartBit = 39; StartBit > EndBit; StartBit -= 9) {
+    for (StartBit = Enable5LevelPaging ? 48 : 39; StartBit > EndBit; StartBit -= 9) {
       PTIndex = BitFieldRead64 (PFAddress, StartBit, StartBit + 8);
       if ((PageTable[PTIndex] & IA32_PG_P) == 0) {
         //
@@ -941,13 +1064,20 @@ SetPageTableAttributes (
   UINTN                 Index2;
   UINTN                 Index3;
   UINTN                 Index4;
+  UINTN                 Index5;
   UINT64                *L1PageTable;
   UINT64                *L2PageTable;
   UINT64                *L3PageTable;
   UINT64                *L4PageTable;
+  UINT64                *L5PageTable;
   BOOLEAN               IsSplitted;
   BOOLEAN               PageTableSplitted;
   BOOLEAN               CetEnabled;
+  IA32_CR4              Cr4;
+  BOOLEAN               Enable5LevelPaging;
+
+  Cr4.UintN = AsmReadCr4 ();
+  Enable5LevelPaging = (BOOLEAN) (Cr4.Bits.LA57 == 1);
 
   //
   // Don't do this if
@@ -991,8 +1121,22 @@ SetPageTableAttributes (
   do {
     DEBUG ((DEBUG_INFO, "Start...\n"));
     PageTableSplitted = FALSE;
+    L5PageTable = NULL;
+    if (Enable5LevelPaging) {
+      L5PageTable = (UINT64 *)GetPageTableBase ();
+      SmmSetMemoryAttributesEx ((EFI_PHYSICAL_ADDRESS)(UINTN)L5PageTable, SIZE_4KB, EFI_MEMORY_RO, &IsSplitted);
+      PageTableSplitted = (PageTableSplitted || IsSplitted);
+    }
 
+    for (Index5 = 0; Index5 < (Enable5LevelPaging ? SIZE_4KB/sizeof(UINT64) : 1); Index5++) {
+      if (Enable5LevelPaging) {
+        L4PageTable = (UINT64 *)(UINTN)(L5PageTable[Index5] & ~mAddressEncMask & PAGING_4K_ADDRESS_MASK_64);
+        if (L4PageTable == NULL) {
+          continue;
+        }
+      } else {
         L4PageTable = (UINT64 *)GetPageTableBase ();
+      }
       SmmSetMemoryAttributesEx ((EFI_PHYSICAL_ADDRESS)(UINTN)L4PageTable, SIZE_4KB, EFI_MEMORY_RO, &IsSplitted);
       PageTableSplitted = (PageTableSplitted || IsSplitted);
 
@@ -1032,6 +1176,7 @@ SetPageTableAttributes (
           }
         }
       }
+    }
   } while (PageTableSplitted);
 
   //

UefiCpuPkg/PiSmmCpuDxeSmm/X64/SmiEntry.nasm

@@ -69,6 +69,7 @@ extern ASM_PFX(mXdSupported)
 global ASM_PFX(gPatchXdSupported)
 global ASM_PFX(gPatchSmiStack)
 global ASM_PFX(gPatchSmiCr3)
+global ASM_PFX(gPatch5LevelPagingSupport)
 global ASM_PFX(gcSmiHandlerTemplate)
 global ASM_PFX(gcSmiHandlerSize)
 
@@ -124,6 +125,17 @@ ProtFlatMode:
 ASM_PFX(gPatchSmiCr3):
     mov     cr3, rax
     mov     eax, 0x668                   ; as cr4.PGE is not set here, refresh cr3
+
+    mov     cl, strict byte 0            ; source operand will be patched
+ASM_PFX(gPatch5LevelPagingSupport):
+    cmp     cl, 0
+    je      SkipEnable5LevelPaging
+    ;
+    ; Enable 5-Level Paging bit
+    ;
+    bts     eax, 12                     ; Set LA57 bit (bit #12)
+SkipEnable5LevelPaging:
+
     mov     cr4, rax                    ; in PreModifyMtrrs() to flush TLB.
 ; Load TSS
     sub     esp, 8                      ; reserve room in stack

UefiCpuPkg/PiSmmCpuDxeSmm/X64/SmmProfileArch.c

@@ -1,7 +1,7 @@
 /** @file
 X64 processor specific functions to enable SMM profile.
 
-Copyright (c) 2012 - 2016, Intel Corporation. All rights reserved.<BR>
+Copyright (c) 2012 - 2019, Intel Corporation. All rights reserved.<BR>
 Copyright (c) 2017, AMD Incorporated. All rights reserved.<BR>
 
 SPDX-License-Identifier: BSD-2-Clause-Patent
@@ -147,9 +147,14 @@ RestorePageTableAbove4G (
   BOOLEAN       Existed;
   UINTN         Index;
   UINTN         PFIndex;
+  IA32_CR4      Cr4;
+  BOOLEAN       Enable5LevelPaging;
 
   ASSERT ((PageTable != NULL) && (IsValidPFAddress != NULL));
 
+  Cr4.UintN = AsmReadCr4 ();
+  Enable5LevelPaging = (BOOLEAN) (Cr4.Bits.LA57 == 1);
+
   //
   // If page fault address is 4GB above.
   //
@@ -161,6 +166,15 @@ RestorePageTableAbove4G (
   //
   Existed = FALSE;
   PageTable = (UINT64*)(AsmReadCr3 () & PHYSICAL_ADDRESS_MASK);
+  PTIndex = 0;
+  if (Enable5LevelPaging) {
+    PTIndex = BitFieldRead64 (PFAddress, 48, 56);
+  }
+  if ((!Enable5LevelPaging) || ((PageTable[PTIndex] & IA32_PG_P) != 0)) {
+    // PML5E
+    if (Enable5LevelPaging) {
+      PageTable = (UINT64*)(UINTN)(PageTable[PTIndex] & ~mAddressEncMask & PHYSICAL_ADDRESS_MASK);
+    }
     PTIndex = BitFieldRead64 (PFAddress, 39, 47);
     if ((PageTable[PTIndex] & IA32_PG_P) != 0) {
       // PML4E
@@ -197,6 +211,7 @@ RestorePageTableAbove4G (
         }
       }
     }
+  }
 
   //
   // If page entry does not existed in page table at all, create a new entry.
@@ -221,6 +236,11 @@ RestorePageTableAbove4G (
     //
     PageTable = (UINT64*)(AsmReadCr3 () & PHYSICAL_ADDRESS_MASK);
     PFAddress = AsmReadCr2 ();
+    // PML5E
+    if (Enable5LevelPaging) {
+      PTIndex = BitFieldRead64 (PFAddress, 48, 56);
+      PageTable = (UINT64*)(UINTN)(PageTable[PTIndex] & ~mAddressEncMask & PHYSICAL_ADDRESS_MASK);
+    }
     // PML4E
     PTIndex = BitFieldRead64 (PFAddress, 39, 47);
     PageTable = (UINT64*)(UINTN)(PageTable[PTIndex] & ~mAddressEncMask & PHYSICAL_ADDRESS_MASK);