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