]> git.proxmox.com Git - mirror_edk2.git/blobdiff - UefiCpuPkg/Library/RegisterCpuFeaturesLib/RegisterCpuFeaturesLib.c
UefiCpuPkg/RegisterCpuFeaturesLib: Add logic to support semaphore type.
[mirror_edk2.git] / UefiCpuPkg / Library / RegisterCpuFeaturesLib / RegisterCpuFeaturesLib.c
index fa7e107e39c0ac95dd1448cc6efb3fba9beb87ae..5b49bc450494cca3815894525b74192664bb8ff1 100644 (file)
@@ -112,6 +112,302 @@ IsBitMaskMatchCheck (
   return FALSE;\r
 }\r
 \r
+/**\r
+  Return feature dependence result.\r
+\r
+  @param[in]  CpuFeature        Pointer to CPU feature.\r
+  @param[in]  Before            Check before dependence or after.\r
+\r
+  @retval     return the dependence result.\r
+**/\r
+CPU_FEATURE_DEPENDENCE_TYPE\r
+DetectFeatureScope (\r
+  IN CPU_FEATURES_ENTRY         *CpuFeature,\r
+  IN BOOLEAN                    Before\r
+  )\r
+{\r
+  if (Before) {\r
+    if (CpuFeature->PackageBeforeFeatureBitMask != NULL) {\r
+      return PackageDepType;\r
+    }\r
+\r
+    if (CpuFeature->CoreBeforeFeatureBitMask != NULL) {\r
+      return CoreDepType;\r
+    }\r
+\r
+    if (CpuFeature->BeforeFeatureBitMask != NULL) {\r
+      return ThreadDepType;\r
+    }\r
+\r
+    return NoneDepType;\r
+  }\r
+\r
+  if (CpuFeature->PackageAfterFeatureBitMask != NULL) {\r
+    return PackageDepType;\r
+  }\r
+\r
+  if (CpuFeature->CoreAfterFeatureBitMask != NULL) {\r
+    return CoreDepType;\r
+  }\r
+\r
+  if (CpuFeature->AfterFeatureBitMask != NULL) {\r
+    return ThreadDepType;\r
+  }\r
+\r
+  return NoneDepType;\r
+}\r
+\r
+/**\r
+  Clear dependence for the specified type.\r
+\r
+  @param[in]  CurrentFeature     Cpu feature need to clear.\r
+  @param[in]  Before             Before or after dependence relationship.\r
+\r
+**/\r
+VOID\r
+ClearFeatureScope (\r
+  IN CPU_FEATURES_ENTRY           *CpuFeature,\r
+  IN BOOLEAN                      Before\r
+  )\r
+{\r
+  if (Before) {\r
+    if (CpuFeature->BeforeFeatureBitMask != NULL) {\r
+      FreePool (CpuFeature->BeforeFeatureBitMask);\r
+      CpuFeature->BeforeFeatureBitMask = NULL;\r
+    }\r
+    if (CpuFeature->CoreBeforeFeatureBitMask != NULL) {\r
+      FreePool (CpuFeature->CoreBeforeFeatureBitMask);\r
+      CpuFeature->CoreBeforeFeatureBitMask = NULL;\r
+    }\r
+    if (CpuFeature->PackageBeforeFeatureBitMask != NULL) {\r
+      FreePool (CpuFeature->PackageBeforeFeatureBitMask);\r
+      CpuFeature->PackageBeforeFeatureBitMask = NULL;\r
+    }\r
+  } else {\r
+    if (CpuFeature->PackageAfterFeatureBitMask != NULL) {\r
+      FreePool (CpuFeature->PackageAfterFeatureBitMask);\r
+      CpuFeature->PackageAfterFeatureBitMask = NULL;\r
+    }\r
+    if (CpuFeature->CoreAfterFeatureBitMask != NULL) {\r
+      FreePool (CpuFeature->CoreAfterFeatureBitMask);\r
+      CpuFeature->CoreAfterFeatureBitMask = NULL;\r
+    }\r
+    if (CpuFeature->AfterFeatureBitMask != NULL) {\r
+      FreePool (CpuFeature->AfterFeatureBitMask);\r
+      CpuFeature->AfterFeatureBitMask = NULL;\r
+    }\r
+  }\r
+}\r
+\r
+/**\r
+  Base on dependence relationship to asjust feature dependence.\r
+\r
+  ONLY when the feature before(or after) the find feature also has \r
+  dependence with the find feature. In this case, driver need to base\r
+  on dependce relationship to decide how to insert current feature and\r
+  adjust the feature dependence.\r
+\r
+  @param[in]  PreviousFeature    CPU feature current before the find one.\r
+  @param[in]  CurrentFeature     Cpu feature need to adjust.\r
+  @param[in]  Before             Before or after dependence relationship.\r
+\r
+  @retval   TRUE   means the current feature dependence has been adjusted.\r
+\r
+  @retval   FALSE  means the previous feature dependence has been adjusted.\r
+                   or previous feature has no dependence with the find one.\r
+\r
+**/\r
+BOOLEAN\r
+AdjustFeaturesDependence (\r
+  IN OUT CPU_FEATURES_ENTRY         *PreviousFeature,\r
+  IN OUT CPU_FEATURES_ENTRY         *CurrentFeature,\r
+  IN     BOOLEAN                    Before\r
+  )\r
+{\r
+  CPU_FEATURE_DEPENDENCE_TYPE            PreDependType;\r
+  CPU_FEATURE_DEPENDENCE_TYPE            CurrentDependType;\r
+\r
+  PreDependType     = DetectFeatureScope(PreviousFeature, Before);\r
+  CurrentDependType = DetectFeatureScope(CurrentFeature, Before);\r
+\r
+  //\r
+  // If previous feature has no dependence with the find featue.\r
+  // return FALSE.\r
+  //\r
+  if (PreDependType == NoneDepType) {\r
+    return FALSE;\r
+  }\r
+\r
+  //\r
+  // If both feature have dependence, keep the one which needs use more \r
+  // processors and clear the dependence for the other one.\r
+  //\r
+  if (PreDependType >= CurrentDependType) {\r
+    ClearFeatureScope (CurrentFeature, Before);\r
+    return TRUE;\r
+  } else {\r
+    ClearFeatureScope (PreviousFeature, Before);\r
+    return FALSE;\r
+  }\r
+}\r
+\r
+/**\r
+  Base on dependence relationship to asjust feature order.\r
+\r
+  @param[in]  FeatureList        Pointer to CPU feature list\r
+  @param[in]  FindEntry          The entry this feature depend on.\r
+  @param[in]  CurrentEntry       The entry for this feature.\r
+  @param[in]  Before             Before or after dependence relationship.\r
+\r
+**/\r
+VOID\r
+AdjustEntry (\r
+  IN      LIST_ENTRY                *FeatureList,\r
+  IN OUT  LIST_ENTRY                *FindEntry,\r
+  IN OUT  LIST_ENTRY                *CurrentEntry,\r
+  IN      BOOLEAN                   Before\r
+  )\r
+{\r
+  LIST_ENTRY                *PreviousEntry;\r
+  CPU_FEATURES_ENTRY        *PreviousFeature;\r
+  CPU_FEATURES_ENTRY        *CurrentFeature;\r
+\r
+  //\r
+  // For CPU feature which has core or package type dependence, later code need to insert\r
+  // AcquireSpinLock/ReleaseSpinLock logic to sequency the execute order.\r
+  // So if driver finds both feature A and B need to execute before feature C, driver will\r
+  // base on dependence type of feature A and B to update the logic here.\r
+  // For example, feature A has package type dependence and feature B has core type dependence,\r
+  // because package type dependence need to wait for more processors which has strong dependence\r
+  // than core type dependence. So driver will adjust the feature order to B -> A -> C. and driver \r
+  // will remove the feature dependence in feature B. \r
+  // Driver just needs to make sure before feature C been executed, feature A has finished its task\r
+  // in all all thread. Feature A finished in all threads also means feature B have finshed in all\r
+  // threads.\r
+  //\r
+  if (Before) {\r
+    PreviousEntry = GetPreviousNode (FeatureList, FindEntry);\r
+  } else {\r\r
+    PreviousEntry = GetNextNode (FeatureList, FindEntry);\r
+  }\r
+\r
+  CurrentFeature  = CPU_FEATURE_ENTRY_FROM_LINK (CurrentEntry);\r
+  RemoveEntryList (CurrentEntry);\r
+\r
+  if (IsNull (FeatureList, PreviousEntry)) {\r
+    //\r
+    // If not exist the previous or next entry, just insert the current entry.\r
+    //\r
+    if (Before) {\r
+      InsertTailList (FindEntry, CurrentEntry);\r
+    } else {\r
+      InsertHeadList (FindEntry, CurrentEntry);\r
+    }\r
+  } else {\r
+    //\r
+    // If exist the previous or next entry, need to check it before insert curent entry.\r
+    //\r
+    PreviousFeature = CPU_FEATURE_ENTRY_FROM_LINK (PreviousEntry);\r
+\r
+    if (AdjustFeaturesDependence (PreviousFeature, CurrentFeature, Before)) {\r
+      //\r
+      // Return TRUE means current feature dependence has been cleared and the previous\r
+      // feature dependence has been kept and used. So insert current feature before (or after)\r
+      // the previous feature.\r
+      //\r
+      if (Before) {\r
+        InsertTailList (PreviousEntry, CurrentEntry);\r
+      } else {\r
+        InsertHeadList (PreviousEntry, CurrentEntry);\r
+      }\r
+    } else {\r
+      if (Before) {\r
+        InsertTailList (FindEntry, CurrentEntry);\r
+      } else {\r
+        InsertHeadList (FindEntry, CurrentEntry);\r
+      }\r
+    }\r
+  }\r
+}\r\r
+\r
+/**\r
+  Checks and adjusts current CPU features per dependency relationship.\r
+\r
+  @param[in]  FeatureList        Pointer to CPU feature list\r
+  @param[in]  CurrentEntry       Pointer to current checked CPU feature\r
+  @param[in]  FeatureMask        The feature bit mask.\r
+\r
+  @retval     return Swapped info.\r
+**/\r
+BOOLEAN\r
+InsertToBeforeEntry (\r
+  IN LIST_ENTRY              *FeatureList,\r
+  IN LIST_ENTRY              *CurrentEntry,\r
+  IN UINT8                   *FeatureMask\r
+  )\r
+{\r
+  LIST_ENTRY                 *CheckEntry;\r
+  CPU_FEATURES_ENTRY         *CheckFeature;\r
+  BOOLEAN                    Swapped;\r
+\r
+  Swapped = FALSE;\r
+\r
+  //\r
+  // Check all features dispatched before this entry\r
+  //\r
+  CheckEntry = GetFirstNode (FeatureList);\r
+  while (CheckEntry != CurrentEntry) {\r
+    CheckFeature = CPU_FEATURE_ENTRY_FROM_LINK (CheckEntry);\r
+    if (IsBitMaskMatchCheck (CheckFeature->FeatureMask, FeatureMask)) {\r
+      AdjustEntry (FeatureList, CheckEntry, CurrentEntry, TRUE);\r
+      Swapped = TRUE;\r
+      break;\r
+    }\r
+    CheckEntry = CheckEntry->ForwardLink;\r
+  }\r
+\r
+  return Swapped;\r
+}\r
+\r
+/**\r
+  Checks and adjusts current CPU features per dependency relationship.\r
+\r
+  @param[in]  FeatureList        Pointer to CPU feature list\r
+  @param[in]  CurrentEntry       Pointer to current checked CPU feature\r
+  @param[in]  FeatureMask        The feature bit mask.\r
+\r
+  @retval     return Swapped info.\r
+**/\r
+BOOLEAN\r
+InsertToAfterEntry (\r
+  IN LIST_ENTRY              *FeatureList,\r
+  IN LIST_ENTRY              *CurrentEntry,\r
+  IN UINT8                   *FeatureMask\r
+  )\r
+{\r
+  LIST_ENTRY                 *CheckEntry;\r
+  CPU_FEATURES_ENTRY         *CheckFeature;\r
+  BOOLEAN                    Swapped;\r
+\r
+  Swapped = FALSE;\r
+\r
+  //\r
+  // Check all features dispatched after this entry\r
+  //\r
+  CheckEntry = GetNextNode (FeatureList, CurrentEntry);\r
+  while (!IsNull (FeatureList, CheckEntry)) {\r
+    CheckFeature = CPU_FEATURE_ENTRY_FROM_LINK (CheckEntry);\r
+    if (IsBitMaskMatchCheck (CheckFeature->FeatureMask, FeatureMask)) {\r
+      AdjustEntry (FeatureList, CheckEntry, CurrentEntry, FALSE);\r
+      Swapped = TRUE;\r
+      break;\r
+    }\r
+    CheckEntry = CheckEntry->ForwardLink;\r
+  }\r
+\r
+  return Swapped;\r
+}\r
+\r
 /**\r
   Checks and adjusts CPU features order per dependency relationship.\r
 \r
@@ -128,11 +424,13 @@ CheckCpuFeaturesDependency (
   CPU_FEATURES_ENTRY         *CheckFeature;\r
   BOOLEAN                    Swapped;\r
   LIST_ENTRY                 *TempEntry;\r
+  LIST_ENTRY                 *NextEntry;\r
 \r
   CurrentEntry = GetFirstNode (FeatureList);\r
   while (!IsNull (FeatureList, CurrentEntry)) {\r
     Swapped = FALSE;\r
     CpuFeature = CPU_FEATURE_ENTRY_FROM_LINK (CurrentEntry);\r
+    NextEntry = CurrentEntry->ForwardLink;\r
     if (CpuFeature->BeforeAll) {\r
       //\r
       // Check all features dispatched before this entry\r
@@ -153,6 +451,7 @@ CheckCpuFeaturesDependency (
         CheckEntry = CheckEntry->ForwardLink;\r
       }\r
       if (Swapped) {\r
+        CurrentEntry = NextEntry;\r
         continue;\r
       }\r
     }\r
@@ -179,60 +478,59 @@ CheckCpuFeaturesDependency (
         CheckEntry = CheckEntry->ForwardLink;\r
       }\r
       if (Swapped) {\r
+        CurrentEntry = NextEntry;\r
         continue;\r
       }\r
     }\r
 \r
     if (CpuFeature->BeforeFeatureBitMask != NULL) {\r
-      //\r
-      // Check all features dispatched before this entry\r
-      //\r
-      CheckEntry = GetFirstNode (FeatureList);\r
-      while (CheckEntry != CurrentEntry) {\r
-        CheckFeature = CPU_FEATURE_ENTRY_FROM_LINK (CheckEntry);\r
-        if (IsBitMaskMatchCheck (CheckFeature->FeatureMask, CpuFeature->BeforeFeatureBitMask)) {\r
-          //\r
-          // If there is dependency, swap them\r
-          //\r
-          RemoveEntryList (CurrentEntry);\r
-          InsertTailList (CheckEntry, CurrentEntry);\r
-          Swapped = TRUE;\r
-          break;\r
-        }\r
-        CheckEntry = CheckEntry->ForwardLink;\r
-      }\r
+      Swapped = InsertToBeforeEntry (FeatureList, CurrentEntry, CpuFeature->BeforeFeatureBitMask);\r
       if (Swapped) {\r
+        CurrentEntry = NextEntry;\r
         continue;\r
       }\r
     }\r
 \r
     if (CpuFeature->AfterFeatureBitMask != NULL) {\r
-      //\r
-      // Check all features dispatched after this entry\r
-      //\r
-      CheckEntry = GetNextNode (FeatureList, CurrentEntry);\r
-      while (!IsNull (FeatureList, CheckEntry)) {\r
-        CheckFeature = CPU_FEATURE_ENTRY_FROM_LINK (CheckEntry);\r
-        if (IsBitMaskMatchCheck (CheckFeature->FeatureMask, CpuFeature->AfterFeatureBitMask)) {\r
-          //\r
-          // If there is dependency, swap them\r
-          //\r
-          TempEntry = GetNextNode (FeatureList, CurrentEntry);\r
-          RemoveEntryList (CurrentEntry);\r
-          InsertHeadList (CheckEntry, CurrentEntry);\r
-          CurrentEntry = TempEntry;\r
-          Swapped = TRUE;\r
-          break;\r
-        }\r
-        CheckEntry = CheckEntry->ForwardLink;\r
+      Swapped = InsertToAfterEntry (FeatureList, CurrentEntry, CpuFeature->AfterFeatureBitMask);\r
+      if (Swapped) {\r
+        CurrentEntry = NextEntry;\r
+        continue;\r
+      }\r
+    }\r
+\r
+    if (CpuFeature->CoreBeforeFeatureBitMask != NULL) {\r
+      Swapped = InsertToBeforeEntry (FeatureList, CurrentEntry, CpuFeature->CoreBeforeFeatureBitMask);\r
+      if (Swapped) {\r
+        CurrentEntry = NextEntry;\r
+        continue;\r
       }\r
+    }\r
+\r
+    if (CpuFeature->CoreAfterFeatureBitMask != NULL) {\r
+      Swapped = InsertToAfterEntry (FeatureList, CurrentEntry, CpuFeature->CoreAfterFeatureBitMask);\r
       if (Swapped) {\r
+        CurrentEntry = NextEntry;\r
         continue;\r
       }\r
     }\r
-    //\r
-    // No swap happened, check the next feature\r
-    //\r
+\r
+    if (CpuFeature->PackageBeforeFeatureBitMask != NULL) {\r
+      Swapped = InsertToBeforeEntry (FeatureList, CurrentEntry, CpuFeature->PackageBeforeFeatureBitMask);\r
+      if (Swapped) {\r
+        CurrentEntry = NextEntry;\r
+        continue;\r
+      }\r
+    }\r
+\r
+    if (CpuFeature->PackageAfterFeatureBitMask != NULL) {\r
+      Swapped = InsertToAfterEntry (FeatureList, CurrentEntry, CpuFeature->PackageAfterFeatureBitMask);\r
+      if (Swapped) {\r
+        CurrentEntry = NextEntry;\r
+        continue;\r
+      }\r
+    }\r
+\r
     CurrentEntry = CurrentEntry->ForwardLink;\r
   }\r
 }\r
@@ -265,8 +563,8 @@ RegisterCpuFeatureWorker (
   CpuFeaturesData = GetCpuFeaturesData ();\r
   if (CpuFeaturesData->FeaturesCount == 0) {\r
     InitializeListHead (&CpuFeaturesData->FeatureList);\r
-    InitializeSpinLock (&CpuFeaturesData->MsrLock);\r
-    InitializeSpinLock (&CpuFeaturesData->MemoryMappedLock);\r
+    InitializeSpinLock (&CpuFeaturesData->CpuFlags.MemoryMappedLock);\r
+    InitializeSpinLock (&CpuFeaturesData->CpuFlags.ConsoleLogLock);\r
     CpuFeaturesData->BitMaskSize = (UINT32) BitMaskSize;\r
   }\r
   ASSERT (CpuFeaturesData->BitMaskSize == BitMaskSize);\r
@@ -328,6 +626,31 @@ RegisterCpuFeatureWorker (
       }\r
       CpuFeatureEntry->AfterFeatureBitMask = CpuFeature->AfterFeatureBitMask;\r
     }\r
+    if (CpuFeature->CoreBeforeFeatureBitMask != NULL) {\r
+      if (CpuFeatureEntry->CoreBeforeFeatureBitMask != NULL) {\r
+        FreePool (CpuFeatureEntry->CoreBeforeFeatureBitMask);\r
+      }\r
+      CpuFeatureEntry->CoreBeforeFeatureBitMask = CpuFeature->CoreBeforeFeatureBitMask;\r
+    }\r
+    if (CpuFeature->CoreAfterFeatureBitMask != NULL) {\r
+      if (CpuFeatureEntry->CoreAfterFeatureBitMask != NULL) {\r
+        FreePool (CpuFeatureEntry->CoreAfterFeatureBitMask);\r
+      }\r
+      CpuFeatureEntry->CoreAfterFeatureBitMask = CpuFeature->CoreAfterFeatureBitMask;\r
+    }\r
+    if (CpuFeature->PackageBeforeFeatureBitMask != NULL) {\r
+      if (CpuFeatureEntry->PackageBeforeFeatureBitMask != NULL) {\r
+        FreePool (CpuFeatureEntry->PackageBeforeFeatureBitMask);\r
+      }\r
+      CpuFeatureEntry->PackageBeforeFeatureBitMask = CpuFeature->PackageBeforeFeatureBitMask;\r
+    }\r
+    if (CpuFeature->PackageAfterFeatureBitMask != NULL) {\r
+      if (CpuFeatureEntry->PackageAfterFeatureBitMask != NULL) {\r
+        FreePool (CpuFeatureEntry->PackageAfterFeatureBitMask);\r
+      }\r
+      CpuFeatureEntry->PackageAfterFeatureBitMask = CpuFeature->PackageAfterFeatureBitMask;\r
+    }\r
+\r
     CpuFeatureEntry->BeforeAll = CpuFeature->BeforeAll;\r
     CpuFeatureEntry->AfterAll  = CpuFeature->AfterAll;\r
 \r
@@ -410,6 +733,8 @@ SetCpuFeaturesBitMask (
   @retval  RETURN_UNSUPPORTED       Registration of the CPU feature is not\r
                                     supported due to a circular dependency between\r
                                     BEFORE and AFTER features.\r
+  @retval  RETURN_NOT_READY         CPU feature PCD PcdCpuFeaturesUserConfiguration\r
+                                    not updated by Platform driver yet.\r
 \r
   @note This service could be called by BSP only.\r
 **/\r
