UefiCpuPkg/MpInitLib: Sort processor by ascending order of APIC ID

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

diff --git a/UefiCpuPkg/Library/MpInitLib/MpLib.c b/UefiCpuPkg/Library/MpInitLib/MpLib.c
index de169e68501efd89be46be7da314a69b9552be66..d6eef138ac13f0ff82a0452385be564e78602a5b 100644 (file)
--- a/UefiCpuPkg/Library/MpInitLib/MpLib.c
+++ b/UefiCpuPkg/Library/MpInitLib/MpLib.c
@@ -216,6 +216,87 @@ GetApLoopMode (
   return ApLoopMode;\r
 }\r
 \r
+/**\r
+  Sort the APIC ID of all processors.\r
+\r
+  This function sorts the APIC ID of all processors so that processor number is\r
+  assigned in the ascending order of APIC ID which eases MP debugging.\r
+\r
+  @param[in] CpuMpData        Pointer to PEI CPU MP Data\r
+**/\r
+VOID\r
+SortApicId (\r
+  IN CPU_MP_DATA   *CpuMpData\r
+  )\r
+{\r
+  UINTN             Index1;\r
+  UINTN             Index2;\r
+  UINTN             Index3;\r
+  UINT32            ApicId;\r
+  CPU_AP_DATA       CpuData;\r
+  UINT32            ApCount;\r
+  CPU_INFO_IN_HOB   *CpuInfoInHob;\r
+\r
+  ApCount = CpuMpData->CpuCount - 1;\r
+\r
+  if (ApCount != 0) {\r
+    for (Index1 = 0; Index1 < ApCount; Index1++) {\r
+      Index3 = Index1;\r
+      //\r
+      // Sort key is the hardware default APIC ID\r
+      //\r
+      ApicId = CpuMpData->CpuData[Index1].ApicId;\r
+      for (Index2 = Index1 + 1; Index2 <= ApCount; Index2++) {\r
+        if (ApicId > CpuMpData->CpuData[Index2].ApicId) {\r
+          Index3 = Index2;\r
+          ApicId = CpuMpData->CpuData[Index2].ApicId;\r
+        }\r
+      }\r
+      if (Index3 != Index1) {\r
+        CopyMem (&CpuData, &CpuMpData->CpuData[Index3], sizeof (CPU_AP_DATA));\r
+        CopyMem (\r
+          &CpuMpData->CpuData[Index3],\r
+          &CpuMpData->CpuData[Index1],\r
+          sizeof (CPU_AP_DATA)\r
+          );\r
+        CopyMem (&CpuMpData->CpuData[Index1], &CpuData, sizeof (CPU_AP_DATA));\r
+      }\r
+    }\r
+\r
+    //\r
+    // Get the processor number for the BSP\r
+    //\r
+    ApicId = GetInitialApicId ();\r
+    for (Index1 = 0; Index1 < CpuMpData->CpuCount; Index1++) {\r
+      if (CpuMpData->CpuData[Index1].ApicId == ApicId) {\r
+        CpuMpData->BspNumber = (UINT32) Index1;\r
+        break;\r
+      }\r
+    }\r
+\r
+    CpuInfoInHob = (CPU_INFO_IN_HOB *) (UINTN) CpuMpData->CpuInfoInHob;\r
+    for (Index1 = 0; Index1 < CpuMpData->CpuCount; Index1++) {\r
+      CpuInfoInHob[Index1].InitialApicId = CpuMpData->CpuData[Index1].InitialApicId;\r
+      CpuInfoInHob[Index1].ApicId        = CpuMpData->CpuData[Index1].ApicId;\r
+      CpuInfoInHob[Index1].Health        = CpuMpData->CpuData[Index1].Health;\r
+    }\r
+  }\r
+}\r
+\r
+/**\r
+  Enable x2APIC mode on APs.\r
+\r
+  @param[in, out] Buffer  Pointer to private data buffer.\r
+**/\r
+VOID\r
+EFIAPI\r
+ApFuncEnableX2Apic (\r
+  IN OUT VOID  *Buffer\r
+  )\r
+{\r
+  SetApicMode (LOCAL_APIC_MODE_X2APIC);\r
+}\r
+\r
 /**\r
   Do sync on APs.\r
 \r
@@ -268,6 +349,65 @@ GetProcessorNumber (
   return EFI_NOT_FOUND;\r
 }\r
 \r
+/**\r
+  This function will get CPU count in the system.\r
+\r
+  @param[in] CpuMpData        Pointer to PEI CPU MP Data\r
+\r
+  @return  CPU count detected\r
+**/\r
+UINTN\r
+CollectProcessorCount (\r
+  IN CPU_MP_DATA         *CpuMpData\r
+  )\r
+{\r
+  //\r
+  // Send 1st broadcast IPI to APs to wakeup APs\r
+  //\r
+  CpuMpData->InitFlag     = ApInitConfig;\r
+  CpuMpData->X2ApicEnable = FALSE;\r
+  WakeUpAP (CpuMpData, TRUE, 0, NULL, NULL);\r
+  //\r
+  // Wait for AP task to complete and then exit.\r
+  //\r
+  MicroSecondDelay (PcdGet32(PcdCpuApInitTimeOutInMicroSeconds));\r
+  CpuMpData->InitFlag = ApInitDone;\r
+  ASSERT (CpuMpData->CpuCount <= PcdGet32 (PcdCpuMaxLogicalProcessorNumber));\r
+  //\r
+  // Wait for all APs finished the initialization\r
+  //\r
+  while (CpuMpData->FinishedCount < (CpuMpData->CpuCount - 1)) {\r
+    CpuPause ();\r
+  }\r
+\r
+  if (CpuMpData->X2ApicEnable) {\r
+    DEBUG ((DEBUG_INFO, "Force x2APIC mode!\n"));\r
+    //\r
+    // Wakeup all APs to enable x2APIC mode\r
+    //\r
+    WakeUpAP (CpuMpData, TRUE, 0, ApFuncEnableX2Apic, NULL);\r
+    //\r
+    // Wait for all known APs finished\r
+    //\r
+    while (CpuMpData->FinishedCount < (CpuMpData->CpuCount - 1)) {\r
+      CpuPause ();\r
+    }\r
+    //\r
+    // Enable x2APIC on BSP\r
+    //\r
+    SetApicMode (LOCAL_APIC_MODE_X2APIC);\r
+  }\r
+  DEBUG ((DEBUG_INFO, "APIC MODE is %d\n", GetApicMode ()));\r
+  //\r
+  // Sort BSP/Aps by CPU APIC ID in ascending order\r
+  //\r
+  SortApicId (CpuMpData);\r
+\r
+  DEBUG ((DEBUG_INFO, "MpInitLib: Find %d processors in system.\n", CpuMpData->CpuCount));\r
+\r
+  return CpuMpData->CpuCount;\r
+}\r
+\r
 /*\r
   Initialize CPU AP Data when AP is wakeup at the first time.\r
 \r
@@ -704,6 +844,10 @@ MpInitLibInitialize (
   MtrrGetAllMtrrs (&CpuMpData->MtrrTable);\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