1 files changed, 32 insertions, 1 deletions
diff --git a/IntelFsp2Pkg/FspSecCore/X64/Fsp24ApiEntryM.nasm b/IntelFsp2Pkg/FspSecCore/X64/Fsp24ApiEntryM.nasm
index a3b38e4585..3066156bcf 100644
--- a/IntelFsp2Pkg/FspSecCore/X64/Fsp24ApiEntryM.nasm
+++ b/IntelFsp2Pkg/FspSecCore/X64/Fsp24ApiEntryM.nasm
@@ -4,7 +4,7 @@
 ; Copyright (c) 2022, Intel Corporation. All rights reserved.<BR>
 ; SPDX-License-Identifier: BSD-2-Clause-Patent
 ;;
-
+    DEFAULT REL
     SECTION .text
 
 %include    "PushPopRegsNasm.inc"
@@ -13,6 +13,7 @@
 ; Following are fixed PCDs
 ;
 extern   ASM_PFX(PcdGet8 (PcdFspHeapSizePercentage))
+extern   ASM_PFX(FeaturePcdGet (PcdFspSaveRestorePageTableEnable))
 
 struc FSPM_UPD_COMMON_FSP24
     ; FSP_UPD_HEADER {
@@ -142,6 +143,36 @@ NotMultiPhaseMemoryInitApi:
   cli
   PUSHA_64
 
+  ;
+  ; Allocate 4x8 bytes on the stack.
+  ;
+  sub     rsp, 32
+  lea     rdx, [ASM_PFX(FeaturePcdGet (PcdFspSaveRestorePageTableEnable))]
+  mov     dl, byte [rdx]
+  cmp     dl, 0
+  jz      SkipPagetableSave
+
+  add     rsp, 32
+  ; Save EFER MSR
+  push   rcx
+  push   rax
+  mov    rcx, 0xC0000080
+  rdmsr
+  shl    rdx, 0x20
+  or     rdx, rax
+  pop    rax
+  pop    rcx
+  push   rdx
+
+  ; Save CR registers
+  mov    rdx, cr4
+  push   rdx
+  mov    rdx, cr3
+  push   rdx
+  mov    rdx, cr0
+  push   rdx
+SkipPagetableSave:
+
   ; Reserve 16 bytes for IDT save/restore
   sub     rsp, 16
   sidt    [rsp]