[Patch 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>
Reviewed-by: Jeff Fan <jeff.fan@intel.com>
---
 .../RegisterCpuFeaturesLib/CpuFeaturesInitialize.c | 38 +++++++++++++++++++++-
 1 file changed, 37 insertions(+), 1 deletion(-)

diff --git a/UefiCpuPkg/Library/RegisterCpuFeaturesLib/CpuFeaturesInitialize.c b/UefiCpuPkg/Library/RegisterCpuFeaturesLib/CpuFeaturesInitialize.c
index 5e11b2b..ccdfcda 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.
 
@@ -500,7 +525,18 @@ AnalysisProcessorFeatures (
       } else {
         Status = CpuFeatureInOrder->InitializeFunc (ProcessorNumber, CpuInfo, CpuFeatureInOrder->ConfigData, FALSE);
       }
-      ASSERT_EFI_ERROR (Status);
+      if (EFI_ERROR (Status)) {
+        //
+        // Clean the CpuFeatureInOrder->FeatureMask in setting PCD.
+        //
+        SupportedMaskCleanBit (CpuFeaturesData->SettingPcds, CpuFeatureInOrder->FeatureMask);
+        if (CpuFeatureInOrder->FeatureName != NULL) {
+          DEBUG ((DEBUG_WARN, "Feature %a failed to initialize, status = %x!\n", CpuFeatureInOrder->FeatureName, Status));
+        } else {
+          DEBUG ((DEBUG_WARN, "Feature %x failed to initialize, status = %x!\n", CpuFeatureInOrder->FeatureMask, Status));
+        }
+      }
+
       Entry = Entry->ForwardLink;
     }
     //
-- 
2.7.0.windows.1