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

/** @file\r
  CPU Features Initialize functions.\r
\r
  Copyright (c) 2017 - 2018, Intel Corporation. All rights reserved.<BR>\r
  This program and the accompanying materials\r
  are licensed and made available under the terms and conditions of the BSD License\r
  which accompanies this distribution.  The full text of the license may be found at\r
  http://opensource.org/licenses/bsd-license.php\r
\r
  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,\r
  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.\r
\r
**/\r
\r
#include "RegisterCpuFeatures.h"\r
\r
CHAR16 *mDependTypeStr[]   = {L"None", L"Thread", L"Core", L"Package", L"Invalid" };\r
CHAR16 *mRegisterTypeStr[] = {L"MSR", L"CR", L"MMIO", L"CACHE", L"SEMAP", L"INVALID" };\r
\r
/**\r
  Worker function to save PcdCpuFeaturesCapability.\r
\r
  @param[in]  SupportedFeatureMask  The pointer to CPU feature bits mask buffer\r
**/\r
VOID\r
SetCapabilityPcd (\r
  IN UINT8               *SupportedFeatureMask\r
  )\r
{\r
  EFI_STATUS             Status;\r
  UINTN                  BitMaskSize;\r
\r
  BitMaskSize = PcdGetSize (PcdCpuFeaturesCapability);\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
**/\r
VOID\r
SetSettingPcd (\r
  IN UINT8               *SupportedFeatureMask\r
  )\r
{\r
  EFI_STATUS             Status;\r
  UINTN                  BitMaskSize;\r
\r
  BitMaskSize = PcdGetSize (PcdCpuFeaturesSetting);\r
  Status = PcdSetPtrS (PcdCpuFeaturesSetting, &BitMaskSize, SupportedFeatureMask);\r
  ASSERT_EFI_ERROR (Status);\r
}\r
\r
/**\r
  Worker function to get PcdCpuFeaturesSupport.\r
\r
  @return  The pointer to CPU feature bits mask buffer.\r
**/\r
UINT8 *\r
GetSupportPcd (\r
  VOID\r
  )\r
{\r
  UINT8                  *SupportBitMask;\r
\r
  SupportBitMask = AllocateCopyPool (\r
          PcdGetSize (PcdCpuFeaturesSupport),\r
          PcdGetPtr (PcdCpuFeaturesSupport)\r
          );\r
  ASSERT (SupportBitMask != NULL);\r
\r
  return SupportBitMask;\r
}\r
\r
/**\r
  Worker function to get PcdCpuFeaturesUserConfiguration.\r
\r
  @return  The pointer to CPU feature bits mask buffer.\r
**/\r
UINT8 *\r
GetConfigurationPcd (\r
  VOID\r
  )\r
{\r
  UINT8                  *SupportBitMask;\r
\r
  SupportBitMask = AllocateCopyPool (\r
          PcdGetSize (PcdCpuFeaturesUserConfiguration),\r
          PcdGetPtr (PcdCpuFeaturesUserConfiguration)\r
          );\r
  ASSERT (SupportBitMask != NULL);\r
\r
  return SupportBitMask;\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
  @param[in]  NumberOfCpus  Number of processor in system\r
**/\r
VOID\r
CpuInitDataInitialize (\r
  IN UINTN                             NumberOfCpus\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
  BOOLEAN                              *CoresVisited;\r
  UINTN                                Index;\r
  ACPI_CPU_DATA                        *AcpiCpuData;\r
  CPU_STATUS_INFORMATION               *CpuStatus;\r
  UINT32                               *ValidCoreCountPerPackage;\r
\r
  Core    = 0;\r
  Package = 0;\r
  Thread  = 0;\r
\r
  CpuFeaturesData = GetCpuFeaturesData ();\r
  CpuFeaturesData->InitOrder = AllocateZeroPool (sizeof (CPU_FEATURES_INIT_ORDER) * NumberOfCpus);\r
  ASSERT (CpuFeaturesData->InitOrder != NULL);\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->CpuStatus;\r
  Location = AllocateZeroPool (sizeof (EFI_CPU_PHYSICAL_LOCATION) * NumberOfCpus);\r
  ASSERT (Location != NULL);\r
  AcpiCpuData->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
  ValidCoreCountPerPackage= AllocateZeroPool (sizeof (UINT32) * CpuStatus->PackageCount);\r
  ASSERT (ValidCoreCountPerPackage != 0);\r
  CpuStatus->ValidCoreCountPerPackage = (EFI_PHYSICAL_ADDRESS)(UINTN)ValidCoreCountPerPackage;\r
  CoresVisited = AllocatePool (sizeof (BOOLEAN) * CpuStatus->MaxCoreCount);\r
  ASSERT (CoresVisited != NULL);\r
\r
  for (Index = 0; Index < CpuStatus->PackageCount; Index ++ ) {\r
    ZeroMem (CoresVisited, sizeof (BOOLEAN) * CpuStatus->MaxCoreCount);\r
    //\r
    // Collect valid cores in Current package.\r
    //\r
    for (ProcessorNumber = 0; ProcessorNumber < NumberOfCpus; ProcessorNumber++) {\r
      Location = &CpuFeaturesData->InitOrder[ProcessorNumber].CpuInfo.ProcessorInfo.Location;\r
      if (Location->Package == Index && !CoresVisited[Location->Core] ) {\r
        //\r
        // The ValidCores position for Location->Core is valid.\r
        // The possible values in ValidCores[Index] are 0 or 1.\r
        // FALSE means no valid threads in this Core.\r
        // TRUE means have valid threads in this core, no matter the thead count is 1 or more.\r
        //\r
        CoresVisited[Location->Core] = TRUE;\r
        ValidCoreCountPerPackage[Index]++;\r
      }\r
    }\r
  }\r
  FreePool (CoresVisited);\r
\r
  for (Index = 0; Index <= Package; Index++) {\r
    DEBUG ((DEBUG_INFO, "Package: %d, Valid Core : %d\n", Index, ValidCoreCountPerPackage[Index]));\r
  }\r
\r
  CpuFeaturesData->CpuFlags.SemaphoreCount = AllocateZeroPool (sizeof (UINT32) * CpuStatus->PackageCount * CpuStatus->MaxCoreCount * CpuStatus->MaxThreadCount);\r
  ASSERT (CpuFeaturesData->CpuFlags.SemaphoreCount != NULL);\r
\r
  //\r
  // Get support and configuration PCDs\r
  //\r
  CpuFeaturesData->SupportPcd       = GetSupportPcd ();\r
  CpuFeaturesData->ConfigurationPcd = GetConfigurationPcd ();\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
**/\r
VOID\r
SupportedMaskOr (\r
  IN UINT8               *SupportedFeatureMask,\r
  IN UINT8               *OrFeatureBitMask\r
  )\r
{\r
  UINTN                  Index;\r
  UINTN                  BitMaskSize;\r
  UINT8                  *Data1;\r
  UINT8                  *Data2;\r
\r
  BitMaskSize = PcdGetSize (PcdCpuFeaturesSupport);\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
**/\r
VOID\r
SupportedMaskAnd (\r
  IN UINT8               *SupportedFeatureMask,\r
  IN UINT8               *AndFeatureBitMask\r
  )\r
{\r
  UINTN                  Index;\r
  UINTN                  BitMaskSize;\r
  UINT8                  *Data1;\r
  UINT8                  *Data2;\r
\r
  BitMaskSize = PcdGetSize (PcdCpuFeaturesSupport);\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
**/\r
VOID\r
SupportedMaskCleanBit (\r
  IN UINT8               *SupportedFeatureMask,\r
  IN UINT8               *AndFeatureBitMask\r
  )\r
{\r
  UINTN                  Index;\r
  UINTN                  BitMaskSize;\r
  UINT8                  *Data1;\r
  UINT8                  *Data2;\r
\r
  BitMaskSize = PcdGetSize (PcdCpuFeaturesSupport);\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
\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
  )\r
{\r
  UINTN                  Index;\r
  UINTN                  BitMaskSize;\r
  UINT8                  *Data1;\r
  UINT8                  *Data2;\r
\r
  BitMaskSize = PcdGetSize (PcdCpuFeaturesSupport);\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 ();\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 (IsBitMaskMatch (CpuFeaturesData->SupportPcd, CpuFeature->FeatureMask)) {\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
          );\r
      } else if (CpuFeature->SupportFunc (ProcessorNumber, CpuInfo, CpuFeature->ConfigData)) {\r
        SupportedMaskOr (\r
          CpuFeaturesData->InitOrder[ProcessorNumber].FeaturesSupportedMask,\r
          CpuFeature->FeatureMask\r
          );\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: %d:   MSR: %x, Bit Start: %d, Bit Length: %d, Value: %lx\r\n",\r
        ProcessorNumber,\r
        RegisterTableEntry->Index,\r
        RegisterTableEntry->ValidBitStart,\r
        RegisterTableEntry->ValidBitLength,\r
        RegisterTableEntry->Value\r
        ));\r
      break;\r
    case ControlRegister:\r
      DEBUG ((\r
        DebugPrintErrorLevel,\r
        "Processor: %d:    CR: %x, Bit Start: %d, Bit Length: %d, Value: %lx\r\n",\r
        ProcessorNumber,\r
        RegisterTableEntry->Index,\r
        RegisterTableEntry->ValidBitStart,\r
        RegisterTableEntry->ValidBitLength,\r
        RegisterTableEntry->Value\r
        ));\r
      break;\r
    case MemoryMapped:\r
      DEBUG ((\r
        DebugPrintErrorLevel,\r
        "Processor: %d:  MMIO: %lx, Bit Start: %d, Bit Length: %d, Value: %lx\r\n",\r
        ProcessorNumber,\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: %d: CACHE: %x, Bit Start: %d, Bit Length: %d, Value: %lx\r\n",\r
        ProcessorNumber,\r
        RegisterTableEntry->Index,\r
        RegisterTableEntry->ValidBitStart,\r
        RegisterTableEntry->ValidBitLength,\r
        RegisterTableEntry->Value\r
        ));\r
      break;\r
    case Semaphore:\r
      DEBUG ((\r
        DebugPrintErrorLevel,\r
        "Processor: %d: Semaphore: Scope Value: %s\r\n",\r
        ProcessorNumber,\r
        mDependTypeStr[MIN ((UINT32)RegisterTableEntry->Value, InvalidDepType)]\r
        ));\r
      break;\r
\r
    default:\r
      break;\r
    }\r
  }\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
\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);\r
  }\r
  //\r
  // Calculate the last setting\r
  //\r
