UefiCpuPkg/PiSmmCpuDxeSmm: implement non-stop mode for SMM

[mirror_edk2.git] / UefiCpuPkg / PiSmmCpuDxeSmm / SmmProfile.c

diff --git a/UefiCpuPkg/PiSmmCpuDxeSmm/SmmProfile.c b/UefiCpuPkg/PiSmmCpuDxeSmm/SmmProfile.c
index b4fe0bc23b6cb6f9d0b0a53050d5255700366456..a743cf64f90715a51358da2209abd97cce1b2039 100644 (file)
--- a/UefiCpuPkg/PiSmmCpuDxeSmm/SmmProfile.c
+++ b/UefiCpuPkg/PiSmmCpuDxeSmm/SmmProfile.c
@@ -51,6 +51,11 @@ BOOLEAN                   mBtsSupported     = TRUE;
 //\r
 BOOLEAN                   mSmmProfileStart = FALSE;\r
 \r
+//\r
+// The flag indicates if #DB will be setup in #PF handler.\r
+//\r
+BOOLEAN                   mSetupDebugTrap = FALSE;\r
+\r
 //\r
 // Record the page fault exception count for one instruction execution.\r
 //\r
@@ -229,7 +234,9 @@ DebugExceptionHandler (
   UINTN  CpuIndex;\r
   UINTN  PFEntry;\r
 \r
-  if (!mSmmProfileStart) {\r
+  if (!mSmmProfileStart &&\r
+      !HEAP_GUARD_NONSTOP_MODE &&\r
+      !NULL_DETECTION_NONSTOP_MODE) {\r
     return;\r
   }\r
   CpuIndex = GetCpuIndex ();\r
@@ -1174,7 +1181,9 @@ InitSmmProfile (
   //\r
   // Skip SMM profile initialization if feature is disabled\r
   //\r
-  if (!FeaturePcdGet (PcdCpuSmmProfileEnable)) {\r
+  if (!FeaturePcdGet (PcdCpuSmmProfileEnable) &&\r
+      !HEAP_GUARD_NONSTOP_MODE &&\r
+      !NULL_DETECTION_NONSTOP_MODE) {\r
     return;\r
   }\r
 \r
@@ -1187,6 +1196,11 @@ InitSmmProfile (
   // Initialize profile IDT.\r
   //\r
   InitIdtr ();\r
+\r
+  //\r
+  // Tell #PF handler to prepare a #DB subsequently.\r
+  //\r
+  mSetupDebugTrap = TRUE;\r
 }\r
 \r
 /**\r
@@ -1294,6 +1308,46 @@ RestorePageTableBelow4G (
   }\r
 }\r
 \r
+/**\r
+  Handler for Page Fault triggered by Guard page.\r
+\r
+  @param  ErrorCode  The Error code of exception.\r
+\r
+**/\r
+VOID\r
+GuardPagePFHandler (\r
+  UINTN ErrorCode\r
+  )\r
+{\r
+  UINT64                *PageTable;\r
+  UINT64                PFAddress;\r
+  UINT64                RestoreAddress;\r
+  UINTN                 RestorePageNumber;\r
+  UINTN                 CpuIndex;\r
+\r
+  PageTable         = (UINT64 *)AsmReadCr3 ();\r
+  PFAddress         = AsmReadCr2 ();\r
+  CpuIndex          = GetCpuIndex ();\r
+\r
+  //\r
+  // Memory operation cross pages, like "rep mov" instruction, will cause\r
+  // infinite loop between this and Debug Trap handler. We have to make sure\r
+  // that current page and the page followed are both in PRESENT state.\r
+  //\r
+  RestorePageNumber = 2;\r
+  RestoreAddress = PFAddress;\r
+  while (RestorePageNumber > 0) {\r
+    RestorePageTableBelow4G (PageTable, RestoreAddress, CpuIndex, ErrorCode);\r
+    RestoreAddress += EFI_PAGE_SIZE;\r
+    RestorePageNumber--;\r
+  }\r
+\r
+  //\r
+  // Flush TLB\r
+  //\r
+  CpuFlushTlb ();\r
+}\r
+\r
 /**\r
   The Page fault handler to save SMM profile data.\r
 \r