[PATCH 0/6] UefiCpuPkg/MpInitLib: Program AP stack in fixed address

[PATCH 0/6] UefiCpuPkg/MpInitLib: Program AP stack in fixed address

[Jeff Fan]

Currently, MpInitLib will program AP stack in dynamic address. Each processor
will calculate its stack address by adding stack size based on the last stack
address. That means AP may have the different stack address everytime it is
wakeup by INIT-SIPI-SIPI.

When all APs have wakeup to execute AP task, each each has been assigned one
stack address. Once the timeout happened on some of APs, BSP will send INIT-
SIPI-SIPI to wake up APs. We need to re-assign stack for APs. Based on the
current implementation, we might assign one stack address used by other APs.
It will cause the unexpected stack overlapped issue.

This fix changed the stack assignment policy. We will record the stack address
assigned to AP at first time AP wakeup. When AP failed on AP task, BSP could
reassigned the same stack for it.

Cc: Feng Tian <feng.tian@intel.com>
Cc: Hao Wu <hao.a.wu@intel.com>
Cc: Michael D Kinney <michael.d.kinney@intel.com>
Contributed-under: TianoCore Contribution Agreement 1.0
Signed-off-by: Jeff Fan <jeff.fan@intel.com>

Jeff Fan (6):
  UefiCpuPkg/MpInitLib: Fixed offset error on Cr3Location
  UefiCpuPkg/MpInitLib: Force sending INIT-SIPI-SIPI to reset APs
  UefiCpuPkg/MpInitLib: Remove CPU information from CPU_AP_DATA
  UefiCpuPkg/MpInitLib: Add InitFlag and CpuInfo in MP_CPU_EXCHANGE_INFO
  UefiCpuPkg/MpInitLib: Program AP stack in fixed address
  UefiCpuPkg/MpInitLib: Update AP information when BSP switched

 UefiCpuPkg/Library/MpInitLib/Ia32/MpEqu.inc    |  4 +-
 UefiCpuPkg/Library/MpInitLib/Ia32/MpFuncs.nasm | 65 +++++++++++++++---
 UefiCpuPkg/Library/MpInitLib/MpLib.c           | 95 +++++++++++++++-----------
 UefiCpuPkg/Library/MpInitLib/MpLib.h           |  7 +-
 UefiCpuPkg/Library/MpInitLib/X64/MpEqu.inc     |  4 +-
 UefiCpuPkg/Library/MpInitLib/X64/MpFuncs.nasm  | 68 ++++++++++++++----
 6 files changed, 176 insertions(+), 67 deletions(-)

-- 
2.9.3.windows.2

[PATCH 1/6] UefiCpuPkg/MpInitLib: Fixed offset error on Cr3Location

[Jeff Fan]

Cr3Location offset value should be 0x34 not 0x3C.

Cc: Feng Tian <feng.tian@intel.com>
Cc: Hao Wu <hao.a.wu@intel.com>
Cc: Michael D Kinney <michael.d.kinney@intel.com>
Contributed-under: TianoCore Contribution Agreement 1.0
Signed-off-by: Jeff Fan <jeff.fan@intel.com>
---
 UefiCpuPkg/Library/MpInitLib/Ia32/MpEqu.inc | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/UefiCpuPkg/Library/MpInitLib/Ia32/MpEqu.inc b/UefiCpuPkg/Library/MpInitLib/Ia32/MpEqu.inc
index 60add86..5c6d4ef 100644
--- a/UefiCpuPkg/Library/MpInitLib/Ia32/MpEqu.inc
+++ b/UefiCpuPkg/Library/MpInitLib/Ia32/MpEqu.inc
@@ -37,5 +37,5 @@ NumApsExecutingLocation       equ        LockLocation + 24h
 CodeSegmentLocation           equ        LockLocation + 28h
 DataSegmentLocation           equ        LockLocation + 2Ch
 EnableExecuteDisableLocation  equ        LockLocation + 30h
-Cr3Location                   equ        LockLocation + 3Ch
+Cr3Location                   equ        LockLocation + 34h
 
-- 
2.9.3.windows.2

[PATCH 2/6] UefiCpuPkg/MpInitLib: Force sending INIT-SIPI-SIPI to reset APs

[Jeff Fan]

If BSP found APs timeout happened when AP executing AP task, BSP will reset APs
by WakeUpAP(). However, if ApLoopMode is ApMwaitLoop or ApRunLoop, WakeUpAp()
will try to write semaphore in memory to wake up AP. It cannot wake up APs
actually if APs still executing AP task.

This fix is to set ApInitReconfig flag to force BSP to send INIT-SIPI-SIPI to
wake up APs.

Cc: Feng Tian <feng.tian@intel.com>
Cc: Michael D Kinney <michael.d.kinney@intel.com>
Contributed-under: TianoCore Contribution Agreement 1.0
Signed-off-by: Jeff Fan <jeff.fan@intel.com>
---
 UefiCpuPkg/Library/MpInitLib/MpLib.c | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/UefiCpuPkg/Library/MpInitLib/MpLib.c b/UefiCpuPkg/Library/MpInitLib/MpLib.c
