[Patch V2 1/2] UefiCpuPkg RegisterCpuFeaturesLib: Add error handling code.

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

Add error handling code when initialize the CPU feature failed.

Contributed-under: TianoCore Contribution Agreement 1.0
Signed-off-by: Eric Dong <eric.dong@intel.com>
Cc: Jeff Fan <jeff.fan@intel.com>
---
 .../RegisterCpuFeaturesLib/CpuFeaturesInitialize.c | 47 +++++++++++++++++++++-
 1 file changed, 46 insertions(+), 1 deletion(-)

diff --git a/UefiCpuPkg/Library/RegisterCpuFeaturesLib/CpuFeaturesInitialize.c b/UefiCpuPkg/Library/RegisterCpuFeaturesLib/CpuFeaturesInitialize.c
index 5e11b2b..e91a438 100644
--- a/UefiCpuPkg/Library/RegisterCpuFeaturesLib/CpuFeaturesInitialize.c
+++ b/UefiCpuPkg/Library/RegisterCpuFeaturesLib/CpuFeaturesInitialize.c
@@ -235,6 +235,31 @@ SupportedMaskAnd (
 }
 
 /**
+  Worker function to clean bit operation on CPU feature supported bits mask buffer.
+
+  @param[in]  SupportedFeatureMask  The pointer to CPU feature bits mask buffer
+  @param[in]  AndFeatureBitMask     The feature bit mask to do XOR operation
+**/
+VOID
+SupportedMaskCleanBit (
+  IN UINT8               *SupportedFeatureMask,
+  IN UINT8               *AndFeatureBitMask
+  )
+{
+  UINTN                  Index;
+  UINTN                  BitMaskSize;
+  UINT8                  *Data1;
+  UINT8                  *Data2;
+
+  BitMaskSize = PcdGetSize (PcdCpuFeaturesSupport);
+  Data1 = SupportedFeatureMask;
+  Data2 = AndFeatureBitMask;
+  for (Index = 0; Index < BitMaskSize; Index++) {
+    *(Data1++) &=  ~(*(Data2++));
+  }
+}
+
+/**
   Worker function to check if the compared CPU feature set in the CPU feature
   supported bits mask buffer.
 
@@ -497,12 +522,32 @@ AnalysisProcessorFeatures (
       CpuFeatureInOrder = CPU_FEATURE_ENTRY_FROM_LINK (Entry);
       if (IsBitMaskMatch (CpuFeatureInOrder->FeatureMask, CpuFeaturesData->SettingPcds)) {
         Status = CpuFeatureInOrder->InitializeFunc (ProcessorNumber, CpuInfo, CpuFeatureInOrder->ConfigData, TRUE);
+        if (EFI_ERROR (Status)) {
+          //
+          // Clean the CpuFeatureInOrder->FeatureMask in setting PCD.
+          //
+          SupportedMaskCleanBit (CpuFeaturesData->SettingPcds, CpuFeatureInOrder->FeatureMask);
+          if (CpuFeatureInOrder->FeatureName != NULL) {
+            DEBUG ((DEBUG_WARN, "Warning :: Failed to enable Feature Name = %a.\n", CpuFeatureInOrder->FeatureName));
+          } else {
+            DEBUG ((DEBUG_WARN, "Warning :: Failed to enable Feature Mask = "));
+            DumpCpuFeatureMask (CpuFeatureInOrder->FeatureMask);
+          }
+        }
       } else {
         Status = CpuFeatureInOrder->InitializeFunc (ProcessorNumber, CpuInfo, CpuFeatureInOrder->ConfigData, FALSE);
+        ASSERT_EFI_ERROR (Status);
       }
-      ASSERT_EFI_ERROR (Status);
       Entry = Entry->ForwardLink;
     }
+
+    //
+    // Dump PcdCpuFeaturesSetting again because this value maybe updated
+    // again during initialize the features.
+    //
+    DEBUG ((DEBUG_INFO, "Dump final value for PcdCpuFeaturesSetting:\n"));
+    DumpCpuFeatureMask (CpuFeaturesData->SettingPcds);
+
     //
     // Dump the RegisterTable
     //
-- 
2.7.0.windows.1