[Patch v5 38/48] UefiCpuPkg/CpuMpPei: Consume MpInitLib to produce CPU MP PPI services

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

Consume MP initialize library to produce CPU MP PPI, it could simply the code.

Add STATIC for some internal functions to avoid build issue with the same
functions name in PeiMpInit instance. They will be removed by the next patch.

v4:
  1. Update BistData type from UINT32 to EFI_HEALTH_FLAGS.

v3:
  1. Rename MpInitLibSwitchBSP to MpInitLibSwitchBSP
  2. Add PeiMpInitLib.inf in DSC file

Cc: Michael Kinney <michael.d.kinney@intel.com>
Cc: Feng Tian <feng.tian@intel.com>
Cc: Giri P Mudusuru <giri.p.mudusuru@intel.com>
Cc: Laszlo Ersek <lersek@redhat.com>
Contributed-under: TianoCore Contribution Agreement 1.0
Signed-off-by: Jeff Fan <jeff.fan@intel.com>
---
 UefiCpuPkg/CpuMpPei/CpuBist.c       |  53 ++--
 UefiCpuPkg/CpuMpPei/CpuMpPei.c      |  70 ++++--
 UefiCpuPkg/CpuMpPei/CpuMpPei.h      |  10 +-
 UefiCpuPkg/CpuMpPei/CpuMpPei.inf    |   1 +
 UefiCpuPkg/CpuMpPei/PeiMpServices.c | 469 +++---------------------------------
 UefiCpuPkg/UefiCpuPkg.dsc           |   1 +
 6 files changed, 113 insertions(+), 491 deletions(-)

