[mirror_edk2.git] / UefiCpuPkg / Library / RegisterCpuFeaturesLib / CpuFeaturesInitialize.c

/** @file\r
  CPU Features Initialize functions.\r
\r
  Copyright (c) 2017 - 2021, Intel Corporation. All rights reserved.<BR>\r
  SPDX-License-Identifier: BSD-2-Clause-Patent\r
\r
**/\r
\r
#include "RegisterCpuFeatures.h"\r
\r
CHAR16 *mDependTypeStr[]   = {L"None", L"Thread", L"Core", L"Package", L"Invalid" };\r
\r
/**\r
  Worker function to save PcdCpuFeaturesCapability.\r
\r
  @param[in]  SupportedFeatureMask  The pointer to CPU feature bits mask buffer\r
  @param[in]  BitMaskSize           CPU feature bits mask buffer size.\r
\r
**/\r
VOID\r
SetCapabilityPcd (\r
  IN UINT8               *SupportedFeatureMask,\r
  IN UINTN               BitMaskSize\r
  )\r
{\r
  EFI_STATUS             Status;\r
\r
  Status = PcdSetPtrS (PcdCpuFeaturesCapability, &BitMaskSize, SupportedFeatureMask);\r
  ASSERT_EFI_ERROR (Status);\r
}\r
\r
/**\r
  Worker function to save PcdCpuFeaturesSetting.\r
\r
  @param[in]  SupportedFeatureMask  The pointer to CPU feature bits mask buffer\r
  @param[in]  BitMaskSize           CPU feature bits mask buffer size.\r
**/\r
VOID\r
SetSettingPcd (\r
  IN UINT8               *SupportedFeatureMask,\r
  IN UINTN               BitMaskSize\r
  )\r
{\r
  EFI_STATUS             Status;\r
\r
  Status = PcdSetPtrS (PcdCpuFeaturesSetting, &BitMaskSize, SupportedFeatureMask);\r
  ASSERT_EFI_ERROR (Status);\r
}\r
\r
/**\r
  Collects CPU type and feature information.\r
\r
  @param[in, out]  CpuInfo  The pointer to CPU feature information\r
**/\r
VOID\r
FillProcessorInfo (\r
  IN OUT REGISTER_CPU_FEATURE_INFORMATION        *CpuInfo\r
  )\r
{\r
  CPUID_VERSION_INFO_EAX Eax;\r
  CPUID_VERSION_INFO_ECX Ecx;\r
  CPUID_VERSION_INFO_EDX Edx;\r
  UINT32                 DisplayedFamily;\r
  UINT32                 DisplayedModel;\r
\r
  AsmCpuid (CPUID_VERSION_INFO, &Eax.Uint32, NULL, &Ecx.Uint32, &Edx.Uint32);\r
\r
  DisplayedFamily = Eax.Bits.FamilyId;\r
  if (Eax.Bits.FamilyId == 0x0F) {\r
    DisplayedFamily |= (Eax.Bits.ExtendedFamilyId << 4);\r
  }\r
\r
  DisplayedModel = Eax.Bits.Model;\r
  if (Eax.Bits.FamilyId == 0x06 || Eax.Bits.FamilyId == 0x0f) {\r
    DisplayedModel |= (Eax.Bits.ExtendedModelId << 4);\r
  }\r
\r
  CpuInfo->DisplayFamily = DisplayedFamily;\r
  CpuInfo->DisplayModel  = DisplayedModel;\r
  CpuInfo->SteppingId    = Eax.Bits.SteppingId;\r
  CpuInfo->ProcessorType = Eax.Bits.ProcessorType;\r
  CpuInfo->CpuIdVersionInfoEcx.Uint32 = Ecx.Uint32;\r
  CpuInfo->CpuIdVersionInfoEdx.Uint32 = Edx.Uint32;\r
}\r
\r
/**\r
  Prepares for private data used for CPU features.\r
\r
**/\r
VOID\r
CpuInitDataInitialize (\r
  VOID\r
  )\r
{\r
  EFI_STATUS                           Status;\r
  UINTN                                ProcessorNumber;\r
  EFI_PROCESSOR_INFORMATION            ProcessorInfoBuffer;\r
  CPU_FEATURES_ENTRY                   *CpuFeature;\r
  CPU_FEATURES_INIT_ORDER              *InitOrder;\r
  CPU_FEATURES_DATA                    *CpuFeaturesData;\r
  LIST_ENTRY                           *Entry;\r
  UINT32                               Core;\r
  UINT32                               Package;\r
  UINT32                               Thread;\r
  EFI_CPU_PHYSICAL_LOCATION            *Location;\r
  UINT32                               PackageIndex;\r
  UINT32                               CoreIndex;\r
  UINTN                                Pages;\r
  UINT32                               FirstPackage;\r
  UINT32                               *FirstCore;\r
  UINT32                               *FirstThread;\r
  ACPI_CPU_DATA                        *AcpiCpuData;\r
  CPU_STATUS_INFORMATION               *CpuStatus;\r
  UINT32                               *ThreadCountPerPackage;\r
  UINT8                                *ThreadCountPerCore;\r
  UINTN                                NumberOfCpus;\r
  UINTN                                NumberOfEnabledProcessors;\r
\r
  Core    = 0;\r
  Package = 0;\r
  Thread  = 0;\r
\r
  CpuFeaturesData = GetCpuFeaturesData ();\r
\r
  //\r
  // Initialize CpuFeaturesData->MpService as early as possile, so later function can use it.\r
  //\r
  CpuFeaturesData->MpService = GetMpService ();\r
\r
  GetNumberOfProcessor (&NumberOfCpus, &NumberOfEnabledProcessors);\r
\r
  CpuFeaturesData->InitOrder = AllocatePages (EFI_SIZE_TO_PAGES (sizeof (CPU_FEATURES_INIT_ORDER) * NumberOfCpus));\r
  ASSERT (CpuFeaturesData->InitOrder != NULL);\r
  ZeroMem (CpuFeaturesData->InitOrder, sizeof (CPU_FEATURES_INIT_ORDER) * NumberOfCpus);\r
\r
  //\r
  // Collect CPU Features information\r
  //\r
  Entry = GetFirstNode (&CpuFeaturesData->FeatureList);\r
  while (!IsNull (&CpuFeaturesData->FeatureList, Entry)) {\r
    CpuFeature = CPU_FEATURE_ENTRY_FROM_LINK (Entry);\r
    ASSERT (CpuFeature->InitializeFunc != NULL);\r
    if (CpuFeature->GetConfigDataFunc != NULL) {\r
      CpuFeature->ConfigData = CpuFeature->GetConfigDataFunc (NumberOfCpus);\r
    }\r
    Entry = Entry->ForwardLink;\r
  }\r
\r
  CpuFeaturesData->NumberOfCpus = (UINT32) NumberOfCpus;\r
\r
  AcpiCpuData = GetAcpiCpuData ();\r
  ASSERT (AcpiCpuData != NULL);\r
  CpuFeaturesData->AcpiCpuData= AcpiCpuData;\r
\r
  CpuStatus = &AcpiCpuData->CpuFeatureInitData.CpuStatus;\r
  Location = AllocateZeroPool (sizeof (EFI_CPU_PHYSICAL_LOCATION) * NumberOfCpus);\r
  ASSERT (Location != NULL);\r
  AcpiCpuData->CpuFeatureInitData.ApLocation = (EFI_PHYSICAL_ADDRESS)(UINTN)Location;\r
\r
  for (ProcessorNumber = 0; ProcessorNumber < NumberOfCpus; ProcessorNumber++) {\r
    InitOrder = &CpuFeaturesData->InitOrder[ProcessorNumber];\r
    InitOrder->FeaturesSupportedMask = AllocateZeroPool (CpuFeaturesData->BitMaskSize);\r
    ASSERT (InitOrder->FeaturesSupportedMask != NULL);\r
    InitializeListHead (&InitOrder->OrderList);\r
    Status = GetProcessorInformation (ProcessorNumber, &ProcessorInfoBuffer);\r
    ASSERT_EFI_ERROR (Status);\r
    CopyMem (\r
      &InitOrder->CpuInfo.ProcessorInfo,\r
      &ProcessorInfoBuffer,\r
      sizeof (EFI_PROCESSOR_INFORMATION)\r
      );\r
    CopyMem (\r
      &Location[ProcessorNumber],\r
      &ProcessorInfoBuffer.Location,\r
      sizeof (EFI_CPU_PHYSICAL_LOCATION)\r
      );\r
\r
    //\r
    // Collect CPU package count info.\r
    //\r
    if (Package < ProcessorInfoBuffer.Location.Package) {\r
      Package = ProcessorInfoBuffer.Location.Package;\r
    }\r
    //\r
    // Collect CPU max core count info.\r
    //\r
    if (Core < ProcessorInfoBuffer.Location.Core) {\r
      Core = ProcessorInfoBuffer.Location.Core;\r
    }\r
    //\r
    // Collect CPU max thread count info.\r
    //\r
    if (Thread < ProcessorInfoBuffer.Location.Thread) {\r
      Thread = ProcessorInfoBuffer.Location.Thread;\r
    }\r
  }\r
  CpuStatus->PackageCount    = Package + 1;\r
  CpuStatus->MaxCoreCount    = Core + 1;\r
  CpuStatus->MaxThreadCount  = Thread + 1;\r
  DEBUG ((DEBUG_INFO, "Processor Info: Package: %d, MaxCore : %d, MaxThread: %d\n",\r
         CpuStatus->PackageCount,\r
         CpuStatus->MaxCoreCount,\r
         CpuStatus->MaxThreadCount));\r
\r
  //\r
  // Collect valid core count in each package because not all cores are valid.\r
  //\r
  ThreadCountPerPackage = AllocateZeroPool (sizeof (UINT32) * CpuStatus->PackageCount);\r
  ASSERT (ThreadCountPerPackage != NULL);\r
  CpuStatus->ThreadCountPerPackage = (EFI_PHYSICAL_ADDRESS)(UINTN)ThreadCountPerPackage;\r
\r
  ThreadCountPerCore = AllocateZeroPool (sizeof (UINT8) * CpuStatus->PackageCount * CpuStatus->MaxCoreCount);\r
  ASSERT (ThreadCountPerCore != NULL);\r
  CpuStatus->ThreadCountPerCore = (EFI_PHYSICAL_ADDRESS)(UINTN)ThreadCountPerCore;\r
\r
  for (ProcessorNumber = 0; ProcessorNumber < NumberOfCpus; ProcessorNumber++) {\r
    Location = &CpuFeaturesData->InitOrder[ProcessorNumber].CpuInfo.ProcessorInfo.Location;\r
    ThreadCountPerPackage[Location->Package]++;\r
    ThreadCountPerCore[Location->Package * CpuStatus->MaxCoreCount + Location->Core]++;\r
  }\r
\r
  for (PackageIndex = 0; PackageIndex < CpuStatus->PackageCount; PackageIndex++) {\r
    if (ThreadCountPerPackage[PackageIndex] != 0) {\r
      DEBUG ((DEBUG_INFO, "P%02d: Thread Count = %d\n", PackageIndex, ThreadCountPerPackage[PackageIndex]));\r
      for (CoreIndex = 0; CoreIndex < CpuStatus->MaxCoreCount; CoreIndex++) {\r
        if (ThreadCountPerCore[PackageIndex * CpuStatus->MaxCoreCount + CoreIndex] != 0) {\r
          DEBUG ((\r
            DEBUG_INFO, "  P%02d C%04d, Thread Count = %d\n", PackageIndex, CoreIndex,\r
            ThreadCountPerCore[PackageIndex * CpuStatus->MaxCoreCount + CoreIndex]\r
            ));\r
        }\r
      }\r
    }\r
  }\r
\r
  CpuFeaturesData->CpuFlags.CoreSemaphoreCount = AllocateZeroPool (sizeof (UINT32) * CpuStatus->PackageCount * CpuStatus->MaxCoreCount * CpuStatus->MaxThreadCount);\r
  ASSERT (CpuFeaturesData->CpuFlags.CoreSemaphoreCount != NULL);\r
  CpuFeaturesData->CpuFlags.PackageSemaphoreCount = AllocateZeroPool (sizeof (UINT32) * CpuStatus->PackageCount * CpuStatus->MaxCoreCount * CpuStatus->MaxThreadCount);\r
  ASSERT (CpuFeaturesData->CpuFlags.PackageSemaphoreCount != NULL);\r
\r
  //\r
  // Initialize CpuFeaturesData->InitOrder[].CpuInfo.First\r
  // Use AllocatePages () instead of AllocatePool () because pool cannot be freed in PEI phase but page can.\r
  //\r
  Pages     = EFI_SIZE_TO_PAGES (CpuStatus->PackageCount * sizeof (UINT32) + CpuStatus->PackageCount * CpuStatus->MaxCoreCount * sizeof (UINT32));\r
  FirstCore = AllocatePages (Pages);\r
  ASSERT (FirstCore != NULL);\r
  FirstThread  = FirstCore + CpuStatus->PackageCount;\r
\r
  //\r
  // Set FirstPackage, FirstCore[], FirstThread[] to maximum package ID, core ID, thread ID.\r
  //\r
  FirstPackage = MAX_UINT32;\r
  SetMem32 (FirstCore,   CpuStatus->PackageCount * sizeof (UINT32), MAX_UINT32);\r
  SetMem32 (FirstThread, CpuStatus->PackageCount * CpuStatus->MaxCoreCount * sizeof (UINT32), MAX_UINT32);\r
\r
  for (ProcessorNumber = 0; ProcessorNumber < NumberOfCpus; ProcessorNumber++) {\r
    Location = &CpuFeaturesData->InitOrder[ProcessorNumber].CpuInfo.ProcessorInfo.Location;\r
\r
    //\r
    // Save the minimum package ID in the platform.\r
    //\r
    FirstPackage                 = MIN (Location->Package, FirstPackage);\r
\r
    //\r
    // Save the minimum core ID per package.\r
    //\r
    FirstCore[Location->Package] = MIN (Location->Core, FirstCore[Location->Package]);\r
\r
    //\r
    // Save the minimum thread ID per core.\r
    //\r
    FirstThread[Location->Package * CpuStatus->MaxCoreCount + Location->Core] = MIN (\r
      Location->Thread,\r
      FirstThread[Location->Package * CpuStatus->MaxCoreCount + Location->Core]\r
    );\r
  }\r
\r
  //\r
  // Update the First field.\r
  //\r
  for (ProcessorNumber = 0; ProcessorNumber < NumberOfCpus; ProcessorNumber++) {\r
    Location = &CpuFeaturesData->InitOrder[ProcessorNumber].CpuInfo.ProcessorInfo.Location;\r
\r
    if (Location->Package == FirstPackage) {\r
      CpuFeaturesData->InitOrder[ProcessorNumber].CpuInfo.First.Package = 1;\r
    }\r
\r
    //\r
    // Set First.Die/Tile/Module for each thread assuming:\r
    //  single Die under each package, single Tile under each Die, single Module under each Tile\r
    //\r
    CpuFeaturesData->InitOrder[ProcessorNumber].CpuInfo.First.Die = 1;\r
    CpuFeaturesData->InitOrder[ProcessorNumber].CpuInfo.First.Tile = 1;\r
    CpuFeaturesData->InitOrder[ProcessorNumber].CpuInfo.First.Module = 1;\r
\r
    if (Location->Core == FirstCore[Location->Package]) {\r
      CpuFeaturesData->InitOrder[ProcessorNumber].CpuInfo.First.Core = 1;\r
    }\r
    if (Location->Thread == FirstThread[Location->Package * CpuStatus->MaxCoreCount + Location->Core]) {\r
      CpuFeaturesData->InitOrder[ProcessorNumber].CpuInfo.First.Thread = 1;\r
    }\r
  }\r
\r
  FreePages (FirstCore, Pages);\r
}\r
\r
/**\r
  Worker function to do OR operation on CPU feature supported bits mask buffer.\r
\r
  @param[in]  SupportedFeatureMask  The pointer to CPU feature bits mask buffer\r
  @param[in]  OrFeatureBitMask      The feature bit mask to do OR operation\r
  @param[in]  BitMaskSize           The CPU feature bits mask buffer size.\r
\r
**/\r
VOID\r
SupportedMaskOr (\r
  IN UINT8               *SupportedFeatureMask,\r
  IN UINT8               *OrFeatureBitMask,\r
  IN UINT32              BitMaskSize\r
  )\r
{\r
  UINTN                  Index;\r
  UINT8                  *Data1;\r
  UINT8                  *Data2;\r
\r
  Data1 = SupportedFeatureMask;\r
  Data2 = OrFeatureBitMask;\r
  for (Index = 0; Index < BitMaskSize; Index++) {\r
    *(Data1++) |=  *(Data2++);\r
  }\r
}\r
\r
/**\r
  Worker function to do AND operation on CPU feature supported bits mask buffer.\r
\r
  @param[in]  SupportedFeatureMask  The pointer to CPU feature bits mask buffer\r
  @param[in]  AndFeatureBitMask     The feature bit mask to do AND operation\r
  @param[in]  BitMaskSize           CPU feature bits mask buffer size.\r
\r
**/\r
VOID\r
SupportedMaskAnd (\r
  IN       UINT8               *SupportedFeatureMask,\r
  IN CONST UINT8               *AndFeatureBitMask,\r
  IN       UINT32              BitMaskSize\r
  )\r
{\r
  UINTN                  Index;\r
  UINT8                  *Data1;\r
  CONST UINT8            *Data2;\r
\r
  Data1 = SupportedFeatureMask;\r
  Data2 = AndFeatureBitMask;\r
  for (Index = 0; Index < BitMaskSize; Index++) {\r
    *(Data1++) &=  *(Data2++);\r
  }\r
}\r
\r
/**\r
  Worker function to clean bit operation on CPU feature supported bits mask buffer.\r
\r
  @param[in]  SupportedFeatureMask  The pointer to CPU feature bits mask buffer\r
  @param[in]  AndFeatureBitMask     The feature bit mask to do XOR operation\r
  @param[in]  BitMaskSize           CPU feature bits mask buffer size.\r
**/\r
VOID\r
SupportedMaskCleanBit (\r
  IN UINT8               *SupportedFeatureMask,\r
  IN UINT8               *AndFeatureBitMask,\r
  IN UINT32              BitMaskSize\r
  )\r
{\r
  UINTN                  Index;\r
  UINT8                  *Data1;\r
  UINT8                  *Data2;\r
\r
  Data1 = SupportedFeatureMask;\r
  Data2 = AndFeatureBitMask;\r
  for (Index = 0; Index < BitMaskSize; Index++) {\r
    *(Data1++) &=  ~(*(Data2++));\r
  }\r
}\r
\r
/**\r
  Worker function to check if the compared CPU feature set in the CPU feature\r
  supported bits mask buffer.\r
\r
  @param[in]  SupportedFeatureMask   The pointer to CPU feature bits mask buffer\r
  @param[in]  ComparedFeatureBitMask The feature bit mask to be compared\r
  @param[in]  BitMaskSize            CPU feature bits mask buffer size.\r
\r
  @retval TRUE   The ComparedFeatureBitMask is set in CPU feature supported bits\r
                 mask buffer.\r
  @retval FALSE  The ComparedFeatureBitMask is not set in CPU feature supported bits\r
                 mask buffer.\r
**/\r
BOOLEAN\r
IsBitMaskMatch (\r
  IN UINT8               *SupportedFeatureMask,\r
  IN UINT8               *ComparedFeatureBitMask,\r
  IN UINT32              BitMaskSize\r
  )\r
{\r
  UINTN                  Index;\r
  UINT8                  *Data1;\r
  UINT8                  *Data2;\r
\r
  Data1 = SupportedFeatureMask;\r
  Data2 = ComparedFeatureBitMask;\r
  for (Index = 0; Index < BitMaskSize; Index++) {\r
    if (((*(Data1++)) & (*(Data2++))) != 0) {\r
      return TRUE;\r
    }\r
  }\r
  return FALSE;\r
}\r
\r
/**\r
  Collects processor data for calling processor.\r
\r
  @param[in,out]  Buffer  The pointer to private data buffer.\r
**/\r
VOID\r
EFIAPI\r
CollectProcessorData (\r
  IN OUT VOID                          *Buffer\r
  )\r
{\r
  UINTN                                ProcessorNumber;\r
  CPU_FEATURES_ENTRY                   *CpuFeature;\r
  REGISTER_CPU_FEATURE_INFORMATION     *CpuInfo;\r
  LIST_ENTRY                           *Entry;\r
  CPU_FEATURES_DATA                    *CpuFeaturesData;\r
\r
  CpuFeaturesData = (CPU_FEATURES_DATA *)Buffer;\r
  ProcessorNumber = GetProcessorIndex (CpuFeaturesData);\r
  CpuInfo = &CpuFeaturesData->InitOrder[ProcessorNumber].CpuInfo;\r
  //\r
  // collect processor information\r
  //\r
  FillProcessorInfo (CpuInfo);\r
  Entry = GetFirstNode (&CpuFeaturesData->FeatureList);\r
  while (!IsNull (&CpuFeaturesData->FeatureList, Entry)) {\r
    CpuFeature = CPU_FEATURE_ENTRY_FROM_LINK (Entry);\r
    if (CpuFeature->SupportFunc == NULL) {\r
      //\r
      // If SupportFunc is NULL, then the feature is supported.\r
      //\r
      SupportedMaskOr (\r
        CpuFeaturesData->InitOrder[ProcessorNumber].FeaturesSupportedMask,\r
        CpuFeature->FeatureMask,\r
        CpuFeaturesData->BitMaskSize\r
        );\r
    } else if (CpuFeature->SupportFunc (ProcessorNumber, CpuInfo, CpuFeature->ConfigData)) {\r
      SupportedMaskOr (\r
        CpuFeaturesData->InitOrder[ProcessorNumber].FeaturesSupportedMask,\r
        CpuFeature->FeatureMask,\r
        CpuFeaturesData->BitMaskSize\r
        );\r
    }\r
    Entry = Entry->ForwardLink;\r
  }\r
}\r
\r
/**\r
  Dump the contents of a CPU register table.\r
\r
  @param[in]  ProcessorNumber  The index of the CPU to show the register table contents\r
\r
  @note This service could be called by BSP only.\r
**/\r
VOID\r
DumpRegisterTableOnProcessor (\r
  IN UINTN                             ProcessorNumber\r
  )\r
{\r
  CPU_FEATURES_DATA                    *CpuFeaturesData;\r
  UINTN                                FeatureIndex;\r
  CPU_REGISTER_TABLE                   *RegisterTable;\r
  CPU_REGISTER_TABLE_ENTRY             *RegisterTableEntry;\r
  CPU_REGISTER_TABLE_ENTRY             *RegisterTableEntryHead;\r
  UINT32                               DebugPrintErrorLevel;\r
\r
  DebugPrintErrorLevel = (ProcessorNumber == 0) ? DEBUG_INFO : DEBUG_VERBOSE;\r
  CpuFeaturesData      = GetCpuFeaturesData ();\r
  //\r
  // Debug information\r
  //\r
  RegisterTable = &CpuFeaturesData->RegisterTable[ProcessorNumber];\r
  DEBUG ((DebugPrintErrorLevel, "RegisterTable->TableLength = %d\n", RegisterTable->TableLength));\r
\r
  RegisterTableEntryHead = (CPU_REGISTER_TABLE_ENTRY *) (UINTN) RegisterTable->RegisterTableEntry;\r
\r
  for (FeatureIndex = 0; FeatureIndex < RegisterTable->TableLength; FeatureIndex++) {\r
    RegisterTableEntry = &RegisterTableEntryHead[FeatureIndex];\r
    switch (RegisterTableEntry->RegisterType) {\r
    case Msr:\r
      DEBUG ((\r
        DebugPrintErrorLevel,\r
        "Processor: %04d: Index %04d, MSR  : %08x, Bit Start: %02d, Bit Length: %02d, Value: %016lx\r\n",\r
        (UINT32) ProcessorNumber,\r
        (UINT32) FeatureIndex,\r
        RegisterTableEntry->Index,\r
        RegisterTableEntry->ValidBitStart,\r
        RegisterTableEntry->ValidBitLength,\r
        RegisterTableEntry->Value\r
        ));\r
      break;\r
    case ControlRegister:\r
      DEBUG ((\r
        DebugPrintErrorLevel,\r
        "Processor: %04d: Index %04d, CR   : %08x, Bit Start: %02d, Bit Length: %02d, Value: %016lx\r\n",\r
        (UINT32) ProcessorNumber,\r
        (UINT32) FeatureIndex,\r
        RegisterTableEntry->Index,\r
        RegisterTableEntry->ValidBitStart,\r
        RegisterTableEntry->ValidBitLength,\r
        RegisterTableEntry->Value\r
        ));\r
      break;\r
    case MemoryMapped:\r
      DEBUG ((\r
        DebugPrintErrorLevel,\r
        "Processor: %04d: Index %04d, MMIO : %016lx, Bit Start: %02d, Bit Length: %02d, Value: %016lx\r\n",\r
        (UINT32) ProcessorNumber,\r
        (UINT32) FeatureIndex,\r
        RegisterTableEntry->Index | LShiftU64 (RegisterTableEntry->HighIndex, 32),\r
        RegisterTableEntry->ValidBitStart,\r
        RegisterTableEntry->ValidBitLength,\r
        RegisterTableEntry->Value\r
        ));\r
      break;\r
    case CacheControl:\r
      DEBUG ((\r
        DebugPrintErrorLevel,\r
        "Processor: %04d: Index %04d, CACHE: %08x, Bit Start: %02d, Bit Length: %02d, Value: %016lx\r\n",\r
        (UINT32) ProcessorNumber,\r
        (UINT32) FeatureIndex,\r
        RegisterTableEntry->Index,\r
        RegisterTableEntry->ValidBitStart,\r
        RegisterTableEntry->ValidBitLength,\r
        RegisterTableEntry->Value\r
        ));\r
      break;\r
    case Semaphore:\r
      DEBUG ((\r
        DebugPrintErrorLevel,\r
        "Processor: %04d: Index %04d, SEMAP: %s\r\n",\r
        (UINT32) ProcessorNumber,\r
        (UINT32) FeatureIndex,\r
        mDependTypeStr[MIN ((UINT32)RegisterTableEntry->Value, InvalidDepType)]\r
        ));\r
      break;\r
\r
    default:\r
      break;\r
    }\r
  }\r
}\r
\r
/**\r
  Get the biggest dependence type.\r
  PackageDepType > CoreDepType > ThreadDepType > NoneDepType.\r
\r
  @param[in]  BeforeDep           Before dependence type.\r
  @param[in]  AfterDep            After dependence type.\r
  @param[in]  NoneNeibBeforeDep   Before dependence type for not neighborhood features.\r
  @param[in]  NoneNeibAfterDep    After dependence type for not neighborhood features.\r
\r
  @retval  Return the biggest dependence type.\r
**/\r
CPU_FEATURE_DEPENDENCE_TYPE\r
BiggestDep (\r
  IN CPU_FEATURE_DEPENDENCE_TYPE  BeforeDep,\r
  IN CPU_FEATURE_DEPENDENCE_TYPE  AfterDep,\r
  IN CPU_FEATURE_DEPENDENCE_TYPE  NoneNeibBeforeDep,\r
  IN CPU_FEATURE_DEPENDENCE_TYPE  NoneNeibAfterDep\r
  )\r
{\r
  CPU_FEATURE_DEPENDENCE_TYPE Bigger;\r
\r
  Bigger = MAX (BeforeDep, AfterDep);\r
  Bigger = MAX (Bigger, NoneNeibBeforeDep);\r
  return MAX(Bigger, NoneNeibAfterDep);\r
}\r
\r
/**\r
  Analysis register CPU features on each processor and save CPU setting in CPU register table.\r
\r
  @param[in]  NumberOfCpus  Number of processor in system\r
\r
**/\r
VOID\r
AnalysisProcessorFeatures (\r
  IN UINTN                             NumberOfCpus\r
  )\r
{\r
  EFI_STATUS                           Status;\r
  UINTN                                ProcessorNumber;\r
  CPU_FEATURES_ENTRY                   *CpuFeature;\r
  CPU_FEATURES_ENTRY                   *CpuFeatureInOrder;\r
  CPU_FEATURES_INIT_ORDER              *CpuInitOrder;\r
  REGISTER_CPU_FEATURE_INFORMATION     *CpuInfo;\r
  LIST_ENTRY                           *Entry;\r
  CPU_FEATURES_DATA                    *CpuFeaturesData;\r
  LIST_ENTRY                           *NextEntry;\r
  CPU_FEATURES_ENTRY                   *NextCpuFeatureInOrder;\r
  BOOLEAN                              Success;\r
  CPU_FEATURE_DEPENDENCE_TYPE          BeforeDep;\r
  CPU_FEATURE_DEPENDENCE_TYPE          AfterDep;\r
  CPU_FEATURE_DEPENDENCE_TYPE          NoneNeibBeforeDep;\r
  CPU_FEATURE_DEPENDENCE_TYPE          NoneNeibAfterDep;\r
\r
  CpuFeaturesData = GetCpuFeaturesData ();\r
  CpuFeaturesData->CapabilityPcd = AllocatePool (CpuFeaturesData->BitMaskSize);\r
  ASSERT (CpuFeaturesData->CapabilityPcd != NULL);\r
  SetMem (CpuFeaturesData->CapabilityPcd, CpuFeaturesData->BitMaskSize, 0xFF);\r
  for (ProcessorNumber = 0; ProcessorNumber < NumberOfCpus; ProcessorNumber++) {\r
    CpuInitOrder = &CpuFeaturesData->InitOrder[ProcessorNumber];\r
    //\r
    // Calculate the last capability on all processors\r
    //\r
    SupportedMaskAnd (CpuFeaturesData->CapabilityPcd, CpuInitOrder->FeaturesSupportedMask, CpuFeaturesData->BitMaskSize);\r
  }\r
  //\r
  // Calculate the last setting\r
  //\r
  CpuFeaturesData->SettingPcd = AllocateCopyPool (CpuFeaturesData->BitMaskSize, CpuFeaturesData->CapabilityPcd);\r
  ASSERT (CpuFeaturesData->SettingPcd != NULL);\r
  SupportedMaskAnd (CpuFeaturesData->SettingPcd, PcdGetPtr (PcdCpuFeaturesSetting), CpuFeaturesData->BitMaskSize);\r
\r
  //\r
  // Dump the last CPU feature list\r
  //\r
  DEBUG_CODE (\r
    DEBUG ((DEBUG_INFO, "Last CPU features list...\n"));\r
    Entry = GetFirstNode (&CpuFeaturesData->FeatureList);\r
    while (!IsNull (&CpuFeaturesData->FeatureList, Entry)) {\r
      CpuFeature = CPU_FEATURE_ENTRY_FROM_LINK (Entry);\r
      if (IsBitMaskMatch (CpuFeature->FeatureMask, CpuFeaturesData->CapabilityPcd, CpuFeaturesData->BitMaskSize)) {\r
        if (IsBitMaskMatch (CpuFeature->FeatureMask, CpuFeaturesData->SettingPcd, CpuFeaturesData->BitMaskSize)) {\r
          DEBUG ((DEBUG_INFO, "[Enable   ] "));\r
        } else {\r
          DEBUG ((DEBUG_INFO, "[Disable  ] "));\r
        }\r
      } else {\r
        DEBUG ((DEBUG_INFO, "[Unsupport] "));\r
      }\r
      DumpCpuFeature (CpuFeature, CpuFeaturesData->BitMaskSize);\r
      Entry = Entry->ForwardLink;\r
    }\r
    DEBUG ((DEBUG_INFO, "PcdCpuFeaturesCapability:\n"));\r
    DumpCpuFeatureMask (CpuFeaturesData->CapabilityPcd, CpuFeaturesData->BitMaskSize);\r
    DEBUG ((DEBUG_INFO, "Origin PcdCpuFeaturesSetting:\n"));\r
    DumpCpuFeatureMask (PcdGetPtr (PcdCpuFeaturesSetting), CpuFeaturesData->BitMaskSize);\r
    DEBUG ((DEBUG_INFO, "Final PcdCpuFeaturesSetting:\n"));\r
    DumpCpuFeatureMask (CpuFeaturesData->SettingPcd, CpuFeaturesData->BitMaskSize);\r
  );\r
\r
  //\r
  // Save PCDs and display CPU PCDs\r
  //\r
  SetCapabilityPcd (CpuFeaturesData->CapabilityPcd, CpuFeaturesData->BitMaskSize);\r
  SetSettingPcd (CpuFeaturesData->SettingPcd, CpuFeaturesData->BitMaskSize);\r
\r
  for (ProcessorNumber = 0; ProcessorNumber < NumberOfCpus; ProcessorNumber++) {\r
    CpuInitOrder = &CpuFeaturesData->InitOrder[ProcessorNumber];\r
    Entry = GetFirstNode (&CpuFeaturesData->FeatureList);\r
    while (!IsNull (&CpuFeaturesData->FeatureList, Entry)) {\r
      //\r
      // Insert each feature into processor's order list\r
      //\r
      CpuFeature = CPU_FEATURE_ENTRY_FROM_LINK (Entry);\r
      if (IsBitMaskMatch (CpuFeature->FeatureMask, CpuFeaturesData->CapabilityPcd, CpuFeaturesData->BitMaskSize)) {\r
        CpuFeatureInOrder = AllocateCopyPool (sizeof (CPU_FEATURES_ENTRY), CpuFeature);\r
        ASSERT (CpuFeatureInOrder != NULL);\r
        InsertTailList (&CpuInitOrder->OrderList, &CpuFeatureInOrder->Link);\r
      }\r
      Entry = Entry->ForwardLink;\r
    }\r
    //\r
    // Go through ordered feature list to initialize CPU features\r
    //\r
    CpuInfo = &CpuFeaturesData->InitOrder[ProcessorNumber].CpuInfo;\r
    Entry = GetFirstNode (&CpuInitOrder->OrderList);\r
    while (!IsNull (&CpuInitOrder->OrderList, Entry)) {\r
      CpuFeatureInOrder = CPU_FEATURE_ENTRY_FROM_LINK (Entry);\r
\r
      Success = FALSE;\r
      if (IsBitMaskMatch (CpuFeatureInOrder->FeatureMask, CpuFeaturesData->SettingPcd, CpuFeaturesData->BitMaskSize)) {\r
        Status = CpuFeatureInOrder->InitializeFunc (ProcessorNumber, CpuInfo, CpuFeatureInOrder->ConfigData, TRUE);\r
        if (EFI_ERROR (Status)) {\r
          //\r
          // Clean the CpuFeatureInOrder->FeatureMask in setting PCD.\r
          //\r
          SupportedMaskCleanBit (CpuFeaturesData->SettingPcd, CpuFeatureInOrder->FeatureMask, CpuFeaturesData->BitMaskSize);\r
          if (CpuFeatureInOrder->FeatureName != NULL) {\r
            DEBUG ((DEBUG_WARN, "Warning :: Failed to enable Feature: Name = %a.\n", CpuFeatureInOrder->FeatureName));\r
          } else {\r
            DEBUG ((DEBUG_WARN, "Warning :: Failed to enable Feature: Mask = "));\r
            DumpCpuFeatureMask (CpuFeatureInOrder->FeatureMask, CpuFeaturesData->BitMaskSize);\r
          }\r
        } else {\r
          Success = TRUE;\r
        }\r
      } else {\r
        Status = CpuFeatureInOrder->InitializeFunc (ProcessorNumber, CpuInfo, CpuFeatureInOrder->ConfigData, FALSE);\r
        if (EFI_ERROR (Status)) {\r
          if (CpuFeatureInOrder->FeatureName != NULL) {\r
            DEBUG ((DEBUG_WARN, "Warning :: Failed to disable Feature: Name = %a.\n", CpuFeatureInOrder->FeatureName));\r
          } else {\r
            DEBUG ((DEBUG_WARN, "Warning :: Failed to disable Feature: Mask = "));\r
            DumpCpuFeatureMask (CpuFeatureInOrder->FeatureMask, CpuFeaturesData->BitMaskSize);\r
          }\r
        } else {\r
          Success = TRUE;\r
        }\r
      }\r
\r
      if (Success) {\r
        NextEntry = Entry->ForwardLink;\r
        if (!IsNull (&CpuInitOrder->OrderList, NextEntry)) {\r
          NextCpuFeatureInOrder = CPU_FEATURE_ENTRY_FROM_LINK (NextEntry);\r
\r
          //\r
          // If feature has dependence with the next feature (ONLY care core/package dependency).\r
          // and feature initialize succeed, add sync semaphere here.\r
          //\r
          BeforeDep = DetectFeatureScope (CpuFeatureInOrder, TRUE, NextCpuFeatureInOrder->FeatureMask);\r
          AfterDep  = DetectFeatureScope (NextCpuFeatureInOrder, FALSE, CpuFeatureInOrder->FeatureMask);\r
          //\r
          // Check whether next feature has After type dependence with not neighborhood CPU\r
          // Features in former CPU features.\r
          //\r
          NoneNeibAfterDep = DetectNoneNeighborhoodFeatureScope(NextCpuFeatureInOrder, FALSE, &CpuInitOrder->OrderList);\r
        } else {\r
          BeforeDep        = NoneDepType;\r
          AfterDep         = NoneDepType;\r
          NoneNeibAfterDep = NoneDepType;\r
        }\r
        //\r
        // Check whether current feature has Before type dependence with none neighborhood\r
        // CPU features in after Cpu features.\r
        //\r
        NoneNeibBeforeDep = DetectNoneNeighborhoodFeatureScope(CpuFeatureInOrder, TRUE, &CpuInitOrder->OrderList);\r
\r
        //\r
        // Get the biggest dependence and add semaphore for it.\r
        // PackageDepType > CoreDepType > ThreadDepType > NoneDepType.\r
        //\r
        BeforeDep = BiggestDep(BeforeDep, AfterDep, NoneNeibBeforeDep, NoneNeibAfterDep);\r
        if (BeforeDep > ThreadDepType) {\r
          CPU_REGISTER_TABLE_WRITE32 (ProcessorNumber, Semaphore, 0, BeforeDep);\r
        }\r
      }\r
\r
      Entry = Entry->ForwardLink;\r
    }\r
\r
    //\r
    // Dump PcdCpuFeaturesSetting again because this value maybe updated\r
    // again during initialize the features.\r
    //\r
    DEBUG ((DEBUG_INFO, "Dump final value for PcdCpuFeaturesSetting:\n"));\r
    DumpCpuFeatureMask (CpuFeaturesData->SettingPcd, CpuFeaturesData->BitMaskSize);\r
\r
    //\r
    // Dump the RegisterTable\r
    //\r
    DumpRegisterTableOnProcessor (ProcessorNumber);\r
  }\r
}\r
\r
/**\r
  Increment semaphore by 1.\r
\r
  @param      Sem            IN:  32-bit unsigned integer\r
\r
**/\r
VOID\r
LibReleaseSemaphore (\r
  IN OUT  volatile UINT32           *Sem\r
  )\r
{\r
  InterlockedIncrement (Sem);\r
}\r
\r
/**\r
  Decrement the semaphore by 1 if it is not zero.\r
\r
  Performs an atomic decrement operation for semaphore.\r
  The compare exchange operation must be performed using\r
  MP safe mechanisms.\r
\r
  @param      Sem            IN:  32-bit unsigned integer\r
\r
**/\r
VOID\r
LibWaitForSemaphore (\r
  IN OUT  volatile UINT32           *Sem\r
  )\r
{\r
  UINT32  Value;\r
\r
  do {\r
    Value = *Sem;\r
  } while (Value == 0 ||\r
           InterlockedCompareExchange32 (\r
             Sem,\r
             Value,\r
             Value - 1\r
             ) != Value);\r
}\r
\r
/**\r
  Read / write CR value.\r
\r
  @param[in]      CrIndex         The CR index which need to read/write.\r
  @param[in]      Read            Read or write. TRUE is read.\r
  @param[in,out]  CrValue         CR value.\r
\r
  @retval    EFI_SUCCESS means read/write success, else return EFI_UNSUPPORTED.\r
**/\r
UINTN\r
ReadWriteCr (\r
  IN     UINT32       CrIndex,\r
  IN     BOOLEAN      Read,\r
  IN OUT UINTN        *CrValue\r
  )\r
{\r
  switch (CrIndex) {\r
  case 0:\r
    if (Read) {\r
      *CrValue = AsmReadCr0 ();\r
    } else {\r
      AsmWriteCr0 (*CrValue);\r
    }\r
    break;\r
  case 2:\r
    if (Read) {\r
      *CrValue = AsmReadCr2 ();\r
    } else {\r
      AsmWriteCr2 (*CrValue);\r
    }\r
    break;\r
  case 3:\r
    if (Read) {\r
      *CrValue = AsmReadCr3 ();\r
    } else {\r
      AsmWriteCr3 (*CrValue);\r
    }\r
    break;\r
  case 4:\r
    if (Read) {\r
      *CrValue = AsmReadCr4 ();\r
    } else {\r
      AsmWriteCr4 (*CrValue);\r
    }\r
    break;\r
  default:\r
    return EFI_UNSUPPORTED;;\r
  }\r
\r
  return EFI_SUCCESS;\r
}\r
\r
/**\r
  Initialize the CPU registers from a register table.\r
\r
  @param[in]  RegisterTable         The register table for this AP.\r
  @param[in]  ApLocation            AP location info for this ap.\r
  @param[in]  CpuStatus             CPU status info for this CPU.\r
  @param[in]  CpuFlags              Flags data structure used when program the register.\r
\r
  @note This service could be called by BSP/APs.\r
**/\r
VOID\r
ProgramProcessorRegister (\r
  IN CPU_REGISTER_TABLE           *RegisterTable,\r
  IN EFI_CPU_PHYSICAL_LOCATION    *ApLocation,\r
  IN CPU_STATUS_INFORMATION       *CpuStatus,\r
  IN PROGRAM_CPU_REGISTER_FLAGS   *CpuFlags\r
  )\r
{\r
  CPU_REGISTER_TABLE_ENTRY  *RegisterTableEntry;\r
  UINTN                     Index;\r
  UINTN                     Value;\r
  CPU_REGISTER_TABLE_ENTRY  *RegisterTableEntryHead;\r
  volatile UINT32           *SemaphorePtr;\r
  UINT32                    FirstThread;\r
  UINT32                    CurrentThread;\r
  UINT32                    CurrentCore;\r
  UINTN                     ProcessorIndex;\r
  UINT32                    *ThreadCountPerPackage;\r
  UINT8                     *ThreadCountPerCore;\r
  EFI_STATUS                Status;\r
  UINT64                    CurrentValue;\r
\r
  //\r
  // Traverse Register Table of this logical processor\r
  //\r
  RegisterTableEntryHead = (CPU_REGISTER_TABLE_ENTRY *) (UINTN) RegisterTable->RegisterTableEntry;\r
\r
  for (Index = 0; Index < RegisterTable->TableLength; Index++) {\r
\r
    RegisterTableEntry = &RegisterTableEntryHead[Index];\r
\r
    //\r
    // Check the type of specified register\r
    //\r
    switch (RegisterTableEntry->RegisterType) {\r
    //\r
    // The specified register is Control Register\r
    //\r
    case ControlRegister:\r
      Status = ReadWriteCr (RegisterTableEntry->Index, TRUE, &Value);\r
      if (EFI_ERROR (Status)) {\r
        break;\r
      }\r
      if (RegisterTableEntry->TestThenWrite) {\r
        CurrentValue = BitFieldRead64 (\r
                         Value,\r
                         RegisterTableEntry->ValidBitStart,\r
                         RegisterTableEntry->ValidBitStart + RegisterTableEntry->ValidBitLength - 1\r
                         );\r
        if (CurrentValue == RegisterTableEntry->Value) {\r
          break;\r
        }\r
      }\r
      Value = (UINTN) BitFieldWrite64 (\r
                        Value,\r
                        RegisterTableEntry->ValidBitStart,\r
                        RegisterTableEntry->ValidBitStart + RegisterTableEntry->ValidBitLength - 1,\r
                        RegisterTableEntry->Value\r
                        );\r
      ReadWriteCr (RegisterTableEntry->Index, FALSE, &Value);\r
      break;\r
\r
    //\r
    // The specified register is Model Specific Register\r
    //\r
    case Msr:\r
      if (RegisterTableEntry->TestThenWrite) {\r
        Value = (UINTN)AsmReadMsr64 (RegisterTableEntry->Index);\r
        if (RegisterTableEntry->ValidBitLength >= 64) {\r
          if (Value == RegisterTableEntry->Value) {\r
            break;\r
          }\r
        } else {\r
          CurrentValue = BitFieldRead64 (\r
                           Value,\r
                           RegisterTableEntry->ValidBitStart,\r
                           RegisterTableEntry->ValidBitStart + RegisterTableEntry->ValidBitLength - 1\r
                           );\r
          if (CurrentValue == RegisterTableEntry->Value) {\r
            break;\r
          }\r
        }\r
      }\r
\r
      if (RegisterTableEntry->ValidBitLength >= 64) {\r
        //\r
        // If length is not less than 64 bits, then directly write without reading\r
        //\r
        AsmWriteMsr64 (\r
          RegisterTableEntry->Index,\r
          RegisterTableEntry->Value\r
          );\r
      } else {\r
        //\r
        // Set the bit section according to bit start and length\r
        //\r
        AsmMsrBitFieldWrite64 (\r
          RegisterTableEntry->Index,\r
          RegisterTableEntry->ValidBitStart,\r
          RegisterTableEntry->ValidBitStart + RegisterTableEntry->ValidBitLength - 1,\r
          RegisterTableEntry->Value\r
          );\r
      }\r
      break;\r
    //\r
    // MemoryMapped operations\r
    //\r
    case MemoryMapped:\r
      AcquireSpinLock (&CpuFlags->MemoryMappedLock);\r
      MmioBitFieldWrite32 (\r
        (UINTN)(RegisterTableEntry->Index | LShiftU64 (RegisterTableEntry->HighIndex, 32)),\r
        RegisterTableEntry->ValidBitStart,\r
        RegisterTableEntry->ValidBitStart + RegisterTableEntry->ValidBitLength - 1,\r
        (UINT32)RegisterTableEntry->Value\r
        );\r
      ReleaseSpinLock (&CpuFlags->MemoryMappedLock);\r
      break;\r
    //\r
    // Enable or disable cache\r
    //\r
    case CacheControl:\r
      //\r
      // If value of the entry is 0, then disable cache.  Otherwise, enable cache.\r
      //\r
      if (RegisterTableEntry->Value == 0) {\r
        AsmDisableCache ();\r
      } else {\r
        AsmEnableCache ();\r
      }\r
      break;\r
\r
    case Semaphore:\r
      // Semaphore works logic like below:\r
      //\r
      //  V(x) = LibReleaseSemaphore (Semaphore[FirstThread + x]);\r
      //  P(x) = LibWaitForSemaphore (Semaphore[FirstThread + x]);\r
      //\r
      //  All threads (T0...Tn) waits in P() line and continues running\r
      //  together.\r
      //\r
      //\r
      //  T0             T1            ...           Tn\r
      //\r
      //  V(0...n)       V(0...n)      ...           V(0...n)\r
      //  n * P(0)       n * P(1)      ...           n * P(n)\r
      //\r
      switch (RegisterTableEntry->Value) {\r
      case CoreDepType:\r
        SemaphorePtr = CpuFlags->CoreSemaphoreCount;\r
        ThreadCountPerCore = (UINT8 *)(UINTN)CpuStatus->ThreadCountPerCore;\r
\r
        CurrentCore = ApLocation->Package * CpuStatus->MaxCoreCount + ApLocation->Core;\r
        //\r
        // Get Offset info for the first thread in the core which current thread belongs to.\r
        //\r
        FirstThread   = CurrentCore * CpuStatus->MaxThreadCount;\r
        CurrentThread = FirstThread + ApLocation->Thread;\r
\r
        //\r
        // Different cores may have different valid threads in them. If driver maintail clearly\r
        // thread index in different cores, the logic will be much complicated.\r
        // Here driver just simply records the max thread number in all cores and use it as expect\r
        // thread number for all cores.\r
        // In below two steps logic, first current thread will Release semaphore for each thread\r
        // in current core. Maybe some threads are not valid in this core, but driver don't\r
        // care. Second, driver will let current thread wait semaphore for all valid threads in\r
        // current core. Because only the valid threads will do release semaphore for this\r
        // thread, driver here only need to wait the valid thread count.\r
        //\r
\r
        //\r
        // First Notify ALL THREADs in current Core that this thread is ready.\r
        //\r
        for (ProcessorIndex = 0; ProcessorIndex < CpuStatus->MaxThreadCount; ProcessorIndex ++) {\r
          LibReleaseSemaphore (&SemaphorePtr[FirstThread + ProcessorIndex]);\r
        }\r
        //\r
        // Second, check whether all VALID THREADs (not all threads) in current core are ready.\r
        //\r
        for (ProcessorIndex = 0; ProcessorIndex < ThreadCountPerCore[CurrentCore]; ProcessorIndex ++) {\r
          LibWaitForSemaphore (&SemaphorePtr[CurrentThread]);\r
        }\r
        break;\r
\r
      case PackageDepType:\r
        SemaphorePtr = CpuFlags->PackageSemaphoreCount;\r
        ThreadCountPerPackage = (UINT32 *)(UINTN)CpuStatus->ThreadCountPerPackage;\r
        //\r
        // Get Offset info for the first thread in the package which current thread belongs to.\r
        //\r
        FirstThread = ApLocation->Package * CpuStatus->MaxCoreCount * CpuStatus->MaxThreadCount;\r
        //\r
        // Get the possible threads count for current package.\r
        //\r
        CurrentThread = FirstThread + CpuStatus->MaxThreadCount * ApLocation->Core + ApLocation->Thread;\r
\r
        //\r
        // Different packages may have different valid threads in them. If driver maintail clearly\r
        // thread index in different packages, the logic will be much complicated.\r
        // Here driver just simply records the max thread number in all packages and use it as expect\r
        // thread number for all packages.\r
        // In below two steps logic, first current thread will Release semaphore for each thread\r
        // in current package. Maybe some threads are not valid in this package, but driver don't\r
        // care. Second, driver will let current thread wait semaphore for all valid threads in\r
        // current package. Because only the valid threads will do release semaphore for this\r
        // thread, driver here only need to wait the valid thread count.\r
        //\r
\r
        //\r
        // First Notify ALL THREADS in current package that this thread is ready.\r
        //\r
        for (ProcessorIndex = 0; ProcessorIndex < CpuStatus->MaxThreadCount * CpuStatus->MaxCoreCount; ProcessorIndex ++) {\r
          LibReleaseSemaphore (&SemaphorePtr[FirstThread + ProcessorIndex]);\r
        }\r
        //\r
        // Second, check whether VALID THREADS (not all threads) in current package are ready.\r
        //\r
        for (ProcessorIndex = 0; ProcessorIndex < ThreadCountPerPackage[ApLocation->Package]; ProcessorIndex ++) {\r
          LibWaitForSemaphore (&SemaphorePtr[CurrentThread]);\r
        }\r
        break;\r
\r
      default:\r
        break;\r
      }\r
      break;\r
\r
    default:\r
      break;\r
    }\r
  }\r
}\r
\r
/**\r
  Programs registers for the calling processor.\r
\r
  @param[in,out] Buffer  The pointer to private data buffer.\r
\r
**/\r
VOID\r
EFIAPI\r
SetProcessorRegister (\r
  IN OUT VOID            *Buffer\r
  )\r
{\r
  CPU_FEATURES_DATA         *CpuFeaturesData;\r
  CPU_REGISTER_TABLE        *RegisterTable;\r
  CPU_REGISTER_TABLE        *RegisterTables;\r
  UINT32                    InitApicId;\r
  UINTN                     ProcIndex;\r
  UINTN                     Index;\r
  ACPI_CPU_DATA             *AcpiCpuData;\r
\r
  CpuFeaturesData = (CPU_FEATURES_DATA *) Buffer;\r
  AcpiCpuData = CpuFeaturesData->AcpiCpuData;\r
\r
  RegisterTables = (CPU_REGISTER_TABLE *)(UINTN)AcpiCpuData->CpuFeatureInitData.RegisterTable;\r
\r
  InitApicId = GetInitialApicId ();\r
  RegisterTable = NULL;\r
  ProcIndex = (UINTN)-1;\r
  for (Index = 0; Index < AcpiCpuData->NumberOfCpus; Index++) {\r
    if (RegisterTables[Index].InitialApicId == InitApicId) {\r
      RegisterTable =  &RegisterTables[Index];\r
      ProcIndex = Index;\r
      break;\r
    }\r
  }\r
  ASSERT (RegisterTable != NULL);\r
\r
  ProgramProcessorRegister (\r
    RegisterTable,\r
    (EFI_CPU_PHYSICAL_LOCATION *)(UINTN)AcpiCpuData->CpuFeatureInitData.ApLocation + ProcIndex,\r
    &AcpiCpuData->CpuFeatureInitData.CpuStatus,\r
    &CpuFeaturesData->CpuFlags\r
    );\r
}\r
\r
/**\r
  Performs CPU features detection.\r
\r
  This service will invoke MP service to check CPU features'\r
  capabilities on BSP/APs.\r
\r
  @note This service could be called by BSP only.\r
**/\r
VOID\r
EFIAPI\r
CpuFeaturesDetect (\r
  VOID\r
  )\r
{\r
  CPU_FEATURES_DATA      *CpuFeaturesData;\r
\r
  CpuFeaturesData = GetCpuFeaturesData();\r
\r
  CpuInitDataInitialize ();\r
\r
  if (CpuFeaturesData->NumberOfCpus > 1) {\r
    //\r
    // Wakeup all APs for data collection.\r
    //\r
    StartupAllAPsWorker (CollectProcessorData, NULL);\r
  }\r
\r
  //\r
  // Collect data on BSP\r
  //\r
  CollectProcessorData (CpuFeaturesData);\r
\r
  AnalysisProcessorFeatures (CpuFeaturesData->NumberOfCpus);\r
}\r
\r