UefiCpuPkg/PiSmmCpuDxeSmm: Place AP to 32bit protected mode on S3 path

[mirror_edk2.git] / UefiCpuPkg / PiSmmCpuDxeSmm / X64 / SmmFuncsArch.c

diff --git a/UefiCpuPkg/PiSmmCpuDxeSmm/X64/SmmFuncsArch.c b/UefiCpuPkg/PiSmmCpuDxeSmm/X64/SmmFuncsArch.c
index bd465c7a067b6414865b4f7334351bdd544b9231..62338b79bd1f32d3c99886920d1c62130ec84b86 100644 (file)
--- a/UefiCpuPkg/PiSmmCpuDxeSmm/X64/SmmFuncsArch.c
+++ b/UefiCpuPkg/PiSmmCpuDxeSmm/X64/SmmFuncsArch.c
@@ -69,6 +69,37 @@ InitGdt (
   return GdtTssTables;\r
 }\r
 \r
+/**\r
+  Get Protected mode code segment from current GDT table.\r
+\r
+  @return  Protected mode code segment value.\r
+**/\r
+UINT16\r
+GetProtectedModeCS (\r
+  VOID\r
+  )\r
+{\r
+  IA32_DESCRIPTOR          GdtrDesc;\r
+  IA32_SEGMENT_DESCRIPTOR  *GdtEntry;\r
+  UINTN                    GdtEntryCount;\r
+  UINT16                   Index;\r
+\r
+  Index = (UINT16) -1;\r
+  AsmReadGdtr (&GdtrDesc);\r
+  GdtEntryCount = (GdtrDesc.Limit + 1) / sizeof (IA32_SEGMENT_DESCRIPTOR);\r
+  GdtEntry = (IA32_SEGMENT_DESCRIPTOR *) GdtrDesc.Base;\r
+  for (Index = 0; Index < GdtEntryCount; Index++) {\r
+    if (GdtEntry->Bits.L == 0) {\r
+      if (GdtEntry->Bits.Type > 8 && GdtEntry->Bits.L == 0) {\r
+        break;\r
+      }\r
+    }\r
+    GdtEntry++;\r
+  }\r
+  ASSERT (Index != -1);\r
+  return Index * 8;\r
+}\r
+\r
 /**\r
   Transfer AP to safe hlt-loop after it finished restore CPU features on S3 patch.\r
 \r
@@ -82,11 +113,12 @@ TransferApToSafeState (
   IN UINT32             TopOfStack\r
   )\r
 {\r
-  SwitchStack (\r
-    (SWITCH_STACK_ENTRY_POINT) (UINTN) ApHltLoopCode,\r
-    NULL,\r
-    NULL,\r
-    (VOID *) (UINTN) TopOfStack\r
+  AsmDisablePaging64 (\r
+    GetProtectedModeCS (),\r
+    (UINT32) (UINTN) ApHltLoopCode,\r
+    0,\r
+    0,\r
+    TopOfStack\r
     );\r
   //\r
   // It should never reach here\r