UefiCpuPkg/RegisterCpuFeaturesLib: Supports test then write new value logic.

[mirror_edk2.git] / UefiCpuPkg / Library / RegisterCpuFeaturesLib / CpuFeaturesInitialize.c

diff --git a/UefiCpuPkg/Library/RegisterCpuFeaturesLib/CpuFeaturesInitialize.c b/UefiCpuPkg/Library/RegisterCpuFeaturesLib/CpuFeaturesInitialize.c
index 63bc50a55fe979ff06dc15f04411d80eda92653f..0a4fcff033a3cb30641cb061321b3c776ba512c1 100644 (file)
--- a/UefiCpuPkg/Library/RegisterCpuFeaturesLib/CpuFeaturesInitialize.c
+++ b/UefiCpuPkg/Library/RegisterCpuFeaturesLib/CpuFeaturesInitialize.c
@@ -826,6 +826,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
@@ -848,7 +849,16 @@ ProgramProcessorRegister (
       if (EFI_ERROR (Status)) {\r
         break;\r
       }\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
@@ -857,10 +867,29 @@ ProgramProcessorRegister (
                         );\r
       ReadWriteCr (RegisterTableEntry->Index, FALSE, &Value);\r
       break;\r
+\r
     //\r
     // 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
       if (RegisterTableEntry->ValidBitLength >= 64) {\r
         //\r
         // If length is not less than 64 bits, then directly write without reading\r