UefiCpuPkg/MpInitLib: wait no longer than necessary for initial AP startup

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

diff --git a/UefiCpuPkg/Library/MpInitLib/MpLib.c b/UefiCpuPkg/Library/MpInitLib/MpLib.c
index 2fde765744eabd0fa14cec00e9576f555a714a67..ed22ce67782ef48b1148b5aac9868b1789a30e50 100644 (file)
--- a/UefiCpuPkg/Library/MpInitLib/MpLib.c
+++ b/UefiCpuPkg/Library/MpInitLib/MpLib.c
@@ -432,7 +432,8 @@ VOID
 InitializeApData (\r
   IN OUT CPU_MP_DATA      *CpuMpData,\r
   IN     UINTN            ProcessorNumber,\r
-  IN     UINT32           BistData\r
+  IN     UINT32           BistData,\r
+  IN     UINT64           ApTopOfStack\r
   )\r
 {\r
   CPU_INFO_IN_HOB          *CpuInfoInHob;\r
@@ -441,6 +442,7 @@ InitializeApData (
   CpuInfoInHob[ProcessorNumber].InitialApicId = GetInitialApicId ();\r
   CpuInfoInHob[ProcessorNumber].ApicId        = GetApicId ();\r
   CpuInfoInHob[ProcessorNumber].Health        = BistData;\r
+  CpuInfoInHob[ProcessorNumber].ApTopOfStack  = ApTopOfStack;\r
 \r
   CpuMpData->CpuData[ProcessorNumber].Waiting    = FALSE;\r
   CpuMpData->CpuData[ProcessorNumber].CpuHealthy = (BistData == 0) ? TRUE : FALSE;\r
@@ -478,6 +480,7 @@ ApWakeupFunction (
   UINT32                     BistData;\r
   volatile UINT32            *ApStartupSignalBuffer;\r
   CPU_INFO_IN_HOB            *CpuInfoInHob;\r
+  UINT64                     ApTopOfStack;\r
 \r
   //\r
   // AP finished assembly code and begin to execute C code\r
@@ -496,7 +499,8 @@ ApWakeupFunction (
       //\r
       // This is first time AP wakeup, get BIST information from AP stack\r
       //\r
-      BistData = *(UINT32 *) (CpuMpData->Buffer + ProcessorNumber * CpuMpData->CpuApStackSize - sizeof (UINTN));\r
+      ApTopOfStack  = CpuMpData->Buffer + (ProcessorNumber + 1) * CpuMpData->CpuApStackSize;\r
+      BistData = *(UINT32 *) ((UINTN) ApTopOfStack - sizeof (UINTN));\r
       //\r
       // Do some AP initialize sync\r
       //\r
@@ -505,7 +509,7 @@ ApWakeupFunction (
       // Sync BSP's Control registers to APs\r
       //\r
       RestoreVolatileRegisters (&CpuMpData->CpuData[0].VolatileRegisters, FALSE);\r
-      InitializeApData (CpuMpData, ProcessorNumber, BistData);\r
+      InitializeApData (CpuMpData, ProcessorNumber, BistData, ApTopOfStack);\r
       ApStartupSignalBuffer = CpuMpData->CpuData[ProcessorNumber].StartupApSignal;\r
     } else {\r
       //\r
@@ -545,6 +549,8 @@ ApWakeupFunction (
             GetProcessorNumber (CpuMpData, &ProcessorNumber);\r
             CpuMpData->CpuData[ProcessorNumber].ApFunction = 0;\r
             CpuMpData->CpuData[ProcessorNumber].ApFunctionArgument = 0;\r
+            ApStartupSignalBuffer = CpuMpData->CpuData[ProcessorNumber].StartupApSignal;\r
+            CpuInfoInHob[ProcessorNumber].ApTopOfStack = CpuInfoInHob[CpuMpData->NewBspNumber].ApTopOfStack;\r
           } else {\r
             //\r
             // Re-get the CPU APICID and Initial APICID\r
@@ -677,6 +683,21 @@ FillExchangeInfoData (
   AsmReadIdtr ((IA32_DESCRIPTOR *) &ExchangeInfo->IdtrProfile);\r
 }\r
 \r
+/**\r
+  Helper function that waits until the finished AP count reaches the specified\r
+  limit, or the specified timeout elapses (whichever comes first).\r
+\r
+  @param[in] CpuMpData        Pointer to CPU MP Data.\r
+  @param[in] FinishedApLimit  The number of finished APs to wait for.\r
+  @param[in] TimeLimit        The number of microseconds to wait for.\r
+**/\r
+VOID\r
+TimedWaitForApFinish (\r
+  IN CPU_MP_DATA               *CpuMpData,\r
+  IN UINT32                    FinishedApLimit,\r
+  IN UINT32                    TimeLimit\r
+  );\r
+\r
 /**\r
   This function will be called by BSP to wakeup AP.\r
 \r
@@ -742,7 +763,11 @@ WakeUpAP (
       //\r
       // Wait for all potential APs waken up in one specified period\r
       //\r
-      MicroSecondDelay (PcdGet32(PcdCpuApInitTimeOutInMicroSeconds));\r
+      TimedWaitForApFinish (\r
+        CpuMpData,\r
+        PcdGet32 (PcdCpuMaxLogicalProcessorNumber) - 1,\r
+        PcdGet32 (PcdCpuApInitTimeOutInMicroSeconds)\r
+        );\r
     } else {\r
       //\r
       // Wait all APs waken up if this is not the 1st broadcast of SIPI\r
@@ -888,6 +913,58 @@ CheckTimeout (
   return FALSE;\r
 }\r
 \r
+/**\r
+  Helper function that waits until the finished AP count reaches the specified\r
+  limit, or the specified timeout elapses (whichever comes first).\r
+\r
+  @param[in] CpuMpData        Pointer to CPU MP Data.\r
+  @param[in] FinishedApLimit  The number of finished APs to wait for.\r
+  @param[in] TimeLimit        The number of microseconds to wait for.\r
+**/\r
+VOID\r
+TimedWaitForApFinish (\r
+  IN CPU_MP_DATA               *CpuMpData,\r
+  IN UINT32                    FinishedApLimit,\r
+  IN UINT32                    TimeLimit\r
+  )\r
+{\r
+  //\r
+  // CalculateTimeout() and CheckTimeout() consider a TimeLimit of 0\r
+  // "infinity", so check for (TimeLimit == 0) explicitly.\r
+  //\r
+  if (TimeLimit == 0) {\r
+    return;\r
+  }\r
+\r
+  CpuMpData->TotalTime = 0;\r
+  CpuMpData->ExpectedTime = CalculateTimeout (\r
+                              TimeLimit,\r
+                              &CpuMpData->CurrentTime\r
+                              );\r
+  while (CpuMpData->FinishedCount < FinishedApLimit &&\r
+         !CheckTimeout (\r
+            &CpuMpData->CurrentTime,\r
+            &CpuMpData->TotalTime,\r
+            CpuMpData->ExpectedTime\r
+            )) {\r
+    CpuPause ();\r
+  }\r
+\r
+  if (CpuMpData->FinishedCount >= FinishedApLimit) {\r
+    DEBUG ((\r
+      DEBUG_VERBOSE,\r
+      "%a: reached FinishedApLimit=%u in %Lu microseconds\n",\r
+      __FUNCTION__,\r
+      FinishedApLimit,\r
+      DivU64x64Remainder (\r
+        MultU64x32 (CpuMpData->TotalTime, 1000000),\r
+        GetPerformanceCounterProperties (NULL, NULL),\r
+        NULL\r
+        )\r
+      ));\r
+  }\r
+}\r
+\r
 /**\r
   Reset an AP to Idle state.\r
 \r
@@ -1194,7 +1271,7 @@ MpInitLibInitialize (
   //\r
   // Set BSP basic information\r
   //\r
-  InitializeApData (CpuMpData, 0, 0);\r
+  InitializeApData (CpuMpData, 0, 0, CpuMpData->Buffer);\r
   //\r
   // Save assembly code information\r
   //\r
@@ -1416,6 +1493,7 @@ SwitchBSPWorker (
   CpuMpData->BSPInfo.State = CPU_SWITCH_STATE_IDLE;\r
   CpuMpData->APInfo.State  = CPU_SWITCH_STATE_IDLE;\r
   CpuMpData->SwitchBspFlag = TRUE;\r
+  CpuMpData->NewBspNumber  = ProcessorNumber;\r
 \r
   //\r
   // Clear the BSP bit of MSR_IA32_APIC_BASE\r