[Patch v2 5/6] UefiCpuPkg/RegisterCpuFeaturesLib: Supports test then write new value logic.

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

REF: https://bugzilla.tianocore.org/show_bug.cgi?id=2040

Supports new logic which test current value before write new value.
Only write new value when current value not same as new value.

Signed-off-by: Eric Dong <eric.dong@intel.com>
Cc: Ray Ni <ray.ni@intel.com>
Cc: Laszlo Ersek <lersek@redhat.com>
---
 .../CpuFeaturesInitialize.c                   | 31 ++++++++++++++++++-
 1 file changed, 30 insertions(+), 1 deletion(-)

diff --git a/UefiCpuPkg/Library/RegisterCpuFeaturesLib/CpuFeaturesInitialize.c b/UefiCpuPkg/Library/RegisterCpuFeaturesLib/CpuFeaturesInitialize.c
index ef7452e2b8..6988a75bfe 100644
--- a/UefiCpuPkg/Library/RegisterCpuFeaturesLib/CpuFeaturesInitialize.c
+++ b/UefiCpuPkg/Library/RegisterCpuFeaturesLib/CpuFeaturesInitialize.c
@@ -826,6 +826,7 @@ ProgramProcessorRegister (
   UINTN                     ValidThreadCount;
   UINT32                    *ValidCoreCountPerPackage;
   EFI_STATUS                Status;
+  UINT64                    CurrentValue;
 
   //
   // Traverse Register Table of this logical processor
@@ -848,7 +849,16 @@ ProgramProcessorRegister (
       if (EFI_ERROR (Status)) {
         continue;
       }
-
+      if (RegisterTableEntry->TestThenWrite) {
+        CurrentValue = BitFieldRead64 (
+                         Value,
+                         RegisterTableEntry->ValidBitStart,
+                         RegisterTableEntry->ValidBitStart + RegisterTableEntry->ValidBitLength - 1
+                         );
+        if (CurrentValue == RegisterTableEntry->Value) {
+          continue;
+        }
+      }
       Value = (UINTN) BitFieldWrite64 (
                         Value,
                         RegisterTableEntry->ValidBitStart,
@@ -857,10 +867,29 @@ ProgramProcessorRegister (
                         );
       ReadWriteCr (RegisterTableEntry->Index, FALSE, &Value);
       break;
+
     //
     // The specified register is Model Specific Register
     //
     case Msr:
+      if (RegisterTableEntry->TestThenWrite) {
+        Value = (UINTN)AsmReadMsr64 (RegisterTableEntry->Index);
+        if (RegisterTableEntry->ValidBitLength >= 64) {
+          if (Value == RegisterTableEntry->Value) {
+            continue;
+          }
+        } else {
+          CurrentValue = BitFieldRead64 (
+                           Value,
+                           RegisterTableEntry->ValidBitStart,
+                           RegisterTableEntry->ValidBitStart + RegisterTableEntry->ValidBitLength - 1
+                           );
+          if (CurrentValue == RegisterTableEntry->Value) {
+            continue;
+          }
+        }
+      }
+
       if (RegisterTableEntry->ValidBitLength >= 64) {
         //
         // If length is not less than 64 bits, then directly write without reading
-- 
2.21.0.windows.1