\r
  CpuFeaturesData->SettingPcd = AllocateCopyPool (CpuFeaturesData->BitMaskSize, CpuFeaturesData->CapabilityPcd);\r
  ASSERT (CpuFeaturesData->SettingPcd != NULL);\r
  SupportedMaskAnd (CpuFeaturesData->SettingPcd, CpuFeaturesData->ConfigurationPcd);\r
\r
  //\r
  // Save PCDs and display CPU PCDs\r
  //\r
  SetCapabilityPcd (CpuFeaturesData->CapabilityPcd);\r
  SetSettingPcd (CpuFeaturesData->SettingPcd);\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)) {\r
        if (IsBitMaskMatch (CpuFeature->FeatureMask, CpuFeaturesData->SettingPcd)) {\r
          DEBUG ((DEBUG_INFO, "[Enable   ] "));\r
        } else {\r
          DEBUG ((DEBUG_INFO, "[Disable  ] "));\r
        }\r
      } else {\r
        DEBUG ((DEBUG_INFO, "[Unsupport] "));\r
      }\r
      DumpCpuFeature (CpuFeature);\r
      Entry = Entry->ForwardLink;\r
    }\r
    DEBUG ((DEBUG_INFO, "PcdCpuFeaturesSupport:\n"));\r
    DumpCpuFeatureMask (CpuFeaturesData->SupportPcd);\r
    DEBUG ((DEBUG_INFO, "PcdCpuFeaturesUserConfiguration:\n"));\r
    DumpCpuFeatureMask (CpuFeaturesData->ConfigurationPcd);\r
    DEBUG ((DEBUG_INFO, "PcdCpuFeaturesCapability:\n"));\r
    DumpCpuFeatureMask (CpuFeaturesData->CapabilityPcd);\r
    DEBUG ((DEBUG_INFO, "PcdCpuFeaturesSetting:\n"));\r
    DumpCpuFeatureMask (CpuFeaturesData->SettingPcd);\r
  );\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)) {\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
    NextEntry = Entry->ForwardLink;\r
    while (!IsNull (&CpuInitOrder->OrderList, Entry)) {\r
      CpuFeatureInOrder = CPU_FEATURE_ENTRY_FROM_LINK (Entry);\r
      if (!IsNull (&CpuInitOrder->OrderList, NextEntry)) {\r
        NextCpuFeatureInOrder = CPU_FEATURE_ENTRY_FROM_LINK (NextEntry);\r
      } else {\r
        NextCpuFeatureInOrder = NULL;\r
      }\r
      Success = FALSE;\r
      if (IsBitMaskMatch (CpuFeatureInOrder->FeatureMask, CpuFeaturesData->SettingPcd)) {\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);\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);\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);\r
          }\r
        } else {\r
          Success = TRUE;\r
        }\r
      }\r
