Create 4G page table by default, and using PF to handle >4G MMIO access, to improve S3 performance.

signed-off-by: jiewen.yao@intel.com
reviewed-by: rui.sun@intel.com

git-svn-id: https://edk2.svn.sourceforge.net/svnroot/edk2/trunk/edk2@13631 6f19259b-4bc3-4df7-8a09-765794883524

MdeModulePkg/Universal/Acpi/BootScriptExecutorDxe/BootScriptExecutorDxe.inf

@@ -78,6 +78,9 @@
 [FeaturePcd]
   gEfiMdeModulePkgTokenSpaceGuid.PcdDxeIplSwitchToLongMode
 
+[Pcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdUse1GPageTable
+
 [Depex]
   gEfiLockBoxProtocolGuid
 

MdeModulePkg/Universal/Acpi/BootScriptExecutorDxe/IA32/SetIdtEntry.c

@@ -3,7 +3,7 @@
 
   Set a IDT entry for interrupt vector 3 for debug purpose for IA32 platform
 
-Copyright (c) 2006 - 2010, Intel Corporation. All rights reserved.<BR>
+Copyright (c) 2006 - 2012, Intel Corporation. All rights reserved.<BR>
 
 This program and the accompanying materials
 are licensed and made available under the terms and conditions of the BSD License
@@ -54,7 +54,7 @@ SetIdtEntry (
   S3DebugBuffer = (UINTN) (AcpiS3Context->S3DebugBufferAddress);
 
   IdtEntry->OffsetLow       = (UINT16)S3DebugBuffer;
-  IdtEntry->SegmentSelector = (UINT16)AsmReadCs ();;
+  IdtEntry->SegmentSelector = (UINT16)AsmReadCs ();
   IdtEntry->Attributes      = (UINT16)INTERRUPT_GATE_ATTRIBUTE;
   IdtEntry->OffsetHigh      = (UINT16)(S3DebugBuffer >> 16);
 

MdeModulePkg/Universal/Acpi/BootScriptExecutorDxe/X64/S3Asm.S

@@ -2,7 +2,7 @@
 #   This is the assembly code for transferring to control to OS S3 waking vector
 #   for X64 platform
 #
-# Copyright (c) 2006, Intel Corporation. All rights reserved.<BR>
+# Copyright (c) 2006 - 2012, Intel Corporation. All rights reserved.<BR>
 #
 # This program and the accompanying materials are
 # licensed and made available under the terms and conditions of the BSD License
@@ -80,3 +80,51 @@ ASM_PFX(AsmTransferControl16):
 ASM_GLOBAL ASM_PFX(AsmJmpAddr32)
 ASM_PFX(AsmJmpAddr32):
     .long    0
+
+ASM_GLOBAL ASM_PFX(PageFaultHandlerHook)
+ASM_PFX(PageFaultHandlerHook):
+    pushq    %rax                         # save all volatile registers
+    pushq    %rcx
+    pushq    %rdx
+    pushq    %r8
+    pushq    %r9
+    pushq    %r10
+    pushq    %r11
+    # save volatile fp registers
+    addq     $-0x68, %rsp
+    stmxcsr  0x60(%rsp)
+    movdqa   %xmm0, 0x0(%rsp) 
+    movdqa   %xmm1, 0x10(%rsp) 
+    movdqa   %xmm2, 0x20(%rsp) 
+    movdqa   %xmm3, 0x30(%rsp) 
+    movdqa   %xmm4, 0x40(%rsp) 
+    movdqa   %xmm5, 0x50(%rsp) 
+
+    addq     $-0x20, %rsp
+    call     ASM_PFX(PageFaultHandler)
+    addq     $0x20, %rsp
+
+    # load volatile fp registers
+    ldmxcsr  0x60(%rsp)
+    movdqa   0x0(%rsp), %xmm0
+    movdqa   0x10(%rsp), %xmm1
+    movdqa   0x20(%rsp), %xmm2
+    movdqa   0x30(%rsp), %xmm3
+    movdqa   0x40(%rsp), %xmm4
+    movdqa   0x50(%rsp), %xmm5
+    addq     $0x68, %rsp
+
+    testb    %al, %al
+
+    popq     %r11
+    popq     %r10
+    popq     %r9
+    popq     %r8
+    popq     %rdx
+    popq     %rcx
+    popq     %rax                         # restore all volatile registers
+    jnz      L1
+    jmpq     *ASM_PFX(mOriginalHandler)
+L1:
+    addq     $0x08, %rsp                  # skip error code for PF
+    iretq

MdeModulePkg/Universal/Acpi/BootScriptExecutorDxe/X64/S3Asm.asm

@@ -2,7 +2,7 @@
 ;   This is the assembly code for transferring to control to OS S3 waking vector
 ;   for X64 platform
 ;
-; Copyright (c) 2006, Intel Corporation. All rights reserved.<BR>
+; Copyright (c) 2006 - 2012, Intel Corporation. All rights reserved.<BR>
 ;
 ; This program and the accompanying materials
 ; are licensed and made available under the terms and conditions of the BSD License
@@ -14,6 +14,9 @@
 ;
 ;;
 
+EXTERN mOriginalHandler:QWORD
+EXTERN PageFaultHandler:PROC
+
     .code
     
 EXTERNDEF   AsmFixAddress16:DWORD
@@ -81,4 +84,52 @@ AsmTransferControl16  PROC
 AsmJmpAddr32 DD  ?
 AsmTransferControl16  ENDP
 
+PageFaultHandlerHook PROC
+    push    rax                         ; save all volatile registers
+    push    rcx
+    push    rdx
+    push    r8
+    push    r9
+    push    r10
+    push    r11
+    ; save volatile fp registers
+    add     rsp, -68h
+    stmxcsr [rsp + 60h]
+    movdqa  [rsp + 0h], xmm0
+    movdqa  [rsp + 10h], xmm1
+    movdqa  [rsp + 20h], xmm2
+    movdqa  [rsp + 30h], xmm3
+    movdqa  [rsp + 40h], xmm4
+    movdqa  [rsp + 50h], xmm5
+
+    add     rsp, -20h
+    call    PageFaultHandler
+    add     rsp, 20h
+    
+    ; load volatile fp registers
+    ldmxcsr [rsp + 60h]
+    movdqa  xmm0,  [rsp + 0h]
+    movdqa  xmm1,  [rsp + 10h]
+    movdqa  xmm2,  [rsp + 20h]
+    movdqa  xmm3,  [rsp + 30h]
+    movdqa  xmm4,  [rsp + 40h]
+    movdqa  xmm5,  [rsp + 50h]
+    add     rsp, 68h
+
+    test    al, al
+    
+    pop     r11
+    pop     r10
+    pop     r9
+    pop     r8
+    pop     rdx
+    pop     rcx
+    pop     rax                         ; restore all volatile registers
+    jnz     @F
+    jmp     mOriginalHandler
+@@:
+    add     rsp, 08h                    ; skip error code for PF
+    iretq
+PageFaultHandlerHook ENDP
+
     END

MdeModulePkg/Universal/Acpi/BootScriptExecutorDxe/X64/SetIdtEntry.c

@@ -33,6 +33,63 @@ typedef struct {
 #define INTERRUPT_GATE_ATTRIBUTE   0x8e00
 
 #pragma pack()
+
+#define IA32_PG_P                   BIT0
+#define IA32_PG_RW                  BIT1
+#define IA32_PG_PS                  BIT7
+
+UINT64                             mPhyMask;
+BOOLEAN                            mPage1GSupport;
+VOID                               *mOriginalHandler;
+UINTN                              mS3NvsPageTableAddress;
+
+VOID
+EFIAPI
+PageFaultHandlerHook (
+  VOID
+  );
+
+VOID
+HookPageFaultHandler (
+  IN INTERRUPT_GATE_DESCRIPTOR                     *IdtEntry
+  )
+{
+  UINT32         RegEax;
+  UINT32         RegEdx;
+
+  AsmCpuid (0x80000008, &RegEax, NULL, NULL, NULL);
+  mPhyMask = LShiftU64 (1, (UINT8)RegEax) - 1;
+  mPhyMask &= (1ull << 48) - SIZE_4KB;
+
+  mPage1GSupport = FALSE;
+  if (PcdGetBool(PcdUse1GPageTable)) {
+    AsmCpuid (0x80000000, &RegEax, NULL, NULL, NULL);
+    if (RegEax >= 0x80000001) {
+      AsmCpuid (0x80000001, NULL, NULL, NULL, &RegEdx);
+      if ((RegEdx & BIT26) != 0) {
+        mPage1GSupport = TRUE;
+      }
+    }
+  }
+
+  //
+  // Set Page Fault entry to catch >4G access
+  //
+  mOriginalHandler = (VOID *)(UINTN)(LShiftU64 (IdtEntry->Offset63To32, 32) + IdtEntry->Offset15To0 + (IdtEntry->Offset31To16 << 16));
+  IdtEntry->Offset15To0     = (UINT16)((UINTN)PageFaultHandlerHook);
+  IdtEntry->SegmentSelector = (UINT16)AsmReadCs ();
+  IdtEntry->Attributes      = (UINT16)INTERRUPT_GATE_ATTRIBUTE;
+  IdtEntry->Offset31To16    = (UINT16)((UINTN)PageFaultHandlerHook >> 16);
+  IdtEntry->Offset63To32    = (UINT32)((UINTN)PageFaultHandlerHook >> 32);
+  IdtEntry->Reserved        = 0;
+
+  if (mPage1GSupport) {
+    mS3NvsPageTableAddress = (UINTN)(AsmReadCr3 () & mPhyMask) + EFI_PAGES_TO_SIZE(2);
+  }else {
+    mS3NvsPageTableAddress = (UINTN)(AsmReadCr3 () & mPhyMask) + EFI_PAGES_TO_SIZE(6);
+  }
+}
+
 /**
   Set a IDT entry for interrupt vector 3 for debug purpose.
 
@@ -66,11 +123,69 @@ SetIdtEntry (
   S3DebugBuffer = (UINTN) (AcpiS3Context->S3DebugBufferAddress);
 
   IdtEntry->Offset15To0     = (UINT16)S3DebugBuffer;
-  IdtEntry->SegmentSelector = (UINT16)AsmReadCs ();;
+  IdtEntry->SegmentSelector = (UINT16)AsmReadCs ();
   IdtEntry->Attributes      = (UINT16)INTERRUPT_GATE_ATTRIBUTE;
   IdtEntry->Offset31To16    = (UINT16)(S3DebugBuffer >> 16);
   IdtEntry->Offset63To32    = (UINT32)(S3DebugBuffer >> 32);
   IdtEntry->Reserved        = 0;
 
+  IdtEntry = (INTERRUPT_GATE_DESCRIPTOR *)(IdtDescriptor->Base + (14 * sizeof (INTERRUPT_GATE_DESCRIPTOR)));
+  HookPageFaultHandler (IdtEntry);
+
+  AsmWriteIdtr (IdtDescriptor);
 }
 
+UINTN
+GetNewPage (
+  IN UINTN  PageNum
+  )
+{
+  UINTN  NewPage;
+  NewPage = mS3NvsPageTableAddress;
+  ZeroMem ((VOID *)NewPage, EFI_PAGES_TO_SIZE(PageNum));
+  mS3NvsPageTableAddress += EFI_PAGES_TO_SIZE(PageNum);
+  return NewPage;
+}
+
+BOOLEAN
+EFIAPI
+PageFaultHandler (
+  VOID
+  )
+{
+  UINT64         *PageTable;
+  UINT64         PFAddress;
+  UINTN          PTIndex;
+
+  PFAddress = AsmReadCr2 ();
+  DEBUG ((EFI_D_ERROR, "BootScript - PageFaultHandler: Cr2 - %lx\n", PFAddress));
+
+  if (PFAddress >= mPhyMask + SIZE_4KB) {
+    return FALSE;
+  }
+  PFAddress &= mPhyMask;
+
+  PageTable = (UINT64*)(UINTN)(AsmReadCr3 () & mPhyMask);
+
+  PTIndex = BitFieldRead64 (PFAddress, 39, 47);
+  // PML4E
+  if ((PageTable[PTIndex] & IA32_PG_P) == 0) {
+    PageTable[PTIndex] = GetNewPage (1) | IA32_PG_P | IA32_PG_RW;
+  }
+  PageTable = (UINT64*)(UINTN)(PageTable[PTIndex] & mPhyMask);
+  PTIndex = BitFieldRead64 (PFAddress, 30, 38);
+  // PDPTE
+  if (mPage1GSupport) {
+    PageTable[PTIndex] = PFAddress | IA32_PG_P | IA32_PG_RW | IA32_PG_PS;
+  } else {
+    if ((PageTable[PTIndex] & IA32_PG_P) == 0) {
+      PageTable[PTIndex] = GetNewPage (1) | IA32_PG_P | IA32_PG_RW;
+    }
+    PageTable = (UINT64*)(UINTN)(PageTable[PTIndex] & mPhyMask);
+    PTIndex = BitFieldRead64 (PFAddress, 21, 29);
+    // PD
+    PageTable[PTIndex] = PFAddress | IA32_PG_P | IA32_PG_RW | IA32_PG_PS;
+  }
+
+  return TRUE;
+}

UefiCpuPkg/Universal/Acpi/S3Resume2Pei/S3Resume.c

@@ -367,6 +367,41 @@ WriteToOsS3PerformanceData (
   PerfHeader->S3EntryNum = (UINT32) Index;
 }
 
+/**
+  The function will check if current waking vector is long mode.
+
+  @param  AcpiS3Context                 a pointer to a structure of ACPI_S3_CONTEXT
+
+  @retval TRUE   Current context need long mode waking vector.
+  @retval FALSE  Current context need not long mode waking vector.
+**/
+BOOLEAN
+IsLongModeWakingVector (
+  IN ACPI_S3_CONTEXT                *AcpiS3Context
+  )
+{
+  EFI_ACPI_4_0_FIRMWARE_ACPI_CONTROL_STRUCTURE  *Facs;
+
+  Facs = (EFI_ACPI_4_0_FIRMWARE_ACPI_CONTROL_STRUCTURE *) ((UINTN) (AcpiS3Context->AcpiFacsTable));
+  if ((Facs == NULL) ||
+      (Facs->Signature != EFI_ACPI_4_0_FIRMWARE_ACPI_CONTROL_STRUCTURE_SIGNATURE) ||
+      ((Facs->FirmwareWakingVector == 0) && (Facs->XFirmwareWakingVector == 0)) ) {
+    // Something wrong with FACS
+    return FALSE;
+  }
+  if (Facs->XFirmwareWakingVector != 0) {
+    if ((Facs->Version == EFI_ACPI_4_0_FIRMWARE_ACPI_CONTROL_STRUCTURE_VERSION) &&
+        ((Facs->Flags & EFI_ACPI_4_0_64BIT_WAKE_SUPPORTED_F) != 0) &&
+        ((Facs->Flags & EFI_ACPI_4_0_OSPM_64BIT_WAKE__F) != 0)) {
+      // Both BIOS and OS wants 64bit vector
+      if (FeaturePcdGet (PcdDxeIplSwitchToLongMode)) {
+        return TRUE;
+      }
+    }
+  }
+  return FALSE;
+}
+
 /**
   Jump to OS waking vector.
   The function will install boot script done PPI, report S3 resume status code, and then jump to OS waking vector.
@@ -483,10 +518,12 @@ S3ResumeBootOs (
   If BootScriptExector driver will not run in 64-bit mode, this function will do nothing. 
 
   @param S3NvsPageTableAddress   PageTableAddress in ACPINvs
+  @param Build4GPageTableOnly    If BIOS just build 4G page table only
 **/
 VOID
 RestoreS3PageTables (
-  IN UINTN                                         S3NvsPageTableAddress
+  IN UINTN                                         S3NvsPageTableAddress,
+  IN BOOLEAN                                       Build4GPageTableOnly
   )
 {
   if (FeaturePcdGet (PcdDxeIplSwitchToLongMode)) {
@@ -513,7 +550,7 @@ RestoreS3PageTables (
     //
     // The assumption is : whole page table is allocated in CONTINOUS memory and CR3 points to TOP page.
     //
-    DEBUG ((EFI_D_ERROR, "S3NvsPageTableAddress - %x\n", S3NvsPageTableAddress));
+    DEBUG ((EFI_D_ERROR, "S3NvsPageTableAddress - %x (%x)\n", (UINTN)S3NvsPageTableAddress, (UINTN)Build4GPageTableOnly));
 
     //
     // By architecture only one PageMapLevel4 exists - so lets allocate storgage for it.
@@ -556,6 +593,14 @@ RestoreS3PageTables (
       PhysicalAddressBits = 48;
     }
 
+    //
+    // NOTE: In order to save time to create full page table, we just create 4G page table by default.
+    // And let PF handler in BootScript driver to create more on request.
+    //
+    if (Build4GPageTableOnly) {
+      PhysicalAddressBits = 32;
+      ZeroMem (PageMap, EFI_PAGES_TO_SIZE(2));
+    }
     //
     // Calculate the table entries needed.
     //
@@ -827,6 +872,7 @@ S3RestoreConfig2 (
   EFI_SMRAM_DESCRIPTOR                          *SmramDescriptor;
   SMM_S3_RESUME_STATE                           *SmmS3ResumeState;
   VOID                                          *GuidHob;
+  BOOLEAN                                       Build4GPageTableOnly;
 
   DEBUG ((EFI_D_ERROR, "Enter S3 PEIM\r\n"));
 
@@ -888,7 +934,12 @@ S3RestoreConfig2 (
     //
     // Need reconstruct page table here, since we do not trust ACPINvs.
     //
-    RestoreS3PageTables ((UINTN)AcpiS3Context->S3NvsPageTableAddress);
+    if (IsLongModeWakingVector (AcpiS3Context)) {
+      Build4GPageTableOnly = FALSE;
+    } else {
+      Build4GPageTableOnly = TRUE;
+    }
+    RestoreS3PageTables ((UINTN)AcpiS3Context->S3NvsPageTableAddress, Build4GPageTableOnly);
   }
 
   //