From f8e4e86bc5705340648724de35890fba56331a12 Mon Sep 17 00:00:00 2001
From: Jeff Fan <jeff.fan@intel.com>
Date: Wed, 15 Jul 2015 03:38:57 +0000
Subject: [PATCH] UefiCpuPkg/CpuMpPei: Sort APIC ID in ascending order

Contributed-under: TianoCore Contribution Agreement 1.0
Signed-off-by: Jeff Fan <jeff.fan@intel.com>
Reviewed-by: Feng Tian <feng.tian@intel.com>
Reviewed-by: Jiewen Yao <jiewen.yao@intel.com>

git-svn-id: https://svn.code.sf.net/p/edk2/code/trunk/edk2@17998 6f19259b-4bc3-4df7-8a09-765794883524
---
 UefiCpuPkg/CpuMpPei/CpuMpPei.c | 60 ++++++++++++++++++++++++++++++++++
 1 file changed, 60 insertions(+)

diff --git a/UefiCpuPkg/CpuMpPei/CpuMpPei.c b/UefiCpuPkg/CpuMpPei/CpuMpPei.c
index 8367f0573f..a2f25b9405 100644
--- a/UefiCpuPkg/CpuMpPei/CpuMpPei.c
+++ b/UefiCpuPkg/CpuMpPei/CpuMpPei.c
@@ -38,6 +38,62 @@ GLOBAL_REMOVE_IF_UNREFERENCED IA32_DESCRIPTOR mGdt = {
   (UINTN) mGdtEntries
   };
 
+/**
+  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 PeiCpuMpData        Pointer to PEI CPU MP Data
+**/
+VOID
+SortApicId (
+  IN PEI_CPU_MP_DATA   *PeiCpuMpData
+  )
+{
+  UINTN             Index1;
+  UINTN             Index2;
+  UINTN             Index3;
+  UINT32            ApicId;
+  EFI_HEALTH_FLAGS  Health;
+  UINT32            ApCount;
+
+  ApCount = PeiCpuMpData->CpuCount - 1;
+
+  if (ApCount != 0) {
+    for (Index1 = 0; Index1 < ApCount; Index1++) {
+      Index3 = Index1;
+      //
+      // Sort key is the hardware default APIC ID
+      //
+      ApicId = PeiCpuMpData->CpuData[Index1].ApicId;
+      for (Index2 = Index1 + 1; Index2 <= ApCount; Index2++) {
+        if (ApicId > PeiCpuMpData->CpuData[Index2].ApicId) {
+          Index3 = Index2;
+          ApicId = PeiCpuMpData->CpuData[Index2].ApicId;
+        }
+      }
+      if (Index3 != Index1) {
+        PeiCpuMpData->CpuData[Index3].ApicId = PeiCpuMpData->CpuData[Index1].ApicId;
+        PeiCpuMpData->CpuData[Index1].ApicId = ApicId;
+        Health = PeiCpuMpData->CpuData[Index3].Health;
+        PeiCpuMpData->CpuData[Index3].Health = PeiCpuMpData->CpuData[Index1].Health;
+        PeiCpuMpData->CpuData[Index1].Health = Health;
+      }
+    }
+
+    //
+    // Get the processor number for the BSP
+    //
+    ApicId = GetInitialApicId ();
+    for (Index1 = 0; Index1 < PeiCpuMpData->CpuCount; Index1++) {
+      if (PeiCpuMpData->CpuData[Index1].ApicId == ApicId) {
+        PeiCpuMpData->BspNumber = (UINT32) Index1;
+        break;
+      }
+    }
+  }
+}
 /**
   This function will be called from AP reset code if BSP uses WakeUpAP.
 
@@ -240,6 +296,10 @@ CountProcessorNumber (
   MicroSecondDelay (PcdGet32 (PcdCpuApInitTimeOutInMicroSeconds));
   PeiCpuMpData->InitFlag  = 0;
   PeiCpuMpData->CpuCount += (UINT32) PeiCpuMpData->MpCpuExchangeInfo->NumApsExecuting;
+  //
+  // Sort BSP/Aps by CPU APIC ID in ascending order
+  //
+  SortApicId (PeiCpuMpData);
 
   DEBUG ((EFI_D_INFO, "CpuMpPei: Find %d processors in system.\n", PeiCpuMpData->CpuCount));
   return PeiCpuMpData->CpuCount;
-- 
2.39.2