[Patch v5 24/48] UefiCpuPkg/MpInitLib: Enable x2APIC mode on BSP/APs

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

If x2APIC flag is set, enable x2APIC mode on all APs and BSP. Before we wakeup
APs to enable x2APIC mode, we should wait all APs have finished initialization.

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/MpLib.c | 32 ++++++++++++++++++++++++++++++++
 1 file changed, 32 insertions(+)

diff --git a/UefiCpuPkg/Library/MpInitLib/MpLib.c b/UefiCpuPkg/Library/MpInitLib/MpLib.c
index 8f7cf43..d84dfec 100644
--- a/UefiCpuPkg/Library/MpInitLib/MpLib.c
+++ b/UefiCpuPkg/Library/MpInitLib/MpLib.c
@@ -217,6 +217,20 @@ GetApLoopMode (
 }
 
 /**
+  Enable x2APIC mode on APs.
+
+  @param[in, out] Buffer  Pointer to private data buffer.
+**/
+VOID
+EFIAPI
+ApFuncEnableX2Apic (
+  IN OUT VOID  *Buffer
+  )
+{
+  SetApicMode (LOCAL_APIC_MODE_X2APIC);
+}
+
+/**
   Do sync on APs.
 
   @param[in, out] Buffer  Pointer to private data buffer.
@@ -299,6 +313,24 @@ CollectProcessorCount (
     CpuPause ();
   }
 
+  if (CpuMpData->X2ApicEnable) {
+    DEBUG ((DEBUG_INFO, "Force x2APIC mode!\n"));
+    //
+    // Wakeup all APs to enable x2APIC mode
+    //
+    WakeUpAP (CpuMpData, TRUE, 0, ApFuncEnableX2Apic, NULL);
+    //
+    // Wait for all known APs finished
+    //
+    while (CpuMpData->FinishedCount < (CpuMpData->CpuCount - 1)) {
+      CpuPause ();
+    }
+    //
+    // Enable x2APIC on BSP
+    //
+    SetApicMode (LOCAL_APIC_MODE_X2APIC);
+  }
+  DEBUG ((DEBUG_INFO, "APIC MODE is %d\n", GetApicMode ()));
   DEBUG ((DEBUG_INFO, "MpInitLib: Find %d processors in system.\n", CpuMpData->CpuCount));
 
   return CpuMpData->CpuCount;
-- 
2.7.4.windows.1