[Patch v2 6/6] UefiCpuPkg/RegisterCpuFeaturesLib: Start all processors simultaneously.

["Dong, Eric" <eric.dong@intel.com>]

REF: https://bugzilla.tianocore.org/show_bug.cgi?id=1973

For semaphore type register, it required all processors to do the
task at the same time.
Current logic begins BSP's task after all APs have finished their tasks.
This will caused set semaphore task hang if semaphore has package
level type.
This patch use new EDKII_PEI_MP_SERVICES2_PPI to start all processors at
the same time to fix the potential hang issue.

Cc: Ray Ni <ray.ni@intel.com>
Cc: Laszlo Ersek <lersek@redhat.com>
Cc: Chandana Kumar <chandana.c.kumar@intel.com>
Cc: Star Zeng <star.zeng@intel.com>
Signed-off-by: Eric Dong <eric.dong@intel.com>
---
 .../PeiRegisterCpuFeaturesLib.c               | 62 ++++++++++++++-----
 .../PeiRegisterCpuFeaturesLib.inf             |  1 +
 2 files changed, 47 insertions(+), 16 deletions(-)

diff --git a/UefiCpuPkg/Library/RegisterCpuFeaturesLib/PeiRegisterCpuFeaturesLib.c b/UefiCpuPkg/Library/RegisterCpuFeaturesLib/PeiRegisterCpuFeaturesLib.c
index 8ad5a40e5a..9ed0fbf381 100644
--- a/UefiCpuPkg/Library/RegisterCpuFeaturesLib/PeiRegisterCpuFeaturesLib.c
+++ b/UefiCpuPkg/Library/RegisterCpuFeaturesLib/PeiRegisterCpuFeaturesLib.c
@@ -12,6 +12,8 @@
 #include <Library/PeiServicesLib.h>
 #include <Library/PeiServicesTablePointerLib.h>
 #include <Ppi/MpServices.h>
+#include <Ppi/EdkiiMpServices2.h>
+
 #include "RegisterCpuFeatures.h"
 
 #define REGISTER_CPU_FEATURES_GUID \
@@ -180,6 +182,48 @@ StartupAPsWorker (
   ASSERT_EFI_ERROR (Status);
 }
 
+/**
+  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]  MpEvent                 The Event used to sync the result.
+
+**/
+VOID
+StartupCPUsWorker (
+  IN  EFI_AP_PROCEDURE                 Procedure
+  )
+{
+  EFI_STATUS                           Status;
+  EDKII_PEI_MP_SERVICES2_PPI           *CpuMp2Ppi;
+  CPU_FEATURES_DATA                    *CpuFeaturesData;
+
+  CpuFeaturesData = GetCpuFeaturesData ();
+
+  //
+  // Get MP Services2 Ppi
+  //
+  Status = PeiServicesLocatePpi (
+             &gEdkiiPeiMpServices2PpiGuid,
+             0,
+             NULL,
+             (VOID **)&CpuMp2Ppi
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Wakeup all APs for data collection.
+  //
+  Status = CpuMp2Ppi->StartupAllCPUs (
+                 CpuMp2Ppi,
+                 Procedure,
+                 0,
+                 CpuFeaturesData
+                 );
+  ASSERT_EFI_ERROR (Status);
+}
+
 /**
   Worker function to switch the requested AP to be the BSP from that point onward.
 
@@ -267,23 +311,9 @@ CpuFeaturesInitialize (
   CpuFeaturesData->BspNumber = OldBspNumber;
 
   //
-  // Known limitation: In PEI phase, CpuFeatures driver not
-  // support async mode execute tasks. So semaphore type
-  // register can't been used for this instance, must use
-  // DXE type instance.
-  //
-
-  if (CpuFeaturesData->NumberOfCpus > 1) {
-    //
-    // Wakeup all APs for programming.
-    //
-    StartupAPsWorker (SetProcessorRegister, NULL);
-  }
-
-  //
-  // Programming BSP
+  // Start to program register for all CPUs.
   //
-  SetProcessorRegister (CpuFeaturesData);
+  StartupCPUsWorker (SetProcessorRegister);
 
   //
   // Switch to new BSP if required
diff --git a/UefiCpuPkg/Library/RegisterCpuFeaturesLib/PeiRegisterCpuFeaturesLib.inf b/UefiCpuPkg/Library/RegisterCpuFeaturesLib/PeiRegisterCpuFeaturesLib.inf
index 63091dfeb8..61f922bf63 100644
--- a/UefiCpuPkg/Library/RegisterCpuFeaturesLib/PeiRegisterCpuFeaturesLib.inf
+++ b/UefiCpuPkg/Library/RegisterCpuFeaturesLib/PeiRegisterCpuFeaturesLib.inf
@@ -46,6 +46,7 @@
 
 [Ppis]
   gEfiPeiMpServicesPpiGuid                                             ## CONSUMES
+  gEdkiiPeiMpServices2PpiGuid                                          ## CONSUMES
 
 [Pcd]
   gUefiCpuPkgTokenSpaceGuid.PcdCpuS3DataAddress                        ## CONSUMES
-- 
2.21.0.windows.1