[Patch v4 19/46] UefiCpuPkg/MpInitLib: Allocate AP reset vector buffer under 1MB

[Jeff Fan <jeff.fan@intel.com>]

In PeiMpInitLib, searching unallocated memory under in
EFI_HOB_TYPE_RESOURCE_DESCRIPTOR hobs to find the memory under 1MB for AP reset
vector. After End of PEI event triggered, we need to restore original the buffer
contents to avoid crash the OS on S3 boot.
In DxeMpInitLib, allocate the memory under 1MB for AP reset vector.

Add helper functions AllocateResetVector()/FreeResetVector() used by WakeupAp().

v3:
  1. Move SetTimer() from Patch #17 to Patch 16.

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>
---
 UefiCpuPkg/Library/MpInitLib/DxeMpLib.c |  60 ++++++++
 UefiCpuPkg/Library/MpInitLib/MpLib.h    |  20 +++
 UefiCpuPkg/Library/MpInitLib/PeiMpLib.c | 245 ++++++++++++++++++++++++++++++++
 3 files changed, 325 insertions(+)

diff --git a/UefiCpuPkg/Library/MpInitLib/DxeMpLib.c b/UefiCpuPkg/Library/MpInitLib/DxeMpLib.c
index b7b5fd5..ce7a554 100644
--- a/UefiCpuPkg/Library/MpInitLib/DxeMpLib.c
+++ b/UefiCpuPkg/Library/MpInitLib/DxeMpLib.c
@@ -49,6 +49,65 @@ SaveCpuMpData (
 }
 
 /**
+  Allocate reset vector buffer.
+
+  @param[in, out]  CpuMpData  The pointer to CPU MP Data structure.
+**/
+VOID
+AllocateResetVector (
+  IN OUT CPU_MP_DATA          *CpuMpData
+  )
+{
+  EFI_STATUS            Status;
+  UINTN                 ApResetVectorSize;
+  EFI_PHYSICAL_ADDRESS  StartAddress;
+
+  ApResetVectorSize = CpuMpData->AddressMap.RendezvousFunnelSize +
+                      sizeof (MP_CPU_EXCHANGE_INFO);
+
+  StartAddress = BASE_1MB;
+  Status = gBS->AllocatePages (
+                  AllocateMaxAddress,
+                  EfiACPIMemoryNVS,
+                  EFI_SIZE_TO_PAGES (ApResetVectorSize),
+                  &StartAddress
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  CpuMpData->WakeupBuffer      = (UINTN) StartAddress;
+  CpuMpData->MpCpuExchangeInfo = (MP_CPU_EXCHANGE_INFO *) (UINTN)
+                  (CpuMpData->WakeupBuffer + CpuMpData->AddressMap.RendezvousFunnelSize);
+  //
+  // copy AP reset code in it
+  //
+  CopyMem (
+    (VOID *) CpuMpData->WakeupBuffer,
+    (VOID *) CpuMpData->AddressMap.RendezvousFunnelAddress,
+    CpuMpData->AddressMap.RendezvousFunnelSize
+    );
+}
+
+/**
+  Free AP reset vector buffer.
+
+  @param[in]  CpuMpData  The pointer to CPU MP Data structure.
+**/
+VOID
+FreeResetVector (
+  IN CPU_MP_DATA              *CpuMpData
+  )
+{
+  EFI_STATUS            Status;
+  UINTN                 ApResetVectorSize;
+  ApResetVectorSize = CpuMpData->AddressMap.RendezvousFunnelSize +
+                      sizeof (MP_CPU_EXCHANGE_INFO);
+  Status = gBS->FreePages(
+             (EFI_PHYSICAL_ADDRESS)CpuMpData->WakeupBuffer,
+             EFI_SIZE_TO_PAGES (ApResetVectorSize)
+             );
+  ASSERT_EFI_ERROR (Status);
+}
+
 /**
   Checks APs status and updates APs status if needed.
 
@@ -85,6 +144,7 @@ CheckApsStatus (
     CheckAndUpdateApsStatus ();
   }
 }
+
 /**
   Initialize global data for MP support.
 
diff --git a/UefiCpuPkg/Library/MpInitLib/MpLib.h b/UefiCpuPkg/Library/MpInitLib/MpLib.h
index a2818e6..81fea02 100644
--- a/UefiCpuPkg/Library/MpInitLib/MpLib.h
+++ b/UefiCpuPkg/Library/MpInitLib/MpLib.h
@@ -236,6 +236,26 @@ SaveCpuMpData (
   );
 
 /**
+  Allocate reset vector buffer.
+
+  @param[in, out]  CpuMpData  The pointer to CPU MP Data structure.
+**/
+VOID
+AllocateResetVector (
+  IN OUT CPU_MP_DATA          *CpuMpData
+  );
+
+/**
+  Free AP reset vector buffer.
+
+  @param[in]  CpuMpData  The pointer to CPU MP Data structure.
+**/
+VOID
+FreeResetVector (
+  IN CPU_MP_DATA              *CpuMpData
+  );
+
+/**
   Initialize global data for MP support.
 
   @param[in] CpuMpData  The pointer to CPU MP Data structure.
diff --git a/UefiCpuPkg/Library/MpInitLib/PeiMpLib.c b/UefiCpuPkg/Library/MpInitLib/PeiMpLib.c
index f37e985..2e021c9 100644
--- a/UefiCpuPkg/Library/MpInitLib/PeiMpLib.c
+++ b/UefiCpuPkg/Library/MpInitLib/PeiMpLib.c
@@ -61,8 +61,45 @@ SaveCpuMpData (
     );
 }
 
+/**
+  Get available system memory below 1MB by specified size.
+
+  @param[in] PeiCpuMpData        Pointer to PEI CPU MP Data
+**/
+VOID
+BackupAndPrepareWakeupBuffer(
+  IN CPU_MP_DATA              *CpuMpData
+  )
+{
+  CopyMem (
+    (VOID *) CpuMpData->BackupBuffer,
+    (VOID *) CpuMpData->WakeupBuffer,
+    CpuMpData->BackupBufferSize
+    );
+  CopyMem (
+    (VOID *) CpuMpData->WakeupBuffer,
+    (VOID *) CpuMpData->AddressMap.RendezvousFunnelAddress,
+    CpuMpData->AddressMap.RendezvousFunnelSize
+    );
+}
 
 /**
+  Restore wakeup buffer data.
+
+  @param[in] PeiCpuMpData        Pointer to PEI CPU MP Data
+**/
+VOID
+RestoreWakeupBuffer(
+  IN CPU_MP_DATA              *CpuMpData
+  )
+{
+  CopyMem (
+    (VOID *) CpuMpData->WakeupBuffer,
+    (VOID *) CpuMpData->BackupBuffer,
+    CpuMpData->BackupBufferSize
+    );
+}
+
 /**
   Notify function on End Of PEI PPI.
 
@@ -83,11 +120,219 @@ CpuMpEndOfPeiCallback (
   IN VOID                         *Ppi
   )
 {
+  EFI_STATUS                Status;
+  EFI_BOOT_MODE             BootMode;
+  CPU_MP_DATA               *CpuMpData;
+  EFI_PEI_HOB_POINTERS      Hob;
+  EFI_HOB_MEMORY_ALLOCATION *MemoryHob;
 
   DEBUG ((DEBUG_INFO, "PeiMpInitLib: CpuMpEndOfPeiCallback () invoked\n"));
 
+  Status = PeiServicesGetBootMode (&BootMode);
+  ASSERT_EFI_ERROR (Status);
+
+  CpuMpData = GetCpuMpData ();
+  if (BootMode != BOOT_ON_S3_RESUME) {
+    //
+    // Get the HOB list for processing
+    //
+    Hob.Raw = GetHobList ();
+    //
+    // Collect memory ranges
+    //
+    while (!END_OF_HOB_LIST (Hob)) {
+      if (Hob.Header->HobType == EFI_HOB_TYPE_MEMORY_ALLOCATION) {
+        MemoryHob = Hob.MemoryAllocation;
+        if (MemoryHob->AllocDescriptor.MemoryBaseAddress == CpuMpData->WakeupBuffer) {
+          //
+          // Flag this HOB type to un-used
+          //
+          GET_HOB_TYPE (Hob) = EFI_HOB_TYPE_UNUSED;
+          break;
+        }
+      }
+      Hob.Raw = GET_NEXT_HOB (Hob);
+    }
+  } else {
+    CpuMpData->EndOfPeiFlag = TRUE;
+    RestoreWakeupBuffer (CpuMpData);
+  }
   return EFI_SUCCESS;
 }
+
+/**
+  Check if AP wakeup buffer is overlapped with existing allocated buffer.
+
+  @param[in]  WakeupBufferStart     AP wakeup buffer start address.
+  @param[in]  WakeupBufferEnd       AP wakeup buffer end address.
+
+  @retval  TRUE       There is overlap.
+  @retval  FALSE      There is no overlap.
+**/
+BOOLEAN
+CheckOverlapWithAllocatedBuffer (
+  IN UINTN                WakeupBufferStart,
+  IN UINTN                WakeupBufferEnd
+  )
+{
+  EFI_PEI_HOB_POINTERS      Hob;
+  EFI_HOB_MEMORY_ALLOCATION *MemoryHob;
+  BOOLEAN                   Overlapped;
+  UINTN                     MemoryStart;
+  UINTN                     MemoryEnd;
+
+  Overlapped = FALSE;
+  //
+  // Get the HOB list for processing
+  //
+  Hob.Raw = GetHobList ();
+  //
+  // Collect memory ranges
+  //
+  while (!END_OF_HOB_LIST (Hob)) {
+    if (Hob.Header->HobType == EFI_HOB_TYPE_MEMORY_ALLOCATION) {
+      MemoryHob   = Hob.MemoryAllocation;
+      MemoryStart = (UINTN) MemoryHob->AllocDescriptor.MemoryBaseAddress;
+      MemoryEnd   = (UINTN) (MemoryHob->AllocDescriptor.MemoryBaseAddress +
+                             MemoryHob->AllocDescriptor.MemoryLength);
+      if (!((WakeupBufferStart >= MemoryEnd) || (WakeupBufferEnd <= MemoryStart))) {
+        Overlapped = TRUE;
+        break;
+      }
+    }
+    Hob.Raw = GET_NEXT_HOB (Hob);
+  }
+  return Overlapped;
+}
+
+/**
+  Get available system memory below 1MB by specified size.
+
+  @param[in] WakeupBufferSize   Wakeup buffer size required
+
+  @retval other   Return wakeup buffer address below 1MB.
+  @retval -1      Cannot find free memory below 1MB.
+**/
+UINTN
+GetWakeupBuffer (
+  IN UINTN                WakeupBufferSize
+  )
+{
+  EFI_PEI_HOB_POINTERS    Hob;
+  UINTN                   WakeupBufferStart;
+  UINTN                   WakeupBufferEnd;
+
+  WakeupBufferSize = (WakeupBufferSize + SIZE_4KB - 1) & ~(SIZE_4KB - 1);
+
+  //
+  // Get the HOB list for processing
+  //
+  Hob.Raw = GetHobList ();
+
+  //
+  // Collect memory ranges
+  //
+  while (!END_OF_HOB_LIST (Hob)) {
+    if (Hob.Header->HobType == EFI_HOB_TYPE_RESOURCE_DESCRIPTOR) {
+      if ((Hob.ResourceDescriptor->PhysicalStart < BASE_1MB) &&
+          (Hob.ResourceDescriptor->ResourceType == EFI_RESOURCE_SYSTEM_MEMORY) &&
+          ((Hob.ResourceDescriptor->ResourceAttribute &
+            (EFI_RESOURCE_ATTRIBUTE_READ_PROTECTED |
+             EFI_RESOURCE_ATTRIBUTE_WRITE_PROTECTED |
+             EFI_RESOURCE_ATTRIBUTE_EXECUTION_PROTECTED
+             )) == 0)
+           ) {
+        //
+        // Need memory under 1MB to be collected here
+        //
+        WakeupBufferEnd = (UINTN) (Hob.ResourceDescriptor->PhysicalStart + Hob.ResourceDescriptor->ResourceLength);
+        if (WakeupBufferEnd > BASE_1MB) {
+          //
+          // Wakeup buffer should be under 1MB
+          //
+          WakeupBufferEnd = BASE_1MB;
+        }
+        while (WakeupBufferEnd > WakeupBufferSize) {
+          //
+          // Wakeup buffer should be aligned on 4KB
+          //
+          WakeupBufferStart = (WakeupBufferEnd - WakeupBufferSize) & ~(SIZE_4KB - 1);
+          if (WakeupBufferStart < Hob.ResourceDescriptor->PhysicalStart) {
+            break;
+          }
+          if (CheckOverlapWithAllocatedBuffer (WakeupBufferStart, WakeupBufferEnd)) {
+            //
+            // If this range is overlapped with existing allocated buffer, skip it
+            // and find the next range
+            //
+            WakeupBufferEnd -= WakeupBufferSize;
+            continue;
+          }
+          DEBUG ((DEBUG_INFO, "WakeupBufferStart = %x, WakeupBufferSize = %x\n",
+                               WakeupBufferStart, WakeupBufferSize));
+          //
+          // Create a memory allocation HOB.
+          //
+          BuildMemoryAllocationHob (
+            WakeupBufferStart,
+            WakeupBufferSize,
+            EfiBootServicesData
+            );
+          return WakeupBufferStart;
+        }
+      }
+    }
+    //
+    // Find the next HOB
+    //
+    Hob.Raw = GET_NEXT_HOB (Hob);
+  }
+
+  return (UINTN) -1;
+}
+
+/**
+  Allocate reset vector buffer.
+
+  @param[in, out]  CpuMpData  The pointer to CPU MP Data structure.
+**/
+VOID
+AllocateResetVector (
+  IN OUT CPU_MP_DATA          *CpuMpData
+  )
+{
+  UINTN           ApResetVectorSize;
+
+  if (CpuMpData->WakeupBuffer == (UINTN) -1) {
+    ApResetVectorSize = CpuMpData->AddressMap.RendezvousFunnelSize +
+                          sizeof (MP_CPU_EXCHANGE_INFO);
+
+    CpuMpData->WakeupBuffer      = GetWakeupBuffer (ApResetVectorSize);
+    CpuMpData->MpCpuExchangeInfo = (MP_CPU_EXCHANGE_INFO *) (UINTN)
+                    (CpuMpData->WakeupBuffer + CpuMpData->AddressMap.RendezvousFunnelSize);
+    BackupAndPrepareWakeupBuffer (CpuMpData);
+  }
+
+  if (CpuMpData->EndOfPeiFlag) {
+    BackupAndPrepareWakeupBuffer (CpuMpData);
+  }
+}
+
+/**
+  Free AP reset vector buffer.
+
+  @param[in]  CpuMpData  The pointer to CPU MP Data structure.
+**/
+VOID
+FreeResetVector (
+  IN CPU_MP_DATA              *CpuMpData
+  )
+{
+  if (CpuMpData->EndOfPeiFlag) {
+    RestoreWakeupBuffer (CpuMpData);
+  }
+}
+
 /**
   Initialize global data for MP support.
 
-- 
2.7.4.windows.1