MdePkg/BaseSynchronizationLib: fix AArch64 return values

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

diff --git a/UefiCpuPkg/CpuDxe/CpuMp.c b/UefiCpuPkg/CpuDxe/CpuMp.c
index da3686e2788e8c50f0652bd743a3a010c42e5a2e..04c2f1f0ac12ff3511ddfd444bf8000ee302bebc 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
@@ -1625,6 +1666,22 @@ ExitBootServicesCallback (
   SendInitIpiAllExcludingSelf ();\r
 }\r
 \r
+/**\r
+  A minimal wrapper function that allows MtrrSetAllMtrrs() to be passed to\r
+  EFI_MP_SERVICES_PROTOCOL.StartupAllAPs() as Procedure.\r
+\r
+  @param[in] Buffer  Pointer to an MTRR_SETTINGS object, to be passed to\r
+                     MtrrSetAllMtrrs().\r
+**/\r
+VOID\r
+EFIAPI\r
+SetMtrrsFromBuffer (\r
+  IN VOID *Buffer\r
+  )\r
+{\r
+  MtrrSetAllMtrrs (Buffer);\r
+}\r
+\r
 /**\r
   Initialize Multi-processor support.\r
 \r
@@ -1634,7 +1691,9 @@ InitializeMpSupport (
   VOID\r
   )\r
 {\r
-  EFI_STATUS Status;\r
+  EFI_STATUS     Status;\r
+  MTRR_SETTINGS  MtrrSettings;\r
+  UINTN          Timeout;\r
 \r
   gMaxLogicalProcessorNumber = (UINTN) PcdGet32 (PcdCpuMaxLogicalProcessorNumber);\r
   if (gMaxLogicalProcessorNumber < 1) {\r
@@ -1683,13 +1742,44 @@ 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
   CollectBistDataFromHob ();\r
 \r
+  //\r
+  // Synchronize MTRR settings to APs.\r
+  //\r
+  MtrrGetAllMtrrs (&MtrrSettings);\r
+  Status = mMpServicesTemplate.StartupAllAPs (\r
+                                 &mMpServicesTemplate, // This\r
+                                 SetMtrrsFromBuffer,   // Procedure\r
+                                 TRUE,                 // SingleThread\r
+                                 NULL,                 // WaitEvent\r
+                                 0,                    // TimeoutInMicrosecsond\r
+                                 &MtrrSettings,        // ProcedureArgument\r
+                                 NULL                  // FailedCpuList\r
+                                 );\r
+  ASSERT (Status == EFI_SUCCESS || Status == EFI_NOT_STARTED);\r
+\r
   Status = gBS->InstallMultipleProtocolInterfaces (\r
                   &mMpServiceHandle,\r
                   &gEfiMpServiceProtocolGuid,  &mMpServicesTemplate,\r