From: Dong, Eric <eric.dong@intel.com>
Date: Fri, 16 Aug 2019 03:57:27 +0000 (+0800)
Subject: UefiCpuPkg/PiSmmCpuDxeSmm: Supports test then write new value logic.
X-Git-Tag: edk2-stable201908~46
X-Git-Url: https://git.proxmox.com/?p=mirror_edk2.git;a=commitdiff_plain;h=cfbcaad25189c02627bf5c73ddeb25b91516f7d6

UefiCpuPkg/PiSmmCpuDxeSmm: 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>
Reviewed-by: Laszlo Ersek <lersek@redhat.com>
---

diff --git a/UefiCpuPkg/PiSmmCpuDxeSmm/CpuS3.c b/UefiCpuPkg/PiSmmCpuDxeSmm/CpuS3.c
index 627a3b87ac..ba5cc0194c 100644
--- a/UefiCpuPkg/PiSmmCpuDxeSmm/CpuS3.c
+++ b/UefiCpuPkg/PiSmmCpuDxeSmm/CpuS3.c
@@ -241,6 +241,7 @@ ProgramProcessorRegister (
   UINTN                     ValidThreadCount;
   UINT32                    *ValidCoreCountPerPackage;
   EFI_STATUS                Status;
+  UINT64                    CurrentValue;
 
   //
   // Traverse Register Table of this logical processor
@@ -263,6 +264,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,
@@ -275,6 +286,24 @@ ProgramProcessorRegister (
     // 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 this function is called to restore register setting after INIT signal,
       // there is no need to restore MSRs in register table.