]> git.proxmox.com Git - mirror_edk2.git/blobdiff - UefiCpuPkg/Library/RegisterCpuFeaturesLib/RegisterCpuFeaturesLib.c
UefiCpuPkg: Apply uncrustify changes
[mirror_edk2.git] / UefiCpuPkg / Library / RegisterCpuFeaturesLib / RegisterCpuFeaturesLib.c
index 493566de5dea244501e07e4b405f47bb2b8bab4a..0285aaf5c91a2cd1dbc65bc5dc5860f0c767ee36 100644 (file)
@@ -1,7 +1,7 @@
 /** @file\r
   CPU Register Table Library functions.\r
 \r
-  Copyright (c) 2017 - 2019, Intel Corporation. All rights reserved.<BR>\r
+  Copyright (c) 2017 - 2021, Intel Corporation. All rights reserved.<BR>\r
   SPDX-License-Identifier: BSD-2-Clause-Patent\r
 \r
 **/\r
 **/\r
 VOID\r
 DumpCpuFeatureMask (\r
-  IN UINT8               *FeatureMask,\r
-  IN UINT32              BitMaskSize\r
+  IN UINT8   *FeatureMask,\r
+  IN UINT32  BitMaskSize\r
   )\r
 {\r
-  UINTN                  Index;\r
-  UINT8                  *Data8;\r
+  UINTN  Index;\r
+  UINT8  *Data8;\r
 \r
-  Data8       = (UINT8 *) FeatureMask;\r
+  Data8 = (UINT8 *)FeatureMask;\r
   for (Index = 0; Index < BitMaskSize; Index++) {\r
     DEBUG ((DEBUG_INFO, " %02x ", *Data8++));\r
   }\r
+\r
   DEBUG ((DEBUG_INFO, "\n"));\r
 }\r
 \r
