]> git.proxmox.com Git - mirror_edk2.git/blobdiff - UefiCpuPkg/Library/MpInitLib/MpLib.c
UefiCpuPkg: Change complex DEBUG_CODE() to DEBUG_CODE_BEGIN/END()
[mirror_edk2.git] / UefiCpuPkg / Library / MpInitLib / MpLib.c
index 377876643f0a1247bbcc58431aead53164712237..b390c558d4bd51f158e4a5ade038be42223af10c 100644 (file)
@@ -1,21 +1,21 @@
 /** @file\r
   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
+  Copyright (c) 2016 - 2021, Intel Corporation. All rights reserved.<BR>\r
+  Copyright (c) 2020, AMD Inc. All rights reserved.<BR>\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
 #include "MpLib.h"\r
+#include <Library/VmgExitLib.h>\r
+#include <Register/Amd/Fam17Msr.h>\r
+#include <Register/Amd/Ghcb.h>\r
 \r
 EFI_GUID mCpuInitMpLibHobGuid = CPU_INIT_MP_LIB_HOB_GUID;\r
 \r
+\r
 /**\r
   The function will check if BSP Execute Disable is enabled.\r
 \r
@@ -217,9 +217,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
@@ -294,6 +294,14 @@ GetApLoopMode (
       //\r
       ApLoopMode = ApInHltLoop;\r
     }\r
+\r
+    if (PcdGetBool (PcdSevEsIsEnabled)) {\r
+      //\r
+      // For SEV-ES, force AP in Hlt-loop mode in order to use the GHCB\r
+      // protocol for starting APs\r
+      //\r
+      ApLoopMode = ApInHltLoop;\r
+    }\r
   }\r
 \r
   if (ApLoopMode != ApInMwaitLoop) {\r
@@ -405,12 +413,16 @@ ApInitializeSync (
   )\r
 {\r
   CPU_MP_DATA  *CpuMpData;\r
+  UINTN        ProcessorNumber;\r
+  EFI_STATUS   Status;\r
 \r
   CpuMpData = (CPU_MP_DATA *) Buffer;\r
+  Status = GetProcessorNumber (CpuMpData, &ProcessorNumber);\r
+  ASSERT_EFI_ERROR (Status);\r
   //\r
   // Load microcode on AP\r
   //\r
-  MicrocodeDetect (CpuMpData, FALSE);\r
+  MicrocodeDetect (CpuMpData, ProcessorNumber);\r
   //\r
   // Sync BSP's MTRR table to AP\r
   //\r
@@ -464,29 +476,44 @@ CollectProcessorCount (
   )\r
 {\r
   UINTN                  Index;\r
+  CPU_INFO_IN_HOB        *CpuInfoInHob;\r
+  BOOLEAN                X2Apic;\r
 \r
   //\r
   // Send 1st broadcast IPI to APs to wakeup APs\r
   //\r
-  CpuMpData->InitFlag     = ApInitConfig;\r
-  CpuMpData->X2ApicEnable = FALSE;\r
+  CpuMpData->InitFlag = ApInitConfig;\r
   WakeUpAP (CpuMpData, TRUE, 0, NULL, NULL, TRUE);\r
   CpuMpData->InitFlag = ApInitDone;\r
-  ASSERT (CpuMpData->CpuCount <= PcdGet32 (PcdCpuMaxLogicalProcessorNumber));\r
   //\r
-  // Wait for all APs finished the initialization\r
+  // When InitFlag == ApInitConfig, WakeUpAP () guarantees all APs are checked in.\r
+  // FinishedCount is the number of check-in APs.\r
   //\r
-  while (CpuMpData->FinishedCount < (CpuMpData->CpuCount - 1)) {\r
-    CpuPause ();\r
-  }\r
+  CpuMpData->CpuCount = CpuMpData->FinishedCount + 1;\r
+  ASSERT (CpuMpData->CpuCount <= PcdGet32 (PcdCpuMaxLogicalProcessorNumber));\r
 \r
+  //\r
+  // Enable x2APIC mode if\r
+  //  1. Number of CPU is greater than 255; or\r
+  //  2. There are any logical processors reporting an Initial APIC ID of 255 or greater.\r
+  //\r
+  X2Apic = FALSE;\r
   if (CpuMpData->CpuCount > 255) {\r
     //\r
     // If there are more than 255 processor found, force to enable X2APIC\r
     //\r
-    CpuMpData->X2ApicEnable = TRUE;\r
+    X2Apic = TRUE;\r
+  } else {\r
+    CpuInfoInHob = (CPU_INFO_IN_HOB *) (UINTN) CpuMpData->CpuInfoInHob;\r
+    for (Index = 0; Index < CpuMpData->CpuCount; Index++) {\r
+      if (CpuInfoInHob[Index].InitialApicId >= 0xFF) {\r
+        X2Apic = TRUE;\r
+        break;\r
+      }\r
+    }\r
   }\r
-  if (CpuMpData->X2ApicEnable) {\r
+\r
+  if (X2Apic) {\r
     DEBUG ((DEBUG_INFO, "Force x2APIC mode!\n"));\r
     //\r
     // Wakeup all APs to enable x2APIC mode\r
@@ -537,7 +564,8 @@ InitializeApData (
   IN     UINT64           ApTopOfStack\r
   )\r
 {\r
-  CPU_INFO_IN_HOB          *CpuInfoInHob;\r
+  CPU_INFO_IN_HOB                  *CpuInfoInHob;\r
+  MSR_IA32_PLATFORM_ID_REGISTER    PlatformIdMsr;\r
 \r
   CpuInfoInHob = (CPU_INFO_IN_HOB *) (UINTN) CpuMpData->CpuInfoInHob;\r
   CpuInfoInHob[ProcessorNumber].InitialApicId = GetInitialApicId ();\r
@@ -547,20 +575,138 @@ InitializeApData (
 \r
   CpuMpData->CpuData[ProcessorNumber].Waiting    = FALSE;\r
   CpuMpData->CpuData[ProcessorNumber].CpuHealthy = (BistData == 0) ? TRUE : FALSE;\r
-  if (CpuInfoInHob[ProcessorNumber].InitialApicId >= 0xFF) {\r
-    //\r
-    // Set x2APIC mode if there are any logical processor reporting\r
-    // an Initial APIC ID of 255 or greater.\r
-    //\r
-    AcquireSpinLock(&CpuMpData->MpLock);\r
-    CpuMpData->X2ApicEnable = TRUE;\r
-    ReleaseSpinLock(&CpuMpData->MpLock);\r
+\r
+  //\r
+  // NOTE: PlatformId is not relevant on AMD platforms.\r
+  //\r
+  if (!StandardSignatureIsAuthenticAMD ()) {\r
+    PlatformIdMsr.Uint64 = AsmReadMsr64 (MSR_IA32_PLATFORM_ID);\r
+    CpuMpData->CpuData[ProcessorNumber].PlatformId = (UINT8)PlatformIdMsr.Bits.PlatformId;\r
   }\r
 \r
+  AsmCpuid (\r
+    CPUID_VERSION_INFO,\r
+    &CpuMpData->CpuData[ProcessorNumber].ProcessorSignature,\r
+    NULL,\r
+    NULL,\r
+    NULL\r
+    );\r
+\r
   InitializeSpinLock(&CpuMpData->CpuData[ProcessorNumber].ApLock);\r
   SetApState (&CpuMpData->CpuData[ProcessorNumber], CpuStateIdle);\r
 }\r
 \r
+/**\r
+  Get Protected mode code segment with 16-bit default addressing\r
+  from current GDT table.\r
+\r
+  @return  Protected mode 16-bit code segment value.\r
+**/\r
+STATIC\r
+UINT16\r
+GetProtectedMode16CS (\r
+  VOID\r
+  )\r
+{\r
+  IA32_DESCRIPTOR          GdtrDesc;\r
+  IA32_SEGMENT_DESCRIPTOR  *GdtEntry;\r
+  UINTN                    GdtEntryCount;\r
+  UINT16                   Index;\r
+\r
+  Index = (UINT16) -1;\r
+  AsmReadGdtr (&GdtrDesc);\r
+  GdtEntryCount = (GdtrDesc.Limit + 1) / sizeof (IA32_SEGMENT_DESCRIPTOR);\r
+  GdtEntry = (IA32_SEGMENT_DESCRIPTOR *) GdtrDesc.Base;\r
+  for (Index = 0; Index < GdtEntryCount; Index++) {\r
+    if (GdtEntry->Bits.L == 0 &&\r
+        GdtEntry->Bits.DB == 0 &&\r
+        GdtEntry->Bits.Type > 8) {\r
+      break;\r
+    }\r
+    GdtEntry++;\r
+  }\r
+  ASSERT (Index != GdtEntryCount);\r
+  return Index * 8;\r
+}\r
+\r
+/**\r
+  Get Protected mode code segment with 32-bit default addressing\r
+  from current GDT table.\r
+\r
+  @return  Protected mode 32-bit code segment value.\r
+**/\r
+STATIC\r
+UINT16\r
+GetProtectedMode32CS (\r
+  VOID\r
+  )\r
+{\r
+  IA32_DESCRIPTOR          GdtrDesc;\r
+  IA32_SEGMENT_DESCRIPTOR  *GdtEntry;\r
+  UINTN                    GdtEntryCount;\r
+  UINT16                   Index;\r
+\r
+  Index = (UINT16) -1;\r
+  AsmReadGdtr (&GdtrDesc);\r
+  GdtEntryCount = (GdtrDesc.Limit + 1) / sizeof (IA32_SEGMENT_DESCRIPTOR);\r
+  GdtEntry = (IA32_SEGMENT_DESCRIPTOR *) GdtrDesc.Base;\r
+  for (Index = 0; Index < GdtEntryCount; Index++) {\r
+    if (GdtEntry->Bits.L == 0 &&\r
+        GdtEntry->Bits.DB == 1 &&\r
+        GdtEntry->Bits.Type > 8) {\r
+      break;\r
+    }\r
+    GdtEntry++;\r
+  }\r
+  ASSERT (Index != GdtEntryCount);\r
+  return Index * 8;\r
+}\r
+\r
+/**\r
+  Reset an AP when in SEV-ES mode.\r
+\r
+  If successful, this function never returns.\r
+\r
+  @param[in] Ghcb                 Pointer to the GHCB\r
+  @param[in] CpuMpData            Pointer to CPU MP Data\r
+\r
+**/\r
+STATIC\r
+VOID\r
+MpInitLibSevEsAPReset (\r
+  IN GHCB                         *Ghcb,\r
+  IN CPU_MP_DATA                  *CpuMpData\r
+  )\r
+{\r
+  EFI_STATUS       Status;\r
+  UINTN            ProcessorNumber;\r
+  UINT16           Code16, Code32;\r
+  AP_RESET         *APResetFn;\r
+  UINTN            BufferStart;\r
+  UINTN            StackStart;\r
+\r
+  Status = GetProcessorNumber (CpuMpData, &ProcessorNumber);\r
+  ASSERT_EFI_ERROR (Status);\r
+\r
+  Code16 = GetProtectedMode16CS ();\r
+  Code32 = GetProtectedMode32CS ();\r
+\r
+  if (CpuMpData->WakeupBufferHigh != 0) {\r
+    APResetFn = (AP_RESET *) (CpuMpData->WakeupBufferHigh + CpuMpData->AddressMap.SwitchToRealNoNxOffset);\r
+  } else {\r
+    APResetFn = (AP_RESET *) (CpuMpData->MpCpuExchangeInfo->BufferStart + CpuMpData->AddressMap.SwitchToRealOffset);\r
+  }\r
+\r
+  BufferStart = CpuMpData->MpCpuExchangeInfo->BufferStart;\r
+  StackStart = CpuMpData->SevEsAPResetStackStart -\r
+                 (AP_RESET_STACK_SIZE * ProcessorNumber);\r
+\r
+  //\r
+  // This call never returns.\r
+  //\r
+  APResetFn (BufferStart, Code16, Code32, StackStart);\r
+}\r
+\r
 /**\r
   This function will be called from AP reset code if BSP uses WakeUpAP.\r
 \r
@@ -603,10 +749,6 @@ ApWakeupFunction (
   CurrentApicMode = GetApicMode ();\r
   while (TRUE) {\r
     if (CpuMpData->InitFlag == ApInitConfig) {\r
-      //\r
-      // Add CPU number\r
-      //\r
-      InterlockedIncrement ((UINT32 *) &CpuMpData->CpuCount);\r
       ProcessorNumber = ApIndex;\r
       //\r
       // This is first time AP wakeup, get BIST information from AP stack\r
@@ -614,10 +756,6 @@ ApWakeupFunction (
       ApTopOfStack  = CpuMpData->Buffer + (ProcessorNumber + 1) * CpuMpData->CpuApStackSize;\r
       BistData = *(UINT32 *) ((UINTN) ApTopOfStack - sizeof (UINTN));\r
       //\r
-      // Do some AP initialize sync\r
-      //\r
-      ApInitializeSync (CpuMpData);\r
-      //\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
@@ -639,18 +777,31 @@ ApWakeupFunction (
         WAKEUP_AP_SIGNAL,\r
         0\r
         );\r
-      if (CpuMpData->ApLoopMode == ApInHltLoop) {\r
-        //\r
-        // Restore AP's volatile registers saved\r
-        //\r
-        RestoreVolatileRegisters (&CpuMpData->CpuData[ProcessorNumber].VolatileRegisters, TRUE);\r
-      } else {\r
+\r
+      if (CpuMpData->InitFlag == ApInitReconfig) {\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
+        // ApInitReconfig happens when:\r
+        // 1. AP is re-enabled after it's disabled, in either PEI or DXE phase.\r
+        // 2. AP is initialized in DXE phase.\r
+        // In either case, use the volatile registers value derived from BSP.\r
+        // NOTE: IDTR.BASE stored in CpuMpData->CpuData[0].VolatileRegisters points to a\r
+        //   different IDT shared by all APs.\r
         //\r
-        CpuFlushTlb ();\r
+        RestoreVolatileRegisters (&CpuMpData->CpuData[0].VolatileRegisters, FALSE);\r
+      }  else {\r
+        if (CpuMpData->ApLoopMode == ApInHltLoop) {\r
+          //\r
+          // Restore AP's volatile registers saved before AP is halted\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
 \r
       if (GetApState (&CpuMpData->CpuData[ProcessorNumber]) == CpuStateReady) {\r
@@ -696,30 +847,87 @@ ApWakeupFunction (
             }\r
           }\r
         }\r
-        SetApState (&CpuMpData->CpuData[ProcessorNumber], CpuStateIdle);\r
+        SetApState (&CpuMpData->CpuData[ProcessorNumber], CpuStateFinished);\r
       }\r
     }\r
 \r
+    if (CpuMpData->ApLoopMode == ApInHltLoop) {\r
+      //\r
+      // Save AP volatile registers\r
+      //\r
+      SaveVolatileRegisters (&CpuMpData->CpuData[ProcessorNumber].VolatileRegisters);\r
+    }\r
+\r
     //\r
     // AP finished executing C code\r
     //\r
     InterlockedIncrement ((UINT32 *) &CpuMpData->FinishedCount);\r
-    InterlockedDecrement ((UINT32 *) &CpuMpData->MpCpuExchangeInfo->NumApsExecuting);\r
+\r
+    if (CpuMpData->InitFlag == ApInitConfig) {\r
+      //\r
+      // Delay decrementing the APs executing count when SEV-ES is enabled\r
+      // to allow the APs to issue an AP_RESET_HOLD before the BSP possibly\r
+      // performs another INIT-SIPI-SIPI sequence.\r
+      //\r
+      if (!CpuMpData->SevEsIsEnabled) {\r
+        InterlockedDecrement ((UINT32 *) &CpuMpData->MpCpuExchangeInfo->NumApsExecuting);\r
+      }\r
+    }\r
 \r
     //\r
     // Place AP is specified loop mode\r
     //\r
     if (CpuMpData->ApLoopMode == ApInHltLoop) {\r
-      //\r
-      // Save AP volatile registers\r
-      //\r
-      SaveVolatileRegisters (&CpuMpData->CpuData[ProcessorNumber].VolatileRegisters);\r
       //\r
       // Place AP in HLT-loop\r
       //\r
       while (TRUE) {\r
         DisableInterrupts ();\r
-        CpuSleep ();\r
+        if (CpuMpData->SevEsIsEnabled) {\r
+          MSR_SEV_ES_GHCB_REGISTER  Msr;\r
+          GHCB                      *Ghcb;\r
+          UINT64                    Status;\r
+          BOOLEAN                   DoDecrement;\r
+          BOOLEAN                   InterruptState;\r
+\r
+          DoDecrement = (BOOLEAN) (CpuMpData->InitFlag == ApInitConfig);\r
+\r
+          while (TRUE) {\r
+            Msr.GhcbPhysicalAddress = AsmReadMsr64 (MSR_SEV_ES_GHCB);\r
+            Ghcb = Msr.Ghcb;\r
+\r
+            VmgInit (Ghcb, &InterruptState);\r
+\r
+            if (DoDecrement) {\r
+              DoDecrement = FALSE;\r
+\r
+              //\r
+              // Perform the delayed decrement just before issuing the first\r
+              // VMGEXIT with AP_RESET_HOLD.\r
+              //\r
+              InterlockedDecrement ((UINT32 *) &CpuMpData->MpCpuExchangeInfo->NumApsExecuting);\r
+            }\r
+\r
+            Status = VmgExit (Ghcb, SVM_EXIT_AP_RESET_HOLD, 0, 0);\r
+            if ((Status == 0) && (Ghcb->SaveArea.SwExitInfo2 != 0)) {\r
+              VmgDone (Ghcb, InterruptState);\r
+              break;\r
+            }\r
+\r
+            VmgDone (Ghcb, InterruptState);\r
+          }\r
+\r
+          //\r
+          // Awakened in a new phase? Use the new CpuMpData\r
+          //\r
+          if (CpuMpData->NewCpuMpData != NULL) {\r
+            CpuMpData = CpuMpData->NewCpuMpData;\r
+          }\r
+\r
+          MpInitLibSevEsAPReset (Ghcb, CpuMpData);\r
+        } else {\r
+          CpuSleep ();\r
+        }\r
         CpuPause ();\r
       }\r
     }\r
@@ -795,9 +1003,9 @@ FillExchangeInfoData (
   volatile MP_CPU_EXCHANGE_INFO    *ExchangeInfo;\r
   UINTN                            Size;\r
   IA32_SEGMENT_DESCRIPTOR          *Selector;\r
+  IA32_CR4                         Cr4;\r
 \r
   ExchangeInfo                  = CpuMpData->MpCpuExchangeInfo;\r
-  ExchangeInfo->Lock            = 0;\r
   ExchangeInfo->StackStart      = CpuMpData->Buffer;\r
   ExchangeInfo->StackSize       = CpuMpData->CpuApStackSize;\r
   ExchangeInfo->BufferStart     = CpuMpData->WakeupBuffer;\r
@@ -819,6 +1027,21 @@ FillExchangeInfoData (
 \r
   ExchangeInfo->InitializeFloatingPointUnitsAddress = (UINTN)InitializeFloatingPointUnits;\r
 \r
+  //\r
+  // We can check either CPUID(7).ECX[bit16] or check CR4.LA57[bit12]\r
+  //  to determin whether 5-Level Paging is enabled.\r
+  // CPUID(7).ECX[bit16] shows CPU's capability, CR4.LA57[bit12] shows\r
+  // current system setting.\r
+  // Using latter way is simpler because it also eliminates the needs to\r
+  //  check whether platform wants to enable it.\r
+  //\r
+  Cr4.UintN = AsmReadCr4 ();\r
+  ExchangeInfo->Enable5LevelPaging = (BOOLEAN) (Cr4.Bits.LA57 == 1);\r
+  DEBUG ((DEBUG_INFO, "%a: 5-Level Paging = %d\n", gEfiCallerBaseName, ExchangeInfo->Enable5LevelPaging));\r
+\r
+  ExchangeInfo->SevEsIsEnabled  = CpuMpData->SevEsIsEnabled;\r
+  ExchangeInfo->GhcbBase        = (UINTN) CpuMpData->GhcbBase;\r
+\r
   //\r
   // Get the BSP's data of GDT and IDT\r
   //\r
@@ -845,8 +1068,9 @@ FillExchangeInfoData (
   // 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
+    Size = CpuMpData->AddressMap.RendezvousFunnelSize +\r
+             CpuMpData->AddressMap.SwitchToRealSize -\r
+             CpuMpData->AddressMap.ModeTransitionOffset;\r
     CopyMem (\r
       (VOID *)CpuMpData->WakeupBufferHigh,\r
       CpuMpData->AddressMap.RendezvousFunnelAddress +\r
@@ -899,7 +1123,8 @@ BackupAndPrepareWakeupBuffer(
   CopyMem (\r
     (VOID *) CpuMpData->WakeupBuffer,\r
     (VOID *) CpuMpData->AddressMap.RendezvousFunnelAddress,\r
-    CpuMpData->AddressMap.RendezvousFunnelSize\r
+    CpuMpData->AddressMap.RendezvousFunnelSize +\r
+      CpuMpData->AddressMap.SwitchToRealSize\r
     );\r
 }\r
 \r
@@ -920,6 +1145,28 @@ RestoreWakeupBuffer(
     );\r
 }\r
 \r
+/**\r
+  Calculate the size of the reset vector.\r
+\r
+  @param[in]  AddressMap  The pointer to Address Map structure.\r
+\r
+  @return                 Total amount of memory required for the AP reset area\r
+**/\r
+STATIC\r
+UINTN\r
+GetApResetVectorSize (\r
+  IN MP_ASSEMBLY_ADDRESS_MAP  *AddressMap\r
+  )\r
+{\r
+  UINTN  Size;\r
+\r
+  Size = AddressMap->RendezvousFunnelSize +\r
+           AddressMap->SwitchToRealSize +\r
+           sizeof (MP_CPU_EXCHANGE_INFO);\r
+\r
+  return Size;\r
+}\r
+\r
 /**\r
   Allocate reset vector buffer.\r
 \r
@@ -931,18 +1178,55 @@ AllocateResetVector (
   )\r
 {\r
   UINTN           ApResetVectorSize;\r
+  UINTN           ApResetStackSize;\r
 \r
   if (CpuMpData->WakeupBuffer == (UINTN) -1) {\r
-    ApResetVectorSize = CpuMpData->AddressMap.RendezvousFunnelSize +\r
-                          sizeof (MP_CPU_EXCHANGE_INFO);\r
+    ApResetVectorSize = GetApResetVectorSize (&CpuMpData->AddressMap);\r
 \r
     CpuMpData->WakeupBuffer      = GetWakeupBuffer (ApResetVectorSize);\r
     CpuMpData->MpCpuExchangeInfo = (MP_CPU_EXCHANGE_INFO *) (UINTN)\r
-                    (CpuMpData->WakeupBuffer + CpuMpData->AddressMap.RendezvousFunnelSize);\r
+                    (CpuMpData->WakeupBuffer +\r
+                       CpuMpData->AddressMap.RendezvousFunnelSize +\r
+                       CpuMpData->AddressMap.SwitchToRealSize);\r
     CpuMpData->WakeupBufferHigh  = GetModeTransitionBuffer (\r
-                                    CpuMpData->AddressMap.RendezvousFunnelSize -\r
+                                    CpuMpData->AddressMap.RendezvousFunnelSize +\r
+                                    CpuMpData->AddressMap.SwitchToRealSize -\r
                                     CpuMpData->AddressMap.ModeTransitionOffset\r
                                     );\r
+    //\r
+    // The AP reset stack is only used by SEV-ES guests. Do not allocate it\r
+    // if SEV-ES is not enabled.\r
+    //\r
+    if (PcdGetBool (PcdSevEsIsEnabled)) {\r
+      //\r
+      // Stack location is based on ProcessorNumber, so use the total number\r
+      // of processors for calculating the total stack area.\r
+      //\r
+      ApResetStackSize = (AP_RESET_STACK_SIZE *\r
+                          PcdGet32 (PcdCpuMaxLogicalProcessorNumber));\r
+\r
+      //\r
+      // Invoke GetWakeupBuffer a second time to allocate the stack area\r
+      // below 1MB. The returned buffer will be page aligned and sized and\r
+      // below the previously allocated buffer.\r
+      //\r
+      CpuMpData->SevEsAPResetStackStart = GetWakeupBuffer (ApResetStackSize);\r
+\r
+      //\r
+      // Check to be sure that the "allocate below" behavior hasn't changed.\r
+      // This will also catch a failed allocation, as "-1" is returned on\r
+      // failure.\r
+      //\r
+      if (CpuMpData->SevEsAPResetStackStart >= CpuMpData->WakeupBuffer) {\r
+        DEBUG ((\r
+          DEBUG_ERROR,\r
+          "SEV-ES AP reset stack is not below wakeup buffer\n"\r
+          ));\r
+\r
+        ASSERT (FALSE);\r
+        CpuDeadLoop ();\r
+      }\r
+    }\r
   }\r
   BackupAndPrepareWakeupBuffer (CpuMpData);\r
 }\r
@@ -957,7 +1241,80 @@ FreeResetVector (
   IN CPU_MP_DATA              *CpuMpData\r
   )\r
 {\r
-  RestoreWakeupBuffer (CpuMpData);\r
+  //\r
+  // If SEV-ES is enabled, the reset area is needed for AP parking and\r
+  // and AP startup in the OS, so the reset area is reserved. Do not\r
+  // perform the restore as this will overwrite memory which has data\r
+  // needed by SEV-ES.\r
+  //\r
+  if (!CpuMpData->SevEsIsEnabled) {\r
+    RestoreWakeupBuffer (CpuMpData);\r
+  }\r
+}\r
+\r
+/**\r
+  Allocate the SEV-ES AP jump table buffer.\r
+\r
+  @param[in, out]  CpuMpData  The pointer to CPU MP Data structure.\r
+**/\r
+VOID\r
+AllocateSevEsAPMemory (\r
+  IN OUT CPU_MP_DATA          *CpuMpData\r
+  )\r
+{\r
+  if (CpuMpData->SevEsAPBuffer == (UINTN) -1) {\r
+    CpuMpData->SevEsAPBuffer =\r
+      CpuMpData->SevEsIsEnabled ? GetSevEsAPMemory () : 0;\r
+  }\r
+}\r
+\r
+/**\r
+  Program the SEV-ES AP jump table buffer.\r
+\r
+  @param[in]  SipiVector  The SIPI vector used for the AP Reset\r
+**/\r
+VOID\r
+SetSevEsJumpTable (\r
+  IN UINTN  SipiVector\r
+  )\r
+{\r
+  SEV_ES_AP_JMP_FAR *JmpFar;\r
+  UINT32            Offset, InsnByte;\r
+  UINT8             LoNib, HiNib;\r
+\r
+  JmpFar = (SEV_ES_AP_JMP_FAR *) (UINTN) FixedPcdGet32 (PcdSevEsWorkAreaBase);\r
+  ASSERT (JmpFar != NULL);\r
+\r
+  //\r
+  // Obtain the address of the Segment/Rip location in the workarea.\r
+  // This will be set to a value derived from the SIPI vector and will\r
+  // be the memory address used for the far jump below.\r
+  //\r
+  Offset = FixedPcdGet32 (PcdSevEsWorkAreaBase);\r
+  Offset += sizeof (JmpFar->InsnBuffer);\r
+  LoNib = (UINT8) Offset;\r
+  HiNib = (UINT8) (Offset >> 8);\r
+\r
+  //\r
+  // Program the workarea (which is the initial AP boot address) with\r
+  // far jump to the SIPI vector (where XX and YY represent the\r
+  // address of where the SIPI vector is stored.\r
+  //\r
+  //   JMP FAR [CS:XXYY] => 2E FF 2E YY XX\r
+  //\r
+  InsnByte = 0;\r
+  JmpFar->InsnBuffer[InsnByte++] = 0x2E;  // CS override prefix\r
+  JmpFar->InsnBuffer[InsnByte++] = 0xFF;  // JMP (FAR)\r
+  JmpFar->InsnBuffer[InsnByte++] = 0x2E;  // ModRM (JMP memory location)\r
+  JmpFar->InsnBuffer[InsnByte++] = LoNib; // YY offset ...\r
+  JmpFar->InsnBuffer[InsnByte++] = HiNib; // XX offset ...\r
+\r
+  //\r
+  // Program the Segment/Rip based on the SIPI vector (always at least\r
+  // 16-byte aligned, so Rip is set to 0).\r
+  //\r
+  JmpFar->Rip = 0;\r
+  JmpFar->Segment = (UINT16) (SipiVector >> 4);\r
 }\r
 \r
 /**\r
@@ -976,8 +1333,8 @@ WakeUpAP (
   IN CPU_MP_DATA               *CpuMpData,\r
   IN BOOLEAN                   Broadcast,\r
   IN UINTN                     ProcessorNumber,\r
-  IN EFI_AP_PROCEDURE          Procedure,              OPTIONAL\r
-  IN VOID                      *ProcedureArgument,     OPTIONAL\r
+  IN EFI_AP_PROCEDURE          Procedure               OPTIONAL,\r
+  IN VOID                      *ProcedureArgument      OPTIONAL,\r
   IN BOOLEAN                   WakeUpDisabledAps\r
   )\r
 {\r
@@ -994,6 +1351,7 @@ WakeUpAP (
       CpuMpData->InitFlag   != ApInitDone) {\r
     ResetVectorRequired = TRUE;\r
     AllocateResetVector (CpuMpData);\r
+    AllocateSevEsAPMemory (CpuMpData);\r
     FillExchangeInfoData (CpuMpData);\r
     SaveLocalApicTimerSetting (CpuMpData);\r
   }\r
@@ -1014,7 +1372,7 @@ WakeUpAP (
         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
+        // the AP procedure will be skipped for disabled AP because AP state\r
         // is not CpuStateReady.\r
         //\r
         if (GetApState (CpuData) == CpuStateDisabled && !WakeUpDisabledAps) {\r
@@ -1030,30 +1388,82 @@ WakeUpAP (
       }\r
     }\r
     if (ResetVectorRequired) {\r
+      //\r
+      // For SEV-ES, the initial AP boot address will be defined by\r
+      // PcdSevEsWorkAreaBase. The Segment/Rip must be the jump address\r
+      // from the original INIT-SIPI-SIPI.\r
+      //\r
+      if (CpuMpData->SevEsIsEnabled) {\r
+        SetSevEsJumpTable (ExchangeInfo->BufferStart);\r
+      }\r
+\r
       //\r
       // Wakeup all APs\r
       //\r
       SendInitSipiSipiAllExcludingSelf ((UINT32) ExchangeInfo->BufferStart);\r
     }\r
     if (CpuMpData->InitFlag == ApInitConfig) {\r
-      //\r
-      // Here support two methods to collect AP count through adjust\r
-      // PcdCpuApInitTimeOutInMicroSeconds values.\r
-      //\r
-      // one way is set a value to just let the first AP to start the\r
-      // initialization, then through the later while loop to wait all Aps\r
-      // finsh the initialization.\r
-      // The other way is set a value to let all APs finished the initialzation.\r
-      // In this case, the later while loop is useless.\r
-      //\r
-      TimedWaitForApFinish (\r
-        CpuMpData,\r
-        PcdGet32 (PcdCpuMaxLogicalProcessorNumber) - 1,\r
-        PcdGet32 (PcdCpuApInitTimeOutInMicroSeconds)\r
-        );\r
+      if (PcdGet32 (PcdCpuBootLogicalProcessorNumber) > 0) {\r
+        //\r
+        // The AP enumeration algorithm below is suitable only when the\r
+        // platform can tell us the *exact* boot CPU count in advance.\r
+        //\r
+        // The wait below finishes only when the detected AP count reaches\r
+        // (PcdCpuBootLogicalProcessorNumber - 1), regardless of how long that\r
+        // takes. If at least one AP fails to check in (meaning a platform\r
+        // hardware bug), the detection hangs forever, by design. If the actual\r
+        // boot CPU count in the system is higher than\r
+        // PcdCpuBootLogicalProcessorNumber (meaning a platform\r
+        // misconfiguration), then some APs may complete initialization after\r
+        // the wait finishes, and cause undefined behavior.\r
+        //\r
+        TimedWaitForApFinish (\r
+          CpuMpData,\r
+          PcdGet32 (PcdCpuBootLogicalProcessorNumber) - 1,\r
+          MAX_UINT32 // approx. 71 minutes\r
+          );\r
+      } else {\r
+        //\r
+        // The AP enumeration algorithm below is suitable for two use cases.\r
+        //\r
+        // (1) The check-in time for an individual AP is bounded, and APs run\r
+        //     through their initialization routines strongly concurrently. In\r
+        //     particular, the number of concurrently running APs\r
+        //     ("NumApsExecuting") is never expected to fall to zero\r
+        //     *temporarily* -- it is expected to fall to zero only when all\r
+        //     APs have checked-in.\r
+        //\r
+        //     In this case, the platform is supposed to set\r
+        //     PcdCpuApInitTimeOutInMicroSeconds to a low-ish value (just long\r
+        //     enough for one AP to start initialization). The timeout will be\r
+        //     reached soon, and remaining APs are collected by watching\r
+        //     NumApsExecuting fall to zero. If NumApsExecuting falls to zero\r
+        //     mid-process, while some APs have not completed initialization,\r
+        //     the behavior is undefined.\r
+        //\r
+        // (2) The check-in time for an individual AP is unbounded, and/or APs\r
+        //     may complete their initializations widely spread out. In\r
+        //     particular, some APs may finish initialization before some APs\r
+        //     even start.\r
+        //\r
+        //     In this case, the platform is supposed to set\r
+        //     PcdCpuApInitTimeOutInMicroSeconds to a high-ish value. The AP\r
+        //     enumeration will always take that long (except when the boot CPU\r
+        //     count happens to be maximal, that is,\r
+        //     PcdCpuMaxLogicalProcessorNumber). All APs are expected to\r
+        //     check-in before the timeout, and NumApsExecuting is assumed zero\r
+        //     at timeout. APs that miss the time-out may cause undefined\r
+        //     behavior.\r
+        //\r
+        TimedWaitForApFinish (\r
+          CpuMpData,\r
+          PcdGet32 (PcdCpuMaxLogicalProcessorNumber) - 1,\r
+          PcdGet32 (PcdCpuApInitTimeOutInMicroSeconds)\r
+          );\r
 \r
-      while (CpuMpData->MpCpuExchangeInfo->NumApsExecuting != 0) {\r
-        CpuPause();\r
+        while (CpuMpData->MpCpuExchangeInfo->NumApsExecuting != 0) {\r
+          CpuPause();\r
+        }\r
       }\r
     } else {\r
       //\r
@@ -1078,6 +1488,16 @@ WakeUpAP (
     *(UINT32 *) CpuData->StartupApSignal = WAKEUP_AP_SIGNAL;\r
     if (ResetVectorRequired) {\r
       CpuInfoInHob = (CPU_INFO_IN_HOB *) (UINTN) CpuMpData->CpuInfoInHob;\r
+\r
+      //\r
+      // For SEV-ES, the initial AP boot address will be defined by\r
+      // PcdSevEsWorkAreaBase. The Segment/Rip must be the jump address\r
+      // from the original INIT-SIPI-SIPI.\r
+      //\r
+      if (CpuMpData->SevEsIsEnabled) {\r
+        SetSevEsJumpTable (ExchangeInfo->BufferStart);\r
+      }\r
+\r
       SendInitSipiSipi (\r
         CpuInfoInHob[ProcessorNumber].ApicId,\r
         (UINT32) ExchangeInfo->BufferStart\r
@@ -1370,10 +1790,11 @@ CheckThisAP (
   //\r
   // If the AP finishes for StartupThisAP(), return EFI_SUCCESS.\r
   //\r
-  if (GetApState(CpuData) == CpuStateIdle) {\r
+  if (GetApState(CpuData) == CpuStateFinished) {\r
     if (CpuData->Finished != NULL) {\r
       *(CpuData->Finished) = TRUE;\r
     }\r
+    SetApState (CpuData, CpuStateIdle);\r
     return EFI_SUCCESS;\r
   } else {\r
     //\r
@@ -1434,9 +1855,10 @@ CheckAllAPs (
     // 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 CpuStateIdle, so BSP can safely make use of its value.\r
     //\r
-    if (GetApState(CpuData) == CpuStateIdle) {\r
+    if (GetApState(CpuData) == CpuStateFinished) {\r
       CpuMpData->RunningCount --;\r
       CpuMpData->CpuData[ProcessorNumber].Waiting = FALSE;\r
+      SetApState(CpuData, CpuStateIdle);\r
 \r
       //\r
       // If in Single Thread mode, then search for the next waiting AP for execution.\r
@@ -1542,7 +1964,6 @@ MpInitLibInitialize (
   UINTN                    ApResetVectorSize;\r
   UINTN                    BackupBufferAddr;\r
   UINTN                    ApIdtBase;\r
-  VOID                     *MicrocodePatchInRam;\r
 \r
   OldCpuMpData = GetCpuMpDataFromGuidedHob ();\r
   if (OldCpuMpData == NULL) {\r
@@ -1553,12 +1974,12 @@ MpInitLibInitialize (
   ASSERT (MaxLogicalProcessorNumber != 0);\r
 \r
   AsmGetAddressMap (&AddressMap);\r
-  ApResetVectorSize = AddressMap.RendezvousFunnelSize + sizeof (MP_CPU_EXCHANGE_INFO);\r
+  ApResetVectorSize = GetApResetVectorSize (&AddressMap);\r
   ApStackSize = PcdGet32(PcdCpuApStackSize);\r
   ApLoopMode  = GetApLoopMode (&MonitorFilterSize);\r
 \r
   //\r
-  // Save BSP's Control registers for APs\r
+  // Save BSP's Control registers for APs.\r
   //\r
   SaveVolatileRegisters (&VolatileRegisters);\r
 \r
@@ -1610,39 +2031,10 @@ MpInitLibInitialize (
   CpuMpData->SwitchBspFlag    = FALSE;\r
   CpuMpData->CpuData          = (CPU_AP_DATA *) (CpuMpData + 1);\r
   CpuMpData->CpuInfoInHob     = (UINT64) (UINTN) (CpuMpData->CpuData + MaxLogicalProcessorNumber);\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
+  CpuMpData->SevEsIsEnabled = PcdGetBool (PcdSevEsIsEnabled);\r
+  CpuMpData->SevEsAPBuffer  = (UINTN) -1;\r
+  CpuMpData->GhcbBase       = PcdGet64 (PcdGhcbBase);\r
 \r
   //\r
   // Make sure no memory usage outside of the allocated buffer.\r
@@ -1656,6 +2048,10 @@ MpInitLibInitialize (
   //\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
+  VolatileRegisters.Tr = 0;\r
   CopyMem (&CpuMpData->CpuData[0].VolatileRegisters, &VolatileRegisters, sizeof (VolatileRegisters));\r
   //\r
   // Set BSP basic information\r
@@ -1681,14 +2077,6 @@ MpInitLibInitialize (
       (UINT32 *)(MonitorBuffer + MonitorFilterSize * Index);\r
   }\r
   //\r
-  // Load Microcode on BSP\r
-  //\r
-  MicrocodeDetect (CpuMpData, TRUE);\r
-  //\r
-  // Store BSP's MTRR setting\r
-  //\r
-  MtrrGetAllMtrrs (&CpuMpData->MtrrTable);\r
-  //\r
   // Enable the local APIC for Virtual Wire Mode.\r
   //\r
   ProgramVirtualWireMode ();\r
@@ -1705,38 +2093,90 @@ MpInitLibInitialize (
     // APs have been wakeup before, just get the CPU Information\r
     // from HOB\r
     //\r
+    OldCpuMpData->NewCpuMpData = CpuMpData;\r
     CpuMpData->CpuCount  = OldCpuMpData->CpuCount;\r
     CpuMpData->BspNumber = OldCpuMpData->BspNumber;\r
-    CpuMpData->InitFlag  = ApInitReconfig;\r
     CpuMpData->CpuInfoInHob = OldCpuMpData->CpuInfoInHob;\r
     CpuInfoInHob = (CPU_INFO_IN_HOB *) (UINTN) CpuMpData->CpuInfoInHob;\r
     for (Index = 0; Index < CpuMpData->CpuCount; Index++) {\r
       InitializeSpinLock(&CpuMpData->CpuData[Index].ApLock);\r
-      if (CpuInfoInHob[Index].InitialApicId >= 255 || Index > 254) {\r
-        CpuMpData->X2ApicEnable = TRUE;\r
-      }\r
       CpuMpData->CpuData[Index].CpuHealthy = (CpuInfoInHob[Index].Health == 0)? TRUE:FALSE;\r
       CpuMpData->CpuData[Index].ApFunction = 0;\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, TRUE);\r
+  }\r
+\r
+  if (!GetMicrocodePatchInfoFromHob (\r
+         &CpuMpData->MicrocodePatchAddress,\r
+         &CpuMpData->MicrocodePatchRegionSize\r
+         )) {\r
+    //\r
+    // The microcode patch information cache HOB does not exist, which means\r
+    // the microcode patches data has not been loaded into memory yet\r
+    //\r
+    ShadowMicrocodeUpdatePatch (CpuMpData);\r
+  }\r
+\r
+  //\r
+  // Detect and apply Microcode on BSP\r
+  //\r
+  MicrocodeDetect (CpuMpData, CpuMpData->BspNumber);\r
+  //\r
+  // Store BSP's MTRR setting\r
+  //\r
+  MtrrGetAllMtrrs (&CpuMpData->MtrrTable);\r
+\r
+  //\r
+  // Wakeup APs to do some AP initialize sync (Microcode & MTRR)\r
+  //\r
+  if (CpuMpData->CpuCount > 1) {\r
+    if (OldCpuMpData != NULL) {\r
       //\r
-      // Wait for all APs finished initialization\r
+      // Only needs to use this flag for DXE phase to update the wake up\r
+      // buffer. Wakeup buffer allocated in PEI phase is no longer valid\r
+      // in DXE.\r
       //\r
-      while (CpuMpData->FinishedCount < (CpuMpData->CpuCount - 1)) {\r
-        CpuPause ();\r
-      }\r
+      CpuMpData->InitFlag = ApInitReconfig;\r
+    }\r
+    WakeUpAP (CpuMpData, TRUE, 0, ApInitializeSync, CpuMpData, TRUE);\r
+    //\r
+    // Wait for all APs finished initialization\r
+    //\r
+    while (CpuMpData->FinishedCount < (CpuMpData->CpuCount - 1)) {\r
+      CpuPause ();\r
+    }\r
+    if (OldCpuMpData != NULL) {\r
       CpuMpData->InitFlag = ApInitDone;\r
-      for (Index = 0; Index < CpuMpData->CpuCount; Index++) {\r
-        SetApState (&CpuMpData->CpuData[Index], CpuStateIdle);\r
-      }\r
+    }\r
+    for (Index = 0; Index < CpuMpData->CpuCount; Index++) {\r
+      SetApState (&CpuMpData->CpuData[Index], CpuStateIdle);\r
     }\r
   }\r
 \r
+  //\r
+  // Dump the microcode revision for each core.\r
+  //\r
+  DEBUG_CODE_BEGIN ();\r
+    UINT32 ThreadId;\r
+    UINT32 ExpectedMicrocodeRevision;\r
+    CpuInfoInHob = (CPU_INFO_IN_HOB *) (UINTN) CpuMpData->CpuInfoInHob;\r
+    for (Index = 0; Index < CpuMpData->CpuCount; Index++) {\r
+      GetProcessorLocationByApicId (CpuInfoInHob[Index].InitialApicId, NULL, NULL, &ThreadId);\r
+      if (ThreadId == 0) {\r
+        //\r
+        // MicrocodeDetect() loads microcode in first thread of each core, so,\r
+        // CpuMpData->CpuData[Index].MicrocodeEntryAddr is initialized only for first thread of each core.\r
+        //\r
+        ExpectedMicrocodeRevision = 0;\r
+        if (CpuMpData->CpuData[Index].MicrocodeEntryAddr != 0) {\r
+          ExpectedMicrocodeRevision = ((CPU_MICROCODE_HEADER *)(UINTN)CpuMpData->CpuData[Index].MicrocodeEntryAddr)->UpdateRevision;\r
+        }\r
+        DEBUG ((\r
+          DEBUG_INFO, "CPU[%04d]: Microcode revision = %08x, expected = %08x\n",\r
+          Index, CpuMpData->CpuData[Index].MicrocodeRevision, ExpectedMicrocodeRevision\r
+          ));\r
+      }\r
+    }\r
+  DEBUG_CODE_END ();\r
   //\r
   // Initialize global data for MP support\r
   //\r
@@ -1773,10 +2213,17 @@ MpInitLibGetProcessorInfo (
   CPU_MP_DATA            *CpuMpData;\r
   UINTN                  CallerNumber;\r
   CPU_INFO_IN_HOB        *CpuInfoInHob;\r
+  UINTN                  OriginalProcessorNumber;\r
 \r
   CpuMpData = GetCpuMpData ();\r
   CpuInfoInHob = (CPU_INFO_IN_HOB *) (UINTN) CpuMpData->CpuInfoInHob;\r
 \r
+  //\r
+  // Lower 24 bits contains the actual processor number.\r
+  //\r
+  OriginalProcessorNumber = ProcessorNumber;\r
+  ProcessorNumber &= BIT24 - 1;\r
+\r
   //\r
   // Check whether caller processor is BSP\r
   //\r
@@ -1817,6 +2264,18 @@ MpInitLibGetProcessorInfo (
     &ProcessorInfoBuffer->Location.Thread\r
     );\r
 \r
+  if ((OriginalProcessorNumber & CPU_V2_EXTENDED_TOPOLOGY) != 0) {\r
+    GetProcessorLocation2ByApicId (\r
+      CpuInfoInHob[ProcessorNumber].ApicId,\r
+      &ProcessorInfoBuffer->ExtendedInformation.Location2.Package,\r
+      &ProcessorInfoBuffer->ExtendedInformation.Location2.Die,\r
+      &ProcessorInfoBuffer->ExtendedInformation.Location2.Tile,\r
+      &ProcessorInfoBuffer->ExtendedInformation.Location2.Module,\r
+      &ProcessorInfoBuffer->ExtendedInformation.Location2.Core,\r
+      &ProcessorInfoBuffer->ExtendedInformation.Location2.Thread\r
+      );\r
+  }\r
+\r
   if (HealthData != NULL) {\r
     HealthData->Uint32 = CpuInfoInHob[ProcessorNumber].Health;\r
   }\r
@@ -1933,7 +2392,7 @@ SwitchBSPWorker (
   //\r
   // Wait for old BSP finished AP task\r
   //\r
-  while (GetApState (&CpuMpData->CpuData[CallerNumber]) != CpuStateIdle) {\r
+  while (GetApState (&CpuMpData->CpuData[CallerNumber]) != CpuStateFinished) {\r
     CpuPause ();\r
   }\r
 \r
@@ -2075,7 +2534,7 @@ MpInitLibWhoAmI (
 EFI_STATUS\r
 EFIAPI\r
 MpInitLibGetNumberOfProcessors (\r
-  OUT UINTN                     *NumberOfProcessors,       OPTIONAL\r
+  OUT UINTN                     *NumberOfProcessors        OPTIONAL,\r
   OUT UINTN                     *NumberOfEnabledProcessors OPTIONAL\r
   )\r
 {\r
@@ -2129,6 +2588,7 @@ MpInitLibGetNumberOfProcessors (
                                       number.  If FALSE, then all the enabled APs\r
                                       execute the function specified by Procedure\r
                                       simultaneously.\r
+  @param[in]  ExcludeBsp              Whether let BSP also trig this task.\r
   @param[in]  WaitEvent               The event created by the caller with CreateEvent()\r
                                       service.\r
   @param[in]  TimeoutInMicroseconds   Indicates the time limit in microseconds for\r
@@ -2150,9 +2610,10 @@ MpInitLibGetNumberOfProcessors (
 \r
 **/\r
 EFI_STATUS\r
