[Patch v3 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>
---
 .../CpuFeaturesInitialize.c                   |  2 +-
 .../DxeRegisterCpuFeaturesLib.c               |  6 +-
 .../PeiRegisterCpuFeaturesLib.c               | 66 ++++++++++++++-----
 .../PeiRegisterCpuFeaturesLib.inf             |  1 +
 .../RegisterCpuFeatures.h                     |  2 +-
 5 files changed, 54 insertions(+), 23 deletions(-)

diff --git a/UefiCpuPkg/Library/RegisterCpuFeaturesLib/CpuFeaturesInitialize.c b/UefiCpuPkg/Library/RegisterCpuFeaturesLib/CpuFeaturesInitialize.c
index 33752c1a9f..4e97e863c7 100644
--- a/UefiCpuPkg/Library/RegisterCpuFeaturesLib/CpuFeaturesInitialize.c
+++ b/UefiCpuPkg/Library/RegisterCpuFeaturesLib/CpuFeaturesInitialize.c
@@ -1074,7 +1074,7 @@ CpuFeaturesDetect (
     //
     // Wakeup all APs for data collection.
     //
-    StartupAPsWorker (CollectProcessorData, NULL);
+    StartupAllAPsWorker (CollectProcessorData, NULL);
   }
 
   //
diff --git a/UefiCpuPkg/Library/RegisterCpuFeaturesLib/DxeRegisterCpuFeaturesLib.c b/UefiCpuPkg/Library/RegisterCpuFeaturesLib/DxeRegisterCpuFeaturesLib.c
index f87b2892aa..e2e06e88e0 100644
--- a/UefiCpuPkg/Library/RegisterCpuFeaturesLib/DxeRegisterCpuFeaturesLib.c
+++ b/UefiCpuPkg/Library/RegisterCpuFeaturesLib/DxeRegisterCpuFeaturesLib.c
@@ -1,7 +1,7 @@
 /** @file
   CPU Register Table Library functions.
 
-  Copyright (c) 2017, Intel Corporation. All rights reserved.<BR>
+  Copyright (c) 2017 - 2019, Intel Corporation. All rights reserved.<BR>
   SPDX-License-Identifier: BSD-2-Clause-Patent
 
 **/
@@ -116,7 +116,7 @@ GetProcessorInformation (
                                       to check whether procedure has done.
 **/
 VOID
-StartupAPsWorker (
+StartupAllAPsWorker (
   IN  EFI_AP_PROCEDURE                 Procedure,
   IN  EFI_EVENT                        MpEvent
   )
@@ -248,7 +248,7 @@ CpuFeaturesInitialize (
     //
     // Wakeup all APs for programming.
     //
-    StartupAPsWorker (SetProcessorRegister, MpEvent);
+    StartupAllAPsWorker (SetProcessorRegister, MpEvent);
   }
 
   //
diff --git a/UefiCpuPkg/Library/RegisterCpuFeaturesLib/PeiRegisterCpuFeaturesLib.c b/UefiCpuPkg/Library/RegisterCpuFeaturesLib/PeiRegisterCpuFeaturesLib.c
index 8ad5a40e5a..52b849ce13 100644
--- a/UefiCpuPkg/Library/RegisterCpuFeaturesLib/PeiRegisterCpuFeaturesLib.c
+++ b/UefiCpuPkg/Library/RegisterCpuFeaturesLib/PeiRegisterCpuFeaturesLib.c
@@ -1,7 +1,7 @@
 /** @file
   CPU Register Table Library functions.
 
-  Copyright (c) 2016, Intel Corporation. All rights reserved.<BR>
+  Copyright (c) 2016 - 2019, Intel Corporation. All rights reserved.<BR>
   SPDX-License-Identifier: BSD-2-Clause-Patent
 
 **/
@@ -12,6 +12,8 @@
 #include <Library/PeiServicesLib.h>
 #include <Library/PeiServicesTablePointerLib.h>
 #include <Ppi/MpServices.h>
+#include <Ppi/MpServices2.h>
+
 #include "RegisterCpuFeatures.h"
 
 #define REGISTER_CPU_FEATURES_GUID \
@@ -154,7 +156,7 @@ GetProcessorInformation (
 
 **/
 VOID
-StartupAPsWorker (
+StartupAllAPsWorker (
   IN  EFI_AP_PROCEDURE                 Procedure,
   IN  EFI_EVENT                        MpEvent
   )
@@ -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
+StartupAllCPUsWorker (
+  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);
+  StartupAllCPUsWorker (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
diff --git a/UefiCpuPkg/Library/RegisterCpuFeaturesLib/RegisterCpuFeatures.h b/UefiCpuPkg/Library/RegisterCpuFeaturesLib/RegisterCpuFeatures.h
index a18f926641..a25dd69292 100644
--- a/UefiCpuPkg/Library/RegisterCpuFeaturesLib/RegisterCpuFeatures.h
+++ b/UefiCpuPkg/Library/RegisterCpuFeaturesLib/RegisterCpuFeatures.h
@@ -145,7 +145,7 @@ GetProcessorInformation (
                                       to check whether procedure has done.
 **/
 VOID
-StartupAPsWorker (
+StartupAllAPsWorker (
   IN  EFI_AP_PROCEDURE                 Procedure,
   IN  EFI_EVENT                        MpEvent
   );
-- 
2.21.0.windows.1