@@ -44,7 +45,6 @@ DumpCpuFeature (
   IN UINT32              BitMaskSize\r
   )\r
 {\r
-\r
   if (CpuFeature->FeatureName != NULL) {\r
     DEBUG ((DEBUG_INFO, "FeatureName: %a\n", CpuFeature->FeatureName));\r
   } else {\r
@@ -64,8 +64,8 @@ DumpCpuFeature (
 **/\r
 BOOLEAN\r
 IsBitMaskMatchCheck (\r
-  IN UINT8        *FeatureMask,\r
-  IN UINT8        *DependentBitMask\r
+  IN UINT8  *FeatureMask,\r
+  IN UINT8  *DependentBitMask\r
   )\r
 {\r
   UINTN              Index;\r
@@ -82,6 +82,7 @@ IsBitMaskMatchCheck (
       return TRUE;\r
     }\r
   }\r
+\r
   return FALSE;\r
 }\r
 \r
@@ -98,14 +99,14 @@ IsBitMaskMatchCheck (
 **/\r
 BOOLEAN\r
 FindSpecifyFeature (\r
-  IN LIST_ENTRY              *FeatureList,\r
-  IN LIST_ENTRY              *CurrentEntry,\r
-  IN BOOLEAN                 SearchFormer,\r
-  IN UINT8                   *FeatureMask\r
+  IN LIST_ENTRY  *FeatureList,\r
+  IN LIST_ENTRY  *CurrentEntry,\r
+  IN BOOLEAN     SearchFormer,\r
+  IN UINT8       *FeatureMask\r
   )\r
 {\r
-  CPU_FEATURES_ENTRY         *CpuFeature;\r
-  LIST_ENTRY                 *NextEntry;\r
+  CPU_FEATURES_ENTRY  *CpuFeature;\r
+  LIST_ENTRY          *NextEntry;\r
 \r
   //\r
   // Check whether exist the not neighborhood entry first.\r
@@ -165,9 +166,9 @@ FindSpecifyFeature (
 **/\r
 CPU_FEATURE_DEPENDENCE_TYPE\r
 DetectFeatureScope (\r
-  IN CPU_FEATURES_ENTRY         *CpuFeature,\r
-  IN BOOLEAN                    Before,\r
-  IN UINT8                      *NextCpuFeatureMask\r
+  IN CPU_FEATURES_ENTRY  *CpuFeature,\r
+  IN BOOLEAN             Before,\r
+  IN UINT8               *NextCpuFeatureMask\r
   )\r
 {\r
   //\r
@@ -185,17 +186,20 @@ DetectFeatureScope (
 \r
   if (Before) {\r
     if ((CpuFeature->PackageBeforeFeatureBitMask != NULL) &&\r
-        IsBitMaskMatchCheck (NextCpuFeatureMask, CpuFeature->PackageBeforeFeatureBitMask)) {\r
+        IsBitMaskMatchCheck (NextCpuFeatureMask, CpuFeature->PackageBeforeFeatureBitMask))\r
+    {\r
       return PackageDepType;\r
     }\r
 \r
     if ((CpuFeature->CoreBeforeFeatureBitMask != NULL) &&\r
-        IsBitMaskMatchCheck (NextCpuFeatureMask, CpuFeature->CoreBeforeFeatureBitMask)) {\r
+        IsBitMaskMatchCheck (NextCpuFeatureMask, CpuFeature->CoreBeforeFeatureBitMask))\r
+    {\r
       return CoreDepType;\r
     }\r
 \r
-    if ((CpuFeature->BeforeFeatureBitMask != NULL) &&\r
-        IsBitMaskMatchCheck (NextCpuFeatureMask, CpuFeature->BeforeFeatureBitMask)) {\r
+    if ((CpuFeature->ThreadBeforeFeatureBitMask != NULL) &&\r
+        IsBitMaskMatchCheck (NextCpuFeatureMask, CpuFeature->ThreadBeforeFeatureBitMask))\r
+    {\r
       return ThreadDepType;\r
     }\r
 \r
@@ -203,17 +207,20 @@ DetectFeatureScope (
   }\r
 \r
   if ((CpuFeature->PackageAfterFeatureBitMask != NULL) &&\r
-      IsBitMaskMatchCheck (NextCpuFeatureMask, CpuFeature->PackageAfterFeatureBitMask)) {\r
+      IsBitMaskMatchCheck (NextCpuFeatureMask, CpuFeature->PackageAfterFeatureBitMask))\r
+  {\r
     return PackageDepType;\r
   }\r
 \r
   if ((CpuFeature->CoreAfterFeatureBitMask != NULL) &&\r
-      IsBitMaskMatchCheck (NextCpuFeatureMask, CpuFeature->CoreAfterFeatureBitMask)) {\r
+      IsBitMaskMatchCheck (NextCpuFeatureMask, CpuFeature->CoreAfterFeatureBitMask))\r
+  {\r
     return CoreDepType;\r
   }\r
 \r
-  if ((CpuFeature->AfterFeatureBitMask != NULL) &&\r
-      IsBitMaskMatchCheck (NextCpuFeatureMask, CpuFeature->AfterFeatureBitMask)) {\r
+  if ((CpuFeature->ThreadAfterFeatureBitMask != NULL) &&\r
+      IsBitMaskMatchCheck (NextCpuFeatureMask, CpuFeature->ThreadAfterFeatureBitMask))\r
+  {\r
     return ThreadDepType;\r
   }\r
 \r
@@ -231,24 +238,27 @@ DetectFeatureScope (
 **/\r
 CPU_FEATURE_DEPENDENCE_TYPE\r
 DetectNoneNeighborhoodFeatureScope (\r
-  IN CPU_FEATURES_ENTRY         *CpuFeature,\r
-  IN BOOLEAN                    Before,\r
-  IN LIST_ENTRY                 *FeatureList\r
+  IN CPU_FEATURES_ENTRY  *CpuFeature,\r
+  IN BOOLEAN             Before,\r
+  IN LIST_ENTRY          *FeatureList\r
   )\r
 {\r
   if (Before) {\r
     if ((CpuFeature->PackageBeforeFeatureBitMask != NULL) &&\r
-        FindSpecifyFeature(FeatureList, &CpuFeature->Link, FALSE, CpuFeature->PackageBeforeFeatureBitMask)) {\r
+        FindSpecifyFeature (FeatureList, &CpuFeature->Link, FALSE, CpuFeature->PackageBeforeFeatureBitMask))\r
+    {\r
       return PackageDepType;\r
     }\r
 \r
     if ((CpuFeature->CoreBeforeFeatureBitMask != NULL) &&\r
-        FindSpecifyFeature(FeatureList, &CpuFeature->Link, FALSE, CpuFeature->CoreBeforeFeatureBitMask)) {\r
+        FindSpecifyFeature (FeatureList, &CpuFeature->Link, FALSE, CpuFeature->CoreBeforeFeatureBitMask))\r
+    {\r
       return CoreDepType;\r
     }\r
 \r
-    if ((CpuFeature->BeforeFeatureBitMask != NULL) &&\r
-        FindSpecifyFeature(FeatureList, &CpuFeature->Link, FALSE, CpuFeature->BeforeFeatureBitMask)) {\r
+    if ((CpuFeature->ThreadBeforeFeatureBitMask != NULL) &&\r
+        FindSpecifyFeature (FeatureList, &CpuFeature->Link, FALSE, CpuFeature->ThreadBeforeFeatureBitMask))\r
+    {\r
       return ThreadDepType;\r
     }\r
 \r
@@ -256,17 +266,20 @@ DetectNoneNeighborhoodFeatureScope (
   }\r
 \r
   if ((CpuFeature->PackageAfterFeatureBitMask != NULL) &&\r
-      FindSpecifyFeature(FeatureList, &CpuFeature->Link, TRUE, CpuFeature->PackageAfterFeatureBitMask)) {\r
+      FindSpecifyFeature (FeatureList, &CpuFeature->Link, TRUE, CpuFeature->PackageAfterFeatureBitMask))\r
+  {\r
     return PackageDepType;\r
   }\r
 \r
   if ((CpuFeature->CoreAfterFeatureBitMask != NULL) &&\r
-      FindSpecifyFeature(FeatureList, &CpuFeature->Link, TRUE, CpuFeature->CoreAfterFeatureBitMask)) {\r
+      FindSpecifyFeature (FeatureList, &CpuFeature->Link, TRUE, CpuFeature->CoreAfterFeatureBitMask))\r
+  {\r
     return CoreDepType;\r
   }\r
 \r
-  if ((CpuFeature->AfterFeatureBitMask != NULL) &&\r
-      FindSpecifyFeature(FeatureList, &CpuFeature->Link, TRUE, CpuFeature->AfterFeatureBitMask)) {\r
+  if ((CpuFeature->ThreadAfterFeatureBitMask != NULL) &&\r
+      FindSpecifyFeature (FeatureList, &CpuFeature->Link, TRUE, CpuFeature->ThreadAfterFeatureBitMask))\r
+  {\r
     return ThreadDepType;\r
   }\r
 \r
@@ -294,17 +307,17 @@ DetectNoneNeighborhoodFeatureScope (
 **/\r
 BOOLEAN\r
 AdjustFeaturesDependence (\r
-  IN OUT CPU_FEATURES_ENTRY         *PreviousFeature,\r
-  IN OUT CPU_FEATURES_ENTRY         *CurrentFeature,\r
-  IN     CPU_FEATURES_ENTRY         *FindFeature,\r
-  IN     BOOLEAN                    Before\r
+  IN OUT CPU_FEATURES_ENTRY  *PreviousFeature,\r
+  IN OUT CPU_FEATURES_ENTRY  *CurrentFeature,\r
+  IN     CPU_FEATURES_ENTRY  *FindFeature,\r
+  IN     BOOLEAN             Before\r
   )\r
 {\r
-  CPU_FEATURE_DEPENDENCE_TYPE            PreDependType;\r
-  CPU_FEATURE_DEPENDENCE_TYPE            CurrentDependType;\r
+  CPU_FEATURE_DEPENDENCE_TYPE  PreDependType;\r
+  CPU_FEATURE_DEPENDENCE_TYPE  CurrentDependType;\r
 \r
-  PreDependType     = DetectFeatureScope(PreviousFeature, Before, FindFeature->FeatureMask);\r
-  CurrentDependType = DetectFeatureScope(CurrentFeature, Before, FindFeature->FeatureMask);\r
+  PreDependType     = DetectFeatureScope (PreviousFeature, Before, FindFeature->FeatureMask);\r
+  CurrentDependType = DetectFeatureScope (CurrentFeature, Before, FindFeature->FeatureMask);\r
 \r
   //\r
   // If previous feature has no dependence with the find featue.\r
@@ -336,16 +349,16 @@ AdjustFeaturesDependence (
 **/\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
+  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
-  CPU_FEATURES_ENTRY        *FindFeature;\r
+  LIST_ENTRY          *PreviousEntry;\r
+  CPU_FEATURES_ENTRY  *PreviousFeature;\r
+  CPU_FEATURES_ENTRY  *CurrentFeature;\r
+  CPU_FEATURES_ENTRY  *FindFeature;\r
 \r
   //\r
   // For CPU feature which has core or package type dependence, later code need to insert\r
@@ -363,11 +376,10 @@ AdjustEntry (
   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
+  CurrentFeature = CPU_FEATURE_ENTRY_FROM_LINK (CurrentEntry);\r
   RemoveEntryList (CurrentEntry);\r
 \r
   if (IsNull (FeatureList, PreviousEntry)) {\r
@@ -407,7 +419,6 @@ AdjustEntry (
   }\r
 }\r
 \r
-\r
 /**\r
   Checks and adjusts current CPU features per dependency relationship.\r
 \r
@@ -419,14 +430,14 @@ AdjustEntry (
 **/\r
 BOOLEAN\r
 InsertToBeforeEntry (\r
-  IN LIST_ENTRY              *FeatureList,\r
-  IN LIST_ENTRY              *CurrentEntry,\r
-  IN UINT8                   *FeatureMask\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
+  LIST_ENTRY          *CheckEntry;\r
+  CPU_FEATURES_ENTRY  *CheckFeature;\r
+  BOOLEAN             Swapped;\r
 \r
   Swapped = FALSE;\r
 \r
@@ -441,6 +452,7 @@ InsertToBeforeEntry (
       Swapped = TRUE;\r
       break;\r
     }\r
+\r
     CheckEntry = CheckEntry->ForwardLink;\r
   }\r
 \r
@@ -458,14 +470,14 @@ InsertToBeforeEntry (
 **/\r
 BOOLEAN\r
 InsertToAfterEntry (\r
-  IN LIST_ENTRY              *FeatureList,\r
-  IN LIST_ENTRY              *CurrentEntry,\r
-  IN UINT8                   *FeatureMask\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
+  LIST_ENTRY          *CheckEntry;\r
+  CPU_FEATURES_ENTRY  *CheckFeature;\r
+  BOOLEAN             Swapped;\r
 \r
   Swapped = FALSE;\r
 \r
@@ -480,6 +492,7 @@ InsertToAfterEntry (
       Swapped = TRUE;\r
       break;\r
     }\r
+\r
     CheckEntry = CheckEntry->ForwardLink;\r
   }\r
 \r
@@ -493,22 +506,22 @@ InsertToAfterEntry (
 **/\r
 VOID\r
 CheckCpuFeaturesDependency (\r
-  IN LIST_ENTRY              *FeatureList\r
+  IN LIST_ENTRY  *FeatureList\r
   )\r
 {\r
-  LIST_ENTRY                 *CurrentEntry;\r
-  CPU_FEATURES_ENTRY         *CpuFeature;\r
-  LIST_ENTRY                 *CheckEntry;\r
-  CPU_FEATURES_ENTRY         *CheckFeature;\r
-  BOOLEAN                    Swapped;\r
-  LIST_ENTRY                 *TempEntry;\r
-  LIST_ENTRY                 *NextEntry;\r
+  LIST_ENTRY          *CurrentEntry;\r
+  CPU_FEATURES_ENTRY  *CpuFeature;\r
+  LIST_ENTRY          *CheckEntry;\r
+  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
+    Swapped    = FALSE;\r
     CpuFeature = CPU_FEATURE_ENTRY_FROM_LINK (CurrentEntry);\r
-    NextEntry = CurrentEntry->ForwardLink;\r
+    NextEntry  = CurrentEntry->ForwardLink;\r
     if (CpuFeature->BeforeAll) {\r
       //\r
       // Check all features dispatched before this entry\r
@@ -526,8 +539,10 @@ CheckCpuFeaturesDependency (
           Swapped = TRUE;\r
           break;\r
         }\r
+\r
         CheckEntry = CheckEntry->ForwardLink;\r
       }\r
+\r
       if (Swapped) {\r
         CurrentEntry = NextEntry;\r
         continue;\r
@@ -550,26 +565,28 @@ CheckCpuFeaturesDependency (
           RemoveEntryList (CurrentEntry);\r
           InsertHeadList (CheckEntry, CurrentEntry);\r
           CurrentEntry = TempEntry;\r
-          Swapped = TRUE;\r
+          Swapped      = TRUE;\r
           break;\r
         }\r
+\r
         CheckEntry = CheckEntry->ForwardLink;\r
       }\r
+\r
       if (Swapped) {\r
         CurrentEntry = NextEntry;\r
         continue;\r
       }\r
     }\r
 \r
-    if (CpuFeature->BeforeFeatureBitMask != NULL) {\r
-      Swapped = InsertToBeforeEntry (FeatureList, CurrentEntry, CpuFeature->BeforeFeatureBitMask);\r
+    if (CpuFeature->ThreadBeforeFeatureBitMask != NULL) {\r
+      Swapped = InsertToBeforeEntry (FeatureList, CurrentEntry, CpuFeature->ThreadBeforeFeatureBitMask);\r
       if (Swapped) {\r
         continue;\r
       }\r
     }\r
 \r
-    if (CpuFeature->AfterFeatureBitMask != NULL) {\r
-      Swapped = InsertToAfterEntry (FeatureList, CurrentEntry, CpuFeature->AfterFeatureBitMask);\r
+    if (CpuFeature->ThreadAfterFeatureBitMask != NULL) {\r
+      Swapped = InsertToAfterEntry (FeatureList, CurrentEntry, CpuFeature->ThreadAfterFeatureBitMask);\r
       if (Swapped) {\r
         continue;\r
       }\r
@@ -622,18 +639,18 @@ CheckCpuFeaturesDependency (
 **/\r
 RETURN_STATUS\r
 RegisterCpuFeatureWorker (\r
-  IN CPU_FEATURES_DATA       *CpuFeaturesData,\r
-  IN CPU_FEATURES_ENTRY      *CpuFeature\r
+  IN CPU_FEATURES_DATA   *CpuFeaturesData,\r
+  IN CPU_FEATURES_ENTRY  *CpuFeature\r
   )\r
 {\r
-  EFI_STATUS                 Status;\r
-  CPU_FEATURES_ENTRY         *CpuFeatureEntry;\r
-  LIST_ENTRY                 *Entry;\r
-  BOOLEAN                    FeatureExist;\r
+  EFI_STATUS          Status;\r
+  CPU_FEATURES_ENTRY  *CpuFeatureEntry;\r
+  LIST_ENTRY          *Entry;\r
+  BOOLEAN             FeatureExist;\r
 \r
-  FeatureExist = FALSE;\r
+  FeatureExist    = FALSE;\r
   CpuFeatureEntry = NULL;\r
-  Entry = GetFirstNode (&CpuFeaturesData->FeatureList);\r
+  Entry           = GetFirstNode (&CpuFeaturesData->FeatureList);\r
   while (!IsNull (&CpuFeaturesData->FeatureList, Entry)) {\r
     CpuFeatureEntry = CPU_FEATURE_ENTRY_FROM_LINK (Entry);\r
     if (CompareMem (CpuFeature->FeatureMask, CpuFeatureEntry->FeatureMask, CpuFeaturesData->BitMaskSize) == 0) {\r
@@ -643,6 +660,7 @@ RegisterCpuFeatureWorker (
       FeatureExist = TRUE;\r
       break;\r
     }\r
+\r
     Entry = Entry->ForwardLink;\r
   }\r
 \r
@@ -661,55 +679,71 @@ RegisterCpuFeatureWorker (
     if (CpuFeature->GetConfigDataFunc != NULL) {\r
       CpuFeatureEntry->GetConfigDataFunc = CpuFeature->GetConfigDataFunc;\r
     }\r
+\r
     if (CpuFeature->SupportFunc != NULL) {\r
       CpuFeatureEntry->SupportFunc = CpuFeature->SupportFunc;\r
     }\r
+\r
     if (CpuFeature->InitializeFunc != NULL) {\r
       CpuFeatureEntry->InitializeFunc = CpuFeature->InitializeFunc;\r
     }\r
+\r
     if (CpuFeature->FeatureName != NULL) {\r
       if (CpuFeatureEntry->FeatureName == NULL) {\r
         CpuFeatureEntry->FeatureName = AllocatePool (CPU_FEATURE_NAME_SIZE);\r
         ASSERT (CpuFeatureEntry->FeatureName != NULL);\r
       }\r
+\r
       Status = AsciiStrCpyS (CpuFeatureEntry->FeatureName, CPU_FEATURE_NAME_SIZE, CpuFeature->FeatureName);\r
       ASSERT_EFI_ERROR (Status);\r
       FreePool (CpuFeature->FeatureName);\r
     }\r
-    if (CpuFeature->BeforeFeatureBitMask != NULL) {\r
-      if (CpuFeatureEntry->BeforeFeatureBitMask != NULL) {\r
-        FreePool (CpuFeatureEntry->BeforeFeatureBitMask);\r
+\r
+    if (CpuFeature->ThreadBeforeFeatureBitMask != NULL) {\r
+      if (CpuFeatureEntry->ThreadBeforeFeatureBitMask != NULL) {\r
+        FreePool (CpuFeatureEntry->ThreadBeforeFeatureBitMask);\r
       }\r
-      CpuFeatureEntry->BeforeFeatureBitMask = CpuFeature->BeforeFeatureBitMask;\r
+\r
+      CpuFeatureEntry->ThreadBeforeFeatureBitMask = CpuFeature->ThreadBeforeFeatureBitMask;\r
     }\r
-    if (CpuFeature->AfterFeatureBitMask != NULL) {\r
-      if (CpuFeatureEntry->AfterFeatureBitMask != NULL) {\r
-        FreePool (CpuFeatureEntry->AfterFeatureBitMask);\r
+\r
+    if (CpuFeature->ThreadAfterFeatureBitMask != NULL) {\r
+      if (CpuFeatureEntry->ThreadAfterFeatureBitMask != NULL) {\r
+        FreePool (CpuFeatureEntry->ThreadAfterFeatureBitMask);\r
       }\r
-      CpuFeatureEntry->AfterFeatureBitMask = CpuFeature->AfterFeatureBitMask;\r
+\r
+      CpuFeatureEntry->ThreadAfterFeatureBitMask = CpuFeature->ThreadAfterFeatureBitMask;\r
     }\r
+\r
     if (CpuFeature->CoreBeforeFeatureBitMask != NULL) {\r
       if (CpuFeatureEntry->CoreBeforeFeatureBitMask != NULL) {\r
         FreePool (CpuFeatureEntry->CoreBeforeFeatureBitMask);\r
       }\r
+\r
       CpuFeatureEntry->CoreBeforeFeatureBitMask = CpuFeature->CoreBeforeFeatureBitMask;\r
     }\r
+\r
     if (CpuFeature->CoreAfterFeatureBitMask != NULL) {\r
       if (CpuFeatureEntry->CoreAfterFeatureBitMask != NULL) {\r
         FreePool (CpuFeatureEntry->CoreAfterFeatureBitMask);\r
       }\r
+\r
       CpuFeatureEntry->CoreAfterFeatureBitMask = CpuFeature->CoreAfterFeatureBitMask;\r
     }\r
+\r
     if (CpuFeature->PackageBeforeFeatureBitMask != NULL) {\r
       if (CpuFeatureEntry->PackageBeforeFeatureBitMask != NULL) {\r
         FreePool (CpuFeatureEntry->PackageBeforeFeatureBitMask);\r
       }\r
+\r
       CpuFeatureEntry->PackageBeforeFeatureBitMask = CpuFeature->PackageBeforeFeatureBitMask;\r
     }\r
+\r
     if (CpuFeature->PackageAfterFeatureBitMask != NULL) {\r
       if (CpuFeatureEntry->PackageAfterFeatureBitMask != NULL) {\r
         FreePool (CpuFeatureEntry->PackageAfterFeatureBitMask);\r
       }\r
+\r
       CpuFeatureEntry->PackageAfterFeatureBitMask = CpuFeature->PackageAfterFeatureBitMask;\r
     }\r
 \r
@@ -719,6 +753,7 @@ RegisterCpuFeatureWorker (
     FreePool (CpuFeature->FeatureMask);\r
     FreePool (CpuFeature);\r
   }\r
+\r
   //\r
   // Verify CPU features dependency can change CPU feature order\r
   //\r
@@ -735,12 +770,12 @@ RegisterCpuFeatureWorker (
 **/\r
 VOID\r
 SetCpuFeaturesBitMask (\r
-  IN UINT8               **FeaturesBitMask,\r
-  IN UINT32              Feature,\r
-  IN UINTN               BitMaskSize\r
+  IN UINT8   **FeaturesBitMask,\r
+  IN UINT32  Feature,\r
+  IN UINTN   BitMaskSize\r
   )\r
 {\r
-  UINT8                  *CpuFeaturesBitMask;\r
+  UINT8  *CpuFeaturesBitMask;\r
 \r
   ASSERT (FeaturesBitMask != NULL);\r
   CpuFeaturesBitMask = *FeaturesBitMask;\r
@@ -751,7 +786,7 @@ SetCpuFeaturesBitMask (
   }\r
 \r
   CpuFeaturesBitMask  += (Feature / 8);\r
-  *CpuFeaturesBitMask |= (UINT8) (1 << (Feature % 8));\r
+  *CpuFeaturesBitMask |= (UINT8)(1 << (Feature % 8));\r
 }\r
 \r
 /**\r
@@ -803,72 +838,87 @@ SetCpuFeaturesBitMask (
 RETURN_STATUS\r
 EFIAPI\r
 RegisterCpuFeature (\r
-  IN CHAR8                             *FeatureName,       OPTIONAL\r
-  IN CPU_FEATURE_GET_CONFIG_DATA       GetConfigDataFunc,  OPTIONAL\r
-  IN CPU_FEATURE_SUPPORT               SupportFunc,        OPTIONAL\r
-  IN CPU_FEATURE_INITIALIZE            InitializeFunc,     OPTIONAL\r
+  IN CHAR8                        *FeatureName        OPTIONAL,\r
+  IN CPU_FEATURE_GET_CONFIG_DATA  GetConfigDataFunc   OPTIONAL,\r
+  IN CPU_FEATURE_SUPPORT          SupportFunc         OPTIONAL,\r
+  IN CPU_FEATURE_INITIALIZE       InitializeFunc      OPTIONAL,\r
   ...\r
   )\r
 {\r
-  EFI_STATUS                 Status;\r
-  VA_LIST                    Marker;\r
-  UINT32                     Feature;\r
-  CPU_FEATURES_ENTRY         *CpuFeature;\r
-  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
-  CPU_FEATURES_DATA          *CpuFeaturesData;\r
+  EFI_STATUS          Status;\r
+  VA_LIST             Marker;\r
+  UINT32              Feature;\r
+  CPU_FEATURES_ENTRY  *CpuFeature;\r
+  UINT8               *FeatureMask;\r
+  UINT8               *ThreadBeforeFeatureBitMask;\r
+  UINT8               *ThreadAfterFeatureBitMask;\r
+  UINT8               *CoreBeforeFeatureBitMask;\r
+  UINT8               *CoreAfterFeatureBitMask;\r
+  UINT8               *PackageBeforeFeatureBitMask;\r
+  UINT8               *PackageAfterFeatureBitMask;\r
+  BOOLEAN             BeforeAll;\r
+  BOOLEAN             AfterAll;\r
+  CPU_FEATURES_DATA   *CpuFeaturesData;\r
 \r
   FeatureMask                 = NULL;\r
-  BeforeFeatureBitMask        = NULL;\r
-  AfterFeatureBitMask         = NULL;\r
+  ThreadBeforeFeatureBitMask  = NULL;\r
+  ThreadAfterFeatureBitMask   = NULL;\r
   CoreBeforeFeatureBitMask    = NULL;\r
   CoreAfterFeatureBitMask     = NULL;\r
   PackageBeforeFeatureBitMask = NULL;\r
   PackageAfterFeatureBitMask  = NULL;\r
-  BeforeAll            = FALSE;\r
-  AfterAll             = FALSE;\r
+  BeforeAll                   = FALSE;\r
+  AfterAll                    = FALSE;\r
 \r
   CpuFeaturesData = GetCpuFeaturesData ();\r
   if (CpuFeaturesData->FeaturesCount == 0) {\r
     InitializeListHead (&CpuFeaturesData->FeatureList);\r
     InitializeSpinLock (&CpuFeaturesData->CpuFlags.MemoryMappedLock);\r
-    InitializeSpinLock (&CpuFeaturesData->CpuFlags.ConsoleLogLock);\r
     //\r
     // Code assumes below three PCDs have PCD same buffer size.\r
     //\r
     ASSERT (PcdGetSize (PcdCpuFeaturesSetting) == PcdGetSize (PcdCpuFeaturesCapability));\r
     ASSERT (PcdGetSize (PcdCpuFeaturesSetting) == PcdGetSize (PcdCpuFeaturesSupport));\r
-    CpuFeaturesData->BitMaskSize = (UINT32) PcdGetSize (PcdCpuFeaturesSetting);\r
+    CpuFeaturesData->BitMaskSize = (UINT32)PcdGetSize (PcdCpuFeaturesSetting);\r
   }\r
 \r
   VA_START (Marker, InitializeFunc);\r
   Feature = VA_ARG (Marker, UINT32);\r
   while (Feature != CPU_FEATURE_END) {\r
-    ASSERT ((Feature & (CPU_FEATURE_BEFORE | CPU_FEATURE_AFTER))\r
-                    != (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
+    //\r
+    // It's invalid to require a feature is before AND after all other features.\r
+    //\r
+    ASSERT (\r
+      (Feature & (CPU_FEATURE_BEFORE_ALL | CPU_FEATURE_AFTER_ALL))\r
+      != (CPU_FEATURE_BEFORE_ALL | CPU_FEATURE_AFTER_ALL)\r
+      );\r
+\r
+    //\r
+    // It's invalid to require feature A is before AND after before feature B,\r
+    // either in thread level, core level or package level.\r
+    //\r
+    ASSERT (\r
+      (Feature & (CPU_FEATURE_THREAD_BEFORE | CPU_FEATURE_THREAD_AFTER))\r
+      != (CPU_FEATURE_THREAD_BEFORE | CPU_FEATURE_THREAD_AFTER)\r
+      );\r
+    ASSERT (\r
+      (Feature & (CPU_FEATURE_CORE_BEFORE | CPU_FEATURE_CORE_AFTER))\r
+      != (CPU_FEATURE_CORE_BEFORE | CPU_FEATURE_CORE_AFTER)\r
+      );\r
+    ASSERT (\r
+      (Feature & (CPU_FEATURE_PACKAGE_BEFORE | CPU_FEATURE_PACKAGE_AFTER))\r
+      != (CPU_FEATURE_PACKAGE_BEFORE | CPU_FEATURE_PACKAGE_AFTER)\r
+      );\r
+    if (Feature < CPU_FEATURE_THREAD_BEFORE) {\r
       BeforeAll = ((Feature & CPU_FEATURE_BEFORE_ALL) != 0) ? TRUE : FALSE;\r
       AfterAll  = ((Feature & CPU_FEATURE_AFTER_ALL) != 0) ? TRUE : FALSE;\r
       Feature  &= ~(CPU_FEATURE_BEFORE_ALL | CPU_FEATURE_AFTER_ALL);\r
       ASSERT (FeatureMask == NULL);\r
       SetCpuFeaturesBitMask (&FeatureMask, Feature, CpuFeaturesData->BitMaskSize);\r
-    } else if ((Feature & CPU_FEATURE_BEFORE) != 0) {\r
-      SetCpuFeaturesBitMask (&BeforeFeatureBitMask, Feature & ~CPU_FEATURE_BEFORE, CpuFeaturesData->BitMaskSize);\r
-    } else if ((Feature & CPU_FEATURE_AFTER) != 0) {\r
-      SetCpuFeaturesBitMask (&AfterFeatureBitMask, Feature & ~CPU_FEATURE_AFTER, CpuFeaturesData->BitMaskSize);\r
+    } else if ((Feature & CPU_FEATURE_THREAD_BEFORE) != 0) {\r
+      SetCpuFeaturesBitMask (&ThreadBeforeFeatureBitMask, Feature & ~CPU_FEATURE_THREAD_BEFORE, CpuFeaturesData->BitMaskSize);\r
+    } else if ((Feature & CPU_FEATURE_THREAD_AFTER) != 0) {\r
+      SetCpuFeaturesBitMask (&ThreadAfterFeatureBitMask, Feature & ~CPU_FEATURE_THREAD_AFTER, CpuFeaturesData->BitMaskSize);\r
     } else if ((Feature & CPU_FEATURE_CORE_BEFORE) != 0) {\r
       SetCpuFeaturesBitMask (&CoreBeforeFeatureBitMask, Feature & ~CPU_FEATURE_CORE_BEFORE, CpuFeaturesData->BitMaskSize);\r
     } else if ((Feature & CPU_FEATURE_CORE_AFTER) != 0) {\r
@@ -878,16 +928,18 @@ RegisterCpuFeature (
     } else if ((Feature & CPU_FEATURE_PACKAGE_AFTER) != 0) {\r
       SetCpuFeaturesBitMask (&PackageAfterFeatureBitMask, Feature & ~CPU_FEATURE_PACKAGE_AFTER, CpuFeaturesData->BitMaskSize);\r
     }\r
+\r
     Feature = VA_ARG (Marker, UINT32);\r
   }\r
+\r
   VA_END (Marker);\r
 \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->ThreadBeforeFeatureBitMask  = ThreadBeforeFeatureBitMask;\r
+  CpuFeature->ThreadAfterFeatureBitMask   = ThreadAfterFeatureBitMask;\r
   CpuFeature->CoreBeforeFeatureBitMask    = CoreBeforeFeatureBitMask;\r
   CpuFeature->CoreAfterFeatureBitMask     = CoreAfterFeatureBitMask;\r
   CpuFeature->PackageBeforeFeatureBitMask = PackageBeforeFeatureBitMask;\r
@@ -898,7 +950,7 @@ RegisterCpuFeature (
   CpuFeature->SupportFunc                 = SupportFunc;\r
   CpuFeature->InitializeFunc              = InitializeFunc;\r
   if (FeatureName != NULL) {\r
-    CpuFeature->FeatureName          = AllocatePool (CPU_FEATURE_NAME_SIZE);\r
+    CpuFeature->FeatureName = AllocatePool (CPU_FEATURE_NAME_SIZE);\r
     ASSERT (CpuFeature->FeatureName != NULL);\r
     Status = AsciiStrCpyS (CpuFeature->FeatureName, CPU_FEATURE_NAME_SIZE, FeatureName);\r
     ASSERT_EFI_ERROR (Status);\r
@@ -920,55 +972,65 @@ GetAcpiCpuData (
   VOID\r
   )\r
 {\r
-  EFI_STATUS                           Status;\r
-  UINTN                                NumberOfCpus;\r
-  UINTN                                NumberOfEnabledProcessors;\r
-  ACPI_CPU_DATA                        *AcpiCpuData;\r
-  UINTN                                TableSize;\r
-  CPU_REGISTER_TABLE                   *RegisterTable;\r
-  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
+  EFI_STATUS                 Status;\r
+  UINTN                      NumberOfCpus;\r
+  UINTN                      NumberOfEnabledProcessors;\r
+  ACPI_CPU_DATA              *AcpiCpuData;\r
+  UINTN                      TableSize;\r
+  CPU_REGISTER_TABLE         *RegisterTable;\r
+  UINTN                      Index;\r
+  EFI_PROCESSOR_INFORMATION  ProcessorInfoBuffer;\r
+\r
+  AcpiCpuData = (ACPI_CPU_DATA *)(UINTN)PcdGet64 (PcdCpuS3DataAddress);\r
+  if (AcpiCpuData == NULL) {\r
+    AcpiCpuData = AllocatePages (EFI_SIZE_TO_PAGES (sizeof (ACPI_CPU_DATA)));\r
+    ASSERT (AcpiCpuData != NULL);\r
+    ZeroMem (AcpiCpuData, sizeof (ACPI_CPU_DATA));\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
-  AcpiCpuData  = AllocatePages (EFI_SIZE_TO_PAGES (sizeof (ACPI_CPU_DATA)));\r
-  ASSERT (AcpiCpuData != NULL);\r
+    GetNumberOfProcessor (&NumberOfCpus, &NumberOfEnabledProcessors);\r
+    AcpiCpuData->NumberOfCpus = (UINT32)NumberOfCpus;\r
+  }\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
+  if ((AcpiCpuData->CpuFeatureInitData.RegisterTable == 0) ||\r
+      (AcpiCpuData->CpuFeatureInitData.PreSmmInitRegisterTable == 0))\r
+  {\r
+    //\r
+    // Allocate buffer for empty RegisterTable and PreSmmInitRegisterTable for all CPUs\r
+    //\r
+    NumberOfCpus  = AcpiCpuData->NumberOfCpus;\r
+    TableSize     = 2 * NumberOfCpus * sizeof (CPU_REGISTER_TABLE);\r
+    RegisterTable = AllocatePages (EFI_SIZE_TO_PAGES (TableSize));\r
+    ASSERT (RegisterTable != NULL);\r
 \r
-  GetNumberOfProcessor (&NumberOfCpus, &NumberOfEnabledProcessors);\r
-  AcpiCpuData->NumberOfCpus = (UINT32)NumberOfCpus;\r
+    for (Index = 0; Index < NumberOfCpus; Index++) {\r
+      Status = GetProcessorInformation (Index, &ProcessorInfoBuffer);\r
+      ASSERT_EFI_ERROR (Status);\r
 \r
-  //\r
-  // Allocate buffer for empty RegisterTable and PreSmmInitRegisterTable for all CPUs\r
-  //\r
-  TableSize = 2 * NumberOfCpus * sizeof (CPU_REGISTER_TABLE);\r
-  RegisterTable  = AllocatePages (EFI_SIZE_TO_PAGES (TableSize));\r
-  ASSERT (RegisterTable != NULL);\r
+      RegisterTable[Index].InitialApicId      = (UINT32)ProcessorInfoBuffer.ProcessorId;\r
+      RegisterTable[Index].TableLength        = 0;\r
+      RegisterTable[Index].AllocatedSize      = 0;\r
+      RegisterTable[Index].RegisterTableEntry = 0;\r
 \r
-  for (Index = 0; Index < NumberOfCpus; Index++) {\r
-    Status = GetProcessorInformation (Index, &ProcessorInfoBuffer);\r
-    ASSERT_EFI_ERROR (Status);\r
+      RegisterTable[NumberOfCpus + Index].InitialApicId      = (UINT32)ProcessorInfoBuffer.ProcessorId;\r
+      RegisterTable[NumberOfCpus + Index].TableLength        = 0;\r
+      RegisterTable[NumberOfCpus + Index].AllocatedSize      = 0;\r
+      RegisterTable[NumberOfCpus + Index].RegisterTableEntry = 0;\r
+    }\r
 \r
-    RegisterTable[Index].InitialApicId      = (UINT32)ProcessorInfoBuffer.ProcessorId;\r
-    RegisterTable[Index].TableLength        = 0;\r
-    RegisterTable[Index].AllocatedSize      = 0;\r
-    RegisterTable[Index].RegisterTableEntry = 0;\r
+    if (AcpiCpuData->CpuFeatureInitData.RegisterTable == 0) {\r
+      AcpiCpuData->CpuFeatureInitData.RegisterTable = (EFI_PHYSICAL_ADDRESS)(UINTN)RegisterTable;\r
+    }\r
 \r
-    RegisterTable[NumberOfCpus + Index].InitialApicId      = (UINT32)ProcessorInfoBuffer.ProcessorId;\r
-    RegisterTable[NumberOfCpus + Index].TableLength        = 0;\r
-    RegisterTable[NumberOfCpus + Index].AllocatedSize      = 0;\r
-    RegisterTable[NumberOfCpus + Index].RegisterTableEntry = 0;\r
+    if (AcpiCpuData->CpuFeatureInitData.PreSmmInitRegisterTable == 0) {\r
+      AcpiCpuData->CpuFeatureInitData.PreSmmInitRegisterTable = (EFI_PHYSICAL_ADDRESS)(UINTN)(RegisterTable + NumberOfCpus);\r
+    }\r
   }\r
-  AcpiCpuData->RegisterTable           = (EFI_PHYSICAL_ADDRESS)(UINTN)RegisterTable;\r
-  AcpiCpuData->PreSmmInitRegisterTable = (EFI_PHYSICAL_ADDRESS)(UINTN)(RegisterTable + NumberOfCpus);\r
 \r
   return AcpiCpuData;\r
 }\r
@@ -981,14 +1043,14 @@ GetAcpiCpuData (
 STATIC\r
 VOID\r
 EnlargeRegisterTable (\r
-  IN OUT CPU_REGISTER_TABLE            *RegisterTable\r
+  IN OUT CPU_REGISTER_TABLE  *RegisterTable\r
   )\r
 {\r
   EFI_PHYSICAL_ADDRESS  Address;\r
   UINTN                 UsedPages;\r
 \r
   UsedPages = RegisterTable->AllocatedSize / EFI_PAGE_SIZE;\r
-  Address  = (UINTN)AllocatePages (UsedPages + 1);\r
+  Address   = (UINTN)AllocatePages (UsedPages + 1);\r
   ASSERT (Address != 0);\r
 \r
   //\r
@@ -997,8 +1059,8 @@ EnlargeRegisterTable (
   //\r
   if (RegisterTable->AllocatedSize > 0) {\r
     CopyMem (\r
-      (VOID *) (UINTN) Address,\r
-      (VOID *) (UINTN) RegisterTable->RegisterTableEntry,\r
+      (VOID *)(UINTN)Address,\r
+      (VOID *)(UINTN)RegisterTable->RegisterTableEntry,\r
       RegisterTable->AllocatedSize\r
       );\r
 \r
@@ -1026,29 +1088,32 @@ EnlargeRegisterTable (
   @param[in]  ValidBitStart    Start of the bit section\r
   @param[in]  ValidBitLength   Length of the bit section\r
   @param[in]  Value            Value to write\r
+  @param[in]  TestThenWrite    Whether need to test current Value before writing.\r
+\r
 **/\r
 VOID\r
 CpuRegisterTableWriteWorker (\r
-  IN BOOLEAN                 PreSmmFlag,\r
-  IN UINTN                   ProcessorNumber,\r
-  IN REGISTER_TYPE           RegisterType,\r
-  IN UINT64                  Index,\r
-  IN UINT8                   ValidBitStart,\r
-  IN UINT8                   ValidBitLength,\r
-  IN UINT64                  Value\r
+  IN BOOLEAN        PreSmmFlag,\r
+  IN UINTN          ProcessorNumber,\r
+  IN REGISTER_TYPE  RegisterType,\r
+  IN UINT64         Index,\r
+  IN UINT8          ValidBitStart,\r
+  IN UINT8          ValidBitLength,\r
+  IN UINT64         Value,\r
+  IN BOOLEAN        TestThenWrite\r
   )\r
 {\r
-  CPU_FEATURES_DATA        *CpuFeaturesData;\r
-  ACPI_CPU_DATA            *AcpiCpuData;\r
-  CPU_REGISTER_TABLE       *RegisterTable;\r
-  CPU_REGISTER_TABLE_ENTRY *RegisterTableEntry;\r
+  CPU_FEATURES_DATA         *CpuFeaturesData;\r
+  ACPI_CPU_DATA             *AcpiCpuData;\r
+  CPU_REGISTER_TABLE        *RegisterTable;\r
+  CPU_REGISTER_TABLE_ENTRY  *RegisterTableEntry;\r
 \r
   CpuFeaturesData = GetCpuFeaturesData ();\r
   if (CpuFeaturesData->RegisterTable == NULL) {\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
+    ASSERT ((AcpiCpuData != NULL) && (AcpiCpuData->CpuFeatureInitData.RegisterTable != 0));\r
+    CpuFeaturesData->RegisterTable       = (CPU_REGISTER_TABLE *)(UINTN)AcpiCpuData->CpuFeatureInitData.RegisterTable;\r
+    CpuFeaturesData->PreSmmRegisterTable = (CPU_REGISTER_TABLE *)(UINTN)AcpiCpuData->CpuFeatureInitData.PreSmmInitRegisterTable;\r
   }\r
 \r
   if (PreSmmFlag) {\r
@@ -1064,13 +1129,14 @@ CpuRegisterTableWriteWorker (
   //\r
   // Append entry in the register table.\r
   //\r
-  RegisterTableEntry = (CPU_REGISTER_TABLE_ENTRY *) (UINTN) RegisterTable->RegisterTableEntry;\r
+  RegisterTableEntry                                            = (CPU_REGISTER_TABLE_ENTRY *)(UINTN)RegisterTable->RegisterTableEntry;\r
   RegisterTableEntry[RegisterTable->TableLength].RegisterType   = RegisterType;\r
-  RegisterTableEntry[RegisterTable->TableLength].Index          = (UINT32) Index;\r
-  RegisterTableEntry[RegisterTable->TableLength].HighIndex      = (UINT32) RShiftU64 (Index, 32);\r
+  RegisterTableEntry[RegisterTable->TableLength].Index          = (UINT32)Index;\r
+  RegisterTableEntry[RegisterTable->TableLength].HighIndex      = (UINT32)RShiftU64 (Index, 32);\r
   RegisterTableEntry[RegisterTable->TableLength].ValidBitStart  = ValidBitStart;\r
   RegisterTableEntry[RegisterTable->TableLength].ValidBitLength = ValidBitLength;\r
   RegisterTableEntry[RegisterTable->TableLength].Value          = Value;\r
+  RegisterTableEntry[RegisterTable->TableLength].TestThenWrite  = TestThenWrite;\r
 \r
   RegisterTable->TableLength++;\r
 }\r
@@ -1092,21 +1158,55 @@ CpuRegisterTableWriteWorker (
 VOID\r
 EFIAPI\r
 CpuRegisterTableWrite (\r
-  IN UINTN               ProcessorNumber,\r
-  IN REGISTER_TYPE       RegisterType,\r
-  IN UINT64              Index,\r
-  IN UINT64              ValueMask,\r
-  IN UINT64              Value\r
+  IN UINTN          ProcessorNumber,\r
+  IN REGISTER_TYPE  RegisterType,\r
+  IN UINT64         Index,\r
+  IN UINT64         ValueMask,\r
+  IN UINT64         Value\r
+  )\r
+{\r
+  UINT8  Start;\r
+  UINT8  End;\r
+  UINT8  Length;\r
+\r
+  Start  = (UINT8)LowBitSet64 (ValueMask);\r
+  End    = (UINT8)HighBitSet64 (ValueMask);\r
+  Length = End - Start + 1;\r
+  CpuRegisterTableWriteWorker (FALSE, ProcessorNumber, RegisterType, Index, Start, Length, Value, FALSE);\r
+}\r
+\r
+/**\r
+  Adds an entry in specified register table.\r
+\r
+  This function adds an entry in specified register table, with given register type,\r
+  register index, bit section and value.\r
+\r
+  @param[in]  ProcessorNumber  The index of the CPU to add a register table entry\r
+  @param[in]  RegisterType     Type of the register to program\r
+  @param[in]  Index            Index of the register to program\r
+  @param[in]  ValueMask        Mask of bits in register to write\r
+  @param[in]  Value            Value to write\r
+\r
+  @note This service could be called by BSP only.\r
+**/\r
+VOID\r
+EFIAPI\r
+CpuRegisterTableTestThenWrite (\r
+  IN UINTN          ProcessorNumber,\r
+  IN REGISTER_TYPE  RegisterType,\r
+  IN UINT64         Index,\r
+  IN UINT64         ValueMask,\r
+  IN UINT64         Value\r
   )\r
 {\r
-  UINT8                   Start;\r
-  UINT8                   End;\r
-  UINT8                   Length;\r
+  UINT8  Start;\r
+  UINT8  End;\r
+  UINT8  Length;\r
 \r
-  Start  = (UINT8)LowBitSet64  (ValueMask);\r
+  Start  = (UINT8)LowBitSet64 (ValueMask);\r
   End    = (UINT8)HighBitSet64 (ValueMask);\r
   Length = End - Start + 1;\r
-  CpuRegisterTableWriteWorker (FALSE, ProcessorNumber, RegisterType, Index, Start, Length, Value);\r
+  CpuRegisterTableWriteWorker (FALSE, ProcessorNumber, RegisterType, Index, Start, Length, Value, TRUE);\r
 }\r
 \r
 /**\r
@@ -1126,21 +1226,21 @@ CpuRegisterTableWrite (
 VOID\r
 EFIAPI\r
 PreSmmCpuRegisterTableWrite (\r
-  IN UINTN               ProcessorNumber,\r
-  IN REGISTER_TYPE       RegisterType,\r
-  IN UINT64              Index,\r
-  IN UINT64              ValueMask,\r
-  IN UINT64              Value\r
+  IN UINTN          ProcessorNumber,\r
+  IN REGISTER_TYPE  RegisterType,\r
+  IN UINT64         Index,\r
+  IN UINT64         ValueMask,\r
+  IN UINT64         Value\r
   )\r
 {\r
   UINT8  Start;\r
   UINT8  End;\r
   UINT8  Length;\r
 \r
-  Start  = (UINT8)LowBitSet64  (ValueMask);\r
+  Start  = (UINT8)LowBitSet64 (ValueMask);\r
   End    = (UINT8)HighBitSet64 (ValueMask);\r
   Length = End - Start + 1;\r
-  CpuRegisterTableWriteWorker (TRUE, ProcessorNumber, RegisterType, Index, Start, Length, Value);\r
+  CpuRegisterTableWriteWorker (TRUE, ProcessorNumber, RegisterType, Index, Start, Length, Value, FALSE);\r
 }\r
 \r
 /**\r
@@ -1156,14 +1256,15 @@ PreSmmCpuRegisterTableWrite (
 **/\r
 BOOLEAN\r
 IsCpuFeatureSetInCpuPcd (\r
-  IN UINT8               *CpuBitMask,\r
-  IN UINTN               CpuBitMaskSize,\r
-  IN UINT32              Feature\r
+  IN UINT8   *CpuBitMask,\r
+  IN UINTN   CpuBitMaskSize,\r
+  IN UINT32  Feature\r
   )\r
 {\r
   if ((Feature >> 3) >= CpuBitMaskSize) {\r
     return FALSE;\r
   }\r
+\r
   return ((*(CpuBitMask + (Feature >> 3)) & (1 << (Feature & 0x07))) != 0);\r
 }\r
 \r
@@ -1184,7 +1285,7 @@ IsCpuFeatureSetInCpuPcd (
 BOOLEAN\r
 EFIAPI\r
 IsCpuFeatureSupported (\r
-  IN UINT32              Feature\r
+  IN UINT32  Feature\r
   )\r
 {\r
   return IsCpuFeatureSetInCpuPcd (\r
@@ -1208,7 +1309,7 @@ IsCpuFeatureSupported (
 BOOLEAN\r
 EFIAPI\r
 IsCpuFeatureInSetting (\r
-  IN UINT32              Feature\r
+  IN UINT32  Feature\r
   )\r
 {\r
   return IsCpuFeatureSetInCpuPcd (\r
@@ -1228,12 +1329,11 @@ IsCpuFeatureInSetting (
 VOID\r
 EFIAPI\r
 SwitchBspAfterFeaturesInitialize (\r
-  IN UINTN               ProcessorNumber\r
+  IN UINTN  ProcessorNumber\r
   )\r
 {\r
-  CPU_FEATURES_DATA      *CpuFeaturesData;\r
+  CPU_FEATURES_DATA  *CpuFeaturesData;\r
 \r
-  CpuFeaturesData = GetCpuFeaturesData ();\r
+  CpuFeaturesData            = GetCpuFeaturesData ();\r
   CpuFeaturesData->BspNumber = ProcessorNumber;\r
 }\r
-\r