index a0edc55..9641e5e 100644
--- a/UefiCpuPkg/Library/MpInitLib/MpLib.c
+++ b/UefiCpuPkg/Library/MpInitLib/MpLib.c
@@ -899,7 +899,12 @@ ResetProcessorToIdleState (
 
   CpuMpData = GetCpuMpData ();
 
+  CpuMpData->InitFlag = ApInitReconfig;
   WakeUpAP (CpuMpData, FALSE, ProcessorNumber, NULL, NULL);
+  while (CpuMpData->FinishedCount < 1) {
+    CpuPause ();
+  }
+  CpuMpData->InitFlag = ApInitDone;
 
   SetApState (&CpuMpData->CpuData[ProcessorNumber], CpuStateIdle);
 }
-- 
2.9.3.windows.2

[PATCH 3/6] UefiCpuPkg/MpInitLib: Remove CPU information from CPU_AP_DATA

[Jeff Fan]

Some CPU information (InitialApicId/ApicId/Health) are duplicated in CPU_AP_DATA
and CPU_INFO_IN_HOB.

This update is to remove the ones from CPU_AP_DATA and update MpInitLib to
consume the ones from CPU_INFO_IN_HOB.

Cc: Feng Tian <feng.tian@intel.com>
Cc: Michael D Kinney <michael.d.kinney@intel.com>
Contributed-under: TianoCore Contribution Agreement 1.0
Signed-off-by: Jeff Fan <jeff.fan@intel.com>
---
 UefiCpuPkg/Library/MpInitLib/MpLib.c | 73 +++++++++++++++++++-----------------
 UefiCpuPkg/Library/MpInitLib/MpLib.h |  3 --
 2 files changed, 39 insertions(+), 37 deletions(-)