@@ -431,12 +756,20 @@ RegisterCpuFeature (
   UINT8                      *FeatureMask;\r
   UINT8                      *BeforeFeatureBitMask;\r
   UINT8                      *AfterFeatureBitMask;\r
+  UINT8                      *CoreBeforeFeatureBitMask;\r
+  UINT8                      *CoreAfterFeatureBitMask;\r
+  UINT8                      *PackageBeforeFeatureBitMask;\r
+  UINT8                      *PackageAfterFeatureBitMask;\r
   BOOLEAN                    BeforeAll;\r
   BOOLEAN                    AfterAll;\r
 \r
-  FeatureMask          = NULL;\r
-  BeforeFeatureBitMask = NULL;\r
-  AfterFeatureBitMask  = NULL;\r
+  FeatureMask                 = NULL;\r
+  BeforeFeatureBitMask        = NULL;\r
+  AfterFeatureBitMask         = NULL;\r
+  CoreBeforeFeatureBitMask    = NULL;\r
+  CoreAfterFeatureBitMask     = NULL;\r
+  PackageBeforeFeatureBitMask = NULL;\r
+  PackageAfterFeatureBitMask  = NULL;\r
   BeforeAll            = FALSE;\r
   AfterAll             = FALSE;\r
 \r
@@ -449,6 +782,10 @@ RegisterCpuFeature (
                     != (CPU_FEATURE_BEFORE | CPU_FEATURE_AFTER));\r
     ASSERT ((Feature & (CPU_FEATURE_BEFORE_ALL | CPU_FEATURE_AFTER_ALL))\r
                     != (CPU_FEATURE_BEFORE_ALL | CPU_FEATURE_AFTER_ALL));\r
+    ASSERT ((Feature & (CPU_FEATURE_CORE_BEFORE | CPU_FEATURE_CORE_AFTER))\r
+                    != (CPU_FEATURE_CORE_BEFORE | CPU_FEATURE_CORE_AFTER));\r
+    ASSERT ((Feature & (CPU_FEATURE_PACKAGE_BEFORE | CPU_FEATURE_PACKAGE_AFTER))\r
+                    != (CPU_FEATURE_PACKAGE_BEFORE | CPU_FEATURE_PACKAGE_AFTER));\r
     if (Feature < CPU_FEATURE_BEFORE) {\r
       BeforeAll = ((Feature & CPU_FEATURE_BEFORE_ALL) != 0) ? TRUE : FALSE;\r
       AfterAll  = ((Feature & CPU_FEATURE_AFTER_ALL) != 0) ? TRUE : FALSE;\r
@@ -459,6 +796,14 @@ RegisterCpuFeature (
       SetCpuFeaturesBitMask (&BeforeFeatureBitMask, Feature & ~CPU_FEATURE_BEFORE, BitMaskSize);\r
     } else if ((Feature & CPU_FEATURE_AFTER) != 0) {\r
       SetCpuFeaturesBitMask (&AfterFeatureBitMask, Feature & ~CPU_FEATURE_AFTER, BitMaskSize);\r
+    } else if ((Feature & CPU_FEATURE_CORE_BEFORE) != 0) {\r
+      SetCpuFeaturesBitMask (&CoreBeforeFeatureBitMask, Feature & ~CPU_FEATURE_CORE_BEFORE, BitMaskSize);\r
+    } else if ((Feature & CPU_FEATURE_CORE_AFTER) != 0) {\r
+      SetCpuFeaturesBitMask (&CoreAfterFeatureBitMask, Feature & ~CPU_FEATURE_CORE_AFTER, BitMaskSize);\r
+    } else if ((Feature & CPU_FEATURE_PACKAGE_BEFORE) != 0) {\r
+      SetCpuFeaturesBitMask (&PackageBeforeFeatureBitMask, Feature & ~CPU_FEATURE_PACKAGE_BEFORE, BitMaskSize);\r
+    } else if ((Feature & CPU_FEATURE_PACKAGE_AFTER) != 0) {\r
+      SetCpuFeaturesBitMask (&PackageAfterFeatureBitMask, Feature & ~CPU_FEATURE_PACKAGE_AFTER, BitMaskSize);\r
     }\r
     Feature = VA_ARG (Marker, UINT32);\r
   }\r
@@ -466,15 +811,19 @@ RegisterCpuFeature (
 \r
   CpuFeature = AllocateZeroPool (sizeof (CPU_FEATURES_ENTRY));\r
   ASSERT (CpuFeature != NULL);\r
-  CpuFeature->Signature            = CPU_FEATURE_ENTRY_SIGNATURE;\r
-  CpuFeature->FeatureMask          = FeatureMask;\r
-  CpuFeature->BeforeFeatureBitMask = BeforeFeatureBitMask;\r
-  CpuFeature->AfterFeatureBitMask  = AfterFeatureBitMask;\r
-  CpuFeature->BeforeAll            = BeforeAll;\r
-  CpuFeature->AfterAll             = AfterAll;\r
-  CpuFeature->GetConfigDataFunc    = GetConfigDataFunc;\r
-  CpuFeature->SupportFunc          = SupportFunc;\r
-  CpuFeature->InitializeFunc       = InitializeFunc;\r
+  CpuFeature->Signature                   = CPU_FEATURE_ENTRY_SIGNATURE;\r
+  CpuFeature->FeatureMask                 = FeatureMask;\r
+  CpuFeature->BeforeFeatureBitMask        = BeforeFeatureBitMask;\r
+  CpuFeature->AfterFeatureBitMask         = AfterFeatureBitMask;\r
+  CpuFeature->CoreBeforeFeatureBitMask    = CoreBeforeFeatureBitMask;\r
+  CpuFeature->CoreAfterFeatureBitMask     = CoreAfterFeatureBitMask;\r
+  CpuFeature->PackageBeforeFeatureBitMask = PackageBeforeFeatureBitMask;\r
+  CpuFeature->PackageAfterFeatureBitMask  = PackageAfterFeatureBitMask;\r
+  CpuFeature->BeforeAll                   = BeforeAll;\r
+  CpuFeature->AfterAll                    = AfterAll;\r
+  CpuFeature->GetConfigDataFunc           = GetConfigDataFunc;\r
+  CpuFeature->SupportFunc                 = SupportFunc;\r
+  CpuFeature->InitializeFunc              = InitializeFunc;\r
   if (FeatureName != NULL) {\r
     CpuFeature->FeatureName          = AllocatePool (CPU_FEATURE_NAME_SIZE);\r
     ASSERT (CpuFeature->FeatureName != NULL);\r
@@ -489,13 +838,12 @@ RegisterCpuFeature (
 }\r
 \r
 /**\r
-  Allocates boot service data to save ACPI_CPU_DATA.\r
+  Return ACPI_CPU_DATA data.\r
 \r
-  @return  Pointer to allocated ACPI_CPU_DATA.\r
+  @return  Pointer to ACPI_CPU_DATA data.\r
 **/\r
-STATIC\r
 ACPI_CPU_DATA *\r
-AllocateAcpiCpuData (\r
+GetAcpiCpuData (\r
   VOID\r
   )\r
 {\r
@@ -508,9 +856,20 @@ AllocateAcpiCpuData (
   UINTN                                Index;\r
   EFI_PROCESSOR_INFORMATION            ProcessorInfoBuffer;\r
 \r
+  AcpiCpuData = (ACPI_CPU_DATA *) (UINTN) PcdGet64 (PcdCpuS3DataAddress);\r
+  if (AcpiCpuData != NULL) {\r
+    return AcpiCpuData;\r
+  }\r
+\r
   AcpiCpuData  = AllocatePages (EFI_SIZE_TO_PAGES (sizeof (ACPI_CPU_DATA)));\r
   ASSERT (AcpiCpuData != NULL);\r
 \r
+  //\r
+  // Set PcdCpuS3DataAddress to the base address of the ACPI_CPU_DATA structure\r
+  //\r
+  Status = PcdSet64S (PcdCpuS3DataAddress, (UINT64)(UINTN)AcpiCpuData);\r
+  ASSERT_EFI_ERROR (Status);\r
+\r
   GetNumberOfProcessor (&NumberOfCpus, &NumberOfEnabledProcessors);\r
   AcpiCpuData->NumberOfCpus = (UINT32)NumberOfCpus;\r
 \r
@@ -606,7 +965,6 @@ CpuRegisterTableWriteWorker (
   IN UINT64                  Value\r
   )\r
 {\r
-  EFI_STATUS               Status;\r
   CPU_FEATURES_DATA        *CpuFeaturesData;\r
   ACPI_CPU_DATA            *AcpiCpuData;\r
   CPU_REGISTER_TABLE       *RegisterTable;\r
@@ -614,17 +972,8 @@ CpuRegisterTableWriteWorker (
 \r
   CpuFeaturesData = GetCpuFeaturesData ();\r
   if (CpuFeaturesData->RegisterTable == NULL) {\r
-    AcpiCpuData = (ACPI_CPU_DATA *) (UINTN) PcdGet64 (PcdCpuS3DataAddress);\r
-    if (AcpiCpuData == NULL) {\r
-      AcpiCpuData = AllocateAcpiCpuData ();\r
-      ASSERT (AcpiCpuData != NULL);\r
-      //\r
-      // Set PcdCpuS3DataAddress to the base address of the ACPI_CPU_DATA structure\r
-      //\r
-      Status = PcdSet64S (PcdCpuS3DataAddress, (UINT64)(UINTN)AcpiCpuData);\r
-      ASSERT_EFI_ERROR (Status);\r
-    }\r
-    ASSERT (AcpiCpuData->RegisterTable != 0);\r
+    AcpiCpuData = GetAcpiCpuData ();\r
+    ASSERT ((AcpiCpuData != NULL) && (AcpiCpuData->RegisterTable != 0));\r
     CpuFeaturesData->RegisterTable = (CPU_REGISTER_TABLE *) (UINTN) AcpiCpuData->RegisterTable;\r
     CpuFeaturesData->PreSmmRegisterTable = (CPU_REGISTER_TABLE *) (UINTN) AcpiCpuData->PreSmmInitRegisterTable;\r
   }\r