UefiCpuPkg/CpuMpPei: Prepare for buffer for AP wakeup and CPU MP data

Get AP wakeup buffer and copy AP reset code into it. Allocate APs' stack and CPU
MP data buffer. Fill CPU MP data fields accordingly.

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@17995 6f19259b-4bc3-4df7-8a09-765794883524

1 files changed, 87 insertions, 0 deletions

diff --git a/UefiCpuPkg/CpuMpPei/CpuMpPei.c b/UefiCpuPkg/CpuMpPei/CpuMpPei.c
index 0e34f26afc..45243d804e 100644
--- a/UefiCpuPkg/CpuMpPei/CpuMpPei.c
+++ b/UefiCpuPkg/CpuMpPei/CpuMpPei.c
@@ -111,6 +111,88 @@ GetWakeupBuffer (
 }
 
 /**
+  Get available system memory below 1MB by specified size.
+
+  @param PeiCpuMpData        Pointer to PEI CPU MP Data
+**/
+VOID
+BackupAndPrepareWakeupBuffer(
+  IN PEI_CPU_MP_DATA         *PeiCpuMpData
+  )
+{
+  CopyMem (
+    (VOID *) PeiCpuMpData->BackupBuffer,
+    (VOID *) PeiCpuMpData->WakeupBuffer,
+    PeiCpuMpData->BackupBufferSize
+    );
+  CopyMem (
+    (VOID *) PeiCpuMpData->WakeupBuffer,
+    (VOID *) PeiCpuMpData->AddressMap.RendezvousFunnelAddress,
+    PeiCpuMpData->AddressMap.RendezvousFunnelSize
+    );
+}
+/**
+  Prepare for AP wakeup buffer and copy AP reset code into it.
+
+  Get wakeup buffer below 1MB. Allocate memory for CPU MP Data and APs Stack.
+
+  @return   Pointer to PEI CPU MP Data
+**/
+PEI_CPU_MP_DATA *
+PrepareAPStartupVector (
+  VOID
+  )
+{
+  EFI_STATUS                    Status;
+  UINT32                        MaxCpuCount;
+  PEI_CPU_MP_DATA               *PeiCpuMpData;
+  EFI_PHYSICAL_ADDRESS          Buffer;
+  UINTN                         BufferSize;
+  UINTN                         WakeupBuffer;
+  UINTN                         WakeupBufferSize;
+  MP_ASSEMBLY_ADDRESS_MAP       AddressMap;
+
+  AsmGetAddressMap (&AddressMap);
+  WakeupBufferSize = AddressMap.RendezvousFunnelSize + sizeof (MP_CPU_EXCHANGE_INFO);
+  WakeupBuffer     = GetWakeupBuffer ((WakeupBufferSize + SIZE_4KB - 1) & ~(SIZE_4KB - 1));
+  DEBUG ((EFI_D_INFO, "CpuMpPei: WakeupBuffer = 0x%x\n", WakeupBuffer));
+
+  //
+  // Allocate Pages for APs stack, CPU MP Data and backup buffer for wakeup buffer
+  //
+  MaxCpuCount = PcdGet32(PcdCpuMaxLogicalProcessorNumber);
+  BufferSize  = PcdGet32 (PcdCpuApStackSize) * MaxCpuCount + sizeof (PEI_CPU_MP_DATA)
+                  + WakeupBufferSize + sizeof (PEI_CPU_DATA) * MaxCpuCount;
+  Status = PeiServicesAllocatePages (
+             EfiBootServicesData,
+             EFI_SIZE_TO_PAGES (BufferSize),
+             &Buffer
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  PeiCpuMpData = (PEI_CPU_MP_DATA *) (UINTN) (Buffer + PcdGet32 (PcdCpuApStackSize) * MaxCpuCount);
+  PeiCpuMpData->Buffer            = (UINTN) Buffer;
+  PeiCpuMpData->CpuApStackSize    = PcdGet32 (PcdCpuApStackSize);
+  PeiCpuMpData->WakeupBuffer      = WakeupBuffer;
+  PeiCpuMpData->BackupBuffer      = (UINTN)PeiCpuMpData + sizeof (PEI_CPU_MP_DATA);
+  PeiCpuMpData->BackupBufferSize  = WakeupBufferSize;
+  PeiCpuMpData->MpCpuExchangeInfo = (MP_CPU_EXCHANGE_INFO *) (UINTN) (WakeupBuffer + AddressMap.RendezvousFunnelSize);
+
+  PeiCpuMpData->CpuCount                 = 1;
+  PeiCpuMpData->BspNumber                = 0;
+  PeiCpuMpData->CpuData                  = (PEI_CPU_DATA *) (PeiCpuMpData->MpCpuExchangeInfo + 1);
+  PeiCpuMpData->CpuData[0].ApicId        = GetInitialApicId ();
+  PeiCpuMpData->CpuData[0].Health.Uint32 = 0;
+  CopyMem (&PeiCpuMpData->AddressMap, &AddressMap, sizeof (MP_ASSEMBLY_ADDRESS_MAP));
+
+  //
+  // Backup original data and copy AP reset code in it
+  //
+  BackupAndPrepareWakeupBuffer(PeiCpuMpData);
+
+  return PeiCpuMpData;
+}
+/**
   The Entry point of the MP CPU PEIM.
 
   This function will wakeup APs and collect CPU AP count and install the
@@ -130,11 +212,16 @@ CpuMpPeimInit (
   )
 {
 
+  PEI_CPU_MP_DATA      *PeiCpuMpData;
 
   //
   // Load new GDT table on BSP
   //
   AsmInitializeGdt (&mGdt);
+  //
+  // Get wakeup buffer and copy AP reset code in it
+  //
+  PeiCpuMpData = PrepareAPStartupVector ();
 
   return EFI_SUCCESS;
 }