[Patch] UefiCpuPkg/RegisterCpuFeaturesLib: Adjust Order.

[Eric Dong <eric.dong@intel.com>]

In current code logic, only adjust feature position if current CPU
feature position not follow the request order. Just like Feature A
need to be executed before feature B, but current feature A registers
after feature B. So code will adjust the position for feature A, move
it to just before feature B. If the position already met the
requirement, code will not adjust the position.

This logic has issue when met all below cases:
1. feature A has core or package level dependence with feature B.
2. feature A is register before feature B.
3. Also exist other features exist between feature A and B.

Root cause is driver ignores the dependence for this case, so threads
may execute not follow the dependence order.

Fix this issue by change code logic to adjust feature position for
CPU features which has dependence relationship.

Change-Id: I86171cb1dbf44a2f6fd8d5d2209cafee9451b866
Cc: Laszlo Ersek <lersek@redhat.com>
Cc: Ruiyu Ni <ruiyu.ni@intel.com>
Contributed-under: TianoCore Contribution Agreement 1.1
Signed-off-by: Eric Dong <eric.dong@intel.com>
---
 .../RegisterCpuFeaturesLib.c                       | 62 ++++++++++++++++++++--
 1 file changed, 58 insertions(+), 4 deletions(-)

diff --git a/UefiCpuPkg/Library/RegisterCpuFeaturesLib/RegisterCpuFeaturesLib.c b/UefiCpuPkg/Library/RegisterCpuFeaturesLib/RegisterCpuFeaturesLib.c
index 394695baf2..31a44b6bad 100644
--- a/UefiCpuPkg/Library/RegisterCpuFeaturesLib/RegisterCpuFeaturesLib.c
+++ b/UefiCpuPkg/Library/RegisterCpuFeaturesLib/RegisterCpuFeaturesLib.c
@@ -271,6 +271,10 @@ AdjustEntry (
     PreviousEntry = GetNextNode (FeatureList, FindEntry);
   }
 
+  if (PreviousEntry == CurrentEntry) {
+    return;
+  }
+
   CurrentFeature  = CPU_FEATURE_ENTRY_FROM_LINK (CurrentEntry);
   RemoveEntryList (CurrentEntry);
 
@@ -389,6 +393,56 @@ InsertToAfterEntry (
   return Swapped;
 }
 
+/**
+  Checks and adjusts current CPU features per dependency relationship.
+
+  @param[in]  FeatureList        Pointer to CPU feature list
+  @param[in]  CurrentEntry       Pointer to current checked CPU feature
+  @param[in]  FeatureMask        The feature bit mask.
+  @param[in]  Before             The dependence is before type or after type.
+
+  @retval     return Swapped info.
+**/
+BOOLEAN
+AdjustToAllEntry (
+  IN LIST_ENTRY              *FeatureList,
+  IN LIST_ENTRY              *CurrentEntry,
+  IN UINT8                   *FeatureMask,
+  IN BOOLEAN                 Before
+  )
+{
+  LIST_ENTRY                 *CheckEntry;
+  CPU_FEATURES_ENTRY         *CheckFeature;
+  BOOLEAN                    Swapped;
+  LIST_ENTRY                 *NextEntry;
+
+  Swapped = FALSE;
+  //
+  // Record neighbor for later compre.
+  //
+  NextEntry = CurrentEntry->ForwardLink;
+  //
+  // Check all features in current list.
+  //
+  CheckEntry = GetFirstNode (FeatureList);
+  while (!IsNull (FeatureList, CheckEntry)) {
+    CheckFeature = CPU_FEATURE_ENTRY_FROM_LINK (CheckEntry);
+    if (IsBitMaskMatchCheck (CheckFeature->FeatureMask, FeatureMask)) {
+      AdjustEntry (FeatureList, CheckEntry, CurrentEntry, Before);
+      //
+      // Base on former record neighbor to detect whether current entry
+      // adjust the position.
+      // Current Entry position maybe changed in AdjustEntry function.
+      //
+      Swapped = (NextEntry != CurrentEntry->ForwardLink);
+      break;
+    }
+    CheckEntry = CheckEntry->ForwardLink;
+  }
+
+  return Swapped;
+}
+
 /**
   Checks and adjusts CPU features order per dependency relationship.
 
@@ -479,28 +533,28 @@ CheckCpuFeaturesDependency (
     }
 
     if (CpuFeature->CoreBeforeFeatureBitMask != NULL) {
-      Swapped = InsertToBeforeEntry (FeatureList, CurrentEntry, CpuFeature->CoreBeforeFeatureBitMask);
+      Swapped = AdjustToAllEntry (FeatureList, CurrentEntry, CpuFeature->CoreBeforeFeatureBitMask, TRUE);
       if (Swapped) {
         continue;
       }
     }
 
     if (CpuFeature->CoreAfterFeatureBitMask != NULL) {
-      Swapped = InsertToAfterEntry (FeatureList, CurrentEntry, CpuFeature->CoreAfterFeatureBitMask);
+      Swapped = AdjustToAllEntry (FeatureList, CurrentEntry, CpuFeature->CoreAfterFeatureBitMask, FALSE);
       if (Swapped) {
         continue;
       }
     }
 
     if (CpuFeature->PackageBeforeFeatureBitMask != NULL) {
-      Swapped = InsertToBeforeEntry (FeatureList, CurrentEntry, CpuFeature->PackageBeforeFeatureBitMask);
+      Swapped = AdjustToAllEntry (FeatureList, CurrentEntry, CpuFeature->PackageBeforeFeatureBitMask, TRUE);
       if (Swapped) {
         continue;
       }
     }
 
     if (CpuFeature->PackageAfterFeatureBitMask != NULL) {
-      Swapped = InsertToAfterEntry (FeatureList, CurrentEntry, CpuFeature->PackageAfterFeatureBitMask);
+      Swapped = AdjustToAllEntry (FeatureList, CurrentEntry, CpuFeature->PackageAfterFeatureBitMask, FALSE);
       if (Swapped) {
         continue;
       }
-- 
2.15.0.windows.1