[Patch v5 34/48] UefiCpuPkg/MpInitLib: Implementation of MpInitLibStartupAllAPs()

[Jeff Fan <jeff.fan@intel.com>]

v4:
  1. Simply the internal function StartupAllAPsWorker()'s function
     header due to it is duplicated with MpInitLibStartupAllAPs().
v3:
  1. Use CamelCase for mStopCheckAllApsStatus and
     CheckAndUpdateApsStatus()

Cc: Michael Kinney <michael.d.kinney@intel.com>
Cc: Feng Tian <feng.tian@intel.com>
Cc: Giri P Mudusuru <giri.p.mudusuru@intel.com>
Cc: Laszlo Ersek <lersek@redhat.com>
Contributed-under: TianoCore Contribution Agreement 1.0
Signed-off-by: Jeff Fan <jeff.fan@intel.com>
---
 UefiCpuPkg/Library/MpInitLib/DxeMpLib.c |  23 ++++-
 UefiCpuPkg/Library/MpInitLib/MpLib.c    | 158 ++++++++++++++++++++++++++++++++
 UefiCpuPkg/Library/MpInitLib/MpLib.h    |  41 +++++++++
 UefiCpuPkg/Library/MpInitLib/PeiMpLib.c |  13 ++-
 4 files changed, 233 insertions(+), 2 deletions(-)

