]> git.proxmox.com Git - mirror_edk2.git/blobdiff - UefiCpuPkg/Universal/Acpi/S3Resume2Pei/S3Resume.c
OvmfPkg: get boot order from QEMU
[mirror_edk2.git] / UefiCpuPkg / Universal / Acpi / S3Resume2Pei / S3Resume.c
index 3242876ea549486bb7c9d4fa902075dc5f4757db..ad81c19590f34a92e12c5bce96ed8dbc2d463927 100644 (file)
@@ -4,7 +4,7 @@
   This module will excute the boot script saved during last boot and after that,\r
   control is passed to OS waking up handler.\r
 \r
-  Copyright (c) 2006 - 2011, Intel Corporation. All rights reserved.<BR>\r
+  Copyright (c) 2006 - 2012, Intel Corporation. All rights reserved.<BR>\r
 \r
   This program and the accompanying materials\r
   are licensed and made available under the terms and conditions\r
@@ -44,6 +44,7 @@
 #include <Library/LocalApicLib.h>\r
 #include <Library/ReportStatusCodeLib.h>\r
 #include <Library/PrintLib.h>\r
+#include <Library/HobLib.h>\r
 #include <Library/LockBoxLib.h>\r
 #include <IndustryStandard/Acpi.h>\r
 \r
@@ -113,6 +114,30 @@ typedef union {
   UINT64    Uint64;\r
 } PAGE_TABLE_ENTRY;\r
 \r
+//\r
+// Page Table Entry 1GB\r
+//\r
+typedef union {\r
+  struct {\r
+    UINT64  Present:1;                // 0 = Not present in memory, 1 = Present in memory\r
+    UINT64  ReadWrite:1;              // 0 = Read-Only, 1= Read/Write\r
+    UINT64  UserSupervisor:1;         // 0 = Supervisor, 1=User\r
+    UINT64  WriteThrough:1;           // 0 = Write-Back caching, 1=Write-Through caching\r
+    UINT64  CacheDisabled:1;          // 0 = Cached, 1=Non-Cached\r
+    UINT64  Accessed:1;               // 0 = Not accessed, 1 = Accessed (set by CPU)\r
+    UINT64  Dirty:1;                  // 0 = Not Dirty, 1 = written by processor on access to page\r
+    UINT64  MustBe1:1;                // Must be 1 \r
+    UINT64  Global:1;                 // 0 = Not global page, 1 = global page TLB not cleared on CR3 write\r
+    UINT64  Available:3;              // Available for use by system software\r
+    UINT64  PAT:1;                    //\r
+    UINT64  MustBeZero:17;            // Must be zero;\r
+    UINT64  PageTableBaseAddress:22;  // Page Table Base Address\r
+    UINT64  AvabilableHigh:11;        // Available for use by system software\r
+    UINT64  Nx:1;                     // 0 = Execute Code, 1 = No Code Execution\r
+  } Bits;\r
+  UINT64    Uint64;\r
+} PAGE_TABLE_1G_ENTRY;\r
+\r
 #pragma pack()\r
 \r
 //\r
@@ -168,6 +193,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
@@ -207,6 +244,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
@@ -259,7 +298,9 @@ WriteToOsS3PerformanceData (
              NULL,\r
              (VOID **) &VariableServices\r
              );\r
