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

5 files changed, 222 insertions, 5 deletions

diff --git a/MdeModulePkg/Universal/Acpi/BootScriptExecutorDxe/BootScriptExecutorDxe.inf b/MdeModulePkg/Universal/Acpi/BootScriptExecutorDxe/BootScriptExecutorDxe.inf
index 2fbbdb2e31..04d4893a97 100644
--- a/MdeModulePkg/Universal/Acpi/BootScriptExecutorDxe/BootScriptExecutorDxe.inf
+++ b/MdeModulePkg/Universal/Acpi/BootScriptExecutorDxe/BootScriptExecutorDxe.inf
@@ -78,6 +78,9 @@
 [FeaturePcd]

   gEfiMdeModulePkgTokenSpaceGuid.PcdDxeIplSwitchToLongMode

 

+[Pcd]

+  gEfiMdeModulePkgTokenSpaceGuid.PcdUse1GPageTable

+

 [Depex]

   gEfiLockBoxProtocolGuid

 

diff --git a/MdeModulePkg/Universal/Acpi/BootScriptExecutorDxe/IA32/SetIdtEntry.c b/MdeModulePkg/Universal/Acpi/BootScriptExecutorDxe/IA32/SetIdtEntry.c
index 8221be6c87..9f04959cd9 100644
--- a/MdeModulePkg/Universal/Acpi/BootScriptExecutorDxe/IA32/SetIdtEntry.c
+++ b/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);

 

diff --git a/MdeModulePkg/Universal/Acpi/BootScriptExecutorDxe/X64/S3Asm.S b/MdeModulePkg/Universal/Acpi/BootScriptExecutorDxe/X64/S3Asm.S
index 7f5bdebfd2..dcce6fb6ae 100644
--- a/MdeModulePkg/Universal/Acpi/BootScriptExecutorDxe/X64/S3Asm.S
+++ b/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

diff --git a/MdeModulePkg/Universal/Acpi/BootScriptExecutorDxe/X64/S3Asm.asm b/MdeModulePkg/Universal/Acpi/BootScriptExecutorDxe/X64/S3Asm.asm
index f3d327df75..0b7432daf7 100644
--- a/MdeModulePkg/Universal/Acpi/BootScriptExecutorDxe/X64/S3Asm.asm
+++ b/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

diff --git a/MdeModulePkg/Universal/Acpi/BootScriptExecutorDxe/X64/SetIdtEntry.c b/MdeModulePkg/Universal/Acpi/BootScriptExecutorDxe/X64/SetIdtEntry.c
index 975cf3a561..db11697e7c 100644
--- a/MdeModulePkg/Universal/Acpi/BootScriptExecutorDxe/X64/SetIdtEntry.c
+++ b/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;

+}