\r
      if (Success) {\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
        if (NextCpuFeatureInOrder != NULL) {\r
          BeforeDep = DetectFeatureScope (CpuFeatureInOrder, TRUE, NextCpuFeatureInOrder->FeatureMask);\r
          AfterDep  = DetectFeatureScope (NextCpuFeatureInOrder, FALSE, CpuFeatureInOrder->FeatureMask);\r
        } else {\r
          BeforeDep = DetectFeatureScope (CpuFeatureInOrder, TRUE, NULL);\r
          AfterDep = NoneDepType;\r
        }\r
        //\r
        // Assume only one of the depend is valid.\r
        //\r
        ASSERT (!(BeforeDep > ThreadDepType && AfterDep > ThreadDepType));\r
        if (BeforeDep > ThreadDepType) {\r
          CPU_REGISTER_TABLE_WRITE32 (ProcessorNumber, Semaphore, 0, BeforeDep);\r
        }\r
        if (AfterDep > ThreadDepType) {\r
          CPU_REGISTER_TABLE_WRITE32 (ProcessorNumber, Semaphore, 0, AfterDep);\r
        }\r
      }\r
\r
      Entry     = Entry->ForwardLink;\r
      NextEntry = 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);\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
  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                    PackageThreadsCount;\r
  UINT32                    CurrentThread;\r
  UINTN                     ProcessorIndex;\r
  UINTN                     ThreadIndex;\r
  UINTN                     ValidThreadCount;\r
  UINT32                    *ValidCoreCountPerPackage;\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
    DEBUG_CODE_BEGIN ();\r
      AcquireSpinLock (&CpuFlags->ConsoleLogLock);\r
      ThreadIndex = ApLocation->Package * CpuStatus->MaxCoreCount * CpuStatus->MaxThreadCount +\r
              ApLocation->Core * CpuStatus->MaxThreadCount +\r
              ApLocation->Thread;\r
      DEBUG ((\r
        DEBUG_INFO,\r
        "Processor = %lu, Entry Index %lu, Type = %s!\n",\r
        (UINT64)ThreadIndex,\r
        (UINT64)Index,\r
        mRegisterTypeStr[MIN ((REGISTER_TYPE)RegisterTableEntry->RegisterType, InvalidReg)]\r
        ));\r
      ReleaseSpinLock (&CpuFlags->ConsoleLogLock);\r
    DEBUG_CODE_END ();\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
      switch (RegisterTableEntry->Index) {\r
      case 0:\r
        Value = AsmReadCr0 ();\r
        Value = (UINTN) BitFieldWrite64 (\r
                          Value,\r
                          RegisterTableEntry->ValidBitStart,\r
                          RegisterTableEntry->ValidBitStart + RegisterTableEntry->ValidBitLength - 1,\r
                          RegisterTableEntry->Value\r
                          );\r
        AsmWriteCr0 (Value);\r
        break;\r
      case 2:\r
        Value = AsmReadCr2 ();\r
        Value = (UINTN) BitFieldWrite64 (\r
                          Value,\r
                          RegisterTableEntry->ValidBitStart,\r
                          RegisterTableEntry->ValidBitStart + RegisterTableEntry->ValidBitLength - 1,\r
                          RegisterTableEntry->Value\r
                          );\r
        AsmWriteCr2 (Value);\r
        break;\r
      case 3:\r
        Value = AsmReadCr3 ();\r
        Value = (UINTN) BitFieldWrite64 (\r
                          Value,\r
                          RegisterTableEntry->ValidBitStart,\r
                          RegisterTableEntry->ValidBitStart + RegisterTableEntry->ValidBitLength - 1,\r
                          RegisterTableEntry->Value\r
                          );\r
        AsmWriteCr3 (Value);\r
        break;\r
      case 4:\r
        Value = AsmReadCr4 ();\r
        Value = (UINTN) BitFieldWrite64 (\r
                          Value,\r
                          RegisterTableEntry->ValidBitStart,\r
                          RegisterTableEntry->ValidBitStart + RegisterTableEntry->ValidBitLength - 1,\r
                          RegisterTableEntry->Value\r
                          );\r
        AsmWriteCr4 (Value);\r
        break;\r
      case 8:\r
        //\r
        //  Do we need to support CR8?\r
        //\r
        break;\r
      default:\r
        break;\r
      }\r
      break;\r
    //\r
    // The specified register is Model Specific Register\r
    //\r
    case Msr:\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
      SemaphorePtr = CpuFlags->SemaphoreCount;\r
      switch (RegisterTableEntry->Value) {\r
      case CoreDepType:\r
        //\r
        // Get Offset info for the first thread in the core which current thread belongs to.\r
        //\r
        FirstThread = (ApLocation->Package * CpuStatus->MaxCoreCount + ApLocation->Core) * CpuStatus->MaxThreadCount;\r
        CurrentThread = FirstThread + ApLocation->Thread;\r
        //\r
        // First Notify all threads in current Core that this thread has ready.\r
        //\r
        for (ProcessorIndex = 0; ProcessorIndex < CpuStatus->MaxThreadCount; ProcessorIndex ++) {\r
          LibReleaseSemaphore ((UINT32 *) &SemaphorePtr[FirstThread + ProcessorIndex]);\r
        }\r
        //\r
        // Second, check whether all valid threads in current core have ready.\r
        //\r
        for (ProcessorIndex = 0; ProcessorIndex < CpuStatus->MaxThreadCount; ProcessorIndex ++) {\r
          LibWaitForSemaphore (&SemaphorePtr[CurrentThread]);\r
        }\r
        break;\r
\r
      case PackageDepType:\r
        ValidCoreCountPerPackage = (UINT32 *)(UINTN)CpuStatus->ValidCoreCountPerPackage;\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
        PackageThreadsCount = CpuStatus->MaxThreadCount * CpuStatus->MaxCoreCount;\r
        CurrentThread = FirstThread + CpuStatus->MaxThreadCount * ApLocation->Core + ApLocation->Thread;\r
        //\r
        // Get the valid thread count for current package.\r
        //\r
        ValidThreadCount = CpuStatus->MaxThreadCount * ValidCoreCountPerPackage[ApLocation->Package];\r
\r
        //\r
        // Different packages may have different valid cores in them. If driver maintail clearly\r
        // cores number in different packages, the logic will be much complicated.\r
        // Here driver just simply records the max core number in all packages and use it as expect\r
        // core 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 has ready.\r
        //\r
        for (ProcessorIndex = 0; ProcessorIndex < PackageThreadsCount ; ProcessorIndex ++) {\r
          LibReleaseSemaphore ((UINT32 *) &SemaphorePtr[FirstThread + ProcessorIndex]);\r
        }\r
        //\r
        // Second, check whether VALID THREADS (not all threads) in current package have ready.\r
        //\r
        for (ProcessorIndex = 0; ProcessorIndex < ValidThreadCount; 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->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->ApLocation + ProcIndex,\r
    &AcpiCpuData->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
  UINTN                  NumberOfCpus;\r
  UINTN                  NumberOfEnabledProcessors;\r
  CPU_FEATURES_DATA      *CpuFeaturesData;\r
\r
  CpuFeaturesData = GetCpuFeaturesData();\r
\r
  GetNumberOfProcessor (&NumberOfCpus, &NumberOfEnabledProcessors);\r
\r
  CpuInitDataInitialize (NumberOfCpus);\r
\r
  //\r
  // Wakeup all APs for data collection.\r
  //\r
  StartupAPsWorker (CollectProcessorData, NULL);\r
\r
  //\r
  // Collect data on BSP\r
  //\r
  CollectProcessorData (CpuFeaturesData);\r
\r
  AnalysisProcessorFeatures (NumberOfCpus);\r
}\r
\r