diff --git a/UefiCpuPkg/CpuMpPei/CpuBist.c b/UefiCpuPkg/CpuMpPei/CpuBist.c
index 5629245..641eb10 100644
--- a/UefiCpuPkg/CpuMpPei/CpuBist.c
+++ b/UefiCpuPkg/CpuMpPei/CpuBist.c
@@ -44,15 +44,18 @@ SecPlatformInformation2 (
      OUT EFI_SEC_PLATFORM_INFORMATION_RECORD2 *PlatformInformationRecord2
   )
 {
-  PEI_CPU_MP_DATA                      *PeiCpuMpData;
   UINTN                                BistInformationSize;
   UINTN                                CpuIndex;
   EFI_SEC_PLATFORM_INFORMATION_CPU     *CpuInstance;
+  EFI_PROCESSOR_INFORMATION            ProcessorInfo;
+  EFI_HEALTH_FLAGS                     BistData;
+  UINTN                                NumberOfProcessors;
+  UINTN                                NumberOfEnabledProcessors;
 
-  PeiCpuMpData = GetMpHobData ();
+  MpInitLibGetNumberOfProcessors(&NumberOfProcessors, &NumberOfEnabledProcessors);
 
   BistInformationSize = sizeof (EFI_SEC_PLATFORM_INFORMATION_RECORD2) +
-                        sizeof (EFI_SEC_PLATFORM_INFORMATION_CPU) * PeiCpuMpData->CpuCount;
+                        sizeof (EFI_SEC_PLATFORM_INFORMATION_CPU) * NumberOfProcessors;
   //
   // return the information size if input buffer size is too small
   //
@@ -61,11 +64,12 @@ SecPlatformInformation2 (
     return EFI_BUFFER_TOO_SMALL;
   }
 
-  PlatformInformationRecord2->NumberOfCpus = PeiCpuMpData->CpuCount;
+  PlatformInformationRecord2->NumberOfCpus = (UINT32)NumberOfProcessors;
   CpuInstance = PlatformInformationRecord2->CpuInstance;
-  for (CpuIndex = 0; CpuIndex < PeiCpuMpData->CpuCount; CpuIndex ++) {
-    CpuInstance[CpuIndex].CpuLocation                = PeiCpuMpData->CpuData[CpuIndex].ApicId;
-    CpuInstance[CpuIndex].InfoRecord.IA32HealthFlags = PeiCpuMpData->CpuData[CpuIndex].Health;
+  for (CpuIndex = 0; CpuIndex < NumberOfProcessors; CpuIndex ++) {
+    MpInitLibGetProcessorInfo (CpuIndex, &ProcessorInfo, &BistData);
+    CpuInstance[CpuIndex].CpuLocation = (UINT32) ProcessorInfo.ProcessorId;
+    CpuInstance[CpuIndex].InfoRecord.IA32HealthFlags = BistData;
   }
 
   return EFI_SUCCESS;
@@ -152,13 +156,11 @@ GetBistInfoFromPpi (
   or SEC Platform Information PPI.
 
   @param PeiServices         Pointer to PEI Services Table
-  @param PeiCpuMpData        Pointer to PEI CPU MP Data
 
 **/
 VOID
 CollectBistDataFromPpi (
-  IN CONST EFI_PEI_SERVICES             **PeiServices,
-  IN PEI_CPU_MP_DATA                    *PeiCpuMpData
+  IN CONST EFI_PEI_SERVICES             **PeiServices
   )
 {
   EFI_STATUS                            Status;
@@ -170,7 +172,12 @@ CollectBistDataFromPpi (
   EFI_SEC_PLATFORM_INFORMATION_CPU      BspCpuInstance;
   UINTN                                 ProcessorNumber;
   UINTN                                 CpuIndex;
-  PEI_CPU_DATA                          *CpuData;
+  EFI_PROCESSOR_INFORMATION             ProcessorInfo;
+  EFI_HEALTH_FLAGS                      BistData;
+  UINTN                                 NumberOfProcessors;
+  UINTN                                 NumberOfEnabledProcessors;
+
+  MpInitLibGetNumberOfProcessors(&NumberOfProcessors, &NumberOfEnabledProcessors);
 
   SecPlatformInformation2 = NULL;
   SecPlatformInformation  = NULL;
@@ -215,21 +222,18 @@ CollectBistDataFromPpi (
       DEBUG ((EFI_D_INFO, "Does not find any stored CPU BIST information from PPI!\n"));
     }
   }
-  for (ProcessorNumber = 0; ProcessorNumber < PeiCpuMpData->CpuCount; ProcessorNumber ++) {
-    CpuData = &PeiCpuMpData->CpuData[ProcessorNumber];
+  for (ProcessorNumber = 0; ProcessorNumber < NumberOfProcessors; ProcessorNumber ++) {
+    MpInitLibGetProcessorInfo (ProcessorNumber, &ProcessorInfo, &BistData);
     for (CpuIndex = 0; CpuIndex < NumberOfData; CpuIndex ++) {
       ASSERT (CpuInstance != NULL);
-      if (CpuData->ApicId == CpuInstance[CpuIndex].CpuLocation) {
+      if (ProcessorInfo.ProcessorId == CpuInstance[CpuIndex].CpuLocation) {
         //
         // Update processor's BIST data if it is already stored before
         //
-        CpuData->Health = CpuInstance[CpuIndex].InfoRecord.IA32HealthFlags;
+        BistData = CpuInstance[CpuIndex].InfoRecord.IA32HealthFlags;
       }
     }
-    if (CpuData->Health.Uint32 == 0) {
-      CpuData->CpuHealthy = TRUE;
-    } else {
-      CpuData->CpuHealthy = FALSE;
+    if (BistData.Uint32 != 0) {
       //
       // Report Status Code that self test is failed
       //
@@ -239,14 +243,14 @@ CollectBistDataFromPpi (
         );
     }
     DEBUG ((EFI_D_INFO, "  APICID - 0x%08x, BIST - 0x%08x\n",
-            PeiCpuMpData->CpuData[ProcessorNumber].ApicId,
-            PeiCpuMpData->CpuData[ProcessorNumber].Health.Uint32
+            ProcessorInfo.ProcessorId,
+            BistData
             ));
   }
 
-  if (SecPlatformInformation2 != NULL && NumberOfData < PeiCpuMpData->CpuCount) {
+  if (SecPlatformInformation2 != NULL && NumberOfData < NumberOfProcessors) {
     //
-    // Reinstall SecPlatformInformation2 PPI to include new BIST inforamtion
+    // Reinstall SecPlatformInformation2 PPI to include new BIST information
     //
     Status = PeiServicesReInstallPpi (
                SecInformationDescriptor,
@@ -255,9 +259,10 @@ CollectBistDataFromPpi (
     ASSERT_EFI_ERROR (Status);
   } else {
     //
-    // Install SecPlatformInformation2 PPI to include new BIST inforamtion
+    // Install SecPlatformInformation2 PPI to include new BIST information
     //
     Status = PeiServicesInstallPpi (&mPeiSecPlatformInformation2Ppi);
     ASSERT_EFI_ERROR(Status);
   }
 }
+
diff --git a/UefiCpuPkg/CpuMpPei/CpuMpPei.c b/UefiCpuPkg/CpuMpPei/CpuMpPei.c
index 4a453c6..b5f8887 100644
--- a/UefiCpuPkg/CpuMpPei/CpuMpPei.c
+++ b/UefiCpuPkg/CpuMpPei/CpuMpPei.c
@@ -28,6 +28,7 @@ GLOBAL_REMOVE_IF_UNREFERENCED EFI_PEI_NOTIFY_DESCRIPTOR mNotifyList = {
 
   @param PeiCpuMpData        Pointer to PEI CPU MP Data
 **/
+STATIC
 VOID
 SortApicId (
   IN PEI_CPU_MP_DATA   *PeiCpuMpData
@@ -84,6 +85,7 @@ SortApicId (
 
   @param Buffer  Pointer to private data buffer.
 **/
+STATIC
 VOID
 EFIAPI
 ApFuncEnableX2Apic (
@@ -100,6 +102,7 @@ ApFuncEnableX2Apic (
 
   @return The AP loop mode.
 **/
+STATIC
 UINT8
 GetApLoopMode (
   OUT UINT16     *MonitorFilterSize
@@ -170,6 +173,7 @@ GetMpHobData (
   
   @param  VolatileRegisters    Returns buffer saved the volatile resisters
 **/
+STATIC
 VOID
 SaveVolatileRegisters (
   OUT CPU_VOLATILE_REGISTERS    *VolatileRegisters
@@ -203,6 +207,7 @@ SaveVolatileRegisters (
   @param  IsRestoreDr         TRUE:  Restore DRx if supported
                               FALSE: Do not restore DRx
 **/
+STATIC
 VOID
 RestoreVolatileRegisters (
   IN CPU_VOLATILE_REGISTERS    *VolatileRegisters,
@@ -233,11 +238,41 @@ RestoreVolatileRegisters (
 }
 
 /**
+  Find the current Processor number by APIC ID.
+
+  @param PeiCpuMpData        Pointer to PEI CPU MP Data
+  @param ProcessorNumber     Return the pocessor number found
+
+  @retval EFI_SUCCESS        ProcessorNumber is found and returned.
+  @retval EFI_NOT_FOUND      ProcessorNumber is not found.
+**/
+STATIC
+EFI_STATUS
+GetProcessorNumber (
+  IN PEI_CPU_MP_DATA         *PeiCpuMpData,
+  OUT UINTN                  *ProcessorNumber
+  )
+{
+  UINTN                   TotalProcessorNumber;
+  UINTN                   Index;
+
+  TotalProcessorNumber = PeiCpuMpData->CpuCount;
+  for (Index = 0; Index < TotalProcessorNumber; Index ++) {
+    if (PeiCpuMpData->CpuData[Index].ApicId == GetInitialApicId ()) {
+      *ProcessorNumber = Index;
+      return EFI_SUCCESS;
+    }
+  }
+  return EFI_NOT_FOUND;
+}
+
+/**
   This function will be called from AP reset code if BSP uses WakeUpAP.
 
   @param ExchangeInfo     Pointer to the MP exchange info buffer
   @param NumApsExecuting  Number of current executing AP
 **/
+STATIC
 VOID
 EFIAPI
 ApCFunction (
@@ -407,6 +442,7 @@ WriteStartupSignal (
   @param Procedure          The function to be invoked by AP
   @param ProcedureArgument  The argument to be passed into AP function
 **/
+STATIC
 VOID
 WakeUpAP (
   IN PEI_CPU_MP_DATA           *PeiCpuMpData,
@@ -487,6 +523,7 @@ WakeUpAP (
   @retval other   Return wakeup buffer address below 1MB.
   @retval -1      Cannot find free memory below 1MB.
 **/
+STATIC
 UINTN
 GetWakeupBuffer (
   IN UINTN                WakeupBufferSize
@@ -556,6 +593,7 @@ GetWakeupBuffer (
 
   @param PeiCpuMpData        Pointer to PEI CPU MP Data
 **/
+STATIC
 VOID
 BackupAndPrepareWakeupBuffer(
   IN PEI_CPU_MP_DATA         *PeiCpuMpData
@@ -578,6 +616,7 @@ BackupAndPrepareWakeupBuffer(
 
   @param PeiCpuMpData        Pointer to PEI CPU MP Data
 **/
+STATIC
 VOID
 RestoreWakeupBuffer(
   IN PEI_CPU_MP_DATA         *PeiCpuMpData
@@ -760,6 +799,7 @@ PrepareAPStartupVector (
   @retval EFI_SUCCESS        When everything is OK.
 
 **/
+STATIC
 EFI_STATUS
 EFIAPI
 CpuMpEndOfPeiCallback (
@@ -829,8 +869,7 @@ CpuMpPeimInit (
   IN CONST EFI_PEI_SERVICES     **PeiServices
   )
 {
-  EFI_STATUS                       Status;
-  PEI_CPU_MP_DATA                 *PeiCpuMpData;
+  EFI_STATUS           Status;
   EFI_VECTOR_HANDOFF_INFO         *VectorInfo;
   EFI_PEI_VECTOR_HANDOFF_INFO_PPI *VectorHandoffInfoPpi;
 
@@ -849,31 +888,18 @@ CpuMpPeimInit (
   }
   Status = InitializeCpuExceptionHandlers (VectorInfo);
   ASSERT_EFI_ERROR (Status);
+  
   //
-  // Get wakeup buffer and copy AP reset code in it
-  //
-  PeiCpuMpData = PrepareAPStartupVector ();
-  //
-  // Count processor number and collect processor information
-  //
-  CountProcessorNumber (PeiCpuMpData);
-  //
-  // Build location of PEI CPU MP DATA buffer in HOB
+  // Wakeup APs to do initialization
   //
-  BuildGuidDataHob (
-    &gEfiCallerIdGuid,
-    (VOID *)&PeiCpuMpData,
-    sizeof(UINT64)
-    );
+  Status = MpInitLibInitialize ();
+  ASSERT_EFI_ERROR (Status);
+
   //
   // Update and publish CPU BIST information
   //
-  CollectBistDataFromPpi (PeiServices, PeiCpuMpData);
-  //
-  // register an event for EndOfPei
-  //
-  Status  = PeiServicesNotifyPpi (&mNotifyList);
-  ASSERT_EFI_ERROR (Status);
+  CollectBistDataFromPpi (PeiServices);
+
   //
   // Install CPU MP PPI
   //
diff --git a/UefiCpuPkg/CpuMpPei/CpuMpPei.h b/UefiCpuPkg/CpuMpPei/CpuMpPei.h
index 0d1a14a..fb57669 100644
--- a/UefiCpuPkg/CpuMpPei/CpuMpPei.h
+++ b/UefiCpuPkg/CpuMpPei/CpuMpPei.h
@@ -42,6 +42,7 @@
 #include <Library/UefiCpuLib.h>
 #include <Library/CpuLib.h>
 #include <Library/CpuExceptionHandlerLib.h>
+#include <Library/MpInitLib.h>
 
 #include "Microcode.h"
 
@@ -187,6 +188,7 @@ AsmInitializeGdt (
 
   @param PeiCpuMpData        Pointer to PEI CPU MP Data
 **/
+STATIC
 VOID
 BackupAndPrepareWakeupBuffer(
   IN PEI_CPU_MP_DATA         *PeiCpuMpData
@@ -197,6 +199,7 @@ BackupAndPrepareWakeupBuffer(
 
   @param PeiCpuMpData        Pointer to PEI CPU MP Data
 **/
+STATIC
 VOID
 RestoreWakeupBuffer(
   IN PEI_CPU_MP_DATA         *PeiCpuMpData
@@ -215,6 +218,7 @@ RestoreWakeupBuffer(
   @retval EFI_SUCCESS        When everything is OK.
 
 **/
+STATIC
 EFI_STATUS
 EFIAPI
 CpuMpEndOfPeiCallback (
@@ -233,6 +237,7 @@ CpuMpEndOfPeiCallback (
   @param Procedure          The function to be invoked by AP
   @param ProcedureArgument  The argument to be passed into AP function
 **/
+STATIC
 VOID
 WakeUpAP (
   IN PEI_CPU_MP_DATA           *PeiCpuMpData,
@@ -261,6 +266,7 @@ GetMpHobData (
   @retval EFI_SUCCESS        ProcessorNumber is found and returned.
   @retval EFI_NOT_FOUND      ProcessorNumber is not found.
 **/
+STATIC
 EFI_STATUS
 GetProcessorNumber (
   IN PEI_CPU_MP_DATA         *PeiCpuMpData,
@@ -274,13 +280,11 @@ GetProcessorNumber (
   or SEC Platform Information PPI.
 
   @param PeiServices         Pointer to PEI Services Table
-  @param PeiCpuMpData        Pointer to PEI CPU MP Data
 
 **/
 VOID
 CollectBistDataFromPpi (
-  IN CONST EFI_PEI_SERVICES             **PeiServices,
-  IN PEI_CPU_MP_DATA                    *PeiCpuMpData
+  IN CONST EFI_PEI_SERVICES             **PeiServices
   );
 
 /**
diff --git a/UefiCpuPkg/CpuMpPei/CpuMpPei.inf b/UefiCpuPkg/CpuMpPei/CpuMpPei.inf
index 5f45662..532e8a7 100644
--- a/UefiCpuPkg/CpuMpPei/CpuMpPei.inf
+++ b/UefiCpuPkg/CpuMpPei/CpuMpPei.inf
@@ -67,6 +67,7 @@ [LibraryClasses]
   UefiCpuLib
   CpuLib
   CpuExceptionHandlerLib
+  MpInitLib
 
 [Ppis]
   gEfiPeiMpServicesPpiGuid                      ## PRODUCES
diff --git a/UefiCpuPkg/CpuMpPei/PeiMpServices.c b/UefiCpuPkg/CpuMpPei/PeiMpServices.c
index e06fdf1..44e8f21 100644
--- a/UefiCpuPkg/CpuMpPei/PeiMpServices.c
+++ b/UefiCpuPkg/CpuMpPei/PeiMpServices.c
@@ -33,12 +33,14 @@ EFI_PEI_PPI_DESCRIPTOR           mPeiCpuMpPpiDesc = {
   &mMpServicesPpi
 };
 
+
 /**
   Get CPU Package/Core/Thread location information.
 
   @param InitialApicId     CPU APIC ID
   @param Location          Pointer to CPU location information
 **/
+STATIC
 VOID
 ExtractProcessorLocation (
   IN  UINT32                     InitialApicId,
@@ -144,40 +146,13 @@ ExtractProcessorLocation (
 }
 
 /**
-  Find the current Processor number by APIC ID.
-
-  @param PeiCpuMpData        Pointer to PEI CPU MP Data
-  @param ProcessorNumber     Return the pocessor number found
-
-  @retval EFI_SUCCESS        ProcessorNumber is found and returned.
-  @retval EFI_NOT_FOUND      ProcessorNumber is not found.
-**/
-EFI_STATUS
-GetProcessorNumber (
-  IN PEI_CPU_MP_DATA         *PeiCpuMpData,
-  OUT UINTN                  *ProcessorNumber
-  )
-{
-  UINTN                   TotalProcessorNumber;
-  UINTN                   Index;
-
-  TotalProcessorNumber = PeiCpuMpData->CpuCount;
-  for (Index = 0; Index < TotalProcessorNumber; Index ++) {
-    if (PeiCpuMpData->CpuData[Index].ApicId == GetInitialApicId ()) {
-      *ProcessorNumber = Index;
-      return EFI_SUCCESS;
-    }
-  }
-  return EFI_NOT_FOUND;
-}
-
-/**
   Worker function for SwitchBSP().
 
   Worker function for SwitchBSP(), assigned to the AP which is intended to become BSP.
 
   @param Buffer        Pointer to CPU MP Data
 **/
+STATIC
 VOID
 EFIAPI
 FutureBSPProc (
@@ -233,41 +208,14 @@ PeiGetNumberOfProcessors (
   OUT UINTN                     *NumberOfEnabledProcessors
   )
 {
-  PEI_CPU_MP_DATA         *PeiCpuMpData;
-  UINTN                   CallerNumber;
-  UINTN                   ProcessorNumber;
-  UINTN                   EnabledProcessorNumber;
-  UINTN                   Index;
-
-  PeiCpuMpData = GetMpHobData ();
-  if (PeiCpuMpData == NULL) {
-    return EFI_NOT_FOUND;
-  }
-
   if ((NumberOfProcessors == NULL) || (NumberOfEnabledProcessors == NULL)) {
     return EFI_INVALID_PARAMETER;
   }
 
-  //
-  // Check whether caller processor is BSP
-  //
-  PeiWhoAmI (PeiServices, This, &CallerNumber);
-  if (CallerNumber != PeiCpuMpData->BspNumber) {
-    return EFI_DEVICE_ERROR;
-  }
-
-  ProcessorNumber        = PeiCpuMpData->CpuCount;
-  EnabledProcessorNumber = 0;
-  for (Index = 0; Index < ProcessorNumber; Index++) {
-    if (PeiCpuMpData->CpuData[Index].State != CpuStateDisabled) {
-      EnabledProcessorNumber ++;
-    }
-  }
-
-  *NumberOfProcessors = ProcessorNumber;
-  *NumberOfEnabledProcessors = EnabledProcessorNumber;
-
-  return EFI_SUCCESS;
+  return MpInitLibGetNumberOfProcessors (
+           NumberOfProcessors,
+           NumberOfEnabledProcessors
+           );
 }
 
 /**
@@ -305,50 +253,7 @@ PeiGetProcessorInfo (
   OUT EFI_PROCESSOR_INFORMATION  *ProcessorInfoBuffer
   )
 {
-  PEI_CPU_MP_DATA         *PeiCpuMpData;
-  UINTN                   CallerNumber;
-
-  PeiCpuMpData = GetMpHobData ();
-  if (PeiCpuMpData == NULL) {
-    return EFI_NOT_FOUND;
-  }
-
-  //
-  // Check whether caller processor is BSP
-  //
-  PeiWhoAmI (PeiServices, This, &CallerNumber);
-  if (CallerNumber != PeiCpuMpData->BspNumber) {
-    return EFI_DEVICE_ERROR;
-  }
-
-  if (ProcessorInfoBuffer == NULL) {
-    return EFI_INVALID_PARAMETER;
-  }
-
-  if (ProcessorNumber >= PeiCpuMpData->CpuCount) {
-    return EFI_NOT_FOUND;
-  }
-
-  ProcessorInfoBuffer->ProcessorId = (UINT64) PeiCpuMpData->CpuData[ProcessorNumber].ApicId;
-  ProcessorInfoBuffer->StatusFlag  = 0;
-  if (PeiCpuMpData->CpuData[ProcessorNumber].ApicId == GetInitialApicId()) {
-    ProcessorInfoBuffer->StatusFlag |= PROCESSOR_AS_BSP_BIT;
-  }
-  if (PeiCpuMpData->CpuData[ProcessorNumber].CpuHealthy) {
-    ProcessorInfoBuffer->StatusFlag |= PROCESSOR_HEALTH_STATUS_BIT;
-  }
-  if (PeiCpuMpData->CpuData[ProcessorNumber].State == CpuStateDisabled) {
-    ProcessorInfoBuffer->StatusFlag &= ~PROCESSOR_ENABLED_BIT;
-  } else {
-    ProcessorInfoBuffer->StatusFlag |= PROCESSOR_ENABLED_BIT;
-  }
-
-  //
-  // Get processor location information
-  //
-  ExtractProcessorLocation (PeiCpuMpData->CpuData[ProcessorNumber].ApicId, &ProcessorInfoBuffer->Location);
-
-  return EFI_SUCCESS;
+  return MpInitLibGetProcessorInfo (ProcessorNumber, ProcessorInfoBuffer, NULL);
 }
 
 /**
@@ -425,131 +330,14 @@ PeiStartupAllAPs (
   IN  VOID                      *ProcedureArgument      OPTIONAL
   )
 {
-  PEI_CPU_MP_DATA         *PeiCpuMpData;
-  UINTN                   ProcessorNumber;
-  UINTN                   Index;
-  UINTN                   CallerNumber;
-  BOOLEAN                 HasEnabledAp;
-  BOOLEAN                 HasEnabledIdleAp;
-  volatile UINT32         *FinishedCount;
-  EFI_STATUS              Status;
-  UINTN                   WaitCountIndex;
-  UINTN                   WaitCountNumber;
-
-  PeiCpuMpData = GetMpHobData ();
-  if (PeiCpuMpData == NULL) {
-    return EFI_NOT_FOUND;
-  }
-
-  if (Procedure == NULL) {
-    return EFI_INVALID_PARAMETER;
-  }
-
-  //
-  // Check whether caller processor is BSP
-  //
-  PeiWhoAmI (PeiServices, This, &CallerNumber);
-  if (CallerNumber != PeiCpuMpData->BspNumber) {
-    return EFI_DEVICE_ERROR;
-  }
-
-  ProcessorNumber = PeiCpuMpData->CpuCount;
-
-  HasEnabledAp     = FALSE;
-  HasEnabledIdleAp = FALSE;
-  for (Index = 0; Index < ProcessorNumber; Index ++) {
-    if (Index == CallerNumber) {
-      //
-      // Skip BSP
-      //
-      continue;
-    }
-    if (PeiCpuMpData->CpuData[Index].State != CpuStateDisabled) {
-      HasEnabledAp = TRUE;
-      if (PeiCpuMpData->CpuData[Index].State != CpuStateBusy) {
-        HasEnabledIdleAp = TRUE;
-      }
-    }
-  }
-  if (!HasEnabledAp) {
-    //
-    // If no enabled AP exists, return EFI_NOT_STARTED.
-    //
-    return EFI_NOT_STARTED;
-  }
-  if (!HasEnabledIdleAp) {
-    //
-    // If any enabled APs are busy, return EFI_NOT_READY.
-    //
-    return EFI_NOT_READY;
-  }
-
-  if (PeiCpuMpData->EndOfPeiFlag) {
-    //
-    // Backup original data and copy AP reset vector in it
-    //
-    BackupAndPrepareWakeupBuffer(PeiCpuMpData);
-  }
-
-  WaitCountNumber = TimeoutInMicroSeconds / CPU_CHECK_AP_INTERVAL + 1;
-  WaitCountIndex = 0;
-  FinishedCount = &PeiCpuMpData->FinishedCount;
-  if (!SingleThread) {
-    WakeUpAP (PeiCpuMpData, TRUE, 0, Procedure, ProcedureArgument);
-    //
-    // Wait to finish
-    //
-    if (TimeoutInMicroSeconds == 0) {
-      while (*FinishedCount < ProcessorNumber - 1) {
-        CpuPause ();
-      }
-      Status = EFI_SUCCESS;
-    } else {
-      Status = EFI_TIMEOUT;
-      for (WaitCountIndex = 0; WaitCountIndex < WaitCountNumber; WaitCountIndex++) {
-        MicroSecondDelay (CPU_CHECK_AP_INTERVAL);
-        if (*FinishedCount >= ProcessorNumber - 1) {
-          Status = EFI_SUCCESS;
-          break;
-        }
-      }
-    }
-  } else {
-    Status = EFI_SUCCESS;
-    for (Index = 0; Index < ProcessorNumber; Index++) {
-      if (Index == CallerNumber) {
-        continue;
-      }
-      WakeUpAP (PeiCpuMpData, FALSE, Index, Procedure, ProcedureArgument);
-      //
-      // Wait to finish
-      //
-      if (TimeoutInMicroSeconds == 0) {
-        while (*FinishedCount < 1) {
-          CpuPause ();
-        }
-      } else {
-        for (WaitCountIndex = 0; WaitCountIndex < WaitCountNumber; WaitCountIndex++) {
-          MicroSecondDelay (CPU_CHECK_AP_INTERVAL);
-          if (*FinishedCount >= 1) {
-            break;
-          }
-        }
-        if (WaitCountIndex == WaitCountNumber) {
-          Status = EFI_TIMEOUT;
-        }
-      }
-    }
-  }
-
-  if (PeiCpuMpData->EndOfPeiFlag) {
-    //
-    // Restore original data
-    //
-    RestoreWakeupBuffer(PeiCpuMpData);
-  }
-
-  return Status;
+  return MpInitLibStartupAllAPs (
+           Procedure,
+           SingleThread,
+           NULL,
+           TimeoutInMicroSeconds,
+           ProcedureArgument,
+           NULL
+           );
 }
 
 /**
@@ -609,81 +397,14 @@ PeiStartupThisAP (
   IN  VOID                      *ProcedureArgument      OPTIONAL
   )
 {
-  PEI_CPU_MP_DATA         *PeiCpuMpData;
-  UINTN                   CallerNumber;
-  volatile UINT32         *FinishedCount;
-  EFI_STATUS              Status;
-  UINTN                   WaitCountIndex;
-  UINTN                   WaitCountNumber;
-
-  PeiCpuMpData = GetMpHobData ();
-  if (PeiCpuMpData == NULL) {
-    return EFI_NOT_FOUND;
-  }
-
-  //
-  // Check whether caller processor is BSP
-  //
-  PeiWhoAmI (PeiServices, This, &CallerNumber);
-  if (CallerNumber != PeiCpuMpData->BspNumber) {
-    return EFI_DEVICE_ERROR;
-  }
-
-  if (ProcessorNumber >= PeiCpuMpData->CpuCount) {
-    return EFI_NOT_FOUND;
-  }
-
-  if (ProcessorNumber == PeiCpuMpData->BspNumber || Procedure == NULL) {
-    return EFI_INVALID_PARAMETER;
-  }
-
-  //
-  // Check whether specified AP is disabled
-  //
-  if (PeiCpuMpData->CpuData[ProcessorNumber].State == CpuStateDisabled) {
-    return EFI_INVALID_PARAMETER;
-  }
-
-  if (PeiCpuMpData->EndOfPeiFlag) {
-    //
-    // Backup original data and copy AP reset vector in it
-    //
-    BackupAndPrepareWakeupBuffer(PeiCpuMpData);
-  }
-
-  WaitCountNumber = TimeoutInMicroseconds / CPU_CHECK_AP_INTERVAL + 1;
-  WaitCountIndex = 0;
-  FinishedCount = &PeiCpuMpData->FinishedCount;
-
-  WakeUpAP (PeiCpuMpData, FALSE, ProcessorNumber, Procedure, ProcedureArgument);
-
-  //
-  // Wait to finish
-  //
-  if (TimeoutInMicroseconds == 0) {
-    while (*FinishedCount < 1) {
-      CpuPause() ;
-    }
-    Status = EFI_SUCCESS;
-  } else {
-    Status = EFI_TIMEOUT;
-    for (WaitCountIndex = 0; WaitCountIndex < WaitCountNumber; WaitCountIndex++) {
-      MicroSecondDelay (CPU_CHECK_AP_INTERVAL);
-      if (*FinishedCount >= 1) {
-        Status = EFI_SUCCESS;
-        break;
-      }
-    }
-  }
-
-  if (PeiCpuMpData->EndOfPeiFlag) {
-    //
-    // Backup original data and copy AP reset vector in it
-    //
-    RestoreWakeupBuffer(PeiCpuMpData);
-  }
-
-  return Status;
+  return MpInitLibStartupThisAP (
+           Procedure,
+           ProcessorNumber,
+           NULL,
+           TimeoutInMicroseconds,
+           ProcedureArgument,
+           NULL
+           );
 }
 
 /**
@@ -729,97 +450,7 @@ PeiSwitchBSP (
   IN  BOOLEAN                  EnableOldBSP
   )
 {
-  PEI_CPU_MP_DATA              *PeiCpuMpData;
-  UINTN                        CallerNumber;
-  MSR_IA32_APIC_BASE_REGISTER  ApicBaseMsr;
-
-  PeiCpuMpData = GetMpHobData ();
-  if (PeiCpuMpData == NULL) {
-    return EFI_NOT_FOUND;
-  }
-
-  //
-  // Check whether caller processor is BSP
-  //
-  PeiWhoAmI (PeiServices, This, &CallerNumber);
-  if (CallerNumber != PeiCpuMpData->BspNumber) {
-    return EFI_SUCCESS;
-  }
-
-  if (ProcessorNumber >= PeiCpuMpData->CpuCount) {
-    return EFI_NOT_FOUND;
-  }
-
-  //
-  // Check whether specified AP is disabled
-  //
-  if (PeiCpuMpData->CpuData[ProcessorNumber].State == CpuStateDisabled) {
-    return EFI_INVALID_PARAMETER;
-  }
-
-  //
-  // Check whether ProcessorNumber specifies the current BSP
-  //
-  if (ProcessorNumber == PeiCpuMpData->BspNumber) {
-    return EFI_INVALID_PARAMETER;
-  }
-
-  //
-  // Check whether specified AP is busy
-  //
-  if (PeiCpuMpData->CpuData[ProcessorNumber].State == CpuStateBusy) {
-    return EFI_NOT_READY;
-  }
-
-  //
-  // Clear the BSP bit of MSR_IA32_APIC_BASE
-  //
-  ApicBaseMsr.Uint64 = AsmReadMsr64 (MSR_IA32_APIC_BASE);
-  ApicBaseMsr.Bits.BSP = 0;
-  AsmWriteMsr64 (MSR_IA32_APIC_BASE, ApicBaseMsr.Uint64);
-
-  PeiCpuMpData->BSPInfo.State = CPU_SWITCH_STATE_IDLE;
-  PeiCpuMpData->APInfo.State  = CPU_SWITCH_STATE_IDLE;
-
-  if (PeiCpuMpData->EndOfPeiFlag) {
-    //
-    // Backup original data and copy AP reset vector in it
-    //
-    BackupAndPrepareWakeupBuffer(PeiCpuMpData);
-  }
-
-  //
-  // Need to wakeUp AP (future BSP).
-  //
-  WakeUpAP (PeiCpuMpData, FALSE, ProcessorNumber, FutureBSPProc, PeiCpuMpData);
-
-  AsmExchangeRole (&PeiCpuMpData->BSPInfo, &PeiCpuMpData->APInfo);
-
-  if (PeiCpuMpData->EndOfPeiFlag) {
-    //
-    // Backup original data and copy AP reset vector in it
-    //
-    RestoreWakeupBuffer(PeiCpuMpData);
-  }
-
-  //
-  // Set the BSP bit of MSR_IA32_APIC_BASE on new BSP
-  //
-  ApicBaseMsr.Uint64 = AsmReadMsr64 (MSR_IA32_APIC_BASE);
-  ApicBaseMsr.Bits.BSP = 1;
-  AsmWriteMsr64 (MSR_IA32_APIC_BASE, ApicBaseMsr.Uint64);
-  //
-  // Set old BSP enable state
-  //
-  if (!EnableOldBSP) {
-    PeiCpuMpData->CpuData[PeiCpuMpData->BspNumber].State = CpuStateDisabled;
-  }
-  //
-  // Save new BSP number
-  //
-  PeiCpuMpData->BspNumber = (UINT32) ProcessorNumber;
-
-  return EFI_SUCCESS;
+  return MpInitLibSwitchBSP (ProcessorNumber, EnableOldBSP);
 }
 
 /**
@@ -871,41 +502,7 @@ PeiEnableDisableAP (
   IN  UINT32                    *HealthFlag OPTIONAL
   )
 {
-  PEI_CPU_MP_DATA         *PeiCpuMpData;
-  UINTN                   CallerNumber;
-
-  PeiCpuMpData = GetMpHobData ();
-  if (PeiCpuMpData == NULL) {
-    return EFI_NOT_FOUND;
-  }
-
-  //
-  // Check whether caller processor is BSP
-  //
-  PeiWhoAmI (PeiServices, This, &CallerNumber);
-  if (CallerNumber != PeiCpuMpData->BspNumber) {
-    return EFI_DEVICE_ERROR;
-  }
-
-  if (ProcessorNumber == PeiCpuMpData->BspNumber) {
-    return EFI_INVALID_PARAMETER;
-  }
-
-  if (ProcessorNumber >= PeiCpuMpData->CpuCount) {
-    return EFI_NOT_FOUND;
-  }
-
-  if (!EnableAP) {
-    PeiCpuMpData->CpuData[ProcessorNumber].State = CpuStateDisabled;
-  } else {
-    PeiCpuMpData->CpuData[ProcessorNumber].State = CpuStateIdle;
-  }
-
-  if (HealthFlag != NULL) {
-    PeiCpuMpData->CpuData[ProcessorNumber].CpuHealthy =
-          (BOOLEAN) ((*HealthFlag & PROCESSOR_HEALTH_STATUS_BIT) != 0);
-  }
-  return EFI_SUCCESS;
+  return MpInitLibEnableDisableAP (ProcessorNumber, EnableAP, HealthFlag);
 }
 
 /**
@@ -940,17 +537,5 @@ PeiWhoAmI (
   OUT UINTN                    *ProcessorNumber
   )
 {
-  PEI_CPU_MP_DATA         *PeiCpuMpData;
-
-  PeiCpuMpData = GetMpHobData ();
-  if (PeiCpuMpData == NULL) {
-    return EFI_NOT_FOUND;
-  }
-
-  if (ProcessorNumber == NULL) {
-    return EFI_INVALID_PARAMETER;
-  }
-
-  return GetProcessorNumber (PeiCpuMpData, ProcessorNumber);
+  return MpInitLibWhoAmI (ProcessorNumber);
 }
-
diff --git a/UefiCpuPkg/UefiCpuPkg.dsc b/UefiCpuPkg/UefiCpuPkg.dsc
index 5701a7a..ff5738b 100644
--- a/UefiCpuPkg/UefiCpuPkg.dsc
+++ b/UefiCpuPkg/UefiCpuPkg.dsc
@@ -70,6 +70,7 @@ [LibraryClasses.common.PEIM]
   MemoryAllocationLib|MdePkg/Library/PeiMemoryAllocationLib/PeiMemoryAllocationLib.inf
   HobLib|MdePkg/Library/PeiHobLib/PeiHobLib.inf
   LockBoxLib|MdeModulePkg/Library/SmmLockBoxLib/SmmLockBoxPeiLib.inf
+  MpInitLib|UefiCpuPkg/Library/MpInitLib/PeiMpInitLib.inf
 
 [LibraryClasses.IA32.PEIM, LibraryClasses.X64.PEIM]
   PeiServicesTablePointerLib|MdePkg/Library/PeiServicesTablePointerLibIdt/PeiServicesTablePointerLibIdt.inf
-- 
2.7.4.windows.1