UefiCpuPkg/Library/RegisterCpuFeaturesLib/CpuFeaturesInitialize.c

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