]> git.proxmox.com Git - mirror_edk2.git/blobdiff - UefiCpuPkg/CpuMpPei/CpuMpPei.c
UefiCpuPkg: Replace BSD License with BSD+Patent License
[mirror_edk2.git] / UefiCpuPkg / CpuMpPei / CpuMpPei.c
index d5aee86d11e2d1082f6986eaf00bc6021b87afc5..926541c93712ade823158540b99f3035b6a369b5 100644 (file)
 /** @file\r
   CPU PEI Module installs CPU Multiple Processor PPI.\r
 \r
-  Copyright (c) 2015, 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
+  Copyright (c) 2015 - 2018, Intel Corporation. All rights reserved.<BR>\r
+  SPDX-License-Identifier: BSD-2-Clause-Patent\r
 \r
 **/\r
 \r
 #include "CpuMpPei.h"\r
 \r
 //\r
-// Global Descriptor Table (GDT)\r
+// CPU MP PPI to be installed\r
 //\r
-GLOBAL_REMOVE_IF_UNREFERENCED IA32_GDT mGdtEntries[] = {\r
-/* selector { Global Segment Descriptor                              } */\r
-/* 0x00 */  {{0,      0,  0,  0,    0,  0,  0,  0,    0,  0, 0,  0,  0}}, //null descriptor\r
-/* 0x08 */  {{0xffff, 0,  0,  0x2,  1,  0,  1,  0xf,  0,  0, 1,  1,  0}}, //linear data segment descriptor\r
-/* 0x10 */  {{0xffff, 0,  0,  0xf,  1,  0,  1,  0xf,  0,  0, 1,  1,  0}}, //linear code segment descriptor\r
-/* 0x18 */  {{0xffff, 0,  0,  0x3,  1,  0,  1,  0xf,  0,  0, 1,  1,  0}}, //system data segment descriptor\r
-/* 0x20 */  {{0xffff, 0,  0,  0xa,  1,  0,  1,  0xf,  0,  0, 1,  1,  0}}, //system code segment descriptor\r
-/* 0x28 */  {{0,      0,  0,  0,    0,  0,  0,  0,    0,  0, 0,  0,  0}}, //spare segment descriptor\r
-/* 0x30 */  {{0xffff, 0,  0,  0x2,  1,  0,  1,  0xf,  0,  0, 1,  1,  0}}, //system data segment descriptor\r
-/* 0x38 */  {{0xffff, 0,  0,  0xa,  1,  0,  1,  0xf,  0,  1, 0,  1,  0}}, //system code segment descriptor\r
-/* 0x40 */  {{0,      0,  0,  0,    0,  0,  0,  0,    0,  0, 0,  0,  0}}, //spare segment descriptor\r
+EFI_PEI_MP_SERVICES_PPI                mMpServicesPpi = {\r
+  PeiGetNumberOfProcessors,\r
+  PeiGetProcessorInfo,\r
+  PeiStartupAllAPs,\r
+  PeiStartupThisAP,\r
+  PeiSwitchBSP,\r
+  PeiEnableDisableAP,\r
+  PeiWhoAmI,\r
 };\r
 \r
-//\r
-// IA32 Gdt register\r
-//\r
-GLOBAL_REMOVE_IF_UNREFERENCED IA32_DESCRIPTOR mGdt = {\r
-  sizeof (mGdtEntries) - 1,\r
-  (UINTN) mGdtEntries\r
-  };\r
-\r
-GLOBAL_REMOVE_IF_UNREFERENCED EFI_PEI_NOTIFY_DESCRIPTOR mNotifyList = {\r
-  (EFI_PEI_PPI_DESCRIPTOR_NOTIFY_CALLBACK | EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST),\r
-  &gEfiEndOfPeiSignalPpiGuid,\r
-  CpuMpEndOfPeiCallback\r
+EFI_PEI_PPI_DESCRIPTOR           mPeiCpuMpPpiDesc = {\r
+  (EFI_PEI_PPI_DESCRIPTOR_PPI | EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST),\r
+  &gEfiPeiMpServicesPpiGuid,\r
+  &mMpServicesPpi\r
 };\r
 \r
 /**\r
-  Sort the APIC ID of all processors.\r
-\r
-  This function sorts the APIC ID of all processors so that processor number is\r
-  assigned in the ascending order of APIC ID which eases MP debugging.\r
-\r
-  @param PeiCpuMpData        Pointer to PEI CPU MP Data\r
+  This service retrieves the number of logical processor in the platform\r
+  and the number of those logical processors that are enabled on this boot.\r
+  This service may only be called from the BSP.\r
+\r
+  This function is used to retrieve the following information:\r
+    - The number of logical processors that are present in the system.\r
+    - The number of enabled logical processors in the system at the instant\r
+      this call is made.\r
+\r
+  Because MP Service Ppi provides services to enable and disable processors\r
+  dynamically, the number of enabled logical processors may vary during the\r
+  course of a boot session.\r
+\r
+  If this service is called from an AP, then EFI_DEVICE_ERROR is returned.\r
+  If NumberOfProcessors or NumberOfEnabledProcessors is NULL, then\r
+  EFI_INVALID_PARAMETER is returned. Otherwise, the total number of processors\r
+  is returned in NumberOfProcessors, the number of currently enabled processor\r
+  is returned in NumberOfEnabledProcessors, and EFI_SUCCESS is returned.\r
+\r
+  @param[in]  PeiServices         An indirect pointer to the PEI Services Table\r
+                                  published by the PEI Foundation.\r
+  @param[in]  This                Pointer to this instance of the PPI.\r
+  @param[out] NumberOfProcessors  Pointer to the total number of logical processors in\r
+                                  the system, including the BSP and disabled APs.\r
+  @param[out] NumberOfEnabledProcessors\r
+                                  Number of processors in the system that are enabled.\r
+\r
+  @retval EFI_SUCCESS             The number of logical processors and enabled\r
+                                  logical processors was retrieved.\r
+  @retval EFI_DEVICE_ERROR        The calling processor is an AP.\r
+  @retval EFI_INVALID_PARAMETER   NumberOfProcessors is NULL.\r
+                                  NumberOfEnabledProcessors is NULL.\r
 **/\r
-VOID\r
-SortApicId (\r
-  IN PEI_CPU_MP_DATA   *PeiCpuMpData\r
+EFI_STATUS\r
+EFIAPI\r
+PeiGetNumberOfProcessors (\r
+  IN  CONST EFI_PEI_SERVICES    **PeiServices,\r
+  IN  EFI_PEI_MP_SERVICES_PPI   *This,\r
+  OUT UINTN                     *NumberOfProcessors,\r
+  OUT UINTN                     *NumberOfEnabledProcessors\r
   )\r
 {\r
-  UINTN             Index1;\r
-  UINTN             Index2;\r
-  UINTN             Index3;\r
-  UINT32            ApicId;\r
-  EFI_HEALTH_FLAGS  Health;\r
-  UINT32            ApCount;\r
-\r
-  ApCount = PeiCpuMpData->CpuCount - 1;\r
-\r
-  if (ApCount != 0) {\r
-    for (Index1 = 0; Index1 < ApCount; Index1++) {\r
-      Index3 = Index1;\r
-      //\r
-      // Sort key is the hardware default APIC ID\r
-      //\r
-      ApicId = PeiCpuMpData->CpuData[Index1].ApicId;\r
-      for (Index2 = Index1 + 1; Index2 <= ApCount; Index2++) {\r
-        if (ApicId > PeiCpuMpData->CpuData[Index2].ApicId) {\r
-          Index3 = Index2;\r
-          ApicId = PeiCpuMpData->CpuData[Index2].ApicId;\r
-        }\r
-      }\r
-      if (Index3 != Index1) {\r
-        PeiCpuMpData->CpuData[Index3].ApicId = PeiCpuMpData->CpuData[Index1].ApicId;\r
-        PeiCpuMpData->CpuData[Index1].ApicId = ApicId;\r
-        Health = PeiCpuMpData->CpuData[Index3].Health;\r
-        PeiCpuMpData->CpuData[Index3].Health = PeiCpuMpData->CpuData[Index1].Health;\r
-        PeiCpuMpData->CpuData[Index1].Health = Health;\r
-      }\r
-    }\r
-\r
-    //\r
-    // Get the processor number for the BSP\r
-    //\r
-    ApicId = GetInitialApicId ();\r
-    for (Index1 = 0; Index1 < PeiCpuMpData->CpuCount; Index1++) {\r
-      if (PeiCpuMpData->CpuData[Index1].ApicId == ApicId) {\r
-        PeiCpuMpData->BspNumber = (UINT32) Index1;\r
-        break;\r
-      }\r
-    }\r
+  if ((NumberOfProcessors == NULL) || (NumberOfEnabledProcessors == NULL)) {\r
+    return EFI_INVALID_PARAMETER;\r
   }\r
+\r
+  return MpInitLibGetNumberOfProcessors (\r
+           NumberOfProcessors,\r
+           NumberOfEnabledProcessors\r
+           );\r
 }\r
 \r
 /**\r
-  Get CPU MP Data pointer from the Guided HOB.\r
-\r
-  @return  Pointer to Pointer to PEI CPU MP Data\r
+  Gets detailed MP-related information on the requested processor at the\r
+  instant this call is made. This service may only be called from the BSP.\r
+\r
+  This service retrieves detailed MP-related information about any processor\r
+  on the platform. Note the following:\r
+    - The processor information may change during the course of a boot session.\r
+    - The information presented here is entirely MP related.\r
+\r
+  Information regarding the number of caches and their sizes, frequency of operation,\r
+  slot numbers is all considered platform-related information and is not provided\r
+  by this service.\r
+\r
+  @param[in]  PeiServices         An indirect pointer to the PEI Services Table\r
+                                  published by the PEI Foundation.\r
+  @param[in]  This                Pointer to this instance of the PPI.\r
+  @param[in]  ProcessorNumber     Pointer to the total number of logical processors in\r
+                                  the system, including the BSP and disabled APs.\r
+  @param[out] ProcessorInfoBuffer Number of processors in the system that are enabled.\r
+\r
+  @retval EFI_SUCCESS             Processor information was returned.\r
+  @retval EFI_DEVICE_ERROR        The calling processor is an AP.\r
+  @retval EFI_INVALID_PARAMETER   ProcessorInfoBuffer is NULL.\r
+  @retval EFI_NOT_FOUND           The processor with the handle specified by\r
+                                  ProcessorNumber does not exist in the platform.\r
 **/\r
-PEI_CPU_MP_DATA *\r
-GetMpHobData (\r
-  VOID\r
+EFI_STATUS\r
+EFIAPI\r
+PeiGetProcessorInfo (\r
+  IN  CONST EFI_PEI_SERVICES     **PeiServices,\r
+  IN  EFI_PEI_MP_SERVICES_PPI    *This,\r
+  IN  UINTN                      ProcessorNumber,\r
+  OUT EFI_PROCESSOR_INFORMATION  *ProcessorInfoBuffer\r
   )\r
 {\r
-  EFI_HOB_GUID_TYPE       *GuidHob;\r
-  VOID                    *DataInHob;\r
-  PEI_CPU_MP_DATA         *CpuMpData;\r
-\r
-  CpuMpData = NULL;\r
-  GuidHob = GetFirstGuidHob (&gEfiCallerIdGuid);\r
-  if (GuidHob != NULL) {\r
-    DataInHob = GET_GUID_HOB_DATA (GuidHob);\r
-    CpuMpData = (PEI_CPU_MP_DATA *)(*(UINTN *)DataInHob);\r
-  }\r
-  ASSERT (CpuMpData != NULL);\r
-  return CpuMpData;\r
+  return MpInitLibGetProcessorInfo (ProcessorNumber, ProcessorInfoBuffer, NULL);\r
 }\r
 \r
 /**\r
-  This function will be called from AP reset code if BSP uses WakeUpAP.\r
-\r
-  @param ExchangeInfo     Pointer to the MP exchange info buffer\r
-  @param NumApsExecuting  Number of curret executing AP\r
+  This service executes a caller provided function on all enabled APs. APs can\r
+  run either simultaneously or one at a time in sequence. This service supports\r
+  both blocking requests only. This service may only\r
+  be called from the BSP.\r
+\r
+  This function is used to dispatch all the enabled APs to the function specified\r
+  by Procedure.  If any enabled AP is busy, then EFI_NOT_READY is returned\r
+  immediately and Procedure is not started on any AP.\r
+\r
+  If SingleThread is TRUE, all the enabled APs execute the function specified by\r
+  Procedure one by one, in ascending order of processor handle number. Otherwise,\r
+  all the enabled APs execute the function specified by Procedure simultaneously.\r
+\r
+  If the timeout specified by TimeoutInMicroSeconds expires before all APs return\r
+  from Procedure, then Procedure on the failed APs is terminated. All enabled APs\r
+  are always available for further calls to EFI_PEI_MP_SERVICES_PPI.StartupAllAPs()\r
+  and EFI_PEI_MP_SERVICES_PPI.StartupThisAP(). If FailedCpuList is not NULL, its\r
+  content points to the list of processor handle numbers in which Procedure was\r
+  terminated.\r
+\r
+  Note: It is the responsibility of the consumer of the EFI_PEI_MP_SERVICES_PPI.StartupAllAPs()\r
+  to make sure that the nature of the code that is executed on the BSP and the\r
+  dispatched APs is well controlled. The MP Services Ppi does not guarantee\r
+  that the Procedure function is MP-safe. Hence, the tasks that can be run in\r
+  parallel are limited to certain independent tasks and well-controlled exclusive\r
+  code. PEI services and Ppis may not be called by APs unless otherwise\r
+  specified.\r
+\r
+  In blocking execution mode, BSP waits until all APs finish or\r
+  TimeoutInMicroSeconds expires.\r
+\r
+  @param[in] PeiServices          An indirect pointer to the PEI Services Table\r
+                                  published by the PEI Foundation.\r
+  @param[in] This                 A pointer to the EFI_PEI_MP_SERVICES_PPI instance.\r
+  @param[in] Procedure            A pointer to the function to be run on enabled APs of\r
+                                  the system.\r
+  @param[in] SingleThread         If TRUE, then all the enabled APs execute the function\r
+                                  specified by Procedure one by one, in ascending order\r
+                                  of processor handle number. If FALSE, then all the\r
+                                  enabled APs execute the function specified by Procedure\r
+                                  simultaneously.\r
+  @param[in] TimeoutInMicroSeconds\r
+                                  Indicates the time limit in microseconds for APs to\r
+                                  return from Procedure, for blocking mode only. Zero\r
+                                  means infinity. If the timeout expires before all APs\r
+                                  return from Procedure, then Procedure on the failed APs\r
+                                  is terminated. All enabled APs are available for next\r
+                                  function assigned by EFI_PEI_MP_SERVICES_PPI.StartupAllAPs()\r
+                                  or EFI_PEI_MP_SERVICES_PPI.StartupThisAP(). If the\r
+                                  timeout expires in blocking mode, BSP returns\r
+                                  EFI_TIMEOUT.\r
+  @param[in] ProcedureArgument    The parameter passed into Procedure for all APs.\r
+\r
+  @retval EFI_SUCCESS             In blocking mode, all APs have finished before the\r
+                                  timeout expired.\r
+  @retval EFI_DEVICE_ERROR        Caller processor is AP.\r
+  @retval EFI_NOT_STARTED         No enabled APs exist in the system.\r
+  @retval EFI_NOT_READY           Any enabled APs are busy.\r
+  @retval EFI_TIMEOUT             In blocking mode, the timeout expired before all\r
+                                  enabled APs have finished.\r
+  @retval EFI_INVALID_PARAMETER   Procedure is NULL.\r
 **/\r
-VOID\r
+EFI_STATUS\r
 EFIAPI\r
-ApCFunction (\r
-  IN MP_CPU_EXCHANGE_INFO      *ExchangeInfo,\r
-  IN UINTN                     NumApsExecuting\r
+PeiStartupAllAPs (\r
+  IN  CONST EFI_PEI_SERVICES    **PeiServices,\r
+  IN  EFI_PEI_MP_SERVICES_PPI   *This,\r
+  IN  EFI_AP_PROCEDURE          Procedure,\r
+  IN  BOOLEAN                   SingleThread,\r
+  IN  UINTN                     TimeoutInMicroSeconds,\r
+  IN  VOID                      *ProcedureArgument      OPTIONAL\r
   )\r
 {\r
-  PEI_CPU_MP_DATA            *PeiCpuMpData;\r
-  UINTN                      ProcessorNumber;\r
-  EFI_AP_PROCEDURE           Procedure;\r
-  UINTN                      BistData;\r
-\r
-  PeiCpuMpData = ExchangeInfo->PeiCpuMpData;\r
-  if (PeiCpuMpData->InitFlag) {\r
-    //\r
-    // This is first time AP wakeup, get BIST inforamtion from AP stack\r
-    //\r
-    BistData = *(UINTN *) (PeiCpuMpData->Buffer + NumApsExecuting * PeiCpuMpData->CpuApStackSize - sizeof (UINTN));\r
-    PeiCpuMpData->CpuData[NumApsExecuting].ApicId        = GetInitialApicId ();\r
-    PeiCpuMpData->CpuData[NumApsExecuting].Health.Uint32 = (UINT32) BistData;\r
-    //\r
-    // Sync BSP's Mtrr table to all wakeup APs and load microcode on APs.\r
-    //\r
-    MtrrSetAllMtrrs (&PeiCpuMpData->MtrrTable);\r
-    MicrocodeDetect ();\r
-  } else {\r
-    //\r
-    // Execute AP function if AP is not disabled\r
-    //\r
-    GetProcessorNumber (PeiCpuMpData, &ProcessorNumber);\r
-    if ((PeiCpuMpData->CpuData[ProcessorNumber].State != CpuStateDisabled) &&\r
-        (PeiCpuMpData->ApFunction != 0)) {\r
-      PeiCpuMpData->CpuData[ProcessorNumber].State = CpuStateBusy;\r
-      Procedure = (EFI_AP_PROCEDURE)(UINTN)PeiCpuMpData->ApFunction;\r
-      Procedure ((VOID *)(UINTN)PeiCpuMpData->ApFunctionArgument);\r
-      PeiCpuMpData->CpuData[ProcessorNumber].State = CpuStateIdle;\r
-    }\r
-  }\r
-\r
-  //\r
-  // AP finished executing C code\r
-  //\r
-  InterlockedIncrement ((UINT32 *)&PeiCpuMpData->FinishedCount);\r
-\r
-  AsmCliHltLoop ();\r
+  return MpInitLibStartupAllAPs (\r
+           Procedure,\r
+           SingleThread,\r
+           NULL,\r
+           TimeoutInMicroSeconds,\r
+           ProcedureArgument,\r
+           NULL\r
+           );\r
 }\r
 \r
 /**\r
-  This function will be called by BSP to wakeup AP.\r
-\r
-  @param PeiCpuMpData       Pointer to PEI CPU MP Data\r
-  @param Broadcast          TRUE:  Send broadcast IPI to all APs\r
-                            FALSE: Send IPI to AP by ApicId\r
-  @param ApicId             Apic ID for the processor to be waked\r
-  @param Procedure          The function to be invoked by AP\r
-  @param ProcedureArgument  The argument to be passed into AP function\r
+  This service lets the caller get one enabled AP to execute a caller-provided\r
+  function. The caller can request the BSP to wait for the completion\r
+  of the AP. This service may only be called from the BSP.\r
+\r
+  This function is used to dispatch one enabled AP to the function specified by\r
+  Procedure passing in the argument specified by ProcedureArgument.\r
+  The execution is in blocking mode. The BSP waits until the AP finishes or\r
+  TimeoutInMicroSecondss expires.\r
+\r
+  If the timeout specified by TimeoutInMicroseconds expires before the AP returns\r
+  from Procedure, then execution of Procedure by the AP is terminated. The AP is\r
+  available for subsequent calls to EFI_PEI_MP_SERVICES_PPI.StartupAllAPs() and\r
+  EFI_PEI_MP_SERVICES_PPI.StartupThisAP().\r
+\r
+  @param[in] PeiServices          An indirect pointer to the PEI Services Table\r
+                                  published by the PEI Foundation.\r
+  @param[in] This                 A pointer to the EFI_PEI_MP_SERVICES_PPI instance.\r
+  @param[in] Procedure            A pointer to the function to be run on enabled APs of\r
+                                  the system.\r
+  @param[in] ProcessorNumber      The handle number of the AP. The range is from 0 to the\r
+                                  total number of logical processors minus 1. The total\r
+                                  number of logical processors can be retrieved by\r
+                                  EFI_PEI_MP_SERVICES_PPI.GetNumberOfProcessors().\r
+  @param[in] TimeoutInMicroseconds\r
+                                  Indicates the time limit in microseconds for APs to\r
+                                  return from Procedure, for blocking mode only. Zero\r
+                                  means infinity. If the timeout expires before all APs\r
+                                  return from Procedure, then Procedure on the failed APs\r
+                                  is terminated. All enabled APs are available for next\r
+                                  function assigned by EFI_PEI_MP_SERVICES_PPI.StartupAllAPs()\r
+                                  or EFI_PEI_MP_SERVICES_PPI.StartupThisAP(). If the\r
+                                  timeout expires in blocking mode, BSP returns\r
+                                  EFI_TIMEOUT.\r
+  @param[in] ProcedureArgument    The parameter passed into Procedure for all APs.\r
+\r
+  @retval EFI_SUCCESS             In blocking mode, specified AP finished before the\r
+                                  timeout expires.\r
+  @retval EFI_DEVICE_ERROR        The calling processor is an AP.\r
+  @retval EFI_TIMEOUT             In blocking mode, the timeout expired before the\r
+                                  specified AP has finished.\r
+  @retval EFI_NOT_FOUND           The processor with the handle specified by\r
+                                  ProcessorNumber does not exist.\r
+  @retval EFI_INVALID_PARAMETER   ProcessorNumber specifies the BSP or disabled AP.\r
+  @retval EFI_INVALID_PARAMETER   Procedure is NULL.\r
 **/\r
-VOID\r
-WakeUpAP (\r
-  IN PEI_CPU_MP_DATA           *PeiCpuMpData,\r
-  IN BOOLEAN                   Broadcast,\r
-  IN UINT32                    ApicId,\r
-  IN EFI_AP_PROCEDURE          Procedure,              OPTIONAL\r
-  IN VOID                      *ProcedureArgument      OPTIONAL\r
+EFI_STATUS\r
+EFIAPI\r
+PeiStartupThisAP (\r
+  IN  CONST EFI_PEI_SERVICES    **PeiServices,\r
+  IN  EFI_PEI_MP_SERVICES_PPI   *This,\r
+  IN  EFI_AP_PROCEDURE          Procedure,\r
+  IN  UINTN                     ProcessorNumber,\r
+  IN  UINTN                     TimeoutInMicroseconds,\r
+  IN  VOID                      *ProcedureArgument      OPTIONAL\r
   )\r
 {\r
-  volatile MP_CPU_EXCHANGE_INFO    *ExchangeInfo;\r
-\r
-  PeiCpuMpData->ApFunction         = (UINTN) Procedure;\r
-  PeiCpuMpData->ApFunctionArgument = (UINTN) ProcedureArgument;\r
-  PeiCpuMpData->FinishedCount      = 0;\r
-\r
-  ExchangeInfo                     = PeiCpuMpData->MpCpuExchangeInfo;\r
-  ExchangeInfo->Lock               = 0;\r
-  ExchangeInfo->StackStart         = PeiCpuMpData->Buffer;\r
-  ExchangeInfo->StackSize          = PeiCpuMpData->CpuApStackSize;\r
-  ExchangeInfo->BufferStart        = PeiCpuMpData->WakeupBuffer;\r
-  ExchangeInfo->PmodeOffset        = PeiCpuMpData->AddressMap.PModeEntryOffset;\r
-  ExchangeInfo->LmodeOffset        = PeiCpuMpData->AddressMap.LModeEntryOffset;\r
-  ExchangeInfo->Cr3                = AsmReadCr3 ();\r
-  ExchangeInfo->CFunction          = (UINTN) ApCFunction;\r
-  ExchangeInfo->NumApsExecuting    = 0;\r
-  ExchangeInfo->PeiCpuMpData       = PeiCpuMpData;\r
-\r
-  //\r
-  // Get the BSP's data of GDT and IDT\r
-  //\r
-  CopyMem ((VOID *)&ExchangeInfo->GdtrProfile, &mGdt, sizeof(mGdt));\r
-  AsmReadIdtr ((IA32_DESCRIPTOR *) &ExchangeInfo->IdtrProfile);\r
-\r
-  if (Broadcast) {\r
-    SendInitSipiSipiAllExcludingSelf ((UINT32) ExchangeInfo->BufferStart);\r
-  } else {\r
-    SendInitSipiSipi (ApicId, (UINT32) ExchangeInfo->BufferStart);\r
-  }\r
-\r
-  return ;\r
+  return MpInitLibStartupThisAP (\r
+           Procedure,\r
+           ProcessorNumber,\r
+           NULL,\r
+           TimeoutInMicroseconds,\r
+           ProcedureArgument,\r
+           NULL\r
+           );\r
 }\r
 \r
 /**\r
-  Get available system memory below 1MB by specified size.\r
-\r
-  @param  WakeupBufferSize   Wakeup buffer size required\r
-\r
-  @retval other   Return wakeup buffer address below 1MB.\r
-  @retval -1      Cannot find free memory below 1MB.\r
+  This service switches the requested AP to be the BSP from that point onward.\r
+  This service changes the BSP for all purposes.   This call can only be performed\r
+  by the current BSP.\r
+\r
+  This service switches the requested AP to be the BSP from that point onward.\r
+  This service changes the BSP for all purposes. The new BSP can take over the\r
+  execution of the old BSP and continue seamlessly from where the old one left\r
+  off.\r
+\r
+  If the BSP cannot be switched prior to the return from this service, then\r
+  EFI_UNSUPPORTED must be returned.\r
+\r
+  @param[in] PeiServices          An indirect pointer to the PEI Services Table\r
+                                  published by the PEI Foundation.\r
+  @param[in] This                 A pointer to the EFI_PEI_MP_SERVICES_PPI instance.\r
+  @param[in] ProcessorNumber      The handle number of the AP. The range is from 0 to the\r
+                                  total number of logical processors minus 1. The total\r
+                                  number of logical processors can be retrieved by\r
+                                  EFI_PEI_MP_SERVICES_PPI.GetNumberOfProcessors().\r
+  @param[in] EnableOldBSP         If TRUE, then the old BSP will be listed as an enabled\r
+                                  AP. Otherwise, it will be disabled.\r
+\r
+  @retval EFI_SUCCESS             BSP successfully switched.\r
+  @retval EFI_UNSUPPORTED         Switching the BSP cannot be completed prior to this\r
+                                  service returning.\r
+  @retval EFI_UNSUPPORTED         Switching the BSP is not supported.\r
+  @retval EFI_DEVICE_ERROR        The calling processor is an AP.\r
+  @retval EFI_NOT_FOUND           The processor with the handle specified by\r
+                                  ProcessorNumber does not exist.\r
+  @retval EFI_INVALID_PARAMETER   ProcessorNumber specifies the current BSP or a disabled\r
+                                  AP.\r
+  @retval EFI_NOT_READY           The specified AP is busy.\r
 **/\r
-UINTN\r
-GetWakeupBuffer (\r
-  IN UINTN                WakeupBufferSize\r
+EFI_STATUS\r
+EFIAPI\r
+PeiSwitchBSP (\r
+  IN  CONST EFI_PEI_SERVICES   **PeiServices,\r
+  IN  EFI_PEI_MP_SERVICES_PPI  *This,\r
+  IN  UINTN                    ProcessorNumber,\r
+  IN  BOOLEAN                  EnableOldBSP\r
   )\r
 {\r
-  EFI_PEI_HOB_POINTERS    Hob;\r
-  UINTN                   WakeupBufferStart;\r
-  UINTN                   WakeupBufferEnd;\r
-\r
-  //\r
-  // Get the HOB list for processing\r
-  //\r
-  Hob.Raw = GetHobList ();\r
-\r
-  //\r
-  // Collect memory ranges\r
-  //\r
-  while (!END_OF_HOB_LIST (Hob)) {\r
-    if (Hob.Header->HobType == EFI_HOB_TYPE_RESOURCE_DESCRIPTOR) {\r
-      if ((Hob.ResourceDescriptor->PhysicalStart < BASE_1MB) &&\r
-          (Hob.ResourceDescriptor->ResourceType == EFI_RESOURCE_SYSTEM_MEMORY) &&\r
-          ((Hob.ResourceDescriptor->ResourceAttribute &\r
-            (EFI_RESOURCE_ATTRIBUTE_READ_PROTECTED |\r
-             EFI_RESOURCE_ATTRIBUTE_WRITE_PROTECTED |\r
-             EFI_RESOURCE_ATTRIBUTE_EXECUTION_PROTECTED\r
-             )) == 0)\r
-           ) {\r
-        //\r
-        // Need memory under 1MB to be collected here\r
-        //\r
-        WakeupBufferEnd = (UINTN) (Hob.ResourceDescriptor->PhysicalStart + Hob.ResourceDescriptor->ResourceLength);\r
-        if (WakeupBufferEnd > BASE_1MB) {\r
-          //\r
-          // Wakeup buffer should be under 1MB\r
-          //\r
-          WakeupBufferEnd = BASE_1MB;\r
-        }\r
-        //\r
-        // Wakeup buffer should be aligned on 4KB\r
-        //\r
-        WakeupBufferStart = (WakeupBufferEnd - WakeupBufferSize) & ~(SIZE_4KB - 1);\r
-        if (WakeupBufferStart < Hob.ResourceDescriptor->PhysicalStart) {\r
-          continue;\r
-        }\r
-        //\r
-        // Create a memory allocation HOB.\r
-        //\r
-        BuildMemoryAllocationHob (\r
-          WakeupBufferStart,\r
-          WakeupBufferSize,\r
-          EfiBootServicesData\r
-          );\r
-        return WakeupBufferStart;\r
-      }\r
-    }\r
-    //\r
-    // Find the next HOB\r
-    //\r
-    Hob.Raw = GET_NEXT_HOB (Hob);\r
-  }\r
-\r
-  return (UINTN) -1;\r
+  return MpInitLibSwitchBSP (ProcessorNumber, EnableOldBSP);\r
 }\r
 \r
 /**\r
-  Get available system memory below 1MB by specified size.\r
+  This service lets the caller enable or disable an AP from this point onward.\r
+  This service may only be called from the BSP.\r
+\r
+  This service allows the caller enable or disable an AP from this point onward.\r
+  The caller can optionally specify the health status of the AP by Health. If\r
+  an AP is being disabled, then the state of the disabled AP is implementation\r
+  dependent. If an AP is enabled, then the implementation must guarantee that a\r
+  complete initialization sequence is performed on the AP, so the AP is in a state\r
+  that is compatible with an MP operating system.\r
+\r
+  If the enable or disable AP operation cannot be completed prior to the return\r
+  from this service, then EFI_UNSUPPORTED must be returned.\r
+\r
+  @param[in] PeiServices          An indirect pointer to the PEI Services Table\r
+                                  published by the PEI Foundation.\r
+  @param[in] This                 A pointer to the EFI_PEI_MP_SERVICES_PPI instance.\r
+  @param[in] ProcessorNumber      The handle number of the AP. The range is from 0 to the\r
+                                  total number of logical processors minus 1. The total\r
+                                  number of logical processors can be retrieved by\r
+                                  EFI_PEI_MP_SERVICES_PPI.GetNumberOfProcessors().\r
+  @param[in] EnableAP             Specifies the new state for the processor for enabled,\r
+                                  FALSE for disabled.\r
+  @param[in] HealthFlag           If not NULL, a pointer to a value that specifies the\r
+                                  new health status of the AP. This flag corresponds to\r
+                                  StatusFlag defined in EFI_PEI_MP_SERVICES_PPI.GetProcessorInfo().\r
+                                  Only the PROCESSOR_HEALTH_STATUS_BIT is used. All other\r
+                                  bits are ignored. If it is NULL, this parameter is\r
+                                  ignored.\r
+\r
+  @retval EFI_SUCCESS             The specified AP was enabled or disabled successfully.\r
+  @retval EFI_UNSUPPORTED         Enabling or disabling an AP cannot be completed prior\r
+                                  to this service returning.\r
+  @retval EFI_UNSUPPORTED         Enabling or disabling an AP is not supported.\r
+  @retval EFI_DEVICE_ERROR        The calling processor is an AP.\r
+  @retval EFI_NOT_FOUND           Processor with the handle specified by ProcessorNumber\r
+                                  does not exist.\r
+  @retval EFI_INVALID_PARAMETER   ProcessorNumber specifies the BSP.\r
+**/\r
+EFI_STATUS\r
+EFIAPI\r
+PeiEnableDisableAP (\r
+  IN  CONST EFI_PEI_SERVICES    **PeiServices,\r
+  IN  EFI_PEI_MP_SERVICES_PPI   *This,\r
+  IN  UINTN                     ProcessorNumber,\r
+  IN  BOOLEAN                   EnableAP,\r
+  IN  UINT32                    *HealthFlag OPTIONAL\r
+  )\r
+{\r
+  return MpInitLibEnableDisableAP (ProcessorNumber, EnableAP, HealthFlag);\r
+}\r
 \r
-  @param PeiCpuMpData        Pointer to PEI CPU MP Data\r
+/**\r
+  This return the handle number for the calling processor.  This service may be\r
+  called from the BSP and APs.\r
+\r
+  This service returns the processor handle number for the calling processor.\r
+  The returned value is in the range from 0 to the total number of logical\r
+  processors minus 1. The total number of logical processors can be retrieved\r
+  with EFI_PEI_MP_SERVICES_PPI.GetNumberOfProcessors(). This service may be\r
+  called from the BSP and APs. If ProcessorNumber is NULL, then EFI_INVALID_PARAMETER\r
+  is returned. Otherwise, the current processors handle number is returned in\r
+  ProcessorNumber, and EFI_SUCCESS is returned.\r
+\r
+  @param[in]  PeiServices         An indirect pointer to the PEI Services Table\r
+                                  published by the PEI Foundation.\r
+  @param[in]  This                A pointer to the EFI_PEI_MP_SERVICES_PPI instance.\r
+  @param[out] ProcessorNumber     The handle number of the AP. The range is from 0 to the\r
+                                  total number of logical processors minus 1. The total\r
+                                  number of logical processors can be retrieved by\r
+                                  EFI_PEI_MP_SERVICES_PPI.GetNumberOfProcessors().\r
+\r
+  @retval EFI_SUCCESS             The current processor handle number was returned in\r
+                                  ProcessorNumber.\r
+  @retval EFI_INVALID_PARAMETER   ProcessorNumber is NULL.\r
 **/\r
-VOID\r
-BackupAndPrepareWakeupBuffer(\r
-  IN PEI_CPU_MP_DATA         *PeiCpuMpData\r
+EFI_STATUS\r
+EFIAPI\r
+PeiWhoAmI (\r
+  IN  CONST EFI_PEI_SERVICES   **PeiServices,\r
+  IN  EFI_PEI_MP_SERVICES_PPI  *This,\r
+  OUT UINTN                    *ProcessorNumber\r
   )\r
 {\r
-  CopyMem (\r
-    (VOID *) PeiCpuMpData->BackupBuffer,\r
-    (VOID *) PeiCpuMpData->WakeupBuffer,\r
-    PeiCpuMpData->BackupBufferSize\r
-    );\r
-  CopyMem (\r
-    (VOID *) PeiCpuMpData->WakeupBuffer,\r
-    (VOID *) PeiCpuMpData->AddressMap.RendezvousFunnelAddress,\r
-    PeiCpuMpData->AddressMap.RendezvousFunnelSize\r
-    );\r
+  return MpInitLibWhoAmI (ProcessorNumber);\r
 }\r
 \r
 /**\r
-  Restore wakeup buffer data.\r
+  Get GDT register value.\r
+\r
+  This function is mainly for AP purpose because AP may have different GDT\r
+  table than BSP.\r
+\r
+  @param[in,out] Buffer  The pointer to private data buffer.\r
 \r
-  @param PeiCpuMpData        Pointer to PEI CPU MP Data\r
 **/\r
 VOID\r
-RestoreWakeupBuffer(\r
-  IN PEI_CPU_MP_DATA         *PeiCpuMpData\r
+EFIAPI\r
+GetGdtr (\r
+  IN OUT VOID *Buffer\r
   )\r
 {\r
-  CopyMem ((VOID *) PeiCpuMpData->WakeupBuffer, (VOID *) PeiCpuMpData->BackupBuffer, PeiCpuMpData->BackupBufferSize);\r
+  AsmReadGdtr ((IA32_DESCRIPTOR *)Buffer);\r
 }\r
 \r
 /**\r
-  This function will get CPU count in the system.\r
+  Initializes CPU exceptions handlers for the sake of stack switch requirement.\r
+\r
+  This function is a wrapper of InitializeCpuExceptionHandlersEx. It's mainly\r
+  for the sake of AP's init because of EFI_AP_PROCEDURE API requirement.\r
 \r
-  @param PeiCpuMpData        Pointer to PEI CPU MP Data\r
+  @param[in,out] Buffer  The pointer to private data buffer.\r
 \r
-  @return  AP processor count\r
 **/\r
-UINT32\r
-CountProcessorNumber (\r
-  IN PEI_CPU_MP_DATA            *PeiCpuMpData\r
+VOID\r
+EFIAPI\r
+InitializeExceptionStackSwitchHandlers (\r
+  IN OUT VOID *Buffer\r
   )\r
 {\r
+  CPU_EXCEPTION_INIT_DATA           *EssData;\r
+  IA32_DESCRIPTOR                   Idtr;\r
+  EFI_STATUS                        Status;\r
+\r
+  EssData = Buffer;\r
   //\r
-  // Load Microcode on BSP\r
-  //\r
-  MicrocodeDetect ();\r
-  //\r
-  // Store BSP's MTRR setting\r
-  //\r
-  MtrrGetAllMtrrs (&PeiCpuMpData->MtrrTable);\r
-  //\r
-  // Send broadcast IPI to APs to wakeup APs\r
-  //\r
-  PeiCpuMpData->InitFlag = 1;\r
-  WakeUpAP (PeiCpuMpData, TRUE, 0, NULL, NULL);\r
-  //\r
-  // Wait for AP task to complete and then exit.\r
-  //\r
-  MicroSecondDelay (PcdGet32 (PcdCpuApInitTimeOutInMicroSeconds));\r
-  PeiCpuMpData->InitFlag  = 0;\r
-  PeiCpuMpData->CpuCount += (UINT32) PeiCpuMpData->MpCpuExchangeInfo->NumApsExecuting;\r
-  //\r
-  // Sort BSP/Aps by CPU APIC ID in ascending order\r
+  // We don't plan to replace IDT table with a new one, but we should not assume\r
+  // the AP's IDT is the same as BSP's IDT either.\r
   //\r
-  SortApicId (PeiCpuMpData);\r
-\r
-  DEBUG ((EFI_D_INFO, "CpuMpPei: Find %d processors in system.\n", PeiCpuMpData->CpuCount));\r
-  return PeiCpuMpData->CpuCount;\r
+  AsmReadIdtr (&Idtr);\r
+  EssData->Ia32.IdtTable = (VOID *)Idtr.Base;\r
+  EssData->Ia32.IdtTableSize = Idtr.Limit + 1;\r
+  Status = InitializeCpuExceptionHandlersEx (NULL, EssData);\r
+  ASSERT_EFI_ERROR (Status);\r
 }\r
 \r
 /**\r
-  Prepare for AP wakeup buffer and copy AP reset code into it.\r
+  Initializes MP exceptions handlers for the sake of stack switch requirement.\r
 \r
-  Get wakeup buffer below 1MB. Allocate memory for CPU MP Data and APs Stack.\r
+  This function will allocate required resources required to setup stack switch\r
+  and pass them through CPU_EXCEPTION_INIT_DATA to each logic processor.\r
 \r
-  @return   Pointer to PEI CPU MP Data\r
 **/\r
-PEI_CPU_MP_DATA *\r
-PrepareAPStartupVector (\r
+VOID\r
+InitializeMpExceptionStackSwitchHandlers (\r
   VOID\r
   )\r
 {\r
-  EFI_STATUS                    Status;\r
-  UINT32                        MaxCpuCount;\r
-  PEI_CPU_MP_DATA               *PeiCpuMpData;\r
-  EFI_PHYSICAL_ADDRESS          Buffer;\r
-  UINTN                         BufferSize;\r
-  UINTN                         WakeupBuffer;\r
-  UINTN                         WakeupBufferSize;\r
-  MP_ASSEMBLY_ADDRESS_MAP       AddressMap;\r
-\r
-  AsmGetAddressMap (&AddressMap);\r
-  WakeupBufferSize = AddressMap.RendezvousFunnelSize + sizeof (MP_CPU_EXCHANGE_INFO);\r
-  WakeupBuffer     = GetWakeupBuffer ((WakeupBufferSize + SIZE_4KB - 1) & ~(SIZE_4KB - 1));\r
-  ASSERT (WakeupBuffer != (UINTN) -1);\r
-  DEBUG ((EFI_D_INFO, "CpuMpPei: WakeupBuffer = 0x%x\n", WakeupBuffer));\r
+  EFI_STATUS                      Status;\r
+  UINTN                           Index;\r
+  UINTN                           Bsp;\r
+  UINTN                           ExceptionNumber;\r
+  UINTN                           OldGdtSize;\r
+  UINTN                           NewGdtSize;\r
+  UINTN                           NewStackSize;\r
+  IA32_DESCRIPTOR                 Gdtr;\r
+  CPU_EXCEPTION_INIT_DATA         EssData;\r
+  UINT8                           *GdtBuffer;\r
+  UINT8                           *StackTop;\r
+  UINTN                           NumberOfProcessors;\r
+\r
+  if (!PcdGetBool (PcdCpuStackGuard)) {\r
+    return;\r
+  }\r
+\r
+  MpInitLibGetNumberOfProcessors(&NumberOfProcessors, NULL);\r
+  MpInitLibWhoAmI (&Bsp);\r
+\r
+  ExceptionNumber = FixedPcdGetSize (PcdCpuStackSwitchExceptionList);\r
+  NewStackSize = FixedPcdGet32 (PcdCpuKnownGoodStackSize) * ExceptionNumber;\r
+\r
+  Status = PeiServicesAllocatePool (\r
+             NewStackSize * NumberOfProcessors,\r
+             (VOID **)&StackTop\r
+             );\r
+  ASSERT(StackTop != NULL);\r
+  if (EFI_ERROR (Status)) {\r
+    ASSERT_EFI_ERROR (Status);\r
+    return;\r
+  }\r
+  StackTop += NewStackSize  * NumberOfProcessors;\r
 \r
   //\r
-  // Allocate Pages for APs stack, CPU MP Data and backup buffer for wakeup buffer\r
+  // The default exception handlers must have been initialized. Let's just skip\r
+  // it in this method.\r
   //\r
-  MaxCpuCount = PcdGet32(PcdCpuMaxLogicalProcessorNumber);\r
-  BufferSize  = PcdGet32 (PcdCpuApStackSize) * MaxCpuCount + sizeof (PEI_CPU_MP_DATA)\r
-                  + WakeupBufferSize + sizeof (PEI_CPU_DATA) * MaxCpuCount;\r
-  Status = PeiServicesAllocatePages (\r
-             EfiBootServicesData,\r
-             EFI_SIZE_TO_PAGES (BufferSize),\r
-             &Buffer\r
-             );\r
-  ASSERT_EFI_ERROR (Status);\r
+  EssData.Ia32.Revision = CPU_EXCEPTION_INIT_DATA_REV;\r
+  EssData.Ia32.InitDefaultHandlers = FALSE;\r
 \r
-  PeiCpuMpData = (PEI_CPU_MP_DATA *) (UINTN) (Buffer + PcdGet32 (PcdCpuApStackSize) * MaxCpuCount);\r
-  PeiCpuMpData->Buffer            = (UINTN) Buffer;\r
-  PeiCpuMpData->CpuApStackSize    = PcdGet32 (PcdCpuApStackSize);\r
-  PeiCpuMpData->WakeupBuffer      = WakeupBuffer;\r
-  PeiCpuMpData->BackupBuffer      = (UINTN)PeiCpuMpData + sizeof (PEI_CPU_MP_DATA);\r
-  PeiCpuMpData->BackupBufferSize  = WakeupBufferSize;\r
-  PeiCpuMpData->MpCpuExchangeInfo = (MP_CPU_EXCHANGE_INFO *) (UINTN) (WakeupBuffer + AddressMap.RendezvousFunnelSize);\r
-\r
-  PeiCpuMpData->CpuCount                 = 1;\r
-  PeiCpuMpData->BspNumber                = 0;\r
-  PeiCpuMpData->CpuData                  = (PEI_CPU_DATA *) (PeiCpuMpData->MpCpuExchangeInfo + 1);\r
-  PeiCpuMpData->CpuData[0].ApicId        = GetInitialApicId ();\r
-  PeiCpuMpData->CpuData[0].Health.Uint32 = 0;\r
-  PeiCpuMpData->EndOfPeiFlag             = FALSE;\r
-  CopyMem (&PeiCpuMpData->AddressMap, &AddressMap, sizeof (MP_ASSEMBLY_ADDRESS_MAP));\r
+  EssData.Ia32.StackSwitchExceptions = FixedPcdGetPtr(PcdCpuStackSwitchExceptionList);\r
+  EssData.Ia32.StackSwitchExceptionNumber = ExceptionNumber;\r
+  EssData.Ia32.KnownGoodStackSize = FixedPcdGet32(PcdCpuKnownGoodStackSize);\r
 \r
   //\r
-  // Backup original data and copy AP reset code in it\r
+  // Initialize Gdtr to suppress incorrect compiler/analyzer warnings.\r
   //\r
-  BackupAndPrepareWakeupBuffer(PeiCpuMpData);\r
+  Gdtr.Base = 0;\r
+  Gdtr.Limit = 0;\r
+  for (Index = 0; Index < NumberOfProcessors; ++Index) {\r
+    //\r
+    // To support stack switch, we need to re-construct GDT but not IDT.\r
+    //\r
+    if (Index == Bsp) {\r
+      GetGdtr(&Gdtr);\r
+    } else {\r
+      //\r
+      // AP might have different size of GDT from BSP.\r
+      //\r
+      MpInitLibStartupThisAP (GetGdtr, Index, NULL, 0, (VOID *)&Gdtr, NULL);\r
+    }\r
+\r
+    //\r
+    // X64 needs only one TSS of current task working for all exceptions\r
+    // because of its IST feature. IA32 needs one TSS for each exception\r
+    // in addition to current task. Since AP is not supposed to allocate\r
+    // memory, we have to do it in BSP. To simplify the code, we allocate\r
+    // memory for IA32 case to cover both IA32 and X64 exception stack\r
+    // switch.\r
+    //\r
+    // Layout of memory to allocate for each processor:\r
+    //    --------------------------------\r
+    //    |            Alignment         |  (just in case)\r
+    //    --------------------------------\r
+    //    |                              |\r
+    //    |        Original GDT          |\r
+    //    |                              |\r
+    //    --------------------------------\r
+    //    |    Current task descriptor   |\r
+    //    --------------------------------\r
+    //    |                              |\r
+    //    |  Exception task descriptors  |  X ExceptionNumber\r
+    //    |                              |\r
+    //    --------------------------------\r
+    //    |  Current task-state segment  |\r
+    //    --------------------------------\r
+    //    |                              |\r
+    //    | Exception task-state segment |  X ExceptionNumber\r
+    //    |                              |\r
+    //    --------------------------------\r
+    //\r
+    OldGdtSize = Gdtr.Limit + 1;\r
+    EssData.Ia32.ExceptionTssDescSize = sizeof (IA32_TSS_DESCRIPTOR) *\r
+                                        (ExceptionNumber + 1);\r
+    EssData.Ia32.ExceptionTssSize = sizeof (IA32_TASK_STATE_SEGMENT) *\r
+                                    (ExceptionNumber + 1);\r
+    NewGdtSize = sizeof (IA32_TSS_DESCRIPTOR) +\r
+                 OldGdtSize +\r
+                 EssData.Ia32.ExceptionTssDescSize +\r
+                 EssData.Ia32.ExceptionTssSize;\r
+\r
+    Status = PeiServicesAllocatePool (\r
+               NewGdtSize,\r
+               (VOID **)&GdtBuffer\r
+               );\r
+    ASSERT (GdtBuffer != NULL);\r
+    if (EFI_ERROR (Status)) {\r
+      ASSERT_EFI_ERROR (Status);\r
+      return;\r
+    }\r
+\r
+    //\r
+    // Make sure GDT table alignment\r
+    //\r
+    EssData.Ia32.GdtTable = ALIGN_POINTER(GdtBuffer, sizeof (IA32_TSS_DESCRIPTOR));\r
+    NewGdtSize -= ((UINT8 *)EssData.Ia32.GdtTable - GdtBuffer);\r
+    EssData.Ia32.GdtTableSize = NewGdtSize;\r
+\r
+    EssData.Ia32.ExceptionTssDesc = ((UINT8 *)EssData.Ia32.GdtTable + OldGdtSize);\r
+    EssData.Ia32.ExceptionTss = ((UINT8 *)EssData.Ia32.GdtTable + OldGdtSize +\r
+                                 EssData.Ia32.ExceptionTssDescSize);\r
+\r
+    EssData.Ia32.KnownGoodStackTop = (UINTN)StackTop;\r
+    DEBUG ((DEBUG_INFO,\r
+            "Exception stack top[cpu%lu]: 0x%lX\n",\r
+            (UINT64)(UINTN)Index,\r
+            (UINT64)(UINTN)StackTop));\r
+\r
+    if (Index == Bsp) {\r
+      InitializeExceptionStackSwitchHandlers (&EssData);\r
+    } else {\r
+      MpInitLibStartupThisAP (\r
+        InitializeExceptionStackSwitchHandlers,\r
+        Index,\r
+        NULL,\r
+        0,\r
+        (VOID *)&EssData,\r
+        NULL\r
+        );\r
+    }\r
 \r
-  return PeiCpuMpData;\r
+    StackTop  -= NewStackSize;\r
+  }\r
 }\r
 \r
 /**\r
-  Notify function on End Of Pei PPI.\r
-\r
-  On S3 boot, this function will restore wakeup buffer data.\r
-  On normal boot, this function will flag wakeup buffer to be un-used type.\r
+  Initializes MP and exceptions handlers.\r
 \r
-  @param  PeiServices        The pointer to the PEI Services Table.\r
-  @param  NotifyDescriptor   Address of the notification descriptor data structure.\r
-  @param  Ppi                Address of the PPI that was installed.\r
+  @param  PeiServices                The pointer to the PEI Services Table.\r
 \r
-  @retval EFI_SUCCESS        When everything is OK.\r
+  @retval EFI_SUCCESS     MP was successfully initialized.\r
+  @retval others          Error occurred in MP initialization.\r
 \r
 **/\r
 EFI_STATUS\r
-EFIAPI\r
-CpuMpEndOfPeiCallback (\r
-  IN      EFI_PEI_SERVICES        **PeiServices,\r
-  IN EFI_PEI_NOTIFY_DESCRIPTOR    *NotifyDescriptor,\r
-  IN VOID                         *Ppi\r
+InitializeCpuMpWorker (\r
+  IN CONST EFI_PEI_SERVICES     **PeiServices\r
   )\r
 {\r
-  EFI_STATUS                Status;\r
-  EFI_BOOT_MODE             BootMode;\r
-  PEI_CPU_MP_DATA           *PeiCpuMpData;\r
-  EFI_PEI_HOB_POINTERS      Hob;\r
-  EFI_HOB_MEMORY_ALLOCATION *MemoryHob;\r
+  EFI_STATUS                      Status;\r
+  EFI_VECTOR_HANDOFF_INFO         *VectorInfo;\r
+  EFI_PEI_VECTOR_HANDOFF_INFO_PPI *VectorHandoffInfoPpi;\r
 \r
-  DEBUG ((EFI_D_INFO, "CpuMpPei: CpuMpEndOfPeiCallback () invokded\n"));\r
-\r
-  Status = PeiServicesGetBootMode (&BootMode);\r
-  ASSERT_EFI_ERROR (Status);\r
+  //\r
+  // Get Vector Hand-off Info PPI\r
+  //\r
+  VectorInfo = NULL;\r
+  Status = PeiServicesLocatePpi (\r
+             &gEfiVectorHandoffInfoPpiGuid,\r
+             0,\r
+             NULL,\r
+             (VOID **)&VectorHandoffInfoPpi\r
+             );\r
+  if (Status == EFI_SUCCESS) {\r
+    VectorInfo = VectorHandoffInfoPpi->Info;\r
+  }\r
 \r
-  PeiCpuMpData = GetMpHobData ();\r
-  ASSERT (PeiCpuMpData != NULL);\r
+  //\r
+  // Initialize default handlers\r
+  //\r
+  Status = InitializeCpuExceptionHandlers (VectorInfo);\r
+  if (EFI_ERROR (Status)) {\r
+    return Status;\r
+  }\r
 \r
-  if (BootMode != BOOT_ON_S3_RESUME) {\r
-    //\r
-    // Get the HOB list for processing\r
-    //\r
-    Hob.Raw = GetHobList ();\r
-    //\r
-    // Collect memory ranges\r
-    //\r
-    while (!END_OF_HOB_LIST (Hob)) {\r
-      if (Hob.Header->HobType == EFI_HOB_TYPE_MEMORY_ALLOCATION) {\r
-        MemoryHob = Hob.MemoryAllocation;\r
-        if(MemoryHob->AllocDescriptor.MemoryBaseAddress == PeiCpuMpData->WakeupBuffer) {\r
-          //\r
-          // Flag this HOB type to un-used\r
-          //\r
-          GET_HOB_TYPE (Hob) = EFI_HOB_TYPE_UNUSED;\r
-          break;\r
-        }\r
-      }\r
-      Hob.Raw = GET_NEXT_HOB (Hob);\r
-    }\r
-  } else {\r
-    RestoreWakeupBuffer (PeiCpuMpData);\r
-    PeiCpuMpData->EndOfPeiFlag = TRUE;\r
+  Status = MpInitLibInitialize ();\r
+  if (EFI_ERROR (Status)) {\r
+    return Status;\r
   }\r
-  return EFI_SUCCESS;\r
+\r
+  //\r
+  // Special initialization for the sake of Stack Guard\r
+  //\r
+  InitializeMpExceptionStackSwitchHandlers ();\r
+\r
+  //\r
+  // Update and publish CPU BIST information\r
+  //\r
+  CollectBistDataFromPpi (PeiServices);\r
+\r
+  //\r
+  // Install CPU MP PPI\r
+  //\r
+  Status = PeiServicesInstallPpi(&mPeiCpuMpPpiDesc);\r
+  ASSERT_EFI_ERROR (Status);\r
+\r
+  return Status;\r
 }\r
 \r
 /**\r
@@ -524,42 +693,12 @@ CpuMpPeimInit (
   )\r
 {\r
   EFI_STATUS           Status;\r
-  PEI_CPU_MP_DATA      *PeiCpuMpData;\r
-  UINT32               ProcessorCount;\r
 \r
   //\r
-  // Load new GDT table on BSP\r
-  //\r
-  AsmInitializeGdt (&mGdt);\r
-  //\r
-  // Get wakeup buffer and copy AP reset code in it\r
-  //\r
-  PeiCpuMpData = PrepareAPStartupVector ();\r
-  //\r
-  // Count processor number and collect processor information\r
-  //\r
-  ProcessorCount = CountProcessorNumber (PeiCpuMpData);\r
-  //\r
-  // Build location of PEI CPU MP DATA buffer in HOB\r
+  // For the sake of special initialization needing to be done right after\r
+  // memory discovery.\r
   //\r
-  BuildGuidDataHob (\r
-    &gEfiCallerIdGuid,\r
-    (VOID *)&PeiCpuMpData,\r
-    sizeof(UINT64)\r
-    );\r
-  //\r
-  // Update and publish CPU BIST information\r
-  //\r
-  CollectBistDataFromPpi (PeiServices, PeiCpuMpData);\r
-  //\r
-  // register an event for EndOfPei\r
-  //\r
-  Status  = PeiServicesNotifyPpi (&mNotifyList);\r
-  ASSERT_EFI_ERROR (Status);\r
-  //\r
-  // Install CPU MP PPI\r
-  //\r
-  Status = PeiServicesInstallPpi(&mPeiCpuMpPpiDesc);\r
+  Status = PeiServicesNotifyPpi (&mPostMemNotifyList[0]);\r
   ASSERT_EFI_ERROR (Status);\r
 \r
   return Status;\r