[Patch v3 3/3] UefiCpuPkg/MpInitLib: Not use disabled AP when call StartAllAPs.

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

Base on UEFI spec requirement, StartAllAPs function should not
use the APs which has been disabled before. This patch just
change current code to follow this rule.

V3 changes:
Only called by StartUpAllAps, WakeUpAp will not wake up
the disabled APs, in other cases also need to include the
disabled APs, such as CpuDxe driver start up and
ChangeApLoopCallback function.

Cc: Laszlo Ersek <lersek@redhat.com>
Cc: Ruiyu Ni <ruiyu.ni@intel.com>
Contributed-under: TianoCore Contribution Agreement 1.1
Signed-off-by: Eric Dong <eric.dong@intel.com>
---
 UefiCpuPkg/Library/MpInitLib/DxeMpLib.c |  2 +-
 UefiCpuPkg/Library/MpInitLib/MpLib.c    | 27 +++++++++++++++++----------
 UefiCpuPkg/Library/MpInitLib/MpLib.h    |  4 +++-
 3 files changed, 21 insertions(+), 12 deletions(-)

diff --git a/UefiCpuPkg/Library/MpInitLib/DxeMpLib.c b/UefiCpuPkg/Library/MpInitLib/DxeMpLib.c
index d82e9aea45..c17daa0fc0 100644
--- a/UefiCpuPkg/Library/MpInitLib/DxeMpLib.c
+++ b/UefiCpuPkg/Library/MpInitLib/DxeMpLib.c
@@ -306,7 +306,7 @@ MpInitChangeApLoopCallback (
   CpuMpData->PmCodeSegment = GetProtectedModeCS ();
   CpuMpData->ApLoopMode = PcdGet8 (PcdCpuApLoopMode);
   mNumberToFinish = CpuMpData->CpuCount - 1;
-  WakeUpAP (CpuMpData, TRUE, 0, RelocateApLoop, NULL);
+  WakeUpAP (CpuMpData, TRUE, 0, RelocateApLoop, NULL, TRUE);
   while (mNumberToFinish > 0) {
     CpuPause ();
   }
diff --git a/UefiCpuPkg/Library/MpInitLib/MpLib.c b/UefiCpuPkg/Library/MpInitLib/MpLib.c
index 0e57cc86bf..1a81062a3f 100644
--- a/UefiCpuPkg/Library/MpInitLib/MpLib.c
+++ b/UefiCpuPkg/Library/MpInitLib/MpLib.c
@@ -470,7 +470,7 @@ CollectProcessorCount (
   //
   CpuMpData->InitFlag     = ApInitConfig;
   CpuMpData->X2ApicEnable = FALSE;
-  WakeUpAP (CpuMpData, TRUE, 0, NULL, NULL);
+  WakeUpAP (CpuMpData, TRUE, 0, NULL, NULL, TRUE);
   CpuMpData->InitFlag = ApInitDone;
   ASSERT (CpuMpData->CpuCount <= PcdGet32 (PcdCpuMaxLogicalProcessorNumber));
   //
@@ -491,7 +491,7 @@ CollectProcessorCount (
     //
     // Wakeup all APs to enable x2APIC mode
     //
-    WakeUpAP (CpuMpData, TRUE, 0, ApFuncEnableX2Apic, NULL);
+    WakeUpAP (CpuMpData, TRUE, 0, ApFuncEnableX2Apic, NULL, TRUE);
     //
     // Wait for all known APs finished
     //
@@ -969,6 +969,7 @@ FreeResetVector (
   @param[in] ProcessorNumber    The handle number of specified processor
   @param[in] Procedure          The function to be invoked by AP
   @param[in] ProcedureArgument  The argument to be passed into AP function
+  @param[in] WakeUpDisabledAps  Whether need to wake up disabled APs in broadcast mode.
 **/
 VOID
 WakeUpAP (
@@ -976,7 +977,8 @@ WakeUpAP (
   IN BOOLEAN                   Broadcast,
   IN UINTN                     ProcessorNumber,
   IN EFI_AP_PROCEDURE          Procedure,              OPTIONAL
-  IN VOID                      *ProcedureArgument      OPTIONAL
+  IN VOID                      *ProcedureArgument,     OPTIONAL
+  IN BOOLEAN                   WakeUpDisabledAps
   )
 {
   volatile MP_CPU_EXCHANGE_INFO    *ExchangeInfo;
@@ -1010,6 +1012,10 @@ WakeUpAP (
     for (Index = 0; Index < CpuMpData->CpuCount; Index++) {
       if (Index != CpuMpData->BspNumber) {
         CpuData = &CpuMpData->CpuData[Index];
+        if (GetApState (CpuData) == CpuStateDisabled && !WakeUpDisabledAps) {
+          continue;
+        }
+
         CpuData->ApFunction         = (UINTN) Procedure;
         CpuData->ApFunctionArgument = (UINTN) ProcedureArgument;
         SetApState (CpuData, CpuStateReady);
@@ -1289,7 +1295,7 @@ ResetProcessorToIdleState (
   CpuMpData = GetCpuMpData ();
 
   CpuMpData->InitFlag = ApInitReconfig;
-  WakeUpAP (CpuMpData, FALSE, ProcessorNumber, NULL, NULL);
+  WakeUpAP (CpuMpData, FALSE, ProcessorNumber, NULL, NULL, TRUE);
   while (CpuMpData->FinishedCount < 1) {
     CpuPause ();
   }
@@ -1439,7 +1445,8 @@ CheckAllAPs (
             FALSE,
             (UINT32) NextProcessorNumber,
             CpuMpData->Procedure,
-            CpuMpData->ProcArguments
+            CpuMpData->ProcArguments,
+            TRUE
             );
          }
       }
@@ -1711,7 +1718,7 @@ MpInitLibInitialize (
       //
       // Wakeup APs to do some AP initialize sync
       //
-      WakeUpAP (CpuMpData, TRUE, 0, ApInitializeSync, CpuMpData);
+      WakeUpAP (CpuMpData, TRUE, 0, ApInitializeSync, CpuMpData, TRUE);
       //
       // Wait for all APs finished initialization
       //
@@ -1906,7 +1913,7 @@ SwitchBSPWorker (
   //
   // Need to wakeUp AP (future BSP).
   //
-  WakeUpAP (CpuMpData, FALSE, ProcessorNumber, FutureBSPProc, CpuMpData);
+  WakeUpAP (CpuMpData, FALSE, ProcessorNumber, FutureBSPProc, CpuMpData, TRUE);
 
   AsmExchangeRole (&CpuMpData->BSPInfo, &CpuMpData->APInfo);
 
@@ -2240,14 +2247,14 @@ StartupAllAPsWorker (
   CpuMpData->WaitEvent     = WaitEvent;
 
   if (!SingleThread) {
-    WakeUpAP (CpuMpData, TRUE, 0, Procedure, ProcedureArgument);
+    WakeUpAP (CpuMpData, TRUE, 0, Procedure, ProcedureArgument, FALSE);
   } else {
     for (ProcessorNumber = 0; ProcessorNumber < ProcessorCount; ProcessorNumber++) {
       if (ProcessorNumber == CallerNumber) {
         continue;
       }
       if (CpuMpData->CpuData[ProcessorNumber].Waiting) {
-        WakeUpAP (CpuMpData, FALSE, ProcessorNumber, Procedure, ProcedureArgument);
+        WakeUpAP (CpuMpData, FALSE, ProcessorNumber, Procedure, ProcedureArgument, TRUE);
         break;
       }
     }
@@ -2359,7 +2366,7 @@ StartupThisAPWorker (
   CpuData->ExpectedTime = CalculateTimeout (TimeoutInMicroseconds, &CpuData->CurrentTime);
   CpuData->TotalTime    = 0;
 
-  WakeUpAP (CpuMpData, FALSE, ProcessorNumber, Procedure, ProcedureArgument);
+  WakeUpAP (CpuMpData, FALSE, ProcessorNumber, Procedure, ProcedureArgument, TRUE);
 
   //
   // If WaitEvent is NULL, execute in blocking mode.
diff --git a/UefiCpuPkg/Library/MpInitLib/MpLib.h b/UefiCpuPkg/Library/MpInitLib/MpLib.h
index 5002b7e9c0..fe7a917e49 100644
--- a/UefiCpuPkg/Library/MpInitLib/MpLib.h
+++ b/UefiCpuPkg/Library/MpInitLib/MpLib.h
@@ -373,6 +373,7 @@ GetModeTransitionBuffer (
   @param[in] ProcessorNumber    The handle number of specified processor
   @param[in] Procedure          The function to be invoked by AP
   @param[in] ProcedureArgument  The argument to be passed into AP function
+  @param[in] WakeUpDisabledAps  Whether need to wake up disabled APs in broadcast mode.
 **/
 VOID
 WakeUpAP (
@@ -380,7 +381,8 @@ WakeUpAP (
   IN BOOLEAN                   Broadcast,
   IN UINTN                     ProcessorNumber,
   IN EFI_AP_PROCEDURE          Procedure,              OPTIONAL
-  IN VOID                      *ProcedureArgument      OPTIONAL
+  IN VOID                      *ProcedureArgument,     OPTIONAL
+  IN BOOLEAN                   WakeUpDisabledAps       OPTIONAL
   );
 
 /**
-- 
2.15.0.windows.1