UefiCpuPkg/Library/RegisterCpuFeaturesLib/CpuFeaturesInitialize.c

   1 /** @file
   2   CPU Features Initialize functions.
   3
   4   Copyright (c) 2017 - 2020, Intel Corporation. All rights reserved.<BR>
   5   SPDX-License-Identifier: BSD-2-Clause-Patent
   6
   7 **/
   8
   9 #include "RegisterCpuFeatures.h"
  10
  11 CHAR16 *mDependTypeStr[]   = {L"None", L"Thread", L"Core", L"Package", L"Invalid" };
  12
  13 /**
  14   Worker function to save PcdCpuFeaturesCapability.
  15
  16   @param[in]  SupportedFeatureMask  The pointer to CPU feature bits mask buffer
  17   @param[in]  BitMaskSize           CPU feature bits mask buffer size.
  18
  19 **/
  20 VOID
  21 SetCapabilityPcd (
  22   IN UINT8               *SupportedFeatureMask,
  23   IN UINTN               BitMaskSize
  24   )
  25 {
  26   EFI_STATUS             Status;
  27
  28   Status = PcdSetPtrS (PcdCpuFeaturesCapability, &BitMaskSize, SupportedFeatureMask);
  29   ASSERT_EFI_ERROR (Status);
  30 }
  31
  32 /**
  33   Worker function to save PcdCpuFeaturesSetting.
  34
  35   @param[in]  SupportedFeatureMask  The pointer to CPU feature bits mask buffer
  36   @param[in]  BitMaskSize           CPU feature bits mask buffer size.
  37 **/
  38 VOID
  39 SetSettingPcd (
  40   IN UINT8               *SupportedFeatureMask,
  41   IN UINTN               BitMaskSize
  42   )
  43 {
  44   EFI_STATUS             Status;
  45
  46   Status = PcdSetPtrS (PcdCpuFeaturesSetting, &BitMaskSize, SupportedFeatureMask);
  47   ASSERT_EFI_ERROR (Status);
  48 }
  49
  50 /**
  51   Collects CPU type and feature information.
  52
  53   @param[in, out]  CpuInfo  The pointer to CPU feature information
  54 **/
  55 VOID
  56 FillProcessorInfo (
  57   IN OUT REGISTER_CPU_FEATURE_INFORMATION        *CpuInfo
  58   )
  59 {
  60   CPUID_VERSION_INFO_EAX Eax;
  61   CPUID_VERSION_INFO_ECX Ecx;
  62   CPUID_VERSION_INFO_EDX Edx;
  63   UINT32                 DisplayedFamily;
  64   UINT32                 DisplayedModel;
  65
  66   AsmCpuid (CPUID_VERSION_INFO, &Eax.Uint32, NULL, &Ecx.Uint32, &Edx.Uint32);
  67
  68   DisplayedFamily = Eax.Bits.FamilyId;
  69   if (Eax.Bits.FamilyId == 0x0F) {
  70     DisplayedFamily |= (Eax.Bits.ExtendedFamilyId << 4);
  71   }
  72
  73   DisplayedModel = Eax.Bits.Model;
  74   if (Eax.Bits.FamilyId == 0x06 || Eax.Bits.FamilyId == 0x0f) {
  75     DisplayedModel |= (Eax.Bits.ExtendedModelId << 4);
  76   }
  77
  78   CpuInfo->DisplayFamily = DisplayedFamily;
  79   CpuInfo->DisplayModel  = DisplayedModel;
  80   CpuInfo->SteppingId    = Eax.Bits.SteppingId;
  81   CpuInfo->ProcessorType = Eax.Bits.ProcessorType;
  82   CpuInfo->CpuIdVersionInfoEcx.Uint32 = Ecx.Uint32;
  83   CpuInfo->CpuIdVersionInfoEdx.Uint32 = Edx.Uint32;
  84 }
  85
  86 /**
  87   Prepares for private data used for CPU features.
  88
  89 **/
  90 VOID
  91 CpuInitDataInitialize (
  92   VOID
  93   )
  94 {
  95   EFI_STATUS                           Status;
  96   UINTN                                ProcessorNumber;
  97   EFI_PROCESSOR_INFORMATION            ProcessorInfoBuffer;
  98   CPU_FEATURES_ENTRY                   *CpuFeature;
  99   CPU_FEATURES_INIT_ORDER              *InitOrder;
 100   CPU_FEATURES_DATA                    *CpuFeaturesData;
 101   LIST_ENTRY                           *Entry;
 102   UINT32                               Core;
 103   UINT32                               Package;
 104   UINT32                               Thread;
 105   EFI_CPU_PHYSICAL_LOCATION            *Location;
 106   BOOLEAN                              *CoresVisited;
 107   UINTN                                Index;
 108   UINT32                               PackageIndex;
 109   UINT32                               CoreIndex;
 110   UINT32                               First;
 111   ACPI_CPU_DATA                        *AcpiCpuData;
 112   CPU_STATUS_INFORMATION               *CpuStatus;
 113   UINT32                               *ValidCoreCountPerPackage;
 114   UINTN                                NumberOfCpus;
 115   UINTN                                NumberOfEnabledProcessors;
 116
 117   Core    = 0;
 118   Package = 0;
 119   Thread  = 0;
 120
 121   CpuFeaturesData = GetCpuFeaturesData ();
 122
 123   //
 124   // Initialize CpuFeaturesData->MpService as early as possile, so later function can use it.
 125   //
 126   CpuFeaturesData->MpService = GetMpService ();
 127
 128   GetNumberOfProcessor (&NumberOfCpus, &NumberOfEnabledProcessors);
 129
 130   CpuFeaturesData->InitOrder = AllocateZeroPool (sizeof (CPU_FEATURES_INIT_ORDER) * NumberOfCpus);
 131   ASSERT (CpuFeaturesData->InitOrder != NULL);
 132
 133   //
 134   // Collect CPU Features information
 135   //
 136   Entry = GetFirstNode (&CpuFeaturesData->FeatureList);
 137   while (!IsNull (&CpuFeaturesData->FeatureList, Entry)) {
 138     CpuFeature = CPU_FEATURE_ENTRY_FROM_LINK (Entry);
 139     ASSERT (CpuFeature->InitializeFunc != NULL);
 140     if (CpuFeature->GetConfigDataFunc != NULL) {
 141       CpuFeature->ConfigData = CpuFeature->GetConfigDataFunc (NumberOfCpus);
 142     }
 143     Entry = Entry->ForwardLink;
 144   }
 145
 146   CpuFeaturesData->NumberOfCpus = (UINT32) NumberOfCpus;
 147
 148   AcpiCpuData = GetAcpiCpuData ();
 149   ASSERT (AcpiCpuData != NULL);
 150   CpuFeaturesData->AcpiCpuData= AcpiCpuData;
 151
 152   CpuStatus = &AcpiCpuData->CpuStatus;
 153   Location = AllocateZeroPool (sizeof (EFI_CPU_PHYSICAL_LOCATION) * NumberOfCpus);
 154   ASSERT (Location != NULL);
 155   AcpiCpuData->ApLocation = (EFI_PHYSICAL_ADDRESS)(UINTN)Location;
 156
 157   for (ProcessorNumber = 0; ProcessorNumber < NumberOfCpus; ProcessorNumber++) {
 158     InitOrder = &CpuFeaturesData->InitOrder[ProcessorNumber];
 159     InitOrder->FeaturesSupportedMask = AllocateZeroPool (CpuFeaturesData->BitMaskSize);
 160     ASSERT (InitOrder->FeaturesSupportedMask != NULL);
 161     InitializeListHead (&InitOrder->OrderList);
 162     Status = GetProcessorInformation (ProcessorNumber, &ProcessorInfoBuffer);
 163     ASSERT_EFI_ERROR (Status);
 164     CopyMem (
 165       &InitOrder->CpuInfo.ProcessorInfo,
 166       &ProcessorInfoBuffer,
 167       sizeof (EFI_PROCESSOR_INFORMATION)
 168       );
 169     CopyMem (
 170       &Location[ProcessorNumber],
 171       &ProcessorInfoBuffer.Location,
 172       sizeof (EFI_CPU_PHYSICAL_LOCATION)
 173       );
 174
 175     //
 176     // Collect CPU package count info.
 177     //
 178     if (Package < ProcessorInfoBuffer.Location.Package) {
 179       Package = ProcessorInfoBuffer.Location.Package;
 180     }
 181     //
 182     // Collect CPU max core count info.
 183     //
 184     if (Core < ProcessorInfoBuffer.Location.Core) {
 185       Core = ProcessorInfoBuffer.Location.Core;
 186     }
 187     //
 188     // Collect CPU max thread count info.
 189     //
 190     if (Thread < ProcessorInfoBuffer.Location.Thread) {
 191       Thread = ProcessorInfoBuffer.Location.Thread;
 192     }
 193   }
 194   CpuStatus->PackageCount    = Package + 1;
 195   CpuStatus->MaxCoreCount    = Core + 1;
 196   CpuStatus->MaxThreadCount  = Thread + 1;
 197   DEBUG ((DEBUG_INFO, "Processor Info: Package: %d, MaxCore : %d, MaxThread: %d\n",
 198          CpuStatus->PackageCount,
 199          CpuStatus->MaxCoreCount,
 200          CpuStatus->MaxThreadCount));
 201
 202   //
 203   // Collect valid core count in each package because not all cores are valid.
 204   //
 205   ValidCoreCountPerPackage= AllocateZeroPool (sizeof (UINT32) * CpuStatus->PackageCount);
 206   ASSERT (ValidCoreCountPerPackage != 0);
 207   CpuStatus->ValidCoreCountPerPackage = (EFI_PHYSICAL_ADDRESS)(UINTN)ValidCoreCountPerPackage;
 208   CoresVisited = AllocatePool (sizeof (BOOLEAN) * CpuStatus->MaxCoreCount);
 209   ASSERT (CoresVisited != NULL);
 210
 211   for (Index = 0; Index < CpuStatus->PackageCount; Index ++ ) {
 212     ZeroMem (CoresVisited, sizeof (BOOLEAN) * CpuStatus->MaxCoreCount);
 213     //
 214     // Collect valid cores in Current package.
 215     //
 216     for (ProcessorNumber = 0; ProcessorNumber < NumberOfCpus; ProcessorNumber++) {
 217       Location = &CpuFeaturesData->InitOrder[ProcessorNumber].CpuInfo.ProcessorInfo.Location;
 218       if (Location->Package == Index && !CoresVisited[Location->Core] ) {
 219         //
 220         // The ValidCores position for Location->Core is valid.
 221         // The possible values in ValidCores[Index] are 0 or 1.
 222         // FALSE means no valid threads in this Core.
 223         // TRUE means have valid threads in this core, no matter the thead count is 1 or more.
 224         //
 225         CoresVisited[Location->Core] = TRUE;
 226         ValidCoreCountPerPackage[Index]++;
 227       }
 228     }
 229   }
 230   FreePool (CoresVisited);
 231
 232   for (Index = 0; Index <= Package; Index++) {
 233     DEBUG ((DEBUG_INFO, "Package: %d, Valid Core : %d\n", Index, ValidCoreCountPerPackage[Index]));
 234   }
 235
 236   CpuFeaturesData->CpuFlags.CoreSemaphoreCount = AllocateZeroPool (sizeof (UINT32) * CpuStatus->PackageCount * CpuStatus->MaxCoreCount * CpuStatus->MaxThreadCount);
 237   ASSERT (CpuFeaturesData->CpuFlags.CoreSemaphoreCount != NULL);
 238   CpuFeaturesData->CpuFlags.PackageSemaphoreCount = AllocateZeroPool (sizeof (UINT32) * CpuStatus->PackageCount * CpuStatus->MaxCoreCount * CpuStatus->MaxThreadCount);
 239   ASSERT (CpuFeaturesData->CpuFlags.PackageSemaphoreCount != NULL);
 240
 241   //
 242   // Initialize CpuFeaturesData->InitOrder[].CpuInfo.First
 243   //
 244
 245   //
 246   // Set First.Package for each thread belonging to the first package.
 247   //
 248   First = MAX_UINT32;
 249   for (ProcessorNumber = 0; ProcessorNumber < NumberOfCpus; ProcessorNumber++) {
 250     Location = &CpuFeaturesData->InitOrder[ProcessorNumber].CpuInfo.ProcessorInfo.Location;
 251     First = MIN (Location->Package, First);
 252   }
 253   for (ProcessorNumber = 0; ProcessorNumber < NumberOfCpus; ProcessorNumber++) {
 254     Location = &CpuFeaturesData->InitOrder[ProcessorNumber].CpuInfo.ProcessorInfo.Location;
 255     if (Location->Package == First) {
 256       CpuFeaturesData->InitOrder[ProcessorNumber].CpuInfo.First.Package = 1;
 257     }
 258   }
 259
 260   //
 261   // Set First.Die/Tile/Module for each thread assuming:
 262   //  single Die under each package, single Tile under each Die, single Module under each Tile
 263   //
 264   for (ProcessorNumber = 0; ProcessorNumber < NumberOfCpus; ProcessorNumber++) {
 265     CpuFeaturesData->InitOrder[ProcessorNumber].CpuInfo.First.Die = 1;
 266     CpuFeaturesData->InitOrder[ProcessorNumber].CpuInfo.First.Tile = 1;
 267     CpuFeaturesData->InitOrder[ProcessorNumber].CpuInfo.First.Module = 1;
 268   }
 269
 270   for (PackageIndex = 0; PackageIndex < CpuStatus->PackageCount; PackageIndex++) {
 271     //
 272     // Set First.Core for each thread in the first core of each package.
 273     //
 274     First = MAX_UINT32;
 275     for (ProcessorNumber = 0; ProcessorNumber < NumberOfCpus; ProcessorNumber++) {
 276       Location = &CpuFeaturesData->InitOrder[ProcessorNumber].CpuInfo.ProcessorInfo.Location;
 277       if (Location->Package == PackageIndex) {
 278         First = MIN (Location->Core, First);
 279       }
 280     }
 281
 282     for (ProcessorNumber = 0; ProcessorNumber < NumberOfCpus; ProcessorNumber++) {
 283       Location = &CpuFeaturesData->InitOrder[ProcessorNumber].CpuInfo.ProcessorInfo.Location;
 284       if (Location->Package == PackageIndex && Location->Core == First) {
 285         CpuFeaturesData->InitOrder[ProcessorNumber].CpuInfo.First.Core = 1;
 286       }
 287     }
 288   }
 289
 290   for (PackageIndex = 0; PackageIndex < CpuStatus->PackageCount; PackageIndex++) {
 291     for (CoreIndex = 0; CoreIndex < CpuStatus->MaxCoreCount; CoreIndex++) {
 292       //
 293       // Set First.Thread for the first thread of each core.
 294       //
 295       First = MAX_UINT32;
 296       for (ProcessorNumber = 0; ProcessorNumber < NumberOfCpus; ProcessorNumber++) {
 297         Location = &CpuFeaturesData->InitOrder[ProcessorNumber].CpuInfo.ProcessorInfo.Location;
 298         if (Location->Package == PackageIndex && Location->Core == CoreIndex) {
 299           First = MIN (Location->Thread, First);
 300         }
 301       }
 302
 303       for (ProcessorNumber = 0; ProcessorNumber < NumberOfCpus; ProcessorNumber++) {
 304         Location = &CpuFeaturesData->InitOrder[ProcessorNumber].CpuInfo.ProcessorInfo.Location;
 305         if (Location->Package == PackageIndex && Location->Core == CoreIndex && Location->Thread == First) {
 306           CpuFeaturesData->InitOrder[ProcessorNumber].CpuInfo.First.Thread = 1;
 307         }
 308       }
 309     }
 310   }
 311 }
 312
 313 /**
 314   Worker function to do OR operation on CPU feature supported bits mask buffer.
 315
 316   @param[in]  SupportedFeatureMask  The pointer to CPU feature bits mask buffer
 317   @param[in]  OrFeatureBitMask      The feature bit mask to do OR operation
 318   @param[in]  BitMaskSize           The CPU feature bits mask buffer size.
 319
 320 **/
 321 VOID
 322 SupportedMaskOr (
 323   IN UINT8               *SupportedFeatureMask,
 324   IN UINT8               *OrFeatureBitMask,
 325   IN UINT32              BitMaskSize
 326   )
 327 {
 328   UINTN                  Index;
 329   UINT8                  *Data1;
 330   UINT8                  *Data2;
 331
 332   Data1 = SupportedFeatureMask;
 333   Data2 = OrFeatureBitMask;
 334   for (Index = 0; Index < BitMaskSize; Index++) {
 335     *(Data1++) |=  *(Data2++);
 336   }
 337 }
 338
 339 /**
 340   Worker function to do AND operation on CPU feature supported bits mask buffer.
 341
 342   @param[in]  SupportedFeatureMask  The pointer to CPU feature bits mask buffer
 343   @param[in]  AndFeatureBitMask     The feature bit mask to do AND operation
 344   @param[in]  BitMaskSize           CPU feature bits mask buffer size.
 345
 346 **/
 347 VOID
 348 SupportedMaskAnd (
 349   IN       UINT8               *SupportedFeatureMask,
 350   IN CONST UINT8               *AndFeatureBitMask,
 351   IN       UINT32              BitMaskSize
 352   )
 353 {
 354   UINTN                  Index;
 355   UINT8                  *Data1;
 356   CONST UINT8            *Data2;
 357
 358   Data1 = SupportedFeatureMask;
 359   Data2 = AndFeatureBitMask;
 360   for (Index = 0; Index < BitMaskSize; Index++) {
 361     *(Data1++) &=  *(Data2++);
 362   }
 363 }
 364
 365 /**
 366   Worker function to clean bit operation on CPU feature supported bits mask buffer.
 367
 368   @param[in]  SupportedFeatureMask  The pointer to CPU feature bits mask buffer
 369   @param[in]  AndFeatureBitMask     The feature bit mask to do XOR operation
 370   @param[in]  BitMaskSize           CPU feature bits mask buffer size.
 371 **/
 372 VOID
 373 SupportedMaskCleanBit (
 374   IN UINT8               *SupportedFeatureMask,
 375   IN UINT8               *AndFeatureBitMask,
 376   IN UINT32              BitMaskSize
 377   )
 378 {
 379   UINTN                  Index;
 380   UINT8                  *Data1;
 381   UINT8                  *Data2;
 382
 383   Data1 = SupportedFeatureMask;
 384   Data2 = AndFeatureBitMask;
 385   for (Index = 0; Index < BitMaskSize; Index++) {
 386     *(Data1++) &=  ~(*(Data2++));
 387   }
 388 }
 389
 390 /**
 391   Worker function to check if the compared CPU feature set in the CPU feature
 392   supported bits mask buffer.
 393
 394   @param[in]  SupportedFeatureMask   The pointer to CPU feature bits mask buffer
 395   @param[in]  ComparedFeatureBitMask The feature bit mask to be compared
 396   @param[in]  BitMaskSize            CPU feature bits mask buffer size.
 397
 398   @retval TRUE   The ComparedFeatureBitMask is set in CPU feature supported bits
 399                  mask buffer.
 400   @retval FALSE  The ComparedFeatureBitMask is not set in CPU feature supported bits
 401                  mask buffer.
 402 **/
 403 BOOLEAN
 404 IsBitMaskMatch (
 405   IN UINT8               *SupportedFeatureMask,
 406   IN UINT8               *ComparedFeatureBitMask,
 407   IN UINT32              BitMaskSize
 408   )
 409 {
 410   UINTN                  Index;
 411   UINT8                  *Data1;
 412   UINT8                  *Data2;
 413
 414   Data1 = SupportedFeatureMask;
 415   Data2 = ComparedFeatureBitMask;
 416   for (Index = 0; Index < BitMaskSize; Index++) {
 417     if (((*(Data1++)) & (*(Data2++))) != 0) {
 418       return TRUE;
 419     }
 420   }
 421   return FALSE;
 422 }
 423
 424 /**
 425   Collects processor data for calling processor.
 426
 427   @param[in,out]  Buffer  The pointer to private data buffer.
 428 **/
 429 VOID
 430 EFIAPI
 431 CollectProcessorData (
 432   IN OUT VOID                          *Buffer
 433   )
 434 {
 435   UINTN                                ProcessorNumber;
 436   CPU_FEATURES_ENTRY                   *CpuFeature;
 437   REGISTER_CPU_FEATURE_INFORMATION     *CpuInfo;
 438   LIST_ENTRY                           *Entry;
 439   CPU_FEATURES_DATA                    *CpuFeaturesData;
 440
 441   CpuFeaturesData = (CPU_FEATURES_DATA *)Buffer;
 442   ProcessorNumber = GetProcessorIndex (CpuFeaturesData);
 443   CpuInfo = &CpuFeaturesData->InitOrder[ProcessorNumber].CpuInfo;
 444   //
 445   // collect processor information
 446   //
 447   FillProcessorInfo (CpuInfo);
 448   Entry = GetFirstNode (&CpuFeaturesData->FeatureList);
 449   while (!IsNull (&CpuFeaturesData->FeatureList, Entry)) {
 450     CpuFeature = CPU_FEATURE_ENTRY_FROM_LINK (Entry);
 451     if (CpuFeature->SupportFunc == NULL) {
 452       //
 453       // If SupportFunc is NULL, then the feature is supported.
 454       //
 455       SupportedMaskOr (
 456         CpuFeaturesData->InitOrder[ProcessorNumber].FeaturesSupportedMask,
 457         CpuFeature->FeatureMask,
 458         CpuFeaturesData->BitMaskSize
 459         );
 460     } else if (CpuFeature->SupportFunc (ProcessorNumber, CpuInfo, CpuFeature->ConfigData)) {
 461       SupportedMaskOr (
 462         CpuFeaturesData->InitOrder[ProcessorNumber].FeaturesSupportedMask,
 463         CpuFeature->FeatureMask,
 464         CpuFeaturesData->BitMaskSize
 465         );
 466     }
 467     Entry = Entry->ForwardLink;
 468   }
 469 }
 470
 471 /**
 472   Dump the contents of a CPU register table.
 473
 474   @param[in]  ProcessorNumber  The index of the CPU to show the register table contents
 475
 476   @note This service could be called by BSP only.
 477 **/
 478 VOID
 479 DumpRegisterTableOnProcessor (
 480   IN UINTN                             ProcessorNumber
 481   )
 482 {
 483   CPU_FEATURES_DATA                    *CpuFeaturesData;
 484   UINTN                                FeatureIndex;
 485   CPU_REGISTER_TABLE                   *RegisterTable;
 486   CPU_REGISTER_TABLE_ENTRY             *RegisterTableEntry;
 487   CPU_REGISTER_TABLE_ENTRY             *RegisterTableEntryHead;
 488   UINT32                               DebugPrintErrorLevel;
 489
 490   DebugPrintErrorLevel = (ProcessorNumber == 0) ? DEBUG_INFO : DEBUG_VERBOSE;
 491   CpuFeaturesData      = GetCpuFeaturesData ();
 492   //
 493   // Debug information
 494   //
 495   RegisterTable = &CpuFeaturesData->RegisterTable[ProcessorNumber];
 496   DEBUG ((DebugPrintErrorLevel, "RegisterTable->TableLength = %d\n", RegisterTable->TableLength));
 497
 498   RegisterTableEntryHead = (CPU_REGISTER_TABLE_ENTRY *) (UINTN) RegisterTable->RegisterTableEntry;
 499
 500   for (FeatureIndex = 0; FeatureIndex < RegisterTable->TableLength; FeatureIndex++) {
 501     RegisterTableEntry = &RegisterTableEntryHead[FeatureIndex];
 502     switch (RegisterTableEntry->RegisterType) {
 503     case Msr:
 504       DEBUG ((
 505         DebugPrintErrorLevel,
 506         "Processor: %04d: Index %04d, MSR  : %08x, Bit Start: %02d, Bit Length: %02d, Value: %016lx\r\n",
 507         (UINT32) ProcessorNumber,
 508         (UINT32) FeatureIndex,
 509         RegisterTableEntry->Index,
 510         RegisterTableEntry->ValidBitStart,
 511         RegisterTableEntry->ValidBitLength,
 512         RegisterTableEntry->Value
 513         ));
 514       break;
 515     case ControlRegister:
 516       DEBUG ((
 517         DebugPrintErrorLevel,
 518         "Processor: %04d: Index %04d, CR   : %08x, Bit Start: %02d, Bit Length: %02d, Value: %016lx\r\n",
 519         (UINT32) ProcessorNumber,
 520         (UINT32) FeatureIndex,
 521         RegisterTableEntry->Index,
 522         RegisterTableEntry->ValidBitStart,
 523         RegisterTableEntry->ValidBitLength,
 524         RegisterTableEntry->Value
 525         ));
 526       break;
 527     case MemoryMapped:
 528       DEBUG ((
 529         DebugPrintErrorLevel,
 530         "Processor: %04d: Index %04d, MMIO : %016lx, Bit Start: %02d, Bit Length: %02d, Value: %016lx\r\n",
 531         (UINT32) ProcessorNumber,
 532         (UINT32) FeatureIndex,
 533         RegisterTableEntry->Index | LShiftU64 (RegisterTableEntry->HighIndex, 32),
 534         RegisterTableEntry->ValidBitStart,
 535         RegisterTableEntry->ValidBitLength,
 536         RegisterTableEntry->Value
 537         ));
 538       break;
 539     case CacheControl:
 540       DEBUG ((
 541         DebugPrintErrorLevel,
 542         "Processor: %04d: Index %04d, CACHE: %08x, Bit Start: %02d, Bit Length: %02d, Value: %016lx\r\n",
 543         (UINT32) ProcessorNumber,
 544         (UINT32) FeatureIndex,
 545         RegisterTableEntry->Index,
 546         RegisterTableEntry->ValidBitStart,
 547         RegisterTableEntry->ValidBitLength,
 548         RegisterTableEntry->Value
 549         ));
 550       break;
 551     case Semaphore:
 552       DEBUG ((
 553         DebugPrintErrorLevel,
 554         "Processor: %04d: Index %04d, SEMAP: %s\r\n",
 555         (UINT32) ProcessorNumber,
 556         (UINT32) FeatureIndex,
 557         mDependTypeStr[MIN ((UINT32)RegisterTableEntry->Value, InvalidDepType)]
 558         ));
 559       break;
 560
 561     default:
 562       break;
 563     }
 564   }
 565 }
 566
 567 /**
 568   Get the biggest dependence type.
 569   PackageDepType > CoreDepType > ThreadDepType > NoneDepType.
 570
 571   @param[in]  BeforeDep           Before dependence type.
 572   @param[in]  AfterDep            After dependence type.
 573   @param[in]  NoneNeibBeforeDep   Before dependence type for not neighborhood features.
 574   @param[in]  NoneNeibAfterDep    After dependence type for not neighborhood features.
 575
 576   @retval  Return the biggest dependence type.
 577 **/
 578 CPU_FEATURE_DEPENDENCE_TYPE
 579 BiggestDep (
 580   IN CPU_FEATURE_DEPENDENCE_TYPE  BeforeDep,
 581   IN CPU_FEATURE_DEPENDENCE_TYPE  AfterDep,
 582   IN CPU_FEATURE_DEPENDENCE_TYPE  NoneNeibBeforeDep,
 583   IN CPU_FEATURE_DEPENDENCE_TYPE  NoneNeibAfterDep
 584   )
 585 {
 586   CPU_FEATURE_DEPENDENCE_TYPE Bigger;
 587
 588   Bigger = MAX (BeforeDep, AfterDep);
 589   Bigger = MAX (Bigger, NoneNeibBeforeDep);
 590   return MAX(Bigger, NoneNeibAfterDep);
 591 }
 592
 593 /**
 594   Analysis register CPU features on each processor and save CPU setting in CPU register table.
 595
 596   @param[in]  NumberOfCpus  Number of processor in system
 597
 598 **/
 599 VOID
 600 AnalysisProcessorFeatures (
 601   IN UINTN                             NumberOfCpus
 602   )
 603 {
 604   EFI_STATUS                           Status;
 605   UINTN                                ProcessorNumber;
 606   CPU_FEATURES_ENTRY                   *CpuFeature;
 607   CPU_FEATURES_ENTRY                   *CpuFeatureInOrder;
 608   CPU_FEATURES_INIT_ORDER              *CpuInitOrder;
 609   REGISTER_CPU_FEATURE_INFORMATION     *CpuInfo;
 610   LIST_ENTRY                           *Entry;
 611   CPU_FEATURES_DATA                    *CpuFeaturesData;
 612   LIST_ENTRY                           *NextEntry;
 613   CPU_FEATURES_ENTRY                   *NextCpuFeatureInOrder;
 614   BOOLEAN                              Success;
 615   CPU_FEATURE_DEPENDENCE_TYPE          BeforeDep;
 616   CPU_FEATURE_DEPENDENCE_TYPE          AfterDep;
 617   CPU_FEATURE_DEPENDENCE_TYPE          NoneNeibBeforeDep;
 618   CPU_FEATURE_DEPENDENCE_TYPE          NoneNeibAfterDep;
 619
 620   CpuFeaturesData = GetCpuFeaturesData ();
 621   CpuFeaturesData->CapabilityPcd = AllocatePool (CpuFeaturesData->BitMaskSize);
 622   ASSERT (CpuFeaturesData->CapabilityPcd != NULL);
 623   SetMem (CpuFeaturesData->CapabilityPcd, CpuFeaturesData->BitMaskSize, 0xFF);
 624   for (ProcessorNumber = 0; ProcessorNumber < NumberOfCpus; ProcessorNumber++) {
 625     CpuInitOrder = &CpuFeaturesData->InitOrder[ProcessorNumber];
 626     //
 627     // Calculate the last capability on all processors
 628     //
 629     SupportedMaskAnd (CpuFeaturesData->CapabilityPcd, CpuInitOrder->FeaturesSupportedMask, CpuFeaturesData->BitMaskSize);
 630   }
 631   //
 632   // Calculate the last setting
 633   //
 634   CpuFeaturesData->SettingPcd = AllocateCopyPool (CpuFeaturesData->BitMaskSize, CpuFeaturesData->CapabilityPcd);
 635   ASSERT (CpuFeaturesData->SettingPcd != NULL);
 636   SupportedMaskAnd (CpuFeaturesData->SettingPcd, PcdGetPtr (PcdCpuFeaturesSetting), CpuFeaturesData->BitMaskSize);
 637
 638   //
 639   // Dump the last CPU feature list
 640   //
 641   DEBUG_CODE (
 642     DEBUG ((DEBUG_INFO, "Last CPU features list...\n"));
 643     Entry = GetFirstNode (&CpuFeaturesData->FeatureList);
 644     while (!IsNull (&CpuFeaturesData->FeatureList, Entry)) {
 645       CpuFeature = CPU_FEATURE_ENTRY_FROM_LINK (Entry);
 646       if (IsBitMaskMatch (CpuFeature->FeatureMask, CpuFeaturesData->CapabilityPcd, CpuFeaturesData->BitMaskSize)) {
 647         if (IsBitMaskMatch (CpuFeature->FeatureMask, CpuFeaturesData->SettingPcd, CpuFeaturesData->BitMaskSize)) {
 648           DEBUG ((DEBUG_INFO, "[Enable   ] "));
 649         } else {
 650           DEBUG ((DEBUG_INFO, "[Disable  ] "));
 651         }
 652       } else {
 653         DEBUG ((DEBUG_INFO, "[Unsupport] "));
 654       }
 655       DumpCpuFeature (CpuFeature, CpuFeaturesData->BitMaskSize);
 656       Entry = Entry->ForwardLink;
 657     }
 658     DEBUG ((DEBUG_INFO, "PcdCpuFeaturesCapability:\n"));
 659     DumpCpuFeatureMask (CpuFeaturesData->CapabilityPcd, CpuFeaturesData->BitMaskSize);
 660     DEBUG ((DEBUG_INFO, "Origin PcdCpuFeaturesSetting:\n"));
 661     DumpCpuFeatureMask (PcdGetPtr (PcdCpuFeaturesSetting), CpuFeaturesData->BitMaskSize);
 662     DEBUG ((DEBUG_INFO, "Final PcdCpuFeaturesSetting:\n"));
 663     DumpCpuFeatureMask (CpuFeaturesData->SettingPcd, CpuFeaturesData->BitMaskSize);
 664   );
 665
 666   //
 667   // Save PCDs and display CPU PCDs
 668   //
 669   SetCapabilityPcd (CpuFeaturesData->CapabilityPcd, CpuFeaturesData->BitMaskSize);
 670   SetSettingPcd (CpuFeaturesData->SettingPcd, CpuFeaturesData->BitMaskSize);
 671
 672   for (ProcessorNumber = 0; ProcessorNumber < NumberOfCpus; ProcessorNumber++) {
 673     CpuInitOrder = &CpuFeaturesData->InitOrder[ProcessorNumber];
 674     Entry = GetFirstNode (&CpuFeaturesData->FeatureList);
 675     while (!IsNull (&CpuFeaturesData->FeatureList, Entry)) {
 676       //
 677       // Insert each feature into processor's order list
 678       //
 679       CpuFeature = CPU_FEATURE_ENTRY_FROM_LINK (Entry);
 680       if (IsBitMaskMatch (CpuFeature->FeatureMask, CpuFeaturesData->CapabilityPcd, CpuFeaturesData->BitMaskSize)) {
 681         CpuFeatureInOrder = AllocateCopyPool (sizeof (CPU_FEATURES_ENTRY), CpuFeature);
 682         ASSERT (CpuFeatureInOrder != NULL);
 683         InsertTailList (&CpuInitOrder->OrderList, &CpuFeatureInOrder->Link);
 684       }
 685       Entry = Entry->ForwardLink;
 686     }
 687     //
 688     // Go through ordered feature list to initialize CPU features
 689     //
 690     CpuInfo = &CpuFeaturesData->InitOrder[ProcessorNumber].CpuInfo;
 691     Entry = GetFirstNode (&CpuInitOrder->OrderList);
 692     while (!IsNull (&CpuInitOrder->OrderList, Entry)) {
 693       CpuFeatureInOrder = CPU_FEATURE_ENTRY_FROM_LINK (Entry);
 694
 695       Success = FALSE;
 696       if (IsBitMaskMatch (CpuFeatureInOrder->FeatureMask, CpuFeaturesData->SettingPcd, CpuFeaturesData->BitMaskSize)) {
 697         Status = CpuFeatureInOrder->InitializeFunc (ProcessorNumber, CpuInfo, CpuFeatureInOrder->ConfigData, TRUE);
 698         if (EFI_ERROR (Status)) {
 699           //
 700           // Clean the CpuFeatureInOrder->FeatureMask in setting PCD.
 701           //
 702           SupportedMaskCleanBit (CpuFeaturesData->SettingPcd, CpuFeatureInOrder->FeatureMask, CpuFeaturesData->BitMaskSize);
 703           if (CpuFeatureInOrder->FeatureName != NULL) {
 704             DEBUG ((DEBUG_WARN, "Warning :: Failed to enable Feature: Name = %a.\n", CpuFeatureInOrder->FeatureName));
 705           } else {
 706             DEBUG ((DEBUG_WARN, "Warning :: Failed to enable Feature: Mask = "));
 707             DumpCpuFeatureMask (CpuFeatureInOrder->FeatureMask, CpuFeaturesData->BitMaskSize);
 708           }
 709         } else {
 710           Success = TRUE;
 711         }
 712       } else {
 713         Status = CpuFeatureInOrder->InitializeFunc (ProcessorNumber, CpuInfo, CpuFeatureInOrder->ConfigData, FALSE);
 714         if (EFI_ERROR (Status)) {
 715           if (CpuFeatureInOrder->FeatureName != NULL) {
 716             DEBUG ((DEBUG_WARN, "Warning :: Failed to disable Feature: Name = %a.\n", CpuFeatureInOrder->FeatureName));
 717           } else {
 718             DEBUG ((DEBUG_WARN, "Warning :: Failed to disable Feature: Mask = "));
 719             DumpCpuFeatureMask (CpuFeatureInOrder->FeatureMask, CpuFeaturesData->BitMaskSize);
 720           }
 721         } else {
 722           Success = TRUE;
 723         }
 724       }
 725
 726       if (Success) {
 727         NextEntry = Entry->ForwardLink;
 728         if (!IsNull (&CpuInitOrder->OrderList, NextEntry)) {
 729           NextCpuFeatureInOrder = CPU_FEATURE_ENTRY_FROM_LINK (NextEntry);
 730
 731           //
 732           // If feature has dependence with the next feature (ONLY care core/package dependency).
 733           // and feature initialize succeed, add sync semaphere here.
 734           //
 735           BeforeDep = DetectFeatureScope (CpuFeatureInOrder, TRUE, NextCpuFeatureInOrder->FeatureMask);
 736           AfterDep  = DetectFeatureScope (NextCpuFeatureInOrder, FALSE, CpuFeatureInOrder->FeatureMask);
 737           //
 738           // Check whether next feature has After type dependence with not neighborhood CPU
 739           // Features in former CPU features.
 740           //
 741           NoneNeibAfterDep = DetectNoneNeighborhoodFeatureScope(NextCpuFeatureInOrder, FALSE, &CpuInitOrder->OrderList);
 742         } else {
 743           BeforeDep        = NoneDepType;
 744           AfterDep         = NoneDepType;
 745           NoneNeibAfterDep = NoneDepType;
 746         }
 747         //
 748         // Check whether current feature has Before type dependence with none neighborhood
 749         // CPU features in after Cpu features.
 750         //
 751         NoneNeibBeforeDep = DetectNoneNeighborhoodFeatureScope(CpuFeatureInOrder, TRUE, &CpuInitOrder->OrderList);
 752
 753         //
 754         // Get the biggest dependence and add semaphore for it.
 755         // PackageDepType > CoreDepType > ThreadDepType > NoneDepType.
 756         //
 757         BeforeDep = BiggestDep(BeforeDep, AfterDep, NoneNeibBeforeDep, NoneNeibAfterDep);
 758         if (BeforeDep > ThreadDepType) {
 759           CPU_REGISTER_TABLE_WRITE32 (ProcessorNumber, Semaphore, 0, BeforeDep);
 760         }
 761       }
 762
 763       Entry = Entry->ForwardLink;
 764     }
 765
 766     //
 767     // Dump PcdCpuFeaturesSetting again because this value maybe updated
 768     // again during initialize the features.
 769     //
 770     DEBUG ((DEBUG_INFO, "Dump final value for PcdCpuFeaturesSetting:\n"));
 771     DumpCpuFeatureMask (CpuFeaturesData->SettingPcd, CpuFeaturesData->BitMaskSize);
 772
 773     //
 774     // Dump the RegisterTable
 775     //
 776     DumpRegisterTableOnProcessor (ProcessorNumber);
 777   }
 778 }
 779
 780 /**
 781   Increment semaphore by 1.
 782
 783   @param      Sem            IN:  32-bit unsigned integer
 784
 785 **/
 786 VOID
 787 LibReleaseSemaphore (
 788   IN OUT  volatile UINT32           *Sem
 789   )
 790 {
 791   InterlockedIncrement (Sem);
 792 }
 793
 794 /**
 795   Decrement the semaphore by 1 if it is not zero.
 796
 797   Performs an atomic decrement operation for semaphore.
 798   The compare exchange operation must be performed using
 799   MP safe mechanisms.
 800
 801   @param      Sem            IN:  32-bit unsigned integer
 802
 803 **/
 804 VOID
 805 LibWaitForSemaphore (
 806   IN OUT  volatile UINT32           *Sem
 807   )
 808 {
 809   UINT32  Value;
 810
 811   do {
 812     Value = *Sem;
 813   } while (Value == 0 ||
 814            InterlockedCompareExchange32 (
 815              Sem,
 816              Value,
 817              Value - 1
 818              ) != Value);
 819 }
 820
 821 /**
 822   Read / write CR value.
 823
 824   @param[in]      CrIndex         The CR index which need to read/write.
 825   @param[in]      Read            Read or write. TRUE is read.
 826   @param[in,out]  CrValue         CR value.
 827
 828   @retval    EFI_SUCCESS means read/write success, else return EFI_UNSUPPORTED.
 829 **/
 830 UINTN
 831 ReadWriteCr (
 832   IN     UINT32       CrIndex,
 833   IN     BOOLEAN      Read,
 834   IN OUT UINTN        *CrValue
 835   )
 836 {
 837   switch (CrIndex) {
 838   case 0:
 839     if (Read) {
 840       *CrValue = AsmReadCr0 ();
 841     } else {
 842       AsmWriteCr0 (*CrValue);
 843     }
 844     break;
 845   case 2:
 846     if (Read) {
 847       *CrValue = AsmReadCr2 ();
 848     } else {
 849       AsmWriteCr2 (*CrValue);
 850     }
 851     break;
 852   case 3:
 853     if (Read) {
 854       *CrValue = AsmReadCr3 ();
 855     } else {
 856       AsmWriteCr3 (*CrValue);
 857     }
 858     break;
 859   case 4:
 860     if (Read) {
 861       *CrValue = AsmReadCr4 ();
 862     } else {
 863       AsmWriteCr4 (*CrValue);
 864     }
 865     break;
 866   default:
 867     return EFI_UNSUPPORTED;;
 868   }
 869
 870   return EFI_SUCCESS;
 871 }
 872
 873 /**
 874   Initialize the CPU registers from a register table.
 875
 876   @param[in]  RegisterTable         The register table for this AP.
 877   @param[in]  ApLocation            AP location info for this ap.
 878   @param[in]  CpuStatus             CPU status info for this CPU.
 879   @param[in]  CpuFlags              Flags data structure used when program the register.
 880
 881   @note This service could be called by BSP/APs.
 882 **/
 883 VOID
 884 ProgramProcessorRegister (
 885   IN CPU_REGISTER_TABLE           *RegisterTable,
 886   IN EFI_CPU_PHYSICAL_LOCATION    *ApLocation,
 887   IN CPU_STATUS_INFORMATION       *CpuStatus,
 888   IN PROGRAM_CPU_REGISTER_FLAGS   *CpuFlags
 889   )
 890 {
 891   CPU_REGISTER_TABLE_ENTRY  *RegisterTableEntry;
 892   UINTN                     Index;
 893   UINTN                     Value;
 894   CPU_REGISTER_TABLE_ENTRY  *RegisterTableEntryHead;
 895   volatile UINT32           *SemaphorePtr;
 896   UINT32                    FirstThread;
 897   UINT32                    PackageThreadsCount;
 898   UINT32                    CurrentThread;
 899   UINTN                     ProcessorIndex;
 900   UINTN                     ValidThreadCount;
 901   UINT32                    *ValidCoreCountPerPackage;
 902   EFI_STATUS                Status;
 903   UINT64                    CurrentValue;
 904
 905   //
 906   // Traverse Register Table of this logical processor
 907   //
 908   RegisterTableEntryHead = (CPU_REGISTER_TABLE_ENTRY *) (UINTN) RegisterTable->RegisterTableEntry;
 909
 910   for (Index = 0; Index < RegisterTable->TableLength; Index++) {
 911
 912     RegisterTableEntry = &RegisterTableEntryHead[Index];
 913
 914     //
 915     // Check the type of specified register
 916     //
 917     switch (RegisterTableEntry->RegisterType) {
 918     //
 919     // The specified register is Control Register
 920     //
 921     case ControlRegister:
 922       Status = ReadWriteCr (RegisterTableEntry->Index, TRUE, &Value);
 923       if (EFI_ERROR (Status)) {
 924         break;
 925       }
 926       if (RegisterTableEntry->TestThenWrite) {
 927         CurrentValue = BitFieldRead64 (
 928                          Value,
 929                          RegisterTableEntry->ValidBitStart,
 930                          RegisterTableEntry->ValidBitStart + RegisterTableEntry->ValidBitLength - 1
 931                          );
 932         if (CurrentValue == RegisterTableEntry->Value) {
 933           break;
 934         }
 935       }
 936       Value = (UINTN) BitFieldWrite64 (
 937                         Value,
 938                         RegisterTableEntry->ValidBitStart,
 939                         RegisterTableEntry->ValidBitStart + RegisterTableEntry->ValidBitLength - 1,
 940                         RegisterTableEntry->Value
 941                         );
 942       ReadWriteCr (RegisterTableEntry->Index, FALSE, &Value);
 943       break;
 944
 945     //
 946     // The specified register is Model Specific Register
 947     //
 948     case Msr:
 949       if (RegisterTableEntry->TestThenWrite) {
 950         Value = (UINTN)AsmReadMsr64 (RegisterTableEntry->Index);
 951         if (RegisterTableEntry->ValidBitLength >= 64) {
 952           if (Value == RegisterTableEntry->Value) {
 953             break;
 954           }
 955         } else {
 956           CurrentValue = BitFieldRead64 (
 957                            Value,
 958                            RegisterTableEntry->ValidBitStart,
 959                            RegisterTableEntry->ValidBitStart + RegisterTableEntry->ValidBitLength - 1
 960                            );
 961           if (CurrentValue == RegisterTableEntry->Value) {
 962             break;
 963           }
 964         }
 965       }
 966
 967       if (RegisterTableEntry->ValidBitLength >= 64) {
 968         //
 969         // If length is not less than 64 bits, then directly write without reading
 970         //
 971         AsmWriteMsr64 (
 972           RegisterTableEntry->Index,
 973           RegisterTableEntry->Value
 974           );
 975       } else {
 976         //
 977         // Set the bit section according to bit start and length
 978         //
 979         AsmMsrBitFieldWrite64 (
 980           RegisterTableEntry->Index,
 981           RegisterTableEntry->ValidBitStart,
 982           RegisterTableEntry->ValidBitStart + RegisterTableEntry->ValidBitLength - 1,
 983           RegisterTableEntry->Value
 984           );
 985       }
 986       break;
 987     //
 988     // MemoryMapped operations
 989     //
 990     case MemoryMapped:
 991       AcquireSpinLock (&CpuFlags->MemoryMappedLock);
 992       MmioBitFieldWrite32 (
 993         (UINTN)(RegisterTableEntry->Index | LShiftU64 (RegisterTableEntry->HighIndex, 32)),
 994         RegisterTableEntry->ValidBitStart,
 995         RegisterTableEntry->ValidBitStart + RegisterTableEntry->ValidBitLength - 1,
 996         (UINT32)RegisterTableEntry->Value
 997         );
 998       ReleaseSpinLock (&CpuFlags->MemoryMappedLock);
 999       break;
1000     //
1001     // Enable or disable cache
1002     //
1003     case CacheControl:
1004       //
1005       // If value of the entry is 0, then disable cache.  Otherwise, enable cache.
1006       //
1007       if (RegisterTableEntry->Value == 0) {
1008         AsmDisableCache ();
1009       } else {
1010         AsmEnableCache ();
1011       }
1012       break;
1013
1014     case Semaphore:
1015       // Semaphore works logic like below:
1016       //
1017       //  V(x) = LibReleaseSemaphore (Semaphore[FirstThread + x]);
1018       //  P(x) = LibWaitForSemaphore (Semaphore[FirstThread + x]);
1019       //
1020       //  All threads (T0...Tn) waits in P() line and continues running
1021       //  together.
1022       //
1023       //
1024       //  T0             T1            ...           Tn
1025       //
1026       //  V(0...n)       V(0...n)      ...           V(0...n)
1027       //  n * P(0)       n * P(1)      ...           n * P(n)
1028       //
1029       switch (RegisterTableEntry->Value) {
1030       case CoreDepType:
1031         SemaphorePtr = CpuFlags->CoreSemaphoreCount;
1032         //
1033         // Get Offset info for the first thread in the core which current thread belongs to.
1034         //
1035         FirstThread = (ApLocation->Package * CpuStatus->MaxCoreCount + ApLocation->Core) * CpuStatus->MaxThreadCount;
1036         CurrentThread = FirstThread + ApLocation->Thread;
1037         //
1038         // First Notify all threads in current Core that this thread has ready.
1039         //
1040         for (ProcessorIndex = 0; ProcessorIndex < CpuStatus->MaxThreadCount; ProcessorIndex ++) {
1041           LibReleaseSemaphore ((UINT32 *) &SemaphorePtr[FirstThread + ProcessorIndex]);
1042         }
1043         //
1044         // Second, check whether all valid threads in current core have ready.
1045         //
1046         for (ProcessorIndex = 0; ProcessorIndex < CpuStatus->MaxThreadCount; ProcessorIndex ++) {
1047           LibWaitForSemaphore (&SemaphorePtr[CurrentThread]);
1048         }
1049         break;
1050
1051       case PackageDepType:
1052         SemaphorePtr = CpuFlags->PackageSemaphoreCount;
1053         ValidCoreCountPerPackage = (UINT32 *)(UINTN)CpuStatus->ValidCoreCountPerPackage;
1054         //
1055         // Get Offset info for the first thread in the package which current thread belongs to.
1056         //
1057         FirstThread = ApLocation->Package * CpuStatus->MaxCoreCount * CpuStatus->MaxThreadCount;
1058         //
1059         // Get the possible threads count for current package.
1060         //
1061         PackageThreadsCount = CpuStatus->MaxThreadCount * CpuStatus->MaxCoreCount;
1062         CurrentThread = FirstThread + CpuStatus->MaxThreadCount * ApLocation->Core + ApLocation->Thread;
1063         //
1064         // Get the valid thread count for current package.
1065         //
1066         ValidThreadCount = CpuStatus->MaxThreadCount * ValidCoreCountPerPackage[ApLocation->Package];
1067
1068         //
1069         // Different packages may have different valid cores in them. If driver maintail clearly
1070         // cores number in different packages, the logic will be much complicated.
1071         // Here driver just simply records the max core number in all packages and use it as expect
1072         // core number for all packages.
1073         // In below two steps logic, first current thread will Release semaphore for each thread
1074         // in current package. Maybe some threads are not valid in this package, but driver don't
1075         // care. Second, driver will let current thread wait semaphore for all valid threads in
1076         // current package. Because only the valid threads will do release semaphore for this
1077         // thread, driver here only need to wait the valid thread count.
1078         //
1079
1080         //
1081         // First Notify ALL THREADS in current package that this thread has ready.
1082         //
1083         for (ProcessorIndex = 0; ProcessorIndex < PackageThreadsCount ; ProcessorIndex ++) {
1084           LibReleaseSemaphore ((UINT32 *) &SemaphorePtr[FirstThread + ProcessorIndex]);
1085         }
1086         //
1087         // Second, check whether VALID THREADS (not all threads) in current package have ready.
1088         //
1089         for (ProcessorIndex = 0; ProcessorIndex < ValidThreadCount; ProcessorIndex ++) {
1090           LibWaitForSemaphore (&SemaphorePtr[CurrentThread]);
1091         }
1092         break;
1093
1094       default:
1095         break;
1096       }
1097       break;
1098
1099     default:
1100       break;
1101     }
1102   }
1103 }
1104
1105 /**
1106   Programs registers for the calling processor.
1107
1108   @param[in,out] Buffer  The pointer to private data buffer.
1109
1110 **/
1111 VOID
1112 EFIAPI
1113 SetProcessorRegister (
1114   IN OUT VOID            *Buffer
1115   )
1116 {
1117   CPU_FEATURES_DATA         *CpuFeaturesData;
1118   CPU_REGISTER_TABLE        *RegisterTable;
1119   CPU_REGISTER_TABLE        *RegisterTables;
1120   UINT32                    InitApicId;
1121   UINTN                     ProcIndex;
1122   UINTN                     Index;
1123   ACPI_CPU_DATA             *AcpiCpuData;
1124
1125   CpuFeaturesData = (CPU_FEATURES_DATA *) Buffer;
1126   AcpiCpuData = CpuFeaturesData->AcpiCpuData;
1127
1128   RegisterTables = (CPU_REGISTER_TABLE *)(UINTN)AcpiCpuData->RegisterTable;
1129
1130   InitApicId = GetInitialApicId ();
1131   RegisterTable = NULL;
1132   ProcIndex = (UINTN)-1;
1133   for (Index = 0; Index < AcpiCpuData->NumberOfCpus; Index++) {
1134     if (RegisterTables[Index].InitialApicId == InitApicId) {
1135       RegisterTable =  &RegisterTables[Index];
1136       ProcIndex = Index;
1137       break;
1138     }
1139   }
1140   ASSERT (RegisterTable != NULL);
1141
1142   ProgramProcessorRegister (
1143     RegisterTable,
1144     (EFI_CPU_PHYSICAL_LOCATION *)(UINTN)AcpiCpuData->ApLocation + ProcIndex,
1145     &AcpiCpuData->CpuStatus,
1146     &CpuFeaturesData->CpuFlags
1147     );
1148 }
1149
1150 /**
1151   Performs CPU features detection.
1152
1153   This service will invoke MP service to check CPU features'
1154   capabilities on BSP/APs.
1155
1156   @note This service could be called by BSP only.
1157 **/
1158 VOID
1159 EFIAPI
1160 CpuFeaturesDetect (
1161   VOID
1162   )
1163 {
1164   CPU_FEATURES_DATA      *CpuFeaturesData;
1165
1166   CpuFeaturesData = GetCpuFeaturesData();
1167
1168   CpuInitDataInitialize ();
1169
1170   if (CpuFeaturesData->NumberOfCpus > 1) {
1171     //
1172     // Wakeup all APs for data collection.
1173     //
1174     StartupAllAPsWorker (CollectProcessorData, NULL);
1175   }
1176
1177   //
1178   // Collect data on BSP
1179   //
1180   CollectProcessorData (CpuFeaturesData);
1181
1182   AnalysisProcessorFeatures (CpuFeaturesData->NumberOfCpus);
1183 }
1184