X-Git-Url: https://git.proxmox.com/?p=mirror_edk2.git;a=blobdiff_plain;f=UefiCpuPkg%2FLibrary%2FMpInitLib%2FMpLib.c;h=32bbaee5cce0e43bc654e2eff161bf1da2fe75b4;hp=12bd04e151d5b246a0e1749c92279de8f5f59c25;hb=9ebcf0f4005239abcde5141414703e2676200550;hpb=3e8ad6bd7684502c6a57796a57fb7c6dbe3ec1c5

diff --git a/UefiCpuPkg/Library/MpInitLib/MpLib.c b/UefiCpuPkg/Library/MpInitLib/MpLib.c
index 12bd04e151..32bbaee5cc 100644
--- a/UefiCpuPkg/Library/MpInitLib/MpLib.c
+++ b/UefiCpuPkg/Library/MpInitLib/MpLib.c
@@ -15,6 +15,65 @@
 #include "MpLib.h"
 
 
+/**
+  Detect whether Mwait-monitor feature is supported.
+
+  @retval TRUE    Mwait-monitor feature is supported.
+  @retval FALSE   Mwait-monitor feature is not supported.
+**/
+BOOLEAN
+IsMwaitSupport (
+  VOID
+  )
+{
+  CPUID_VERSION_INFO_ECX        VersionInfoEcx;
+
+  AsmCpuid (CPUID_VERSION_INFO, NULL, NULL, &VersionInfoEcx.Uint32, NULL);
+  return (VersionInfoEcx.Bits.MONITOR == 1) ? TRUE : FALSE;
+}
+
+/**
+  Get AP loop mode.
+
+  @param[out] MonitorFilterSize  Returns the largest monitor-line size in bytes.
+
+  @return The AP loop mode.
+**/
+UINT8
+GetApLoopMode (
+  OUT UINT32     *MonitorFilterSize
+  )
+{
+  UINT8                         ApLoopMode;
+  CPUID_MONITOR_MWAIT_EBX       MonitorMwaitEbx;
+
+  ASSERT (MonitorFilterSize != NULL);
+
+  ApLoopMode = PcdGet8 (PcdCpuApLoopMode);
+  ASSERT (ApLoopMode >= ApInHltLoop && ApLoopMode <= ApInRunLoop);
+  if (ApLoopMode == ApInMwaitLoop) {
+    if (!IsMwaitSupport ()) {
+      //
+      // If processor does not support MONITOR/MWAIT feature,
+      // force AP in Hlt-loop mode
+      //
+      ApLoopMode = ApInHltLoop;
+    }
+  }
+
+  if (ApLoopMode != ApInMwaitLoop) {
+    *MonitorFilterSize = sizeof (UINT32);
+  } else {
+    //
+    // CPUID.[EAX=05H]:EBX.BIT0-15: Largest monitor-line size in bytes
+    // CPUID.[EAX=05H].EDX: C-states supported using MWAIT
+    //
+    AsmCpuid (CPUID_MONITOR_MWAIT, NULL, &MonitorMwaitEbx.Uint32, NULL, NULL);
+    *MonitorFilterSize = MonitorMwaitEbx.Bits.LargestMonitorLineSize;
+  }
+
+  return ApLoopMode;
+}
 /**
   MP Initialize Library initialization.
 
@@ -34,7 +93,16 @@ MpInitLibInitialize (
   VOID
   )
 {
-  return EFI_UNSUPPORTED;
+  MP_ASSEMBLY_ADDRESS_MAP  AddressMap;
+  UINT32                   MonitorFilterSize;
+  UINT8                    ApLoopMode;
+  UINTN                    ApResetVectorSize;
+
+  AsmGetAddressMap (&AddressMap);
+  ApResetVectorSize = AddressMap.RendezvousFunnelSize + sizeof (MP_CPU_EXCHANGE_INFO);
+  ApLoopMode  = GetApLoopMode (&MonitorFilterSize);
+
+  return EFI_SUCCESS;
 }
 
 /**