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