UefiCpuPkg/Library/RegisterCpuFeaturesLib/CpuFeaturesInitialize.c

   1 /** @file
   2   CPU Features Initialize functions.
   3
   4   Copyright (c) 2017, Intel Corporation. All rights reserved.<BR>
   5   This program and the accompanying materials
   6   are licensed and made available under the terms and conditions of the BSD License
   7   which accompanies this distribution.  The full text of the license may be found at
   8   http://opensource.org/licenses/bsd-license.php
   9
  10   THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
  11   WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
  12
  13 **/
  14
  15 #include "RegisterCpuFeatures.h"
  16
  17 /**
  18   Worker function to save PcdCpuFeaturesCapability.
  19
  20   @param[in]  SupportedFeatureMask  The pointer to CPU feature bits mask buffer
  21 **/
  22 VOID
  23 SetCapabilityPcd (
  24   IN UINT8               *SupportedFeatureMask
  25   )
  26 {
  27   EFI_STATUS             Status;
  28   UINTN                  BitMaskSize;
  29
  30   BitMaskSize = PcdGetSize (PcdCpuFeaturesCapability);
  31   Status = PcdSetPtrS (PcdCpuFeaturesCapability, &BitMaskSize, SupportedFeatureMask);
  32   ASSERT_EFI_ERROR (Status);
  33 }
  34
  35 /**
  36   Worker function to save PcdCpuFeaturesSetting.
  37
  38   @param[in]  SupportedFeatureMask  The pointer to CPU feature bits mask buffer
  39 **/
  40 VOID
  41 SetSettingPcd (
  42   IN UINT8               *SupportedFeatureMask
  43   )
  44 {
  45   EFI_STATUS             Status;
  46   UINTN                  BitMaskSize;
  47
  48   BitMaskSize = PcdGetSize (PcdCpuFeaturesSetting);
  49   Status = PcdSetPtrS (PcdCpuFeaturesSetting, &BitMaskSize, SupportedFeatureMask);
  50   ASSERT_EFI_ERROR (Status);
  51 }
  52
  53 /**
  54   Worker function to get PcdCpuFeaturesSupport.
  55
  56   @return  The pointer to CPU feature bits mask buffer.
  57 **/
  58 UINT8 *
  59 GetSupportPcd (
  60   VOID
  61   )
  62 {
  63   UINT8                  *SupportBitMask;
  64
  65   SupportBitMask = AllocateCopyPool (
  66           PcdGetSize (PcdCpuFeaturesSupport),
  67           PcdGetPtr (PcdCpuFeaturesSupport)
  68           );
  69   ASSERT (SupportBitMask != NULL);
  70
  71   return SupportBitMask;
  72 }
  73
  74 /**
  75   Worker function to get PcdCpuFeaturesUserConfiguration.
  76
  77   @return  The pointer to CPU feature bits mask buffer.
  78 **/
  79 UINT8 *
  80 GetConfigurationPcd (
  81   VOID
  82   )
  83 {
  84   UINT8                  *SupportBitMask;
  85
  86   SupportBitMask = AllocateCopyPool (
  87           PcdGetSize (PcdCpuFeaturesUserConfiguration),
  88           PcdGetPtr (PcdCpuFeaturesUserConfiguration)
  89           );
  90   ASSERT (SupportBitMask != NULL);
  91
  92   return SupportBitMask;
  93 }
  94
  95 /**
  96   Collects CPU type and feature information.
  97
  98   @param[in, out]  CpuInfo  The pointer to CPU feature information
  99 **/
 100 VOID
 101 FillProcessorInfo (
 102   IN OUT REGISTER_CPU_FEATURE_INFORMATION        *CpuInfo
 103   )
 104 {
 105   CPUID_VERSION_INFO_EAX Eax;
 106   CPUID_VERSION_INFO_ECX Ecx;
 107   CPUID_VERSION_INFO_EDX Edx;
 108   UINT32                 DisplayedFamily;
 109   UINT32                 DisplayedModel;
 110
 111   AsmCpuid (CPUID_VERSION_INFO, &Eax.Uint32, NULL, &Ecx.Uint32, &Edx.Uint32);
 112
 113   DisplayedFamily = Eax.Bits.FamilyId;
 114   if (Eax.Bits.FamilyId == 0x0F) {
 115     DisplayedFamily |= (Eax.Bits.ExtendedFamilyId << 4);
 116   }
 117
 118   DisplayedModel = Eax.Bits.Model;
 119   if (Eax.Bits.FamilyId == 0x06 || Eax.Bits.FamilyId == 0x0f) {
 120     DisplayedModel |= (Eax.Bits.ExtendedModelId << 4);
 121   }
 122
 123   CpuInfo->DisplayFamily = DisplayedFamily;
 124   CpuInfo->DisplayModel  = DisplayedModel;
 125   CpuInfo->SteppingId    = Eax.Bits.SteppingId;
 126   CpuInfo->ProcessorType = Eax.Bits.ProcessorType;
 127   CpuInfo->CpuIdVersionInfoEcx.Uint32 = Ecx.Uint32;
 128   CpuInfo->CpuIdVersionInfoEdx.Uint32 = Edx.Uint32;
 129 }
 130
 131 /**
 132   Prepares for private data used for CPU features.
 133
 134   @param[in]  NumberOfCpus  Number of processor in system
 135 **/
 136 VOID
 137 CpuInitDataInitialize (
 138   IN UINTN                             NumberOfCpus
 139   )
 140 {
 141   EFI_STATUS                           Status;
 142   UINTN                                ProcessorNumber;
 143   EFI_PROCESSOR_INFORMATION            ProcessorInfoBuffer;
 144   CPU_FEATURES_ENTRY                   *CpuFeature;
 145   CPU_FEATURES_INIT_ORDER              *InitOrder;
 146   CPU_FEATURES_DATA                    *CpuFeaturesData;
 147   LIST_ENTRY                           *Entry;
 148
 149   CpuFeaturesData = GetCpuFeaturesData ();
 150   CpuFeaturesData->InitOrder = AllocateZeroPool (sizeof (CPU_FEATURES_INIT_ORDER) * NumberOfCpus);
 151   ASSERT (CpuFeaturesData->InitOrder != NULL);
 152
 153   //
 154   // Collect CPU Features information
 155   //
 156   Entry = GetFirstNode (&CpuFeaturesData->FeatureList);
 157   while (!IsNull (&CpuFeaturesData->FeatureList, Entry)) {
 158     CpuFeature = CPU_FEATURE_ENTRY_FROM_LINK (Entry);
 159     ASSERT (CpuFeature->InitializeFunc != NULL);
 160     if (CpuFeature->GetConfigDataFunc != NULL) {
 161       CpuFeature->ConfigData = CpuFeature->GetConfigDataFunc (NumberOfCpus);
 162     }
 163     Entry = Entry->ForwardLink;
 164   }
 165
 166   for (ProcessorNumber = 0; ProcessorNumber < NumberOfCpus; ProcessorNumber++) {
 167     InitOrder = &CpuFeaturesData->InitOrder[ProcessorNumber];
 168     InitOrder->FeaturesSupportedMask = AllocateZeroPool (CpuFeaturesData->BitMaskSize);
 169     ASSERT (InitOrder->FeaturesSupportedMask != NULL);
 170     InitializeListHead (&InitOrder->OrderList);
 171     Status = GetProcessorInformation (ProcessorNumber, &ProcessorInfoBuffer);
 172     ASSERT_EFI_ERROR (Status);
 173     CopyMem (
 174       &InitOrder->CpuInfo.ProcessorInfo,
 175       &ProcessorInfoBuffer,
 176       sizeof (EFI_PROCESSOR_INFORMATION)
 177       );
 178   }
 179   //
 180   // Get support and configuration PCDs
 181   //
 182   CpuFeaturesData->SupportPcd       = GetSupportPcd ();
 183   CpuFeaturesData->ConfigurationPcd = GetConfigurationPcd ();
 184 }
 185
 186 /**
 187   Worker function to do OR operation on CPU feature supported bits mask buffer.
 188
 189   @param[in]  SupportedFeatureMask  The pointer to CPU feature bits mask buffer
 190   @param[in]  OrFeatureBitMask      The feature bit mask to do OR operation
 191 **/
 192 VOID
 193 SupportedMaskOr (
 194   IN UINT8               *SupportedFeatureMask,
 195   IN UINT8               *OrFeatureBitMask
 196   )
 197 {
 198   UINTN                  Index;
 199   UINTN                  BitMaskSize;
 200   UINT8                  *Data1;
 201   UINT8                  *Data2;
 202
 203   BitMaskSize = PcdGetSize (PcdCpuFeaturesSupport);
 204   Data1 = SupportedFeatureMask;
 205   Data2 = OrFeatureBitMask;
 206   for (Index = 0; Index < BitMaskSize; Index++) {
 207     *(Data1++) |=  *(Data2++);
 208   }
 209 }
 210
 211 /**
 212   Worker function to do AND operation on CPU feature supported bits mask buffer.
 213
 214   @param[in]  SupportedFeatureMask  The pointer to CPU feature bits mask buffer
 215   @param[in]  AndFeatureBitMask     The feature bit mask to do AND operation
 216 **/
 217 VOID
 218 SupportedMaskAnd (
 219   IN UINT8               *SupportedFeatureMask,
 220   IN UINT8               *AndFeatureBitMask
 221   )
 222 {
 223   UINTN                  Index;
 224   UINTN                  BitMaskSize;
 225   UINT8                  *Data1;
 226   UINT8                  *Data2;
 227
 228   BitMaskSize = PcdGetSize (PcdCpuFeaturesSupport);
 229   Data1 = SupportedFeatureMask;
 230   Data2 = AndFeatureBitMask;
 231   for (Index = 0; Index < BitMaskSize; Index++) {
 232     *(Data1++) &=  *(Data2++);
 233   }
 234 }
 235
 236 /**
 237   Worker function to clean bit operation on CPU feature supported bits mask buffer.
 238
 239   @param[in]  SupportedFeatureMask  The pointer to CPU feature bits mask buffer
 240   @param[in]  AndFeatureBitMask     The feature bit mask to do XOR operation
 241 **/
 242 VOID
 243 SupportedMaskCleanBit (
 244   IN UINT8               *SupportedFeatureMask,
 245   IN UINT8               *AndFeatureBitMask
 246   )
 247 {
 248   UINTN                  Index;
 249   UINTN                  BitMaskSize;
 250   UINT8                  *Data1;
 251   UINT8                  *Data2;
 252
 253   BitMaskSize = PcdGetSize (PcdCpuFeaturesSupport);
 254   Data1 = SupportedFeatureMask;
 255   Data2 = AndFeatureBitMask;
 256   for (Index = 0; Index < BitMaskSize; Index++) {
 257     *(Data1++) &=  ~(*(Data2++));
 258   }
 259 }
 260
 261 /**
 262   Worker function to check if the compared CPU feature set in the CPU feature
 263   supported bits mask buffer.
 264
 265   @param[in]  SupportedFeatureMask   The pointer to CPU feature bits mask buffer
 266   @param[in]  ComparedFeatureBitMask The feature bit mask to be compared
 267
 268   @retval TRUE   The ComparedFeatureBitMask is set in CPU feature supported bits
 269                  mask buffer.
 270   @retval FALSE  The ComparedFeatureBitMask is not set in CPU feature supported bits
 271                  mask buffer.
 272 **/
 273 BOOLEAN
 274 IsBitMaskMatch (
 275   IN UINT8               *SupportedFeatureMask,
 276   IN UINT8               *ComparedFeatureBitMask
 277   )
 278 {
 279   UINTN                  Index;
 280   UINTN                  BitMaskSize;
 281   UINT8                  *Data1;
 282   UINT8                  *Data2;
 283
 284   BitMaskSize = PcdGetSize (PcdCpuFeaturesSupport);
 285
 286   Data1 = SupportedFeatureMask;
 287   Data2 = ComparedFeatureBitMask;
 288   for (Index = 0; Index < BitMaskSize; Index++) {
 289     if (((*(Data1++)) & (*(Data2++))) != 0) {
 290       return TRUE;
 291     }
 292   }
 293   return FALSE;
 294 }
 295
 296 /**
 297   Collects processor data for calling processor.
 298
 299   @param[in,out]  Buffer  The pointer to private data buffer.
 300 **/
 301 VOID
 302 EFIAPI
 303 CollectProcessorData (
 304   IN OUT VOID                          *Buffer
 305   )
 306 {
 307   UINTN                                ProcessorNumber;
 308   CPU_FEATURES_ENTRY                   *CpuFeature;
 309   REGISTER_CPU_FEATURE_INFORMATION     *CpuInfo;
 310   LIST_ENTRY                           *Entry;
 311   CPU_FEATURES_DATA                    *CpuFeaturesData;
 312
 313   CpuFeaturesData = GetCpuFeaturesData ();
 314   ProcessorNumber = GetProcessorIndex ();
 315   CpuInfo = &CpuFeaturesData->InitOrder[ProcessorNumber].CpuInfo;
 316   //
 317   // collect processor information
 318   //
 319   FillProcessorInfo (CpuInfo);
 320   Entry = GetFirstNode (&CpuFeaturesData->FeatureList);
 321   while (!IsNull (&CpuFeaturesData->FeatureList, Entry)) {
 322     CpuFeature = CPU_FEATURE_ENTRY_FROM_LINK (Entry);
 323     if (IsBitMaskMatch (CpuFeaturesData->SupportPcd, CpuFeature->FeatureMask)) {
 324       if (CpuFeature->SupportFunc == NULL) {
 325         //
 326         // If SupportFunc is NULL, then the feature is supported.
 327         //
 328         SupportedMaskOr (
 329           CpuFeaturesData->InitOrder[ProcessorNumber].FeaturesSupportedMask,
 330           CpuFeature->FeatureMask
 331           );
 332       } else if (CpuFeature->SupportFunc (ProcessorNumber, CpuInfo, CpuFeature->ConfigData)) {
 333         SupportedMaskOr (
 334           CpuFeaturesData->InitOrder[ProcessorNumber].FeaturesSupportedMask,
 335           CpuFeature->FeatureMask
 336           );
 337       }
 338     }
 339     Entry = Entry->ForwardLink;
 340   }
 341 }
 342
 343 /**
 344   Dump the contents of a CPU register table.
 345
 346   @param[in]  ProcessorNumber  The index of the CPU to show the register table contents
 347
 348   @note This service could be called by BSP only.
 349 **/
 350 VOID
 351 DumpRegisterTableOnProcessor (
 352   IN UINTN                             ProcessorNumber
 353   )
 354 {
 355   CPU_FEATURES_DATA                    *CpuFeaturesData;
 356   UINTN                                FeatureIndex;
 357   CPU_REGISTER_TABLE                   *RegisterTable;
 358   CPU_REGISTER_TABLE_ENTRY             *RegisterTableEntry;
 359   CPU_REGISTER_TABLE_ENTRY             *RegisterTableEntryHead;
 360   UINT32                               DebugPrintErrorLevel;
 361
 362   DebugPrintErrorLevel = (ProcessorNumber == 0) ? DEBUG_INFO : DEBUG_VERBOSE;
 363   CpuFeaturesData      = GetCpuFeaturesData ();
 364   //
 365   // Debug information
 366   //
 367   RegisterTable = &CpuFeaturesData->RegisterTable[ProcessorNumber];
 368   DEBUG ((DebugPrintErrorLevel, "RegisterTable->TableLength = %d\n", RegisterTable->TableLength));
 369
 370   RegisterTableEntryHead = (CPU_REGISTER_TABLE_ENTRY *) (UINTN) RegisterTable->RegisterTableEntry;
 371
 372   for (FeatureIndex = 0; FeatureIndex < RegisterTable->TableLength; FeatureIndex++) {
 373     RegisterTableEntry = &RegisterTableEntryHead[FeatureIndex];
 374     switch (RegisterTableEntry->RegisterType) {
 375     case Msr:
 376       DEBUG ((
 377         DebugPrintErrorLevel,
 378         "Processor: %d:   MSR: %x, Bit Start: %d, Bit Length: %d, Value: %lx\r\n",
 379         ProcessorNumber,
 380         RegisterTableEntry->Index,
 381         RegisterTableEntry->ValidBitStart,
 382         RegisterTableEntry->ValidBitLength,
 383         RegisterTableEntry->Value
 384         ));
 385       break;
 386     case ControlRegister:
 387       DEBUG ((
 388         DebugPrintErrorLevel,
 389         "Processor: %d:    CR: %x, Bit Start: %d, Bit Length: %d, Value: %lx\r\n",
 390         ProcessorNumber,
 391         RegisterTableEntry->Index,
 392         RegisterTableEntry->ValidBitStart,
 393         RegisterTableEntry->ValidBitLength,
 394         RegisterTableEntry->Value
 395         ));
 396       break;
 397     case MemoryMapped:
 398       DEBUG ((
 399         DebugPrintErrorLevel,
 400         "Processor: %d:  MMIO: %lx, Bit Start: %d, Bit Length: %d, Value: %lx\r\n",
 401         ProcessorNumber,
 402         RegisterTableEntry->Index | LShiftU64 (RegisterTableEntry->HighIndex, 32),
 403         RegisterTableEntry->ValidBitStart,
 404         RegisterTableEntry->ValidBitLength,
 405         RegisterTableEntry->Value
 406         ));
 407       break;
 408     case CacheControl:
 409       DEBUG ((
 410         DebugPrintErrorLevel,
 411         "Processor: %d: CACHE: %x, Bit Start: %d, Bit Length: %d, Value: %lx\r\n",
 412         ProcessorNumber,
 413         RegisterTableEntry->Index,
 414         RegisterTableEntry->ValidBitStart,
 415         RegisterTableEntry->ValidBitLength,
 416         RegisterTableEntry->Value
 417         ));
 418       break;
 419     default:
 420       break;
 421     }
 422   }
 423 }
 424
 425 /**
 426   Analysis register CPU features on each processor and save CPU setting in CPU register table.
 427
 428   @param[in]  NumberOfCpus  Number of processor in system
 429
 430 **/
 431 VOID
 432 AnalysisProcessorFeatures (
 433   IN UINTN                             NumberOfCpus
 434   )
 435 {
 436   EFI_STATUS                           Status;
 437   UINTN                                ProcessorNumber;
 438   CPU_FEATURES_ENTRY                   *CpuFeature;
 439   CPU_FEATURES_ENTRY                   *CpuFeatureInOrder;
 440   CPU_FEATURES_INIT_ORDER              *CpuInitOrder;
 441   REGISTER_CPU_FEATURE_INFORMATION     *CpuInfo;
 442   LIST_ENTRY                           *Entry;
 443   CPU_FEATURES_DATA                    *CpuFeaturesData;
 444
 445   CpuFeaturesData = GetCpuFeaturesData ();
 446   CpuFeaturesData->CapabilityPcd = AllocatePool (CpuFeaturesData->BitMaskSize);
 447   ASSERT (CpuFeaturesData->CapabilityPcd != NULL);
 448   SetMem (CpuFeaturesData->CapabilityPcd, CpuFeaturesData->BitMaskSize, 0xFF);
 449   for (ProcessorNumber = 0; ProcessorNumber < NumberOfCpus; ProcessorNumber++) {
 450     CpuInitOrder = &CpuFeaturesData->InitOrder[ProcessorNumber];
 451     //
 452     // Calculate the last capability on all processors
 453     //
 454     SupportedMaskAnd (CpuFeaturesData->CapabilityPcd, CpuInitOrder->FeaturesSupportedMask);
 455   }
 456   //
 457   // Calculate the last setting
 458   //
 459
 460   CpuFeaturesData->SettingPcd = AllocateCopyPool (CpuFeaturesData->BitMaskSize, CpuFeaturesData->CapabilityPcd);
 461   ASSERT (CpuFeaturesData->SettingPcd != NULL);
 462   SupportedMaskAnd (CpuFeaturesData->SettingPcd, CpuFeaturesData->ConfigurationPcd);
 463
 464   //
 465   // Save PCDs and display CPU PCDs
 466   //
 467   SetCapabilityPcd (CpuFeaturesData->CapabilityPcd);
 468   SetSettingPcd (CpuFeaturesData->SettingPcd);
 469
 470   //
 471   // Dump the last CPU feature list
 472   //
 473   DEBUG_CODE (
 474     DEBUG ((DEBUG_INFO, "Last CPU features list...\n"));
 475     Entry = GetFirstNode (&CpuFeaturesData->FeatureList);
 476     while (!IsNull (&CpuFeaturesData->FeatureList, Entry)) {
 477       CpuFeature = CPU_FEATURE_ENTRY_FROM_LINK (Entry);
 478       if (IsBitMaskMatch (CpuFeature->FeatureMask, CpuFeaturesData->CapabilityPcd)) {
 479         if (IsBitMaskMatch (CpuFeature->FeatureMask, CpuFeaturesData->SettingPcd)) {
 480           DEBUG ((DEBUG_INFO, "[Enable   ] "));
 481         } else {
 482           DEBUG ((DEBUG_INFO, "[Disable  ] "));
 483         }
 484       } else {
 485         DEBUG ((DEBUG_INFO, "[Unsupport] "));
 486       }
 487       DumpCpuFeature (CpuFeature);
 488       Entry = Entry->ForwardLink;
 489     }
 490     DEBUG ((DEBUG_INFO, "PcdCpuFeaturesSupport:\n"));
 491     DumpCpuFeatureMask (CpuFeaturesData->SupportPcd);
 492     DEBUG ((DEBUG_INFO, "PcdCpuFeaturesUserConfiguration:\n"));
 493     DumpCpuFeatureMask (CpuFeaturesData->ConfigurationPcd);
 494     DEBUG ((DEBUG_INFO, "PcdCpuFeaturesCapability:\n"));
 495     DumpCpuFeatureMask (CpuFeaturesData->CapabilityPcd);
 496     DEBUG ((DEBUG_INFO, "PcdCpuFeaturesSetting:\n"));
 497     DumpCpuFeatureMask (CpuFeaturesData->SettingPcd);
 498   );
 499
 500   for (ProcessorNumber = 0; ProcessorNumber < NumberOfCpus; ProcessorNumber++) {
 501     CpuInitOrder = &CpuFeaturesData->InitOrder[ProcessorNumber];
 502     Entry = GetFirstNode (&CpuFeaturesData->FeatureList);
 503     while (!IsNull (&CpuFeaturesData->FeatureList, Entry)) {
 504       //
 505       // Insert each feature into processor's order list
 506       //
 507       CpuFeature = CPU_FEATURE_ENTRY_FROM_LINK (Entry);
 508       if (IsBitMaskMatch (CpuFeature->FeatureMask, CpuFeaturesData->CapabilityPcd)) {
 509         CpuFeatureInOrder = AllocateCopyPool (sizeof (CPU_FEATURES_ENTRY), CpuFeature);
 510         ASSERT (CpuFeatureInOrder != NULL);
 511         InsertTailList (&CpuInitOrder->OrderList, &CpuFeatureInOrder->Link);
 512       }
 513       Entry = Entry->ForwardLink;
 514     }
 515     //
 516     // Go through ordered feature list to initialize CPU features
 517     //
 518     CpuInfo = &CpuFeaturesData->InitOrder[ProcessorNumber].CpuInfo;
 519     Entry = GetFirstNode (&CpuInitOrder->OrderList);
 520     while (!IsNull (&CpuInitOrder->OrderList, Entry)) {
 521       CpuFeatureInOrder = CPU_FEATURE_ENTRY_FROM_LINK (Entry);
 522       if (IsBitMaskMatch (CpuFeatureInOrder->FeatureMask, CpuFeaturesData->SettingPcd)) {
 523         Status = CpuFeatureInOrder->InitializeFunc (ProcessorNumber, CpuInfo, CpuFeatureInOrder->ConfigData, TRUE);
 524         if (EFI_ERROR (Status)) {
 525           //
 526           // Clean the CpuFeatureInOrder->FeatureMask in setting PCD.
 527           //
 528           SupportedMaskCleanBit (CpuFeaturesData->SettingPcd, CpuFeatureInOrder->FeatureMask);
 529           if (CpuFeatureInOrder->FeatureName != NULL) {
 530             DEBUG ((DEBUG_WARN, "Warning :: Failed to enable Feature: Name = %a.\n", CpuFeatureInOrder->FeatureName));
 531           } else {
 532             DEBUG ((DEBUG_WARN, "Warning :: Failed to enable Feature: Mask = "));
 533             DumpCpuFeatureMask (CpuFeatureInOrder->FeatureMask);
 534           }
 535         }
 536       } else {
 537         Status = CpuFeatureInOrder->InitializeFunc (ProcessorNumber, CpuInfo, CpuFeatureInOrder->ConfigData, FALSE);
 538         if (EFI_ERROR (Status)) {
 539           if (CpuFeatureInOrder->FeatureName != NULL) {
 540             DEBUG ((DEBUG_WARN, "Warning :: Failed to disable Feature: Name = %a.\n", CpuFeatureInOrder->FeatureName));
 541           } else {
 542             DEBUG ((DEBUG_WARN, "Warning :: Failed to disable Feature: Mask = "));
 543             DumpCpuFeatureMask (CpuFeatureInOrder->FeatureMask);
 544           }
 545         }
 546       }
 547       Entry = Entry->ForwardLink;
 548     }
 549
 550     //
 551     // Dump PcdCpuFeaturesSetting again because this value maybe updated
 552     // again during initialize the features.
 553     //
 554     DEBUG ((DEBUG_INFO, "Dump final value for PcdCpuFeaturesSetting:\n"));
 555     DumpCpuFeatureMask (CpuFeaturesData->SettingPcd);
 556
 557     //
 558     // Dump the RegisterTable
 559     //
 560     DumpRegisterTableOnProcessor (ProcessorNumber);
 561   }
 562 }
 563
 564 /**
 565   Initialize the CPU registers from a register table.
 566
 567   @param[in]  ProcessorNumber  The index of the CPU executing this function.
 568
 569   @note This service could be called by BSP/APs.
 570 **/
 571 VOID
 572 ProgramProcessorRegister (
 573   IN UINTN  ProcessorNumber
 574   )
 575 {
 576   CPU_FEATURES_DATA         *CpuFeaturesData;
 577   CPU_REGISTER_TABLE        *RegisterTable;
 578   CPU_REGISTER_TABLE_ENTRY  *RegisterTableEntry;
 579   UINTN                     Index;
 580   UINTN                     Value;
 581   CPU_REGISTER_TABLE_ENTRY  *RegisterTableEntryHead;
 582
 583   CpuFeaturesData = GetCpuFeaturesData ();
 584   RegisterTable = &CpuFeaturesData->RegisterTable[ProcessorNumber];
 585
 586   //
 587   // Traverse Register Table of this logical processor
 588   //
 589   RegisterTableEntryHead = (CPU_REGISTER_TABLE_ENTRY *) (UINTN) RegisterTable->RegisterTableEntry;
 590
 591   for (Index = 0; Index < RegisterTable->TableLength; Index++) {
 592
 593     RegisterTableEntry = &RegisterTableEntryHead[Index];
 594
 595     //
 596     // Check the type of specified register
 597     //
 598     switch (RegisterTableEntry->RegisterType) {
 599     //
 600     // The specified register is Control Register
 601     //
 602     case ControlRegister:
 603       switch (RegisterTableEntry->Index) {
 604       case 0:
 605         Value = AsmReadCr0 ();
 606         Value = (UINTN) BitFieldWrite64 (
 607                           Value,
 608                           RegisterTableEntry->ValidBitStart,
 609                           RegisterTableEntry->ValidBitStart + RegisterTableEntry->ValidBitLength - 1,
 610                           RegisterTableEntry->Value
 611                           );
 612         AsmWriteCr0 (Value);
 613         break;
 614       case 2:
 615         Value = AsmReadCr2 ();
 616         Value = (UINTN) BitFieldWrite64 (
 617                           Value,
 618                           RegisterTableEntry->ValidBitStart,
 619                           RegisterTableEntry->ValidBitStart + RegisterTableEntry->ValidBitLength - 1,
 620                           RegisterTableEntry->Value
 621                           );
 622         AsmWriteCr2 (Value);
 623         break;
 624       case 3:
 625         Value = AsmReadCr3 ();
 626         Value = (UINTN) BitFieldWrite64 (
 627                           Value,
 628                           RegisterTableEntry->ValidBitStart,
 629                           RegisterTableEntry->ValidBitStart + RegisterTableEntry->ValidBitLength - 1,
 630                           RegisterTableEntry->Value
 631                           );
 632         AsmWriteCr3 (Value);
 633         break;
 634       case 4:
 635         Value = AsmReadCr4 ();
 636         Value = (UINTN) BitFieldWrite64 (
 637                           Value,
 638                           RegisterTableEntry->ValidBitStart,
 639                           RegisterTableEntry->ValidBitStart + RegisterTableEntry->ValidBitLength - 1,
 640                           RegisterTableEntry->Value
 641                           );
 642         AsmWriteCr4 (Value);
 643         break;
 644       case 8:
 645         //
 646         //  Do we need to support CR8?
 647         //
 648         break;
 649       default:
 650         break;
 651       }
 652       break;
 653     //
 654     // The specified register is Model Specific Register
 655     //
 656     case Msr:
 657       //
 658       // Get lock to avoid Package/Core scope MSRs programming issue in parallel execution mode
 659       //
 660       AcquireSpinLock (&CpuFeaturesData->MsrLock);
 661       if (RegisterTableEntry->ValidBitLength >= 64) {
 662         //
 663         // If length is not less than 64 bits, then directly write without reading
 664         //
 665         AsmWriteMsr64 (
 666           RegisterTableEntry->Index,
 667           RegisterTableEntry->Value
 668           );
 669       } else {
 670         //
 671         // Set the bit section according to bit start and length
 672         //
 673         AsmMsrBitFieldWrite64 (
 674           RegisterTableEntry->Index,
 675           RegisterTableEntry->ValidBitStart,
 676           RegisterTableEntry->ValidBitStart + RegisterTableEntry->ValidBitLength - 1,
 677           RegisterTableEntry->Value
 678           );
 679       }
 680       ReleaseSpinLock (&CpuFeaturesData->MsrLock);
 681       break;
 682     //
 683     // MemoryMapped operations
 684     //
 685     case MemoryMapped:
 686       AcquireSpinLock (&CpuFeaturesData->MemoryMappedLock);
 687       MmioBitFieldWrite32 (
 688         (UINTN)(RegisterTableEntry->Index | LShiftU64 (RegisterTableEntry->HighIndex, 32)),
 689         RegisterTableEntry->ValidBitStart,
 690         RegisterTableEntry->ValidBitStart + RegisterTableEntry->ValidBitLength - 1,
 691         (UINT32)RegisterTableEntry->Value
 692         );
 693       ReleaseSpinLock (&CpuFeaturesData->MemoryMappedLock);
 694       break;
 695     //
 696     // Enable or disable cache
 697     //
 698     case CacheControl:
 699       //
 700       // If value of the entry is 0, then disable cache.  Otherwise, enable cache.
 701       //
 702       if (RegisterTableEntry->Value == 0) {
 703         AsmDisableCache ();
 704       } else {
 705         AsmEnableCache ();
 706       }
 707       break;
 708
 709     default:
 710       break;
 711     }
 712   }
 713 }
 714
 715 /**
 716   Programs registers for the calling processor.
 717
 718   @param[in,out] Buffer  The pointer to private data buffer.
 719
 720 **/
 721 VOID
 722 EFIAPI
 723 SetProcessorRegister (
 724   IN OUT VOID            *Buffer
 725   )
 726 {
 727   UINTN                  ProcessorNumber;
 728
 729   ProcessorNumber = GetProcessorIndex ();
 730   ProgramProcessorRegister (ProcessorNumber);
 731 }
 732
 733 /**
 734   Performs CPU features detection.
 735
 736   This service will invoke MP service to check CPU features'
 737   capabilities on BSP/APs.
 738
 739   @note This service could be called by BSP only.
 740 **/
 741 VOID
 742 EFIAPI
 743 CpuFeaturesDetect (
 744   VOID
 745   )
 746 {
 747   UINTN                  NumberOfCpus;
 748   UINTN                  NumberOfEnabledProcessors;
 749
 750   GetNumberOfProcessor (&NumberOfCpus, &NumberOfEnabledProcessors);
 751
 752   CpuInitDataInitialize (NumberOfCpus);
 753
 754   //
 755   // Wakeup all APs for data collection.
 756   //
 757   StartupAPsWorker (CollectProcessorData);
 758
 759   //
 760   // Collect data on BSP
 761   //
 762   CollectProcessorData (NULL);
 763
 764   AnalysisProcessorFeatures (NumberOfCpus);
 765 }
 766
 767 /**
 768   Performs CPU features Initialization.
 769
 770   This service will invoke MP service to perform CPU features
 771   initialization on BSP/APs per user configuration.
 772
 773   @note This service could be called by BSP only.
 774 **/
 775 VOID
 776 EFIAPI
 777 CpuFeaturesInitialize (
 778   VOID
 779   )
 780 {
 781   CPU_FEATURES_DATA      *CpuFeaturesData;
 782   UINTN                  OldBspNumber;
 783
 784   CpuFeaturesData = GetCpuFeaturesData ();
 785
 786   OldBspNumber = GetProcessorIndex();
 787   CpuFeaturesData->BspNumber = OldBspNumber;
 788   //
 789   // Wakeup all APs for programming.
 790   //
 791   StartupAPsWorker (SetProcessorRegister);
 792   //
 793   // Programming BSP
 794   //
 795   SetProcessorRegister (NULL);
 796   //
 797   // Switch to new BSP if required
 798   //
 799   if (CpuFeaturesData->BspNumber != OldBspNumber) {
 800     SwitchNewBsp (CpuFeaturesData->BspNumber);
 801   }
 802 }