IntelFsp2Pkg: Support FSP API to save and restore page table

A potential issue may happen when FSP creates/changes page table while
bootloader doesn't expect page table being changed in FSP.
Current, FSP API support to save/restore stack, IDT and general purpose
registers. Following the same pattern, add save/restore page table
support to solve this issue.
Note that this feature only impacts FSP API mode, and is controlled
by PCD PcdFspSaveRestorePageTableEnable. For compatibility, the PCD
default value is set as FALSE.

Signed-off-by: Zhiguang Liu <zhiguang.liu@intel.com>

diff --git a/IntelFsp2Pkg/FspSecCore/Fsp24SecCoreM.inf b/IntelFsp2Pkg/FspSecCore/Fsp24SecCoreM.inf
index 762d485bab04dfdd6b9478bbb11896238819f386..40ff9f22f7c46e2ba629aeb07f4029e224613b90 100644 (file)
--- a/IntelFsp2Pkg/FspSecCore/Fsp24SecCoreM.inf
+++ b/IntelFsp2Pkg/FspSecCore/Fsp24SecCoreM.inf
@@ -69,6 +69,7 @@
   gIntelFsp2PkgTokenSpaceGuid.PcdFspHeapSizePercentage         ## CONSUMES\r
   gIntelFsp2PkgTokenSpaceGuid.PcdFspMaxInterruptSupported      ## CONSUMES\r
   gIntelFsp2PkgTokenSpaceGuid.PcdFspPrivateTemporaryRamSize    ## CONSUMES\r
+  gIntelFsp2PkgTokenSpaceGuid.PcdFspSaveRestorePageTableEnable ## CONSUMES\r
 \r
 [Ppis]\r
   gEfiTemporaryRamSupportPpiGuid                              ## PRODUCES\r

diff --git a/IntelFsp2Pkg/FspSecCore/FspSecCoreM.inf b/IntelFsp2Pkg/FspSecCore/FspSecCoreM.inf
index 3acf4f6f9dc49e5ae3f0c643d5605d9b4c0f85c5..ac572a61687f4ceb726f973907bcf2badf091bec 100644 (file)
--- a/IntelFsp2Pkg/FspSecCore/FspSecCoreM.inf
+++ b/IntelFsp2Pkg/FspSecCore/FspSecCoreM.inf
@@ -68,6 +68,7 @@
   gIntelFsp2PkgTokenSpaceGuid.PcdFspHeapSizePercentage         ## CONSUMES\r
   gIntelFsp2PkgTokenSpaceGuid.PcdFspMaxInterruptSupported      ## CONSUMES\r
   gIntelFsp2PkgTokenSpaceGuid.PcdFspPrivateTemporaryRamSize    ## CONSUMES\r
+  gIntelFsp2PkgTokenSpaceGuid.PcdFspSaveRestorePageTableEnable ## CONSUMES\r
 \r
 [Ppis]\r
   gEfiTemporaryRamSupportPpiGuid                              ## PRODUCES\r

diff --git a/IntelFsp2Pkg/FspSecCore/Ia32/Fsp24ApiEntryM.nasm b/IntelFsp2Pkg/FspSecCore/Ia32/Fsp24ApiEntryM.nasm
index 5fa5c035692af23f9ad549dc6657598469c8a306..e9bf0cbed205c2463974811235c9f708018b32aa 100644 (file)
--- a/IntelFsp2Pkg/FspSecCore/Ia32/Fsp24ApiEntryM.nasm
+++ b/IntelFsp2Pkg/FspSecCore/Ia32/Fsp24ApiEntryM.nasm
@@ -13,6 +13,7 @@
 extern   ASM_PFX(PcdGet32(PcdTemporaryRamBase))\r
 extern   ASM_PFX(PcdGet32(PcdFspTemporaryRamSize))\r
 extern   ASM_PFX(PcdGet8 (PcdFspHeapSizePercentage))\r
