From 05973f9e8aa6f50ff177610bc791a16540963a9a Mon Sep 17 00:00:00 2001
From: Eric Dong <eric.dong@intel.com>
Date: Tue, 11 Jul 2017 10:06:56 +0800
Subject: [PATCH 1/1] UefiCpuPkg RegisterCpuFeaturesLib: Add error handling
 code.

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>
Reviewed-by: Jeff Fan <jeff.fan@intel.com>
---
 .../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 5e11b2b21c..e91a4388b4 100644
--- a/UefiCpuPkg/Library/RegisterCpuFeaturesLib/CpuFeaturesInitialize.c
+++ b/UefiCpuPkg/Library/RegisterCpuFeaturesLib/CpuFeaturesInitialize.c
@@ -234,6 +234,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.39.2