From 562bce0febd641f78df7cd61f2ed5a4c944b31ac Mon Sep 17 00:00:00 2001
From: Ray Ni <ray.ni@intel.com>
Date: Tue, 15 Jul 2025 15:41:50 +0800
Subject: [PATCH] IntelFsp2Pkg: Preserve GDTR and CS/DS/ES/FS/GS/SS

Bootloader does not expect FSP modifies GDTR and segment selectors, update
FSP entry/exit code to preserve these registers.

Signed-off-by: Ray Ni <ray.ni@intel.com>
Cc: Chasel Chiu <chasel.chiu@intel.com>
Cc: Nate DeSimone <nathaniel.l.desimone@intel.com>
Cc: Star Zeng <star.zeng@intel.com>
Cc: Ted Kuo <ted.kuo@intel.com>
Cc: Ashraf Ali S <ashraf.ali.s@intel.com>
---
 .../FspSecCore/X64/Fsp24ApiEntryM.nasm        | 20 ++++++++-
 IntelFsp2Pkg/FspSecCore/X64/FspApiEntryM.nasm | 20 ++++++++-
 .../Library/BaseFspCommonLib/FspCommonLib.c   |  4 +-
 .../BaseFspSwitchStackLib/X64/Stack.nasm      | 41 ++++++++++++++++++-
 4 files changed, 81 insertions(+), 4 deletions(-)

diff --git a/IntelFsp2Pkg/FspSecCore/X64/Fsp24ApiEntryM.nasm b/IntelFsp2Pkg/FspSecCore/X64/Fsp24ApiEntryM.nasm
index 3066156bcf..3552f54f73 100644
--- a/IntelFsp2Pkg/FspSecCore/X64/Fsp24ApiEntryM.nasm
+++ b/IntelFsp2Pkg/FspSecCore/X64/Fsp24ApiEntryM.nasm
@@ -1,7 +1,7 @@
 ;; @file
 ;  Provide FSP API entry points.
 ;
-; Copyright (c) 2022, Intel Corporation. All rights reserved.<BR>
+; Copyright (c) 2022 - 2025, Intel Corporation. All rights reserved.<BR>
 ; SPDX-License-Identifier: BSD-2-Clause-Patent
 ;;
     DEFAULT REL
@@ -173,6 +173,24 @@ NotMultiPhaseMemoryInitApi:
   push   rdx
 SkipPagetableSave:
 
+  ; Save Segment registers
+  mov     rdx, ss
+  push    rdx
+  mov     rdx, gs
+  push    rdx
+  mov     rdx, fs
+  push    rdx
+  mov     rdx, es
+  push    rdx
+  mov     rdx, ds
+  push    rdx
+  mov     rdx, cs
+  push    rdx
+
+  ; Reserve 16 bytes for GDT save/restore
+  sub     rsp, 16
+  sgdt    [rsp]
+
   ; Reserve 16 bytes for IDT save/restore
   sub     rsp, 16
   sidt    [rsp]
diff --git a/IntelFsp2Pkg/FspSecCore/X64/FspApiEntryM.nasm b/IntelFsp2Pkg/FspSecCore/X64/FspApiEntryM.nasm
index b0b6b6a4aa..873adb29be 100644
--- a/IntelFsp2Pkg/FspSecCore/X64/FspApiEntryM.nasm
+++ b/IntelFsp2Pkg/FspSecCore/X64/FspApiEntryM.nasm
@@ -1,7 +1,7 @@
 ;; @file
 ;  Provide FSP API entry points.
 ;
-; Copyright (c) 2022, Intel Corporation. All rights reserved.<BR>
+; Copyright (c) 2022 - 2025, Intel Corporation. All rights reserved.<BR>
 ; SPDX-License-Identifier: BSD-2-Clause-Patent
 ;;
     DEFAULT REL
@@ -141,6 +141,24 @@ ASM_PFX(FspApiCommonContinue):
   push   rdx
 SkipPagetableSave:
 
