[Patch V2 6/6] UefiCpuPkg: Enhance MpHandOff Handling.

["Yuanhao Xie" <yuanhao.xie@intel.com>]

Enhance MpHandOff Handling for Systems with a Large Number of
Processors.

Cc: Gerd Hoffmann <kraxel@redhat.com>
Cc: Eric Dong <eric.dong@intel.com>
Cc: Ray Ni <ray.ni@intel.com>
Cc: Rahul Kumar <rahul1.kumar@intel.com>
Cc: Tom Lendacky <thomas.lendacky@amd.com>
Signed-off-by: Yuanhao Xie <yuanhao.xie@intel.com>
---
 UefiCpuPkg/Library/MpInitLib/MpLib.c    | 128 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++------------------------------------------------------------
 UefiCpuPkg/Library/MpInitLib/MpLib.h    |  22 +++++++++-------------
 UefiCpuPkg/Library/MpInitLib/PeiMpLib.c |  56 +++++++++++++++++++++++++++++++++++++++-----------------
 3 files changed, 116 insertions(+), 90 deletions(-)

diff --git a/UefiCpuPkg/Library/MpInitLib/MpLib.c b/UefiCpuPkg/Library/MpInitLib/MpLib.c
index 391e6f19ef..087bf3ea92 100644
--- a/UefiCpuPkg/Library/MpInitLib/MpLib.c
+++ b/UefiCpuPkg/Library/MpInitLib/MpLib.c
@@ -1528,32 +1528,48 @@ SwitchContextPerAp (
   This procedure allows the AP to switch to another section of
   memory and continue its loop there.
 
-  @param[in] MpHandOff  Pointer to MP hand-off data structure.
+  @param[in] BspNumber  Bsp Number
 **/
 VOID
 SwitchApContext (
-  IN MP_HAND_OFF  *MpHandOff
+  IN UINT32  BspNumber
   )
 {
-  UINTN   Index;
-  UINT32  BspNumber;
-
-  BspNumber = GetBspNumber (MpHandOff);
+  UINTN        Index;
+  UINTN        LimitationOfMpHandOffHob;
+  UINTN        NumberOfProcessorsInCurrentHob;
+  UINTN        CpuCountInCurrentHob;
+  MP_HAND_OFF  *MpHandOff;
+  BOOLEAN      BspFound;
+
+  BspFound = FALSE;
+
+  MpHandOff                = GetMpHandOffHob ();
+  CpuCountInCurrentHob     = 0;
+  LimitationOfMpHandOffHob = (0xFFF8 - sizeof (EFI_HOB_GUID_TYPE) - sizeof (MP_HAND_OFF)) / sizeof (PROCESSOR_HAND_OFF);
+  while (CpuCountInCurrentHob < MpHandOff->CpuCount) {
+    //
+    // Get the processor number for the BSP
+    //
+    NumberOfProcessorsInCurrentHob = MIN (MpHandOff->CpuCount - CpuCountInCurrentHob, LimitationOfMpHandOffHob);
 
-  for (Index = 0; Index < MpHandOff->CpuCount; Index++) {
-    if (Index != BspNumber) {
-      *(UINTN *)(UINTN)MpHandOff->Info[Index].StartupProcedureAddress = (UINTN)SwitchContextPerAp;
-      *(UINT32 *)(UINTN)MpHandOff->Info[Index].StartupSignalAddress   = MpHandOff->StartupSignalValue;
+    for (Index = 0; Index < NumberOfProcessorsInCurrentHob; Index++) {
+      if ((Index == BspNumber) && (BspFound == FALSE)) {
+        BspFound = TRUE;
+      } else {
+        *(UINTN *)(UINTN)MpHandOff->Info[Index].StartupProcedureAddress = (UINTN)SwitchContextPerAp;
+        *(UINT32 *)(UINTN)MpHandOff->Info[Index].StartupSignalAddress   = MpHandOff->StartupSignalValue;
+        WaitApWakeup ((UINT32 *)(UINTN)(MpHandOff->Info[Index].StartupSignalAddress));
+      }
     }
-  }
 
-  //
-  // Wait all APs waken up if this is not the 1st broadcast of SIPI
-  //
-  for (Index = 0; Index < MpHandOff->CpuCount; Index++) {
-    if (Index != BspNumber) {
-      WaitApWakeup ((UINT32 *)(UINTN)(MpHandOff->Info[Index].StartupSignalAddress));
+    if (CpuCountInCurrentHob + NumberOfProcessorsInCurrentHob >= MpHandOff->CpuCount) {
+      break;
     }
+
+    MpHandOff = (MP_HAND_OFF *)((UINT8 *)MpHandOff + sizeof (MP_HAND_OFF) + sizeof (EFI_HOB_GUID_TYPE) + sizeof (PROCESSOR_HAND_OFF) * NumberOfProcessorsInCurrentHob);
+
+    CpuCountInCurrentHob += NumberOfProcessorsInCurrentHob;
   }
 }
 
@@ -1909,37 +1925,6 @@ CheckAllAPs (
   return EFI_NOT_READY;
 }
 
-/**
-  This function Get BspNumber.
-
-  @param[in] MpHandOff        Pointer to MpHandOff
-  @return                     BspNumber
-**/
-UINT32
-GetBspNumber (
-  IN CONST MP_HAND_OFF  *MpHandOff
-  )
-{
-  UINT32  ApicId;
-  UINT32  BspNumber;
-  UINT32  Index;
-
-  //
-  // Get the processor number for the BSP
-  //
-  BspNumber = MAX_UINT32;
-  ApicId    = GetInitialApicId ();
-  for (Index = 0; Index < MpHandOff->CpuCount; Index++) {
-    if (MpHandOff->Info[Index].ApicId == ApicId) {
-      BspNumber = Index;
-    }
-  }
-
-  ASSERT (BspNumber != MAX_UINT32);
-
-  return BspNumber;
-}
-
 /**
   Get pointer to CPU MP Data structure from GUIDed HOB.
 
@@ -1994,8 +1979,13 @@ MpInitLibInitialize (
   UINTN                    ApResetVectorSizeAbove1Mb;
   UINTN                    BackupBufferAddr;
   UINTN                    ApIdtBase;
+  UINTN                    LimitationOfMpHandOffHob;
+  UINTN                    NumberOfProcessorsInCurrentHob;
+  UINTN                    ProcessedCpuCount;
+  UINTN                    CurrentProcessorIndex;
 
   MpHandOff = GetMpHandOffHob ();
+
   if (MpHandOff == NULL) {
     MaxLogicalProcessorNumber = PcdGet32 (PcdCpuMaxLogicalProcessorNumber);
   } else {
@@ -2156,17 +2146,35 @@ MpInitLibInitialize (
     // from HOB
     //
     AmdSevUpdateCpuMpData (CpuMpData);
-    CpuMpData->CpuCount  = MpHandOff->CpuCount;
-    CpuMpData->BspNumber = GetBspNumber (MpHandOff);
-    CpuInfoInHob         = (CPU_INFO_IN_HOB *)(UINTN)CpuMpData->CpuInfoInHob;
-    for (Index = 0; Index < CpuMpData->CpuCount; Index++) {
-      InitializeSpinLock (&CpuMpData->CpuData[Index].ApLock);
-      CpuMpData->CpuData[Index].CpuHealthy = (MpHandOff->Info[Index].Health == 0) ? TRUE : FALSE;
-      CpuMpData->CpuData[Index].ApFunction = 0;
-      CpuInfoInHob[Index].InitialApicId    = MpHandOff->Info[Index].ApicId;
-      CpuInfoInHob[Index].ApTopOfStack     = CpuMpData->Buffer + (Index + 1) * CpuMpData->CpuApStackSize;
-      CpuInfoInHob[Index].ApicId           = MpHandOff->Info[Index].ApicId;
-      CpuInfoInHob[Index].Health           = MpHandOff->Info[Index].Health;
+    CpuMpData->CpuCount = MpHandOff->CpuCount;
+    CpuInfoInHob        = (CPU_INFO_IN_HOB *)(UINTN)CpuMpData->CpuInfoInHob;
+
+    LimitationOfMpHandOffHob = (0xFFF8 - sizeof (EFI_HOB_GUID_TYPE) - sizeof (MP_HAND_OFF)) / sizeof (PROCESSOR_HAND_OFF);
+    ProcessedCpuCount        = 0;
+    while (ProcessedCpuCount < MpHandOff->CpuCount) {
+      NumberOfProcessorsInCurrentHob = MIN (MpHandOff->CpuCount - ProcessedCpuCount, LimitationOfMpHandOffHob);
+
+      for (Index = 0; Index < NumberOfProcessorsInCurrentHob; Index++) {
+        CurrentProcessorIndex = Index+MpHandOff->ProcessorIndex;
+        if (MpHandOff->Info[Index].ApicId == GetInitialApicId ()) {
+          CpuMpData->BspNumber = (UINT32)Index;
+        }
+
+        InitializeSpinLock (&CpuMpData->CpuData[CurrentProcessorIndex].ApLock);
+        CpuMpData->CpuData[CurrentProcessorIndex].CpuHealthy = (MpHandOff->Info[Index].Health == 0) ? TRUE : FALSE;
+        CpuMpData->CpuData[CurrentProcessorIndex].ApFunction = 0;
+        CpuInfoInHob[CurrentProcessorIndex].InitialApicId    = MpHandOff->Info[Index].ApicId;
+        CpuInfoInHob[CurrentProcessorIndex].ApTopOfStack     = CpuMpData->Buffer + (Index + 1) * CpuMpData->CpuApStackSize;
+        CpuInfoInHob[CurrentProcessorIndex].ApicId           = MpHandOff->Info[Index].ApicId;
+        CpuInfoInHob[CurrentProcessorIndex].Health           = MpHandOff->Info[Index].Health;
+      }
+
+      if (ProcessedCpuCount+NumberOfProcessorsInCurrentHob >= MpHandOff->CpuCount) {
+        break;
+      }
+
+      MpHandOff          = (MP_HAND_OFF *)((UINT8 *)MpHandOff + sizeof (MP_HAND_OFF) + sizeof (EFI_HOB_GUID_TYPE) + sizeof (PROCESSOR_HAND_OFF) * NumberOfProcessorsInCurrentHob);
+      ProcessedCpuCount += NumberOfProcessorsInCurrentHob;
     }
 
     DEBUG ((DEBUG_INFO, "MpHandOff->WaitLoopExecutionMode: %04d, sizeof (VOID *): %04d\n", MpHandOff->WaitLoopExecutionMode, sizeof (VOID *)));
@@ -2183,7 +2191,7 @@ MpInitLibInitialize (
       CpuMpData->FinishedCount = 0;
       CpuMpData->InitFlag      = ApInitDone;
       SaveCpuMpData (CpuMpData);
-      SwitchApContext (MpHandOff);
+      SwitchApContext (CpuMpData->BspNumber);
       ASSERT (CpuMpData->FinishedCount == (CpuMpData->CpuCount - 1));
 
       //
diff --git a/UefiCpuPkg/Library/MpInitLib/MpLib.h b/UefiCpuPkg/Library/MpInitLib/MpLib.h
index 468d3a5925..6af635a80c 100644
--- a/UefiCpuPkg/Library/MpInitLib/MpLib.h
+++ b/UefiCpuPkg/Library/MpInitLib/MpLib.h
@@ -497,17 +497,6 @@ SaveCpuMpData (
   IN CPU_MP_DATA  *CpuMpData
   );
 
-/**
-  This function Get BspNumber.
-
-  @param[in] MpHandOff        Pointer to MpHandOff
-  @return                     BspNumber
-**/
-UINT32
-GetBspNumber (
-  IN CONST MP_HAND_OFF  *MpHandOff
-  );
-
 /**
   Get available system memory below 1MB by specified size.
 
@@ -522,12 +511,19 @@ GetWakeupBuffer (
   );
 
 /**
-  Switch Context for each AP.
+  This function is intended to be invoked by the BSP in order
+  to wake up the AP. The BSP accomplishes this by triggering a
+  start-up signal, which in turn causes any APs that are
+  currently in a loop on the PEI-prepared memory to awaken and
+  begin running the procedure called SwitchContextPerAp.
+  This procedure allows the AP to switch to another section of
+  memory and continue its loop there.
 
+  @param[in] BspNumber  Bsp Number
 **/
 VOID
 SwitchApContext (
-  IN MP_HAND_OFF  *MpHandOff
+  IN UINT32  BspNumber
   );
 
 /**
diff --git a/UefiCpuPkg/Library/MpInitLib/PeiMpLib.c b/UefiCpuPkg/Library/MpInitLib/PeiMpLib.c
index f80e00edcf..18f83d9ed5 100644
--- a/UefiCpuPkg/Library/MpInitLib/PeiMpLib.c
+++ b/UefiCpuPkg/Library/MpInitLib/PeiMpLib.c
@@ -131,31 +131,53 @@ SaveCpuMpData (
   CPU_INFO_IN_HOB  *CpuInfoInHob;
   MP_HAND_OFF      *MpHandOff;
   UINTN            MpHandOffSize;
+  UINT32           LimitationOfMpHandOffHob;
+  UINT32           NumberOfProcessorsInCurrentHob;
+  UINT32           ProcessedCpuCount;
+  UINTN            CurrentProcessorIndex;
 
   //
   // When APs are in a state that can be waken up by a store operation to a memory address,
   // report the MP_HAND_OFF data for DXE to use.
   //
-  CpuInfoInHob  = (CPU_INFO_IN_HOB *)(UINTN)CpuMpData->CpuInfoInHob;
-  MpHandOffSize = sizeof (MP_HAND_OFF) + sizeof (PROCESSOR_HAND_OFF) * CpuMpData->CpuCount;
-  MpHandOff     = (MP_HAND_OFF *)BuildGuidHob (&mMpHandOffGuid, MpHandOffSize);
-  ASSERT (MpHandOff != NULL);
-  ZeroMem (MpHandOff, MpHandOffSize);
-  MpHandOff->ProcessorIndex = 0;
-
-  MpHandOff->CpuCount = CpuMpData->CpuCount;
-  if (CpuMpData->ApLoopMode != ApInHltLoop) {
-    MpHandOff->StartupSignalValue    = MP_HAND_OFF_SIGNAL;
-    MpHandOff->WaitLoopExecutionMode = sizeof (VOID *);
-  }
+  CpuInfoInHob = (CPU_INFO_IN_HOB *)(UINTN)CpuMpData->CpuInfoInHob;
+  //
+  // Maximum number of processor could be saved in the HOB.
+  //
+  LimitationOfMpHandOffHob = (0xFFF8 - sizeof (EFI_HOB_GUID_TYPE) - sizeof (MP_HAND_OFF)) / sizeof (PROCESSOR_HAND_OFF);
+  ProcessedCpuCount        = 0;
+  while (ProcessedCpuCount < CpuMpData->CpuCount) {
+    NumberOfProcessorsInCurrentHob = MIN ((UINT32)CpuMpData->CpuCount - ProcessedCpuCount, LimitationOfMpHandOffHob);
+    MpHandOffSize                  = sizeof (MP_HAND_OFF) + sizeof (PROCESSOR_HAND_OFF) * NumberOfProcessorsInCurrentHob;
+    MpHandOff                      = (MP_HAND_OFF *)BuildGuidHob (
+                                                      &mMpHandOffGuid,
+                                                      MpHandOffSize
+                                                      );
+    ASSERT (MpHandOff != NULL);
+    ZeroMem (MpHandOff, MpHandOffSize);
+    //
+    // Each individual processor can be uniquely identified by
+    // combining the processorIndex with the mMpHandOffGuid.
+    //
+    MpHandOff->ProcessorIndex = ProcessedCpuCount;
+    MpHandOff->CpuCount       = CpuMpData->CpuCount;
 
-  for (Index = 0; Index < MpHandOff->CpuCount; Index++) {
-    MpHandOff->Info[Index].ApicId = CpuInfoInHob[Index].ApicId;
-    MpHandOff->Info[Index].Health = CpuInfoInHob[Index].Health;
     if (CpuMpData->ApLoopMode != ApInHltLoop) {
-      MpHandOff->Info[Index].StartupSignalAddress    = (UINT64)(UINTN)CpuMpData->CpuData[Index].StartupApSignal;
-      MpHandOff->Info[Index].StartupProcedureAddress = (UINT64)(UINTN)&CpuMpData->CpuData[Index].ApFunction;
+      MpHandOff->StartupSignalValue    = MP_HAND_OFF_SIGNAL;
+      MpHandOff->WaitLoopExecutionMode = sizeof (VOID *);
     }
+
+    for (Index = MpHandOff->ProcessorIndex; Index < NumberOfProcessorsInCurrentHob + MpHandOff->ProcessorIndex; Index++) {
+      CurrentProcessorIndex                         = Index-MpHandOff->ProcessorIndex;
+      MpHandOff->Info[CurrentProcessorIndex].ApicId = CpuInfoInHob[Index].ApicId;
+      MpHandOff->Info[CurrentProcessorIndex].Health = CpuInfoInHob[Index].Health;
+      if (CpuMpData->ApLoopMode != ApInHltLoop) {
+        MpHandOff->Info[CurrentProcessorIndex].StartupSignalAddress    = (UINT64)(UINTN)CpuMpData->CpuData[Index].StartupApSignal;
+        MpHandOff->Info[CurrentProcessorIndex].StartupProcedureAddress = (UINT64)(UINTN)&CpuMpData->CpuData[Index].ApFunction;
+      }
+    }
+
+    ProcessedCpuCount += LimitationOfMpHandOffHob;
   }
 
   //
-- 
2.36.1.windows.1