[Patch 4/4] UefiCpuPkg/CpuCommonFeaturesLib: Use "Test then set" action.

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

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

Below code is current implementation:
  if (MsrRegister[ProcessorNumber].Bits.Lock == 0) {
    CPU_REGISTER_TABLE_WRITE_FIELD (
      ProcessorNumber,
      Msr,
      MSR_IA32_FEATURE_CONTROL,
      MSR_IA32_FEATURE_CONTROL_REGISTER,
      Bits.Lock,
      1
    );
  }

With below steps, the Bits.Lock bit will lose its value:
1. In first normal boot, the Bits.Lock is 0. 1 will be added
   into the register table and then will set to the MSR.
2. Trig warm reboot, MSR value preserves. After normal boot phase,
   the Bits.Lock is 1, so it will not be added into the register
   table during the warm reboot phase.
3. Trig S3 then resume, the Bits.Lock change to 0 and Bits.Lock is
   not added in register table during normal boot phase. so it's
   still 0 after resume. 
This is not an expect behavior. The expect result is the value should
always 1 after booting or resuming from S3.

The root cause for this issue is
1. driver bases on current value to insert the "set value action" to
   the register table.
2. Some MSRs may reserve their value during warm reboot. So the insert
   action may be skip after warm reboot.

The solution for this issue is:
1. Always add "Test then Set" action for above referred MSRs.
2. Detect current value before set new value. Only set 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>
---
 .../CpuCommonFeaturesLib/CpuCommonFeatures.h  |  15 --
 .../CpuCommonFeaturesLib.c                    |   8 +-
 .../CpuCommonFeaturesLib/FeatureControl.c     | 141 ++++++------------
 .../CpuCommonFeaturesLib/MachineCheck.c       |  23 ++-
 4 files changed, 58 insertions(+), 129 deletions(-)

diff --git a/UefiCpuPkg/Library/CpuCommonFeaturesLib/CpuCommonFeatures.h b/UefiCpuPkg/Library/CpuCommonFeaturesLib/CpuCommonFeatures.h
index 25d0174727..b2390e6c39 100644
--- a/UefiCpuPkg/Library/CpuCommonFeaturesLib/CpuCommonFeatures.h
+++ b/UefiCpuPkg/Library/CpuCommonFeaturesLib/CpuCommonFeatures.h
@@ -848,21 +848,6 @@ X2ApicInitialize (
   IN BOOLEAN                           State
   );
 
