UefiCpuPkg: CpuDxe: Wait for APs to enter idle loop

[mirror_edk2.git] / UefiCpuPkg / CpuDxe / CpuMp.c

diff --git a/UefiCpuPkg/CpuDxe/CpuMp.c b/UefiCpuPkg/CpuDxe/CpuMp.c
index da3686e2788e8c50f0652bd743a3a010c42e5a2e..f3a5a24b0f6639aa9005a74eff9a54990fe532e9 100644 (file)
--- a/UefiCpuPkg/CpuDxe/CpuMp.c
+++ b/UefiCpuPkg/CpuDxe/CpuMp.c
@@ -312,6 +312,47 @@ CheckAndUpdateAllAPsToIdleState (
   }\r
 }\r
 \r
+/**\r
+  Check if all APs are in state CpuStateSleeping.\r
+\r
+  Return TRUE if all APs are in the CpuStateSleeping state.  Do not\r
+  check the state of the BSP or any disabled APs.\r
+\r
+  @retval TRUE   All APs are in CpuStateSleeping state.\r
+  @retval FALSE  One or more APs are not in CpuStateSleeping state.\r
+\r
+**/\r
+BOOLEAN\r
+CheckAllAPsSleeping (\r
+  VOID\r
+  )\r
+{\r
+  UINTN           ProcessorNumber;\r
+  CPU_DATA_BLOCK  *CpuData;\r
+\r
+  for (ProcessorNumber = 0; ProcessorNumber < mMpSystemData.NumberOfProcessors; ProcessorNumber++) {\r
+    CpuData = &mMpSystemData.CpuDatas[ProcessorNumber];\r
+    if (TestCpuStatusFlag (CpuData, PROCESSOR_AS_BSP_BIT)) {\r
+      //\r
+      // Skip BSP\r
+      //\r
+      continue;\r
+    }\r
+\r
+    if (!TestCpuStatusFlag (CpuData, PROCESSOR_ENABLED_BIT)) {\r
+      //\r
+      // Skip Disabled processors\r
+      //\r
+      continue;\r
+    }\r
+\r
+    if (GetApState (CpuData) != CpuStateSleeping) {\r
+      return FALSE;\r
+    }\r
+  }\r
+  return TRUE;\r
+}\r
+\r
 /**\r
   If the timeout expires before all APs returns from Procedure,\r
   we should forcibly terminate the executing AP and fill FailedList back\r
@@ -1634,7 +1675,8 @@ InitializeMpSupport (
   VOID\r
   )\r
 {\r
-  EFI_STATUS Status;\r
+  EFI_STATUS     Status;\r
+  UINTN          Timeout;\r
 \r
   gMaxLogicalProcessorNumber = (UINTN) PcdGet32 (PcdCpuMaxLogicalProcessorNumber);\r
   if (gMaxLogicalProcessorNumber < 1) {\r
@@ -1683,8 +1725,24 @@ InitializeMpSupport (
                              sizeof (CPU_DATA_BLOCK) * mMpSystemData.NumberOfProcessors,\r
                              mMpSystemData.CpuDatas);\r
 \r
+  //\r
+  // Release all APs to complete initialization and enter idle loop\r
+  //\r
   mAPsAlreadyInitFinished = TRUE;\r
 \r
+  //\r
+  // Wait for all APs to enter idle loop.\r
+  //\r
+  Timeout = 0;\r
+  do {\r
+    if (CheckAllAPsSleeping ()) {\r
+      break;\r
+    }\r
+    gBS->Stall (gPollInterval);\r
+    Timeout += gPollInterval;\r
+  } while (Timeout <= PcdGet32 (PcdCpuApInitTimeOutInMicroSeconds));\r
+  ASSERT (Timeout <= PcdGet32 (PcdCpuApInitTimeOutInMicroSeconds));\r
+\r
   //\r
   // Update CPU healthy information from Guided HOB\r
   //\r