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 627a3b87ac517a76bf8e630a83658dc30ba565b8..ba5cc0194c2d2121e1efdc1a3cb0a424e3349e1e 100644 (file)
--- a/UefiCpuPkg/PiSmmCpuDxeSmm/CpuS3.c
+++ b/UefiCpuPkg/PiSmmCpuDxeSmm/CpuS3.c
@@ -241,6 +241,7 @@ ProgramProcessorRegister (
   UINTN                     ValidThreadCount;\r
   UINT32                    *ValidCoreCountPerPackage;\r
   EFI_STATUS                Status;\r
+  UINT64                    CurrentValue;\r
 \r
   //\r
   // Traverse Register Table of this logical processor\r
@@ -263,6 +264,16 @@ ProgramProcessorRegister (
       if (EFI_ERROR (Status)) {\r
         break;\r
       }\r
+      if (RegisterTableEntry->TestThenWrite) {\r
+        CurrentValue = BitFieldRead64 (\r
+                         Value,\r
+                         RegisterTableEntry->ValidBitStart,\r
+                         RegisterTableEntry->ValidBitStart + RegisterTableEntry->ValidBitLength - 1\r
+                         );\r
+        if (CurrentValue == RegisterTableEntry->Value) {\r
+          break;\r
+        }\r
+      }\r
       Value = (UINTN) BitFieldWrite64 (\r
                         Value,\r
                         RegisterTableEntry->ValidBitStart,\r
@@ -275,6 +286,24 @@ ProgramProcessorRegister (
     // The specified register is Model Specific Register\r
     //\r
     case Msr:\r
+      if (RegisterTableEntry->TestThenWrite) {\r
+        Value = (UINTN)AsmReadMsr64 (RegisterTableEntry->Index);\r
+        if (RegisterTableEntry->ValidBitLength >= 64) {\r
+          if (Value == RegisterTableEntry->Value) {\r
+            break;\r
+          }\r
+        } else {\r
+          CurrentValue = BitFieldRead64 (\r
+                           Value,\r
+                           RegisterTableEntry->ValidBitStart,\r
+                           RegisterTableEntry->ValidBitStart + RegisterTableEntry->ValidBitLength - 1\r
+                           );\r
+          if (CurrentValue == RegisterTableEntry->Value) {\r
+            break;\r
+          }\r
+        }\r
+      }\r
+\r
       //\r
       // If this function is called to restore register setting after INIT signal,\r
       // there is no need to restore MSRs in register table.\r