+extern   ASM_PFX(FeaturePcdGet (PcdFspSaveRestorePageTableEnable))\r
 \r
 struc FSPM_UPD_COMMON\r
     ; FSP_UPD_HEADER {\r
@@ -64,7 +65,7 @@ extern ASM_PFX(AsmGetFspInfoHeader)
 extern ASM_PFX(FspMultiPhaseMemInitApiHandler)\r
 \r
 STACK_SAVED_EAX_OFFSET       EQU   4 * 7 ; size of a general purpose register * eax index\r
-API_PARAM1_OFFSET            EQU   34h  ; ApiParam1 [ sub esp,8 + pushad + pushfd + push eax + call]\r
+API_PARAM1_OFFSET            EQU   44h  ; ApiParam1 [ sub esp,8 + push cr0/cr3/cr4/EFER + pushad + pushfd + push eax + call]\r
 FSP_HEADER_IMGBASE_OFFSET    EQU   1Ch\r
 FSP_HEADER_CFGREG_OFFSET     EQU   24h\r
 \r
@@ -153,6 +154,33 @@ NotMultiPhaseMemoryInitApi:
   cli\r
   pushad\r
 \r
+  ;\r
+  ; Allocate 4x4 bytes on the stack.\r
+  ;\r
+  sub     esp, 16\r
+  cmp     byte [dword ASM_PFX(FeaturePcdGet (PcdFspSaveRestorePageTableEnable))], 0\r
+  jz      SkipPagetableSave\r
+\r
+  add     esp, 16\r
+  ; Save EFER MSR lower 32 bits\r
+  push   ecx\r
+  push   eax\r
+  mov    ecx, 0xC0000080\r
+  rdmsr\r
+  mov    edx, eax\r
+  pop    eax\r
+  pop    ecx\r
+  push   edx\r
+\r
+  ; Save CR registers\r
+  mov    edx, cr4\r
+  push   edx\r
+  mov    edx, cr3\r
+  push   edx\r
+  mov    edx, cr0\r
+  push   edx\r
+SkipPagetableSave:\r
+\r
   ; Reserve 8 bytes for IDT save/restore\r
   sub     esp, 8\r
   sidt    [esp]\r

diff --git a/IntelFsp2Pkg/FspSecCore/Ia32/FspApiEntryM.nasm b/IntelFsp2Pkg/FspSecCore/Ia32/FspApiEntryM.nasm
index 861cce4d01a19668d26cddcc5365a068036f1768..b1623063ef1bc5922daf58b17d0b020cce066002 100644 (file)
--- a/IntelFsp2Pkg/FspSecCore/Ia32/FspApiEntryM.nasm
+++ b/IntelFsp2Pkg/FspSecCore/Ia32/FspApiEntryM.nasm
@@ -13,6 +13,7 @@
 extern   ASM_PFX(PcdGet32(PcdTemporaryRamBase))\r
 extern   ASM_PFX(PcdGet32(PcdFspTemporaryRamSize))\r
 extern   ASM_PFX(PcdGet8 (PcdFspHeapSizePercentage))\r
+extern   ASM_PFX(FeaturePcdGet (PcdFspSaveRestorePageTableEnable))\r
 \r
 struc FSPM_UPD_COMMON\r
     ; FSP_UPD_HEADER {\r
@@ -62,7 +63,7 @@ extern ASM_PFX(FspApiCommon)
 extern ASM_PFX(AsmGetFspBaseAddress)\r
 extern ASM_PFX(AsmGetFspInfoHeader)\r
 \r
-API_PARAM1_OFFSET            EQU   34h  ; ApiParam1 [ sub esp,8 + pushad + pushfd + push eax + call]\r
+API_PARAM1_OFFSET            EQU   44h  ; ApiParam1 [ sub esp,8 + push cr0/cr3/cr4/EFER +pushad + pushfd + push eax + call]\r
 FSP_HEADER_IMGBASE_OFFSET    EQU   1Ch\r
 FSP_HEADER_CFGREG_OFFSET     EQU   24h\r
 \r
@@ -124,6 +125,33 @@ ASM_PFX(FspApiCommonContinue):
   cli\r
   pushad\r
 \r
+  ;\r
+  ; Allocate 4x4 bytes on the stack.\r
+  ;\r
+  sub     esp, 16\r
+  cmp     byte [dword ASM_PFX(FeaturePcdGet (PcdFspSaveRestorePageTableEnable))], 0\r
+  jz      SkipPagetableSave\r
+\r
+  add     esp, 16\r
+  ; Save EFER MSR lower 32-bit\r
+  push   ecx\r
+  push   eax\r
+  mov    ecx, 0xC0000080\r
+  rdmsr\r
+  mov    edx, eax\r
+  pop    eax\r
+  pop    ecx\r
+  push   edx\r
+\r
+  ; Save CR registers\r
+  mov    edx, cr4\r
+  push   edx\r
+  mov    edx, cr3\r
+  push   edx\r
+  mov    edx, cr0\r
+  push   edx\r
+\r
+SkipPagetableSave:\r
   ; Reserve 8 bytes for IDT save/restore\r
   sub     esp, 8\r
   sidt    [esp]\r

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

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

diff --git a/IntelFsp2Pkg/IntelFsp2Pkg.dec b/IntelFsp2Pkg/IntelFsp2Pkg.dec
index d1c3d3ee7b90bf83c86ff21c9a7a0c58883cd537..8fe6b64f9971ba4eeb8d263e014f911eb9095d33 100644 (file)
--- a/IntelFsp2Pkg/IntelFsp2Pkg.dec
+++ b/IntelFsp2Pkg/IntelFsp2Pkg.dec
@@ -114,6 +114,14 @@
   #\r
   gIntelFsp2PkgTokenSpaceGuid.PcdFspPrivateTemporaryRamSize |0x00000000|UINT32|0x10000006\r
 \r
+[PcdsFeatureFlag]\r
+  #\r
+  # Indicates if the FSP will save and restore page table. Only works in FSP API mode\r
+  #   TRUE  - FSP will save and restore page table\r
+  #   FALSE - FSP will not save and restore page table\r
+  #\r
+  gIntelFsp2PkgTokenSpaceGuid.PcdFspSaveRestorePageTableEnable |FALSE|BOOLEAN|0x10000007\r
+\r
 [PcdsFixedAtBuild,PcdsDynamic,PcdsDynamicEx]\r
   gIntelFsp2PkgTokenSpaceGuid.PcdFspReservedMemoryLength  |0x00100000|UINT32|0x46530000\r
   gIntelFsp2PkgTokenSpaceGuid.PcdBootLoaderEntry          |0xFFFFFFE4|UINT32|0x46530100\r

diff --git a/IntelFsp2Pkg/Library/BaseFspCommonLib/FspCommonLib.c b/IntelFsp2Pkg/Library/BaseFspCommonLib/FspCommonLib.c
index 54dbf546c3608f206f6de9e1505e09e0f937bd0a..3ecc5bd265b267fdae4a2bba54a53968ad6b0efe 100644 (file)
--- a/IntelFsp2Pkg/Library/BaseFspCommonLib/FspCommonLib.c
+++ b/IntelFsp2Pkg/Library/BaseFspCommonLib/FspCommonLib.c
@@ -15,21 +15,14 @@
 \r
 #pragma pack(1)\r
 \r
-//\r
-//   API Parameter                +0x34\r
-//   API return address           +0x30\r
-//\r
-//   push    FspInfoHeader        +0x2C\r
-//   pushfd                       +0x28\r
-//   cli\r
-//   pushad                       +0x24\r
-//   sub     esp, 8               +0x00\r
-//   sidt    fword ptr [esp]\r
-//\r
 typedef struct {\r
   UINT16    IdtrLimit;\r
   UINT32    IdtrBase;\r
   UINT16    Reserved;\r
+  UINT32    Cr0;\r
+  UINT32    Cr3;\r
+  UINT32    Cr4;\r
+  UINT32    Efer;           // lower 32-bit of EFER since only NXE bit (BIT11) need to be restored.\r
   UINT32    Registers[8];   // General Purpose Registers: Edi, Esi, Ebp, Esp, Ebx, Edx, Ecx and Eax\r
   UINT16    Flags[2];\r
   UINT32    FspInfoHeader;\r
@@ -37,20 +30,12 @@ typedef struct {
   UINT32    ApiParam[2];\r
 } CONTEXT_STACK;\r
 \r
-//\r
-//   API return address           +0xB8\r
-//   Reserved                     +0xB0\r
-//   push    API Parameter2       +0xA8\r
-//   push    API Parameter1       +0xA0\r
-//   push    FspInfoHeader        +0x98\r
-//   pushfq                       +0x90\r
-//   cli\r
-//   PUSHA_64                     +0x10\r
-//   sub     rsp, 16              +0x00\r
-//   sidt    [rsp]\r
-//\r
 typedef struct {\r
   UINT64    Idtr[2];        // IDTR Limit - bit0:bi15, IDTR Base - bit16:bit79\r
+  UINT64    Cr0;\r
+  UINT64    Cr3;\r
+  UINT64    Cr4;\r
+  UINT64    Efer;\r
   UINT64    Registers[16];  // General Purpose Registers: RDI, RSI, RBP, RSP, RBX, RDX, RCX, RAX, and R15 to R8\r
   UINT32    Flags[2];\r
   UINT64    FspInfoHeader;\r

diff --git a/IntelFsp2Pkg/Library/BaseFspSwitchStackLib/BaseFspSwitchStackLib.inf b/IntelFsp2Pkg/Library/BaseFspSwitchStackLib/BaseFspSwitchStackLib.inf
index 6909aec651b0a1fd6e78f65b1b857fcb9dc6db88..0194c2e9554b254292fe94a8ade135f216f38ce3 100644 (file)
--- a/IntelFsp2Pkg/Library/BaseFspSwitchStackLib/BaseFspSwitchStackLib.inf
+++ b/IntelFsp2Pkg/Library/BaseFspSwitchStackLib/BaseFspSwitchStackLib.inf
@@ -32,5 +32,5 @@
   BaseLib\r
   IoLib\r
 \r
-\r
-\r
+[Pcd]\r
+  gIntelFsp2PkgTokenSpaceGuid.PcdFspSaveRestorePageTableEnable\r

diff --git a/IntelFsp2Pkg/Library/BaseFspSwitchStackLib/Ia32/Stack.nasm b/IntelFsp2Pkg/Library/BaseFspSwitchStackLib/Ia32/Stack.nasm
index 6599901906bf85202ccf4727fd49e579f7940347..d13842451ca822db7f5d99e1d6848d667c72fcb6 100644 (file)
--- a/IntelFsp2Pkg/Library/BaseFspSwitchStackLib/Ia32/Stack.nasm
+++ b/IntelFsp2Pkg/Library/BaseFspSwitchStackLib/Ia32/Stack.nasm
@@ -12,6 +12,12 @@
     SECTION .text\r
 \r
 extern ASM_PFX(SwapStack)\r
+extern ASM_PFX(FeaturePcdGet (PcdFspSaveRestorePageTableEnable))\r
+\r
+; Page table related bits in CR0/CR4/EFER\r
+%define CR0_PG_MASK          0x80010000  ; CR0.PG and CR0.WP\r
+%define CR4_PG_MASK          0x10B0      ; CR4.PSE, CR4.PAE, CR4.PGE and CR4.LA57\r
+%define EFER_PG_MASK         0x800       ; EFER.NXE\r
 \r
 ;------------------------------------------------------------------------------\r
 ; UINT32\r
@@ -50,6 +56,34 @@ ASM_PFX(FspSwitchStack):
     pushfd\r
     cli\r
     pushad\r
+\r
+    ;\r
+    ; Allocate 4x4 bytes on the stack.\r
+    ;\r
+    sub     esp, 16\r
+    cmp     byte [dword ASM_PFX(FeaturePcdGet (PcdFspSaveRestorePageTableEnable))], 0\r
+    jz      SkipPagetableSave\r
+\r
+    add     esp, 16\r
+    ; Save EFER MSR lower 32 bits\r
+    push   ecx\r
+    push   eax\r
+    mov    ecx, 0xC0000080\r
+    rdmsr\r
+    mov    edx, eax\r
+    pop    eax\r
+    pop    ecx\r
+    push   edx\r
+\r
+    ; Save CR registers\r
+    mov    eax, cr4\r
+    push   eax\r
+    mov    eax, cr3\r
+    push   eax\r
+    mov    eax, cr0\r
+    push   eax\r
+SkipPagetableSave:\r
+\r
     sub     esp, 8\r
     sidt    [esp]\r
 \r
@@ -61,6 +95,104 @@ ASM_PFX(FspSwitchStack):
     ; Restore previous contexts\r
     lidt    [esp]\r
     add     esp, 8\r
+\r
+    cmp     byte [dword ASM_PFX(FeaturePcdGet (PcdFspSaveRestorePageTableEnable))], 0\r
+    jz      SkipPagetableRestore\r
+    ; [esp]    stores new cr0\r
+    ; [esp+4]  stores new cr3\r
+    ; [esp+8]  stores new cr4\r
+    ; [esp+12] stores new Efer\r
+    ;\r
+    ; When new EFER.NXE == 1, the restore flow is: EFER --> CRx\r
+    ; Otherwise: CRx --> EFER\r
+    ; When new CR0.PG == 1, the restore flow for CRx is: CR3 --> CR4 --> CR0\r
+    ; Otherwise, the restore flow is: CR0 --> CR3 --> CR4\r
+    ;\r
+    ; If NXE bit is changed to 1, change NXE before CR register\r
+    ; This is because Nx bit in page table entry in new CR3 will be invalid\r
+    ; if updating CR3 before EFER MSR.\r
+    ;\r
+    mov eax, [esp+12]\r
+    bt  eax, 11\r
+    jnc SkipEferLabel1\r
+\r
+    ; Restore EFER MSR\r
+    mov    ecx, 0xC0000080\r
+    rdmsr\r
+    and    eax, ~EFER_PG_MASK\r
+    mov    ebx, [esp+12]\r
+    and    ebx, EFER_PG_MASK\r
+    or     eax, ebx\r
+    wrmsr\r
+\r
+SkipEferLabel1:\r
+\r
+    ;\r
+    ; if new cr0 is to disable page table, change CR0 before CR3/CR4\r
+    ;\r
+    mov     eax, [esp]\r
+    bt      eax, 31\r
+    jc      SkipCr0Label1\r
+\r
+    ; Restore CR0\r
+    mov     edx, cr0\r
+    and     edx, ~CR0_PG_MASK\r
+    mov     eax, [esp]\r
+    and     eax, CR0_PG_MASK\r
+    or      edx, eax\r
+    mov     cr0, edx\r
+\r
+SkipCr0Label1:\r
+\r
+    ; Restore CR3/CR4\r
+    mov     eax, [esp+4]\r
+    mov     cr3, eax\r
+\r
+    mov     edx, cr4\r
+    and     edx, ~CR4_PG_MASK\r
+    mov     eax, [esp+8]\r
+    and     eax, CR4_PG_MASK\r
+    or      edx, eax\r
+    mov     cr4, edx\r
+\r
+    ;\r
+    ; if new cr0 is to enable page table, change CR0 after CR3/CR4\r
+    ;\r
+    mov     eax, [esp]\r
+    bt      eax, 31\r
+    jnc     SkipCr0Label2\r
+\r
+    ; Restore CR0\r
+    mov     edx, cr0\r
+    and     edx, ~CR0_PG_MASK\r
+    mov     eax, [esp]\r
+    and     eax, CR0_PG_MASK\r
+    or      edx, eax\r
+    mov     cr0, edx\r
+\r
+SkipCr0Label2:\r
+    ;\r
+    ; If NXE bit is changed to 0, change NXE after than CR regiser\r
+    ;\r
+    mov eax, [esp+12]\r
+    bt  eax, 11\r
+    jc SkipEferLabel2\r
+\r
+    ; Restore EFER MSR\r
+    mov    ecx, 0xC0000080\r
+    rdmsr\r
+    and    eax, ~EFER_PG_MASK\r
+    mov    ebx, [esp+12]\r
+    and    ebx, EFER_PG_MASK\r
+    or     eax, ebx\r
+    wrmsr\r
+\r
+SkipEferLabel2:\r
+SkipPagetableRestore:\r
+\r
+    ; pop page table related registers.\r
+    add     esp, 16\r
+\r
     popad\r
     popfd\r
     add     esp, 4\r

diff --git a/IntelFsp2Pkg/Library/BaseFspSwitchStackLib/X64/Stack.nasm b/IntelFsp2Pkg/Library/BaseFspSwitchStackLib/X64/Stack.nasm
index e3a7cf002fe6c32285197c827c294d85b15b68a5..f40df51ab4ada65302843206e5c931818a53a66d 100644 (file)
--- a/IntelFsp2Pkg/Library/BaseFspSwitchStackLib/X64/Stack.nasm
+++ b/IntelFsp2Pkg/Library/BaseFspSwitchStackLib/X64/Stack.nasm
@@ -8,12 +8,18 @@
 ;   Switch the stack from temporary memory to permanent memory.\r
 ;\r
 ;------------------------------------------------------------------------------\r
-\r
+    DEFAULT REL\r
     SECTION .text\r
 \r
 %include    "PushPopRegsNasm.inc"\r
 \r
+; Page table related bits in CR0/CR4/EFER\r
+%define CR0_PG_MASK          0x80010000  ; CR0.PG and CR0.WP\r
+%define CR4_PG_MASK          0x10B0      ; CR4.PSE, CR4.PAE, CR4.PGE and CR4.LA57\r
+%define EFER_PG_MASK         0x800       ; EFER.NXE\r
+\r
 extern ASM_PFX(SwapStack)\r
+extern ASM_PFX(FeaturePcdGet (PcdFspSaveRestorePageTableEnable))\r
 \r
 ;------------------------------------------------------------------------------\r
 ; UINT32\r
@@ -55,6 +61,37 @@ ASM_PFX(FspSwitchStack):
     pushfq\r
     cli\r
     PUSHA_64\r
+\r
+    ;\r
+    ; Allocate 4x8 bytes on the stack.\r
+    ;\r
+    sub     rsp, 32\r
+    lea     rdx, [ASM_PFX(FeaturePcdGet (PcdFspSaveRestorePageTableEnable))]\r
+    mov     dl, byte [rdx]\r
+    cmp     dl, 0\r
+    jz      SkipPagetableSave\r
+\r
+    add     rsp, 32\r
+    ; Save EFER MSR\r
+    push   rcx\r
+    push   rax\r
+    mov    rcx, 0xC0000080\r
+    rdmsr\r
+    shl    rdx, 0x20\r
+    or     rdx, rax\r
+    pop    rax\r
+    pop    rcx\r
+    push   rdx\r
+\r
+    ; Save CR registers\r
+    mov    rdx, cr4\r
+    push   rdx\r
+    mov    rdx, cr3\r
+    push   rdx\r
+    mov    rdx, cr0\r
+    push   rdx\r
+SkipPagetableSave:\r
+\r
     sub     rsp, 16\r
     sidt    [rsp]\r
 \r
@@ -68,6 +105,76 @@ ASM_PFX(FspSwitchStack):
     ; Restore previous contexts\r
     lidt    [rsp]\r
     add     rsp, 16\r
+\r
+    lea     rax, [ASM_PFX(FeaturePcdGet (PcdFspSaveRestorePageTableEnable))]\r
+    mov     al, byte [rax]\r
+    cmp     al, 0\r
+    jz      SkipPagetableRestore\r
+    ; [rsp]    stores new cr0\r
+    ; [rsp+8]  stores new cr3\r
+    ; [rsp+16] stores new cr4\r
+    ; [rsp+24] stores new Efer\r
+    ;\r
+    ; When new EFER.NXE == 1, the restore flow is: EFER --> CRx\r
+    ; Otherwise: CRx --> EFER\r
+    ;\r
+    ; If NXE bit is changed to 1, change NXE before CR register\r
+    ; This is because Nx bit in page table entry in new CR3 will be invalid\r
+    ; if updating CR3 before EFER MSR.\r
+    ;\r
+    mov rax, [rsp + 24]\r
+    bt  rax, 11\r
+    jnc SkipEferLabel1\r
+\r
+    ; Restore EFER MSR\r
+    mov    ecx, 0xC0000080\r
+    rdmsr\r
+    and    eax, ~EFER_PG_MASK\r
+    mov    ebx, [rsp + 24]\r
+    and    ebx, EFER_PG_MASK\r
+    or     eax, ebx\r
+    wrmsr\r
+\r
+SkipEferLabel1:\r
+\r
+    mov     rbx, [rsp]\r
+    mov     rdx, cr0\r
+    and     rdx, ~CR0_PG_MASK\r
+    and     rbx, CR0_PG_MASK\r
+    or      rdx, rbx\r
+    mov     cr0, rdx\r
+\r
+    mov     rbx, [rsp + 8]\r
+    mov     cr3, rbx\r
+\r
+    mov     rbx, [rsp + 16]\r
+    mov     rdx, cr4\r
+    and     rdx, ~CR4_PG_MASK\r
+    and     rbx, CR4_PG_MASK\r
+    or      rdx, rbx\r
+    mov     cr4, rdx\r
+\r
+    ;\r
+    ; If NXE bit is changed to 0, change NXE after than CR regiser\r
+    ;\r
+    mov rax, [rsp + 24]\r
+    bt  rax, 11\r
+    jc  SkipEferLabel2\r
+\r
+    ; Restore EFER MSR\r
+    mov    ecx, 0xC0000080\r
+    rdmsr\r
+    and    eax, ~EFER_PG_MASK\r
+    mov    ebx, [rsp + 24]\r
+    and    ebx, EFER_PG_MASK\r
+    or     eax, ebx\r
+    wrmsr\r
+\r
+SkipEferLabel2:\r
+SkipPagetableRestore:\r
+    ; pop page table related registers.\r
+    add     rsp, 32\r
+\r
     POPA_64\r
     popfq\r
     add     rsp, 32 ; FspInfoHeader + ApiParam[2] + Reserved QWORD\r