UefiCpuPkg/Library/RegisterCpuFeaturesLib/RegisterCpuFeaturesLib.c

   1 /** @file
   2   CPU Register Table Library functions.
   3
   4   Copyright (c) 2017 - 2020, Intel Corporation. All rights reserved.<BR>
   5   SPDX-License-Identifier: BSD-2-Clause-Patent
   6
   7 **/
   8
   9 #include "RegisterCpuFeatures.h"
  10
  11 /**
  12   Function that uses DEBUG() macros to display the contents of a a CPU feature bit mask.
  13
  14   @param[in]  FeatureMask  A pointer to the CPU feature bit mask.
  15   @param[in]  BitMaskSize  CPU feature bits mask buffer size.
  16
  17 **/
  18 VOID
  19 DumpCpuFeatureMask (
  20   IN UINT8               *FeatureMask,
  21   IN UINT32              BitMaskSize
  22   )
  23 {
  24   UINTN                  Index;
  25   UINT8                  *Data8;
  26
  27   Data8       = (UINT8 *) FeatureMask;
  28   for (Index = 0; Index < BitMaskSize; Index++) {
  29     DEBUG ((DEBUG_INFO, " %02x ", *Data8++));
  30   }
  31   DEBUG ((DEBUG_INFO, "\n"));
  32 }
  33
  34 /**
  35   Dump CPU feature name or CPU feature bit mask.
  36
  37   @param[in]  CpuFeature   Pointer to CPU_FEATURES_ENTRY
  38   @param[in]  BitMaskSize  CPU feature bits mask buffer size.
  39
  40 **/
  41 VOID
  42 DumpCpuFeature (
  43   IN CPU_FEATURES_ENTRY  *CpuFeature,
  44   IN UINT32              BitMaskSize
  45   )
  46 {
  47
  48   if (CpuFeature->FeatureName != NULL) {
  49     DEBUG ((DEBUG_INFO, "FeatureName: %a\n", CpuFeature->FeatureName));
  50   } else {
  51     DEBUG ((DEBUG_INFO, "FeatureMask = "));
  52     DumpCpuFeatureMask (CpuFeature->FeatureMask, BitMaskSize);
  53   }
  54 }
  55
  56 /**
  57   Determines if the feature bit mask is in dependent CPU feature bit mask buffer.
  58
  59   @param[in]  FeatureMask        Pointer to CPU feature bit mask
  60   @param[in]  DependentBitMask   Pointer to dependent CPU feature bit mask buffer
  61
  62   @retval TRUE  The feature bit mask is in dependent CPU feature bit mask buffer.
  63   @retval FALSE The feature bit mask is not in dependent CPU feature bit mask buffer.
  64 **/
  65 BOOLEAN
  66 IsBitMaskMatchCheck (
  67   IN UINT8        *FeatureMask,
  68   IN UINT8        *DependentBitMask
  69   )
  70 {
  71   UINTN              Index;
  72   UINT8              *Data1;
  73   UINT8              *Data2;
  74   CPU_FEATURES_DATA  *CpuFeaturesData;
  75
  76   CpuFeaturesData = GetCpuFeaturesData ();
  77
  78   Data1 = FeatureMask;
  79   Data2 = DependentBitMask;
  80   for (Index = 0; Index < CpuFeaturesData->BitMaskSize; Index++) {
  81     if (((*(Data1++)) & (*(Data2++))) != 0) {
  82       return TRUE;
  83     }
  84   }
  85   return FALSE;
  86 }
  87
  88 /**
  89   Try to find the specify cpu featuren in former/after feature list.
  90
  91   @param[in]  FeatureList        Pointer to dependent CPU feature list
  92   @param[in]  CurrentEntry       Pointer to current CPU feature entry.
  93   @param[in]  SearchFormer       Find in former feature or after features.
  94   @param[in]  FeatureMask        Pointer to CPU feature bit mask
  95
  96   @retval TRUE  The feature bit mask is in dependent CPU feature bit mask buffer.
  97   @retval FALSE The feature bit mask is not in dependent CPU feature bit mask buffer.
  98 **/
  99 BOOLEAN
 100 FindSpecifyFeature (
 101   IN LIST_ENTRY              *FeatureList,
 102   IN LIST_ENTRY              *CurrentEntry,
 103   IN BOOLEAN                 SearchFormer,
 104   IN UINT8                   *FeatureMask
 105   )
 106 {
 107   CPU_FEATURES_ENTRY         *CpuFeature;
 108   LIST_ENTRY                 *NextEntry;
 109
 110   //
 111   // Check whether exist the not neighborhood entry first.
 112   // If not exist, return FALSE means not found status.
 113   //
 114   if (SearchFormer) {
 115     NextEntry = CurrentEntry->BackLink;
 116     if (IsNull (FeatureList, NextEntry)) {
 117       return FALSE;
 118     }
 119
 120     NextEntry = NextEntry->BackLink;
 121     if (IsNull (FeatureList, NextEntry)) {
 122       return FALSE;
 123     }
 124
 125     NextEntry = CurrentEntry->BackLink->BackLink;
 126   } else {
 127     NextEntry = CurrentEntry->ForwardLink;
 128     if (IsNull (FeatureList, NextEntry)) {
 129       return FALSE;
 130     }
 131
 132     NextEntry = NextEntry->ForwardLink;
 133     if (IsNull (FeatureList, NextEntry)) {
 134       return FALSE;
 135     }
 136
 137     NextEntry = CurrentEntry->ForwardLink->ForwardLink;
 138   }
 139
 140   while (!IsNull (FeatureList, NextEntry)) {
 141     CpuFeature = CPU_FEATURE_ENTRY_FROM_LINK (NextEntry);
 142
 143     if (IsBitMaskMatchCheck (FeatureMask, CpuFeature->FeatureMask)) {
 144       return TRUE;
 145     }
 146
 147     if (SearchFormer) {
 148       NextEntry = NextEntry->BackLink;
 149     } else {
 150       NextEntry = NextEntry->ForwardLink;
 151     }
 152   }
 153
 154   return FALSE;
 155 }
 156
 157 /**
 158   Return feature dependence result.
 159
 160   @param[in]  CpuFeature            Pointer to CPU feature.
 161   @param[in]  Before                Check before dependence or after.
 162   @param[in]  NextCpuFeatureMask    Pointer to next CPU feature Mask.
 163
 164   @retval     return the dependence result.
 165 **/
 166 CPU_FEATURE_DEPENDENCE_TYPE
 167 DetectFeatureScope (
 168   IN CPU_FEATURES_ENTRY         *CpuFeature,
 169   IN BOOLEAN                    Before,
 170   IN UINT8                      *NextCpuFeatureMask
 171   )
 172 {
 173   //
 174   // if need to check before type dependence but the feature after current feature is not
 175   // exist, means this before type dependence not valid, just return NoneDepType.
 176   // Just like Feature A has a dependence of feature B, but Feature B not installed, so
 177   // Feature A maybe insert to the last entry of the list. In this case, for below code,
 178   // Featrure A has depend of feature B, but it is the last entry of the list, so the
 179   // NextCpuFeatureMask is NULL, so the dependence for feature A here is useless and code
 180   // just return NoneDepType.
 181   //
 182   if (NextCpuFeatureMask == NULL) {
 183     return NoneDepType;
 184   }
 185
 186   if (Before) {
 187     if ((CpuFeature->PackageBeforeFeatureBitMask != NULL) &&
 188         IsBitMaskMatchCheck (NextCpuFeatureMask, CpuFeature->PackageBeforeFeatureBitMask)) {
 189       return PackageDepType;
 190     }
 191
 192     if ((CpuFeature->CoreBeforeFeatureBitMask != NULL) &&
 193         IsBitMaskMatchCheck (NextCpuFeatureMask, CpuFeature->CoreBeforeFeatureBitMask)) {
 194       return CoreDepType;
 195     }
 196
 197     if ((CpuFeature->BeforeFeatureBitMask != NULL) &&
 198         IsBitMaskMatchCheck (NextCpuFeatureMask, CpuFeature->BeforeFeatureBitMask)) {
 199       return ThreadDepType;
 200     }
 201
 202     return NoneDepType;
 203   }
 204
 205   if ((CpuFeature->PackageAfterFeatureBitMask != NULL) &&
 206       IsBitMaskMatchCheck (NextCpuFeatureMask, CpuFeature->PackageAfterFeatureBitMask)) {
 207     return PackageDepType;
 208   }
 209
 210   if ((CpuFeature->CoreAfterFeatureBitMask != NULL) &&
 211       IsBitMaskMatchCheck (NextCpuFeatureMask, CpuFeature->CoreAfterFeatureBitMask)) {
 212     return CoreDepType;
 213   }
 214
 215   if ((CpuFeature->AfterFeatureBitMask != NULL) &&
 216       IsBitMaskMatchCheck (NextCpuFeatureMask, CpuFeature->AfterFeatureBitMask)) {
 217     return ThreadDepType;
 218   }
 219
 220   return NoneDepType;
 221 }
 222
 223 /**
 224   Return feature dependence result.
 225
 226   @param[in]  CpuFeature            Pointer to CPU feature.
 227   @param[in]  Before                Check before dependence or after.
 228   @param[in]  FeatureList           Pointer to CPU feature list.
 229
 230   @retval     return the dependence result.
 231 **/
 232 CPU_FEATURE_DEPENDENCE_TYPE
 233 DetectNoneNeighborhoodFeatureScope (
 234   IN CPU_FEATURES_ENTRY         *CpuFeature,
 235   IN BOOLEAN                    Before,
 236   IN LIST_ENTRY                 *FeatureList
 237   )
 238 {
 239   if (Before) {
 240     if ((CpuFeature->PackageBeforeFeatureBitMask != NULL) &&
 241         FindSpecifyFeature(FeatureList, &CpuFeature->Link, FALSE, CpuFeature->PackageBeforeFeatureBitMask)) {
 242       return PackageDepType;
 243     }
 244
 245     if ((CpuFeature->CoreBeforeFeatureBitMask != NULL) &&
 246         FindSpecifyFeature(FeatureList, &CpuFeature->Link, FALSE, CpuFeature->CoreBeforeFeatureBitMask)) {
 247       return CoreDepType;
 248     }
 249
 250     if ((CpuFeature->BeforeFeatureBitMask != NULL) &&
 251         FindSpecifyFeature(FeatureList, &CpuFeature->Link, FALSE, CpuFeature->BeforeFeatureBitMask)) {
 252       return ThreadDepType;
 253     }
 254
 255     return NoneDepType;
 256   }
 257
 258   if ((CpuFeature->PackageAfterFeatureBitMask != NULL) &&
 259       FindSpecifyFeature(FeatureList, &CpuFeature->Link, TRUE, CpuFeature->PackageAfterFeatureBitMask)) {
 260     return PackageDepType;
 261   }
 262
 263   if ((CpuFeature->CoreAfterFeatureBitMask != NULL) &&
 264       FindSpecifyFeature(FeatureList, &CpuFeature->Link, TRUE, CpuFeature->CoreAfterFeatureBitMask)) {
 265     return CoreDepType;
 266   }
 267
 268   if ((CpuFeature->AfterFeatureBitMask != NULL) &&
 269       FindSpecifyFeature(FeatureList, &CpuFeature->Link, TRUE, CpuFeature->AfterFeatureBitMask)) {
 270     return ThreadDepType;
 271   }
 272
 273   return NoneDepType;
 274 }
 275
 276 /**
 277   Base on dependence relationship to asjust feature dependence.
 278
 279   ONLY when the feature before(or after) the find feature also has
 280   dependence with the find feature. In this case, driver need to base
 281   on dependce relationship to decide how to insert current feature and
 282   adjust the feature dependence.
 283
 284   @param[in, out]  PreviousFeature    CPU feature current before the find one.
 285   @param[in, out]  CurrentFeature     Cpu feature need to adjust.
 286   @param[in]       FindFeature        Cpu feature which current feature depends.
 287   @param[in]       Before             Before or after dependence relationship.
 288
 289   @retval   TRUE   means the current feature dependence has been adjusted.
 290
 291   @retval   FALSE  means the previous feature dependence has been adjusted.
 292                    or previous feature has no dependence with the find one.
 293
 294 **/
 295 BOOLEAN
 296 AdjustFeaturesDependence (
 297   IN OUT CPU_FEATURES_ENTRY         *PreviousFeature,
 298   IN OUT CPU_FEATURES_ENTRY         *CurrentFeature,
 299   IN     CPU_FEATURES_ENTRY         *FindFeature,
 300   IN     BOOLEAN                    Before
 301   )
 302 {
 303   CPU_FEATURE_DEPENDENCE_TYPE            PreDependType;
 304   CPU_FEATURE_DEPENDENCE_TYPE            CurrentDependType;
 305
 306   PreDependType     = DetectFeatureScope(PreviousFeature, Before, FindFeature->FeatureMask);
 307   CurrentDependType = DetectFeatureScope(CurrentFeature, Before, FindFeature->FeatureMask);
 308
 309   //
 310   // If previous feature has no dependence with the find featue.
 311   // return FALSE.
 312   //
 313   if (PreDependType == NoneDepType) {
 314     return FALSE;
 315   }
 316
 317   //
 318   // If both feature have dependence, keep the one which needs use more
 319   // processors and clear the dependence for the other one.
 320   //
 321   if (PreDependType >= CurrentDependType) {
 322     return TRUE;
 323   } else {
 324     return FALSE;
 325   }
 326 }
 327
 328 /**
 329   Base on dependence relationship to asjust feature order.
 330
 331   @param[in]       FeatureList        Pointer to CPU feature list
 332   @param[in, out]  FindEntry          The entry this feature depend on.
 333   @param[in, out]  CurrentEntry       The entry for this feature.
 334   @param[in]       Before             Before or after dependence relationship.
 335
 336 **/
 337 VOID
 338 AdjustEntry (
 339   IN      LIST_ENTRY                *FeatureList,
 340   IN OUT  LIST_ENTRY                *FindEntry,
 341   IN OUT  LIST_ENTRY                *CurrentEntry,
 342   IN      BOOLEAN                   Before
 343   )
 344 {
 345   LIST_ENTRY                *PreviousEntry;
 346   CPU_FEATURES_ENTRY        *PreviousFeature;
 347   CPU_FEATURES_ENTRY        *CurrentFeature;
 348   CPU_FEATURES_ENTRY        *FindFeature;
 349
 350   //
 351   // For CPU feature which has core or package type dependence, later code need to insert
 352   // AcquireSpinLock/ReleaseSpinLock logic to sequency the execute order.
 353   // So if driver finds both feature A and B need to execute before feature C, driver will
 354   // base on dependence type of feature A and B to update the logic here.
 355   // For example, feature A has package type dependence and feature B has core type dependence,
 356   // because package type dependence need to wait for more processors which has strong dependence
 357   // than core type dependence. So driver will adjust the feature order to B -> A -> C. and driver
 358   // will remove the feature dependence in feature B.
 359   // Driver just needs to make sure before feature C been executed, feature A has finished its task
 360   // in all all thread. Feature A finished in all threads also means feature B have finshed in all
 361   // threads.
 362   //
 363   if (Before) {
 364     PreviousEntry = GetPreviousNode (FeatureList, FindEntry);
 365   } else {
 366
 367     PreviousEntry = GetNextNode (FeatureList, FindEntry);
 368   }
 369
 370   CurrentFeature  = CPU_FEATURE_ENTRY_FROM_LINK (CurrentEntry);
 371   RemoveEntryList (CurrentEntry);
 372
 373   if (IsNull (FeatureList, PreviousEntry)) {
 374     //
 375     // If not exist the previous or next entry, just insert the current entry.
 376     //
 377     if (Before) {
 378       InsertTailList (FindEntry, CurrentEntry);
 379     } else {
 380       InsertHeadList (FindEntry, CurrentEntry);
 381     }
 382   } else {
 383     //
 384     // If exist the previous or next entry, need to check it before insert curent entry.
 385     //
 386     PreviousFeature = CPU_FEATURE_ENTRY_FROM_LINK (PreviousEntry);
 387     FindFeature     = CPU_FEATURE_ENTRY_FROM_LINK (FindEntry);
 388
 389     if (AdjustFeaturesDependence (PreviousFeature, CurrentFeature, FindFeature, Before)) {
 390       //
 391       // Return TRUE means current feature dependence has been cleared and the previous
 392       // feature dependence has been kept and used. So insert current feature before (or after)
 393       // the previous feature.
 394       //
 395       if (Before) {
 396         InsertTailList (PreviousEntry, CurrentEntry);
 397       } else {
 398         InsertHeadList (PreviousEntry, CurrentEntry);
 399       }
 400     } else {
 401       if (Before) {
 402         InsertTailList (FindEntry, CurrentEntry);
 403       } else {
 404         InsertHeadList (FindEntry, CurrentEntry);
 405       }
 406     }
 407   }
 408 }
 409
 410
 411 /**
 412   Checks and adjusts current CPU features per dependency relationship.
 413
 414   @param[in]  FeatureList        Pointer to CPU feature list
 415   @param[in]  CurrentEntry       Pointer to current checked CPU feature
 416   @param[in]  FeatureMask        The feature bit mask.
 417
 418   @retval     return Swapped info.
 419 **/
 420 BOOLEAN
 421 InsertToBeforeEntry (
 422   IN LIST_ENTRY              *FeatureList,
 423   IN LIST_ENTRY              *CurrentEntry,
 424   IN UINT8                   *FeatureMask
 425   )
 426 {
 427   LIST_ENTRY                 *CheckEntry;
 428   CPU_FEATURES_ENTRY         *CheckFeature;
 429   BOOLEAN                    Swapped;
 430
 431   Swapped = FALSE;
 432
 433   //
 434   // Check all features dispatched before this entry
 435   //
 436   CheckEntry = GetFirstNode (FeatureList);
 437   while (CheckEntry != CurrentEntry) {
 438     CheckFeature = CPU_FEATURE_ENTRY_FROM_LINK (CheckEntry);
 439     if (IsBitMaskMatchCheck (CheckFeature->FeatureMask, FeatureMask)) {
 440       AdjustEntry (FeatureList, CheckEntry, CurrentEntry, TRUE);
 441       Swapped = TRUE;
 442       break;
 443     }
 444     CheckEntry = CheckEntry->ForwardLink;
 445   }
 446
 447   return Swapped;
 448 }
 449
 450 /**
 451   Checks and adjusts current CPU features per dependency relationship.
 452
 453   @param[in]  FeatureList        Pointer to CPU feature list
 454   @param[in]  CurrentEntry       Pointer to current checked CPU feature
 455   @param[in]  FeatureMask        The feature bit mask.
 456
 457   @retval     return Swapped info.
 458 **/
 459 BOOLEAN
 460 InsertToAfterEntry (
 461   IN LIST_ENTRY              *FeatureList,
 462   IN LIST_ENTRY              *CurrentEntry,
 463   IN UINT8                   *FeatureMask
 464   )
 465 {
 466   LIST_ENTRY                 *CheckEntry;
 467   CPU_FEATURES_ENTRY         *CheckFeature;
 468   BOOLEAN                    Swapped;
 469
 470   Swapped = FALSE;
 471
 472   //
 473   // Check all features dispatched after this entry
 474   //
 475   CheckEntry = GetNextNode (FeatureList, CurrentEntry);
 476   while (!IsNull (FeatureList, CheckEntry)) {
 477     CheckFeature = CPU_FEATURE_ENTRY_FROM_LINK (CheckEntry);
 478     if (IsBitMaskMatchCheck (CheckFeature->FeatureMask, FeatureMask)) {
 479       AdjustEntry (FeatureList, CheckEntry, CurrentEntry, FALSE);
 480       Swapped = TRUE;
 481       break;
 482     }
 483     CheckEntry = CheckEntry->ForwardLink;
 484   }
 485
 486   return Swapped;
 487 }
 488
 489 /**
 490   Checks and adjusts CPU features order per dependency relationship.
 491
 492   @param[in]  FeatureList        Pointer to CPU feature list
 493 **/
 494 VOID
 495 CheckCpuFeaturesDependency (
 496   IN LIST_ENTRY              *FeatureList
 497   )
 498 {
 499   LIST_ENTRY                 *CurrentEntry;
 500   CPU_FEATURES_ENTRY         *CpuFeature;
 501   LIST_ENTRY                 *CheckEntry;
 502   CPU_FEATURES_ENTRY         *CheckFeature;
 503   BOOLEAN                    Swapped;
 504   LIST_ENTRY                 *TempEntry;
 505   LIST_ENTRY                 *NextEntry;
 506
 507   CurrentEntry = GetFirstNode (FeatureList);
 508   while (!IsNull (FeatureList, CurrentEntry)) {
 509     Swapped = FALSE;
 510     CpuFeature = CPU_FEATURE_ENTRY_FROM_LINK (CurrentEntry);
 511     NextEntry = CurrentEntry->ForwardLink;
 512     if (CpuFeature->BeforeAll) {
 513       //
 514       // Check all features dispatched before this entry
 515       //
 516       CheckEntry = GetFirstNode (FeatureList);
 517       while (CheckEntry != CurrentEntry) {
 518         CheckFeature = CPU_FEATURE_ENTRY_FROM_LINK (CheckEntry);
 519         if (!CheckFeature->BeforeAll) {
 520           //
 521           // If this feature has no BeforeAll flag and is dispatched before CpuFeature,
 522           // insert currentEntry before Checked feature
 523           //
 524           RemoveEntryList (CurrentEntry);
 525           InsertTailList (CheckEntry, CurrentEntry);
 526           Swapped = TRUE;
 527           break;
 528         }
 529         CheckEntry = CheckEntry->ForwardLink;
 530       }
 531       if (Swapped) {
 532         CurrentEntry = NextEntry;
 533         continue;
 534       }
 535     }
 536
 537     if (CpuFeature->AfterAll) {
 538       //
 539       // Check all features dispatched after this entry
 540       //
 541       CheckEntry = GetNextNode (FeatureList, CurrentEntry);
 542       while (!IsNull (FeatureList, CheckEntry)) {
 543         CheckFeature = CPU_FEATURE_ENTRY_FROM_LINK (CheckEntry);
 544         if (!CheckFeature->AfterAll) {
 545           //
 546           // If this feature has no AfterAll flag and is dispatched after CpuFeature,
 547           // insert currentEntry after Checked feature
 548           //
 549           TempEntry = GetNextNode (FeatureList, CurrentEntry);
 550           RemoveEntryList (CurrentEntry);
 551           InsertHeadList (CheckEntry, CurrentEntry);
 552           CurrentEntry = TempEntry;
 553           Swapped = TRUE;
 554           break;
 555         }
 556         CheckEntry = CheckEntry->ForwardLink;
 557       }
 558       if (Swapped) {
 559         CurrentEntry = NextEntry;
 560         continue;
 561       }
 562     }
 563
 564     if (CpuFeature->BeforeFeatureBitMask != NULL) {
 565       Swapped = InsertToBeforeEntry (FeatureList, CurrentEntry, CpuFeature->BeforeFeatureBitMask);
 566       if (Swapped) {
 567         continue;
 568       }
 569     }
 570
 571     if (CpuFeature->AfterFeatureBitMask != NULL) {
 572       Swapped = InsertToAfterEntry (FeatureList, CurrentEntry, CpuFeature->AfterFeatureBitMask);
 573       if (Swapped) {
 574         continue;
 575       }
 576     }
 577
 578     if (CpuFeature->CoreBeforeFeatureBitMask != NULL) {
 579       Swapped = InsertToBeforeEntry (FeatureList, CurrentEntry, CpuFeature->CoreBeforeFeatureBitMask);
 580       if (Swapped) {
 581         continue;
 582       }
 583     }
 584
 585     if (CpuFeature->CoreAfterFeatureBitMask != NULL) {
 586       Swapped = InsertToAfterEntry (FeatureList, CurrentEntry, CpuFeature->CoreAfterFeatureBitMask);
 587       if (Swapped) {
 588         continue;
 589       }
 590     }
 591
 592     if (CpuFeature->PackageBeforeFeatureBitMask != NULL) {
 593       Swapped = InsertToBeforeEntry (FeatureList, CurrentEntry, CpuFeature->PackageBeforeFeatureBitMask);
 594       if (Swapped) {
 595         continue;
 596       }
 597     }
 598
 599     if (CpuFeature->PackageAfterFeatureBitMask != NULL) {
 600       Swapped = InsertToAfterEntry (FeatureList, CurrentEntry, CpuFeature->PackageAfterFeatureBitMask);
 601       if (Swapped) {
 602         continue;
 603       }
 604     }
 605
 606     CurrentEntry = CurrentEntry->ForwardLink;
 607   }
 608 }
 609
 610 /**
 611   Worker function to register CPU Feature.
 612
 613   @param[in]  CpuFeaturesData       Pointer to CPU feature data structure.
 614   @param[in]  CpuFeature            Pointer to CPU feature entry
 615
 616   @retval  RETURN_SUCCESS           The CPU feature was successfully registered.
 617   @retval  RETURN_OUT_OF_RESOURCES  There are not enough resources to register
 618                                     the CPU feature.
 619   @retval  RETURN_UNSUPPORTED       Registration of the CPU feature is not
 620                                     supported due to a circular dependency between
 621                                     BEFORE and AFTER features.
 622 **/
 623 RETURN_STATUS
 624 RegisterCpuFeatureWorker (
 625   IN CPU_FEATURES_DATA       *CpuFeaturesData,
 626   IN CPU_FEATURES_ENTRY      *CpuFeature
 627   )
 628 {
 629   EFI_STATUS                 Status;
 630   CPU_FEATURES_ENTRY         *CpuFeatureEntry;
 631   LIST_ENTRY                 *Entry;
 632   BOOLEAN                    FeatureExist;
 633
 634   FeatureExist = FALSE;
 635   CpuFeatureEntry = NULL;
 636   Entry = GetFirstNode (&CpuFeaturesData->FeatureList);
 637   while (!IsNull (&CpuFeaturesData->FeatureList, Entry)) {
 638     CpuFeatureEntry = CPU_FEATURE_ENTRY_FROM_LINK (Entry);
 639     if (CompareMem (CpuFeature->FeatureMask, CpuFeatureEntry->FeatureMask, CpuFeaturesData->BitMaskSize) == 0) {
 640       //
 641       // If this feature already registered
 642       //
 643       FeatureExist = TRUE;
 644       break;
 645     }
 646     Entry = Entry->ForwardLink;
 647   }
 648
 649   if (!FeatureExist) {
 650     DEBUG ((DEBUG_INFO, "[NEW] "));
 651     DumpCpuFeature (CpuFeature, CpuFeaturesData->BitMaskSize);
 652     InsertTailList (&CpuFeaturesData->FeatureList, &CpuFeature->Link);
 653     CpuFeaturesData->FeaturesCount++;
 654   } else {
 655     DEBUG ((DEBUG_INFO, "[OVERRIDE] "));
 656     DumpCpuFeature (CpuFeature, CpuFeaturesData->BitMaskSize);
 657     ASSERT (CpuFeatureEntry != NULL);
 658     //
 659     // Overwrite original parameters of CPU feature
 660     //
 661     if (CpuFeature->GetConfigDataFunc != NULL) {
 662       CpuFeatureEntry->GetConfigDataFunc = CpuFeature->GetConfigDataFunc;
 663     }
 664     if (CpuFeature->SupportFunc != NULL) {
 665       CpuFeatureEntry->SupportFunc = CpuFeature->SupportFunc;
 666     }
 667     if (CpuFeature->InitializeFunc != NULL) {
 668       CpuFeatureEntry->InitializeFunc = CpuFeature->InitializeFunc;
 669     }
 670     if (CpuFeature->FeatureName != NULL) {
 671       if (CpuFeatureEntry->FeatureName == NULL) {
 672         CpuFeatureEntry->FeatureName = AllocatePool (CPU_FEATURE_NAME_SIZE);
 673         ASSERT (CpuFeatureEntry->FeatureName != NULL);
 674       }
 675       Status = AsciiStrCpyS (CpuFeatureEntry->FeatureName, CPU_FEATURE_NAME_SIZE, CpuFeature->FeatureName);
 676       ASSERT_EFI_ERROR (Status);
 677       FreePool (CpuFeature->FeatureName);
 678     }
 679     if (CpuFeature->BeforeFeatureBitMask != NULL) {
 680       if (CpuFeatureEntry->BeforeFeatureBitMask != NULL) {
 681         FreePool (CpuFeatureEntry->BeforeFeatureBitMask);
 682       }
 683       CpuFeatureEntry->BeforeFeatureBitMask = CpuFeature->BeforeFeatureBitMask;
 684     }
 685     if (CpuFeature->AfterFeatureBitMask != NULL) {
 686       if (CpuFeatureEntry->AfterFeatureBitMask != NULL) {
 687         FreePool (CpuFeatureEntry->AfterFeatureBitMask);
 688       }
 689       CpuFeatureEntry->AfterFeatureBitMask = CpuFeature->AfterFeatureBitMask;
 690     }
 691     if (CpuFeature->CoreBeforeFeatureBitMask != NULL) {
 692       if (CpuFeatureEntry->CoreBeforeFeatureBitMask != NULL) {
 693         FreePool (CpuFeatureEntry->CoreBeforeFeatureBitMask);
 694       }
 695       CpuFeatureEntry->CoreBeforeFeatureBitMask = CpuFeature->CoreBeforeFeatureBitMask;
 696     }
 697     if (CpuFeature->CoreAfterFeatureBitMask != NULL) {
 698       if (CpuFeatureEntry->CoreAfterFeatureBitMask != NULL) {
 699         FreePool (CpuFeatureEntry->CoreAfterFeatureBitMask);
 700       }
 701       CpuFeatureEntry->CoreAfterFeatureBitMask = CpuFeature->CoreAfterFeatureBitMask;
 702     }
 703     if (CpuFeature->PackageBeforeFeatureBitMask != NULL) {
 704       if (CpuFeatureEntry->PackageBeforeFeatureBitMask != NULL) {
 705         FreePool (CpuFeatureEntry->PackageBeforeFeatureBitMask);
 706       }
 707       CpuFeatureEntry->PackageBeforeFeatureBitMask = CpuFeature->PackageBeforeFeatureBitMask;
 708     }
 709     if (CpuFeature->PackageAfterFeatureBitMask != NULL) {
 710       if (CpuFeatureEntry->PackageAfterFeatureBitMask != NULL) {
 711         FreePool (CpuFeatureEntry->PackageAfterFeatureBitMask);
 712       }
 713       CpuFeatureEntry->PackageAfterFeatureBitMask = CpuFeature->PackageAfterFeatureBitMask;
 714     }
 715
 716     CpuFeatureEntry->BeforeAll = CpuFeature->BeforeAll;
 717     CpuFeatureEntry->AfterAll  = CpuFeature->AfterAll;
 718
 719     FreePool (CpuFeature->FeatureMask);
 720     FreePool (CpuFeature);
 721   }
 722   //
 723   // Verify CPU features dependency can change CPU feature order
 724   //
 725   CheckCpuFeaturesDependency (&CpuFeaturesData->FeatureList);
 726   return RETURN_SUCCESS;
 727 }
 728
 729 /**
 730   Sets CPU feature bit mask in CPU feature bit mask buffer.
 731
 732   @param[in]  FeaturesBitMask       Pointer to CPU feature bit mask buffer
 733   @param[in]  Feature               The bit number of the CPU feature
 734   @param[in]  BitMaskSize           CPU feature bit mask buffer size
 735 **/
 736 VOID
 737 SetCpuFeaturesBitMask (
 738   IN UINT8               **FeaturesBitMask,
 739   IN UINT32              Feature,
 740   IN UINTN               BitMaskSize
 741   )
 742 {
 743   UINT8                  *CpuFeaturesBitMask;
 744
 745   ASSERT (FeaturesBitMask != NULL);
 746   CpuFeaturesBitMask = *FeaturesBitMask;
 747   if (CpuFeaturesBitMask == NULL) {
 748     CpuFeaturesBitMask = AllocateZeroPool (BitMaskSize);
 749     ASSERT (CpuFeaturesBitMask != NULL);
 750     *FeaturesBitMask = CpuFeaturesBitMask;
 751   }
 752
 753   CpuFeaturesBitMask  += (Feature / 8);
 754   *CpuFeaturesBitMask |= (UINT8) (1 << (Feature % 8));
 755 }
 756
 757 /**
 758   Registers a CPU Feature.
 759
 760   @param[in]  FeatureName        A Null-terminated Ascii string indicates CPU feature
 761                                  name.
 762   @param[in]  GetConfigDataFunc  CPU feature get configuration data function.  This
 763                                  is an optional parameter that may be NULL.  If NULL,
 764                                  then the most recently registered function for the
 765                                  CPU feature is used.  If no functions are registered
 766                                  for a CPU feature, then the CPU configuration data
 767                                  for the registered feature is NULL.
 768   @param[in]  SupportFunc        CPU feature support function.  This is an optional
 769                                  parameter that may be NULL.  If NULL, then the most
 770                                  recently registered function for the CPU feature is
 771                                  used. If no functions are registered for a CPU
 772                                  feature, then the CPU feature is assumed to be
 773                                  supported by all CPUs.
 774   @param[in]  InitializeFunc     CPU feature initialize function.  This is an optional
 775                                  parameter that may be NULL.  If NULL, then the most
 776                                  recently registered function for the CPU feature is
 777                                  used. If no functions are registered for a CPU
 778                                  feature, then the CPU feature initialization is
 779                                  skipped.
 780   @param[in]  ...                Variable argument list of UINT32 CPU feature value.
 781                                  Values with no modifiers are the features provided
 782                                  by the registered functions.
 783                                  Values with CPU_FEATURE_BEFORE modifier are features
 784                                  that must be initialized after the features provided
 785                                  by the registered functions are used.
 786                                  Values with CPU_FEATURE_AFTER modifier are features
 787                                  that must be initialized before the features provided
 788                                  by the registered functions are used.
 789                                  The last argument in this variable argument list must
 790                                  always be CPU_FEATURE_END.
 791
 792   @retval  RETURN_SUCCESS           The CPU feature was successfully registered.
 793   @retval  RETURN_OUT_OF_RESOURCES  There are not enough resources to register
 794                                     the CPU feature.
 795   @retval  RETURN_UNSUPPORTED       Registration of the CPU feature is not
 796                                     supported due to a circular dependency between
 797                                     BEFORE and AFTER features.
 798   @retval  RETURN_NOT_READY         CPU feature PCD PcdCpuFeaturesUserConfiguration
 799                                     not updated by Platform driver yet.
 800
 801   @note This service could be called by BSP only.
 802 **/
 803 RETURN_STATUS
 804 EFIAPI
 805 RegisterCpuFeature (
 806   IN CHAR8                             *FeatureName,       OPTIONAL
 807   IN CPU_FEATURE_GET_CONFIG_DATA       GetConfigDataFunc,  OPTIONAL
 808   IN CPU_FEATURE_SUPPORT               SupportFunc,        OPTIONAL
 809   IN CPU_FEATURE_INITIALIZE            InitializeFunc,     OPTIONAL
 810   ...
 811   )
 812 {
 813   EFI_STATUS                 Status;
 814   VA_LIST                    Marker;
 815   UINT32                     Feature;
 816   CPU_FEATURES_ENTRY         *CpuFeature;
 817   UINT8                      *FeatureMask;
 818   UINT8                      *BeforeFeatureBitMask;
 819   UINT8                      *AfterFeatureBitMask;
 820   UINT8                      *CoreBeforeFeatureBitMask;
 821   UINT8                      *CoreAfterFeatureBitMask;
 822   UINT8                      *PackageBeforeFeatureBitMask;
 823   UINT8                      *PackageAfterFeatureBitMask;
 824   BOOLEAN                    BeforeAll;
 825   BOOLEAN                    AfterAll;
 826   CPU_FEATURES_DATA          *CpuFeaturesData;
 827
 828   FeatureMask                 = NULL;
 829   BeforeFeatureBitMask        = NULL;
 830   AfterFeatureBitMask         = NULL;
 831   CoreBeforeFeatureBitMask    = NULL;
 832   CoreAfterFeatureBitMask     = NULL;
 833   PackageBeforeFeatureBitMask = NULL;
 834   PackageAfterFeatureBitMask  = NULL;
 835   BeforeAll            = FALSE;
 836   AfterAll             = FALSE;
 837
 838   CpuFeaturesData = GetCpuFeaturesData ();
 839   if (CpuFeaturesData->FeaturesCount == 0) {
 840     InitializeListHead (&CpuFeaturesData->FeatureList);
 841     InitializeSpinLock (&CpuFeaturesData->CpuFlags.MemoryMappedLock);
 842     //
 843     // Code assumes below three PCDs have PCD same buffer size.
 844     //
 845     ASSERT (PcdGetSize (PcdCpuFeaturesSetting) == PcdGetSize (PcdCpuFeaturesCapability));
 846     ASSERT (PcdGetSize (PcdCpuFeaturesSetting) == PcdGetSize (PcdCpuFeaturesSupport));
 847     CpuFeaturesData->BitMaskSize = (UINT32) PcdGetSize (PcdCpuFeaturesSetting);
 848   }
 849
 850   VA_START (Marker, InitializeFunc);
 851   Feature = VA_ARG (Marker, UINT32);
 852   while (Feature != CPU_FEATURE_END) {
 853     ASSERT ((Feature & (CPU_FEATURE_BEFORE | CPU_FEATURE_AFTER))
 854                     != (CPU_FEATURE_BEFORE | CPU_FEATURE_AFTER));
 855     ASSERT ((Feature & (CPU_FEATURE_BEFORE_ALL | CPU_FEATURE_AFTER_ALL))
 856                     != (CPU_FEATURE_BEFORE_ALL | CPU_FEATURE_AFTER_ALL));
 857     ASSERT ((Feature & (CPU_FEATURE_CORE_BEFORE | CPU_FEATURE_CORE_AFTER))
 858                     != (CPU_FEATURE_CORE_BEFORE | CPU_FEATURE_CORE_AFTER));
 859     ASSERT ((Feature & (CPU_FEATURE_PACKAGE_BEFORE | CPU_FEATURE_PACKAGE_AFTER))
 860                     != (CPU_FEATURE_PACKAGE_BEFORE | CPU_FEATURE_PACKAGE_AFTER));
 861     if (Feature < CPU_FEATURE_THREAD_BEFORE) {
 862       BeforeAll = ((Feature & CPU_FEATURE_BEFORE_ALL) != 0) ? TRUE : FALSE;
 863       AfterAll  = ((Feature & CPU_FEATURE_AFTER_ALL) != 0) ? TRUE : FALSE;
 864       Feature  &= ~(CPU_FEATURE_BEFORE_ALL | CPU_FEATURE_AFTER_ALL);
 865       ASSERT (FeatureMask == NULL);
 866       SetCpuFeaturesBitMask (&FeatureMask, Feature, CpuFeaturesData->BitMaskSize);
 867     } else if ((Feature & CPU_FEATURE_THREAD_BEFORE) != 0) {
 868       SetCpuFeaturesBitMask (&BeforeFeatureBitMask, Feature & ~CPU_FEATURE_THREAD_BEFORE, CpuFeaturesData->BitMaskSize);
 869     } else if ((Feature & CPU_FEATURE_THREAD_AFTER) != 0) {
 870       SetCpuFeaturesBitMask (&AfterFeatureBitMask, Feature & ~CPU_FEATURE_THREAD_AFTER, CpuFeaturesData->BitMaskSize);
 871     } else if ((Feature & CPU_FEATURE_CORE_BEFORE) != 0) {
 872       SetCpuFeaturesBitMask (&CoreBeforeFeatureBitMask, Feature & ~CPU_FEATURE_CORE_BEFORE, CpuFeaturesData->BitMaskSize);
 873     } else if ((Feature & CPU_FEATURE_CORE_AFTER) != 0) {
 874       SetCpuFeaturesBitMask (&CoreAfterFeatureBitMask, Feature & ~CPU_FEATURE_CORE_AFTER, CpuFeaturesData->BitMaskSize);
 875     } else if ((Feature & CPU_FEATURE_PACKAGE_BEFORE) != 0) {
 876       SetCpuFeaturesBitMask (&PackageBeforeFeatureBitMask, Feature & ~CPU_FEATURE_PACKAGE_BEFORE, CpuFeaturesData->BitMaskSize);
 877     } else if ((Feature & CPU_FEATURE_PACKAGE_AFTER) != 0) {
 878       SetCpuFeaturesBitMask (&PackageAfterFeatureBitMask, Feature & ~CPU_FEATURE_PACKAGE_AFTER, CpuFeaturesData->BitMaskSize);
 879     }
 880     Feature = VA_ARG (Marker, UINT32);
 881   }
 882   VA_END (Marker);
 883
 884   CpuFeature = AllocateZeroPool (sizeof (CPU_FEATURES_ENTRY));
 885   ASSERT (CpuFeature != NULL);
 886   CpuFeature->Signature                   = CPU_FEATURE_ENTRY_SIGNATURE;
 887   CpuFeature->FeatureMask                 = FeatureMask;
 888   CpuFeature->BeforeFeatureBitMask        = BeforeFeatureBitMask;
 889   CpuFeature->AfterFeatureBitMask         = AfterFeatureBitMask;
 890   CpuFeature->CoreBeforeFeatureBitMask    = CoreBeforeFeatureBitMask;
 891   CpuFeature->CoreAfterFeatureBitMask     = CoreAfterFeatureBitMask;
 892   CpuFeature->PackageBeforeFeatureBitMask = PackageBeforeFeatureBitMask;
 893   CpuFeature->PackageAfterFeatureBitMask  = PackageAfterFeatureBitMask;
 894   CpuFeature->BeforeAll                   = BeforeAll;
 895   CpuFeature->AfterAll                    = AfterAll;
 896   CpuFeature->GetConfigDataFunc           = GetConfigDataFunc;
 897   CpuFeature->SupportFunc                 = SupportFunc;
 898   CpuFeature->InitializeFunc              = InitializeFunc;
 899   if (FeatureName != NULL) {
 900     CpuFeature->FeatureName          = AllocatePool (CPU_FEATURE_NAME_SIZE);
 901     ASSERT (CpuFeature->FeatureName != NULL);
 902     Status = AsciiStrCpyS (CpuFeature->FeatureName, CPU_FEATURE_NAME_SIZE, FeatureName);
 903     ASSERT_EFI_ERROR (Status);
 904   }
 905
 906   Status = RegisterCpuFeatureWorker (CpuFeaturesData, CpuFeature);
 907   ASSERT_EFI_ERROR (Status);
 908
 909   return RETURN_SUCCESS;
 910 }
 911
 912 /**
 913   Return ACPI_CPU_DATA data.
 914
 915   @return  Pointer to ACPI_CPU_DATA data.
 916 **/
 917 ACPI_CPU_DATA *
 918 GetAcpiCpuData (
 919   VOID
 920   )
 921 {
 922   EFI_STATUS                           Status;
 923   UINTN                                NumberOfCpus;
 924   UINTN                                NumberOfEnabledProcessors;
 925   ACPI_CPU_DATA                        *AcpiCpuData;
 926   UINTN                                TableSize;
 927   CPU_REGISTER_TABLE                   *RegisterTable;
 928   UINTN                                Index;
 929   EFI_PROCESSOR_INFORMATION            ProcessorInfoBuffer;
 930
 931   AcpiCpuData = (ACPI_CPU_DATA *) (UINTN) PcdGet64 (PcdCpuS3DataAddress);
 932   if (AcpiCpuData != NULL) {
 933     return AcpiCpuData;
 934   }
 935
 936   AcpiCpuData  = AllocatePages (EFI_SIZE_TO_PAGES (sizeof (ACPI_CPU_DATA)));
 937   ASSERT (AcpiCpuData != NULL);
 938
 939   //
 940   // Set PcdCpuS3DataAddress to the base address of the ACPI_CPU_DATA structure
 941   //
 942   Status = PcdSet64S (PcdCpuS3DataAddress, (UINT64)(UINTN)AcpiCpuData);
 943   ASSERT_EFI_ERROR (Status);
 944
 945   GetNumberOfProcessor (&NumberOfCpus, &NumberOfEnabledProcessors);
 946   AcpiCpuData->NumberOfCpus = (UINT32)NumberOfCpus;
 947
 948   //
 949   // Allocate buffer for empty RegisterTable and PreSmmInitRegisterTable for all CPUs
 950   //
 951   TableSize = 2 * NumberOfCpus * sizeof (CPU_REGISTER_TABLE);
 952   RegisterTable  = AllocatePages (EFI_SIZE_TO_PAGES (TableSize));
 953   ASSERT (RegisterTable != NULL);
 954
 955   for (Index = 0; Index < NumberOfCpus; Index++) {
 956     Status = GetProcessorInformation (Index, &ProcessorInfoBuffer);
 957     ASSERT_EFI_ERROR (Status);
 958
 959     RegisterTable[Index].InitialApicId      = (UINT32)ProcessorInfoBuffer.ProcessorId;
 960     RegisterTable[Index].TableLength        = 0;
 961     RegisterTable[Index].AllocatedSize      = 0;
 962     RegisterTable[Index].RegisterTableEntry = 0;
 963
 964     RegisterTable[NumberOfCpus + Index].InitialApicId      = (UINT32)ProcessorInfoBuffer.ProcessorId;
 965     RegisterTable[NumberOfCpus + Index].TableLength        = 0;
 966     RegisterTable[NumberOfCpus + Index].AllocatedSize      = 0;
 967     RegisterTable[NumberOfCpus + Index].RegisterTableEntry = 0;
 968   }
 969   AcpiCpuData->RegisterTable           = (EFI_PHYSICAL_ADDRESS)(UINTN)RegisterTable;
 970   AcpiCpuData->PreSmmInitRegisterTable = (EFI_PHYSICAL_ADDRESS)(UINTN)(RegisterTable + NumberOfCpus);
 971
 972   return AcpiCpuData;
 973 }
 974
 975 /**
 976   Enlarges CPU register table for each processor.
 977
 978   @param[in, out]  RegisterTable   Pointer processor's CPU register table
 979 **/
 980 STATIC
 981 VOID
 982 EnlargeRegisterTable (
 983   IN OUT CPU_REGISTER_TABLE            *RegisterTable
 984   )
 985 {
 986   EFI_PHYSICAL_ADDRESS  Address;
 987   UINTN                 UsedPages;
 988
 989   UsedPages = RegisterTable->AllocatedSize / EFI_PAGE_SIZE;
 990   Address  = (UINTN)AllocatePages (UsedPages + 1);
 991   ASSERT (Address != 0);
 992
 993   //
 994   // If there are records existing in the register table, then copy its contents
 995   // to new region and free the old one.
 996   //
 997   if (RegisterTable->AllocatedSize > 0) {
 998     CopyMem (
 999       (VOID *) (UINTN) Address,
1000       (VOID *) (UINTN) RegisterTable->RegisterTableEntry,
1001       RegisterTable->AllocatedSize
1002       );
1003
1004     FreePages ((VOID *)(UINTN)RegisterTable->RegisterTableEntry, UsedPages);
1005   }
1006
1007   //
1008   // Adjust the allocated size and register table base address.
1009   //
1010   RegisterTable->AllocatedSize     += EFI_PAGE_SIZE;
1011   RegisterTable->RegisterTableEntry = Address;
1012 }
1013
1014 /**
1015   Add an entry in specified register table.
1016
1017   This function adds an entry in specified register table, with given register type,
1018   register index, bit section and value.
1019
1020   @param[in]  PreSmmFlag       If TRUE, entry will be added into PreSmm register table
1021                                If FALSE, entry will be added into register table
1022   @param[in]  ProcessorNumber  The index of the CPU to add a register table entry
1023   @param[in]  RegisterType     Type of the register to program
1024   @param[in]  Index            Index of the register to program
1025   @param[in]  ValidBitStart    Start of the bit section
1026   @param[in]  ValidBitLength   Length of the bit section
1027   @param[in]  Value            Value to write
1028   @param[in]  TestThenWrite    Whether need to test current Value before writing.
1029
1030 **/
1031 VOID
1032 CpuRegisterTableWriteWorker (
1033   IN BOOLEAN                 PreSmmFlag,
1034   IN UINTN                   ProcessorNumber,
1035   IN REGISTER_TYPE           RegisterType,
1036   IN UINT64                  Index,
1037   IN UINT8                   ValidBitStart,
1038   IN UINT8                   ValidBitLength,
1039   IN UINT64                  Value,
1040   IN BOOLEAN                 TestThenWrite
1041   )
1042 {
1043   CPU_FEATURES_DATA        *CpuFeaturesData;
1044   ACPI_CPU_DATA            *AcpiCpuData;
1045   CPU_REGISTER_TABLE       *RegisterTable;
1046   CPU_REGISTER_TABLE_ENTRY *RegisterTableEntry;
1047
1048   CpuFeaturesData = GetCpuFeaturesData ();
1049   if (CpuFeaturesData->RegisterTable == NULL) {
1050     AcpiCpuData = GetAcpiCpuData ();
1051     ASSERT ((AcpiCpuData != NULL) && (AcpiCpuData->RegisterTable != 0));
1052     CpuFeaturesData->RegisterTable = (CPU_REGISTER_TABLE *) (UINTN) AcpiCpuData->RegisterTable;
1053     CpuFeaturesData->PreSmmRegisterTable = (CPU_REGISTER_TABLE *) (UINTN) AcpiCpuData->PreSmmInitRegisterTable;
1054   }
1055
1056   if (PreSmmFlag) {
1057     RegisterTable = &CpuFeaturesData->PreSmmRegisterTable[ProcessorNumber];
1058   } else {
1059     RegisterTable = &CpuFeaturesData->RegisterTable[ProcessorNumber];
1060   }
1061
1062   if (RegisterTable->TableLength == RegisterTable->AllocatedSize / sizeof (CPU_REGISTER_TABLE_ENTRY)) {
1063     EnlargeRegisterTable (RegisterTable);
1064   }
1065
1066   //
1067   // Append entry in the register table.
1068   //
1069   RegisterTableEntry = (CPU_REGISTER_TABLE_ENTRY *) (UINTN) RegisterTable->RegisterTableEntry;
1070   RegisterTableEntry[RegisterTable->TableLength].RegisterType   = RegisterType;
1071   RegisterTableEntry[RegisterTable->TableLength].Index          = (UINT32) Index;
1072   RegisterTableEntry[RegisterTable->TableLength].HighIndex      = (UINT32) RShiftU64 (Index, 32);
1073   RegisterTableEntry[RegisterTable->TableLength].ValidBitStart  = ValidBitStart;
1074   RegisterTableEntry[RegisterTable->TableLength].ValidBitLength = ValidBitLength;
1075   RegisterTableEntry[RegisterTable->TableLength].Value          = Value;
1076   RegisterTableEntry[RegisterTable->TableLength].TestThenWrite  = TestThenWrite;
1077
1078   RegisterTable->TableLength++;
1079 }
1080
1081 /**
1082   Adds an entry in specified register table.
1083
1084   This function adds an entry in specified register table, with given register type,
1085   register index, bit section and value.
1086
1087   @param[in]  ProcessorNumber  The index of the CPU to add a register table entry
1088   @param[in]  RegisterType     Type of the register to program
1089   @param[in]  Index            Index of the register to program
1090   @param[in]  ValueMask        Mask of bits in register to write
1091   @param[in]  Value            Value to write
1092
1093   @note This service could be called by BSP only.
1094 **/
1095 VOID
1096 EFIAPI
1097 CpuRegisterTableWrite (
1098   IN UINTN               ProcessorNumber,
1099   IN REGISTER_TYPE       RegisterType,
1100   IN UINT64              Index,
1101   IN UINT64              ValueMask,
1102   IN UINT64              Value
1103   )
1104 {
1105   UINT8                   Start;
1106   UINT8                   End;
1107   UINT8                   Length;
1108
1109   Start  = (UINT8)LowBitSet64  (ValueMask);
1110   End    = (UINT8)HighBitSet64 (ValueMask);
1111   Length = End - Start + 1;
1112   CpuRegisterTableWriteWorker (FALSE, ProcessorNumber, RegisterType, Index, Start, Length, Value, FALSE);
1113 }
1114
1115 /**
1116   Adds an entry in specified register table.
1117
1118   This function adds an entry in specified register table, with given register type,
1119   register index, bit section and value.
1120
1121   @param[in]  ProcessorNumber  The index of the CPU to add a register table entry
1122   @param[in]  RegisterType     Type of the register to program
1123   @param[in]  Index            Index of the register to program
1124   @param[in]  ValueMask        Mask of bits in register to write
1125   @param[in]  Value            Value to write
1126
1127   @note This service could be called by BSP only.
1128 **/
1129 VOID
1130 EFIAPI
1131 CpuRegisterTableTestThenWrite (
1132   IN UINTN               ProcessorNumber,
1133   IN REGISTER_TYPE       RegisterType,
1134   IN UINT64              Index,
1135   IN UINT64              ValueMask,
1136   IN UINT64              Value
1137   )
1138 {
1139   UINT8                   Start;
1140   UINT8                   End;
1141   UINT8                   Length;
1142
1143   Start  = (UINT8)LowBitSet64  (ValueMask);
1144   End    = (UINT8)HighBitSet64 (ValueMask);
1145   Length = End - Start + 1;
1146   CpuRegisterTableWriteWorker (FALSE, ProcessorNumber, RegisterType, Index, Start, Length, Value, TRUE);
1147 }
1148
1149 /**
1150   Adds an entry in specified Pre-SMM register table.
1151
1152   This function adds an entry in specified register table, with given register type,
1153   register index, bit section and value.
1154
1155   @param[in]  ProcessorNumber  The index of the CPU to add a register table entry.
1156   @param[in]  RegisterType     Type of the register to program
1157   @param[in]  Index            Index of the register to program
1158   @param[in]  ValueMask        Mask of bits in register to write
1159   @param[in]  Value            Value to write
1160
1161   @note This service could be called by BSP only.
1162 **/
1163 VOID
1164 EFIAPI
1165 PreSmmCpuRegisterTableWrite (
1166   IN UINTN               ProcessorNumber,
1167   IN REGISTER_TYPE       RegisterType,
1168   IN UINT64              Index,
1169   IN UINT64              ValueMask,
1170   IN UINT64              Value
1171   )
1172 {
1173   UINT8  Start;
1174   UINT8  End;
1175   UINT8  Length;
1176
1177   Start  = (UINT8)LowBitSet64  (ValueMask);
1178   End    = (UINT8)HighBitSet64 (ValueMask);
1179   Length = End - Start + 1;
1180   CpuRegisterTableWriteWorker (TRUE, ProcessorNumber, RegisterType, Index, Start, Length, Value, FALSE);
1181 }
1182
1183 /**
1184   Worker function to determine if a CPU feature is set in input CPU feature bit mask buffer.
1185
1186   @param[in]  CpuBitMask      CPU feature bit mask buffer
1187   @param[in]  CpuBitMaskSize  The size of CPU feature bit mask buffer
1188   @param[in]  Feature         The bit number of the CPU feature
1189
1190   @retval  TRUE   The CPU feature is set in CpuBitMask.
1191   @retval  FALSE  The CPU feature is not set in CpuBitMask.
1192
1193 **/
1194 BOOLEAN
1195 IsCpuFeatureSetInCpuPcd (
1196   IN UINT8               *CpuBitMask,
1197   IN UINTN               CpuBitMaskSize,
1198   IN UINT32              Feature
1199   )
1200 {
1201   if ((Feature >> 3) >= CpuBitMaskSize) {
1202     return FALSE;
1203   }
1204   return ((*(CpuBitMask + (Feature >> 3)) & (1 << (Feature & 0x07))) != 0);
1205 }
1206
1207 /**
1208   Determines if a CPU feature is enabled in PcdCpuFeaturesSupport bit mask.
1209   If a CPU feature is disabled in PcdCpuFeaturesSupport then all the code/data
1210   associated with that feature should be optimized away if compiler
1211   optimizations are enabled.
1212
1213   @param[in]  Feature  The bit number of the CPU feature to check in the PCD
1214                        PcdCpuFeaturesSupport
1215
1216   @retval  TRUE   The CPU feature is set in PcdCpuFeaturesSupport.
1217   @retval  FALSE  The CPU feature is not set in PcdCpuFeaturesSupport.
1218
1219   @note This service could be called by BSP only.
1220 **/
1221 BOOLEAN
1222 EFIAPI
1223 IsCpuFeatureSupported (
1224   IN UINT32              Feature
1225   )
1226 {
1227   return IsCpuFeatureSetInCpuPcd (
1228            (UINT8 *)PcdGetPtr (PcdCpuFeaturesSupport),
1229            PcdGetSize (PcdCpuFeaturesSupport),
1230            Feature
1231            );
1232 }
1233
1234 /**
1235   Determines if a CPU feature is set in PcdCpuFeaturesSetting bit mask.
1236
1237   @param[in]  Feature  The bit number of the CPU feature to check in the PCD
1238                        PcdCpuFeaturesSetting
1239
1240   @retval  TRUE   The CPU feature is set in PcdCpuFeaturesSetting.
1241   @retval  FALSE  The CPU feature is not set in PcdCpuFeaturesSetting.
1242
1243   @note This service could be called by BSP only.
1244 **/
1245 BOOLEAN
1246 EFIAPI
1247 IsCpuFeatureInSetting (
1248   IN UINT32              Feature
1249   )
1250 {
1251   return IsCpuFeatureSetInCpuPcd (
1252            (UINT8 *)PcdGetPtr (PcdCpuFeaturesSetting),
1253            PcdGetSize (PcdCpuFeaturesSetting),
1254            Feature
1255            );
1256 }
1257
1258 /**
1259   Switches to assigned BSP after CPU features initialization.
1260
1261   @param[in]  ProcessorNumber  The index of the CPU executing this function.
1262
1263   @note This service could be called by BSP only.
1264 **/
1265 VOID
1266 EFIAPI
1267 SwitchBspAfterFeaturesInitialize (
1268   IN UINTN               ProcessorNumber
1269   )
1270 {
1271   CPU_FEATURES_DATA      *CpuFeaturesData;
1272
1273   CpuFeaturesData = GetCpuFeaturesData ();
1274   CpuFeaturesData->BspNumber = ProcessorNumber;
1275 }
1276