From 8a2d564b2a811b8dbc85f90e14a67ae4ade21201 Mon Sep 17 00:00:00 2001
From: Jeff Fan <jeff.fan@intel.com>
Date: Thu, 21 Jul 2016 00:31:36 +0800
Subject: [PATCH] UefiCpuPkg/MpInitLib: Sort processor by ascending order of
 APIC ID

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 | 72 ++++++++++++++++++++++++++++
 1 file changed, 72 insertions(+)

diff --git a/UefiCpuPkg/Library/MpInitLib/MpLib.c b/UefiCpuPkg/Library/MpInitLib/MpLib.c
index d84dfec506..d6eef138ac 100644
--- a/UefiCpuPkg/Library/MpInitLib/MpLib.c
+++ b/UefiCpuPkg/Library/MpInitLib/MpLib.c
@@ -216,6 +216,73 @@ GetApLoopMode (
   return ApLoopMode;
 }
 
+/**
+  Sort the APIC ID of all processors.
+
+  This function sorts the APIC ID of all processors so that processor number is
+  assigned in the ascending order of APIC ID which eases MP debugging.
+
+  @param[in] CpuMpData        Pointer to PEI CPU MP Data
+**/
+VOID
+SortApicId (
+  IN CPU_MP_DATA   *CpuMpData
+  )
+{
+  UINTN             Index1;
+  UINTN             Index2;
+  UINTN             Index3;
+  UINT32            ApicId;
+  CPU_AP_DATA       CpuData;
+  UINT32            ApCount;
+  CPU_INFO_IN_HOB   *CpuInfoInHob;
+
+  ApCount = CpuMpData->CpuCount - 1;
+
+  if (ApCount != 0) {
+    for (Index1 = 0; Index1 < ApCount; Index1++) {
+      Index3 = Index1;
+      //
+      // Sort key is the hardware default APIC ID
+      //
+      ApicId = CpuMpData->CpuData[Index1].ApicId;
+      for (Index2 = Index1 + 1; Index2 <= ApCount; Index2++) {
+        if (ApicId > CpuMpData->CpuData[Index2].ApicId) {
+          Index3 = Index2;
+          ApicId = CpuMpData->CpuData[Index2].ApicId;
+        }
+      }
+      if (Index3 != Index1) {
+        CopyMem (&CpuData, &CpuMpData->CpuData[Index3], sizeof (CPU_AP_DATA));
+        CopyMem (
+          &CpuMpData->CpuData[Index3],
+          &CpuMpData->CpuData[Index1],
+          sizeof (CPU_AP_DATA)
+          );
+        CopyMem (&CpuMpData->CpuData[Index1], &CpuData, sizeof (CPU_AP_DATA));
+      }
+    }
+
+    //
+    // Get the processor number for the BSP
+    //
+    ApicId = GetInitialApicId ();
+    for (Index1 = 0; Index1 < CpuMpData->CpuCount; Index1++) {
+      if (CpuMpData->CpuData[Index1].ApicId == ApicId) {
+        CpuMpData->BspNumber = (UINT32) Index1;
+        break;
+      }
+    }
+
+    CpuInfoInHob = (CPU_INFO_IN_HOB *) (UINTN) CpuMpData->CpuInfoInHob;
+    for (Index1 = 0; Index1 < CpuMpData->CpuCount; Index1++) {
+      CpuInfoInHob[Index1].InitialApicId = CpuMpData->CpuData[Index1].InitialApicId;
+      CpuInfoInHob[Index1].ApicId        = CpuMpData->CpuData[Index1].ApicId;
+      CpuInfoInHob[Index1].Health        = CpuMpData->CpuData[Index1].Health;
+    }
+  }
+}
+
 /**
   Enable x2APIC mode on APs.
 
@@ -331,6 +398,11 @@ CollectProcessorCount (
     SetApicMode (LOCAL_APIC_MODE_X2APIC);
   }
   DEBUG ((DEBUG_INFO, "APIC MODE is %d\n", GetApicMode ()));
+  //
+  // Sort BSP/Aps by CPU APIC ID in ascending order
+  //
+  SortApicId (CpuMpData);
+
   DEBUG ((DEBUG_INFO, "MpInitLib: Find %d processors in system.\n", CpuMpData->CpuCount));
 
   return CpuMpData->CpuCount;