[Patch 6/6] UefiCpuPkg/MpInitLib: Don't allocate reset vector in Exit Boot Service

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

In Exit Boot Services callback function, we cannot use allocate memory services
because it may change the memory map that has been gotten by OS.

This fix is not to allocate reset vector buffer after SaveRestoreFlag is set to
TRUE in MpInitExitBootServicesCallback(). Instead AllocateResetVector() will use
the previous allocated buffer address and save the contents before copying reset
vector code. At the same time, FreeResetVector() will restore original contents
after if SaveRestoreFlag is TRUE.

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

diff --git a/UefiCpuPkg/Library/MpInitLib/DxeMpLib.c b/UefiCpuPkg/Library/MpInitLib/DxeMpLib.c
index e459ebc..50b5b27 100644
--- a/UefiCpuPkg/Library/MpInitLib/DxeMpLib.c
+++ b/UefiCpuPkg/Library/MpInitLib/DxeMpLib.c
@@ -66,29 +66,33 @@ AllocateResetVector (
   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)
+  if (CpuMpData->SaveRestoreFlag) {
+    BackupAndPrepareWakeupBuffer (CpuMpData);
+  } else {
+    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
-    );
+    //
+    // copy AP reset code in it
+    //
+    CopyMem (
+      (VOID *) CpuMpData->WakeupBuffer,
+      (VOID *) CpuMpData->AddressMap.RendezvousFunnelAddress,
+      CpuMpData->AddressMap.RendezvousFunnelSize
+      );
+  }
 }
 
 /**
@@ -103,13 +107,18 @@ FreeResetVector (
 {
   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);
+
+  if (CpuMpData->SaveRestoreFlag) {
+    RestoreWakeupBuffer (CpuMpData);
+  } else {
+    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);
+  }
 }
 
 /**
@@ -260,6 +269,7 @@ MpInitExitBootServicesCallback (
   CPU_MP_DATA               *CpuMpData;
 
   CpuMpData = GetCpuMpData ();
+  CpuMpData->SaveRestoreFlag = TRUE;
   CpuMpData->PmCodeSegment = GetProtectedModeCS ();
   CpuMpData->ApLoopMode = PcdGet8 (PcdCpuApLoopMode);
   WakeUpAP (CpuMpData, TRUE, 0, RelocateApLoop, mReservedApLoopFunc);
-- 
2.7.4.windows.1