diff --git a/UefiCpuPkg/Library/MpInitLib/DxeMpLib.c b/UefiCpuPkg/Library/MpInitLib/DxeMpLib.c
index 75845bd..1043bec 100644
--- a/UefiCpuPkg/Library/MpInitLib/DxeMpLib.c
+++ b/UefiCpuPkg/Library/MpInitLib/DxeMpLib.c
@@ -305,7 +305,28 @@ MpInitLibStartupAllAPs (
   OUT UINTN                     **FailedCpuList         OPTIONAL
   )
 {
-  return EFI_UNSUPPORTED;
+  EFI_STATUS              Status;
+
+  //
+  // Temporarily stop checkAllApsStatus for avoid resource dead-lock.
+  //
+  mStopCheckAllApsStatus = TRUE;
+
+  Status = StartupAllAPsWorker (
+             Procedure,
+             SingleThread,
+             WaitEvent,
+             TimeoutInMicroseconds,
+             ProcedureArgument,
+             FailedCpuList
+             );
+
+  //
+  // Start checkAllApsStatus
+  //
+  mStopCheckAllApsStatus = FALSE;
+
+  return Status;
 }
 
 /**
diff --git a/UefiCpuPkg/Library/MpInitLib/MpLib.c b/UefiCpuPkg/Library/MpInitLib/MpLib.c
index c10dcd3..fbe2e8b 100644
--- a/UefiCpuPkg/Library/MpInitLib/MpLib.c
+++ b/UefiCpuPkg/Library/MpInitLib/MpLib.c
@@ -1720,6 +1720,164 @@ MpInitLibGetNumberOfProcessors (
 
 
 /**
+  Worker function to execute a caller provided function on all enabled APs.
+
+  @param[in]  Procedure               A pointer to the function to be run on
+                                      enabled APs of the system.
+  @param[in]  SingleThread            If TRUE, then all the enabled APs execute
+                                      the function specified by Procedure one by
+                                      one, in ascending order of processor handle
+                                      number.  If FALSE, then all the enabled APs
+                                      execute the function specified by Procedure
+                                      simultaneously.
+  @param[in]  WaitEvent               The event created by the caller with CreateEvent()
+                                      service.
+  @param[in]  TimeoutInMicrosecsond   Indicates the time limit in microseconds for
+                                      APs to return from Procedure, either for
+                                      blocking or non-blocking mode.
+  @param[in]  ProcedureArgument       The parameter passed into Procedure for
+                                      all APs.
+  @param[out] FailedCpuList           If all APs finish successfully, then its
+                                      content is set to NULL. If not all APs
+                                      finish before timeout expires, then its
+                                      content is set to address of the buffer
+                                      holding handle numbers of the failed APs.
+
+  @retval EFI_SUCCESS             In blocking mode, all APs have finished before
+                                  the timeout expired.
+  @retval EFI_SUCCESS             In non-blocking mode, function has been dispatched
+                                  to all enabled APs.
+  @retval others                  Failed to Startup all APs.
+
+**/
+EFI_STATUS
+StartupAllAPsWorker (
+  IN  EFI_AP_PROCEDURE          Procedure,
+  IN  BOOLEAN                   SingleThread,
+  IN  EFI_EVENT                 WaitEvent               OPTIONAL,
+  IN  UINTN                     TimeoutInMicroseconds,
+  IN  VOID                      *ProcedureArgument      OPTIONAL,
+  OUT UINTN                     **FailedCpuList         OPTIONAL
+  )
+{
+  EFI_STATUS              Status;
+  CPU_MP_DATA             *CpuMpData;
+  UINTN                   ProcessorCount;
+  UINTN                   ProcessorNumber;
+  UINTN                   CallerNumber;
+  CPU_AP_DATA             *CpuData;
+  BOOLEAN                 HasEnabledAp;
+  CPU_STATE               ApState;
+
+  CpuMpData = GetCpuMpData ();
+
+  if (FailedCpuList != NULL) {
+    *FailedCpuList = NULL;
+  }
+
+  if (CpuMpData->CpuCount == 1) {
+    return EFI_NOT_STARTED;
+  }
+
+  if (Procedure == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Check whether caller processor is BSP
+  //
+  MpInitLibWhoAmI (&CallerNumber);
+  if (CallerNumber != CpuMpData->BspNumber) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  //
+  // Update AP state
+  //
+  CheckAndUpdateApsStatus ();
+
+  ProcessorCount = CpuMpData->CpuCount;
+  HasEnabledAp   = FALSE;
+  //
+  // Check whether all enabled APs are idle.
+  // If any enabled AP is not idle, return EFI_NOT_READY.
+  //
+  for (ProcessorNumber = 0; ProcessorNumber < ProcessorCount; ProcessorNumber++) {
+    CpuData = &CpuMpData->CpuData[ProcessorNumber];
+    if (ProcessorNumber != CpuMpData->BspNumber) {
+      ApState = GetApState (CpuData);
+      if (ApState != CpuStateDisabled) {
+        HasEnabledAp = TRUE;
+        if (ApState != CpuStateIdle) {
+          //
+          // If any enabled APs are busy, return EFI_NOT_READY.
+          //
+          return EFI_NOT_READY;
+        }
+      }
+    }
+  }
+
+  if (!HasEnabledAp) {
+    //
+    // If no enabled AP exists, return EFI_NOT_STARTED.
+    //
+    return EFI_NOT_STARTED;
+  }
+
+  CpuMpData->StartCount = 0;
+  for (ProcessorNumber = 0; ProcessorNumber < ProcessorCount; ProcessorNumber++) {
+    CpuData = &CpuMpData->CpuData[ProcessorNumber];
+    CpuData->Waiting = FALSE;
+    if (ProcessorNumber != CpuMpData->BspNumber) {
+      if (CpuData->State == CpuStateIdle) {
+        //
+        // Mark this processor as responsible for current calling.
+        //
+        CpuData->Waiting = TRUE;
+        CpuMpData->StartCount++;
+      }
+    }
+  }
+
+  CpuMpData->Procedure     = Procedure;
+  CpuMpData->ProcArguments = ProcedureArgument;
+  CpuMpData->SingleThread  = SingleThread;
+  CpuMpData->FinishedCount = 0;
+  CpuMpData->RunningCount  = 0;
+  CpuMpData->FailedCpuList = FailedCpuList;
+  CpuMpData->ExpectedTime  = CalculateTimeout (
+                               TimeoutInMicroseconds,
+                               &CpuMpData->CurrentTime
+                               );
+  CpuMpData->TotalTime     = 0;
+  CpuMpData->WaitEvent     = WaitEvent;
+
+  if (!SingleThread) {
+    WakeUpAP (CpuMpData, TRUE, 0, Procedure, ProcedureArgument);
+  } else {
+    for (ProcessorNumber = 0; ProcessorNumber < ProcessorCount; ProcessorNumber++) {
+      if (ProcessorNumber == CallerNumber) {
+        continue;
+      }
+      if (CpuMpData->CpuData[ProcessorNumber].Waiting) {
+        WakeUpAP (CpuMpData, FALSE, ProcessorNumber, Procedure, ProcedureArgument);
+        break;
+      }
+    }
+  }
+
+  Status = EFI_SUCCESS;
+  if (WaitEvent == NULL) {
+    do {
+      Status = CheckAllAPs ();
+    } while (Status == EFI_NOT_READY);
+  }
+
+  return Status;
+}
+
+/**
   Worker function to let the caller get one enabled AP to execute a caller-provided
   function.
 
diff --git a/UefiCpuPkg/Library/MpInitLib/MpLib.h b/UefiCpuPkg/Library/MpInitLib/MpLib.h
index 04ffba5..06af7cc 100644
--- a/UefiCpuPkg/Library/MpInitLib/MpLib.h
+++ b/UefiCpuPkg/Library/MpInitLib/MpLib.h
@@ -348,6 +348,47 @@ InitMpGlobalData (
   );
 
 /**
+  Worker function to execute a caller provided function on all enabled APs.
+
+  @param[in]  Procedure               A pointer to the function to be run on
+                                      enabled APs of the system.
+  @param[in]  SingleThread            If TRUE, then all the enabled APs execute
+                                      the function specified by Procedure one by
+                                      one, in ascending order of processor handle
+                                      number.  If FALSE, then all the enabled APs
+                                      execute the function specified by Procedure
+                                      simultaneously.
+  @param[in]  WaitEvent               The event created by the caller with CreateEvent()
+                                      service.
+  @param[in]  TimeoutInMicrosecsond   Indicates the time limit in microseconds for
+                                      APs to return from Procedure, either for
+                                      blocking or non-blocking mode.
+  @param[in]  ProcedureArgument       The parameter passed into Procedure for
+                                      all APs.
+  @param[out] FailedCpuList           If all APs finish successfully, then its
+                                      content is set to NULL. If not all APs
+                                      finish before timeout expires, then its
+                                      content is set to address of the buffer
+                                      holding handle numbers of the failed APs.
+
+  @retval EFI_SUCCESS             In blocking mode, all APs have finished before
+                                  the timeout expired.
+  @retval EFI_SUCCESS             In non-blocking mode, function has been dispatched
+                                  to all enabled APs.
+  @retval others                  Failed to Startup all APs.
+
+**/
+EFI_STATUS
+StartupAllAPsWorker (
+  IN  EFI_AP_PROCEDURE          Procedure,
+  IN  BOOLEAN                   SingleThread,
+  IN  EFI_EVENT                 WaitEvent               OPTIONAL,
+  IN  UINTN                     TimeoutInMicroseconds,
+  IN  VOID                      *ProcedureArgument      OPTIONAL,
+  OUT UINTN                     **FailedCpuList         OPTIONAL
+  );
+
+/**
   Worker function to let the caller get one enabled AP to execute a caller-provided
   function.
 
diff --git a/UefiCpuPkg/Library/MpInitLib/PeiMpLib.c b/UefiCpuPkg/Library/MpInitLib/PeiMpLib.c
index 4ea8fa9..5e714ea 100644
--- a/UefiCpuPkg/Library/MpInitLib/PeiMpLib.c
+++ b/UefiCpuPkg/Library/MpInitLib/PeiMpLib.c
@@ -453,7 +453,18 @@ MpInitLibStartupAllAPs (
   OUT UINTN                     **FailedCpuList         OPTIONAL
   )
 {
-  return EFI_UNSUPPORTED;
+  if (WaitEvent != NULL) {
+    return EFI_UNSUPPORTED;
+  }
+
+  return StartupAllAPsWorker (
+           Procedure,
+           SingleThread,
+           NULL,
+           TimeoutInMicroseconds,
+           ProcedureArgument,
+           FailedCpuList
+           );
 }
 
 /**
-- 
2.7.4.windows.1