-StartupAllAPsWorker (\r
+StartupAllCPUsWorker (\r
   IN  EFI_AP_PROCEDURE          Procedure,\r
   IN  BOOLEAN                   SingleThread,\r
+  IN  BOOLEAN                   ExcludeBsp,\r
   IN  EFI_EVENT                 WaitEvent               OPTIONAL,\r
   IN  UINTN                     TimeoutInMicroseconds,\r
   IN  VOID                      *ProcedureArgument      OPTIONAL,\r
@@ -2174,7 +2635,7 @@ StartupAllAPsWorker (
     *FailedCpuList = NULL;\r
   }\r
 \r
-  if (CpuMpData->CpuCount == 1) {\r
+  if (CpuMpData->CpuCount == 1 && ExcludeBsp) {\r
     return EFI_NOT_STARTED;\r
   }\r
 \r
@@ -2217,9 +2678,9 @@ StartupAllAPsWorker (
     }\r
   }\r
 \r
-  if (!HasEnabledAp) {\r
+  if (!HasEnabledAp && ExcludeBsp) {\r
     //\r
-    // If no enabled AP exists, return EFI_NOT_STARTED.\r
+    // If no enabled AP exists and not include Bsp to do the procedure, return EFI_NOT_STARTED.\r
     //\r
     return EFI_NOT_STARTED;\r
   }\r
@@ -2265,6 +2726,13 @@ StartupAllAPsWorker (
     }\r
   }\r
 \r
+  if (!ExcludeBsp) {\r
+    //\r
+    // Start BSP.\r
+    //\r
+    Procedure (ProcedureArgument);\r
+  }\r
+\r
   Status = EFI_SUCCESS;\r
   if (WaitEvent == NULL) {\r
     do {\r
@@ -2410,3 +2878,47 @@ GetCpuMpDataFromGuidedHob (
   return CpuMpData;\r
 }\r
 \r
+/**\r
+  This service executes a caller provided function on all enabled CPUs.\r
+\r
+  @param[in]  Procedure               A pointer to the function to be run on\r
+                                      enabled APs of the system. See type\r
+                                      EFI_AP_PROCEDURE.\r
+  @param[in]  TimeoutInMicroseconds   Indicates the time limit in microseconds for\r
+                                      APs to return from Procedure, either for\r
+                                      blocking or non-blocking mode. Zero means\r
+                                      infinity. TimeoutInMicroseconds is ignored\r
+                                      for BSP.\r
+  @param[in]  ProcedureArgument       The parameter passed into Procedure for\r
+                                      all APs.\r
+\r
+  @retval EFI_SUCCESS             In blocking mode, all CPUs have finished before\r
+                                  the timeout expired.\r
+  @retval EFI_SUCCESS             In non-blocking mode, function has been dispatched\r
+                                  to all enabled CPUs.\r
+  @retval EFI_DEVICE_ERROR        Caller processor is AP.\r
+  @retval EFI_NOT_READY           Any enabled APs are busy.\r
+  @retval EFI_NOT_READY           MP Initialize Library is not initialized.\r
+  @retval EFI_TIMEOUT             In blocking mode, the timeout expired before\r
+                                  all enabled APs have finished.\r
+  @retval EFI_INVALID_PARAMETER   Procedure is NULL.\r
+\r
+**/\r
+EFI_STATUS\r
+EFIAPI\r
+MpInitLibStartupAllCPUs (\r
+  IN  EFI_AP_PROCEDURE          Procedure,\r
+  IN  UINTN                     TimeoutInMicroseconds,\r
+  IN  VOID                      *ProcedureArgument      OPTIONAL\r
+  )\r
+{\r
+  return StartupAllCPUsWorker (\r
+           Procedure,\r
+           FALSE,\r
+           FALSE,\r
+           NULL,\r
+           TimeoutInMicroseconds,\r
+           ProcedureArgument,\r
+           NULL\r
+           );\r
+}\r