+  ; Save Segment registers
+  mov     rdx, ss
+  push    rdx
+  mov     rdx, gs
+  push    rdx
+  mov     rdx, fs
+  push    rdx
+  mov     rdx, es
+  push    rdx
+  mov     rdx, ds
+  push    rdx
+  mov     rdx, cs
+  push    rdx
+
+  ; Reserve 16 bytes for GDT save/restore
+  sub     rsp, 16
+  sgdt    [rsp]
+
   ; Reserve 16 bytes for IDT save/restore
   sub     rsp, 16
   sidt    [rsp]
diff --git a/IntelFsp2Pkg/Library/BaseFspCommonLib/FspCommonLib.c b/IntelFsp2Pkg/Library/BaseFspCommonLib/FspCommonLib.c
index f1b1a5ea58..9cb3d52303 100644
--- a/IntelFsp2Pkg/Library/BaseFspCommonLib/FspCommonLib.c
+++ b/IntelFsp2Pkg/Library/BaseFspCommonLib/FspCommonLib.c
@@ -31,7 +31,9 @@ typedef struct {
 } CONTEXT_STACK;
 
 typedef struct {
-  UINT64    Idtr[2];        // IDTR Limit - bit0:bi15, IDTR Base - bit16:bit79
+  UINT64    Idtr[2];        // IDTR Limit - bit0:bit15, IDTR Base - bit16:bit79
+  UINT64    Gdtr[2];        // GDTR Limit - bit0:bit15, GDTR Base - bit16:bit79
+  UINT64    Segment[6];     // Segment Registers: CS, DS, ES, FS, GS, SS
   UINT64    Cr0;
   UINT64    Cr3;
   UINT64    Cr4;
diff --git a/IntelFsp2Pkg/Library/BaseFspSwitchStackLib/X64/Stack.nasm b/IntelFsp2Pkg/Library/BaseFspSwitchStackLib/X64/Stack.nasm
index f40df51ab4..d77f59bc9a 100644
--- a/IntelFsp2Pkg/Library/BaseFspSwitchStackLib/X64/Stack.nasm
+++ b/IntelFsp2Pkg/Library/BaseFspSwitchStackLib/X64/Stack.nasm
@@ -1,6 +1,6 @@
 ;------------------------------------------------------------------------------
 ;
-; Copyright (c) 2022 - 2023, Intel Corporation. All rights reserved.<BR>
+; Copyright (c) 2022 - 2025, Intel Corporation. All rights reserved.<BR>
 ; SPDX-License-Identifier: BSD-2-Clause-Patent
 ;
 ; Abstract:
@@ -92,6 +92,25 @@ ASM_PFX(FspSwitchStack):
     push   rdx
 SkipPagetableSave:
 
+    ; Save Segment registers
+    mov     rdx, ss
+    push    rdx
+    mov     rdx, gs
+    push    rdx
+    mov     rdx, fs
+    push    rdx
+    mov     rdx, es
+    push    rdx
+    mov     rdx, ds
+    push    rdx
+    mov     rdx, cs
+    push    rdx
+
+    ; Reserve 16 bytes for GDT save/restore
+    sub     rsp, 16
+    sgdt    [rsp]
+
+    ; Reserve 16 bytes for IDT save/restore
     sub     rsp, 16
     sidt    [rsp]
 
@@ -106,6 +125,26 @@ SkipPagetableSave:
     lidt    [rsp]
     add     rsp, 16
 
+    ; Restore GDTR
+    lgdt    [rsp]
+    add     rsp, 16
+
+    ; Restore Segment registers
+    lea     rdx, [.0]
+    push    rdx       ; Push return address
+    retfq             ; Far return to restore CS (uses CS from stack + return address)
+.0:
+    pop     rdx
+    mov     ds, dx
+    pop     rdx
+    mov     es, dx
+    pop     rdx
+    mov     fs, dx
+    pop     rdx
+    mov     gs, dx
+    pop     rdx
+    mov     ss, dx
+
     lea     rax, [ASM_PFX(FeaturePcdGet (PcdFspSaveRestorePageTableEnable))]
     mov     al, byte [rax]
     cmp     al, 0