-  ASSERT_EFI_ERROR (Status);\r
+  if (EFI_ERROR (Status)) {\r
+    return;\r
+  }\r
 \r
   VarSize   = sizeof (EFI_PHYSICAL_ADDRESS);\r
   Status = VariableServices->GetVariable (\r
@@ -340,6 +381,41 @@ WriteToOsS3PerformanceData (
   PerfHeader->S3EntryNum = (UINT32) Index;\r
 }\r
 \r
+/**\r
+  The function will check if current waking vector is long mode.\r
+\r
+  @param  AcpiS3Context                 a pointer to a structure of ACPI_S3_CONTEXT\r
+\r
+  @retval TRUE   Current context need long mode waking vector.\r
+  @retval FALSE  Current context need not long mode waking vector.\r
+**/\r
+BOOLEAN\r
+IsLongModeWakingVector (\r
+  IN ACPI_S3_CONTEXT                *AcpiS3Context\r
+  )\r
+{\r
+  EFI_ACPI_4_0_FIRMWARE_ACPI_CONTROL_STRUCTURE  *Facs;\r
+\r
+  Facs = (EFI_ACPI_4_0_FIRMWARE_ACPI_CONTROL_STRUCTURE *) ((UINTN) (AcpiS3Context->AcpiFacsTable));\r
+  if ((Facs == NULL) ||\r
+      (Facs->Signature != EFI_ACPI_4_0_FIRMWARE_ACPI_CONTROL_STRUCTURE_SIGNATURE) ||\r
+      ((Facs->FirmwareWakingVector == 0) && (Facs->XFirmwareWakingVector == 0)) ) {\r
+    // Something wrong with FACS\r
+    return FALSE;\r
+  }\r
+  if (Facs->XFirmwareWakingVector != 0) {\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
+      // Both BIOS and OS wants 64bit vector\r
+      if (FeaturePcdGet (PcdDxeIplSwitchToLongMode)) {\r
+        return TRUE;\r
+      }\r
+    }\r
+  }\r
+  return FALSE;\r
+}\r
+\r
 /**\r
   Jump to OS waking vector.\r
   The function will install boot script done PPI, report S3 resume status code, and then jump to OS waking vector.\r
@@ -365,6 +441,8 @@ S3ResumeBootOs (
   //\r
   AsmWriteIdtr (&PeiS3ResumeState->Idtr);\r
 \r
+  PERF_END (NULL, "ScriptExec", NULL, 0);\r
+\r
   //\r
   // Install BootScriptDonePpi\r
   //\r
@@ -383,17 +461,17 @@ S3ResumeBootOs (
     return ;\r
   }\r
 \r
-  //\r
-  // report status code on S3 resume\r
-  //\r
-  REPORT_STATUS_CODE (EFI_PROGRESS_CODE, EFI_SOFTWARE_PEI_MODULE | EFI_SW_PEI_PC_OS_WAKE);\r
-\r
   //\r
   // Install EndOfPeiPpi\r
   //\r
   Status = PeiServicesInstallPpi (&mPpiListEndOfPeiTable);\r
   ASSERT_EFI_ERROR (Status);\r
 \r
+  //\r
+  // report status code on S3 resume\r
+  //\r
+  REPORT_STATUS_CODE (EFI_PROGRESS_CODE, EFI_SOFTWARE_PEI_MODULE | EFI_SW_PEI_PC_OS_WAKE);\r
+\r
   PERF_CODE (\r
     WriteToOsS3PerformanceData ();\r
     );\r
@@ -454,25 +532,31 @@ S3ResumeBootOs (
   If BootScriptExector driver will not run in 64-bit mode, this function will do nothing. \r
 \r
   @param S3NvsPageTableAddress   PageTableAddress in ACPINvs\r
+  @param Build4GPageTableOnly    If BIOS just build 4G page table only\r
 **/\r
 VOID\r
 RestoreS3PageTables (\r
-  IN UINTN                                         S3NvsPageTableAddress\r
+  IN UINTN                                         S3NvsPageTableAddress,\r
+  IN BOOLEAN                                       Build4GPageTableOnly\r
   )\r
 {\r
   if (FeaturePcdGet (PcdDxeIplSwitchToLongMode)) {\r
     UINT32                                        RegEax;\r
+    UINT32                                        RegEdx;\r
     UINT8                                         PhysicalAddressBits;\r
     EFI_PHYSICAL_ADDRESS                          PageAddress;\r
     UINTN                                         IndexOfPml4Entries;\r
     UINTN                                         IndexOfPdpEntries;\r
     UINTN                                         IndexOfPageDirectoryEntries;\r
-    UINT64                                        NumberOfPml4EntriesNeeded;\r
-    UINT64                                        NumberOfPdpEntriesNeeded;\r
+    UINT32                                        NumberOfPml4EntriesNeeded;\r
+    UINT32                                        NumberOfPdpEntriesNeeded;\r
     PAGE_MAP_AND_DIRECTORY_POINTER                *PageMapLevel4Entry;\r
     PAGE_MAP_AND_DIRECTORY_POINTER                *PageMap;\r
     PAGE_MAP_AND_DIRECTORY_POINTER                *PageDirectoryPointerEntry;\r
     PAGE_TABLE_ENTRY                              *PageDirectoryEntry;\r
+    VOID                                          *Hob;\r
+    BOOLEAN                                       Page1GSupport;\r
+    PAGE_TABLE_1G_ENTRY                           *PageDirectory1GEntry;\r
 \r
     //\r
     // NOTE: We have to ASSUME the page table generation format, because we do not know whole page table information.\r
@@ -480,7 +564,7 @@ RestoreS3PageTables (
     //\r
     // The assumption is : whole page table is allocated in CONTINOUS memory and CR3 points to TOP page.\r
     //\r
-    DEBUG ((EFI_D_ERROR, "S3NvsPageTableAddress - %x\n", S3NvsPageTableAddress));\r
+    DEBUG ((EFI_D_ERROR, "S3NvsPageTableAddress - %x (%x)\n", (UINTN)S3NvsPageTableAddress, (UINTN)Build4GPageTableOnly));\r
 \r
     //\r
     // By architecture only one PageMapLevel4 exists - so lets allocate storgage for it.\r
@@ -488,25 +572,57 @@ RestoreS3PageTables (
     PageMap = (PAGE_MAP_AND_DIRECTORY_POINTER *)S3NvsPageTableAddress;\r
     S3NvsPageTableAddress += SIZE_4KB;\r
     \r
+    Page1GSupport = FALSE;\r
+    if (PcdGetBool(PcdUse1GPageTable)) {\r
+      AsmCpuid (0x80000000, &RegEax, NULL, NULL, NULL);\r
+      if (RegEax >= 0x80000001) {\r
+        AsmCpuid (0x80000001, NULL, NULL, NULL, &RegEdx);\r
+        if ((RegEdx & BIT26) != 0) {\r
+          Page1GSupport = TRUE;\r
+        }\r
+      }\r
+    }\r
+    \r
     //\r
     // Get physical address bits supported.\r
     //\r
-    AsmCpuid (0x80000000, &RegEax, NULL, NULL, NULL);\r
-    if (RegEax >= 0x80000008) {\r
-      AsmCpuid (0x80000008, &RegEax, NULL, NULL, NULL);\r
-      PhysicalAddressBits = (UINT8) RegEax;\r
+    Hob = GetFirstHob (EFI_HOB_TYPE_CPU);\r
+    if (Hob != NULL) {\r
+      PhysicalAddressBits = ((EFI_HOB_CPU *) Hob)->SizeOfMemorySpace;\r
     } else {\r
-      PhysicalAddressBits = 36;\r
+      AsmCpuid (0x80000000, &RegEax, NULL, NULL, NULL);\r
+      if (RegEax >= 0x80000008) {\r
+        AsmCpuid (0x80000008, &RegEax, NULL, NULL, NULL);\r
+        PhysicalAddressBits = (UINT8) RegEax;\r
+      } else {\r
+        PhysicalAddressBits = 36;\r
+      }\r
     }\r
     \r
+    //\r
+    // IA-32e paging translates 48-bit linear addresses to 52-bit physical addresses.\r
+    //\r
+    ASSERT (PhysicalAddressBits <= 52);\r
+    if (PhysicalAddressBits > 48) {\r
+      PhysicalAddressBits = 48;\r
+    }\r
+\r
+    //\r
+    // NOTE: In order to save time to create full page table, we just create 4G page table by default.\r
+    // And let PF handler in BootScript driver to create more on request.\r
+    //\r
+    if (Build4GPageTableOnly) {\r
+      PhysicalAddressBits = 32;\r
+      ZeroMem (PageMap, EFI_PAGES_TO_SIZE(2));\r
+    }\r
     //\r
     // Calculate the table entries needed.\r
     //\r
     if (PhysicalAddressBits <= 39) {\r
       NumberOfPml4EntriesNeeded = 1;\r
-      NumberOfPdpEntriesNeeded = LShiftU64 (1, (PhysicalAddressBits - 30));\r
+      NumberOfPdpEntriesNeeded = (UINT32)LShiftU64 (1, (PhysicalAddressBits - 30));\r
     } else {\r
-      NumberOfPml4EntriesNeeded = LShiftU64 (1, (PhysicalAddressBits - 39));\r
+      NumberOfPml4EntriesNeeded = (UINT32)LShiftU64 (1, (PhysicalAddressBits - 39));\r
       NumberOfPdpEntriesNeeded = 512;\r
     }\r
     \r
@@ -526,30 +642,44 @@ RestoreS3PageTables (
       PageMapLevel4Entry->Uint64 = (UINT64)(UINTN)PageDirectoryPointerEntry;\r
       PageMapLevel4Entry->Bits.ReadWrite = 1;\r
       PageMapLevel4Entry->Bits.Present = 1;\r
+\r
+      if (Page1GSupport) {\r
+        PageDirectory1GEntry = (VOID *) PageDirectoryPointerEntry;\r
     \r
-      for (IndexOfPdpEntries = 0; IndexOfPdpEntries < NumberOfPdpEntriesNeeded; IndexOfPdpEntries++, PageDirectoryPointerEntry++) {\r
-        //\r
-        // Each Directory Pointer entries points to a page of Page Directory entires.\r
-        // So allocate space for them and fill them in in the IndexOfPageDirectoryEntries loop.\r
-        //       \r
-        PageDirectoryEntry = (PAGE_TABLE_ENTRY *)S3NvsPageTableAddress;\r
-        S3NvsPageTableAddress += SIZE_4KB;\r
-    \r
-        //\r
-        // Fill in a Page Directory Pointer Entries\r
-        //\r
-        PageDirectoryPointerEntry->Uint64 = (UINT64)(UINTN)PageDirectoryEntry;\r
-        PageDirectoryPointerEntry->Bits.ReadWrite = 1;\r
-        PageDirectoryPointerEntry->Bits.Present = 1;\r
-    \r
-        for (IndexOfPageDirectoryEntries = 0; IndexOfPageDirectoryEntries < 512; IndexOfPageDirectoryEntries++, PageDirectoryEntry++, PageAddress += SIZE_2MB) {\r
+        for (IndexOfPageDirectoryEntries = 0; IndexOfPageDirectoryEntries < 512; IndexOfPageDirectoryEntries++, PageDirectory1GEntry++, PageAddress += SIZE_1GB) {\r
           //\r
           // Fill in the Page Directory entries\r
           //\r
-          PageDirectoryEntry->Uint64 = (UINT64)PageAddress;\r
-          PageDirectoryEntry->Bits.ReadWrite = 1;\r
-          PageDirectoryEntry->Bits.Present = 1;\r
-          PageDirectoryEntry->Bits.MustBe1 = 1;\r
+          PageDirectory1GEntry->Uint64 = (UINT64)PageAddress;\r
+          PageDirectory1GEntry->Bits.ReadWrite = 1;\r
+          PageDirectory1GEntry->Bits.Present = 1;\r
+          PageDirectory1GEntry->Bits.MustBe1 = 1;\r
+        }\r
+      } else {\r
+        for (IndexOfPdpEntries = 0; IndexOfPdpEntries < NumberOfPdpEntriesNeeded; IndexOfPdpEntries++, PageDirectoryPointerEntry++) {\r
+          //\r
+          // Each Directory Pointer entries points to a page of Page Directory entires.\r
+          // So allocate space for them and fill them in in the IndexOfPageDirectoryEntries loop.\r
+          //       \r
+          PageDirectoryEntry = (PAGE_TABLE_ENTRY *)S3NvsPageTableAddress;\r
+          S3NvsPageTableAddress += SIZE_4KB;\r
+    \r
+          //\r
+          // Fill in a Page Directory Pointer Entries\r
+          //\r
+          PageDirectoryPointerEntry->Uint64 = (UINT64)(UINTN)PageDirectoryEntry;\r
+          PageDirectoryPointerEntry->Bits.ReadWrite = 1;\r
+          PageDirectoryPointerEntry->Bits.Present = 1;\r
+    \r
+          for (IndexOfPageDirectoryEntries = 0; IndexOfPageDirectoryEntries < 512; IndexOfPageDirectoryEntries++, PageDirectoryEntry++, PageAddress += SIZE_2MB) {\r
+            //\r
+            // Fill in the Page Directory entries\r
+            //\r
+            PageDirectoryEntry->Uint64 = (UINT64)PageAddress;\r
+            PageDirectoryEntry->Bits.ReadWrite = 1;\r
+            PageDirectoryEntry->Bits.Present = 1;\r
+            PageDirectoryEntry->Bits.MustBe1 = 1;\r
+          }\r
         }\r
       }\r
     }\r
@@ -585,6 +715,7 @@ 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
 \r
@@ -612,25 +743,22 @@ S3ResumeExecuteBootScript (
                               NULL,\r
                               (VOID **) &SmmAccess\r
                               );\r
+    if (!EFI_ERROR (Status)) {\r
+      DEBUG ((EFI_D_ERROR, "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, "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
-\r
-    for (Index = 0, Status = EFI_SUCCESS; !EFI_ERROR (Status); Index++) {\r
-      Status = SmmAccess->Lock ((EFI_PEI_SERVICES **)GetPeiServicesTablePointer (), SmmAccess, Index);\r
+      DEBUG ((EFI_D_ERROR, "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
+      }\r
     }\r
   }\r
 \r
   if (FeaturePcdGet (PcdDxeIplSwitchToLongMode)) {\r
-    //\r
-    // Need reconstruct page table here, since we do not trust ACPINvs.\r
-    //\r
-    RestoreS3PageTables ((UINTN)AcpiS3Context->S3NvsPageTableAddress);\r
     AsmWriteCr3 ((UINTN)AcpiS3Context->S3NvsPageTableAddress);\r
   }\r
 \r
@@ -645,15 +773,30 @@ S3ResumeExecuteBootScript (
     // \r
     IdtBuffer = AllocatePages (EFI_SIZE_TO_PAGES((IdtDescriptor->Limit + 1) + 16));\r
     ASSERT (IdtBuffer != NULL);\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
+    // \r
+    ZeroMem (IdtBuffer, 16);\r
+    AsmReadIdtr ((IA32_DESCRIPTOR *)IdtBuffer);\r
     CopyMem ((VOID*)((UINT8*)IdtBuffer + 16),(VOID*)(IdtDescriptor->Base), (IdtDescriptor->Limit + 1));\r
     IdtDescriptor->Base = (UINTN)((UINT8*)IdtBuffer + 16);\r
     *(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
@@ -669,6 +812,8 @@ S3ResumeExecuteBootScript (
   //\r
   AsmReadIdtr (&PeiS3ResumeState->Idtr);\r
 \r
+  PERF_START (NULL, "ScriptExec", NULL, 0);\r
+\r
   if (FeaturePcdGet (PcdDxeIplSwitchToLongMode)) {\r
     //\r
     // X64 S3 Resume\r
@@ -744,7 +889,6 @@ S3RestoreConfig2 (
   PEI_SMM_ACCESS_PPI                            *SmmAccess;\r
   UINTN                                         Index;\r
   ACPI_S3_CONTEXT                               *AcpiS3Context;\r
-  EFI_PEI_READ_ONLY_VARIABLE2_PPI               *VariableServices;\r
   EFI_PHYSICAL_ADDRESS                          TempEfiBootScriptExecutorVariable;\r
   EFI_PHYSICAL_ADDRESS                          TempAcpiS3Context;\r
   BOOT_SCRIPT_EXECUTOR_VARIABLE                 *EfiBootScriptExecutorVariable;\r
@@ -752,28 +896,13 @@ S3RestoreConfig2 (
   EFI_SMRAM_DESCRIPTOR                          *SmramDescriptor;\r
   SMM_S3_RESUME_STATE                           *SmmS3ResumeState;\r
   VOID                                          *GuidHob;\r
+  BOOLEAN                                       Build4GPageTableOnly;\r
+  BOOLEAN                                       InterruptStatus;\r
 \r
-  DEBUG ((EFI_D_ERROR, "Enter S3 PEIM\r\n"));\r
-\r
-  Status = PeiServicesLocatePpi (\r
-                            &gPeiSmmAccessPpiGuid,\r
-                            0,\r
-                            NULL,\r
-                            (VOID **) &SmmAccess\r
-                            );\r
-  for (Index = 0; !EFI_ERROR (Status); Index++) {\r
-    Status = SmmAccess->Open ((EFI_PEI_SERVICES **)GetPeiServicesTablePointer (), SmmAccess, Index);\r
-  }\r
+  TempAcpiS3Context = 0;\r
+  TempEfiBootScriptExecutorVariable = 0;\r
 \r
-  Status = PeiServicesLocatePpi (\r
-                            &gEfiPeiReadOnlyVariable2PpiGuid,\r
-                            0,\r
-                            NULL,\r
-                            (VOID **) &VariableServices\r
-                            );\r
-  if (EFI_ERROR (Status)) {\r
-    return Status;\r
-  }\r
+  DEBUG ((EFI_D_ERROR, "Enter S3 PEIM\r\n"));\r
 \r
   VarSize = sizeof (EFI_PHYSICAL_ADDRESS);\r
   Status = RestoreLockBox (\r
@@ -783,9 +912,6 @@ S3RestoreConfig2 (
              );\r
   ASSERT_EFI_ERROR (Status);\r
 \r
-  AcpiS3Context = (ACPI_S3_CONTEXT *)(UINTN)TempAcpiS3Context;\r
-  ASSERT (AcpiS3Context != NULL);\r
-\r
   Status = RestoreLockBox (\r
              &gEfiAcpiS3ContextGuid,\r
              NULL,\r
@@ -793,7 +919,10 @@ S3RestoreConfig2 (
              );\r
   ASSERT_EFI_ERROR (Status);\r
 \r
-  VarSize   = sizeof (TempEfiBootScriptExecutorVariable);\r
+  AcpiS3Context = (ACPI_S3_CONTEXT *)(UINTN)TempAcpiS3Context;\r
+  ASSERT (AcpiS3Context != NULL);\r
+\r
+  VarSize   = sizeof (EFI_PHYSICAL_ADDRESS);\r
   Status = RestoreLockBox (\r
              &gEfiBootScriptExecutorVariableGuid,\r
              &TempEfiBootScriptExecutorVariable,\r
@@ -809,6 +938,7 @@ S3RestoreConfig2 (
   ASSERT_EFI_ERROR (Status);\r
 \r
   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
@@ -829,11 +959,33 @@ S3RestoreConfig2 (
     CpuDeadLoop ();\r
   }\r
 \r
+  if (FeaturePcdGet (PcdDxeIplSwitchToLongMode)) {\r
+    //\r
+    // Need reconstruct page table here, since we do not trust ACPINvs.\r
+    //\r
+    if (IsLongModeWakingVector (AcpiS3Context)) {\r
+      Build4GPageTableOnly = FALSE;\r
+    } else {\r
+      Build4GPageTableOnly = TRUE;\r
+    }\r
+    RestoreS3PageTables ((UINTN)AcpiS3Context->S3NvsPageTableAddress, Build4GPageTableOnly);\r
+  }\r
+\r
   //\r
   // Attempt to use content from SMRAM first\r
   //\r
   GuidHob = GetFirstGuidHob (&gEfiAcpiVariableGuid);\r
   if (GuidHob != NULL) {\r
+    Status = PeiServicesLocatePpi (\r
+                              &gPeiSmmAccessPpiGuid,\r
+                              0,\r
+                              NULL,\r
+                              (VOID **) &SmmAccess\r
+                              );\r
+    for (Index = 0; !EFI_ERROR (Status); Index++) {\r
+      Status = SmmAccess->Open ((EFI_PEI_SERVICES **)GetPeiServicesTablePointer (), SmmAccess, Index);\r
+    }\r
+\r
     SmramDescriptor = (EFI_SMRAM_DESCRIPTOR *) GET_GUID_HOB_DATA (GuidHob);\r
     SmmS3ResumeState = (SMM_S3_RESUME_STATE *)(UINTN)SmramDescriptor->CpuStart;\r
 \r
@@ -875,10 +1027,20 @@ 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
       AsmEnablePaging64 (\r
         0x38,\r