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