UefiCpuPkg/MpInitLib: Add missing point mark

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

diff --git a/UefiCpuPkg/Library/MpInitLib/MpLib.c b/UefiCpuPkg/Library/MpInitLib/MpLib.c
index 0495b0f8a4bda8a27273cb7eb078a0c33fbdfd1d..55fe812d292ab3c8f3609e95f0cab8073ce88034 100644 (file)
--- a/UefiCpuPkg/Library/MpInitLib/MpLib.c
+++ b/UefiCpuPkg/Library/MpInitLib/MpLib.c
@@ -109,6 +109,53 @@ SetApState (
   ReleaseSpinLock (&CpuData->ApLock);\r
 }\r
 \r
+/**\r
+  Save BSP's local APIC timer setting.\r
+\r
+  @param[in] CpuMpData          Pointer to CPU MP Data\r
+**/\r
+VOID\r
+SaveLocalApicTimerSetting (\r
+  IN CPU_MP_DATA   *CpuMpData\r
+  )\r
+{\r
+  //\r
+  // Record the current local APIC timer setting of BSP\r
+  //\r
+  GetApicTimerState (\r
+    &CpuMpData->DivideValue,\r
+    &CpuMpData->PeriodicMode,\r
+    &CpuMpData->Vector\r
+    );\r
+  CpuMpData->CurrentTimerCount   = GetApicTimerCurrentCount ();\r
+  CpuMpData->TimerInterruptState = GetApicTimerInterruptState ();\r
+}\r
+\r
+/**\r
+  Sync local APIC timer setting from BSP to AP.\r
+\r
+  @param[in] CpuMpData          Pointer to CPU MP Data\r
+**/\r
+VOID\r
+SyncLocalApicTimerSetting (\r
+  IN CPU_MP_DATA   *CpuMpData\r
+  )\r
+{\r
+  //\r
+  // Sync local APIC timer setting from BSP to AP\r
+  //\r
+  InitializeApicTimer (\r
+    CpuMpData->DivideValue,\r
+    CpuMpData->CurrentTimerCount,\r
+    CpuMpData->PeriodicMode,\r
+    CpuMpData->Vector\r
+    );\r
+  //\r
+  // Disable AP's local APIC timer interrupt\r
+  //\r
+  DisableApicTimerInterrupt ();\r
+}\r
+\r
 /**\r
   Save the volatile registers required to be restored following INIT IPI.\r
 \r
@@ -488,7 +535,12 @@ ApWakeupFunction (
   //\r
   CpuMpData = ExchangeInfo->CpuMpData;\r
 \r
-  ProgramVirtualWireMode (); \r
+  //\r
+  // AP's local APIC settings will be lost after received INIT IPI\r
+  // We need to re-initialize them at here\r
+  //\r
+  ProgramVirtualWireMode ();\r
+  SyncLocalApicTimerSetting (CpuMpData);\r
 \r
   while (TRUE) {\r
     if (CpuMpData->InitFlag == ApInitConfig) {\r
@@ -539,6 +591,10 @@ ApWakeupFunction (
         if (Procedure != NULL) {\r
           SetApState (&CpuMpData->CpuData[ProcessorNumber], CpuStateBusy);\r
           //\r
+          // Enable source debugging on AP function\r
+          //         \r
+          EnableDebugAgent ();\r
+          //\r
           // Invoke AP function here\r
           //\r
           Procedure (Parameter);\r
@@ -732,6 +788,7 @@ WakeUpAP (
     ResetVectorRequired = TRUE;\r
     AllocateResetVector (CpuMpData);\r
     FillExchangeInfoData (CpuMpData);\r
+    SaveLocalApicTimerSetting (CpuMpData);\r
   } else if (CpuMpData->ApLoopMode == ApInMwaitLoop) {\r
     //\r
     // Get AP target C-state each time when waking up AP,\r
@@ -1454,6 +1511,27 @@ SwitchBSPWorker (
   UINTN                        CallerNumber;\r
   CPU_STATE                    State;\r
   MSR_IA32_APIC_BASE_REGISTER  ApicBaseMsr;\r
+  BOOLEAN                      OldInterruptState;\r
+  BOOLEAN                      OldTimerInterruptState;\r
+\r
+  //\r
+  // Save and Disable Local APIC timer interrupt\r
+  //\r
+  OldTimerInterruptState = GetApicTimerInterruptState ();\r
+  DisableApicTimerInterrupt ();\r
+  //\r
+  // Before send both BSP and AP to a procedure to exchange their roles,\r
+  // interrupt must be disabled. This is because during the exchange role\r
+  // process, 2 CPU may use 1 stack. If interrupt happens, the stack will\r
+  // be corrupted, since interrupt return address will be pushed to stack\r
+  // by hardware.\r
+  //\r
+  OldInterruptState = SaveAndDisableInterrupts ();\r
+\r
+  //\r
+  // Mask LINT0 & LINT1 for the old BSP\r
+  //\r
+  DisableLvtInterrupts ();\r
 \r
   CpuMpData = GetCpuMpData ();\r
 \r
@@ -1530,12 +1608,23 @@ SwitchBSPWorker (
   //\r
   if (!EnableOldBSP) {\r
     SetApState (&CpuMpData->CpuData[CallerNumber], CpuStateDisabled);\r
+  } else {\r
+    SetApState (&CpuMpData->CpuData[CallerNumber], CpuStateIdle);\r
   }\r
   //\r
   // Save new BSP number\r
   //\r
   CpuMpData->BspNumber = (UINT32) ProcessorNumber;\r
 \r
+  //\r
+  // Restore interrupt state.\r
+  //\r
+  SetInterruptState (OldInterruptState);\r
+\r
+  if (OldTimerInterruptState) {\r
+    EnableApicTimerInterrupt ();\r
+  }\r
+\r
   return EFI_SUCCESS;\r
 }\r
 \r