8 files changed, 2523 insertions, 2523 deletions
diff --git a/UefiCpuPkg/Library/RegisterCpuFeaturesLib/CpuFeaturesInitialize.c b/UefiCpuPkg/Library/RegisterCpuFeaturesLib/CpuFeaturesInitialize.c
index e0fe38c935..5e11b2b21c 100644
--- a/UefiCpuPkg/Library/RegisterCpuFeaturesLib/CpuFeaturesInitialize.c
+++ b/UefiCpuPkg/Library/RegisterCpuFeaturesLib/CpuFeaturesInitialize.c
@@ -1,751 +1,751 @@
-/** @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);
-  ASSERT (SupportBitMask != NULL);
-  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);
-  ASSERT (SupportBitMask != NULL);
-  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);
-    ASSERT (InitOrder->FeaturesSupportedMask != NULL);
-    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: %lx, Bit Start: %d, Bit Length: %d, Value: %lx\r\n",
-        ProcessorNumber,
-        RegisterTableEntry->Index | LShiftU64 (RegisterTableEntry->HighIndex, 32),
-        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);
-  ASSERT (CpuFeaturesData->CapabilityPcds != NULL);
-  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);
-  ASSERT (CpuFeaturesData->SettingPcds != NULL);
-  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);
-        ASSERT (CpuFeatureInOrder != NULL);
-        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 (
-        (UINTN)(RegisterTableEntry->Index | LShiftU64 (RegisterTableEntry->HighIndex, 32)),
-        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);
-  }
-}
+/** @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);
+  ASSERT (SupportBitMask != NULL);
+  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);
+  ASSERT (SupportBitMask != NULL);
+  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);
+    ASSERT (InitOrder->FeaturesSupportedMask != NULL);
+    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: %lx, Bit Start: %d, Bit Length: %d, Value: %lx\r\n",
+        ProcessorNumber,
+        RegisterTableEntry->Index | LShiftU64 (RegisterTableEntry->HighIndex, 32),
+        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);
+  ASSERT (CpuFeaturesData->CapabilityPcds != NULL);
+  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);
+  ASSERT (CpuFeaturesData->SettingPcds != NULL);
+  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);
+        ASSERT (CpuFeatureInOrder != NULL);
+        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 (
+        (UINTN)(RegisterTableEntry->Index | LShiftU64 (RegisterTableEntry->HighIndex, 32)),
+        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
index 74c658a16b..902a339529 100644
--- a/UefiCpuPkg/Library/RegisterCpuFeaturesLib/DxeRegisterCpuFeaturesLib.c
+++ b/UefiCpuPkg/Library/RegisterCpuFeaturesLib/DxeRegisterCpuFeaturesLib.c
@@ -1,266 +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};
-EFI_MP_SERVICES_PROTOCOL   *mCpuFeaturesMpServices = 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 (mCpuFeaturesMpServices == NULL) {
-    //
-    // Get MP Services Protocol
-    //
-    Status = gBS->LocateProtocol (
-                  &gEfiMpServiceProtocolGuid,
-                  NULL,
-                  (VOID **)&mCpuFeaturesMpServices
-                  );
-    ASSERT_EFI_ERROR (Status);
-  }
-
-  ASSERT (mCpuFeaturesMpServices != NULL);
-  return mCpuFeaturesMpServices;
-}
-
-/**
-  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] NumberOfCpus                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;
-}
+/** @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};
+EFI_MP_SERVICES_PROTOCOL   *mCpuFeaturesMpServices = 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 (mCpuFeaturesMpServices == NULL) {
+    //
+    // Get MP Services Protocol
+    //
+    Status = gBS->LocateProtocol (
+                  &gEfiMpServiceProtocolGuid,
+                  NULL,
+                  (VOID **)&mCpuFeaturesMpServices
+                  );
+    ASSERT_EFI_ERROR (Status);
+  }
+
+  ASSERT (mCpuFeaturesMpServices != NULL);
+  return mCpuFeaturesMpServices;
+}
+
+/**
+  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] NumberOfCpus                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
index abffd11194..62ac8a9635 100644
--- a/UefiCpuPkg/Library/RegisterCpuFeaturesLib/DxeRegisterCpuFeaturesLib.inf
+++ b/UefiCpuPkg/Library/RegisterCpuFeaturesLib/DxeRegisterCpuFeaturesLib.inf
@@ -1,62 +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
+## @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
index 8fde742dce..6804eddf65 100644
--- a/UefiCpuPkg/Library/RegisterCpuFeaturesLib/PeiRegisterCpuFeaturesLib.c
+++ b/UefiCpuPkg/Library/RegisterCpuFeaturesLib/PeiRegisterCpuFeaturesLib.c
@@ -1,390 +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] NumberOfCpus                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;
-}
+/** @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] NumberOfCpus                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
index 00907145a9..5e9ab2c304 100644
--- a/UefiCpuPkg/Library/RegisterCpuFeaturesLib/PeiRegisterCpuFeaturesLib.inf
+++ b/UefiCpuPkg/Library/RegisterCpuFeaturesLib/PeiRegisterCpuFeaturesLib.inf
@@ -1,64 +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
+## @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
index 11f8eb794d..7731f885b3 100644
--- a/UefiCpuPkg/Library/RegisterCpuFeaturesLib/RegisterCpuFeatures.h
+++ b/UefiCpuPkg/Library/RegisterCpuFeaturesLib/RegisterCpuFeatures.h
@@ -1,193 +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] NumberOfCpus                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
+/** @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] NumberOfCpus                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
index b0c313b1f0..d928952551 100644
--- a/UefiCpuPkg/Library/RegisterCpuFeaturesLib/RegisterCpuFeaturesDxe.uni
+++ b/UefiCpuPkg/Library/RegisterCpuFeaturesLib/RegisterCpuFeaturesDxe.uni
@@ -1,22 +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."
-
+// /** @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
index 7ef0155374..338f1a495c 100644
--- a/UefiCpuPkg/Library/RegisterCpuFeaturesLib/RegisterCpuFeaturesLib.c
+++ b/UefiCpuPkg/Library/RegisterCpuFeaturesLib/RegisterCpuFeaturesLib.c
@@ -1,775 +1,775 @@
-/** @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;
-  CpuFeatureEntry = NULL;
-  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);
-    ASSERT (CpuFeatureEntry != NULL);
-    //
-    // 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[in]  FeatureName        A Null-terminated Ascii string indicates CPU feature
-                                 name.
-  @param[in]  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[in]  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[in]  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[in]  ...                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 UINT64                  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          = (UINT32) Index;
-  RegisterTableEntry[RegisterTable->TableLength].HighIndex      = (UINT32) RShiftU64 (Index, 32);
-  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 UINT64              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 UINT64              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;
-}
-
+/** @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;
+  CpuFeatureEntry = NULL;
+  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);
+    ASSERT (CpuFeatureEntry != NULL);
+    //
+    // 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[in]  FeatureName        A Null-terminated Ascii string indicates CPU feature
+                                 name.
+  @param[in]  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[in]  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[in]  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[in]  ...                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 UINT64                  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          = (UINT32) Index;
+  RegisterTableEntry[RegisterTable->TableLength].HighIndex      = (UINT32) RShiftU64 (Index, 32);
+  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 UINT64              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 UINT64              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;
+}
+