From fe627769c949d368ac048053ec21596cb1c51390 Mon Sep 17 00:00:00 2001
From: Jeff Fan <jeff.fan@intel.com>
Date: Thu, 21 Jul 2016 00:29:49 +0800
Subject: [PATCH] UefiCpuPkg/MpInitLib: Enable x2APIC mode on BSP/APs

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>
Reviewed-by: Michael Kinney <michael.d.kinney@intel.com>
Tested-by: Laszlo Ersek <lersek@redhat.com>
Tested-by: Michael Kinney <michael.d.kinney@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 8f7cf43a7c..d84dfec506 100644
--- a/UefiCpuPkg/Library/MpInitLib/MpLib.c
+++ b/UefiCpuPkg/Library/MpInitLib/MpLib.c
@@ -216,6 +216,20 @@ GetApLoopMode (
   return ApLoopMode;
 }
 
+/**
+  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.
 
@@ -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.39.2