UefiCpuPkg/Library/RegisterCpuFeaturesLib/CpuFeaturesInitialize.c

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