diff --git a/UefiCpuPkg/Library/MpInitLib/MpLib.c b/UefiCpuPkg/Library/MpInitLib/MpLib.c
index 9641e5e..dc4216c 100644
--- a/UefiCpuPkg/Library/MpInitLib/MpLib.c
+++ b/UefiCpuPkg/Library/MpInitLib/MpLib.c
@@ -253,7 +253,7 @@ SortApicId (
   UINTN             Index2;
   UINTN             Index3;
   UINT32            ApicId;
-  CPU_AP_DATA       CpuData;
+  CPU_INFO_IN_HOB   CpuInfo;
   UINT32            ApCount;
   CPU_INFO_IN_HOB   *CpuInfoInHob;
 
@@ -265,21 +265,22 @@ SortApicId (
       //
       // Sort key is the hardware default APIC ID
       //
-      ApicId = CpuMpData->CpuData[Index1].ApicId;
+      CpuInfoInHob = (CPU_INFO_IN_HOB *) (UINTN) CpuMpData->CpuInfoInHob;
+      ApicId = CpuInfoInHob[Index1].ApicId;
       for (Index2 = Index1 + 1; Index2 <= ApCount; Index2++) {
-        if (ApicId > CpuMpData->CpuData[Index2].ApicId) {
+        if (ApicId > CpuInfoInHob[Index2].ApicId) {
           Index3 = Index2;
-          ApicId = CpuMpData->CpuData[Index2].ApicId;
+          ApicId = CpuInfoInHob[Index2].ApicId;
         }
       }
       if (Index3 != Index1) {
-        CopyMem (&CpuData, &CpuMpData->CpuData[Index3], sizeof (CPU_AP_DATA));
+        CopyMem (&CpuInfo, &CpuInfoInHob[Index3], sizeof (CPU_INFO_IN_HOB));
         CopyMem (
-          &CpuMpData->CpuData[Index3],
-          &CpuMpData->CpuData[Index1],
-          sizeof (CPU_AP_DATA)
+          &CpuInfoInHob[Index3],
+          &CpuInfoInHob[Index1],
+          sizeof (CPU_INFO_IN_HOB)
           );
-        CopyMem (&CpuMpData->CpuData[Index1], &CpuData, sizeof (CPU_AP_DATA));
+        CopyMem (&CpuInfoInHob[Index1], &CpuInfo, sizeof (CPU_INFO_IN_HOB));
       }
     }
 
@@ -288,18 +289,11 @@ SortApicId (
     //
     ApicId = GetInitialApicId ();
     for (Index1 = 0; Index1 < CpuMpData->CpuCount; Index1++) {
-      if (CpuMpData->CpuData[Index1].ApicId == ApicId) {
+      if (CpuInfoInHob[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;
-    }
   }
 }
 
@@ -358,10 +352,13 @@ GetProcessorNumber (
 {
   UINTN                   TotalProcessorNumber;
   UINTN                   Index;
+  CPU_INFO_IN_HOB         *CpuInfoInHob;
+
+  CpuInfoInHob = (CPU_INFO_IN_HOB *) (UINTN) CpuMpData->CpuInfoInHob;
 
   TotalProcessorNumber = CpuMpData->CpuCount;
   for (Index = 0; Index < TotalProcessorNumber; Index ++) {
-    if (CpuMpData->CpuData[Index].ApicId == GetApicId ()) {
+    if (CpuInfoInHob[Index].ApicId == GetApicId ()) {
       *ProcessorNumber = Index;
       return EFI_SUCCESS;
     }
@@ -439,12 +436,16 @@ InitializeApData (
   IN     UINT32           BistData
   )
 {
+  CPU_INFO_IN_HOB          *CpuInfoInHob;
+
+  CpuInfoInHob = (CPU_INFO_IN_HOB *) (UINTN) CpuMpData->CpuInfoInHob;
+  CpuInfoInHob[ProcessorNumber].InitialApicId = GetInitialApicId ();
+  CpuInfoInHob[ProcessorNumber].ApicId        = GetApicId ();
+  CpuInfoInHob[ProcessorNumber].Health        = BistData;
+
   CpuMpData->CpuData[ProcessorNumber].Waiting    = FALSE;
-  CpuMpData->CpuData[ProcessorNumber].Health     = BistData;
   CpuMpData->CpuData[ProcessorNumber].CpuHealthy = (BistData == 0) ? TRUE : FALSE;
-  CpuMpData->CpuData[ProcessorNumber].ApicId     = GetApicId ();
-  CpuMpData->CpuData[ProcessorNumber].InitialApicId = GetInitialApicId ();
-  if (CpuMpData->CpuData[ProcessorNumber].InitialApicId >= 0xFF) {
+  if (CpuInfoInHob[ProcessorNumber].InitialApicId >= 0xFF) {
     //
     // Set x2APIC mode if there are any logical processor reporting
     // an Initial APIC ID of 255 or greater.
@@ -477,6 +478,7 @@ ApWakeupFunction (
   VOID                       *Parameter;
   UINT32                     BistData;
   volatile UINT32            *ApStartupSignalBuffer;
+  CPU_INFO_IN_HOB            *CpuInfoInHob;
 
   //
   // AP finished assembly code and begin to execute C code
@@ -547,8 +549,9 @@ ApWakeupFunction (
             //
             // Re-get the CPU APICID and Initial APICID
             //
-            CpuMpData->CpuData[ProcessorNumber].ApicId        = GetApicId ();
-            CpuMpData->CpuData[ProcessorNumber].InitialApicId = GetInitialApicId ();
+            CpuInfoInHob = (CPU_INFO_IN_HOB *) (UINTN) CpuMpData->CpuInfoInHob;
+            CpuInfoInHob[ProcessorNumber].ApicId        = GetApicId ();
+            CpuInfoInHob[ProcessorNumber].InitialApicId = GetInitialApicId ();
           }
         }
         SetApState (&CpuMpData->CpuData[ProcessorNumber], CpuStateFinished);
@@ -696,6 +699,7 @@ WakeUpAP (
   UINTN                            Index;
   CPU_AP_DATA                      *CpuData;
   BOOLEAN                          ResetVectorRequired;
+  CPU_INFO_IN_HOB                  *CpuInfoInHob;
 
   CpuMpData->FinishedCount = 0;
   ResetVectorRequired = FALSE;
@@ -760,8 +764,9 @@ WakeUpAP (
     ASSERT (CpuMpData->InitFlag != ApInitConfig);
     *(UINT32 *) CpuData->StartupApSignal = WAKEUP_AP_SIGNAL;
     if (ResetVectorRequired) {
+      CpuInfoInHob = (CPU_INFO_IN_HOB *) (UINTN) CpuMpData->CpuInfoInHob;
       SendInitSipiSipi (
-        CpuData->ApicId,
+        CpuInfoInHob[ProcessorNumber].ApicId,
         (UINT32) ExchangeInfo->BufferStart
         );
     }
@@ -1229,16 +1234,14 @@ MpInitLibInitialize (
     CpuMpData->CpuCount  = OldCpuMpData->CpuCount;
     CpuMpData->BspNumber = OldCpuMpData->BspNumber;
     CpuMpData->InitFlag  = ApInitReconfig;
-    CpuInfoInHob = (CPU_INFO_IN_HOB *) (UINTN) OldCpuMpData->CpuInfoInHob;
+    CpuMpData->CpuInfoInHob = OldCpuMpData->CpuInfoInHob;
+    CpuInfoInHob = (CPU_INFO_IN_HOB *) (UINTN) CpuMpData->CpuInfoInHob;
     for (Index = 0; Index < CpuMpData->CpuCount; Index++) {
       InitializeSpinLock(&CpuMpData->CpuData[Index].ApLock);
-      CpuMpData->CpuData[Index].ApicId        = CpuInfoInHob[Index].ApicId;
-      CpuMpData->CpuData[Index].InitialApicId = CpuInfoInHob[Index].InitialApicId;
-      if (CpuMpData->CpuData[Index].InitialApicId >= 255) {
+      if (CpuInfoInHob[Index].InitialApicId >= 255) {
         CpuMpData->X2ApicEnable = TRUE;
       }
-      CpuMpData->CpuData[Index].Health     = CpuInfoInHob[Index].Health;
-      CpuMpData->CpuData[Index].CpuHealthy = (CpuMpData->CpuData[Index].Health == 0)? TRUE:FALSE;
+      CpuMpData->CpuData[Index].CpuHealthy = (CpuInfoInHob[Index].Health == 0)? TRUE:FALSE;
       CpuMpData->CpuData[Index].ApFunction = 0;
       CopyMem (
         &CpuMpData->CpuData[Index].VolatileRegisters,
@@ -1299,8 +1302,10 @@ MpInitLibGetProcessorInfo (
 {
   CPU_MP_DATA            *CpuMpData;
   UINTN                  CallerNumber;
+  CPU_INFO_IN_HOB        *CpuInfoInHob;
 
   CpuMpData = GetCpuMpData ();
+  CpuInfoInHob = (CPU_INFO_IN_HOB *) (UINTN) CpuMpData->CpuInfoInHob;
 
   //
   // Check whether caller processor is BSP
@@ -1318,7 +1323,7 @@ MpInitLibGetProcessorInfo (
     return EFI_NOT_FOUND;
   }
 
-  ProcessorInfoBuffer->ProcessorId = (UINT64) CpuMpData->CpuData[ProcessorNumber].ApicId;
+  ProcessorInfoBuffer->ProcessorId = (UINT64) CpuInfoInHob[ProcessorNumber].ApicId;
   ProcessorInfoBuffer->StatusFlag  = 0;
   if (ProcessorNumber == CpuMpData->BspNumber) {
     ProcessorInfoBuffer->StatusFlag |= PROCESSOR_AS_BSP_BIT;
@@ -1336,14 +1341,14 @@ MpInitLibGetProcessorInfo (
   // Get processor location information
   //
   GetProcessorLocationByApicId (
-    CpuMpData->CpuData[ProcessorNumber].ApicId,
+    CpuInfoInHob[ProcessorNumber].ApicId,
     &ProcessorInfoBuffer->Location.Package,
     &ProcessorInfoBuffer->Location.Core,
     &ProcessorInfoBuffer->Location.Thread
     );
 
   if (HealthData != NULL) {
-    HealthData->Uint32 = CpuMpData->CpuData[ProcessorNumber].Health;
+    HealthData->Uint32 = CpuInfoInHob[ProcessorNumber].Health;
   }
 
   return EFI_SUCCESS;
diff --git a/UefiCpuPkg/Library/MpInitLib/MpLib.h b/UefiCpuPkg/Library/MpInitLib/MpLib.h
index ae78c59..f107b6d 100644
--- a/UefiCpuPkg/Library/MpInitLib/MpLib.h
+++ b/UefiCpuPkg/Library/MpInitLib/MpLib.h
@@ -112,9 +112,6 @@ typedef struct {
   volatile UINT32                *StartupApSignal;
   volatile UINTN                 ApFunction;
   volatile UINTN                 ApFunctionArgument;
-  UINT32                         InitialApicId;
-  UINT32                         ApicId;
-  UINT32                         Health;
   BOOLEAN                        CpuHealthy;
   volatile CPU_STATE             State;
   CPU_VOLATILE_REGISTERS         VolatileRegisters;
-- 
2.9.3.windows.2

[PATCH 4/6] UefiCpuPkg/MpInitLib: Add InitFlag and CpuInfo in MP_CPU_EXCHANGE_INFO

[Jeff Fan]

Cc: Feng Tian <feng.tian@intel.com>
Cc: Michael D Kinney <michael.d.kinney@intel.com>
Contributed-under: TianoCore Contribution Agreement 1.0
Signed-off-by: Jeff Fan <jeff.fan@intel.com>
---
 UefiCpuPkg/Library/MpInitLib/Ia32/MpEqu.inc | 2 ++
 UefiCpuPkg/Library/MpInitLib/MpLib.c        | 2 ++
 UefiCpuPkg/Library/MpInitLib/MpLib.h        | 2 ++
 UefiCpuPkg/Library/MpInitLib/X64/MpEqu.inc  | 4 ++--
 4 files changed, 8 insertions(+), 2 deletions(-)

diff --git a/UefiCpuPkg/Library/MpInitLib/Ia32/MpEqu.inc b/UefiCpuPkg/Library/MpInitLib/Ia32/MpEqu.inc
index 5c6d4ef..6276230 100644
--- a/UefiCpuPkg/Library/MpInitLib/Ia32/MpEqu.inc
+++ b/UefiCpuPkg/Library/MpInitLib/Ia32/MpEqu.inc
@@ -38,4 +38,6 @@ CodeSegmentLocation           equ        LockLocation + 28h
 DataSegmentLocation           equ        LockLocation + 2Ch
 EnableExecuteDisableLocation  equ        LockLocation + 30h
 Cr3Location                   equ        LockLocation + 34h
+InitFlagLocation              equ        LockLocation + 38h
+CpuInfoLocation               equ        LockLocation + 3Ch
 
diff --git a/UefiCpuPkg/Library/MpInitLib/MpLib.c b/UefiCpuPkg/Library/MpInitLib/MpLib.c
index dc4216c..da814c6 100644
--- a/UefiCpuPkg/Library/MpInitLib/MpLib.c
+++ b/UefiCpuPkg/Library/MpInitLib/MpLib.c
@@ -665,6 +665,8 @@ FillExchangeInfoData (
 
   ExchangeInfo->CFunction       = (UINTN) ApWakeupFunction;
   ExchangeInfo->NumApsExecuting = 0;
+  ExchangeInfo->InitFlag        = (UINTN) CpuMpData->InitFlag;
+  ExchangeInfo->CpuInfo         = (CPU_INFO_IN_HOB *) (UINTN) CpuMpData->CpuInfoInHob;
   ExchangeInfo->CpuMpData       = CpuMpData;
 
   ExchangeInfo->EnableExecuteDisable = IsBspExecuteDisableEnabled ();
diff --git a/UefiCpuPkg/Library/MpInitLib/MpLib.h b/UefiCpuPkg/Library/MpInitLib/MpLib.h
index f107b6d..a58c855 100644
--- a/UefiCpuPkg/Library/MpInitLib/MpLib.h
+++ b/UefiCpuPkg/Library/MpInitLib/MpLib.h
@@ -171,6 +171,8 @@ typedef struct {
   UINTN                 DataSegment;
   UINTN                 EnableExecuteDisable;
   UINTN                 Cr3;
+  UINTN                 InitFlag;
+  CPU_INFO_IN_HOB       *CpuInfo;
   CPU_MP_DATA           *CpuMpData;
 } MP_CPU_EXCHANGE_INFO;
 
diff --git a/UefiCpuPkg/Library/MpInitLib/X64/MpEqu.inc b/UefiCpuPkg/Library/MpInitLib/X64/MpEqu.inc
index d533741..a63cd23 100644
--- a/UefiCpuPkg/Library/MpInitLib/X64/MpEqu.inc
+++ b/UefiCpuPkg/Library/MpInitLib/X64/MpEqu.inc
@@ -38,6 +38,6 @@ CodeSegmentLocation           equ        LockLocation + 4Ch
 DataSegmentLocation           equ        LockLocation + 54h
 EnableExecuteDisableLocation  equ        LockLocation + 5Ch
 Cr3Location                   equ        LockLocation + 64h
+InitFlagLocation              equ        LockLocation + 6Ch
+CpuInfoLocation               equ        LockLocation + 74h
 
-
-;-------------------------------------------------------------------------------
-- 
2.9.3.windows.2

[PATCH 5/6] UefiCpuPkg/MpInitLib: Program AP stack in fixed address

[Jeff Fan]

Currently, MpInitLib will program AP stack in dynamic address. Each processor
will calculate its stack address by adding stack size based on the last stack
address. That means AP may have the different stack address everytime it is
wakeup by INIT-SIPI-SIPI.

When all APs have wakeup to execute AP task, each each has been assigned one
stack address. Once the timeout happened on some of APs, BSP will send INIT-
SIPI-SIPI to wake up APs. We need to re-assign stack for APs. Based on the
current implementation, we might assign one stack address used by other APs.
It will cause the unexpected stack overlapped issue.

This fix changed the stack assignment policy. We will record the stack address
assigned to AP at first time AP wakeup. When AP failed on AP task, BSP could
reassigned the same stack for it.

Getting initial APIC ID in assembly code could help AP to get saved its stack
address.

Cc: Feng Tian <feng.tian@intel.com>
Cc: Michael D Kinney <michael.d.kinney@intel.com>
Contributed-under: TianoCore Contribution Agreement 1.0
Signed-off-by: Jeff Fan <jeff.fan@intel.com>
---
 UefiCpuPkg/Library/MpInitLib/Ia32/MpFuncs.nasm | 65 +++++++++++++++++++-----
 UefiCpuPkg/Library/MpInitLib/MpLib.c           | 12 +++--
 UefiCpuPkg/Library/MpInitLib/MpLib.h           |  1 +
 UefiCpuPkg/Library/MpInitLib/X64/MpFuncs.nasm  | 68 +++++++++++++++++++++-----
 4 files changed, 119 insertions(+), 27 deletions(-)

diff --git a/UefiCpuPkg/Library/MpInitLib/Ia32/MpFuncs.nasm b/UefiCpuPkg/Library/MpInitLib/Ia32/MpFuncs.nasm
index 8f6f0bf..4bfa084 100644
--- a/UefiCpuPkg/Library/MpInitLib/Ia32/MpFuncs.nasm
+++ b/UefiCpuPkg/Library/MpInitLib/Ia32/MpFuncs.nasm
@@ -114,7 +114,12 @@ Flat32Start:                                   ; protected mode entry point
     mov         cr0, eax
 
 SkipEnableExecuteDisable:
+    mov        edi, esi
+    add        edi, InitFlagLocation
+    cmp        dword [edi], 1       ; 1 == ApInitConfig
+    jnz        GetApicId
 
+    ; AP init
     mov        edi, esi
     add        edi, LockLocation
     mov        eax, NotVacantFlag
@@ -124,27 +129,65 @@ TestLock:
     cmp        eax, NotVacantFlag
     jz         TestLock
 
-    mov        edi, esi
-    add        edi, NumApsExecutingLocation
-    inc        dword [edi]
-    mov        ebx, [edi]
+    mov        ecx, esi
+    add        ecx, NumApsExecutingLocation
+    inc        dword [ecx]
+    mov        ebx, [ecx]
+
+Releaselock:
+    mov        eax, VacantFlag
+    xchg       [edi], eax
 
-ProgramStack:
     mov        edi, esi
     add        edi, StackSizeLocation
     mov        eax, [edi]
+    mov        ecx, ebx
+    inc        ecx
+    mul        ecx                               ; EAX = StackSize * (CpuNumber + 1)
     mov        edi, esi
     add        edi, StackStartAddressLocation
     add        eax, [edi]
     mov        esp, eax
-    mov        [edi], eax
+    jmp        CProcedureInvoke
+
+GetApicId:
+    mov        eax, 0
+    cpuid
+    cmp        eax, 0bh
+    jnb        X2Apic
+    ; Processor is not x2APIC capable, so get 8-bit APIC ID
+    mov        eax, 1
+    cpuid
+    shr        ebx, 24
+    mov        edx, ebx
+    jmp        GetProcessorNumber
+
+X2Apic:
+    ; Processor is x2APIC capable, so get 32-bit x2APIC ID
+    mov        eax, 0bh
+    xor        ecx, ecx
+    cpuid                   
+    ; edx save x2APIC ID
+    
+GetProcessorNumber:
+    ;
+    ; Get processor number for this AP
+    ; Note that BSP may become an AP due to SwitchBsp()
+    ;
+    xor         ebx, ebx
+    lea         eax, [esi + CpuInfoLocation]
+    mov         edi, [eax]
 
-Releaselock:
-    mov        eax, VacantFlag
-    mov        edi, esi
-    add        edi, LockLocation
-    xchg       [edi], eax
+GetNextProcNumber:
+    cmp         [edi], edx                       ; APIC ID match?
+    jz          ProgramStack
+    add         edi, 16
+    inc         ebx
+    jmp         GetNextProcNumber    
 
+ProgramStack:
+    mov         esp, [edi + 12]
+   
 CProcedureInvoke:
     push       ebp               ; push BIST data at top of AP stack
     xor        ebp, ebp          ; clear ebp for call stack trace
diff --git a/UefiCpuPkg/Library/MpInitLib/MpLib.c b/UefiCpuPkg/Library/MpInitLib/MpLib.c
index da814c6..748c8e7 100644
--- a/UefiCpuPkg/Library/MpInitLib/MpLib.c
+++ b/UefiCpuPkg/Library/MpInitLib/MpLib.c
@@ -433,7 +433,8 @@ VOID
 InitializeApData (
   IN OUT CPU_MP_DATA      *CpuMpData,
   IN     UINTN            ProcessorNumber,
-  IN     UINT32           BistData
+  IN     UINT32           BistData,
+  IN     UINTN            ApTopOfStack
   )
 {
   CPU_INFO_IN_HOB          *CpuInfoInHob;
@@ -442,6 +443,7 @@ InitializeApData (
   CpuInfoInHob[ProcessorNumber].InitialApicId = GetInitialApicId ();
   CpuInfoInHob[ProcessorNumber].ApicId        = GetApicId ();
   CpuInfoInHob[ProcessorNumber].Health        = BistData;
+  CpuInfoInHob[ProcessorNumber].ApTopOfStack  = (UINT32) ApTopOfStack;
 
   CpuMpData->CpuData[ProcessorNumber].Waiting    = FALSE;
   CpuMpData->CpuData[ProcessorNumber].CpuHealthy = (BistData == 0) ? TRUE : FALSE;
@@ -479,6 +481,7 @@ ApWakeupFunction (
   UINT32                     BistData;
   volatile UINT32            *ApStartupSignalBuffer;
   CPU_INFO_IN_HOB            *CpuInfoInHob;
+  UINTN                      ApTopOfStack;
 
   //
   // AP finished assembly code and begin to execute C code
@@ -497,7 +500,8 @@ ApWakeupFunction (
       //
       // This is first time AP wakeup, get BIST information from AP stack
       //
-      BistData = *(UINT32 *) (CpuMpData->Buffer + ProcessorNumber * CpuMpData->CpuApStackSize - sizeof (UINTN));
+      ApTopOfStack  = CpuMpData->Buffer + (ProcessorNumber + 1) * CpuMpData->CpuApStackSize;
+      BistData = *(UINT32 *) (ApTopOfStack - sizeof (UINTN));
       //
       // Do some AP initialize sync
       //
@@ -506,7 +510,7 @@ ApWakeupFunction (
       // Sync BSP's Control registers to APs
       //
       RestoreVolatileRegisters (&CpuMpData->CpuData[0].VolatileRegisters, FALSE);
-      InitializeApData (CpuMpData, ProcessorNumber, BistData);
+      InitializeApData (CpuMpData, ProcessorNumber, BistData, ApTopOfStack);
       ApStartupSignalBuffer = CpuMpData->CpuData[ProcessorNumber].StartupApSignal;
     } else {
       //
@@ -1195,7 +1199,7 @@ MpInitLibInitialize (
   //
   // Set BSP basic information
   //
-  InitializeApData (CpuMpData, 0, 0);
+  InitializeApData (CpuMpData, 0, 0, CpuMpData->Buffer);
   //
   // Save assembly code information
   //
diff --git a/UefiCpuPkg/Library/MpInitLib/MpLib.h b/UefiCpuPkg/Library/MpInitLib/MpLib.h
index a58c855..f81c819 100644
--- a/UefiCpuPkg/Library/MpInitLib/MpLib.h
+++ b/UefiCpuPkg/Library/MpInitLib/MpLib.h
@@ -133,6 +133,7 @@ typedef struct {
   UINT32                         InitialApicId;
   UINT32                         ApicId;
   UINT32                         Health;
+  UINT32                         ApTopOfStack;
 } CPU_INFO_IN_HOB;
 
 //
diff --git a/UefiCpuPkg/Library/MpInitLib/X64/MpFuncs.nasm b/UefiCpuPkg/Library/MpInitLib/X64/MpFuncs.nasm
index 090e9fa..bfc3ff1 100644
--- a/UefiCpuPkg/Library/MpInitLib/X64/MpFuncs.nasm
+++ b/UefiCpuPkg/Library/MpInitLib/X64/MpFuncs.nasm
@@ -120,6 +120,12 @@ LongModeStart:
     mov        ss,  ax
 
     mov        esi, ebx
+    lea        edi, [esi + InitFlagLocation]
+    cmp        qword [edi], 1       ; ApInitConfig
+    jnz        GetApicId
+
+    ; AP init
+    mov        esi, ebx
     mov        edi, esi
     add        edi, LockLocation
     mov        rax, NotVacantFlag
@@ -129,26 +135,64 @@ TestLock:
     cmp        rax, NotVacantFlag
     jz         TestLock
 
-    mov        edi, esi
-    add        edi, NumApsExecutingLocation
-    inc        dword [edi]
-    mov        ebx, [edi]
+    lea        ecx, [esi + InitFlagLocation]
+    inc        dword [ecx]
+    mov        ebx, [ecx]
 
-ProgramStack:
+Releaselock:
+    mov        rax, VacantFlag
+    xchg       qword [edi], rax
+    ; program stack
     mov        edi, esi
     add        edi, StackSizeLocation
-    mov        rax, qword [edi]
+    mov        eax, dword [edi]
+    mov        ecx, ebx
+    inc        ecx
+    mul        ecx                               ; EAX = StackSize * (CpuNumber + 1)
     mov        edi, esi
     add        edi, StackStartAddressLocation
     add        rax, qword [edi]
     mov        rsp, rax
-    mov        qword [edi], rax
+    jmp        CProcedureInvoke
+
+GetApicId:
+    mov        eax, 0
+    cpuid
+    cmp        eax, 0bh
+    jnb        X2Apic
+    ; Processor is not x2APIC capable, so get 8-bit APIC ID
+    mov        eax, 1
+    cpuid
+    shr        ebx, 24
+    mov        edx, ebx
+    jmp        GetProcessorNumber
+
+X2Apic:
+    ; Processor is x2APIC capable, so get 32-bit x2APIC ID
+    mov        eax, 0bh
+    xor        ecx, ecx
+    cpuid                   
+    ; edx save x2APIC ID
+    
+GetProcessorNumber:
+    ;
+    ; Get processor number for this AP
+    ; Note that BSP may become an AP due to SwitchBsp()
+    ;
+    xor         ebx, ebx
+    lea         eax, [esi + CpuInfoLocation]
+    mov         edi, [eax]
 
-Releaselock:
-    mov        rax, VacantFlag
-    mov        edi, esi
-    add        edi, LockLocation
-    xchg       qword [edi], rax
+GetNextProcNumber:
+    cmp         dword [edi], edx                      ; APIC ID match?
+    jz          ProgramStack
+    add         edi, 16
+    inc         ebx
+    jmp         GetNextProcNumber    
+
+ProgramStack:
+    xor         rsp, rsp
+    mov         esp, dword [edi + 12]
 
 CProcedureInvoke:
     push       rbp               ; Push BIST data at top of AP stack
-- 
2.9.3.windows.2

[PATCH 6/6] UefiCpuPkg/MpInitLib: Update AP information when BSP switched

[Jeff Fan]

When BSP switched, we need to update some AP information. For example,
ApStartupSignalBuffer and ApTopOfStack.

Cc: Feng Tian <feng.tian@intel.com>
Cc: Michael D Kinney <michael.d.kinney@intel.com>
Contributed-under: TianoCore Contribution Agreement 1.0
Signed-off-by: Jeff Fan <jeff.fan@intel.com>
---
 UefiCpuPkg/Library/MpInitLib/MpLib.c | 3 +++
 UefiCpuPkg/Library/MpInitLib/MpLib.h | 1 +
 2 files changed, 4 insertions(+)

diff --git a/UefiCpuPkg/Library/MpInitLib/MpLib.c b/UefiCpuPkg/Library/MpInitLib/MpLib.c
index 748c8e7..c74efb0 100644
--- a/UefiCpuPkg/Library/MpInitLib/MpLib.c
+++ b/UefiCpuPkg/Library/MpInitLib/MpLib.c
@@ -549,6 +549,8 @@ ApWakeupFunction (
             GetProcessorNumber (CpuMpData, &ProcessorNumber);
             CpuMpData->CpuData[ProcessorNumber].ApFunction = 0;
             CpuMpData->CpuData[ProcessorNumber].ApFunctionArgument = 0;
+            ApStartupSignalBuffer = CpuMpData->CpuData[ProcessorNumber].StartupApSignal;
+            CpuInfoInHob[ProcessorNumber].ApTopOfStack = CpuInfoInHob[CpuMpData->NewBspNumber].ApTopOfStack;
           } else {
             //
             // Re-get the CPU APICID and Initial APICID
@@ -1421,6 +1423,7 @@ SwitchBSPWorker (
   CpuMpData->BSPInfo.State = CPU_SWITCH_STATE_IDLE;
   CpuMpData->APInfo.State  = CPU_SWITCH_STATE_IDLE;
   CpuMpData->SwitchBspFlag = TRUE;
+  CpuMpData->NewBspNumber  = ProcessorNumber;
 
   //
   // Clear the BSP bit of MSR_IA32_APIC_BASE
diff --git a/UefiCpuPkg/Library/MpInitLib/MpLib.h b/UefiCpuPkg/Library/MpInitLib/MpLib.h
index f81c819..0ac777a 100644
--- a/UefiCpuPkg/Library/MpInitLib/MpLib.h
+++ b/UefiCpuPkg/Library/MpInitLib/MpLib.h
@@ -216,6 +216,7 @@ struct _CPU_MP_DATA {
   AP_INIT_STATE                  InitFlag;
   BOOLEAN                        X2ApicEnable;
   BOOLEAN                        SwitchBspFlag;
+  UINTN                          NewBspNumber;
   CPU_EXCHANGE_ROLE_INFO         BSPInfo;
   CPU_EXCHANGE_ROLE_INFO         APInfo;
   MTRR_SETTINGS                  MtrrTable;
-- 
2.9.3.windows.2

Re: [PATCH 0/6] UefiCpuPkg/MpInitLib: Program AP stack in fixed address

[Tian, Feng]

Reviewed-by: Feng Tian <feng.tian@Intel.com>

Thanks
Feng

-----Original Message-----
From: Fan, Jeff 
Sent: Monday, November 14, 2016 11:47 AM
To: edk2-devel@lists.01.org
Cc: Tian, Feng <feng.tian@intel.com>; Wu, Hao A <hao.a.wu@intel.com>; Kinney, Michael D <michael.d.kinney@intel.com>
Subject: [PATCH 0/6] UefiCpuPkg/MpInitLib: Program AP stack in fixed address

Currently, MpInitLib will program AP stack in dynamic address. Each processor will calculate its stack address by adding stack size based on the last stack address. That means AP may have the different stack address everytime it is wakeup by INIT-SIPI-SIPI.

When all APs have wakeup to execute AP task, each each has been assigned one stack address. Once the timeout happened on some of APs, BSP will send INIT- SIPI-SIPI to wake up APs. We need to re-assign stack for APs. Based on the current implementation, we might assign one stack address used by other APs.
It will cause the unexpected stack overlapped issue.

This fix changed the stack assignment policy. We will record the stack address assigned to AP at first time AP wakeup. When AP failed on AP task, BSP could reassigned the same stack for it.

Cc: Feng Tian <feng.tian@intel.com>
Cc: Hao Wu <hao.a.wu@intel.com>
Cc: Michael D Kinney <michael.d.kinney@intel.com>
Contributed-under: TianoCore Contribution Agreement 1.0
Signed-off-by: Jeff Fan <jeff.fan@intel.com>

Jeff Fan (6):
  UefiCpuPkg/MpInitLib: Fixed offset error on Cr3Location
  UefiCpuPkg/MpInitLib: Force sending INIT-SIPI-SIPI to reset APs
  UefiCpuPkg/MpInitLib: Remove CPU information from CPU_AP_DATA
  UefiCpuPkg/MpInitLib: Add InitFlag and CpuInfo in MP_CPU_EXCHANGE_INFO
  UefiCpuPkg/MpInitLib: Program AP stack in fixed address
  UefiCpuPkg/MpInitLib: Update AP information when BSP switched

 UefiCpuPkg/Library/MpInitLib/Ia32/MpEqu.inc    |  4 +-
 UefiCpuPkg/Library/MpInitLib/Ia32/MpFuncs.nasm | 65 +++++++++++++++---
 UefiCpuPkg/Library/MpInitLib/MpLib.c           | 95 +++++++++++++++-----------
 UefiCpuPkg/Library/MpInitLib/MpLib.h           |  7 +-
 UefiCpuPkg/Library/MpInitLib/X64/MpEqu.inc     |  4 +-
 UefiCpuPkg/Library/MpInitLib/X64/MpFuncs.nasm  | 68 ++++++++++++++----
 6 files changed, 176 insertions(+), 67 deletions(-)

--
2.9.3.windows.2