diff --git a/UefiCpuPkg/CpuDxe/CpuMp.c b/UefiCpuPkg/CpuDxe/CpuMp.c
index 13fcda5859..10065762a2 100644
--- a/UefiCpuPkg/CpuDxe/CpuMp.c
+++ b/UefiCpuPkg/CpuDxe/CpuMp.c
@@ -25,7 +25,7 @@ VOID *mCommonStack = 0;
 VOID *mTopOfApCommonStack = 0;
 VOID *mApStackStart = 0;
 
-BOOLEAN mAPsAlreadyInitFinished = FALSE;
+volatile BOOLEAN mAPsAlreadyInitFinished = FALSE;
 volatile BOOLEAN mStopCheckAllAPsStatus = TRUE;
 
 EFI_MP_SERVICES_PROTOCOL  mMpServicesTemplate = {
@@ -1135,11 +1135,15 @@ ProcessorToIdleState (
   EFI_AP_PROCEDURE      Procedure;
   VOID                  *ProcedureArgument;
 
+  AsmApDoneWithCommonStack ();
+
+  while (!mAPsAlreadyInitFinished) {
+    CpuPause ();
+  }
+
   WhoAmI (&mMpServicesTemplate, &ProcessorNumber);
   CpuData = &mMpSystemData.CpuDatas[ProcessorNumber];
 
-  AsmApDoneWithCommonStack ();
-
   //
   // Avoid forcibly reset AP caused the AP State is not updated.
   //
@@ -1486,6 +1490,11 @@ InitializeMpSupport (
     return;
   }
 
+  mMpSystemData.CpuDatas = ReallocatePool (
+                             sizeof (CPU_DATA_BLOCK) * gMaxLogicalProcessorNumber,
+                             sizeof (CPU_DATA_BLOCK) * mMpSystemData.NumberOfProcessors,
+                             mMpSystemData.CpuDatas);
+
   mAPsAlreadyInitFinished = TRUE;
 
   if (mMpSystemData.NumberOfProcessors < gMaxLogicalProcessorNumber) {