From 95cfe6c24714a11c894070a0bf2f2ba08f0ff68b Mon Sep 17 00:00:00 2001
From: "Dong, Eric" <eric.dong@intel.com>
Date: Fri, 16 Aug 2019 11:57:29 +0800
Subject: [PATCH] UefiCpuPkg/RegisterCpuFeaturesLib: Supports test then write
 new value logic.

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>
Reviewed-by: Ray Ni <ray.ni@intel.com>
Acked-by: 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 63bc50a55f..0a4fcff033 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)) {
         break;
       }
-
+      if (RegisterTableEntry->TestThenWrite) {
+        CurrentValue = BitFieldRead64 (
+                         Value,
+                         RegisterTableEntry->ValidBitStart,
+                         RegisterTableEntry->ValidBitStart + RegisterTableEntry->ValidBitLength - 1
+                         );
+        if (CurrentValue == RegisterTableEntry->Value) {
+          break;
+        }
+      }
       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) {
+            break;
+          }
+        } else {
+          CurrentValue = BitFieldRead64 (
+                           Value,
+                           RegisterTableEntry->ValidBitStart,
+                           RegisterTableEntry->ValidBitStart + RegisterTableEntry->ValidBitLength - 1
+                           );
+          if (CurrentValue == RegisterTableEntry->Value) {
+            break;
+          }
+        }
+      }
+
       if (RegisterTableEntry->ValidBitLength >= 64) {
         //
         // If length is not less than 64 bits, then directly write without reading
-- 
2.39.2