UefiCpuPkg/Library/RegisterCpuFeaturesLib/RegisterCpuFeaturesLib.c

   1 /** @file
   2   CPU Register Table Library functions.
   3
   4   Copyright (c) 2017, Intel Corporation. All rights reserved.<BR>
   5   This program and the accompanying materials
   6   are licensed and made available under the terms and conditions of the BSD License
   7   which accompanies this distribution.  The full text of the license may be found at
   8   http://opensource.org/licenses/bsd-license.php
   9
  10   THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
  11   WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
  12
  13 **/
  14
  15 #include "RegisterCpuFeatures.h"
  16
  17 /**
  18   Checks if two CPU feature bit masks are equal.
  19
  20   @param[in]  FirstFeatureMask  The first input CPU feature bit mask
  21   @param[in]  SecondFeatureMask The second input CPU feature bit mask
  22
  23   @retval TRUE  Two CPU feature bit masks are equal.
  24   @retval FALSE Two CPU feature bit masks are not equal.
  25 **/
  26 BOOLEAN
  27 IsCpuFeatureMatch (
  28   IN UINT8               *FirstFeatureMask,
  29   IN UINT8               *SecondFeatureMask
  30   )
  31 {
  32   UINTN                 BitMaskSize;
  33
  34   BitMaskSize = PcdGetSize (PcdCpuFeaturesSupport);
  35   if (CompareMem (FirstFeatureMask, SecondFeatureMask, BitMaskSize) == 0) {
  36     return TRUE;
  37   } else {
  38     return FALSE;
  39   }
  40 }
  41
  42 /**
  43   Function that uses DEBUG() macros to display the contents of a a CPU feature bit mask.
  44
  45   @param[in]  FeatureMask  A pointer to the CPU feature bit mask.
  46 **/
  47 VOID
  48 DumpCpuFeatureMask (
  49   IN UINT8               *FeatureMask
  50   )
  51 {
  52   UINTN                  Index;
  53   UINT8                  *Data8;
  54   UINTN                  BitMaskSize;
  55
  56   BitMaskSize = PcdGetSize (PcdCpuFeaturesSupport);
  57   Data8       = (UINT8 *) FeatureMask;
  58   for (Index = 0; Index < BitMaskSize; Index++) {
  59     DEBUG ((DEBUG_INFO, " %02x ", *Data8++));
  60   }
  61   DEBUG ((DEBUG_INFO, "\n"));
  62 }
  63
  64 /**
  65   Dump CPU feature name or CPU feature bit mask.
  66
  67   @param[in]  CpuFeature   Pointer to CPU_FEATURES_ENTRY
  68 **/
  69 VOID
  70 DumpCpuFeature (
  71   IN CPU_FEATURES_ENTRY  *CpuFeature
  72   )
  73 {
  74
  75   if (CpuFeature->FeatureName != NULL) {
  76     DEBUG ((DEBUG_INFO, "FeatureName: %a\n", CpuFeature->FeatureName));
  77   } else {
  78     DEBUG ((DEBUG_INFO, "FeatureMask = "));
  79     DumpCpuFeatureMask (CpuFeature->FeatureMask);
  80   }
  81 }
  82
  83 /**
  84   Determines if the feature bit mask is in dependent CPU feature bit mask buffer.
  85
  86   @param[in]  FeatureMask        Pointer to CPU feature bit mask
  87   @param[in]  DependentBitMask   Pointer to dependent CPU feature bit mask buffer
  88
  89   @retval TRUE  The feature bit mask is in dependent CPU feature bit mask buffer.
  90   @retval FALSE The feature bit mask is not in dependent CPU feature bit mask buffer.
  91 **/
  92 BOOLEAN
  93 IsBitMaskMatchCheck (
  94   IN UINT8        *FeatureMask,
  95   IN UINT8        *DependentBitMask
  96   )
  97 {
  98   UINTN      Index;
  99   UINTN      BitMaskSize;
 100   UINT8      *Data1;
 101   UINT8      *Data2;
 102
 103   BitMaskSize = PcdGetSize (PcdCpuFeaturesSupport);
 104
 105   Data1 = FeatureMask;
 106   Data2 = DependentBitMask;
 107   for (Index = 0; Index < BitMaskSize; Index++) {
 108     if (((*(Data1++)) & (*(Data2++))) != 0) {
 109       return TRUE;
 110     }
 111   }
 112   return FALSE;
 113 }
 114
 115 /**
 116   Checks and adjusts CPU features order per dependency relationship.
 117
 118   @param[in]  FeatureList        Pointer to CPU feature list
 119 **/
 120 VOID
 121 CheckCpuFeaturesDependency (
 122   IN LIST_ENTRY              *FeatureList
 123   )
 124 {
 125   LIST_ENTRY                 *CurrentEntry;
 126   CPU_FEATURES_ENTRY         *CpuFeature;
 127   LIST_ENTRY                 *CheckEntry;
 128   CPU_FEATURES_ENTRY         *CheckFeature;
 129   BOOLEAN                    Swapped;
 130   LIST_ENTRY                 *TempEntry;
 131
 132   CurrentEntry = GetFirstNode (FeatureList);
 133   while (!IsNull (FeatureList, CurrentEntry)) {
 134     Swapped = FALSE;
 135     CpuFeature = CPU_FEATURE_ENTRY_FROM_LINK (CurrentEntry);
 136     if (CpuFeature->BeforeAll) {
 137       //
 138       // Check all features dispatched before this entry
 139       //
 140       CheckEntry = GetFirstNode (FeatureList);
 141       while (CheckEntry != CurrentEntry) {
 142         CheckFeature = CPU_FEATURE_ENTRY_FROM_LINK (CheckEntry);
 143         if (!CheckFeature->BeforeAll) {
 144           //
 145           // If this feature has no BeforeAll flag and is dispatched before CpuFeature,
 146           // insert currentEntry before Checked feature
 147           //
 148           RemoveEntryList (CurrentEntry);
 149           InsertTailList (CheckEntry, CurrentEntry);
 150           Swapped = TRUE;
 151           break;
 152         }
 153         CheckEntry = CheckEntry->ForwardLink;
 154       }
 155       if (Swapped) {
 156         continue;
 157       }
 158     }
 159
 160     if (CpuFeature->AfterAll) {
 161       //
 162       // Check all features dispatched after this entry
 163       //
 164       CheckEntry = GetNextNode (FeatureList, CurrentEntry);
 165       while (!IsNull (FeatureList, CheckEntry)) {
 166         CheckFeature = CPU_FEATURE_ENTRY_FROM_LINK (CheckEntry);
 167         if (!CheckFeature->AfterAll) {
 168           //
 169           // If this feature has no AfterAll flag and is dispatched after CpuFeature,
 170           // insert currentEntry after Checked feature
 171           //
 172           TempEntry = GetNextNode (FeatureList, CurrentEntry);
 173           RemoveEntryList (CurrentEntry);
 174           InsertHeadList (CheckEntry, CurrentEntry);
 175           CurrentEntry = TempEntry;
 176           Swapped = TRUE;
 177           break;
 178         }
 179         CheckEntry = CheckEntry->ForwardLink;
 180       }
 181       if (Swapped) {
 182         continue;
 183       }
 184     }
 185
 186     if (CpuFeature->BeforeFeatureBitMask != NULL) {
 187       //
 188       // Check all features dispatched before this entry
 189       //
 190       CheckEntry = GetFirstNode (FeatureList);
 191       while (CheckEntry != CurrentEntry) {
 192         CheckFeature = CPU_FEATURE_ENTRY_FROM_LINK (CheckEntry);
 193         if (IsBitMaskMatchCheck (CheckFeature->FeatureMask, CpuFeature->BeforeFeatureBitMask)) {
 194           //
 195           // If there is dependency, swap them
 196           //
 197           RemoveEntryList (CurrentEntry);
 198           InsertTailList (CheckEntry, CurrentEntry);
 199           Swapped = TRUE;
 200           break;
 201         }
 202         CheckEntry = CheckEntry->ForwardLink;
 203       }
 204       if (Swapped) {
 205         continue;
 206       }
 207     }
 208
 209     if (CpuFeature->AfterFeatureBitMask != NULL) {
 210       //
 211       // Check all features dispatched after this entry
 212       //
 213       CheckEntry = GetNextNode (FeatureList, CurrentEntry);
 214       while (!IsNull (FeatureList, CheckEntry)) {
 215         CheckFeature = CPU_FEATURE_ENTRY_FROM_LINK (CheckEntry);
 216         if (IsBitMaskMatchCheck (CheckFeature->FeatureMask, CpuFeature->AfterFeatureBitMask)) {
 217           //
 218           // If there is dependency, swap them
 219           //
 220           TempEntry = GetNextNode (FeatureList, CurrentEntry);
 221           RemoveEntryList (CurrentEntry);
 222           InsertHeadList (CheckEntry, CurrentEntry);
 223           CurrentEntry = TempEntry;
 224           Swapped = TRUE;
 225           break;
 226         }
 227         CheckEntry = CheckEntry->ForwardLink;
 228       }
 229       if (Swapped) {
 230         continue;
 231       }
 232     }
 233     //
 234     // No swap happened, check the next feature
 235     //
 236     CurrentEntry = CurrentEntry->ForwardLink;
 237   }
 238 }
 239
 240 /**
 241   Worker function to register CPU Feature.
 242
 243   @param[in]  CpuFeature            Pointer to CPU feature entry
 244
 245   @retval  RETURN_SUCCESS           The CPU feature was successfully registered.
 246   @retval  RETURN_OUT_OF_RESOURCES  There are not enough resources to register
 247                                     the CPU feature.
 248   @retval  RETURN_UNSUPPORTED       Registration of the CPU feature is not
 249                                     supported due to a circular dependency between
 250                                     BEFORE and AFTER features.
 251 **/
 252 RETURN_STATUS
 253 RegisterCpuFeatureWorker (
 254   IN CPU_FEATURES_ENTRY      *CpuFeature
 255   )
 256 {
 257   EFI_STATUS                 Status;
 258   CPU_FEATURES_DATA          *CpuFeaturesData;
 259   CPU_FEATURES_ENTRY         *CpuFeatureEntry;
 260   LIST_ENTRY                 *Entry;
 261   UINTN                      BitMaskSize;
 262   BOOLEAN                    FeatureExist;
 263
 264   BitMaskSize     = PcdGetSize (PcdCpuFeaturesSupport);
 265   CpuFeaturesData = GetCpuFeaturesData ();
 266   if (CpuFeaturesData->FeaturesCount == 0) {
 267     InitializeListHead (&CpuFeaturesData->FeatureList);
 268     InitializeSpinLock (&CpuFeaturesData->MsrLock);
 269     InitializeSpinLock (&CpuFeaturesData->MemoryMappedLock);
 270     CpuFeaturesData->BitMaskSize = (UINT32) BitMaskSize;
 271   }
 272   ASSERT (CpuFeaturesData->BitMaskSize == BitMaskSize);
 273
 274   FeatureExist = FALSE;
 275   CpuFeatureEntry = NULL;
 276   Entry = GetFirstNode (&CpuFeaturesData->FeatureList);
 277   while (!IsNull (&CpuFeaturesData->FeatureList, Entry)) {
 278     CpuFeatureEntry = CPU_FEATURE_ENTRY_FROM_LINK (Entry);
 279     if (IsCpuFeatureMatch (CpuFeature->FeatureMask, CpuFeatureEntry->FeatureMask)) {
 280       //
 281       // If this feature already registered
 282       //
 283       FeatureExist = TRUE;
 284       break;
 285     }
 286     Entry = Entry->ForwardLink;
 287   }
 288
 289   if (!FeatureExist) {
 290     DEBUG ((DEBUG_INFO, "[NEW] "));
 291     DumpCpuFeature (CpuFeature);
 292     InsertTailList (&CpuFeaturesData->FeatureList, &CpuFeature->Link);
 293     CpuFeaturesData->FeaturesCount++;
 294   } else {
 295     DEBUG ((DEBUG_INFO, "[OVERRIDE] "));
 296     DumpCpuFeature (CpuFeature);
 297     ASSERT (CpuFeatureEntry != NULL);
 298     //
 299     // Overwrite original parameters of CPU feature
 300     //
 301     if (CpuFeature->GetConfigDataFunc != NULL) {
 302       CpuFeatureEntry->GetConfigDataFunc = CpuFeature->GetConfigDataFunc;
 303     }
 304     if (CpuFeature->SupportFunc != NULL) {
 305       CpuFeatureEntry->SupportFunc = CpuFeature->SupportFunc;
 306     }
 307     if (CpuFeature->InitializeFunc != NULL) {
 308       CpuFeatureEntry->InitializeFunc = CpuFeature->InitializeFunc;
 309     }
 310     if (CpuFeature->FeatureName != NULL) {
 311       if (CpuFeatureEntry->FeatureName == NULL) {
 312         CpuFeatureEntry->FeatureName = AllocatePool (CPU_FEATURE_NAME_SIZE);
 313         ASSERT (CpuFeatureEntry->FeatureName != NULL);
 314       }
 315       Status = AsciiStrCpyS (CpuFeatureEntry->FeatureName, CPU_FEATURE_NAME_SIZE, CpuFeature->FeatureName);
 316       ASSERT_EFI_ERROR (Status);
 317       FreePool (CpuFeature->FeatureName);
 318     }
 319     if (CpuFeature->BeforeFeatureBitMask != NULL) {
 320       if (CpuFeatureEntry->BeforeFeatureBitMask != NULL) {
 321         FreePool (CpuFeatureEntry->BeforeFeatureBitMask);
 322       }
 323       CpuFeatureEntry->BeforeFeatureBitMask = CpuFeature->BeforeFeatureBitMask;
 324     }
 325     if (CpuFeature->AfterFeatureBitMask != NULL) {
 326       if (CpuFeatureEntry->AfterFeatureBitMask != NULL) {
 327         FreePool (CpuFeatureEntry->AfterFeatureBitMask);
 328       }
 329       CpuFeatureEntry->AfterFeatureBitMask = CpuFeature->AfterFeatureBitMask;
 330     }
 331     CpuFeatureEntry->BeforeAll = CpuFeature->BeforeAll;
 332     CpuFeatureEntry->AfterAll  = CpuFeature->AfterAll;
 333
 334     FreePool (CpuFeature->FeatureMask);
 335     FreePool (CpuFeature);
 336   }
 337   //
 338   // Verify CPU features dependency can change CPU feature order
 339   //
 340   CheckCpuFeaturesDependency (&CpuFeaturesData->FeatureList);
 341   return RETURN_SUCCESS;
 342 }
 343
 344 /**
 345   Sets CPU feature bit mask in CPU feature bit mask buffer.
 346
 347   @param[in]  FeaturesBitMask       Pointer to CPU feature bit mask buffer
 348   @param[in]  Feature               The bit number of the CPU feature
 349   @param[in]  BitMaskSize           CPU feature bit mask buffer size
 350 **/
 351 VOID
 352 SetCpuFeaturesBitMask (
 353   IN UINT8               **FeaturesBitMask,
 354   IN UINT32              Feature,
 355   IN UINTN               BitMaskSize
 356   )
 357 {
 358   UINT8                  *CpuFeaturesBitMask;
 359
 360   ASSERT (FeaturesBitMask != NULL);
 361   CpuFeaturesBitMask = *FeaturesBitMask;
 362   if (CpuFeaturesBitMask == NULL) {
 363     CpuFeaturesBitMask = AllocateZeroPool (BitMaskSize);
 364     ASSERT (CpuFeaturesBitMask != NULL);
 365     *FeaturesBitMask = CpuFeaturesBitMask;
 366   }
 367
 368   CpuFeaturesBitMask  += (Feature / 8);
 369   *CpuFeaturesBitMask |= (UINT8) (1 << (Feature % 8));
 370 }
 371
 372 /**
 373   Registers a CPU Feature.
 374
 375   @param[in]  FeatureName        A Null-terminated Ascii string indicates CPU feature
 376                                  name.
 377   @param[in]  GetConfigDataFunc  CPU feature get configuration data function.  This
 378                                  is an optional parameter that may be NULL.  If NULL,
 379                                  then the most recently registered function for the
 380                                  CPU feature is used.  If no functions are registered
 381                                  for a CPU feature, then the CPU configuration data
 382                                  for the registered feature is NULL.
 383   @param[in]  SupportFunc        CPU feature support function.  This is an optional
 384                                  parameter that may be NULL.  If NULL, then the most
 385                                  recently registered function for the CPU feature is
 386                                  used. If no functions are registered for a CPU
 387                                  feature, then the CPU feature is assumed to be
 388                                  supported by all CPUs.
 389   @param[in]  InitializeFunc     CPU feature initialize function.  This is an optional
 390                                  parameter that may be NULL.  If NULL, then the most
 391                                  recently registered function for the CPU feature is
 392                                  used. If no functions are registered for a CPU
 393                                  feature, then the CPU feature initialization is
 394                                  skipped.
 395   @param[in]  ...                Variable argument list of UINT32 CPU feature value.
 396                                  Values with no modifiers are the features provided
 397                                  by the registered functions.
 398                                  Values with CPU_FEATURE_BEFORE modifier are features
 399                                  that must be initialized after the features provided
 400                                  by the registered functions are used.
 401                                  Values with CPU_FEATURE_AFTER modifier are features
 402                                  that must be initialized before the features provided
 403                                  by the registered functions are used.
 404                                  The last argument in this variable argument list must
 405                                  always be CPU_FEATURE_END.
 406
 407   @retval  RETURN_SUCCESS           The CPU feature was successfully registered.
 408   @retval  RETURN_OUT_OF_RESOURCES  There are not enough resources to register
 409                                     the CPU feature.
 410   @retval  RETURN_UNSUPPORTED       Registration of the CPU feature is not
 411                                     supported due to a circular dependency between
 412                                     BEFORE and AFTER features.
 413
 414   @note This service could be called by BSP only.
 415 **/
 416 RETURN_STATUS
 417 EFIAPI
 418 RegisterCpuFeature (
 419   IN CHAR8                             *FeatureName,       OPTIONAL
 420   IN CPU_FEATURE_GET_CONFIG_DATA       GetConfigDataFunc,  OPTIONAL
 421   IN CPU_FEATURE_SUPPORT               SupportFunc,        OPTIONAL
 422   IN CPU_FEATURE_INITIALIZE            InitializeFunc,     OPTIONAL
 423   ...
 424   )
 425 {
 426   EFI_STATUS                 Status;
 427   VA_LIST                    Marker;
 428   UINT32                     Feature;
 429   UINTN                      BitMaskSize;
 430   CPU_FEATURES_ENTRY         *CpuFeature;
 431   UINT8                      *FeatureMask;
 432   UINT8                      *BeforeFeatureBitMask;
 433   UINT8                      *AfterFeatureBitMask;
 434   BOOLEAN                    BeforeAll;
 435   BOOLEAN                    AfterAll;
 436
 437   FeatureMask          = NULL;
 438   BeforeFeatureBitMask = NULL;
 439   AfterFeatureBitMask  = NULL;
 440   BeforeAll            = FALSE;
 441   AfterAll             = FALSE;
 442
 443   BitMaskSize = PcdGetSize (PcdCpuFeaturesSupport);
 444
 445   VA_START (Marker, InitializeFunc);
 446   Feature = VA_ARG (Marker, UINT32);
 447   while (Feature != CPU_FEATURE_END) {
 448     ASSERT ((Feature & (CPU_FEATURE_BEFORE | CPU_FEATURE_AFTER))
 449                     != (CPU_FEATURE_BEFORE | CPU_FEATURE_AFTER));
 450     ASSERT ((Feature & (CPU_FEATURE_BEFORE_ALL | CPU_FEATURE_AFTER_ALL))
 451                     != (CPU_FEATURE_BEFORE_ALL | CPU_FEATURE_AFTER_ALL));
 452     if (Feature < CPU_FEATURE_BEFORE) {
 453       BeforeAll = ((Feature & CPU_FEATURE_BEFORE_ALL) != 0) ? TRUE : FALSE;
 454       AfterAll  = ((Feature & CPU_FEATURE_AFTER_ALL) != 0) ? TRUE : FALSE;
 455       Feature  &= ~(CPU_FEATURE_BEFORE_ALL | CPU_FEATURE_AFTER_ALL);
 456       ASSERT (FeatureMask == NULL);
 457       SetCpuFeaturesBitMask (&FeatureMask, Feature, BitMaskSize);
 458     } else if ((Feature & CPU_FEATURE_BEFORE) != 0) {
 459       SetCpuFeaturesBitMask (&BeforeFeatureBitMask, Feature & ~CPU_FEATURE_BEFORE, BitMaskSize);
 460     } else if ((Feature & CPU_FEATURE_AFTER) != 0) {
 461       SetCpuFeaturesBitMask (&AfterFeatureBitMask, Feature & ~CPU_FEATURE_AFTER, BitMaskSize);
 462     }
 463     Feature = VA_ARG (Marker, UINT32);
 464   }
 465   VA_END (Marker);
 466
 467   CpuFeature = AllocateZeroPool (sizeof (CPU_FEATURES_ENTRY));
 468   ASSERT (CpuFeature != NULL);
 469   CpuFeature->Signature            = CPU_FEATURE_ENTRY_SIGNATURE;
 470   CpuFeature->FeatureMask          = FeatureMask;
 471   CpuFeature->BeforeFeatureBitMask = BeforeFeatureBitMask;
 472   CpuFeature->AfterFeatureBitMask  = AfterFeatureBitMask;
 473   CpuFeature->BeforeAll            = BeforeAll;
 474   CpuFeature->AfterAll             = AfterAll;
 475   CpuFeature->GetConfigDataFunc    = GetConfigDataFunc;
 476   CpuFeature->SupportFunc          = SupportFunc;
 477   CpuFeature->InitializeFunc       = InitializeFunc;
 478   if (FeatureName != NULL) {
 479     CpuFeature->FeatureName          = AllocatePool (CPU_FEATURE_NAME_SIZE);
 480     ASSERT (CpuFeature->FeatureName != NULL);
 481     Status = AsciiStrCpyS (CpuFeature->FeatureName, CPU_FEATURE_NAME_SIZE, FeatureName);
 482     ASSERT_EFI_ERROR (Status);
 483   }
 484
 485   Status = RegisterCpuFeatureWorker (CpuFeature);
 486   ASSERT_EFI_ERROR (Status);
 487
 488   return RETURN_SUCCESS;
 489 }
 490
 491 /**
 492   Allocates boot service data to save ACPI_CPU_DATA.
 493
 494   @return  Pointer to allocated ACPI_CPU_DATA.
 495 **/
 496 STATIC
 497 ACPI_CPU_DATA *
 498 AllocateAcpiCpuData (
 499   VOID
 500   )
 501 {
 502   EFI_STATUS                           Status;
 503   UINTN                                NumberOfCpus;
 504   UINTN                                NumberOfEnabledProcessors;
 505   ACPI_CPU_DATA                        *AcpiCpuData;
 506   UINTN                                TableSize;
 507   CPU_REGISTER_TABLE                   *RegisterTable;
 508   UINTN                                Index;
 509   EFI_PROCESSOR_INFORMATION            ProcessorInfoBuffer;
 510
 511   AcpiCpuData  = AllocatePages (EFI_SIZE_TO_PAGES (sizeof (ACPI_CPU_DATA)));
 512   ASSERT (AcpiCpuData != NULL);
 513
 514   GetNumberOfProcessor (&NumberOfCpus, &NumberOfEnabledProcessors);
 515   AcpiCpuData->NumberOfCpus = (UINT32)NumberOfCpus;
 516
 517   //
 518   // Allocate buffer for empty RegisterTable and PreSmmInitRegisterTable for all CPUs
 519   //
 520   TableSize = 2 * NumberOfCpus * sizeof (CPU_REGISTER_TABLE);
 521   RegisterTable  = AllocatePages (EFI_SIZE_TO_PAGES (TableSize));
 522   ASSERT (RegisterTable != NULL);
 523
 524   for (Index = 0; Index < NumberOfCpus; Index++) {
 525     Status = GetProcessorInformation (Index, &ProcessorInfoBuffer);
 526     ASSERT_EFI_ERROR (Status);
 527
 528     RegisterTable[Index].InitialApicId      = (UINT32)ProcessorInfoBuffer.ProcessorId;
 529     RegisterTable[Index].TableLength        = 0;
 530     RegisterTable[Index].AllocatedSize      = 0;
 531     RegisterTable[Index].RegisterTableEntry = 0;
 532
 533     RegisterTable[NumberOfCpus + Index].InitialApicId      = (UINT32)ProcessorInfoBuffer.ProcessorId;
 534     RegisterTable[NumberOfCpus + Index].TableLength        = 0;
 535     RegisterTable[NumberOfCpus + Index].AllocatedSize      = 0;
 536     RegisterTable[NumberOfCpus + Index].RegisterTableEntry = 0;
 537   }
 538   AcpiCpuData->RegisterTable           = (EFI_PHYSICAL_ADDRESS)(UINTN)RegisterTable;
 539   AcpiCpuData->PreSmmInitRegisterTable = (EFI_PHYSICAL_ADDRESS)(UINTN)(RegisterTable + NumberOfCpus);
 540
 541   return AcpiCpuData;
 542 }
 543
 544 /**
 545   Enlarges CPU register table for each processor.
 546
 547   @param[in, out]  RegisterTable   Pointer processor's CPU register table
 548 **/
 549 STATIC
 550 VOID
 551 EnlargeRegisterTable (
 552   IN OUT CPU_REGISTER_TABLE            *RegisterTable
 553   )
 554 {
 555   EFI_PHYSICAL_ADDRESS  Address;
 556   UINTN                 UsedPages;
 557
 558   UsedPages = RegisterTable->AllocatedSize / EFI_PAGE_SIZE;
 559   Address  = (UINTN)AllocatePages (UsedPages + 1);
 560   ASSERT (Address != 0);
 561
 562   //
 563   // If there are records existing in the register table, then copy its contents
 564   // to new region and free the old one.
 565   //
 566   if (RegisterTable->AllocatedSize > 0) {
 567     CopyMem (
 568       (VOID *) (UINTN) Address,
 569       (VOID *) (UINTN) RegisterTable->RegisterTableEntry,
 570       RegisterTable->AllocatedSize
 571       );
 572
 573     FreePages ((VOID *)(UINTN)RegisterTable->RegisterTableEntry, UsedPages);
 574   }
 575
 576   //
 577   // Adjust the allocated size and register table base address.
 578   //
 579   RegisterTable->AllocatedSize     += EFI_PAGE_SIZE;
 580   RegisterTable->RegisterTableEntry = Address;
 581 }
 582
 583 /**
 584   Add an entry in specified register table.
 585
 586   This function adds an entry in specified register table, with given register type,
 587   register index, bit section and value.
 588
 589   @param[in]  PreSmmFlag       If TRUE, entry will be added into PreSmm register table
 590                                If FALSE, entry will be added into register table
 591   @param[in]  ProcessorNumber  The index of the CPU to add a register table entry
 592   @param[in]  RegisterType     Type of the register to program
 593   @param[in]  Index            Index of the register to program
 594   @param[in]  ValidBitStart    Start of the bit section
 595   @param[in]  ValidBitLength   Length of the bit section
 596   @param[in]  Value            Value to write
 597 **/
 598 VOID
 599 CpuRegisterTableWriteWorker (
 600   IN BOOLEAN                 PreSmmFlag,
 601   IN UINTN                   ProcessorNumber,
 602   IN REGISTER_TYPE           RegisterType,
 603   IN UINT64                  Index,
 604   IN UINT8                   ValidBitStart,
 605   IN UINT8                   ValidBitLength,
 606   IN UINT64                  Value
 607   )
 608 {
 609   EFI_STATUS               Status;
 610   CPU_FEATURES_DATA        *CpuFeaturesData;
 611   ACPI_CPU_DATA            *AcpiCpuData;
 612   CPU_REGISTER_TABLE       *RegisterTable;
 613   CPU_REGISTER_TABLE_ENTRY *RegisterTableEntry;
 614
 615   CpuFeaturesData = GetCpuFeaturesData ();
 616   if (CpuFeaturesData->RegisterTable == NULL) {
 617     AcpiCpuData = (ACPI_CPU_DATA *) (UINTN) PcdGet64 (PcdCpuS3DataAddress);
 618     if (AcpiCpuData == NULL) {
 619       AcpiCpuData = AllocateAcpiCpuData ();
 620       ASSERT (AcpiCpuData != NULL);
 621       //
 622       // Set PcdCpuS3DataAddress to the base address of the ACPI_CPU_DATA structure
 623       //
 624       Status = PcdSet64S (PcdCpuS3DataAddress, (UINT64)(UINTN)AcpiCpuData);
 625       ASSERT_EFI_ERROR (Status);
 626     }
 627     ASSERT (AcpiCpuData->RegisterTable != 0);
 628     CpuFeaturesData->RegisterTable = (CPU_REGISTER_TABLE *) (UINTN) AcpiCpuData->RegisterTable;
 629     CpuFeaturesData->PreSmmRegisterTable = (CPU_REGISTER_TABLE *) (UINTN) AcpiCpuData->PreSmmInitRegisterTable;
 630   }
 631
 632   if (PreSmmFlag) {
 633     RegisterTable = &CpuFeaturesData->PreSmmRegisterTable[ProcessorNumber];
 634   } else {
 635     RegisterTable = &CpuFeaturesData->RegisterTable[ProcessorNumber];
 636   }
 637
 638   if (RegisterTable->TableLength == RegisterTable->AllocatedSize / sizeof (CPU_REGISTER_TABLE_ENTRY)) {
 639     EnlargeRegisterTable (RegisterTable);
 640   }
 641
 642   //
 643   // Append entry in the register table.
 644   //
 645   RegisterTableEntry = (CPU_REGISTER_TABLE_ENTRY *) (UINTN) RegisterTable->RegisterTableEntry;
 646   RegisterTableEntry[RegisterTable->TableLength].RegisterType   = RegisterType;
 647   RegisterTableEntry[RegisterTable->TableLength].Index          = (UINT32) Index;
 648   RegisterTableEntry[RegisterTable->TableLength].HighIndex      = (UINT32) RShiftU64 (Index, 32);
 649   RegisterTableEntry[RegisterTable->TableLength].ValidBitStart  = ValidBitStart;
 650   RegisterTableEntry[RegisterTable->TableLength].ValidBitLength = ValidBitLength;
 651   RegisterTableEntry[RegisterTable->TableLength].Value          = Value;
 652
 653   RegisterTable->TableLength++;
 654 }
 655
 656 /**
 657   Adds an entry in specified register table.
 658
 659   This function adds an entry in specified register table, with given register type,
 660   register index, bit section and value.
 661
 662   @param[in]  ProcessorNumber  The index of the CPU to add a register table entry
 663   @param[in]  RegisterType     Type of the register to program
 664   @param[in]  Index            Index of the register to program
 665   @param[in]  ValueMask        Mask of bits in register to write
 666   @param[in]  Value            Value to write
 667
 668   @note This service could be called by BSP only.
 669 **/
 670 VOID
 671 EFIAPI
 672 CpuRegisterTableWrite (
 673   IN UINTN               ProcessorNumber,
 674   IN REGISTER_TYPE       RegisterType,
 675   IN UINT64              Index,
 676   IN UINT64              ValueMask,
 677   IN UINT64              Value
 678   )
 679 {
 680   UINT8                   Start;
 681   UINT8                   End;
 682   UINT8                   Length;
 683
 684   Start  = (UINT8)LowBitSet64  (ValueMask);
 685   End    = (UINT8)HighBitSet64 (ValueMask);
 686   Length = End - Start + 1;
 687   CpuRegisterTableWriteWorker (FALSE, ProcessorNumber, RegisterType, Index, Start, Length, Value);
 688 }
 689
 690 /**
 691   Adds an entry in specified Pre-SMM register table.
 692
 693   This function adds an entry in specified register table, with given register type,
 694   register index, bit section and value.
 695
 696   @param[in]  ProcessorNumber  The index of the CPU to add a register table entry.
 697   @param[in]  RegisterType     Type of the register to program
 698   @param[in]  Index            Index of the register to program
 699   @param[in]  ValueMask        Mask of bits in register to write
 700   @param[in]  Value            Value to write
 701
 702   @note This service could be called by BSP only.
 703 **/
 704 VOID
 705 EFIAPI
 706 PreSmmCpuRegisterTableWrite (
 707   IN UINTN               ProcessorNumber,
 708   IN REGISTER_TYPE       RegisterType,
 709   IN UINT64              Index,
 710   IN UINT64              ValueMask,
 711   IN UINT64              Value
 712   )
 713 {
 714   UINT8  Start;
 715   UINT8  End;
 716   UINT8  Length;
 717
 718   Start  = (UINT8)LowBitSet64  (ValueMask);
 719   End    = (UINT8)HighBitSet64 (ValueMask);
 720   Length = End - Start + 1;
 721   CpuRegisterTableWriteWorker (TRUE, ProcessorNumber, RegisterType, Index, Start, Length, Value);
 722 }
 723
 724 /**
 725   Worker function to determine if a CPU feature is set in input CPU feature bit mask buffer.
 726
 727   @param[in]  CpuBitMask      CPU feature bit mask buffer
 728   @param[in]  CpuBitMaskSize  The size of CPU feature bit mask buffer
 729   @param[in]  Feature         The bit number of the CPU feature
 730
 731   @retval  TRUE   The CPU feature is set in PcdCpuFeaturesSupport.
 732   @retval  FALSE  The CPU feature is not set in PcdCpuFeaturesSupport.
 733
 734 **/
 735 BOOLEAN
 736 IsCpuFeatureSetInCpuPcd (
 737   IN UINT8               *CpuBitMask,
 738   IN UINTN               CpuBitMaskSize,
 739   IN UINT32              Feature
 740   )
 741 {
 742   if ((Feature >> 3) >= CpuBitMaskSize) {
 743     return FALSE;
 744   }
 745   return ((*(CpuBitMask + (Feature >> 3)) & (1 << (Feature & 0x07))) != 0);
 746 }
 747
 748 /**
 749   Determines if a CPU feature is enabled in PcdCpuFeaturesSupport bit mask.
 750   If a CPU feature is disabled in PcdCpuFeaturesSupport then all the code/data
 751   associated with that feature should be optimized away if compiler
 752   optimizations are enabled.
 753
 754   @param[in]  Feature  The bit number of the CPU feature to check in the PCD
 755                        PcdCpuFeaturesSupport
 756
 757   @retval  TRUE   The CPU feature is set in PcdCpuFeaturesSupport.
 758   @retval  FALSE  The CPU feature is not set in PcdCpuFeaturesSupport.
 759
 760   @note This service could be called by BSP only.
 761 **/
 762 BOOLEAN
 763 EFIAPI
 764 IsCpuFeatureSupported (
 765   IN UINT32              Feature
 766   )
 767 {
 768   return IsCpuFeatureSetInCpuPcd (
 769            (UINT8 *)PcdGetPtr (PcdCpuFeaturesSupport),
 770            PcdGetSize (PcdCpuFeaturesSupport),
 771            Feature
 772            );
 773 }
 774
 775 /**
 776   Determines if a CPU feature is set in PcdCpuFeaturesSetting bit mask.
 777
 778   @param[in]  Feature  The bit number of the CPU feature to check in the PCD
 779                        PcdCpuFeaturesSetting
 780
 781   @retval  TRUE   The CPU feature is set in PcdCpuFeaturesSetting.
 782   @retval  FALSE  The CPU feature is not set in PcdCpuFeaturesSetting.
 783
 784   @note This service could be called by BSP only.
 785 **/
 786 BOOLEAN
 787 EFIAPI
 788 IsCpuFeatureInSetting (
 789   IN UINT32              Feature
 790   )
 791 {
 792   return IsCpuFeatureSetInCpuPcd (
 793            (UINT8 *)PcdGetPtr (PcdCpuFeaturesSetting),
 794            PcdGetSize (PcdCpuFeaturesSetting),
 795            Feature
 796            );
 797 }
 798
 799 /**
 800   Determines if a CPU feature is set in PcdCpuFeaturesCapability bit mask.
 801
 802   @param[in]  Feature  The bit number of the CPU feature to check in the PCD
 803                        PcdCpuFeaturesCapability
 804
 805   @retval  TRUE   The CPU feature is set in PcdCpuFeaturesCapability.
 806   @retval  FALSE  The CPU feature is not set in PcdCpuFeaturesCapability.
 807
 808   @note This service could be called by BSP only.
 809 **/
 810 BOOLEAN
 811 EFIAPI
 812 IsCpuFeatureCapability (
 813   IN UINT32              Feature
 814   )
 815 {
 816   return IsCpuFeatureSetInCpuPcd (
 817            (UINT8 *)PcdGetPtr (PcdCpuFeaturesCapability),
 818            PcdGetSize (PcdCpuFeaturesCapability),
 819            Feature
 820            );
 821
 822 }
 823
 824 /**
 825   Determines if a CPU feature is set in PcdCpuFeaturesUserConfiguration bit mask.
 826
 827   @param[in]  Feature  The bit number of the CPU feature to check in the PCD
 828                        PcdCpuFeaturesUserConfiguration
 829
 830   @retval  TRUE   The CPU feature is set in PcdCpuFeaturesUserConfiguration.
 831   @retval  FALSE  The CPU feature is not set in PcdCpuFeaturesUserConfiguration.
 832
 833   @note This service could be called by BSP only.
 834 **/
 835 BOOLEAN
 836 EFIAPI
 837 IsCpuFeatureUserConfiguration (
 838   IN UINT32              Feature
 839   )
 840 {
 841   return IsCpuFeatureSetInCpuPcd (
 842            (UINT8 *)PcdGetPtr (PcdCpuFeaturesUserConfiguration),
 843            PcdGetSize (PcdCpuFeaturesUserConfiguration),
 844            Feature
 845            );
 846
 847 }
 848
 849 /**
 850   Switches to assigned BSP after CPU features initialization.
 851
 852   @param[in]  ProcessorNumber  The index of the CPU executing this function.
 853
 854   @note This service could be called by BSP only.
 855 **/
 856 VOID
 857 EFIAPI
 858 SwitchBspAfterFeaturesInitialize (
 859   IN UINTN               ProcessorNumber
 860   )
 861 {
 862   CPU_FEATURES_DATA      *CpuFeaturesData;
 863
 864   CpuFeaturesData = GetCpuFeaturesData ();
 865   CpuFeaturesData->BspNumber = ProcessorNumber;
 866 }
 867