]> git.proxmox.com Git - mirror_edk2.git/blobdiff - UefiCpuPkg/Library/MpInitLib/MpLib.c
UefiCpuPkg: Replace BSD License with BSD+Patent License
[mirror_edk2.git] / UefiCpuPkg / Library / MpInitLib / MpLib.c
index 8ec016e928559d02526a0509af86cd882d8f93ad..33374888922a85d80fbe4f4735f1f24446b03926 100644 (file)
@@ -2,13 +2,7 @@
   CPU MP Initialize Library common functions.\r
 \r
   Copyright (c) 2016 - 2018, Intel Corporation. All rights reserved.<BR>\r
-  This program and the accompanying materials\r
-  are licensed and made available under the terms and conditions of the BSD License\r
-  which accompanies this distribution.  The full text of the license may be found at\r
-  http://opensource.org/licenses/bsd-license.php\r
-\r
-  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,\r
-  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.\r
+  SPDX-License-Identifier: BSD-2-Clause-Patent\r
 \r
 **/\r
 \r
@@ -217,9 +211,9 @@ RestoreVolatileRegisters (
   CPUID_VERSION_INFO_EDX        VersionInfoEdx;\r
   IA32_TSS_DESCRIPTOR           *Tss;\r
 \r
-  AsmWriteCr0 (VolatileRegisters->Cr0);\r
   AsmWriteCr3 (VolatileRegisters->Cr3);\r
   AsmWriteCr4 (VolatileRegisters->Cr4);\r
+  AsmWriteCr0 (VolatileRegisters->Cr0);\r
 \r
   if (IsRestoreDr) {\r
     AsmCpuid (CPUID_VERSION_INFO, NULL, NULL, NULL, &VersionInfoEdx.Uint32);\r
@@ -330,6 +324,7 @@ SortApicId (
   CPU_INFO_IN_HOB   CpuInfo;\r
   UINT32            ApCount;\r
   CPU_INFO_IN_HOB   *CpuInfoInHob;\r
+  volatile UINT32   *StartupApSignal;\r
 \r
   ApCount = CpuMpData->CpuCount - 1;\r
   CpuInfoInHob = (CPU_INFO_IN_HOB *) (UINTN) CpuMpData->CpuInfoInHob;\r
@@ -354,6 +349,14 @@ SortApicId (
           sizeof (CPU_INFO_IN_HOB)\r
           );\r
         CopyMem (&CpuInfoInHob[Index1], &CpuInfo, sizeof (CPU_INFO_IN_HOB));\r
+\r
+        //\r
+        // Also exchange the StartupApSignal.\r
+        //\r
+        StartupApSignal = CpuMpData->CpuData[Index3].StartupApSignal;\r
+        CpuMpData->CpuData[Index3].StartupApSignal =\r
+          CpuMpData->CpuData[Index1].StartupApSignal;\r
+        CpuMpData->CpuData[Index1].StartupApSignal = StartupApSignal;\r
       }\r
     }\r
 \r
@@ -401,7 +404,7 @@ ApInitializeSync (
   //\r
   // Load microcode on AP\r
   //\r
-  MicrocodeDetect (CpuMpData);\r
+  MicrocodeDetect (CpuMpData, FALSE);\r
   //\r
   // Sync BSP's MTRR table to AP\r
   //\r
@@ -426,16 +429,19 @@ GetProcessorNumber (
   UINTN                   TotalProcessorNumber;\r
   UINTN                   Index;\r
   CPU_INFO_IN_HOB         *CpuInfoInHob;\r
+  UINT32                  CurrentApicId;\r
 \r
   CpuInfoInHob = (CPU_INFO_IN_HOB *) (UINTN) CpuMpData->CpuInfoInHob;\r
 \r
   TotalProcessorNumber = CpuMpData->CpuCount;\r
+  CurrentApicId = GetApicId ();\r
   for (Index = 0; Index < TotalProcessorNumber; Index ++) {\r
-    if (CpuInfoInHob[Index].ApicId == GetApicId ()) {\r
+    if (CpuInfoInHob[Index].ApicId == CurrentApicId) {\r
       *ProcessorNumber = Index;\r
       return EFI_SUCCESS;\r
     }\r
   }\r
+\r
   return EFI_NOT_FOUND;\r
 }\r
 \r
@@ -458,7 +464,7 @@ CollectProcessorCount (
   //\r
   CpuMpData->InitFlag     = ApInitConfig;\r
   CpuMpData->X2ApicEnable = FALSE;\r
-  WakeUpAP (CpuMpData, TRUE, 0, NULL, NULL);\r
+  WakeUpAP (CpuMpData, TRUE, 0, NULL, NULL, TRUE);\r
   CpuMpData->InitFlag = ApInitDone;\r
   ASSERT (CpuMpData->CpuCount <= PcdGet32 (PcdCpuMaxLogicalProcessorNumber));\r
   //\r
@@ -479,7 +485,7 @@ CollectProcessorCount (
     //\r
     // Wakeup all APs to enable x2APIC mode\r
     //\r
-    WakeUpAP (CpuMpData, TRUE, 0, ApFuncEnableX2Apic, NULL);\r
+    WakeUpAP (CpuMpData, TRUE, 0, ApFuncEnableX2Apic, NULL, TRUE);\r
     //\r
     // Wait for all known APs finished\r
     //\r
@@ -606,7 +612,9 @@ ApWakeupFunction (
       //\r
       ApInitializeSync (CpuMpData);\r
       //\r
-      // Sync BSP's Control registers to APs\r
+      // CpuMpData->CpuData[0].VolatileRegisters is initialized based on BSP environment,\r
+      //   to initialize AP in InitConfig path.\r
+      // NOTE: IDTR.BASE stored in CpuMpData->CpuData[0].VolatileRegisters points to a different IDT shared by all APs.\r
       //\r
       RestoreVolatileRegisters (&CpuMpData->CpuData[0].VolatileRegisters, FALSE);\r
       InitializeApData (CpuMpData, ProcessorNumber, BistData, ApTopOfStack);\r
@@ -630,6 +638,13 @@ ApWakeupFunction (
         // Restore AP's volatile registers saved\r
         //\r
         RestoreVolatileRegisters (&CpuMpData->CpuData[ProcessorNumber].VolatileRegisters, TRUE);\r
+      } else {\r
+        //\r
+        // The CPU driver might not flush TLB for APs on spot after updating\r
+        // page attributes. AP in mwait loop mode needs to take care of it when\r
+        // woken up.\r
+        //\r
+        CpuFlushTlb ();\r
       }\r
 \r
       if (GetApState (&CpuMpData->CpuData[ProcessorNumber]) == CpuStateReady) {\r
@@ -639,7 +654,7 @@ ApWakeupFunction (
           SetApState (&CpuMpData->CpuData[ProcessorNumber], CpuStateBusy);\r
           //\r
           // Enable source debugging on AP function\r
-          //         \r
+          //\r
           EnableDebugAgent ();\r
           //\r
           // Invoke AP function here\r
@@ -772,6 +787,8 @@ FillExchangeInfoData (
   )\r
 {\r
   volatile MP_CPU_EXCHANGE_INFO    *ExchangeInfo;\r
+  UINTN                            Size;\r
+  IA32_SEGMENT_DESCRIPTOR          *Selector;\r
 \r
   ExchangeInfo                  = CpuMpData->MpCpuExchangeInfo;\r
   ExchangeInfo->Lock            = 0;\r
@@ -801,6 +818,46 @@ FillExchangeInfoData (
   //\r
   AsmReadGdtr ((IA32_DESCRIPTOR *) &ExchangeInfo->GdtrProfile);\r
   AsmReadIdtr ((IA32_DESCRIPTOR *) &ExchangeInfo->IdtrProfile);\r
+\r
+  //\r
+  // Find a 32-bit code segment\r
+  //\r
+  Selector = (IA32_SEGMENT_DESCRIPTOR *)ExchangeInfo->GdtrProfile.Base;\r
+  Size = ExchangeInfo->GdtrProfile.Limit + 1;\r
+  while (Size > 0) {\r
+    if (Selector->Bits.L == 0 && Selector->Bits.Type >= 8) {\r
+      ExchangeInfo->ModeTransitionSegment =\r
+        (UINT16)((UINTN)Selector - ExchangeInfo->GdtrProfile.Base);\r
+      break;\r
+    }\r
+    Selector += 1;\r
+    Size -= sizeof (IA32_SEGMENT_DESCRIPTOR);\r
+  }\r
+\r
+  //\r
+  // Copy all 32-bit code and 64-bit code into memory with type of\r
+  // EfiBootServicesCode to avoid page fault if NX memory protection is enabled.\r
+  //\r
+  if (CpuMpData->WakeupBufferHigh != 0) {\r
+    Size = CpuMpData->AddressMap.RendezvousFunnelSize -\r
+           CpuMpData->AddressMap.ModeTransitionOffset;\r
+    CopyMem (\r
+      (VOID *)CpuMpData->WakeupBufferHigh,\r
+      CpuMpData->AddressMap.RendezvousFunnelAddress +\r
+      CpuMpData->AddressMap.ModeTransitionOffset,\r
+      Size\r
+      );\r
+\r
+    ExchangeInfo->ModeTransitionMemory = (UINT32)CpuMpData->WakeupBufferHigh;\r
+  } else {\r
+    ExchangeInfo->ModeTransitionMemory = (UINT32)\r
+      (ExchangeInfo->BufferStart + CpuMpData->AddressMap.ModeTransitionOffset);\r
+  }\r
+\r
+  ExchangeInfo->ModeHighMemory = ExchangeInfo->ModeTransitionMemory +\r
+                         (UINT32)ExchangeInfo->ModeOffset -\r
+                         (UINT32)CpuMpData->AddressMap.ModeTransitionOffset;\r
+  ExchangeInfo->ModeHighSegment = (UINT16)ExchangeInfo->CodeSegment;\r
 }\r
 \r
 /**\r
@@ -876,6 +933,10 @@ AllocateResetVector (
     CpuMpData->WakeupBuffer      = GetWakeupBuffer (ApResetVectorSize);\r
     CpuMpData->MpCpuExchangeInfo = (MP_CPU_EXCHANGE_INFO *) (UINTN)\r
                     (CpuMpData->WakeupBuffer + CpuMpData->AddressMap.RendezvousFunnelSize);\r
+    CpuMpData->WakeupBufferHigh  = GetModeTransitionBuffer (\r
+                                    CpuMpData->AddressMap.RendezvousFunnelSize -\r
+                                    CpuMpData->AddressMap.ModeTransitionOffset\r
+                                    );\r
   }\r
   BackupAndPrepareWakeupBuffer (CpuMpData);\r
 }\r
@@ -902,6 +963,7 @@ FreeResetVector (
   @param[in] ProcessorNumber    The handle number of specified processor\r
   @param[in] Procedure          The function to be invoked by AP\r
   @param[in] ProcedureArgument  The argument to be passed into AP function\r
+  @param[in] WakeUpDisabledAps  Whether need to wake up disabled APs in broadcast mode.\r
 **/\r
 VOID\r
 WakeUpAP (\r
@@ -909,7 +971,8 @@ WakeUpAP (
   IN BOOLEAN                   Broadcast,\r
   IN UINTN                     ProcessorNumber,\r
   IN EFI_AP_PROCEDURE          Procedure,              OPTIONAL\r
-  IN VOID                      *ProcedureArgument      OPTIONAL\r
+  IN VOID                      *ProcedureArgument,     OPTIONAL\r
+  IN BOOLEAN                   WakeUpDisabledAps\r
   )\r
 {\r
   volatile MP_CPU_EXCHANGE_INFO    *ExchangeInfo;\r
@@ -921,13 +984,15 @@ WakeUpAP (
   CpuMpData->FinishedCount = 0;\r
   ResetVectorRequired = FALSE;\r
 \r
-  if (CpuMpData->ApLoopMode == ApInHltLoop ||\r
+  if (CpuMpData->WakeUpByInitSipiSipi ||\r
       CpuMpData->InitFlag   != ApInitDone) {\r
     ResetVectorRequired = TRUE;\r
     AllocateResetVector (CpuMpData);\r
     FillExchangeInfoData (CpuMpData);\r
     SaveLocalApicTimerSetting (CpuMpData);\r
-  } else if (CpuMpData->ApLoopMode == ApInMwaitLoop) {\r
+  }\r
+\r
+  if (CpuMpData->ApLoopMode == ApInMwaitLoop) {\r
     //\r
     // Get AP target C-state each time when waking up AP,\r
     // for it maybe updated by platform again\r
@@ -941,6 +1006,15 @@ WakeUpAP (
     for (Index = 0; Index < CpuMpData->CpuCount; Index++) {\r
       if (Index != CpuMpData->BspNumber) {\r
         CpuData = &CpuMpData->CpuData[Index];\r
+        //\r
+        // All AP(include disabled AP) will be woke up by INIT-SIPI-SIPI, but\r
+        // the AP procedure will be skipped for disabled AP because AP state\r
+        // is not CpuStateReady.\r
+        //\r
+        if (GetApState (CpuData) == CpuStateDisabled && !WakeUpDisabledAps) {\r
+          continue;\r
+        }\r
+\r
         CpuData->ApFunction         = (UINTN) Procedure;\r
         CpuData->ApFunctionArgument = (UINTN) ProcedureArgument;\r
         SetApState (CpuData, CpuStateReady);\r
@@ -1012,6 +1086,13 @@ WakeUpAP (
   if (ResetVectorRequired) {\r
     FreeResetVector (CpuMpData);\r
   }\r
+\r
+  //\r
+  // After one round of Wakeup Ap actions, need to re-sync ApLoopMode with\r
+  // WakeUpByInitSipiSipi flag. WakeUpByInitSipiSipi flag maybe changed by\r
+  // S3SmmInitDone Ppi.\r
+  //\r
+  CpuMpData->WakeUpByInitSipiSipi = (CpuMpData->ApLoopMode == ApInHltLoop);\r
 }\r
 \r
 /**\r
@@ -1053,7 +1134,7 @@ CalculateTimeout (
 \r
   //\r
   // GetPerformanceCounterProperties () returns the timestamp counter's frequency\r
-  // in Hz. \r
+  // in Hz.\r
   //\r
   TimestampCounterFreq = GetPerformanceCounterProperties (NULL, NULL);\r
 \r
@@ -1213,7 +1294,7 @@ ResetProcessorToIdleState (
   CpuMpData = GetCpuMpData ();\r
 \r
   CpuMpData->InitFlag = ApInitReconfig;\r
-  WakeUpAP (CpuMpData, FALSE, ProcessorNumber, NULL, NULL);\r
+  WakeUpAP (CpuMpData, FALSE, ProcessorNumber, NULL, NULL, TRUE);\r
   while (CpuMpData->FinishedCount < 1) {\r
     CpuPause ();\r
   }\r
@@ -1276,9 +1357,9 @@ CheckThisAP (
   CpuData   = &CpuMpData->CpuData[ProcessorNumber];\r
 \r
   //\r
-  //  Check the CPU state of AP. If it is CpuStateFinished, then the AP has finished its task.\r
+  //  Check the CPU state of AP. If it is CpuStateIdle, then the AP has finished its task.\r
   //  Only BSP and corresponding AP access this unit of CPU Data. This means the AP will not modify the\r
-  //  value of state after setting the it to CpuStateFinished, so BSP can safely make use of its value.\r
+  //  value of state after setting the it to CpuStateIdle, so BSP can safely make use of its value.\r
   //\r
   //\r
   // If the AP finishes for StartupThisAP(), return EFI_SUCCESS.\r
@@ -1344,12 +1425,12 @@ CheckAllAPs (
 \r
     CpuData = &CpuMpData->CpuData[ProcessorNumber];\r
     //\r
-    // Check the CPU state of AP. If it is CpuStateFinished, then the AP has finished its task.\r
+    // Check the CPU state of AP. If it is CpuStateIdle, then the AP has finished its task.\r
     // Only BSP and corresponding AP access this unit of CPU Data. This means the AP will not modify the\r
-    // value of state after setting the it to CpuStateFinished, so BSP can safely make use of its value.\r
+    // value of state after setting the it to CpuStateIdle, so BSP can safely make use of its value.\r
     //\r
     if (GetApState(CpuData) == CpuStateFinished) {\r
-      CpuMpData->RunningCount ++;\r
+      CpuMpData->RunningCount --;\r
       CpuMpData->CpuData[ProcessorNumber].Waiting = FALSE;\r
       SetApState(CpuData, CpuStateIdle);\r
 \r
@@ -1365,7 +1446,8 @@ CheckAllAPs (
             FALSE,\r
             (UINT32) NextProcessorNumber,\r
             CpuMpData->Procedure,\r
-            CpuMpData->ProcArguments\r
+            CpuMpData->ProcArguments,\r
+            TRUE\r
             );\r
          }\r
       }\r
@@ -1375,7 +1457,7 @@ CheckAllAPs (
   //\r
   // If all APs finish, return EFI_SUCCESS.\r
   //\r
-  if (CpuMpData->RunningCount == CpuMpData->StartCount) {\r
+  if (CpuMpData->RunningCount == 0) {\r
     return EFI_SUCCESS;\r
   }\r
 \r
@@ -1392,7 +1474,7 @@ CheckAllAPs (
     //\r
     if (CpuMpData->FailedCpuList != NULL) {\r
       *CpuMpData->FailedCpuList =\r
-         AllocatePool ((CpuMpData->StartCount - CpuMpData->FinishedCount + 1) * sizeof (UINTN));\r
+         AllocatePool ((CpuMpData->RunningCount + 1) * sizeof (UINTN));\r
       ASSERT (*CpuMpData->FailedCpuList != NULL);\r
     }\r
     ListIndex = 0;\r
@@ -1444,6 +1526,7 @@ MpInitLibInitialize (
   UINT32                   MaxLogicalProcessorNumber;\r
   UINT32                   ApStackSize;\r
   MP_ASSEMBLY_ADDRESS_MAP  AddressMap;\r
+  CPU_VOLATILE_REGISTERS   VolatileRegisters;\r
   UINTN                    BufferSize;\r
   UINT32                   MonitorFilterSize;\r
   VOID                     *MpBuffer;\r
@@ -1454,6 +1537,8 @@ MpInitLibInitialize (
   UINTN                    Index;\r
   UINTN                    ApResetVectorSize;\r
   UINTN                    BackupBufferAddr;\r
+  UINTN                    ApIdtBase;\r
+  VOID                     *MicrocodePatchInRam;\r
 \r
   OldCpuMpData = GetCpuMpDataFromGuidedHob ();\r
   if (OldCpuMpData == NULL) {\r
@@ -1468,19 +1553,48 @@ MpInitLibInitialize (
   ApStackSize = PcdGet32(PcdCpuApStackSize);\r
   ApLoopMode  = GetApLoopMode (&MonitorFilterSize);\r
 \r
+  //\r
+  // Save BSP's Control registers for APs.\r
+  //\r
+  SaveVolatileRegisters (&VolatileRegisters);\r
+\r
   BufferSize  = ApStackSize * MaxLogicalProcessorNumber;\r
   BufferSize += MonitorFilterSize * MaxLogicalProcessorNumber;\r
-  BufferSize += sizeof (CPU_MP_DATA);\r
   BufferSize += ApResetVectorSize;\r
+  BufferSize  = ALIGN_VALUE (BufferSize, 8);\r
+  BufferSize += VolatileRegisters.Idtr.Limit + 1;\r
+  BufferSize += sizeof (CPU_MP_DATA);\r
   BufferSize += (sizeof (CPU_AP_DATA) + sizeof (CPU_INFO_IN_HOB))* MaxLogicalProcessorNumber;\r
   MpBuffer    = AllocatePages (EFI_SIZE_TO_PAGES (BufferSize));\r
   ASSERT (MpBuffer != NULL);\r
   ZeroMem (MpBuffer, BufferSize);\r
   Buffer = (UINTN) MpBuffer;\r
 \r
+  //\r
+  //  The layout of the Buffer is as below:\r
+  //\r
+  //    +--------------------+ <-- Buffer\r
+  //        AP Stacks (N)\r
+  //    +--------------------+ <-- MonitorBuffer\r
+  //    AP Monitor Filters (N)\r
+  //    +--------------------+ <-- BackupBufferAddr (CpuMpData->BackupBuffer)\r
+  //         Backup Buffer\r
+  //    +--------------------+\r
+  //           Padding\r
+  //    +--------------------+ <-- ApIdtBase (8-byte boundary)\r
+  //           AP IDT          All APs share one separate IDT. So AP can get address of CPU_MP_DATA from IDT Base.\r
+  //    +--------------------+ <-- CpuMpData\r
+  //         CPU_MP_DATA\r
+  //    +--------------------+ <-- CpuMpData->CpuData\r
+  //        CPU_AP_DATA (N)\r
+  //    +--------------------+ <-- CpuMpData->CpuInfoInHob\r
+  //      CPU_INFO_IN_HOB (N)\r
+  //    +--------------------+\r
+  //\r
   MonitorBuffer    = (UINT8 *) (Buffer + ApStackSize * MaxLogicalProcessorNumber);\r
   BackupBufferAddr = (UINTN) MonitorBuffer + MonitorFilterSize * MaxLogicalProcessorNumber;\r
-  CpuMpData = (CPU_MP_DATA *) (BackupBufferAddr + ApResetVectorSize);\r
+  ApIdtBase        = ALIGN_VALUE (BackupBufferAddr + ApResetVectorSize, 8);\r
+  CpuMpData        = (CPU_MP_DATA *) (ApIdtBase + VolatileRegisters.Idtr.Limit + 1);\r
   CpuMpData->Buffer           = Buffer;\r
   CpuMpData->CpuApStackSize   = ApStackSize;\r
   CpuMpData->BackupBuffer     = BackupBufferAddr;\r
@@ -1492,13 +1606,57 @@ MpInitLibInitialize (
   CpuMpData->SwitchBspFlag    = FALSE;\r
   CpuMpData->CpuData          = (CPU_AP_DATA *) (CpuMpData + 1);\r
   CpuMpData->CpuInfoInHob     = (UINT64) (UINTN) (CpuMpData->CpuData + MaxLogicalProcessorNumber);\r
-  CpuMpData->MicrocodePatchAddress    = PcdGet64 (PcdCpuMicrocodePatchAddress);\r
   CpuMpData->MicrocodePatchRegionSize = PcdGet64 (PcdCpuMicrocodePatchRegionSize);\r
+  //\r
+  // If platform has more than one CPU, relocate microcode to memory to reduce\r
+  // loading microcode time.\r
+  //\r
+  MicrocodePatchInRam = NULL;\r
+  if (MaxLogicalProcessorNumber > 1) {\r
+    MicrocodePatchInRam = AllocatePages (\r
+                            EFI_SIZE_TO_PAGES (\r
+                              (UINTN)CpuMpData->MicrocodePatchRegionSize\r
+                              )\r
+                            );\r
+  }\r
+  if (MicrocodePatchInRam == NULL) {\r
+    //\r
+    // there is only one processor, or no microcode patch is available, or\r
+    // memory allocation failed\r
+    //\r
+    CpuMpData->MicrocodePatchAddress = PcdGet64 (PcdCpuMicrocodePatchAddress);\r
+  } else {\r
+    //\r
+    // there are multiple processors, and a microcode patch is available, and\r
+    // memory allocation succeeded\r
+    //\r
+    CopyMem (\r
+      MicrocodePatchInRam,\r
+      (VOID *)(UINTN)PcdGet64 (PcdCpuMicrocodePatchAddress),\r
+      (UINTN)CpuMpData->MicrocodePatchRegionSize\r
+      );\r
+    CpuMpData->MicrocodePatchAddress = (UINTN)MicrocodePatchInRam;\r
+  }\r
+\r
   InitializeSpinLock(&CpuMpData->MpLock);\r
+\r
+  //\r
+  // Make sure no memory usage outside of the allocated buffer.\r
   //\r
-  // Save BSP's Control registers to APs\r
+  ASSERT ((CpuMpData->CpuInfoInHob + sizeof (CPU_INFO_IN_HOB) * MaxLogicalProcessorNumber) ==\r
+          Buffer + BufferSize);\r
+\r
+  //\r
+  // Duplicate BSP's IDT to APs.\r
+  // All APs share one separate IDT. So AP can get the address of CpuMpData by using IDTR.BASE + IDTR.LIMIT + 1\r
+  //\r
+  CopyMem ((VOID *)ApIdtBase, (VOID *)VolatileRegisters.Idtr.Base, VolatileRegisters.Idtr.Limit + 1);\r
+  VolatileRegisters.Idtr.Base = ApIdtBase;\r
+  //\r
+  // Don't pass BSP's TR to APs to avoid AP init failure.\r
   //\r
-  SaveVolatileRegisters (&CpuMpData->CpuData[0].VolatileRegisters);\r
+  VolatileRegisters.Tr = 0;\r
+  CopyMem (&CpuMpData->CpuData[0].VolatileRegisters, &VolatileRegisters, sizeof (VolatileRegisters));\r
   //\r
   // Set BSP basic information\r
   //\r
@@ -1512,6 +1670,9 @@ MpInitLibInitialize (
   //\r
   CpuMpData->ApLoopMode = ApLoopMode;\r
   DEBUG ((DEBUG_INFO, "AP Loop Mode is %d\n", CpuMpData->ApLoopMode));\r
+\r
+  CpuMpData->WakeUpByInitSipiSipi = (CpuMpData->ApLoopMode == ApInHltLoop);\r
+\r
   //\r
   // Set up APs wakeup signal buffer\r
   //\r
@@ -1522,7 +1683,7 @@ MpInitLibInitialize (
   //\r
   // Load Microcode on BSP\r
   //\r
-  MicrocodeDetect (CpuMpData);\r
+  MicrocodeDetect (CpuMpData, TRUE);\r
   //\r
   // Store BSP's MTRR setting\r
   //\r
@@ -1556,17 +1717,13 @@ MpInitLibInitialize (
       }\r
       CpuMpData->CpuData[Index].CpuHealthy = (CpuInfoInHob[Index].Health == 0)? TRUE:FALSE;\r
       CpuMpData->CpuData[Index].ApFunction = 0;\r
-      CopyMem (\r
-        &CpuMpData->CpuData[Index].VolatileRegisters,\r
-        &CpuMpData->CpuData[0].VolatileRegisters,\r
-        sizeof (CPU_VOLATILE_REGISTERS)\r
-        );\r
+      CopyMem (&CpuMpData->CpuData[Index].VolatileRegisters, &VolatileRegisters, sizeof (CPU_VOLATILE_REGISTERS));\r
     }\r
     if (MaxLogicalProcessorNumber > 1) {\r
       //\r
       // Wakeup APs to do some AP initialize sync\r
       //\r
-      WakeUpAP (CpuMpData, TRUE, 0, ApInitializeSync, CpuMpData);\r
+      WakeUpAP (CpuMpData, TRUE, 0, ApInitializeSync, CpuMpData, TRUE);\r
       //\r
       // Wait for all APs finished initialization\r
       //\r
@@ -1675,7 +1832,7 @@ MpInitLibGetProcessorInfo (
                                enabled AP. Otherwise, it will be disabled.\r
 \r
   @retval EFI_SUCCESS          BSP successfully switched.\r
-  @retval others               Failed to switch BSP. \r
+  @retval others               Failed to switch BSP.\r
 \r
 **/\r
 EFI_STATUS\r
@@ -1761,7 +1918,7 @@ SwitchBSPWorker (
   //\r
   // Need to wakeUp AP (future BSP).\r
   //\r
-  WakeUpAP (CpuMpData, FALSE, ProcessorNumber, FutureBSPProc, CpuMpData);\r
+  WakeUpAP (CpuMpData, FALSE, ProcessorNumber, FutureBSPProc, CpuMpData, TRUE);\r
 \r
   AsmExchangeRole (&CpuMpData->BSPInfo, &CpuMpData->APInfo);\r
 \r
@@ -2067,7 +2224,7 @@ StartupAllAPsWorker (
     return EFI_NOT_STARTED;\r
   }\r
 \r
-  CpuMpData->StartCount = 0;\r
+  CpuMpData->RunningCount = 0;\r
   for (ProcessorNumber = 0; ProcessorNumber < ProcessorCount; ProcessorNumber++) {\r
     CpuData = &CpuMpData->CpuData[ProcessorNumber];\r
     CpuData->Waiting = FALSE;\r
@@ -2077,7 +2234,7 @@ StartupAllAPsWorker (
         // Mark this processor as responsible for current calling.\r
         //\r
         CpuData->Waiting = TRUE;\r
-        CpuMpData->StartCount++;\r
+        CpuMpData->RunningCount++;\r
       }\r
     }\r
   }\r
@@ -2086,7 +2243,6 @@ StartupAllAPsWorker (
   CpuMpData->ProcArguments = ProcedureArgument;\r
   CpuMpData->SingleThread  = SingleThread;\r
   CpuMpData->FinishedCount = 0;\r
-  CpuMpData->RunningCount  = 0;\r
   CpuMpData->FailedCpuList = FailedCpuList;\r
   CpuMpData->ExpectedTime  = CalculateTimeout (\r
                                TimeoutInMicroseconds,\r
@@ -2096,14 +2252,14 @@ StartupAllAPsWorker (
   CpuMpData->WaitEvent     = WaitEvent;\r
 \r
   if (!SingleThread) {\r
-    WakeUpAP (CpuMpData, TRUE, 0, Procedure, ProcedureArgument);\r
+    WakeUpAP (CpuMpData, TRUE, 0, Procedure, ProcedureArgument, FALSE);\r
   } else {\r
     for (ProcessorNumber = 0; ProcessorNumber < ProcessorCount; ProcessorNumber++) {\r
       if (ProcessorNumber == CallerNumber) {\r
         continue;\r
       }\r
       if (CpuMpData->CpuData[ProcessorNumber].Waiting) {\r
-        WakeUpAP (CpuMpData, FALSE, ProcessorNumber, Procedure, ProcedureArgument);\r
+        WakeUpAP (CpuMpData, FALSE, ProcessorNumber, Procedure, ProcedureArgument, TRUE);\r
         break;\r
       }\r
     }\r
@@ -2215,7 +2371,7 @@ StartupThisAPWorker (
   CpuData->ExpectedTime = CalculateTimeout (TimeoutInMicroseconds, &CpuData->CurrentTime);\r
   CpuData->TotalTime    = 0;\r
 \r
-  WakeUpAP (CpuMpData, FALSE, ProcessorNumber, Procedure, ProcedureArgument);\r
+  WakeUpAP (CpuMpData, FALSE, ProcessorNumber, Procedure, ProcedureArgument, TRUE);\r
 \r
   //\r
   // If WaitEvent is NULL, execute in blocking mode.\r