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

[mirror_edk2.git] / MdeModulePkg / Universal / Acpi / BootScriptExecutorDxe / X64 / SetIdtEntry.c

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