UefiCpuPkg/MpInitLib: Disable and restore system timer interrupt

[mirror_edk2.git] / UefiCpuPkg / Library / MpInitLib / DxeMpLib.c

diff --git a/UefiCpuPkg/Library/MpInitLib/DxeMpLib.c b/UefiCpuPkg/Library/MpInitLib/DxeMpLib.c
index 733a9fb7be9401098ca959b0a57451971c8d3b76..5e50b4c391df7029b52004407b7fc9f8568bc274 100644 (file)
--- a/UefiCpuPkg/Library/MpInitLib/DxeMpLib.c
+++ b/UefiCpuPkg/Library/MpInitLib/DxeMpLib.c
@@ -18,6 +18,8 @@
 #include <Library/UefiBootServicesTableLib.h>\r
 #include <Library/DebugAgentLib.h>\r
 \r
+#include <Protocol/Timer.h>\r
+\r
 #define  AP_CHECK_INTERVAL     (EFI_TIMER_PERIOD_MILLISECONDS (100))\r
 #define  AP_SAFE_STACK_SIZE    128\r
 \r
@@ -645,11 +647,38 @@ MpInitLibSwitchBSP (
   IN BOOLEAN                   EnableOldBSP\r
   )\r
 {\r
-  EFI_STATUS            Status;\r
+  EFI_STATUS                   Status;\r
+  EFI_TIMER_ARCH_PROTOCOL      *Timer;\r
+  UINT64                       TimerPeriod;\r
 \r
+  //\r
+  // Locate Timer Arch Protocol\r
+  //\r
+  Status = gBS->LocateProtocol (&gEfiTimerArchProtocolGuid, NULL, (VOID **) &Timer);\r
+  if (EFI_ERROR (Status)) {\r
+    Timer = NULL;\r
+  }\r
+\r
+  if (Timer != NULL) {\r
+    //\r
+    // Save current rate of DXE Timer\r
+    //\r
+    Timer->GetTimerPeriod (Timer, &TimerPeriod);\r
+    //\r
+    // Disable DXE Timer and drain pending interrupts\r
+    //\r
+    Timer->SetTimerPeriod (Timer, 0);\r
+  }\r
 \r
   Status = SwitchBSPWorker (ProcessorNumber, EnableOldBSP);\r
 \r
+  if (Timer != NULL) {\r
+    //\r
+    // Enable and restore rate of DXE Timer\r
+    //\r
+    Timer->SetTimerPeriod (Timer, TimerPeriod);\r
+  }\r
+\r
   return Status;\r
 }\r
 \r