UefiCpuPkg: Add PEI/DXE Register CPU Features Library instances

PEI Register CPU Features Library instance is used to register/manager/program
CPU features on PEI phase.
DXE Register CPU Features Library instance is used to register/manager/program
CPU features on DXE phase.

v2:
  Format debug messages.

v3:
  Trim white space at end of line.

v4:
  Remove unused local variable.

Cc: Feng Tian <feng.tian@intel.com>
Cc: Michael Kinney <michael.d.kinney@intel.com>
Contributed-under: TianoCore Contribution Agreement 1.0
Signed-off-by: Jeff Fan <jeff.fan@intel.com>
Reviewed-by: Feng Tian <feng.tian@intel.com>

9 files changed, 2517 insertions, 1 deletions

diff --git a/UefiCpuPkg/Library/RegisterCpuFeaturesLib/CpuFeaturesInitialize.c b/UefiCpuPkg/Library/RegisterCpuFeaturesLib/CpuFeaturesInitialize.c
new file mode 100644
index 0000000000..d879591ce0
--- /dev/null
+++ b/UefiCpuPkg/Library/RegisterCpuFeaturesLib/CpuFeaturesInitialize.c
@@ -0,0 +1,745 @@
+/** @file
+  CPU Features Initialize functions.
+
+  Copyright (c) 2017, Intel Corporation. All rights reserved.<BR>
+  This program and the accompanying materials
+  are licensed and made available under the terms and conditions of the BSD License
+  which accompanies this distribution.  The full text of the license may be found at
+  http://opensource.org/licenses/bsd-license.php
+
+  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#include "RegisterCpuFeatures.h"
+
+/**
+  Worker function to save PcdCpuFeaturesCapability.
+
+  @param[in]  SupportedFeatureMask  The pointer to CPU feature bits mask buffer
+*/
+VOID
+SetCapabilityPcd (
+  IN UINT8               *SupportedFeatureMask
+  )
+{
+  EFI_STATUS             Status;
+  UINTN                  BitMaskSize;
+
+  BitMaskSize = PcdGetSize (PcdCpuFeaturesCapability);
+  Status = PcdSetPtrS (PcdCpuFeaturesCapability, &BitMaskSize, SupportedFeatureMask);
+  ASSERT_EFI_ERROR (Status);
+}
+
+/**
+  Worker function to save PcdCpuFeaturesSetting.
+
+  @param[in]  SupportedFeatureMask  The pointer to CPU feature bits mask buffer
+**/
+VOID
+SetSettingPcd (
+  IN UINT8               *SupportedFeatureMask
+  )
+{
+  EFI_STATUS             Status;
+  UINTN                  BitMaskSize;
+
+  BitMaskSize = PcdGetSize (PcdCpuFeaturesSetting);
+  Status = PcdSetPtrS (PcdCpuFeaturesSetting, &BitMaskSize, SupportedFeatureMask);
+  ASSERT_EFI_ERROR (Status);
+}
+
+/**
+  Worker function to get PcdCpuFeaturesSupport.
+
+  @return  The pointer to CPU feature bits mask buffer.
+**/
+UINT8 *
+GetSupportPcds (
+  VOID
+  )
+{
+  UINTN                  BitMaskSize;
+  UINT8                  *SupportBitMask;
+
+  BitMaskSize = PcdGetSize (PcdCpuFeaturesSupport);
+  SupportBitMask = AllocateZeroPool (BitMaskSize);
+  SupportBitMask = (UINT8 *) PcdGetPtr (PcdCpuFeaturesSupport);
+
+  return SupportBitMask;
+}
+
+/**
+  Worker function to get PcdCpuFeaturesUserConfiguration.
+
+  @return  The pointer to CPU feature bits mask buffer.
+**/
+UINT8 *
+GetConfigurationPcds (
+  VOID
+  )
+{
+  UINTN                  BitMaskSize;
+  UINT8                  *SupportBitMask;
+
+  BitMaskSize = PcdGetSize (PcdCpuFeaturesUserConfiguration);
+  SupportBitMask = AllocateZeroPool (BitMaskSize);
+  SupportBitMask = (UINT8 *) PcdGetPtr (PcdCpuFeaturesUserConfiguration);
+
+  return SupportBitMask;
+}
+
+/**
+  Collects CPU type and feature information.
+
+  @param[in, out]  CpuInfo  The pointer to CPU feature information
+**/
+VOID
+FillProcessorInfo (
+  IN OUT REGISTER_CPU_FEATURE_INFORMATION        *CpuInfo
+  )
+{
+  CPUID_VERSION_INFO_EAX Eax;
+  CPUID_VERSION_INFO_ECX Ecx;
+  CPUID_VERSION_INFO_EDX Edx;
+  UINT32                 DisplayedFamily;
+  UINT32                 DisplayedModel;
+
+  AsmCpuid (CPUID_VERSION_INFO, &Eax.Uint32, NULL, &Ecx.Uint32, &Edx.Uint32);
+
+  DisplayedFamily = Eax.Bits.FamilyId;
+  if (Eax.Bits.FamilyId == 0x0F) {
+    DisplayedFamily |= (Eax.Bits.ExtendedFamilyId << 4);
+  }
+
+  DisplayedModel = Eax.Bits.Model;
+  if (Eax.Bits.FamilyId == 0x06 || Eax.Bits.FamilyId == 0x0f) {
+    DisplayedModel |= (Eax.Bits.ExtendedModelId << 4);
+  }
+
+  CpuInfo->DisplayFamily = DisplayedFamily;
+  CpuInfo->DisplayModel  = DisplayedModel;
+  CpuInfo->SteppingId    = Eax.Bits.SteppingId;
+  CpuInfo->ProcessorType = Eax.Bits.ProcessorType;
+  CpuInfo->CpuIdVersionInfoEcx.Uint32 = Ecx.Uint32;
+  CpuInfo->CpuIdVersionInfoEdx.Uint32 = Edx.Uint32;
+}
+
+/**
+  Prepares for private data used for CPU features.
+
+  @param[in]  NumberOfCpus  Number of processor in system
+**/
+VOID
+CpuInitDataInitialize (
+  IN UINTN                             NumberOfCpus
+  )
+{
+  EFI_STATUS                           Status;
+  UINTN                                ProcessorNumber;
+  EFI_PROCESSOR_INFORMATION            ProcessorInfoBuffer;
+  CPU_FEATURES_ENTRY                   *CpuFeature;
+  CPU_FEATURES_INIT_ORDER              *InitOrder;
+  CPU_FEATURES_DATA                    *CpuFeaturesData;
+  LIST_ENTRY                           *Entry;
+
+  CpuFeaturesData = GetCpuFeaturesData ();
+  CpuFeaturesData->InitOrder = AllocateZeroPool (sizeof (CPU_FEATURES_INIT_ORDER) * NumberOfCpus);
+  ASSERT (CpuFeaturesData->InitOrder != NULL);
+  CpuFeaturesData->BitMaskSize = PcdGetSize (PcdCpuFeaturesSupport);
+
+  //
+  // Collect CPU Features information
+  //
+  Entry = GetFirstNode (&CpuFeaturesData->FeatureList);
+  while (!IsNull (&CpuFeaturesData->FeatureList, Entry)) {
+    CpuFeature = CPU_FEATURE_ENTRY_FROM_LINK (Entry);
+    ASSERT (CpuFeature->InitializeFunc != NULL);
+    if (CpuFeature->GetConfigDataFunc != NULL) {
+      CpuFeature->ConfigData = CpuFeature->GetConfigDataFunc (NumberOfCpus);
+    }
+    Entry = Entry->ForwardLink;
+  }
+
+  for (ProcessorNumber = 0; ProcessorNumber < NumberOfCpus; ProcessorNumber++) {
+    InitOrder = &CpuFeaturesData->InitOrder[ProcessorNumber];
+    InitOrder->FeaturesSupportedMask = AllocateZeroPool (CpuFeaturesData->BitMaskSize);
+    InitializeListHead (&InitOrder->OrderList);
+    Status = GetProcessorInformation (ProcessorNumber, &ProcessorInfoBuffer);
+    ASSERT_EFI_ERROR (Status);
+    CopyMem (
+      &InitOrder->CpuInfo.ProcessorInfo,
+      &ProcessorInfoBuffer,
+      sizeof (EFI_PROCESSOR_INFORMATION)
+      );
+  }
+  //
+  // Get support and configuration PCDs
+  //
+  CpuFeaturesData->SupportPcds       = GetSupportPcds ();
+  CpuFeaturesData->ConfigurationPcds = GetConfigurationPcds ();
+}
+
+/**
+  Worker function to do OR operation on CPU feature supported bits mask buffer.
+
+  @param[in]  SupportedFeatureMask  The pointer to CPU feature bits mask buffer
+  @param[in]  OrFeatureBitMask      The feature bit mask to do OR operation
+**/
+VOID
+SupportedMaskOr (
+  IN UINT8               *SupportedFeatureMask,
+  IN UINT8               *OrFeatureBitMask
+  )
+{
+  UINTN                  Index;
+  UINTN                  BitMaskSize;
+  UINT8                  *Data1;
+  UINT8                  *Data2;
+
+  BitMaskSize = PcdGetSize (PcdCpuFeaturesSupport);
+  Data1 = SupportedFeatureMask;
+  Data2 = OrFeatureBitMask;
+  for (Index = 0; Index < BitMaskSize; Index++) {
+    *(Data1++) |=  *(Data2++);
+  }
+}
+
+/**
+  Worker function to do AND operation on CPU feature supported bits mask buffer.
+
+  @param[in]  SupportedFeatureMask  The pointer to CPU feature bits mask buffer
+  @param[in]  AndFeatureBitMask     The feature bit mask to do AND operation
+**/
+VOID
+SupportedMaskAnd (
+  IN UINT8               *SupportedFeatureMask,
+  IN UINT8               *AndFeatureBitMask
+  )
+{
+  UINTN                  Index;
+  UINTN                  BitMaskSize;
+  UINT8                  *Data1;
+  UINT8                  *Data2;
+
+  BitMaskSize = PcdGetSize (PcdCpuFeaturesSupport);
+  Data1 = SupportedFeatureMask;
+  Data2 = AndFeatureBitMask;
+  for (Index = 0; Index < BitMaskSize; Index++) {
+    *(Data1++) &=  *(Data2++);
+  }
+}
+
+/**
+  Worker function to check if the compared CPU feature set in the CPU feature
+  supported bits mask buffer.
+
+  @param[in]  SupportedFeatureMask   The pointer to CPU feature bits mask buffer
+  @param[in]  ComparedFeatureBitMask The feature bit mask to be compared
+
+  @retval TRUE   The ComparedFeatureBitMask is set in CPU feature supported bits
+                 mask buffer.
+  @retval FALSE  The ComparedFeatureBitMask is not set in CPU feature supported bits
+                 mask buffer.
+**/
+BOOLEAN
+IsBitMaskMatch (
+  IN UINT8               *SupportedFeatureMask,
+  IN UINT8               *ComparedFeatureBitMask
+  )
+{
+  UINTN                  Index;
+  UINTN                  BitMaskSize;
+  UINT8                  *Data1;
+  UINT8                  *Data2;
+
+  BitMaskSize = PcdGetSize (PcdCpuFeaturesSupport);
+
+  Data1 = SupportedFeatureMask;
+  Data2 = ComparedFeatureBitMask;
+  for (Index = 0; Index < BitMaskSize; Index++) {
+    if (((*(Data1++)) & (*(Data2++))) != 0) {
+      return TRUE;
+    }
+  }
+  return FALSE;
+}
+
+/**
+  Collects processor data for calling processor.
+
+  @param[in,out]  Buffer  The pointer to private data buffer.
+**/
+VOID
+EFIAPI
+CollectProcessorData (
+  IN OUT VOID                          *Buffer
+  )
+{
+  UINTN                                ProcessorNumber;
+  CPU_FEATURES_ENTRY                   *CpuFeature;
+  REGISTER_CPU_FEATURE_INFORMATION     *CpuInfo;
+  LIST_ENTRY                           *Entry;
+  CPU_FEATURES_DATA                    *CpuFeaturesData;
+
+  CpuFeaturesData = GetCpuFeaturesData ();
+  ProcessorNumber = GetProcessorIndex ();
+  CpuInfo = &CpuFeaturesData->InitOrder[ProcessorNumber].CpuInfo;
+  //
+  // collect processor information
+  //
+  FillProcessorInfo (CpuInfo);
+  Entry = GetFirstNode (&CpuFeaturesData->FeatureList);
+  while (!IsNull (&CpuFeaturesData->FeatureList, Entry)) {
+    CpuFeature = CPU_FEATURE_ENTRY_FROM_LINK (Entry);
+    if (IsBitMaskMatch (CpuFeaturesData->SupportPcds, CpuFeature->FeatureMask)) {
+      if (CpuFeature->SupportFunc == NULL) {
+        //
+        // If SupportFunc is NULL, then the feature is supported.
+        //
+        SupportedMaskOr (
+          CpuFeaturesData->InitOrder[ProcessorNumber].FeaturesSupportedMask,
+          CpuFeature->FeatureMask
+          );
+      } else if (CpuFeature->SupportFunc (ProcessorNumber, CpuInfo, CpuFeature->ConfigData)) {
+        SupportedMaskOr (
+          CpuFeaturesData->InitOrder[ProcessorNumber].FeaturesSupportedMask,
+          CpuFeature->FeatureMask
+          );
+      }
+    }
+    Entry = Entry->ForwardLink;
+  }
+}
+
+/**
+  Dump the contents of a CPU register table.
+
+  @param[in]  ProcessorNumber  The index of the CPU to show the register table contents
+
+  @note This service could be called by BSP only.
+**/
+VOID
+DumpRegisterTableOnProcessor (
+  IN UINTN                             ProcessorNumber
+  )
+{
+  CPU_FEATURES_DATA                    *CpuFeaturesData;
+  UINTN                                FeatureIndex;
+  CPU_REGISTER_TABLE                   *RegisterTable;
+  CPU_REGISTER_TABLE_ENTRY             *RegisterTableEntry;
+  CPU_REGISTER_TABLE_ENTRY             *RegisterTableEntryHead;
+  UINT32                               DebugPrintErrorLevel;
+
+  DebugPrintErrorLevel = (ProcessorNumber == 0) ? DEBUG_INFO : DEBUG_VERBOSE;
+  CpuFeaturesData      = GetCpuFeaturesData ();
+  //
+  // Debug information
+  //
+  RegisterTable = &CpuFeaturesData->RegisterTable[ProcessorNumber];
+  DEBUG ((DebugPrintErrorLevel, "RegisterTable->TableLength = %d\n", RegisterTable->TableLength));
+
+  RegisterTableEntryHead = (CPU_REGISTER_TABLE_ENTRY *) (UINTN) RegisterTable->RegisterTableEntry;
+
+  for (FeatureIndex = 0; FeatureIndex < RegisterTable->TableLength; FeatureIndex++) {
+    RegisterTableEntry = &RegisterTableEntryHead[FeatureIndex];
+    switch (RegisterTableEntry->RegisterType) {
+    case Msr:
+      DEBUG ((
+        DebugPrintErrorLevel,
+        "Processor: %d:   MSR: %x, Bit Start: %d, Bit Length: %d, Value: %lx\r\n",
+        ProcessorNumber,
+        RegisterTableEntry->Index,
+        RegisterTableEntry->ValidBitStart,
+        RegisterTableEntry->ValidBitLength,
+        RegisterTableEntry->Value
+        ));
+      break;
+    case ControlRegister:
+      DEBUG ((
+        DebugPrintErrorLevel,
+        "Processor: %d:    CR: %x, Bit Start: %d, Bit Length: %d, Value: %lx\r\n",
+        ProcessorNumber,
+        RegisterTableEntry->Index,
+        RegisterTableEntry->ValidBitStart,
+        RegisterTableEntry->ValidBitLength,
+        RegisterTableEntry->Value
+        ));
+      break;
+    case MemoryMapped:
+      DEBUG ((
+        DebugPrintErrorLevel,
+        "Processor: %d:  MMIO: %x, Bit Start: %d, Bit Length: %d, Value: %lx\r\n",
+        ProcessorNumber,
+        RegisterTableEntry->Index,
+        RegisterTableEntry->ValidBitStart,
+        RegisterTableEntry->ValidBitLength,
+        RegisterTableEntry->Value
+        ));
+      break;
+    case CacheControl:
+      DEBUG ((
+        DebugPrintErrorLevel,
+        "Processor: %d: CACHE: %x, Bit Start: %d, Bit Length: %d, Value: %lx\r\n",
+        ProcessorNumber,
+        RegisterTableEntry->Index,
+        RegisterTableEntry->ValidBitStart,
+        RegisterTableEntry->ValidBitLength,
+        RegisterTableEntry->Value
+        ));
+      break;
+    default:
+      break;
+    }
+  }
+}
+
+/**
+  Analysis register CPU features on each processor and save CPU setting in CPU register table.
+
+  @param[in]  NumberOfCpus  Number of processor in system
+
+**/
+VOID
+AnalysisProcessorFeatures (
+  IN UINTN                             NumberOfCpus
+  )
+{
+  EFI_STATUS                           Status;
+  UINTN                                ProcessorNumber;
+  CPU_FEATURES_ENTRY                   *CpuFeature;
+  CPU_FEATURES_ENTRY                   *CpuFeatureInOrder;
+  CPU_FEATURES_INIT_ORDER              *CpuInitOrder;
+  REGISTER_CPU_FEATURE_INFORMATION     *CpuInfo;
+  LIST_ENTRY                           *Entry;
+  CPU_FEATURES_DATA                    *CpuFeaturesData;
+
+  CpuFeaturesData = GetCpuFeaturesData ();
+  CpuFeaturesData->CapabilityPcds = AllocatePool (CpuFeaturesData->BitMaskSize);
+  SetMem (CpuFeaturesData->CapabilityPcds, CpuFeaturesData->BitMaskSize, 0xFF);
+  for (ProcessorNumber = 0; ProcessorNumber < NumberOfCpus; ProcessorNumber++) {
+    CpuInitOrder = &CpuFeaturesData->InitOrder[ProcessorNumber];
+    //
+    // Calculate the last capability on all processors
+    //
+    SupportedMaskAnd (CpuFeaturesData->CapabilityPcds, CpuInitOrder->FeaturesSupportedMask);
+  }
+  //
+  // Calculate the last setting
+  //
+
+  CpuFeaturesData->SettingPcds = AllocateCopyPool (CpuFeaturesData->BitMaskSize, CpuFeaturesData->CapabilityPcds);
+  SupportedMaskAnd (CpuFeaturesData->SettingPcds, CpuFeaturesData->ConfigurationPcds);
+
+  //
+  // Save PCDs and display CPU PCDs
+  //
+  SetCapabilityPcd (CpuFeaturesData->CapabilityPcds);
+  SetSettingPcd (CpuFeaturesData->SettingPcds);
+
+  //
+  // Dump the last CPU feature list
+  //
+  DEBUG_CODE (
+    DEBUG ((DEBUG_INFO, "Last CPU features list...\n"));
+    Entry = GetFirstNode (&CpuFeaturesData->FeatureList);
+    while (!IsNull (&CpuFeaturesData->FeatureList, Entry)) {
+      CpuFeature = CPU_FEATURE_ENTRY_FROM_LINK (Entry);
+      if (IsBitMaskMatch (CpuFeature->FeatureMask, CpuFeaturesData->CapabilityPcds)) {
+        if (IsBitMaskMatch (CpuFeature->FeatureMask, CpuFeaturesData->SettingPcds)) {
+          DEBUG ((DEBUG_INFO, "[Enable   ] "));
+        } else {
+          DEBUG ((DEBUG_INFO, "[Disable  ] "));
+        }
+      } else {
+        DEBUG ((DEBUG_INFO, "[Unsupport] "));
+      }
+      DumpCpuFeature (CpuFeature);
+      Entry = Entry->ForwardLink;
+    }
+    DEBUG ((DEBUG_INFO, "PcdCpuFeaturesSupport:\n"));
+    DumpCpuFeatureMask (CpuFeaturesData->SupportPcds);
+    DEBUG ((DEBUG_INFO, "PcdCpuFeaturesUserConfiguration:\n"));
+    DumpCpuFeatureMask (CpuFeaturesData->ConfigurationPcds);
+    DEBUG ((DEBUG_INFO, "PcdCpuFeaturesCapability:\n"));
+    DumpCpuFeatureMask (CpuFeaturesData->CapabilityPcds);
+    DEBUG ((DEBUG_INFO, "PcdCpuFeaturesSetting:\n"));
+    DumpCpuFeatureMask (CpuFeaturesData->SettingPcds);
+  );
+
+  for (ProcessorNumber = 0; ProcessorNumber < NumberOfCpus; ProcessorNumber++) {
+    CpuInitOrder = &CpuFeaturesData->InitOrder[ProcessorNumber];
+    Entry = GetFirstNode (&CpuFeaturesData->FeatureList);
+    while (!IsNull (&CpuFeaturesData->FeatureList, Entry)) {
+      //
+      // Insert each feature into processor's order list
+      //
+      CpuFeature = CPU_FEATURE_ENTRY_FROM_LINK (Entry);
+      if (IsBitMaskMatch (CpuFeature->FeatureMask, CpuFeaturesData->CapabilityPcds)) {
+        CpuFeatureInOrder = AllocateCopyPool (sizeof (CPU_FEATURES_ENTRY), CpuFeature);
+        InsertTailList (&CpuInitOrder->OrderList, &CpuFeatureInOrder->Link);
+      }
+      Entry = Entry->ForwardLink;
+    }
+    //
+    // Go through ordered feature list to initialize CPU features
+    //
+    CpuInfo = &CpuFeaturesData->InitOrder[ProcessorNumber].CpuInfo;
+    Entry = GetFirstNode (&CpuInitOrder->OrderList);
+    while (!IsNull (&CpuInitOrder->OrderList, Entry)) {
+      CpuFeatureInOrder = CPU_FEATURE_ENTRY_FROM_LINK (Entry);
+      if (IsBitMaskMatch (CpuFeatureInOrder->FeatureMask, CpuFeaturesData->SettingPcds)) {
+        Status = CpuFeatureInOrder->InitializeFunc (ProcessorNumber, CpuInfo, CpuFeatureInOrder->ConfigData, TRUE);
+      } else {
+        Status = CpuFeatureInOrder->InitializeFunc (ProcessorNumber, CpuInfo, CpuFeatureInOrder->ConfigData, FALSE);
+      }
+      ASSERT_EFI_ERROR (Status);
+      Entry = Entry->ForwardLink;
+    }
+    //
+    // Dump the RegisterTable
+    //
+    DumpRegisterTableOnProcessor (ProcessorNumber);
+  }
+}
+
+/**
+  Initialize the CPU registers from a register table.
+
+  @param[in]  ProcessorNumber  The index of the CPU executing this function.
+
+  @note This service could be called by BSP/APs.
+**/
+VOID
+ProgramProcessorRegister (
+  IN UINTN  ProcessorNumber
+  )
+{
+  CPU_FEATURES_DATA         *CpuFeaturesData;
+  CPU_REGISTER_TABLE        *RegisterTable;
+  CPU_REGISTER_TABLE_ENTRY  *RegisterTableEntry;
+  UINTN                     Index;
+  UINTN                     Value;
+  CPU_REGISTER_TABLE_ENTRY  *RegisterTableEntryHead;
+
+  CpuFeaturesData = GetCpuFeaturesData ();
+  RegisterTable = &CpuFeaturesData->RegisterTable[ProcessorNumber];
+
+  //
+  // Traverse Register Table of this logical processor
+  //
+  RegisterTableEntryHead = (CPU_REGISTER_TABLE_ENTRY *) (UINTN) RegisterTable->RegisterTableEntry;
+
+  for (Index = 0; Index < RegisterTable->TableLength; Index++) {
+
+    RegisterTableEntry = &RegisterTableEntryHead[Index];
+
+    //
+    // Check the type of specified register
+    //
+    switch (RegisterTableEntry->RegisterType) {
+    //
+    // The specified register is Control Register
+    //
+    case ControlRegister:
+      switch (RegisterTableEntry->Index) {
+      case 0:
+        Value = AsmReadCr0 ();
+        Value = (UINTN) BitFieldWrite64 (
+                          Value,
+                          RegisterTableEntry->ValidBitStart,
+                          RegisterTableEntry->ValidBitStart + RegisterTableEntry->ValidBitLength - 1,
+                          RegisterTableEntry->Value
+                          );
+        AsmWriteCr0 (Value);
+        break;
+      case 2:
+        Value = AsmReadCr2 ();
+        Value = (UINTN) BitFieldWrite64 (
+                          Value,
+                          RegisterTableEntry->ValidBitStart,
+                          RegisterTableEntry->ValidBitStart + RegisterTableEntry->ValidBitLength - 1,
+                          RegisterTableEntry->Value
+                          );
+        AsmWriteCr2 (Value);
+        break;
+      case 3:
+        Value = AsmReadCr3 ();
+        Value = (UINTN) BitFieldWrite64 (
+                          Value,
+                          RegisterTableEntry->ValidBitStart,
+                          RegisterTableEntry->ValidBitStart + RegisterTableEntry->ValidBitLength - 1,
+                          RegisterTableEntry->Value
+                          );
+        AsmWriteCr3 (Value);
+        break;
+      case 4:
+        Value = AsmReadCr4 ();
+        Value = (UINTN) BitFieldWrite64 (
+                          Value,
+                          RegisterTableEntry->ValidBitStart,
+                          RegisterTableEntry->ValidBitStart + RegisterTableEntry->ValidBitLength - 1,
+                          RegisterTableEntry->Value
+                          );
+        AsmWriteCr4 (Value);
+        break;
+      case 8:
+        //
+        //  Do we need to support CR8?
+        //
+        break;
+      default:
+        break;
+      }
+      break;
+    //
+    // The specified register is Model Specific Register
+    //
+    case Msr:
+      //
+      // Get lock to avoid Package/Core scope MSRs programming issue in parallel execution mode
+      //
+      AcquireSpinLock (&CpuFeaturesData->MsrLock);
+      if (RegisterTableEntry->ValidBitLength >= 64) {
+        //
+        // If length is not less than 64 bits, then directly write without reading
+        //
+        AsmWriteMsr64 (
+          RegisterTableEntry->Index,
+          RegisterTableEntry->Value
+          );
+      } else {
+        //
+        // Set the bit section according to bit start and length
+        //
+        AsmMsrBitFieldWrite64 (
+          RegisterTableEntry->Index,
+          RegisterTableEntry->ValidBitStart,
+          RegisterTableEntry->ValidBitStart + RegisterTableEntry->ValidBitLength - 1,
+          RegisterTableEntry->Value
+          );
+      }
+      ReleaseSpinLock (&CpuFeaturesData->MsrLock);
+      break;
+    //
+    // MemoryMapped operations
+    //
+    case MemoryMapped:
+      AcquireSpinLock (&CpuFeaturesData->MemoryMappedLock);
+      MmioBitFieldWrite32 (
+        RegisterTableEntry->Index,
+        RegisterTableEntry->ValidBitStart,
+        RegisterTableEntry->ValidBitStart + RegisterTableEntry->ValidBitLength - 1,
+        (UINT32)RegisterTableEntry->Value
+        );
+      ReleaseSpinLock (&CpuFeaturesData->MemoryMappedLock);
+      break;
+    //
+    // Enable or disable cache
+    //
+    case CacheControl:
+      //
+      // If value of the entry is 0, then disable cache.  Otherwise, enable cache.
+      //
+      if (RegisterTableEntry->Value == 0) {
+        AsmDisableCache ();
+      } else {
+        AsmEnableCache ();
+      }
+      break;
+
+    default:
+      break;
+    }
+  }
+}
+
+/**
+  Programs registers for the calling processor.
+
+  @param[in,out] Buffer  The pointer to private data buffer.
+
+**/
+VOID
+EFIAPI
+SetProcessorRegister (
+  IN OUT VOID            *Buffer
+  )
+{
+  UINTN                  ProcessorNumber;
+
+  ProcessorNumber = GetProcessorIndex ();
+  ProgramProcessorRegister (ProcessorNumber);
+}
+
+/**
+  Performs CPU features detection.
+
+  This service will invoke MP service to check CPU features'
+  capabilities on BSP/APs.
+
+  @note This service could be called by BSP only.
+**/
+VOID
+EFIAPI
+CpuFeaturesDetect (
+  VOID
+  )
+{
+  UINTN                  NumberOfCpus;
+  UINTN                  NumberOfEnabledProcessors;
+
+  GetNumberOfProcessor (&NumberOfCpus, &NumberOfEnabledProcessors);
+
+  CpuInitDataInitialize (NumberOfCpus);
+
+  //
+  // Wakeup all APs for data collection.
+  //
+  StartupAPsWorker (CollectProcessorData);
+
+  //
+  // Collect data on BSP
+  //
+  CollectProcessorData (NULL);
+
+  AnalysisProcessorFeatures (NumberOfCpus);
+}
+
+/**
+  Performs CPU features Initialization.
+
+  This service will invoke MP service to perform CPU features
+  initialization on BSP/APs per user configuration.
+
+  @note This service could be called by BSP only.
+**/
+VOID
+EFIAPI
+CpuFeaturesInitialize (
+  VOID
+  )
+{
+  CPU_FEATURES_DATA      *CpuFeaturesData;
+  UINTN                  OldBspNumber;
+
+  CpuFeaturesData = GetCpuFeaturesData ();
+
+  OldBspNumber = GetProcessorIndex();
+  CpuFeaturesData->BspNumber = OldBspNumber;
+  //
+  // Wakeup all APs for programming.
+  //
+  StartupAPsWorker (SetProcessorRegister);
+  //
+  // Programming BSP
+  //
+  SetProcessorRegister (NULL);
+  //
+  // Switch to new BSP if required
+  //
+  if (CpuFeaturesData->BspNumber != OldBspNumber) {
+    SwitchNewBsp (CpuFeaturesData->BspNumber);
+  }
+}
diff --git a/UefiCpuPkg/Library/RegisterCpuFeaturesLib/DxeRegisterCpuFeaturesLib.c b/UefiCpuPkg/Library/RegisterCpuFeaturesLib/DxeRegisterCpuFeaturesLib.c
new file mode 100644
index 0000000000..b5a1de7719
--- /dev/null
+++ b/UefiCpuPkg/Library/RegisterCpuFeaturesLib/DxeRegisterCpuFeaturesLib.c
@@ -0,0 +1,266 @@
+/** @file
+  CPU Register Table Library functions.
+
+  Copyright (c) 2017, Intel Corporation. All rights reserved.<BR>
+  This program and the accompanying materials
+  are licensed and made available under the terms and conditions of the BSD License
+  which accompanies this distribution.  The full text of the license may be found at
+  http://opensource.org/licenses/bsd-license.php
+
+  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#include <PiDxe.h>
+
+#include <Library/UefiBootServicesTableLib.h>
+
+#include "RegisterCpuFeatures.h"
+
+CPU_FEATURES_DATA          mCpuFeaturesData = {0};
+static EFI_MP_SERVICES_PROTOCOL   *mMpServices = NULL;
+
+/**
+  Worker function to get CPU_FEATURES_DATA pointer.
+
+  @return Pointer to CPU_FEATURES_DATA.
+**/
+CPU_FEATURES_DATA *
+GetCpuFeaturesData (
+  VOID
+  )
+{
+  return &mCpuFeaturesData;
+}
+
+/**
+  Worker function to get EFI_MP_SERVICES_PROTOCOL pointer.
+
+  @return Pointer to EFI_MP_SERVICES_PROTOCOL.
+**/
+EFI_MP_SERVICES_PROTOCOL *
+GetMpProtocol (
+  VOID
+  )
+{
+  EFI_STATUS             Status;
+
+  if (mMpServices == NULL) {
+    //
+    // Get MP Services Protocol
+    //
+    Status = gBS->LocateProtocol (
+                  &gEfiMpServiceProtocolGuid,
+                  NULL,
+                  (VOID **)&mMpServices
+                  );
+    ASSERT_EFI_ERROR (Status);
+  }
+
+  ASSERT (mMpServices != NULL);
+  return mMpServices;
+}
+
+/**
+  Worker function to return processor index.
+
+  @return  The processor index.
+**/
+UINTN
+GetProcessorIndex (
+  VOID
+  )
+{
+  EFI_STATUS                           Status;
+  UINTN                                ProcessorIndex;
+  EFI_MP_SERVICES_PROTOCOL             *MpServices;
+
+  MpServices = GetMpProtocol ();
+  Status = MpServices->WhoAmI(MpServices, &ProcessorIndex);
+  ASSERT_EFI_ERROR (Status);
+  return ProcessorIndex;
+}
+
+/**
+  Gets detailed MP-related information on the requested processor at the
+  instant this call is made.
+
+  @param[in]  ProcessorNumber       The handle number of processor.
+  @param[out] ProcessorInfoBuffer   A pointer to the buffer where information for
+                                    the requested processor is deposited.
+
+  @return Status of MpServices->GetProcessorInfo().
+**/
+EFI_STATUS
+GetProcessorInformation (
+  IN  UINTN                            ProcessorNumber,
+  OUT EFI_PROCESSOR_INFORMATION        *ProcessorInfoBuffer
+  )
+{
+  EFI_STATUS                           Status;
+  EFI_MP_SERVICES_PROTOCOL             *MpServices;
+
+  MpServices = GetMpProtocol ();
+  Status = MpServices->GetProcessorInfo (
+               MpServices,
+               ProcessorNumber,
+               ProcessorInfoBuffer
+               );
+  return Status;
+}
+
+/**
+  Worker function to execute a caller provided function on all enabled APs.
+
+  @param[in]  Procedure               A pointer to the function to be run on
+                                      enabled APs of the system.
+**/
+VOID
+StartupAPsWorker (
+  IN  EFI_AP_PROCEDURE                 Procedure
+  )
+{
+  EFI_STATUS                           Status;
+  EFI_MP_SERVICES_PROTOCOL             *MpServices;
+
+  MpServices = GetMpProtocol ();
+  //
+  // Wakeup all APs
+  //
+  Status = MpServices->StartupAllAPs (
+                 MpServices,
+                 Procedure,
+                 FALSE,
+                 NULL,
+                 0,
+                 NULL,
+                 NULL
+                 );
+  ASSERT_EFI_ERROR (Status);
+}
+
+/**
+  Worker function to switch the requested AP to be the BSP from that point onward.
+
+  @param[in] ProcessorNumber   The handle number of AP that is to become the new BSP.
+**/
+VOID
+SwitchNewBsp (
+  IN  UINTN                            ProcessorNumber
+  )
+{
+  EFI_STATUS                           Status;
+  EFI_MP_SERVICES_PROTOCOL             *MpServices;
+
+  MpServices = GetMpProtocol ();
+  //
+  // Wakeup all APs
+  //
+  Status = MpServices->SwitchBSP (
+                 MpServices,
+                 ProcessorNumber,
+                 TRUE
+                 );
+  ASSERT_EFI_ERROR (Status);
+}
+
+/**
+  Worker function to retrieve the number of logical processor in the platform.
+
+  @param[out] NumberOfProcessors          Pointer to the total number of logical
+                                          processors in the system, including the BSP
+                                          and disabled APs.
+  @param[out] NumberOfEnabledProcessors   Pointer to the number of enabled logical
+                                          processors that exist in system, including
+                                          the BSP.
+**/
+VOID
+GetNumberOfProcessor (
+  OUT UINTN                            *NumberOfCpus,
+  OUT UINTN                            *NumberOfEnabledProcessors
+  )
+{
+  EFI_STATUS                           Status;
+  EFI_MP_SERVICES_PROTOCOL             *MpServices;
+
+  MpServices = GetMpProtocol ();
+
+  //
+  // Get the number of CPUs
+  //
+  Status = MpServices->GetNumberOfProcessors (
+                         MpServices,
+                         NumberOfCpus,
+                         NumberOfEnabledProcessors
+                         );
+  ASSERT_EFI_ERROR (Status);
+}
+
+/**
+  Allocates ACPI NVS memory to save ACPI_CPU_DATA.
+
+  @return  Pointer to allocated ACPI_CPU_DATA.
+**/
+ACPI_CPU_DATA *
+AllocateAcpiCpuData (
+  VOID
+  )
+{
+  //
+  // CpuS3DataDxe will do it.
+  //
+  ASSERT (FALSE);
+  return NULL;
+}
+
+/**
+  Enlarges CPU register table for each processor.
+
+  @param[in, out]  RegisterTable   Pointer processor's CPU register table
+**/
+VOID
+EnlargeRegisterTable (
+  IN OUT CPU_REGISTER_TABLE            *RegisterTable
+  )
+{
+  EFI_STATUS            Status;
+  EFI_PHYSICAL_ADDRESS  Address;
+  UINTN                 AllocatePages;
+
+  Address = BASE_4GB - 1;
+  AllocatePages = RegisterTable->AllocatedSize / EFI_PAGE_SIZE;
+  Status  = gBS->AllocatePages (
+                   AllocateMaxAddress,
+                   EfiACPIMemoryNVS,
+                   AllocatePages + 1,
+                   &Address
+                   );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // If there are records existing in the register table, then copy its contents
+  // to new region and free the old one.
+  //
+  if (RegisterTable->AllocatedSize > 0) {
+    CopyMem (
+      (VOID *) (UINTN) Address,
+      (VOID *) (UINTN) RegisterTable->RegisterTableEntry,
+      RegisterTable->AllocatedSize
+      );
+    //
+    // RegisterTableEntry is allocated by gBS->AllocatePages() service.
+    // So, gBS->FreePages() service is used to free it.
+    //
+    gBS->FreePages (
+      RegisterTable->RegisterTableEntry,
+      AllocatePages
+      );
+  }
+
+  //
+  // Adjust the allocated size and register table base address.
+  //
+  RegisterTable->AllocatedSize     += EFI_PAGE_SIZE;
+  RegisterTable->RegisterTableEntry = Address;
+}
diff --git a/UefiCpuPkg/Library/RegisterCpuFeaturesLib/DxeRegisterCpuFeaturesLib.inf b/UefiCpuPkg/Library/RegisterCpuFeaturesLib/DxeRegisterCpuFeaturesLib.inf
new file mode 100644
index 0000000000..abffd11194
--- /dev/null
+++ b/UefiCpuPkg/Library/RegisterCpuFeaturesLib/DxeRegisterCpuFeaturesLib.inf
@@ -0,0 +1,62 @@
+## @file
+#  Register CPU Features Library DXE instance.
+#
+#  Copyright (c) 2017, Intel Corporation. All rights reserved.<BR>
+#  This program and the accompanying materials
+#  are licensed and made available under the terms and conditions of the BSD License
+#  which accompanies this distribution.  The full text of the license may be found at
+#  http://opensource.org/licenses/bsd-license.php
+#
+#  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+#  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = DxeRegisterCpuFeaturesLib
+  MODULE_UNI_FILE                = DxeRegisterCpuFeaturesLib.uni
+  FILE_GUID                      = ADE8F745-AA2E-49f6-8ED4-746B34867E52
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = RegisterCpuFeaturesLib|DXE_DRIVER
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64
+#
+
+[Sources.common]
+  DxeRegisterCpuFeaturesLib.c
+  RegisterCpuFeaturesLib.c
+  RegisterCpuFeatures.h
+  CpuFeaturesInitialize.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  UefiCpuPkg/UefiCpuPkg.dec
+
+[LibraryClasses]
+  BaseLib
+  DebugLib
+  PcdLib
+  LocalApicLib
+  BaseMemoryLib
+  MemoryAllocationLib
+  SynchronizationLib
+  UefiBootServicesTableLib
+  IoLib
+
+[Protocols]
+  gEfiMpServiceProtocolGuid                                            ## CONSUMES
+
+[Pcd]
+  gUefiCpuPkgTokenSpaceGuid.PcdCpuS3DataAddress                        ## CONSUMES
+  gUefiCpuPkgTokenSpaceGuid.PcdCpuFeaturesSupport                      ## CONSUMES
+  gUefiCpuPkgTokenSpaceGuid.PcdCpuFeaturesUserConfiguration            ## CONSUMES
+  gUefiCpuPkgTokenSpaceGuid.PcdCpuFeaturesCapability                   ## PRODUCES
+  gUefiCpuPkgTokenSpaceGuid.PcdCpuFeaturesSetting                      ## PRODUCES
+
+[Depex]
+  gEfiMpServiceProtocolGuid AND gEdkiiCpuFeaturesSetDoneGuid
diff --git a/UefiCpuPkg/Library/RegisterCpuFeaturesLib/PeiRegisterCpuFeaturesLib.c b/UefiCpuPkg/Library/RegisterCpuFeaturesLib/PeiRegisterCpuFeaturesLib.c
new file mode 100644
index 0000000000..12a516a107
--- /dev/null
+++ b/UefiCpuPkg/Library/RegisterCpuFeaturesLib/PeiRegisterCpuFeaturesLib.c
@@ -0,0 +1,390 @@
+/** @file
+  CPU Register Table Library functions.
+
+  Copyright (c) 2016, Intel Corporation. All rights reserved.<BR>
+  This program and the accompanying materials
+  are licensed and made available under the terms and conditions of the BSD License
+  which accompanies this distribution.  The full text of the license may be found at
+  http://opensource.org/licenses/bsd-license.php
+
+  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#include <PiPei.h>
+
+#include <Library/HobLib.h>
+#include <Library/PeiServicesLib.h>
+#include <Library/PeiServicesTablePointerLib.h>
+#include <Ppi/MpServices.h>
+#include "RegisterCpuFeatures.h"
+
+#define REGISTER_CPU_FEATURES_GUID \
+  { \
+    0xa694c467, 0x697a, 0x446b, { 0xb9, 0x29, 0x5b, 0x14, 0xa0, 0xcf, 0x39, 0xf } \
+  }
+
+EFI_GUID mRegisterCpuFeaturesHobGuid = REGISTER_CPU_FEATURES_GUID;
+
+/**
+  Worker function to get CPU_FEATURES_DATA pointer.
+
+  @return Pointer to CPU_FEATURES_DATA.
+**/
+CPU_FEATURES_DATA *
+GetCpuFeaturesData (
+  VOID
+  )
+{
+  CPU_FEATURES_DATA       *CpuInitData;
+  EFI_HOB_GUID_TYPE       *GuidHob;
+  VOID                    *DataInHob;
+  UINT64                  Data64;
+
+  CpuInitData = NULL;
+  GuidHob = GetFirstGuidHob (&mRegisterCpuFeaturesHobGuid);
+  if (GuidHob != NULL) {
+    DataInHob = GET_GUID_HOB_DATA (GuidHob);
+    CpuInitData = (CPU_FEATURES_DATA *) (*(UINTN *) DataInHob);
+    ASSERT (CpuInitData != NULL);
+  } else {
+    CpuInitData = AllocateZeroPool (sizeof (CPU_FEATURES_DATA));
+    ASSERT (CpuInitData != NULL);
+    //
+    // Build location of CPU MP DATA buffer in HOB
+    //
+    Data64 = (UINT64) (UINTN) CpuInitData;
+    BuildGuidDataHob (
+      &mRegisterCpuFeaturesHobGuid,
+      (VOID *) &Data64,
+      sizeof (UINT64)
+      );
+  }
+
+  return CpuInitData;
+}
+
+/**
+  Worker function to get MP PPI service pointer.
+
+  @return PEI PPI service pointer.
+**/
+EFI_PEI_MP_SERVICES_PPI *
+GetMpPpi (
+  VOID
+  )
+{
+  EFI_STATUS                 Status;
+  EFI_PEI_MP_SERVICES_PPI    *CpuMpPpi;
+
+  //
+  // Get MP Services Protocol
+  //
+  Status = PeiServicesLocatePpi (
+             &gEfiPeiMpServicesPpiGuid,
+             0,
+             NULL,
+             (VOID **)&CpuMpPpi
+             );
+  ASSERT_EFI_ERROR (Status);
+  return CpuMpPpi;
+}
+
+/**
+  Worker function to return processor index.
+
+  @return  The processor index.
+**/
+UINTN
+GetProcessorIndex (
+  VOID
+  )
+{
+  EFI_STATUS                 Status;
+  EFI_PEI_MP_SERVICES_PPI    *CpuMpPpi;
+  UINTN                      ProcessorIndex;
+
+  CpuMpPpi = GetMpPpi ();
+
+  Status = CpuMpPpi->WhoAmI(GetPeiServicesTablePointer (), CpuMpPpi, &ProcessorIndex);
+  ASSERT_EFI_ERROR (Status);
+  return ProcessorIndex;
+}
+
+/**
+  Worker function to MP-related information on the requested processor at the
+  instant this call is made.
+
+  @param[in]  ProcessorNumber       The handle number of processor.
+  @param[out] ProcessorInfoBuffer   A pointer to the buffer where information for
+                                    the requested processor is deposited.
+
+  @return Status of MpServices->GetProcessorInfo().
+**/
+EFI_STATUS
+GetProcessorInformation (
+  IN  UINTN                            ProcessorNumber,
+  OUT EFI_PROCESSOR_INFORMATION        *ProcessorInfoBuffer
+  )
+{
+  EFI_PEI_MP_SERVICES_PPI    *CpuMpPpi;
+  EFI_STATUS                 Status;
+
+  CpuMpPpi = GetMpPpi ();
+  Status = CpuMpPpi->GetProcessorInfo (
+               GetPeiServicesTablePointer(),
+               CpuMpPpi,
+               ProcessorNumber,
+               ProcessorInfoBuffer
+               );
+  return Status;
+}
+
+/**
+  Worker function to execute a caller provided function on all enabled APs.
+
+  @param[in]  Procedure               A pointer to the function to be run on
+                                      enabled APs of the system.
+**/
+VOID
+StartupAPsWorker (
+  IN  EFI_AP_PROCEDURE                 Procedure
+  )
+{
+  EFI_STATUS                           Status;
+  EFI_PEI_MP_SERVICES_PPI              *CpuMpPpi;
+
+  //
+  // Get MP Services Protocol
+  //
+  Status = PeiServicesLocatePpi (
+             &gEfiPeiMpServicesPpiGuid,
+             0,
+             NULL,
+             (VOID **)&CpuMpPpi
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Wakeup all APs for data collection.
+  //
+  Status = CpuMpPpi->StartupAllAPs (
+                 GetPeiServicesTablePointer (),
+                 CpuMpPpi,
+                 Procedure,
+                 FALSE,
+                 0,
+                 NULL
+                 );
+  ASSERT_EFI_ERROR (Status);
+}
+
+/**
+  Worker function to switch the requested AP to be the BSP from that point onward.
+
+  @param[in] ProcessorNumber   The handle number of AP that is to become the new BSP.
+**/
+VOID
+SwitchNewBsp (
+  IN  UINTN                            ProcessorNumber
+  )
+{
+  EFI_STATUS                           Status;
+  EFI_PEI_MP_SERVICES_PPI              *CpuMpPpi;
+
+  //
+  // Get MP Services Protocol
+  //
+  Status = PeiServicesLocatePpi (
+             &gEfiPeiMpServicesPpiGuid,
+             0,
+             NULL,
+             (VOID **)&CpuMpPpi
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Wakeup all APs for data collection.
+  //
+  Status = CpuMpPpi->SwitchBSP (
+                 GetPeiServicesTablePointer (),
+                 CpuMpPpi,
+                 ProcessorNumber,
+                 TRUE
+                 );
+  ASSERT_EFI_ERROR (Status);
+}
+
+/**
+  Worker function to retrieve the number of logical processor in the platform.
+
+  @param[out] NumberOfProcessors          Pointer to the total number of logical
+                                          processors in the system, including the BSP
+                                          and disabled APs.
+  @param[out] NumberOfEnabledProcessors   Pointer to the number of enabled logical
+                                          processors that exist in system, including
+                                          the BSP.
+**/
+VOID
+GetNumberOfProcessor (
+  OUT UINTN                            *NumberOfCpus,
+  OUT UINTN                            *NumberOfEnabledProcessors
+  )
+{
+  EFI_STATUS                 Status;
+  EFI_PEI_MP_SERVICES_PPI    *CpuMpPpi;
+
+  //
+  // Get MP Services Protocol
+  //
+  Status = PeiServicesLocatePpi (
+             &gEfiPeiMpServicesPpiGuid,
+             0,
+             NULL,
+             (VOID **)&CpuMpPpi
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Get the number of CPUs
+  //
+  Status = CpuMpPpi->GetNumberOfProcessors (
+                         GetPeiServicesTablePointer (),
+                         CpuMpPpi,
+                         NumberOfCpus,
+                         NumberOfEnabledProcessors
+                         );
+  ASSERT_EFI_ERROR (Status);
+}
+
+/**
+  Allocates ACPI NVS memory to save ACPI_CPU_DATA.
+
+  @return  Pointer to allocated ACPI_CPU_DATA.
+**/
+ACPI_CPU_DATA *
+AllocateAcpiCpuData (
+  VOID
+  )
+{
+  EFI_STATUS                           Status;
+  EFI_PEI_MP_SERVICES_PPI              *CpuMpPpi;
+  UINTN                                NumberOfCpus;
+  UINTN                                NumberOfEnabledProcessors;
+  ACPI_CPU_DATA                        *AcpiCpuData;
+  EFI_PHYSICAL_ADDRESS                 Address;
+  UINTN                                TableSize;
+  CPU_REGISTER_TABLE                   *RegisterTable;
+  UINTN                                Index;
+  EFI_PROCESSOR_INFORMATION            ProcessorInfoBuffer;
+
+  Status = PeiServicesAllocatePages (
+             EfiACPIMemoryNVS,
+             EFI_SIZE_TO_PAGES (sizeof (ACPI_CPU_DATA)),
+             &Address
+             );
+  ASSERT_EFI_ERROR (Status);
+  AcpiCpuData = (ACPI_CPU_DATA *) (UINTN) Address;
+  ASSERT (AcpiCpuData != NULL);
+
+  //
+  // Get MP Services Protocol
+  //
+  Status = PeiServicesLocatePpi (
+             &gEfiPeiMpServicesPpiGuid,
+             0,
+             NULL,
+             (VOID **)&CpuMpPpi
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Get the number of CPUs
+  //
+  Status = CpuMpPpi->GetNumberOfProcessors (
+                         GetPeiServicesTablePointer (),
+                         CpuMpPpi,
+                         &NumberOfCpus,
+                         &NumberOfEnabledProcessors
+                         );
+  ASSERT_EFI_ERROR (Status);
+  AcpiCpuData->NumberOfCpus = (UINT32)NumberOfCpus;
+
+  //
+  // Allocate buffer for empty RegisterTable and PreSmmInitRegisterTable for all CPUs
+  //
+  TableSize = 2 * NumberOfCpus * sizeof (CPU_REGISTER_TABLE);
+  Status = PeiServicesAllocatePages (
+             EfiACPIMemoryNVS,
+             EFI_SIZE_TO_PAGES (TableSize),
+             &Address
+             );
+  ASSERT_EFI_ERROR (Status);
+  RegisterTable = (CPU_REGISTER_TABLE *) (UINTN) Address;
+
+  for (Index = 0; Index < NumberOfCpus; Index++) {
+    Status = CpuMpPpi->GetProcessorInfo (
+                         GetPeiServicesTablePointer (),
+                         CpuMpPpi,
+                         Index,
+                         &ProcessorInfoBuffer
+                         );
+    ASSERT_EFI_ERROR (Status);
+
+    RegisterTable[Index].InitialApicId      = (UINT32)ProcessorInfoBuffer.ProcessorId;
+    RegisterTable[Index].TableLength        = 0;
+    RegisterTable[Index].AllocatedSize      = 0;
+    RegisterTable[Index].RegisterTableEntry = 0;
+
+    RegisterTable[NumberOfCpus + Index].InitialApicId      = (UINT32)ProcessorInfoBuffer.ProcessorId;
+    RegisterTable[NumberOfCpus + Index].TableLength        = 0;
+    RegisterTable[NumberOfCpus + Index].AllocatedSize      = 0;
+    RegisterTable[NumberOfCpus + Index].RegisterTableEntry = 0;
+  }
+  AcpiCpuData->RegisterTable           = (EFI_PHYSICAL_ADDRESS)(UINTN)RegisterTable;
+  AcpiCpuData->PreSmmInitRegisterTable = (EFI_PHYSICAL_ADDRESS)(UINTN)(RegisterTable + NumberOfCpus);
+
+  return AcpiCpuData;
+}
+
+/**
+  Enlarges CPU register table for each processor.
+
+  @param[in, out]  RegisterTable   Pointer processor's CPU register table
+**/
+VOID
+EnlargeRegisterTable (
+  IN OUT CPU_REGISTER_TABLE            *RegisterTable
+  )
+{
+  EFI_STATUS                           Status;
+  EFI_PHYSICAL_ADDRESS                 Address;
+  UINTN                                AllocatePages;
+
+  AllocatePages = RegisterTable->AllocatedSize / EFI_PAGE_SIZE;
+  Status = PeiServicesAllocatePages (
+             EfiACPIMemoryNVS,
+             AllocatePages + 1,
+             &Address
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // If there are records existing in the register table, then copy its contents
+  // to new region and free the old one.
+  //
+  if (RegisterTable->AllocatedSize > 0) {
+    CopyMem (
+      (VOID *) (UINTN) Address,
+      (VOID *) (UINTN) RegisterTable->RegisterTableEntry,
+      RegisterTable->AllocatedSize
+      );
+  }
+
+  //
+  // Adjust the allocated size and register table base address.
+  //
+  RegisterTable->AllocatedSize += EFI_PAGE_SIZE;
+  RegisterTable->RegisterTableEntry = Address;
+}
diff --git a/UefiCpuPkg/Library/RegisterCpuFeaturesLib/PeiRegisterCpuFeaturesLib.inf b/UefiCpuPkg/Library/RegisterCpuFeaturesLib/PeiRegisterCpuFeaturesLib.inf
new file mode 100644
index 0000000000..00907145a9
--- /dev/null
+++ b/UefiCpuPkg/Library/RegisterCpuFeaturesLib/PeiRegisterCpuFeaturesLib.inf
@@ -0,0 +1,64 @@
+## @file
+#  Register CPU Features Library PEI instance.
+#
+#  Copyright (c) 2017, Intel Corporation. All rights reserved.<BR>
+#  This program and the accompanying materials
+#  are licensed and made available under the terms and conditions of the BSD License
+#  which accompanies this distribution.  The full text of the license may be found at
+#  http://opensource.org/licenses/bsd-license.php
+#
+#  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+#  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = PeiRegisterCpuFeaturesLib
+  MODULE_UNI_FILE                = PeiRegisterCpuFeaturesLib.uni
+  FILE_GUID                      = D8855DB3-8348-41B5-BDA4-385351767D41
+  MODULE_TYPE                    = PEIM
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = RegisterCpuFeaturesLib|PEIM
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64
+#
+
+[Sources.common]
+  PeiRegisterCpuFeaturesLib.c
+  RegisterCpuFeaturesLib.c
+  RegisterCpuFeatures.h
+  CpuFeaturesInitialize.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  UefiCpuPkg/UefiCpuPkg.dec
+
+[LibraryClasses]
+  BaseLib
+  DebugLib
+  PcdLib
+  LocalApicLib
+  BaseMemoryLib
+  MemoryAllocationLib
+  SynchronizationLib
+  HobLib
+  PeiServicesLib
+  PeiServicesTablePointerLib
+  IoLib
+
+[Ppis]
+  gEfiPeiMpServicesPpiGuid                                             ## CONSUMES
+
+[Pcd]
+  gUefiCpuPkgTokenSpaceGuid.PcdCpuS3DataAddress                        ## CONSUMES
+  gUefiCpuPkgTokenSpaceGuid.PcdCpuFeaturesSupport                      ## CONSUMES
+  gUefiCpuPkgTokenSpaceGuid.PcdCpuFeaturesUserConfiguration            ## CONSUMES
+  gUefiCpuPkgTokenSpaceGuid.PcdCpuFeaturesCapability                   ## PRODUCES
+  gUefiCpuPkgTokenSpaceGuid.PcdCpuFeaturesSetting                      ## PRODUCES
+
+[Depex]
+  gEfiPeiMpServicesPpiGuid AND gEdkiiCpuFeaturesSetDoneGuid
diff --git a/UefiCpuPkg/Library/RegisterCpuFeaturesLib/RegisterCpuFeatures.h b/UefiCpuPkg/Library/RegisterCpuFeaturesLib/RegisterCpuFeatures.h
new file mode 100644
index 0000000000..2b136b6ecc
--- /dev/null
+++ b/UefiCpuPkg/Library/RegisterCpuFeaturesLib/RegisterCpuFeatures.h
@@ -0,0 +1,193 @@
+/** @file
+  CPU Register Table Library definitions.
+
+  Copyright (c) 2017, Intel Corporation. All rights reserved.<BR>
+  This program and the accompanying materials
+  are licensed and made available under the terms and conditions of the BSD License
+  which accompanies this distribution.  The full text of the license may be found at
+  http://opensource.org/licenses/bsd-license.php
+
+  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _REGISTER_CPU_FEATURES_H_
+#define _REGISTER_CPU_FEATURES_H_
+
+#include <Library/BaseLib.h>
+#include <Library/DebugLib.h>
+#include <Library/PcdLib.h>
+#include <Library/RegisterCpuFeaturesLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/SynchronizationLib.h>
+#include <Library/IoLib.h>
+
+#include <AcpiCpuData.h>
+
+#define CPU_FEATURE_ENTRY_SIGNATURE  SIGNATURE_32 ('C', 'F', 'E', 'S')
+
+#define CPU_FEATURE_NAME_SIZE        128
+
+typedef struct {
+  REGISTER_CPU_FEATURE_INFORMATION     CpuInfo;
+  UINT8                                *FeaturesSupportedMask;
+  LIST_ENTRY                           OrderList;
+} CPU_FEATURES_INIT_ORDER;
+
+typedef struct {
+  UINT32                       Signature;
+  LIST_ENTRY                   Link;
+  UINT8                        *FeatureMask;
+  CHAR8                        *FeatureName;
+  CPU_FEATURE_GET_CONFIG_DATA  GetConfigDataFunc;
+  CPU_FEATURE_SUPPORT          SupportFunc;
+  CPU_FEATURE_INITIALIZE       InitializeFunc;
+  UINT8                        *BeforeFeatureBitMask;
+  UINT8                        *AfterFeatureBitMask;
+  VOID                         *ConfigData;
+  BOOLEAN                      BeforeAll;
+  BOOLEAN                      AfterAll;
+} CPU_FEATURES_ENTRY;
+
+typedef struct {
+  UINTN                    FeaturesCount;
+  UINT32                   BitMaskSize;
+  SPIN_LOCK                MsrLock;
+  SPIN_LOCK                MemoryMappedLock;
+  LIST_ENTRY               FeatureList;
+
+  CPU_FEATURES_INIT_ORDER  *InitOrder;
+  UINT8                    *SupportPcds;
+  UINT8                    *CapabilityPcds;
+  UINT8                    *ConfigurationPcds;
+  UINT8                    *SettingPcds;
+
+  CPU_REGISTER_TABLE       *RegisterTable;
+  CPU_REGISTER_TABLE       *PreSmmRegisterTable;
+  UINTN                    BspNumber;
+} CPU_FEATURES_DATA;
+
+#define CPU_FEATURE_ENTRY_FROM_LINK(a) \
+  CR ( \
+  (a), \
+  CPU_FEATURES_ENTRY, \
+  Link, \
+  CPU_FEATURE_ENTRY_SIGNATURE \
+  )
+
+/**
+  Worker function to get CPU_FEATURES_DATA pointer.
+
+  @return Pointer to CPU_FEATURES_DATA.
+**/
+CPU_FEATURES_DATA *
+GetCpuFeaturesData (
+  VOID
+  );
+
+/**
+  Enlarges CPU register table for each processor.
+
+  @param[in, out]  RegisterTable   Pointer processor's CPU register table
+**/
+VOID
+EnlargeRegisterTable (
+  IN OUT CPU_REGISTER_TABLE            *RegisterTable
+  );
+
+/**
+  Allocates ACPI NVS memory to save ACPI_CPU_DATA.
+
+  @return  Pointer to allocated ACPI_CPU_DATA.
+**/
+ACPI_CPU_DATA *
+AllocateAcpiCpuData (
+  VOID
+  );
+
+/**
+  Worker function to return processor index.
+
+  @return  The processor index.
+**/
+UINTN
+GetProcessorIndex (
+  VOID
+  );
+
+/**
+  Gets detailed MP-related information on the requested processor at the
+  instant this call is made.
+
+  @param[in]  ProcessorNumber       The handle number of processor.
+  @param[out] ProcessorInfoBuffer   A pointer to the buffer where information for
+                                    the requested processor is deposited.
+
+  @return Status of MpServices->GetProcessorInfo().
+**/
+EFI_STATUS
+GetProcessorInformation (
+  IN  UINTN                            ProcessorNumber,
+  OUT EFI_PROCESSOR_INFORMATION        *ProcessorInfoBuffer
+  );
+
+/**
+  Worker function to execute a caller provided function on all enabled APs.
+
+  @param[in]  Procedure               A pointer to the function to be run on
+                                      enabled APs of the system.
+**/
+VOID
+StartupAPsWorker (
+  IN  EFI_AP_PROCEDURE                 Procedure
+  );
+
+/**
+  Worker function to retrieve the number of logical processor in the platform.
+
+  @param[out] NumberOfProcessors          Pointer to the total number of logical
+                                          processors in the system, including the BSP
+                                          and disabled APs.
+  @param[out] NumberOfEnabledProcessors   Pointer to the number of enabled logical
+                                          processors that exist in system, including
+                                          the BSP.
+**/
+VOID
+GetNumberOfProcessor (
+  OUT UINTN                            *NumberOfCpus,
+  OUT UINTN                            *NumberOfEnabledProcessors
+  );
+
+/**
+  Worker function to switch the requested AP to be the BSP from that point onward.
+
+  @param[in] ProcessorNumber   The handle number of AP that is to become the new BSP.
+**/
+VOID
+SwitchNewBsp (
+  IN  UINTN                            ProcessorNumber
+  );
+
+/**
+  Function that uses DEBUG() macros to display the contents of a a CPU feature bit mask.
+
+  @param[in]  FeatureMask  A pointer to the CPU feature bit mask.
+**/
+VOID
+DumpCpuFeatureMask (
+  IN UINT8               *FeatureMask
+  );
+
+/**
+  Dump CPU feature name or CPU feature bit mask.
+
+  @param[in]  CpuFeature   Pointer to CPU_FEATURES_ENTRY
+**/
+VOID
+DumpCpuFeature (
+  IN CPU_FEATURES_ENTRY  *CpuFeature
+  );
+
+#endif
diff --git a/UefiCpuPkg/Library/RegisterCpuFeaturesLib/RegisterCpuFeaturesDxe.uni b/UefiCpuPkg/Library/RegisterCpuFeaturesLib/RegisterCpuFeaturesDxe.uni
new file mode 100644
index 0000000000..b0c313b1f0
--- /dev/null
+++ b/UefiCpuPkg/Library/RegisterCpuFeaturesLib/RegisterCpuFeaturesDxe.uni
@@ -0,0 +1,22 @@
+// /** @file
+// CPU Register Table Library instance.
+//
+// CPU Register Table Library instance.
+//
+// Copyright (c) 2016, Intel Corporation. All rights reserved.<BR>
+//
+// This program and the accompanying materials
+// are licensed and made available under the terms and conditions of the BSD License
+// which accompanies this distribution.  The full text of the license may be found at
+// http://opensource.org/licenses/bsd-license.php
+//
+// THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+// WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+//
+// **/
+
+
+#string STR_MODULE_ABSTRACT             #language en-US "CPU Register Table Library instance"
+
+#string STR_MODULE_DESCRIPTION          #language en-US "CPU Register Table Library instance."
+
diff --git a/UefiCpuPkg/Library/RegisterCpuFeaturesLib/RegisterCpuFeaturesLib.c b/UefiCpuPkg/Library/RegisterCpuFeaturesLib/RegisterCpuFeaturesLib.c
new file mode 100644
index 0000000000..7a1470bd36
--- /dev/null
+++ b/UefiCpuPkg/Library/RegisterCpuFeaturesLib/RegisterCpuFeaturesLib.c
@@ -0,0 +1,770 @@
+/** @file
+  CPU Register Table Library functions.
+
+  Copyright (c) 2017, Intel Corporation. All rights reserved.<BR>
+  This program and the accompanying materials
+  are licensed and made available under the terms and conditions of the BSD License
+  which accompanies this distribution.  The full text of the license may be found at
+  http://opensource.org/licenses/bsd-license.php
+
+  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#include "RegisterCpuFeatures.h"
+
+/**
+  Checks if two CPU feature bit masks are equal.
+
+  @param[in]  FirstFeatureMask  The first input CPU feature bit mask
+  @param[in]  SecondFeatureMask The second input CPU feature bit mask
+
+  @retval TRUE  Two CPU feature bit masks are equal.
+  @retval FALSE Two CPU feature bit masks are not equal.
+**/
+BOOLEAN
+IsCpuFeatureMatch (
+  IN UINT8               *FirstFeatureMask,
+  IN UINT8               *SecondFeatureMask
+  )
+{
+  UINT32                 BitMaskSize;
+
+  BitMaskSize = PcdGetSize (PcdCpuFeaturesSupport);
+  if (CompareMem (FirstFeatureMask, SecondFeatureMask, BitMaskSize) == 0) {
+    return TRUE;
+  } else {
+    return FALSE;
+  }
+}
+
+/**
+  Function that uses DEBUG() macros to display the contents of a a CPU feature bit mask.
+
+  @param[in]  FeatureMask  A pointer to the CPU feature bit mask.
+**/
+VOID
+DumpCpuFeatureMask (
+  IN UINT8               *FeatureMask
+  )
+{
+  UINTN                  Index;
+  UINT8                  *Data8;
+  UINT32                 BitMaskSize;
+
+  BitMaskSize = PcdGetSize (PcdCpuFeaturesSupport);
+  Data8       = (UINT8 *) FeatureMask;
+  for (Index = 0; Index < BitMaskSize; Index++) {
+    DEBUG ((DEBUG_INFO, " %02x ", *Data8++));
+  }
+  DEBUG ((DEBUG_INFO, "\n"));
+}
+
+/**
+  Dump CPU feature name or CPU feature bit mask.
+
+  @param[in]  CpuFeature   Pointer to CPU_FEATURES_ENTRY
+**/
+VOID
+DumpCpuFeature (
+  IN CPU_FEATURES_ENTRY  *CpuFeature
+  )
+{
+
+  if (CpuFeature->FeatureName != NULL) {
+    DEBUG ((DEBUG_INFO, "FeatureName: %a\n", CpuFeature->FeatureName));
+  } else {
+    DEBUG ((DEBUG_INFO, "FeatureMask = "));
+    DumpCpuFeatureMask (CpuFeature->FeatureMask);
+  }
+}
+
+/**
+  Determines if the feature bit mask is in dependent CPU feature bit mask buffer.
+
+  @param[in]  FeatureMask        Pointer to CPU feature bit mask
+  @param[in]  DependentBitMask   Pointer to dependent CPU feature bit mask buffer
+
+  @retval TRUE  The feature bit mask is in dependent CPU feature bit mask buffer.
+  @retval FALSE The feature bit mask is not in dependent CPU feature bit mask buffer.
+**/
+BOOLEAN
+IsBitMaskMatchCheck (
+  IN UINT8        *FeatureMask,
+  IN UINT8        *DependentBitMask
+  )
+{
+  UINTN      Index;
+  UINTN      BitMaskSize;
+  UINT8      *Data1;
+  UINT8      *Data2;
+
+  BitMaskSize = PcdGetSize (PcdCpuFeaturesSupport);
+
+  Data1 = FeatureMask;
+  Data2 = DependentBitMask;
+  for (Index = 0; Index < BitMaskSize; Index++) {
+    if (((*(Data1++)) & (*(Data2++))) != 0) {
+      return TRUE;
+    }
+  }
+  return FALSE;
+}
+
+/**
+  Checks and adjusts CPU features order per dependency relationship.
+
+  @param[in]  FeatureList        Pointer to CPU feature list
+**/
+VOID
+CheckCpuFeaturesDependency (
+  IN LIST_ENTRY              *FeatureList
+  )
+{
+  LIST_ENTRY                 *CurrentEntry;
+  CPU_FEATURES_ENTRY         *CpuFeature;
+  LIST_ENTRY                 *CheckEntry;
+  CPU_FEATURES_ENTRY         *CheckFeature;
+  BOOLEAN                    Swapped;
+  LIST_ENTRY                 *TempEntry;
+
+  CurrentEntry = GetFirstNode (FeatureList);
+  while (!IsNull (FeatureList, CurrentEntry)) {
+    Swapped = FALSE;
+    CpuFeature = CPU_FEATURE_ENTRY_FROM_LINK (CurrentEntry);
+    if (CpuFeature->BeforeAll) {
+      //
+      // Check all features dispatched before this entry
+      //
+      CheckEntry = GetFirstNode (FeatureList);
+      while (CheckEntry != CurrentEntry) {
+        CheckFeature = CPU_FEATURE_ENTRY_FROM_LINK (CheckEntry);
+        if (!CheckFeature->BeforeAll) {
+          //
+          // If this feature has no BeforeAll flag and is dispatched before CpuFeature,
+          // insert currentEntry before Checked feature
+          //
+          RemoveEntryList (CurrentEntry);
+          InsertTailList (CheckEntry, CurrentEntry);
+          Swapped = TRUE;
+          break;
+        }
+        CheckEntry = CheckEntry->ForwardLink;
+      }
+      if (Swapped) {
+        continue;
+      }
+    }
+
+    if (CpuFeature->AfterAll) {
+      //
+      // Check all features dispatched after this entry
+      //
+      CheckEntry = GetNextNode (FeatureList, CurrentEntry);
+      while (!IsNull (FeatureList, CheckEntry)) {
+        CheckFeature = CPU_FEATURE_ENTRY_FROM_LINK (CheckEntry);
+        if (!CheckFeature->AfterAll) {
+          //
+          // If this feature has no AfterAll flag and is dispatched after CpuFeature,
+          // insert currentEntry after Checked feature
+          //
+          TempEntry = GetNextNode (FeatureList, CurrentEntry);
+          RemoveEntryList (CurrentEntry);
+          InsertHeadList (CheckEntry, CurrentEntry);
+          CurrentEntry = TempEntry;
+          Swapped = TRUE;
+          break;
+        }
+        CheckEntry = CheckEntry->ForwardLink;
+      }
+      if (Swapped) {
+        continue;
+      }
+    }
+
+    if (CpuFeature->BeforeFeatureBitMask != NULL) {
+      //
+      // Check all features dispatched before this entry
+      //
+      CheckEntry = GetFirstNode (FeatureList);
+      while (CheckEntry != CurrentEntry) {
+        CheckFeature = CPU_FEATURE_ENTRY_FROM_LINK (CheckEntry);
+        if (IsBitMaskMatchCheck (CheckFeature->FeatureMask, CpuFeature->BeforeFeatureBitMask)) {
+          //
+          // If there is dependency, swap them
+          //
+          RemoveEntryList (CurrentEntry);
+          InsertTailList (CheckEntry, CurrentEntry);
+          Swapped = TRUE;
+          break;
+        }
+        CheckEntry = CheckEntry->ForwardLink;
+      }
+      if (Swapped) {
+        continue;
+      }
+    }
+
+    if (CpuFeature->AfterFeatureBitMask != NULL) {
+      //
+      // Check all features dispatched after this entry
+      //
+      CheckEntry = GetNextNode (FeatureList, CurrentEntry);
+      while (!IsNull (FeatureList, CheckEntry)) {
+        CheckFeature = CPU_FEATURE_ENTRY_FROM_LINK (CheckEntry);
+        if (IsBitMaskMatchCheck (CheckFeature->FeatureMask, CpuFeature->AfterFeatureBitMask)) {
+          //
+          // If there is dependency, swap them
+          //
+          TempEntry = GetNextNode (FeatureList, CurrentEntry);
+          RemoveEntryList (CurrentEntry);
+          InsertHeadList (CheckEntry, CurrentEntry);
+          CurrentEntry = TempEntry;
+          Swapped = TRUE;
+          break;
+        }
+        CheckEntry = CheckEntry->ForwardLink;
+      }
+      if (Swapped) {
+        continue;
+      }
+    }
+    //
+    // No swap happened, check the next feature
+    //
+    CurrentEntry = CurrentEntry->ForwardLink;
+  }
+}
+
+/**
+  Worker function to register CPU Feature.
+
+  @param[in]  CpuFeature                Pointer to CPU feature entry
+
+  @retval  RETURN_SUCCESS           The CPU feature was successfully registered.
+  @retval  RETURN_OUT_OF_RESOURCES  There are not enough resources to register
+                                    the CPU feature.
+  @retval  RETURN_UNSUPPORTED       Registration of the CPU feature is not
+                                    supported due to a circular dependency between
+                                    BEFORE and AFTER features.
+**/
+RETURN_STATUS
+RegisterCpuFeatureWorker (
+  IN CPU_FEATURES_ENTRY      *CpuFeature
+  )
+{
+  EFI_STATUS                 Status;
+  CPU_FEATURES_DATA          *CpuFeaturesData;
+  CPU_FEATURES_ENTRY         *CpuFeatureEntry;
+  LIST_ENTRY                 *Entry;
+  UINT32                     BitMaskSize;
+  BOOLEAN                    FeatureExist;
+
+  BitMaskSize     = PcdGetSize (PcdCpuFeaturesSupport);
+  CpuFeaturesData = GetCpuFeaturesData ();
+  if (CpuFeaturesData->FeaturesCount == 0) {
+    InitializeListHead (&CpuFeaturesData->FeatureList);
+    InitializeSpinLock (&CpuFeaturesData->MsrLock);
+    InitializeSpinLock (&CpuFeaturesData->MemoryMappedLock);
+    CpuFeaturesData->BitMaskSize = BitMaskSize;
+  }
+  ASSERT (CpuFeaturesData->BitMaskSize == BitMaskSize);
+
+  FeatureExist = FALSE;
+  Entry = GetFirstNode (&CpuFeaturesData->FeatureList);
+  while (!IsNull (&CpuFeaturesData->FeatureList, Entry)) {
+    CpuFeatureEntry = CPU_FEATURE_ENTRY_FROM_LINK (Entry);
+    if (IsCpuFeatureMatch (CpuFeature->FeatureMask, CpuFeatureEntry->FeatureMask)) {
+      //
+      // If this feature already registered
+      //
+      FeatureExist = TRUE;
+      break;
+    }
+    Entry = Entry->ForwardLink;
+  }
+
+  if (!FeatureExist) {
+    DEBUG ((DEBUG_INFO, "[NEW] "));
+    DumpCpuFeature (CpuFeature);
+    InsertTailList (&CpuFeaturesData->FeatureList, &CpuFeature->Link);
+    CpuFeaturesData->FeaturesCount++;
+  } else {
+    DEBUG ((DEBUG_INFO, "[OVERRIDE] "));
+    DumpCpuFeature (CpuFeature);
+    //
+    // Overwrite original parameters of CPU feature
+    //
+    if (CpuFeature->GetConfigDataFunc != NULL) {
+      CpuFeatureEntry->GetConfigDataFunc = CpuFeature->GetConfigDataFunc;
+    }
+    if (CpuFeature->SupportFunc != NULL) {
+      CpuFeatureEntry->SupportFunc = CpuFeature->SupportFunc;
+    }
+    if (CpuFeature->InitializeFunc != NULL) {
+      CpuFeatureEntry->InitializeFunc = CpuFeature->InitializeFunc;
+    }
+    if (CpuFeature->FeatureName != NULL) {
+      if (CpuFeatureEntry->FeatureName == NULL) {
+        CpuFeatureEntry->FeatureName = AllocatePool (CPU_FEATURE_NAME_SIZE);
+        ASSERT (CpuFeatureEntry->FeatureName != NULL);
+      }
+      Status = AsciiStrCpyS (CpuFeatureEntry->FeatureName, CPU_FEATURE_NAME_SIZE, CpuFeature->FeatureName);
+      ASSERT_EFI_ERROR (Status);
+      FreePool (CpuFeature->FeatureName);
+    }
+    if (CpuFeature->BeforeFeatureBitMask != NULL) {
+      if (CpuFeatureEntry->BeforeFeatureBitMask != NULL) {
+        FreePool (CpuFeatureEntry->BeforeFeatureBitMask);
+      }
+      CpuFeatureEntry->BeforeFeatureBitMask = CpuFeature->BeforeFeatureBitMask;
+    }
+    if (CpuFeature->AfterFeatureBitMask != NULL) {
+      if (CpuFeatureEntry->AfterFeatureBitMask != NULL) {
+        FreePool (CpuFeatureEntry->AfterFeatureBitMask);
+      }
+      CpuFeatureEntry->AfterFeatureBitMask = CpuFeature->AfterFeatureBitMask;
+    }
+    CpuFeatureEntry->BeforeAll = CpuFeature->BeforeAll;
+    CpuFeatureEntry->AfterAll  = CpuFeature->AfterAll;
+
+    FreePool (CpuFeature->FeatureMask);
+    FreePool (CpuFeature);
+  }
+  //
+  // Verify CPU features dependency can change CPU feature order
+  //
+  CheckCpuFeaturesDependency (&CpuFeaturesData->FeatureList);
+  return RETURN_SUCCESS;
+}
+
+/**
+  Sets CPU feature bit mask in CPU feature bit mask buffer.
+
+  @param[in]  FeaturesBitMask       Pointer to CPU feature bit mask buffer
+  @param[in]  Feature               The bit number of the CPU feature
+  @param[in]  BitMaskSize           CPU feature bit mask buffer size
+**/
+VOID
+SetCpuFeaturesBitMask (
+  IN UINT8               **FeaturesBitMask,
+  IN UINT32              Feature,
+  IN UINTN               BitMaskSize
+  )
+{
+  UINT8                  *CpuFeaturesBitMask;
+
+  ASSERT (FeaturesBitMask != NULL);
+  CpuFeaturesBitMask = *FeaturesBitMask;
+  if (CpuFeaturesBitMask == NULL) {
+    CpuFeaturesBitMask = AllocateZeroPool (BitMaskSize);
+    ASSERT (CpuFeaturesBitMask != NULL);
+    *FeaturesBitMask = CpuFeaturesBitMask;
+  }
+
+  CpuFeaturesBitMask  += (Feature / 8);
+  *CpuFeaturesBitMask |= (UINT8) (1 << (Feature % 8));
+}
+
+/**
+  Registers a CPU Feature.
+
+  @param  GetConfigDataFunc  CPU feature get configuration data function.  This
+                             is an optional parameter that may be NULL.  If NULL,
+                             then the most recently registered function for the
+                             CPU feature is used.  If no functions are registered
+                             for a CPU feature, then the CPU configuration data
+                             for the registered feature is NULL.
+  @param  SupportFunc        CPU feature support function.  This is an optional
+                             parameter that may be NULL.  If NULL, then the most
+                             recently registered function for the CPU feature is
+                             used. If no functions are registered for a CPU
+                             feature, then the CPU feature is assumed to be
+                             supported by all CPUs.
+  @param  InitializeFunc     CPU feature initialize function.  This is an optional
+                             parameter that may be NULL.  If NULL, then the most
+                             recently registered function for the CPU feature is
+                             used. If no functions are registered for a CPU
+                             feature, then the CPU feature initialization is
+                             skipped.
+  @param  ...                Variable argument list of UINT32 CPU feature value.
+                             Values with no modifiers are the features provided
+                             by the registered functions.
+                             Values with CPU_FEATURE_BEFORE modifier are features
+                             that must be initialized after the features provided
+                             by the registered functions are used.
+                             Values with CPU_FEATURE_AFTER modifier are features
+                             that must be initialized before the features provided
+                             by the registered functions are used.
+                             The last argument in this variable argument list must
+                             always be CPU_FEATURE_END.
+
+  @retval  RETURN_SUCCESS           The CPU feature was successfully registered.
+  @retval  RETURN_OUT_OF_RESOURCES  There are not enough resources to register
+                                    the CPU feature.
+  @retval  RETURN_UNSUPPORTED       Registration of the CPU feature is not
+                                    supported due to a circular dependency between
+                                    BEFORE and AFTER features.
+
+  @note This service could be called by BSP only.
+**/
+RETURN_STATUS
+EFIAPI
+RegisterCpuFeature (
+  IN CHAR8                             *FeatureName,       OPTIONAL
+  IN CPU_FEATURE_GET_CONFIG_DATA       GetConfigDataFunc,  OPTIONAL
+  IN CPU_FEATURE_SUPPORT               SupportFunc,        OPTIONAL
+  IN CPU_FEATURE_INITIALIZE            InitializeFunc,     OPTIONAL
+  ...
+  )
+{
+  EFI_STATUS                 Status;
+  VA_LIST                    Marker;
+  UINT32                     Feature;
+  UINTN                      BitMaskSize;
+  CPU_FEATURES_ENTRY         *CpuFeature;
+  UINT8                      *FeatureMask;
+  UINT8                      *BeforeFeatureBitMask;
+  UINT8                      *AfterFeatureBitMask;
+  BOOLEAN                    BeforeAll;
+  BOOLEAN                    AfterAll;
+
+  FeatureMask          = NULL;
+  BeforeFeatureBitMask = NULL;
+  AfterFeatureBitMask  = NULL;
+  BeforeAll            = FALSE;
+  AfterAll             = FALSE;
+
+  BitMaskSize = PcdGetSize (PcdCpuFeaturesSupport);
+
+  VA_START (Marker, InitializeFunc);
+  Feature = VA_ARG (Marker, UINT32);
+  while (Feature != CPU_FEATURE_END) {
+    ASSERT ((Feature & (CPU_FEATURE_BEFORE | CPU_FEATURE_AFTER))
+                    != (CPU_FEATURE_BEFORE | CPU_FEATURE_AFTER));
+    ASSERT ((Feature & (CPU_FEATURE_BEFORE_ALL | CPU_FEATURE_AFTER_ALL))
+                    != (CPU_FEATURE_BEFORE_ALL | CPU_FEATURE_AFTER_ALL));
+    if (Feature < CPU_FEATURE_BEFORE) {
+      BeforeAll = ((Feature & CPU_FEATURE_BEFORE_ALL) != 0) ? TRUE : FALSE;
+      AfterAll  = ((Feature & CPU_FEATURE_AFTER_ALL) != 0) ? TRUE : FALSE;
+      Feature  &= ~(CPU_FEATURE_BEFORE_ALL | CPU_FEATURE_AFTER_ALL);
+      ASSERT (FeatureMask == NULL);
+      SetCpuFeaturesBitMask (&FeatureMask, Feature, BitMaskSize);
+    } else if ((Feature & CPU_FEATURE_BEFORE) != 0) {
+      SetCpuFeaturesBitMask (&BeforeFeatureBitMask, Feature & ~CPU_FEATURE_BEFORE, BitMaskSize);
+    } else if ((Feature & CPU_FEATURE_AFTER) != 0) {
+      SetCpuFeaturesBitMask (&AfterFeatureBitMask, Feature & ~CPU_FEATURE_AFTER, BitMaskSize);
+    }
+    Feature = VA_ARG (Marker, UINT32);
+  }
+  VA_END (Marker);
+
+  CpuFeature = AllocateZeroPool (sizeof (CPU_FEATURES_ENTRY));
+  ASSERT (CpuFeature != NULL);
+  CpuFeature->Signature            = CPU_FEATURE_ENTRY_SIGNATURE;
+  CpuFeature->FeatureMask          = FeatureMask;
+  CpuFeature->BeforeFeatureBitMask = BeforeFeatureBitMask;
+  CpuFeature->AfterFeatureBitMask  = AfterFeatureBitMask;
+  CpuFeature->BeforeAll            = BeforeAll;
+  CpuFeature->AfterAll             = AfterAll;
+  CpuFeature->GetConfigDataFunc    = GetConfigDataFunc;
+  CpuFeature->SupportFunc          = SupportFunc;
+  CpuFeature->InitializeFunc       = InitializeFunc;
+  if (FeatureName != NULL) {
+    CpuFeature->FeatureName          = AllocatePool (CPU_FEATURE_NAME_SIZE);
+    ASSERT (CpuFeature->FeatureName != NULL);
+    Status = AsciiStrCpyS (CpuFeature->FeatureName, CPU_FEATURE_NAME_SIZE, FeatureName);
+    ASSERT_EFI_ERROR (Status);
+  }
+
+  Status = RegisterCpuFeatureWorker (CpuFeature);
+  ASSERT_EFI_ERROR (Status);
+
+  return RETURN_SUCCESS;
+}
+
+/**
+  Add an entry in specified register table.
+
+  This function adds an entry in specified register table, with given register type,
+  register index, bit section and value.
+
+  @param[in]  PreSmmFlag       If TRUE, entry will be added into PreSmm register table
+                               If FALSE, entry will be added into register table
+  @param[in]  ProcessorNumber  The index of the CPU to add a register table entry
+  @param[in]  RegisterType     Type of the register to program
+  @param[in]  Index            Index of the register to program
+  @param[in]  ValidBitStart    Start of the bit section
+  @param[in]  ValidBitLength   Length of the bit section
+  @param[in]  Value            Value to write
+**/
+VOID
+CpuRegisterTableWriteWorker (
+  IN BOOLEAN                 PreSmmFlag,
+  IN UINTN                   ProcessorNumber,
+  IN REGISTER_TYPE           RegisterType,
+  IN UINT32                  Index,
+  IN UINT8                   ValidBitStart,
+  IN UINT8                   ValidBitLength,
+  IN UINT64                  Value
+  )
+{
+  EFI_STATUS               Status;
+  CPU_FEATURES_DATA        *CpuFeaturesData;
+  ACPI_CPU_DATA            *AcpiCpuData;
+  CPU_REGISTER_TABLE       *RegisterTable;
+  CPU_REGISTER_TABLE_ENTRY *RegisterTableEntry;
+
+  CpuFeaturesData = GetCpuFeaturesData ();
+  if (CpuFeaturesData->RegisterTable == NULL) {
+    AcpiCpuData = (ACPI_CPU_DATA *) (UINTN) PcdGet64 (PcdCpuS3DataAddress);
+    if (AcpiCpuData == NULL) {
+      AcpiCpuData = AllocateAcpiCpuData ();
+      ASSERT (AcpiCpuData != NULL);
+      //
+      // Set PcdCpuS3DataAddress to the base address of the ACPI_CPU_DATA structure
+      //
+      Status = PcdSet64S (PcdCpuS3DataAddress, (UINT64)(UINTN)AcpiCpuData);
+      ASSERT_EFI_ERROR (Status);
+    }
+    ASSERT (AcpiCpuData->RegisterTable != 0);
+    CpuFeaturesData->RegisterTable = (CPU_REGISTER_TABLE *) (UINTN) AcpiCpuData->RegisterTable;
+    CpuFeaturesData->PreSmmRegisterTable = (CPU_REGISTER_TABLE *) (UINTN) AcpiCpuData->PreSmmInitRegisterTable;
+  }
+
+  if (PreSmmFlag) {
+    RegisterTable = &CpuFeaturesData->PreSmmRegisterTable[ProcessorNumber];
+  } else {
+    RegisterTable = &CpuFeaturesData->RegisterTable[ProcessorNumber];
+  }
+
+  if (RegisterTable->TableLength == RegisterTable->AllocatedSize / sizeof (CPU_REGISTER_TABLE_ENTRY)) {
+    EnlargeRegisterTable (RegisterTable);
+  }
+
+  //
+  // Append entry in the register table.
+  //
+  RegisterTableEntry = (CPU_REGISTER_TABLE_ENTRY *) (UINTN) RegisterTable->RegisterTableEntry;
+  RegisterTableEntry[RegisterTable->TableLength].RegisterType   = RegisterType;
+  RegisterTableEntry[RegisterTable->TableLength].Index          = Index;
+  RegisterTableEntry[RegisterTable->TableLength].ValidBitStart  = ValidBitStart;
+  RegisterTableEntry[RegisterTable->TableLength].ValidBitLength = ValidBitLength;
+  RegisterTableEntry[RegisterTable->TableLength].Value          = Value;
+
+  RegisterTable->TableLength++;
+}
+
+/**
+  Adds an entry in specified register table.
+
+  This function adds an entry in specified register table, with given register type,
+  register index, bit section and value.
+
+  @param[in]  ProcessorNumber  The index of the CPU to add a register table entry
+  @param[in]  RegisterType     Type of the register to program
+  @param[in]  Index            Index of the register to program
+  @param[in]  ValueMask        Mask of bits in register to write
+  @param[in]  Value            Value to write
+
+  @note This service could be called by BSP only.
+**/
+VOID
+EFIAPI
+CpuRegisterTableWrite (
+  IN UINTN               ProcessorNumber,
+  IN REGISTER_TYPE       RegisterType,
+  IN UINT32              Index,
+  IN UINT64              ValueMask,
+  IN UINT64              Value
+  )
+{
+  UINT8                   Start;
+  UINT8                   End;
+  UINT8                   Length;
+
+  Start  = (UINT8)LowBitSet64  (ValueMask);
+  End    = (UINT8)HighBitSet64 (ValueMask);
+  Length = End - Start + 1;
+  CpuRegisterTableWriteWorker (FALSE, ProcessorNumber, RegisterType, Index, Start, Length, Value);
+}
+
+/**
+  Adds an entry in specified Pre-SMM register table.
+
+  This function adds an entry in specified register table, with given register type,
+  register index, bit section and value.
+
+  @param[in]  ProcessorNumber  The index of the CPU to add a register table entry.
+  @param[in]  RegisterType     Type of the register to program
+  @param[in]  Index            Index of the register to program
+  @param[in]  ValueMask        Mask of bits in register to write
+  @param[in]  Value            Value to write
+
+  @note This service could be called by BSP only.
+**/
+VOID
+EFIAPI
+PreSmmCpuRegisterTableWrite (
+  IN UINTN               ProcessorNumber,
+  IN REGISTER_TYPE       RegisterType,
+  IN UINT32              Index,
+  IN UINT64              ValueMask,
+  IN UINT64              Value
+  )
+{
+  UINT8  Start;
+  UINT8  End;
+  UINT8  Length;
+
+  Start  = (UINT8)LowBitSet64  (ValueMask);
+  End    = (UINT8)HighBitSet64 (ValueMask);
+  Length = End - Start + 1;
+  CpuRegisterTableWriteWorker (TRUE, ProcessorNumber, RegisterType, Index, Start, Length, Value);
+}
+
+/**
+  Worker function to determine if a CPU feature is set in input CPU feature bit mask buffer.
+
+  @param[in]  CpuBitMask      CPU feature bit mask buffer
+  @param[in]  CpuBitMaskSize  The size of CPU feature bit mask buffer
+  @param[in]  Feature         The bit number of the CPU feature
+
+  @retval  TRUE   The CPU feature is set in PcdCpuFeaturesSupport.
+  @retval  FALSE  The CPU feature is not set in PcdCpuFeaturesSupport.
+
+**/
+BOOLEAN
+IsCpuFeatureSetInCpuPcd (
+  IN UINT8               *CpuBitMask,
+  IN UINTN               CpuBitMaskSize,
+  IN UINT32              Feature
+  )
+{
+  if ((Feature >> 3) >= CpuBitMaskSize) {
+    return FALSE;
+  }
+  return ((*(CpuBitMask + (Feature >> 3)) & (1 << (Feature & 0x07))) != 0);
+}
+
+/**
+  Determines if a CPU feature is enabled in PcdCpuFeaturesSupport bit mask.
+  If a CPU feature is disabled in PcdCpuFeaturesSupport then all the code/data
+  associated with that feature should be optimized away if compiler
+  optimizations are enabled.
+
+  @param[in]  Feature  The bit number of the CPU feature to check in the PCD
+                       PcdCpuFeaturesSupport
+
+  @retval  TRUE   The CPU feature is set in PcdCpuFeaturesSupport.
+  @retval  FALSE  The CPU feature is not set in PcdCpuFeaturesSupport.
+
+  @note This service could be called by BSP only.
+**/
+BOOLEAN
+EFIAPI
+IsCpuFeatureSupported (
+  IN UINT32              Feature
+  )
+{
+  return IsCpuFeatureSetInCpuPcd (
+           (UINT8 *)PcdGetPtr (PcdCpuFeaturesSupport),
+           PcdGetSize (PcdCpuFeaturesSupport),
+           Feature
+           );
+}
+
+/**
+  Determines if a CPU feature is set in PcdCpuFeaturesSetting bit mask.
+
+  @param[in]  Feature  The bit number of the CPU feature to check in the PCD
+                       PcdCpuFeaturesSetting
+
+  @retval  TRUE   The CPU feature is set in PcdCpuFeaturesSetting.
+  @retval  FALSE  The CPU feature is not set in PcdCpuFeaturesSetting.
+
+  @note This service could be called by BSP only.
+**/
+BOOLEAN
+EFIAPI
+IsCpuFeatureInSetting (
+  IN UINT32              Feature
+  )
+{
+  return IsCpuFeatureSetInCpuPcd (
+           (UINT8 *)PcdGetPtr (PcdCpuFeaturesSetting),
+           PcdGetSize (PcdCpuFeaturesSetting),
+           Feature
+           );
+}
+
+/**
+  Determines if a CPU feature is set in PcdCpuFeaturesCapability bit mask.
+
+  @param[in]  Feature  The bit number of the CPU feature to check in the PCD
+                       PcdCpuFeaturesCapability
+
+  @retval  TRUE   The CPU feature is set in PcdCpuFeaturesCapability.
+  @retval  FALSE  The CPU feature is not set in PcdCpuFeaturesCapability.
+
+  @note This service could be called by BSP only.
+**/
+BOOLEAN
+EFIAPI
+IsCpuFeatureCapability (
+  IN UINT32              Feature
+  )
+{
+  return IsCpuFeatureSetInCpuPcd (
+           (UINT8 *)PcdGetPtr (PcdCpuFeaturesCapability),
+           PcdGetSize (PcdCpuFeaturesCapability),
+           Feature
+           );
+
+}
+
+/**
+  Determines if a CPU feature is set in PcdCpuFeaturesUserConfiguration bit mask.
+
+  @param[in]  Feature  The bit number of the CPU feature to check in the PCD
+                       PcdCpuFeaturesUserConfiguration
+
+  @retval  TRUE   The CPU feature is set in PcdCpuFeaturesUserConfiguration.
+  @retval  FALSE  The CPU feature is not set in PcdCpuFeaturesUserConfiguration.
+
+  @note This service could be called by BSP only.
+**/
+BOOLEAN
+EFIAPI
+IsCpuFeatureUserConfiguration (
+  IN UINT32              Feature
+  )
+{
+  return IsCpuFeatureSetInCpuPcd (
+           (UINT8 *)PcdGetPtr (PcdCpuFeaturesUserConfiguration),
+           PcdGetSize (PcdCpuFeaturesUserConfiguration),
+           Feature
+           );
+
+}
+
+/**
+  Switches to assigned BSP after CPU features initialization.
+
+  @param[in]  ProcessorNumber  The index of the CPU executing this function.
+
+  @note This service could be called by BSP only.
+**/
+VOID
+EFIAPI
+SwitchBspAfterFeaturesInitialize (
+  IN UINTN               ProcessorNumber
+  )
+{
+  CPU_FEATURES_DATA      *CpuFeaturesData;
+
+  CpuFeaturesData = GetCpuFeaturesData ();
+  CpuFeaturesData->BspNumber = ProcessorNumber;
+}
+
diff --git a/UefiCpuPkg/UefiCpuPkg.dsc b/UefiCpuPkg/UefiCpuPkg.dsc
index 3922f2ddc8..59a702f80d 100644
--- a/UefiCpuPkg/UefiCpuPkg.dsc
+++ b/UefiCpuPkg/UefiCpuPkg.dsc
@@ -1,7 +1,7 @@
 ## @file

 #  UefiCpuPkg Package

 #

-#  Copyright (c) 2007 - 2016, Intel Corporation. All rights reserved.<BR>

+#  Copyright (c) 2007 - 2017, Intel Corporation. All rights reserved.<BR>

 #

 #  This program and the accompanying materials

 #  are licensed and made available under the terms and conditions of the BSD License

@@ -76,6 +76,7 @@
   HobLib|MdePkg/Library/PeiHobLib/PeiHobLib.inf

   LockBoxLib|MdeModulePkg/Library/SmmLockBoxLib/SmmLockBoxPeiLib.inf

   MpInitLib|UefiCpuPkg/Library/MpInitLib/PeiMpInitLib.inf

+  RegisterCpuFeaturesLib|UefiCpuPkg/Library/RegisterCpuFeaturesLib/PeiRegisterCpuFeaturesLib.inf

 

 [LibraryClasses.IA32.PEIM, LibraryClasses.X64.PEIM]

   PeiServicesTablePointerLib|MdePkg/Library/PeiServicesTablePointerLibIdt/PeiServicesTablePointerLibIdt.inf

@@ -89,6 +90,7 @@
   HobLib|MdePkg/Library/DxeHobLib/DxeHobLib.inf

   CpuExceptionHandlerLib|UefiCpuPkg/Library/CpuExceptionHandlerLib/DxeCpuExceptionHandlerLib.inf

   MpInitLib|UefiCpuPkg/Library/MpInitLib/DxeMpInitLib.inf

+  RegisterCpuFeaturesLib|UefiCpuPkg/Library/RegisterCpuFeaturesLib/DxeRegisterCpuFeaturesLib.inf

 

 [LibraryClasses.common.DXE_SMM_DRIVER]

   SmmServicesTableLib|MdePkg/Library/SmmServicesTableLib/SmmServicesTableLib.inf

@@ -126,6 +128,8 @@
   UefiCpuPkg/Library/MpInitLib/DxeMpInitLib.inf

   UefiCpuPkg/Library/MtrrLib/MtrrLib.inf

   UefiCpuPkg/Library/PlatformSecLibNull/PlatformSecLibNull.inf

+  UefiCpuPkg/Library/RegisterCpuFeaturesLib/PeiRegisterCpuFeaturesLib.inf

+  UefiCpuPkg/Library/RegisterCpuFeaturesLib/DxeRegisterCpuFeaturesLib.inf

   UefiCpuPkg/Library/SmmCpuPlatformHookLibNull/SmmCpuPlatformHookLibNull.inf

   UefiCpuPkg/Library/SmmCpuFeaturesLib/SmmCpuFeaturesLib.inf

   UefiCpuPkg/Library/SmmCpuFeaturesLib/SmmCpuFeaturesLibStm.inf