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