UefiCpuPkg/MpInitLib: Implementation of MpInitLibGetNumberOfProcessors()

[mirror_edk2.git] / UefiCpuPkg / Library / MpInitLib / MpLib.c

diff --git a/UefiCpuPkg/Library/MpInitLib/MpLib.c b/UefiCpuPkg/Library/MpInitLib/MpLib.c
index d6eef138ac13f0ff82a0452385be564e78602a5b..cbaeccc64729ae6a2eda78b8c01fa7c004c83ce7 100644 (file)
--- a/UefiCpuPkg/Library/MpInitLib/MpLib.c
+++ b/UefiCpuPkg/Library/MpInitLib/MpLib.c
@@ -765,6 +765,8 @@ MpInitLibInitialize (
   VOID\r
   )\r
 {\r
+  CPU_MP_DATA              *OldCpuMpData;\r
+  CPU_INFO_IN_HOB          *CpuInfoInHob;\r
   UINT32                   MaxLogicalProcessorNumber;\r
   UINT32                   ApStackSize;\r
   MP_ASSEMBLY_ADDRESS_MAP  AddressMap;\r
@@ -778,7 +780,13 @@ MpInitLibInitialize (
   UINTN                    Index;\r
   UINTN                    ApResetVectorSize;\r
   UINTN                    BackupBufferAddr;\r
-  MaxLogicalProcessorNumber = PcdGet32(PcdCpuMaxLogicalProcessorNumber);\r
+\r
+  OldCpuMpData = GetCpuMpDataFromGuidedHob ();\r
+  if (OldCpuMpData == NULL) {\r
+    MaxLogicalProcessorNumber = PcdGet32(PcdCpuMaxLogicalProcessorNumber);\r
+  } else {\r
+    MaxLogicalProcessorNumber = OldCpuMpData->CpuCount;\r
+  }\r
 \r
   AsmGetAddressMap (&AddressMap);\r
   ApResetVectorSize = AddressMap.RendezvousFunnelSize + sizeof (MP_CPU_EXCHANGE_INFO);\r
@@ -843,11 +851,52 @@ MpInitLibInitialize (
   //\r
   MtrrGetAllMtrrs (&CpuMpData->MtrrTable);\r
 \r
+  if (OldCpuMpData == NULL) {\r
+    //\r
+    // Wakeup all APs and calculate the processor count in system\r
+    //\r
+    CollectProcessorCount (CpuMpData);\r
+  } else {\r
+    //\r
+    // APs have been wakeup before, just get the CPU Information\r
+    // from HOB\r
+    //\r
+    CpuMpData->CpuCount  = OldCpuMpData->CpuCount;\r
+    CpuMpData->BspNumber = OldCpuMpData->BspNumber;\r
+    CpuMpData->InitFlag  = ApInitReconfig;\r
+    CpuInfoInHob = (CPU_INFO_IN_HOB *) (UINTN) OldCpuMpData->CpuInfoInHob;\r
+    for (Index = 0; Index < CpuMpData->CpuCount; Index++) {\r
+      InitializeSpinLock(&CpuMpData->CpuData[Index].ApLock);\r
+      CpuMpData->CpuData[Index].ApicId        = CpuInfoInHob[Index].ApicId;\r
+      CpuMpData->CpuData[Index].InitialApicId = CpuInfoInHob[Index].InitialApicId;\r
+      if (CpuMpData->CpuData[Index].InitialApicId >= 255) {\r
+        CpuMpData->X2ApicEnable = TRUE;\r
+      }\r
+      CpuMpData->CpuData[Index].Health     = CpuInfoInHob[Index].Health;\r
+      CpuMpData->CpuData[Index].CpuHealthy = (CpuMpData->CpuData[Index].Health == 0)? TRUE:FALSE;\r
+      CpuMpData->CpuData[Index].ApFunction = 0;\r
+      CopyMem (\r
+        &CpuMpData->CpuData[Index].VolatileRegisters,\r
+        &CpuMpData->CpuData[0].VolatileRegisters,\r
+        sizeof (CPU_VOLATILE_REGISTERS)\r
+        );\r
+    }\r
+    //\r
+    // Wakeup APs to do some AP initialize sync\r
+    //\r
+    WakeUpAP (CpuMpData, TRUE, 0, ApInitializeSync, CpuMpData);\r
+    //\r
+    // Wait for all APs finished initialization\r
+    //\r
+    while (CpuMpData->FinishedCount < (CpuMpData->CpuCount - 1)) {\r
+      CpuPause ();\r
+    }\r
+    CpuMpData->InitFlag = ApInitDone;\r
+    for (Index = 0; Index < CpuMpData->CpuCount; Index++) {\r
+      SetApState (&CpuMpData->CpuData[Index], CpuStateIdle);\r
+    }\r
+  }\r
 \r
-  //\r
-  // Wakeup all APs and calculate the processor count in system\r
-  //\r
-  CollectProcessorCount (CpuMpData);\r
   //\r
   // Initialize global data for MP support\r
   //\r
@@ -907,6 +956,7 @@ MpInitLibWhoAmI (
 {\r
   return EFI_UNSUPPORTED;\r
 }\r
+\r
 /**\r
   Retrieves the number of logical processor in the platform and the number of\r
   those logical processors that are enabled on this boot. This service may only\r
@@ -934,8 +984,45 @@ MpInitLibGetNumberOfProcessors (
   OUT UINTN                     *NumberOfEnabledProcessors OPTIONAL\r
   )\r
 {\r
-  return EFI_UNSUPPORTED;\r
+  CPU_MP_DATA             *CpuMpData;\r
+  UINTN                   CallerNumber;\r
+  UINTN                   ProcessorNumber;\r
+  UINTN                   EnabledProcessorNumber;\r
+  UINTN                   Index;\r
+\r
+  CpuMpData = GetCpuMpData ();\r
+\r
+  if ((NumberOfProcessors == NULL) && (NumberOfEnabledProcessors == NULL)) {\r
+    return EFI_INVALID_PARAMETER;\r
+  }\r
+\r
+  //\r
+  // Check whether caller processor is BSP\r
+  //\r
+  MpInitLibWhoAmI (&CallerNumber);\r
+  if (CallerNumber != CpuMpData->BspNumber) {\r
+    return EFI_DEVICE_ERROR;\r
+  }\r
+\r
+  ProcessorNumber        = CpuMpData->CpuCount;\r
+  EnabledProcessorNumber = 0;\r
+  for (Index = 0; Index < ProcessorNumber; Index++) {\r
+    if (GetApState (&CpuMpData->CpuData[Index]) != CpuStateDisabled) {\r
+      EnabledProcessorNumber ++;\r
+    }\r
+  }\r
+\r
+  if (NumberOfProcessors != NULL) {\r
+    *NumberOfProcessors = ProcessorNumber;\r
+  }\r
+  if (NumberOfEnabledProcessors != NULL) {\r
+    *NumberOfEnabledProcessors = EnabledProcessorNumber;\r
+  }\r
+\r
+  return EFI_SUCCESS;\r
 }\r
+\r
+\r
 /**\r
   Get pointer to CPU MP Data structure from GUIDed HOB.\r
 \r