-/**
-  Prepares for the data used by CPU feature detection and initialization.
-
-  @param[in]  NumberOfProcessors  The number of CPUs in the platform.
-
-  @return  Pointer to a buffer of CPU related configuration data.
-
-  @note This service could be called by BSP only.
-**/
-VOID *
-EFIAPI
-FeatureControlGetConfigData (
-  IN UINTN               NumberOfProcessors
-  );
-
 /**
   Prepares for the data used by CPU feature detection and initialization.
 
diff --git a/UefiCpuPkg/Library/CpuCommonFeaturesLib/CpuCommonFeaturesLib.c b/UefiCpuPkg/Library/CpuCommonFeaturesLib/CpuCommonFeaturesLib.c
index fd43b8d662..f0dd3a3b43 100644
--- a/UefiCpuPkg/Library/CpuCommonFeaturesLib/CpuCommonFeaturesLib.c
+++ b/UefiCpuPkg/Library/CpuCommonFeaturesLib/CpuCommonFeaturesLib.c
@@ -91,7 +91,7 @@ CpuCommonFeaturesLibConstructor (
   if (IsCpuFeatureSupported (CPU_FEATURE_LOCK_FEATURE_CONTROL_REGISTER)) {
     Status = RegisterCpuFeature (
                "Lock Feature Control Register",
-               FeatureControlGetConfigData,
+               NULL,
                LockFeatureControlRegisterSupport,
                LockFeatureControlRegisterInitialize,
                CPU_FEATURE_LOCK_FEATURE_CONTROL_REGISTER,
@@ -102,7 +102,7 @@ CpuCommonFeaturesLibConstructor (
   if (IsCpuFeatureSupported (CPU_FEATURE_SMX)) {
     Status = RegisterCpuFeature (
                "SMX",
-               FeatureControlGetConfigData,
+               NULL,
                SmxSupport,
                SmxInitialize,
                CPU_FEATURE_SMX,
@@ -114,7 +114,7 @@ CpuCommonFeaturesLibConstructor (
   if (IsCpuFeatureSupported (CPU_FEATURE_VMX)) {
     Status = RegisterCpuFeature (
                "VMX",
-               FeatureControlGetConfigData,
+               NULL,
                VmxSupport,
                VmxInitialize,
                CPU_FEATURE_VMX,
@@ -214,7 +214,7 @@ CpuCommonFeaturesLibConstructor (
   if (IsCpuFeatureSupported (CPU_FEATURE_LMCE)) {
     Status = RegisterCpuFeature (
                "LMCE",
-               FeatureControlGetConfigData,
+               NULL,
                LmceSupport,
                LmceInitialize,
                CPU_FEATURE_LMCE,
diff --git a/UefiCpuPkg/Library/CpuCommonFeaturesLib/FeatureControl.c b/UefiCpuPkg/Library/CpuCommonFeaturesLib/FeatureControl.c
index 3712ef1e5c..6679df8ba4 100644
--- a/UefiCpuPkg/Library/CpuCommonFeaturesLib/FeatureControl.c
+++ b/UefiCpuPkg/Library/CpuCommonFeaturesLib/FeatureControl.c
@@ -8,28 +8,6 @@
 
 #include "CpuCommonFeatures.h"
 
-/**
-  Prepares for the data used by CPU feature detection and initialization.
-
-  @param[in]  NumberOfProcessors  The number of CPUs in the platform.
-
-  @return  Pointer to a buffer of CPU related configuration data.
-
-  @note This service could be called by BSP only.
-**/
-VOID *
-EFIAPI
-FeatureControlGetConfigData (
-  IN UINTN               NumberOfProcessors
-  )
-{
-  VOID          *ConfigData;
-
-  ConfigData = AllocateZeroPool (sizeof (MSR_IA32_FEATURE_CONTROL_REGISTER) * NumberOfProcessors);
-  ASSERT (ConfigData != NULL);
-  return ConfigData;
-}
-
 /**
   Detects if VMX feature supported on current processor.
 
@@ -54,11 +32,6 @@ VmxSupport (
   IN VOID                              *ConfigData  OPTIONAL
   )
 {
-  MSR_IA32_FEATURE_CONTROL_REGISTER    *MsrRegister;
-
-  ASSERT (ConfigData != NULL);
-  MsrRegister = (MSR_IA32_FEATURE_CONTROL_REGISTER *) ConfigData;
-  MsrRegister[ProcessorNumber].Uint64 = AsmReadMsr64 (MSR_IA32_FEATURE_CONTROL);
   return (CpuInfo->CpuIdVersionInfoEcx.Bits.VMX == 1);
 }
 
@@ -88,8 +61,6 @@ VmxInitialize (
   IN BOOLEAN                           State
   )
 {
-  MSR_IA32_FEATURE_CONTROL_REGISTER    *MsrRegister;
-
   //
   // The scope of EnableVmxOutsideSmx bit in the MSR_IA32_FEATURE_CONTROL is core for
   // below processor type, only program MSR_IA32_FEATURE_CONTROL for thread 0 in each
@@ -103,18 +74,15 @@ VmxInitialize (
     }
   }
 
-  ASSERT (ConfigData != NULL);
-  MsrRegister = (MSR_IA32_FEATURE_CONTROL_REGISTER *) ConfigData;
-  if (MsrRegister[ProcessorNumber].Bits.Lock == 0) {
-    CPU_REGISTER_TABLE_WRITE_FIELD (
-      ProcessorNumber,
-      Msr,
-      MSR_IA32_FEATURE_CONTROL,
-      MSR_IA32_FEATURE_CONTROL_REGISTER,
-      Bits.EnableVmxOutsideSmx,
-      (State) ? 1 : 0
-      );
-  }
+  CPU_REGISTER_TABLE_TEST_THEN_WRITE_FIELD (
+    ProcessorNumber,
+    Msr,
+    MSR_IA32_FEATURE_CONTROL,
+    MSR_IA32_FEATURE_CONTROL_REGISTER,
+    Bits.EnableVmxOutsideSmx,
+    (State) ? 1 : 0
+    );
+
   return RETURN_SUCCESS;
 }
 
@@ -142,11 +110,6 @@ LockFeatureControlRegisterSupport (
   IN VOID                              *ConfigData  OPTIONAL
   )
 {
-  MSR_IA32_FEATURE_CONTROL_REGISTER    *MsrRegister;
-
-  ASSERT (ConfigData != NULL);
-  MsrRegister = (MSR_IA32_FEATURE_CONTROL_REGISTER *) ConfigData;
-  MsrRegister[ProcessorNumber].Uint64 = AsmReadMsr64 (MSR_IA32_FEATURE_CONTROL);
   return TRUE;
 }
 
@@ -176,8 +139,6 @@ LockFeatureControlRegisterInitialize (
   IN BOOLEAN                           State
   )
 {
-  MSR_IA32_FEATURE_CONTROL_REGISTER    *MsrRegister;
-
   //
   // The scope of Lock bit in the MSR_IA32_FEATURE_CONTROL is core for
   // below processor type, only program MSR_IA32_FEATURE_CONTROL for thread 0 in each
@@ -191,18 +152,15 @@ LockFeatureControlRegisterInitialize (
     }
   }
 
-  ASSERT (ConfigData != NULL);
-  MsrRegister = (MSR_IA32_FEATURE_CONTROL_REGISTER *) ConfigData;
-  if (MsrRegister[ProcessorNumber].Bits.Lock == 0) {
-    CPU_REGISTER_TABLE_WRITE_FIELD (
-      ProcessorNumber,
-      Msr,
-      MSR_IA32_FEATURE_CONTROL,
-      MSR_IA32_FEATURE_CONTROL_REGISTER,
-      Bits.Lock,
-      1
-      );
-  }
+  CPU_REGISTER_TABLE_TEST_THEN_WRITE_FIELD (
+    ProcessorNumber,
+    Msr,
+    MSR_IA32_FEATURE_CONTROL,
+    MSR_IA32_FEATURE_CONTROL_REGISTER,
+    Bits.Lock,
+    1
+    );
+
   return RETURN_SUCCESS;
 }
 
@@ -230,11 +188,6 @@ SmxSupport (
   IN VOID                              *ConfigData  OPTIONAL
   )
 {
-  MSR_IA32_FEATURE_CONTROL_REGISTER    *MsrRegister;
-
-  ASSERT (ConfigData != NULL);
-  MsrRegister = (MSR_IA32_FEATURE_CONTROL_REGISTER *) ConfigData;
-  MsrRegister[ProcessorNumber].Uint64 = AsmReadMsr64 (MSR_IA32_FEATURE_CONTROL);
   return (CpuInfo->CpuIdVersionInfoEcx.Bits.SMX == 1);
 }
 
@@ -265,7 +218,6 @@ SmxInitialize (
   IN BOOLEAN                           State
   )
 {
-  MSR_IA32_FEATURE_CONTROL_REGISTER    *MsrRegister;
   RETURN_STATUS                        Status;
 
   //
@@ -288,35 +240,32 @@ SmxInitialize (
     Status = RETURN_UNSUPPORTED;
   }
 
-  ASSERT (ConfigData != NULL);
-  MsrRegister = (MSR_IA32_FEATURE_CONTROL_REGISTER *) ConfigData;
-  if (MsrRegister[ProcessorNumber].Bits.Lock == 0) {
-    CPU_REGISTER_TABLE_WRITE_FIELD (
-      ProcessorNumber,
-      Msr,
-      MSR_IA32_FEATURE_CONTROL,
-      MSR_IA32_FEATURE_CONTROL_REGISTER,
-      Bits.SenterLocalFunctionEnables,
-      (State) ? 0x7F : 0
-      );
-
-    CPU_REGISTER_TABLE_WRITE_FIELD (
-      ProcessorNumber,
-      Msr,
-      MSR_IA32_FEATURE_CONTROL,
-      MSR_IA32_FEATURE_CONTROL_REGISTER,
-      Bits.SenterGlobalEnable,
-      (State) ? 1 : 0
-      );
-
-    CPU_REGISTER_TABLE_WRITE_FIELD (
-      ProcessorNumber,
-      Msr,
-      MSR_IA32_FEATURE_CONTROL,
-      MSR_IA32_FEATURE_CONTROL_REGISTER,
-      Bits.EnableVmxInsideSmx,
-      (State) ? 1 : 0
-      );
-  }
+  CPU_REGISTER_TABLE_TEST_THEN_WRITE_FIELD (
+    ProcessorNumber,
+    Msr,
+    MSR_IA32_FEATURE_CONTROL,
+    MSR_IA32_FEATURE_CONTROL_REGISTER,
+    Bits.SenterLocalFunctionEnables,
+    (State) ? 0x7F : 0
+    );
+
+  CPU_REGISTER_TABLE_TEST_THEN_WRITE_FIELD (
+    ProcessorNumber,
+    Msr,
+    MSR_IA32_FEATURE_CONTROL,
+    MSR_IA32_FEATURE_CONTROL_REGISTER,
+    Bits.SenterGlobalEnable,
+    (State) ? 1 : 0
+    );
+
+  CPU_REGISTER_TABLE_TEST_THEN_WRITE_FIELD (
+    ProcessorNumber,
+    Msr,
+    MSR_IA32_FEATURE_CONTROL,
+    MSR_IA32_FEATURE_CONTROL_REGISTER,
+    Bits.EnableVmxInsideSmx,
+    (State) ? 1 : 0
+    );
+
   return Status;
 }
diff --git a/UefiCpuPkg/Library/CpuCommonFeaturesLib/MachineCheck.c b/UefiCpuPkg/Library/CpuCommonFeaturesLib/MachineCheck.c
index 2528e0044e..01fd6bb54d 100644
--- a/UefiCpuPkg/Library/CpuCommonFeaturesLib/MachineCheck.c
+++ b/UefiCpuPkg/Library/CpuCommonFeaturesLib/MachineCheck.c
@@ -319,8 +319,6 @@ LmceInitialize (
   IN BOOLEAN                           State
   )
 {
-  MSR_IA32_FEATURE_CONTROL_REGISTER    *MsrRegister;
-
   //
   // The scope of LcmeOn bit in the MSR_IA32_MISC_ENABLE is core for below processor type, only program
   // MSR_IA32_MISC_ENABLE for thread 0 in each core.
@@ -333,17 +331,14 @@ LmceInitialize (
     }
   }
 
-  ASSERT (ConfigData != NULL);
-  MsrRegister = (MSR_IA32_FEATURE_CONTROL_REGISTER *) ConfigData;
-  if (MsrRegister[ProcessorNumber].Bits.Lock == 0) {
-    CPU_REGISTER_TABLE_WRITE_FIELD (
-      ProcessorNumber,
-      Msr,
-      MSR_IA32_FEATURE_CONTROL,
-      MSR_IA32_FEATURE_CONTROL_REGISTER,
-      Bits.LmceOn,
-      (State) ? 1 : 0
-      );
-  }
+  CPU_REGISTER_TABLE_TEST_THEN_WRITE_FIELD (
+    ProcessorNumber,
+    Msr,
+    MSR_IA32_FEATURE_CONTROL,
+    MSR_IA32_FEATURE_CONTROL_REGISTER,
+    Bits.LmceOn,
+    (State) ? 1 : 0
+    );
+
   return RETURN_SUCCESS;
 }
-- 
2.21.0.windows.1