]> git.proxmox.com Git - mirror_edk2.git/blobdiff - UefiCpuPkg/Universal/Acpi/S3Resume2Pei/S3Resume.c
UefiCpuPkg: Error Level is not used correctly
[mirror_edk2.git] / UefiCpuPkg / Universal / Acpi / S3Resume2Pei / S3Resume.c
index 07d83cc87529e4a550b341a89d93fa1891444e96..e53ed21bdc6a6d9e2b32bf409a8990fb5b65d2b7 100644 (file)
@@ -1,10 +1,11 @@
 /** @file\r
-  This module produces the EFI_PEI_S3_RESUME_PPI.\r
+  This module produces the EFI_PEI_S3_RESUME2_PPI.\r
   This module works with StandAloneBootScriptExecutor to S3 resume to OS.\r
-  This module will excute the boot script saved during last boot and after that,\r
+  This module will execute the boot script saved during last boot and after that,\r
   control is passed to OS waking up handler.\r
 \r
-  Copyright (c) 2006 - 2012, Intel Corporation. All rights reserved.<BR>\r
+  Copyright (c) 2006 - 2017, Intel Corporation. All rights reserved.<BR>\r
+  Copyright (c) 2017, AMD Incorporated. All rights reserved.<BR>\r
 \r
   This program and the accompanying materials\r
   are licensed and made available under the terms and conditions\r
 #include <Library/LockBoxLib.h>\r
 #include <IndustryStandard/Acpi.h>\r
 \r
+/**\r
+  This macro aligns the address of a variable with auto storage\r
+  duration down to CPU_STACK_ALIGNMENT.\r
+\r
+  Since the stack grows downward, the result preserves more of the\r
+  stack than the original address (or the same amount), not less.\r
+**/\r
+#define STACK_ALIGN_DOWN(Ptr) \\r
+          ((UINTN)(Ptr) & ~(UINTN)(CPU_STACK_ALIGNMENT - 1))\r
+\r
+#define PAGING_1G_ADDRESS_MASK_64  0x000FFFFFC0000000ull\r
+\r
 #pragma pack(1)\r
 typedef union {\r
   struct {\r
@@ -193,6 +206,18 @@ S3RestoreConfig2 (
   IN EFI_PEI_S3_RESUME2_PPI  *This\r
   );\r
 \r
+/**\r
+  Set data segment selectors value including DS/ES/FS/GS/SS.\r
+\r
+  @param[in]  SelectorValue      Segment selector value to be set.\r
+\r
+**/\r
+VOID\r
+EFIAPI\r
+AsmSetDataSelectors (\r
+  IN UINT16   SelectorValue\r
+  );\r
+\r
 //\r
 // Globals\r
 //\r
@@ -232,6 +257,8 @@ GLOBAL_REMOVE_IF_UNREFERENCED IA32_GDT mGdtEntries[] = {
 /* 0x40 */  {{0,      0,  0,  0,    0,  0,  0,  0,    0,  0, 0,  0,  0}},\r
 };\r
 \r
+#define DATA_SEGEMENT_SELECTOR        0x18\r
+\r
 //\r
 // IA32 Gdt register\r
 //\r
@@ -270,7 +297,7 @@ WriteToOsS3PerformanceData (
   UINT64                                        Freq;\r
 \r
   //\r
-  // Retrive time stamp count as early as possilbe\r
+  // Retrieve time stamp count as early as possible\r
   //\r
   Ticker = GetPerformanceCounter ();\r
 \r
@@ -342,7 +369,7 @@ WriteToOsS3PerformanceData (
       if ((AsciiStrnCmp (Token, "PEIM", PEI_PERFORMANCE_STRING_SIZE) == 0) && (Handle != NULL)) {\r
         AsciiSPrint (PerfData->Token, PERF_TOKEN_LENGTH, "0x%11p", Handle);\r
       } else {\r
-        AsciiStrnCpy (PerfData->Token, Token, PERF_TOKEN_LENGTH);\r
+        AsciiStrnCpyS (PerfData->Token, PERF_TOKEN_SIZE, Token, PERF_TOKEN_LENGTH);\r
       }\r
       if (StartTicker == 1) {\r
         StartTicker = StartValue;\r
@@ -427,6 +454,22 @@ S3ResumeBootOs (
   //\r
   AsmWriteIdtr (&PeiS3ResumeState->Idtr);\r
 \r
+  if (PeiS3ResumeState->ReturnStatus != EFI_SUCCESS) {\r
+    //\r
+    // Report Status code that boot script execution is failed\r
+    //\r
+    REPORT_STATUS_CODE (\r
+      EFI_ERROR_CODE | EFI_ERROR_MINOR,\r
+      (EFI_SOFTWARE_PEI_MODULE | EFI_SW_PEI_EC_S3_BOOT_SCRIPT_ERROR)\r
+      );\r
+  }\r
+\r
+  //\r
+  // NOTE: Because Debug Timer interrupt and system interrupts will be disabled \r
+  // in BootScriptExecuteDxe, the rest code in S3ResumeBootOs() cannot be halted\r
+  // by soft debugger.\r
+  //\r
+\r
   PERF_END (NULL, "ScriptExec", NULL, 0);\r
 \r
   //\r
@@ -443,6 +486,13 @@ S3ResumeBootOs (
   if ((Facs == NULL) ||\r
       (Facs->Signature != EFI_ACPI_4_0_FIRMWARE_ACPI_CONTROL_STRUCTURE_SIGNATURE) ||\r
       ((Facs->FirmwareWakingVector == 0) && (Facs->XFirmwareWakingVector == 0)) ) {\r
+    //\r
+    // Report Status code that no valid vector is found\r
+    //\r
+    REPORT_STATUS_CODE (\r
+      EFI_ERROR_CODE | EFI_ERROR_MAJOR,\r
+      (EFI_SOFTWARE_PEI_MODULE | EFI_SW_PEI_EC_S3_OS_WAKE_ERROR)\r
+      );\r
     CpuDeadLoop ();\r
     return ;\r
   }\r
@@ -468,13 +518,20 @@ S3ResumeBootOs (
     // Switch to native waking vector\r
     //\r
     TempStackTop = (UINTN)&TempStack + sizeof(TempStack);\r
+    DEBUG ((\r
+      DEBUG_INFO,\r
+      "%a() Stack Base: 0x%x, Stack Size: 0x%x\n",\r
+      __FUNCTION__,\r
+      TempStackTop,\r
+      sizeof (TempStack)\r
+      ));\r
     if ((Facs->Version == EFI_ACPI_4_0_FIRMWARE_ACPI_CONTROL_STRUCTURE_VERSION) &&\r
         ((Facs->Flags & EFI_ACPI_4_0_64BIT_WAKE_SUPPORTED_F) != 0) &&\r
         ((Facs->Flags & EFI_ACPI_4_0_OSPM_64BIT_WAKE__F) != 0)) {\r
       //\r
       // X64 long mode waking vector\r
       //\r
-      DEBUG (( EFI_D_ERROR, "Transfer to 64bit OS waking vector - %x\r\n", (UINTN)Facs->XFirmwareWakingVector));\r
+      DEBUG ((DEBUG_INFO, "Transfer to 64bit OS waking vector - %x\r\n", (UINTN)Facs->XFirmwareWakingVector));\r
       if (FeaturePcdGet (PcdDxeIplSwitchToLongMode)) {\r
         AsmEnablePaging64 (\r
           0x38,\r
@@ -484,14 +541,23 @@ S3ResumeBootOs (
           (UINT64)(UINTN)TempStackTop\r
           );\r
       } else {\r
+        //\r
+        // Report Status code that no valid waking vector is found\r
+        //\r
+        REPORT_STATUS_CODE (\r
+          EFI_ERROR_CODE | EFI_ERROR_MAJOR,\r
+          (EFI_SOFTWARE_PEI_MODULE | EFI_SW_PEI_EC_S3_OS_WAKE_ERROR)\r
+          );\r
         DEBUG (( EFI_D_ERROR, "Unsupported for 32bit DXE transfer to 64bit OS waking vector!\r\n"));\r
         ASSERT (FALSE);\r
+        CpuDeadLoop ();\r
+        return ;\r
       }\r
     } else {\r
       //\r
       // IA32 protected mode waking vector (Page disabled)\r
       //\r
-      DEBUG (( EFI_D_ERROR, "Transfer to 32bit OS waking vector - %x\r\n", (UINTN)Facs->XFirmwareWakingVector));\r
+      DEBUG ((DEBUG_INFO, "Transfer to 32bit OS waking vector - %x\r\n", (UINTN)Facs->XFirmwareWakingVector));\r
       SwitchStack (\r
         (SWITCH_STACK_ENTRY_POINT) (UINTN) Facs->XFirmwareWakingVector,\r
         NULL,\r
@@ -503,10 +569,18 @@ S3ResumeBootOs (
     //\r
     // 16bit Realmode waking vector\r
     //\r
-    DEBUG (( EFI_D_ERROR, "Transfer to 16bit OS waking vector - %x\r\n", (UINTN)Facs->FirmwareWakingVector));\r
+    DEBUG ((DEBUG_INFO, "Transfer to 16bit OS waking vector - %x\r\n", (UINTN)Facs->FirmwareWakingVector));\r
     AsmTransferControl (Facs->FirmwareWakingVector, 0x0);\r
   }\r
 \r
+  //\r
+  // Report Status code the failure of S3Resume\r
+  //\r
+  REPORT_STATUS_CODE (\r
+    EFI_ERROR_CODE | EFI_ERROR_MAJOR,\r
+    (EFI_SOFTWARE_PEI_MODULE | EFI_SW_PEI_EC_S3_OS_WAKE_ERROR)\r
+    );\r
+\r
   //\r
   // Never run to here\r
   //\r
@@ -543,17 +617,23 @@ RestoreS3PageTables (
     VOID                                          *Hob;\r
     BOOLEAN                                       Page1GSupport;\r
     PAGE_TABLE_1G_ENTRY                           *PageDirectory1GEntry;\r
+    UINT64                                        AddressEncMask;\r
+\r
+    //\r
+    // Make sure AddressEncMask is contained to smallest supported address field\r
+    //\r
+    AddressEncMask = PcdGet64 (PcdPteMemoryEncryptionAddressOrMask) & PAGING_1G_ADDRESS_MASK_64;\r
 \r
     //\r
     // NOTE: We have to ASSUME the page table generation format, because we do not know whole page table information.\r
     // The whole page table is too large to be saved in SMRAM.\r
     //\r
-    // The assumption is : whole page table is allocated in CONTINOUS memory and CR3 points to TOP page.\r
+    // The assumption is : whole page table is allocated in CONTINUOUS memory and CR3 points to TOP page.\r
     //\r
-    DEBUG ((EFI_D_ERROR, "S3NvsPageTableAddress - %x (%x)\n", (UINTN)S3NvsPageTableAddress, (UINTN)Build4GPageTableOnly));\r
+    DEBUG ((DEBUG_INFO, "S3NvsPageTableAddress - %x (%x)\n", (UINTN)S3NvsPageTableAddress, (UINTN)Build4GPageTableOnly));\r
 \r
     //\r
-    // By architecture only one PageMapLevel4 exists - so lets allocate storgage for it.\r
+    // By architecture only one PageMapLevel4 exists - so lets allocate storage for it.\r
     //\r
     PageMap = (PAGE_MAP_AND_DIRECTORY_POINTER *)S3NvsPageTableAddress;\r
     S3NvsPageTableAddress += SIZE_4KB;\r
@@ -625,7 +705,7 @@ RestoreS3PageTables (
       //\r
       // Make a PML4 Entry\r
       //\r
-      PageMapLevel4Entry->Uint64 = (UINT64)(UINTN)PageDirectoryPointerEntry;\r
+      PageMapLevel4Entry->Uint64 = (UINT64)(UINTN)PageDirectoryPointerEntry | AddressEncMask;\r
       PageMapLevel4Entry->Bits.ReadWrite = 1;\r
       PageMapLevel4Entry->Bits.Present = 1;\r
 \r
@@ -636,7 +716,7 @@ RestoreS3PageTables (
           //\r
           // Fill in the Page Directory entries\r
           //\r
-          PageDirectory1GEntry->Uint64 = (UINT64)PageAddress;\r
+          PageDirectory1GEntry->Uint64 = (UINT64)PageAddress | AddressEncMask;\r
           PageDirectory1GEntry->Bits.ReadWrite = 1;\r
           PageDirectory1GEntry->Bits.Present = 1;\r
           PageDirectory1GEntry->Bits.MustBe1 = 1;\r
@@ -653,7 +733,7 @@ RestoreS3PageTables (
           //\r
           // Fill in a Page Directory Pointer Entries\r
           //\r
-          PageDirectoryPointerEntry->Uint64 = (UINT64)(UINTN)PageDirectoryEntry;\r
+          PageDirectoryPointerEntry->Uint64 = (UINT64)(UINTN)PageDirectoryEntry | AddressEncMask;\r
           PageDirectoryPointerEntry->Bits.ReadWrite = 1;\r
           PageDirectoryPointerEntry->Bits.Present = 1;\r
     \r
@@ -661,7 +741,7 @@ RestoreS3PageTables (
             //\r
             // Fill in the Page Directory entries\r
             //\r
-            PageDirectoryEntry->Uint64 = (UINT64)PageAddress;\r
+            PageDirectoryEntry->Uint64 = (UINT64)PageAddress | AddressEncMask;\r
             PageDirectoryEntry->Bits.ReadWrite = 1;\r
             PageDirectoryEntry->Bits.Present = 1;\r
             PageDirectoryEntry->Bits.MustBe1 = 1;\r
@@ -701,8 +781,9 @@ S3ResumeExecuteBootScript (
   IA32_DESCRIPTOR            *IdtDescriptor;\r
   VOID                       *IdtBuffer;\r
   PEI_S3_RESUME_STATE        *PeiS3ResumeState;\r
+  BOOLEAN                    InterruptStatus;\r
 \r
-  DEBUG ((EFI_D_ERROR, "S3ResumeExecuteBootScript()\n"));\r
+  DEBUG ((DEBUG_INFO, "S3ResumeExecuteBootScript()\n"));\r
 \r
   //\r
   // Attempt to use content from SMRAM first\r
@@ -729,13 +810,13 @@ S3ResumeExecuteBootScript (
                               (VOID **) &SmmAccess\r
                               );\r
     if (!EFI_ERROR (Status)) {\r
-      DEBUG ((EFI_D_ERROR, "Close all SMRAM regions before executing boot script\n"));\r
+      DEBUG ((DEBUG_INFO, "Close all SMRAM regions before executing boot script\n"));\r
   \r
       for (Index = 0, Status = EFI_SUCCESS; !EFI_ERROR (Status); Index++) {\r
         Status = SmmAccess->Close ((EFI_PEI_SERVICES **)GetPeiServicesTablePointer (), SmmAccess, Index);\r
       }\r
 \r
-      DEBUG ((EFI_D_ERROR, "Lock all SMRAM regions before executing boot script\n"));\r
+      DEBUG ((DEBUG_INFO, "Lock all SMRAM regions before executing boot script\n"));\r
   \r
       for (Index = 0, Status = EFI_SUCCESS; !EFI_ERROR (Status); Index++) {\r
         Status = SmmAccess->Lock ((EFI_PEI_SERVICES **)GetPeiServicesTablePointer (), SmmAccess, Index);\r
@@ -754,10 +835,16 @@ S3ResumeExecuteBootScript (
     //\r
     IdtDescriptor = (IA32_DESCRIPTOR *) (UINTN) (AcpiS3Context->IdtrProfile);\r
     //\r
-    // Make sure the newly allcated IDT align with 16-bytes\r
+    // Make sure the newly allocated IDT align with 16-bytes\r
     // \r
     IdtBuffer = AllocatePages (EFI_SIZE_TO_PAGES((IdtDescriptor->Limit + 1) + 16));\r
-    ASSERT (IdtBuffer != NULL);\r
+    if (IdtBuffer == NULL) {\r
+      REPORT_STATUS_CODE (\r
+        EFI_ERROR_CODE | EFI_ERROR_MAJOR,\r
+        (EFI_SOFTWARE_PEI_MODULE | EFI_SW_PEI_EC_S3_RESUME_FAILED)\r
+        );\r
+      ASSERT (FALSE);\r
+    }\r
     //\r
     // Additional 16 bytes allocated to save IA32 IDT descriptor and Pei Service Table Pointer\r
     // IA32 IDT descriptor will be used to setup IA32 IDT table for 32-bit Framework Boot Script code\r
@@ -769,24 +856,44 @@ S3ResumeExecuteBootScript (
     *(UINTN*)(IdtDescriptor->Base - sizeof(UINTN)) = (UINTN)GetPeiServicesTablePointer ();\r
   }\r
 \r
+  InterruptStatus = SaveAndDisableInterrupts ();\r
   //\r
   // Need to make sure the GDT is loaded with values that support long mode and real mode.\r
   //\r
   AsmWriteGdtr (&mGdt);\r
+  //\r
+  // update segment selectors per the new GDT.\r
+  //\r
+  AsmSetDataSelectors (DATA_SEGEMENT_SELECTOR);\r
+  //\r
+  // Restore interrupt state.\r
+  //\r
+  SetInterruptState (InterruptStatus);\r
 \r
   //\r
   // Prepare data for return back\r
   //\r
   PeiS3ResumeState = AllocatePool (sizeof(*PeiS3ResumeState));\r
-  ASSERT (PeiS3ResumeState != NULL);\r
-  DEBUG (( EFI_D_ERROR, "PeiS3ResumeState - %x\r\n", PeiS3ResumeState));\r
+  if (PeiS3ResumeState == NULL) {\r
+    REPORT_STATUS_CODE (\r
+      EFI_ERROR_CODE | EFI_ERROR_MAJOR,\r
+      (EFI_SOFTWARE_PEI_MODULE | EFI_SW_PEI_EC_S3_RESUME_FAILED)\r
+      );\r
+    ASSERT (FALSE);\r
+  }\r
+  DEBUG ((DEBUG_INFO, "PeiS3ResumeState - %x\r\n", PeiS3ResumeState));\r
   PeiS3ResumeState->ReturnCs           = 0x10;\r
   PeiS3ResumeState->ReturnEntryPoint   = (EFI_PHYSICAL_ADDRESS)(UINTN)S3ResumeBootOs;\r
-  PeiS3ResumeState->ReturnStackPointer = (EFI_PHYSICAL_ADDRESS)(UINTN)&Status;\r
+  PeiS3ResumeState->ReturnStackPointer = (EFI_PHYSICAL_ADDRESS)STACK_ALIGN_DOWN (&Status);\r
   //\r
   // Save IDT\r
   //\r
   AsmReadIdtr (&PeiS3ResumeState->Idtr);\r
+  \r
+  //\r
+  // Report Status Code to indicate S3 boot script execution\r
+  //\r
+  REPORT_STATUS_CODE (EFI_PROGRESS_CODE, EFI_SOFTWARE_PEI_MODULE | EFI_SW_PEI_PC_S3_BOOT_SCRIPT);\r
 \r
   PERF_START (NULL, "ScriptExec", NULL, 0);\r
 \r
@@ -794,7 +901,7 @@ S3ResumeExecuteBootScript (
     //\r
     // X64 S3 Resume\r
     //\r
-    DEBUG (( EFI_D_ERROR, "Enable X64 and transfer control to Standalone Boot Script Executor\r\n"));\r
+    DEBUG ((DEBUG_INFO, "Enable X64 and transfer control to Standalone Boot Script Executor\r\n"));\r
 \r
     //\r
     // Switch to long mode to complete resume.\r
@@ -810,7 +917,7 @@ S3ResumeExecuteBootScript (
     //\r
     // IA32 S3 Resume\r
     //\r
-    DEBUG (( EFI_D_ERROR, "transfer control to Standalone Boot Script Executor\r\n"));\r
+    DEBUG ((DEBUG_INFO, "transfer control to Standalone Boot Script Executor\r\n"));\r
     SwitchStack (\r
       (SWITCH_STACK_ENTRY_POINT) (UINTN) EfiBootScriptExecutorVariable->BootScriptExecutorEntrypoint,\r
       (VOID *)AcpiS3Context,\r
@@ -873,11 +980,12 @@ S3RestoreConfig2 (
   SMM_S3_RESUME_STATE                           *SmmS3ResumeState;\r
   VOID                                          *GuidHob;\r
   BOOLEAN                                       Build4GPageTableOnly;\r
+  BOOLEAN                                       InterruptStatus;\r
 \r
   TempAcpiS3Context = 0;\r
   TempEfiBootScriptExecutorVariable = 0;\r
 \r
-  DEBUG ((EFI_D_ERROR, "Enter S3 PEIM\r\n"));\r
+  DEBUG ((DEBUG_INFO, "Enter S3 PEIM\r\n"));\r
 \r
   VarSize = sizeof (EFI_PHYSICAL_ADDRESS);\r
   Status = RestoreLockBox (\r
@@ -915,12 +1023,15 @@ S3RestoreConfig2 (
   EfiBootScriptExecutorVariable = (BOOT_SCRIPT_EXECUTOR_VARIABLE *) (UINTN) TempEfiBootScriptExecutorVariable;\r
   ASSERT (EfiBootScriptExecutorVariable != NULL);\r
 \r
-  DEBUG (( EFI_D_ERROR, "AcpiS3Context = %x\n", AcpiS3Context));\r
-  DEBUG (( EFI_D_ERROR, "Waking Vector = %x\n", ((EFI_ACPI_2_0_FIRMWARE_ACPI_CONTROL_STRUCTURE *) ((UINTN) (AcpiS3Context->AcpiFacsTable)))->FirmwareWakingVector));\r
-  DEBUG (( EFI_D_ERROR, "AcpiS3Context->AcpiFacsTable = %x\n", AcpiS3Context->AcpiFacsTable));\r
-  DEBUG (( EFI_D_ERROR, "AcpiS3Context->S3NvsPageTableAddress = %x\n", AcpiS3Context->S3NvsPageTableAddress));\r
-  DEBUG (( EFI_D_ERROR, "AcpiS3Context->S3DebugBufferAddress = %x\n", AcpiS3Context->S3DebugBufferAddress));\r
-  DEBUG (( EFI_D_ERROR, "EfiBootScriptExecutorVariable->BootScriptExecutorEntrypoint = %x\n", EfiBootScriptExecutorVariable->BootScriptExecutorEntrypoint));\r
+  DEBUG (( DEBUG_INFO, "AcpiS3Context = %x\n", AcpiS3Context));\r
+  DEBUG (( DEBUG_INFO, "Waking Vector = %x\n", ((EFI_ACPI_2_0_FIRMWARE_ACPI_CONTROL_STRUCTURE *) ((UINTN) (AcpiS3Context->AcpiFacsTable)))->FirmwareWakingVector));\r
+  DEBUG (( DEBUG_INFO, "AcpiS3Context->AcpiFacsTable = %x\n", AcpiS3Context->AcpiFacsTable));\r
+  DEBUG (( DEBUG_INFO, "AcpiS3Context->IdtrProfile = %x\n", AcpiS3Context->IdtrProfile));  \r
+  DEBUG (( DEBUG_INFO, "AcpiS3Context->S3NvsPageTableAddress = %x\n", AcpiS3Context->S3NvsPageTableAddress));\r
+  DEBUG (( DEBUG_INFO, "AcpiS3Context->S3DebugBufferAddress = %x\n", AcpiS3Context->S3DebugBufferAddress));\r
+  DEBUG (( DEBUG_INFO, "AcpiS3Context->BootScriptStackBase = %x\n", AcpiS3Context->BootScriptStackBase));\r
+  DEBUG (( DEBUG_INFO, "AcpiS3Context->BootScriptStackSize = %x\n", AcpiS3Context->BootScriptStackSize));\r
+  DEBUG (( DEBUG_INFO, "EfiBootScriptExecutorVariable->BootScriptExecutorEntrypoint = %x\n", EfiBootScriptExecutorVariable->BootScriptExecutorEntrypoint));\r
 \r
   //\r
   // Additional step for BootScript integrity - we only handle BootScript and BootScriptExecutor.\r
@@ -968,26 +1079,21 @@ S3RestoreConfig2 (
     SmmS3ResumeState->ReturnEntryPoint   = (EFI_PHYSICAL_ADDRESS)(UINTN)S3ResumeExecuteBootScript;\r
     SmmS3ResumeState->ReturnContext1     = (EFI_PHYSICAL_ADDRESS)(UINTN)AcpiS3Context;\r
     SmmS3ResumeState->ReturnContext2     = (EFI_PHYSICAL_ADDRESS)(UINTN)EfiBootScriptExecutorVariable;\r
-    SmmS3ResumeState->ReturnStackPointer = (EFI_PHYSICAL_ADDRESS)(UINTN)&Status;\r
-\r
-    DEBUG (( EFI_D_ERROR, "SMM S3 Signature                = %x\n", SmmS3ResumeState->Signature));\r
-    DEBUG (( EFI_D_ERROR, "SMM S3 Stack Base               = %x\n", SmmS3ResumeState->SmmS3StackBase));\r
-    DEBUG (( EFI_D_ERROR, "SMM S3 Stack Size               = %x\n", SmmS3ResumeState->SmmS3StackSize));\r
-    DEBUG (( EFI_D_ERROR, "SMM S3 Resume Entry Point       = %x\n", SmmS3ResumeState->SmmS3ResumeEntryPoint));\r
-    DEBUG (( EFI_D_ERROR, "SMM S3 CR0                      = %x\n", SmmS3ResumeState->SmmS3Cr0));\r
-    DEBUG (( EFI_D_ERROR, "SMM S3 CR3                      = %x\n", SmmS3ResumeState->SmmS3Cr3));\r
-    DEBUG (( EFI_D_ERROR, "SMM S3 CR4                      = %x\n", SmmS3ResumeState->SmmS3Cr4));\r
-    DEBUG (( EFI_D_ERROR, "SMM S3 Return CS                = %x\n", SmmS3ResumeState->ReturnCs));\r
-    DEBUG (( EFI_D_ERROR, "SMM S3 Return Entry Point       = %x\n", SmmS3ResumeState->ReturnEntryPoint));\r
-    DEBUG (( EFI_D_ERROR, "SMM S3 Return Context1          = %x\n", SmmS3ResumeState->ReturnContext1));\r
-    DEBUG (( EFI_D_ERROR, "SMM S3 Return Context2          = %x\n", SmmS3ResumeState->ReturnContext2));\r
-    DEBUG (( EFI_D_ERROR, "SMM S3 Return Stack Pointer     = %x\n", SmmS3ResumeState->ReturnStackPointer));\r
-    DEBUG (( EFI_D_ERROR, "SMM S3 Smst                     = %x\n", SmmS3ResumeState->Smst));\r
-\r
-    //\r
-    // Disable interrupt of Debug timer.\r
-    //\r
-    SaveAndSetDebugTimerInterrupt (FALSE);\r
+    SmmS3ResumeState->ReturnStackPointer = (EFI_PHYSICAL_ADDRESS)STACK_ALIGN_DOWN (&Status);\r
+\r
+    DEBUG (( DEBUG_INFO, "SMM S3 Signature                = %x\n", SmmS3ResumeState->Signature));\r
+    DEBUG (( DEBUG_INFO, "SMM S3 Stack Base               = %x\n", SmmS3ResumeState->SmmS3StackBase));\r
+    DEBUG (( DEBUG_INFO, "SMM S3 Stack Size               = %x\n", SmmS3ResumeState->SmmS3StackSize));\r
+    DEBUG (( DEBUG_INFO, "SMM S3 Resume Entry Point       = %x\n", SmmS3ResumeState->SmmS3ResumeEntryPoint));\r
+    DEBUG (( DEBUG_INFO, "SMM S3 CR0                      = %x\n", SmmS3ResumeState->SmmS3Cr0));\r
+    DEBUG (( DEBUG_INFO, "SMM S3 CR3                      = %x\n", SmmS3ResumeState->SmmS3Cr3));\r
+    DEBUG (( DEBUG_INFO, "SMM S3 CR4                      = %x\n", SmmS3ResumeState->SmmS3Cr4));\r
+    DEBUG (( DEBUG_INFO, "SMM S3 Return CS                = %x\n", SmmS3ResumeState->ReturnCs));\r
+    DEBUG (( DEBUG_INFO, "SMM S3 Return Entry Point       = %x\n", SmmS3ResumeState->ReturnEntryPoint));\r
+    DEBUG (( DEBUG_INFO, "SMM S3 Return Context1          = %x\n", SmmS3ResumeState->ReturnContext1));\r
+    DEBUG (( DEBUG_INFO, "SMM S3 Return Context2          = %x\n", SmmS3ResumeState->ReturnContext2));\r
+    DEBUG (( DEBUG_INFO, "SMM S3 Return Stack Pointer     = %x\n", SmmS3ResumeState->ReturnStackPointer));\r
+    DEBUG (( DEBUG_INFO, "SMM S3 Smst                     = %x\n", SmmS3ResumeState->Smst));\r
 \r
     if (SmmS3ResumeState->Signature == SMM_S3_RESUME_SMM_32) {\r
       SwitchStack (\r
@@ -1002,11 +1108,29 @@ S3RestoreConfig2 (
       // Switch to long mode to complete resume.\r
       //\r
 \r
+      InterruptStatus = SaveAndDisableInterrupts ();\r
       //\r
       // Need to make sure the GDT is loaded with values that support long mode and real mode.\r
       //\r
       AsmWriteGdtr (&mGdt);\r
+      //\r
+      // update segment selectors per the new GDT.\r
+      //      \r
+      AsmSetDataSelectors (DATA_SEGEMENT_SELECTOR);\r
+      //\r
+      // Restore interrupt state.\r
+      //\r
+      SetInterruptState (InterruptStatus);\r
+\r
       AsmWriteCr3 ((UINTN)SmmS3ResumeState->SmmS3Cr3);\r
+\r
+      //\r
+      // Disable interrupt of Debug timer, since IDT table cannot work in long mode.\r
+      // NOTE: On x64 platforms, because DisablePaging64() will disable interrupts,\r
+      // the code in S3ResumeExecuteBootScript() cannot be halted by soft debugger.\r
+      //\r
+      SaveAndSetDebugTimerInterrupt (FALSE);\r
+\r
       AsmEnablePaging64 (\r
         0x38,\r
         SmmS3ResumeState->SmmS3ResumeEntryPoint,\r