UefiCpuPkg/RegisterCpuFeaturesLib: Add "Test Then Write" Macros.

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

Add below new micros which test the current value before write the new
value. Only write new value when current value not same as new value.
  CPU_REGISTER_TABLE_TEST_THEN_WRITE32
  CPU_REGISTER_TABLE_TEST_THEN_WRITE64
  CPU_REGISTER_TABLE_TEST_THEN_WRITE_FIELD

Also add below API:
  CpuRegisterTableTestThenWrite

Signed-off-by: Eric Dong <eric.dong@intel.com>
Reviewed-by: Ray Ni <ray.ni@intel.com>
Acked-by: Laszlo Ersek <lersek@redhat.com>
Cc: Star Zeng <star.zeng@intel.com>

UefiCpuPkg/Include/AcpiCpuData.h

@@ -78,7 +78,8 @@ typedef struct {
   UINT32         Index;                 // offset 4 - 7
   UINT8          ValidBitStart;         // offset 8
   UINT8          ValidBitLength;        // offset 9
-  UINT16         Reserved;              // offset 10 - 11
+  BOOLEAN        TestThenWrite;         // offset 10
+  UINT8          Reserved1;             // offset 11
   UINT32         HighIndex;             // offset 12-15, only valid for MemoryMapped
   UINT64         Value;                 // offset 16-23
 } CPU_REGISTER_TABLE_ENTRY;

UefiCpuPkg/Include/Library/RegisterCpuFeaturesLib.h

@@ -348,6 +348,32 @@ CpuRegisterTableWrite (
   IN UINT64              Value
   );
 
+/**
+  Adds an entry in specified register table.
+
+  This function adds an entry in specified register table, with given register type,
+  register index, bit section and value.
+
+  Driver will  test the current value before setting new value.
+
+  @param[in]  ProcessorNumber  The index of the CPU to add a register table entry
+  @param[in]  RegisterType     Type of the register to program
+  @param[in]  Index            Index of the register to program
+  @param[in]  ValueMask        Mask of bits in register to write
+  @param[in]  Value            Value to write
+
+  @note This service could be called by BSP only.
+**/
+VOID
+EFIAPI
+CpuRegisterTableTestThenWrite (
+  IN UINTN               ProcessorNumber,
+  IN REGISTER_TYPE       RegisterType,
+  IN UINT64              Index,
+  IN UINT64              ValueMask,
+  IN UINT64              Value
+  );
+
 /**
   Adds an entry in specified Pre-SMM register table.
 
@@ -390,6 +416,26 @@ PreSmmCpuRegisterTableWrite (
     CpuRegisterTableWrite (ProcessorNumber, RegisterType, Index, MAX_UINT32, Value);  \
   } while(FALSE);
 
+/**
+  Adds a 32-bit register write entry in specified register table.
+
+  This macro adds an entry in specified register table, with given register type,
+  register index, and value.
+
+  Driver will  test the current value before setting new value.
+
+  @param[in]  ProcessorNumber  The index of the CPU to add a register table entry.
+  @param[in]  RegisterType     Type of the register to program
+  @param[in]  Index            Index of the register to program
+  @param[in]  Value            Value to write
+
+  @note This service could be called by BSP only.
+**/
+#define CPU_REGISTER_TABLE_TEST_THEN_WRITE32(ProcessorNumber, RegisterType, Index, Value)     \
+  do {                                                                                        \
+    CpuRegisterTableTestThenWrite (ProcessorNumber, RegisterType, Index, MAX_UINT32, Value);  \
+  } while(FALSE);
+
 /**
   Adds a 64-bit register write entry in specified register table.
 
@@ -408,6 +454,26 @@ PreSmmCpuRegisterTableWrite (
     CpuRegisterTableWrite (ProcessorNumber, RegisterType, Index, MAX_UINT64, Value);  \
   } while(FALSE);
 
+/**
+  Adds a 64-bit register write entry in specified register table.
+
+  This macro adds an entry in specified register table, with given register type,
+  register index, and value.
+
+  Driver will  test the current value before setting new value.
+
+  @param[in]  ProcessorNumber  The index of the CPU to add a register table entry.
+  @param[in]  RegisterType     Type of the register to program
+  @param[in]  Index            Index of the register to program
+  @param[in]  Value            Value to write
+
+  @note This service could be called by BSP only.
+**/
+#define CPU_REGISTER_TABLE_TEST_THEN_WRITE64(ProcessorNumber, RegisterType, Index, Value)     \
+  do {                                                                                        \
+    CpuRegisterTableTestThenWrite (ProcessorNumber, RegisterType, Index, MAX_UINT64, Value);  \
+  } while(FALSE);
+
 /**
   Adds a bit field write entry in specified register table.
 
@@ -431,6 +497,31 @@ PreSmmCpuRegisterTableWrite (
     CpuRegisterTableWrite (ProcessorNumber, RegisterType, Index, ~ValueMask, Value);             \
   } while(FALSE);
 
+/**
+  Adds a bit field write entry in specified register table.
+
+  This macro adds an entry in specified register table, with given register type,
+  register index, bit field section, and value.
+
+  Driver will  test the current value before setting new value.
+
+  @param[in]  ProcessorNumber  The index of the CPU to add a register table entry.
+  @param[in]  RegisterType     Type of the register to program.
+  @param[in]  Index            Index of the register to program.
+  @param[in]  Type             The data type name of a register structure.
+  @param[in]  Field            The bit fiel name in register structure to write.
+  @param[in]  Value            Value to write to the bit field.
+
+  @note This service could be called by BSP only.
+**/
+#define CPU_REGISTER_TABLE_TEST_THEN_WRITE_FIELD(ProcessorNumber, RegisterType, Index, Type, Field, Value) \
+  do {                                                                                                     \
+    UINT64  ValueMask;                                                                                     \
+    ValueMask = MAX_UINT64;                                                                                \
+    ((Type *)(&ValueMask))->Field = 0;                                                                     \
+    CpuRegisterTableTestThenWrite (ProcessorNumber, RegisterType, Index, ~ValueMask, Value);               \
+  } while(FALSE);
+
 /**
   Adds a 32-bit register write entry in specified register table.
 

UefiCpuPkg/Library/RegisterCpuFeaturesLib/RegisterCpuFeaturesLib.c

@@ -1025,6 +1025,8 @@ EnlargeRegisterTable (
   @param[in]  ValidBitStart    Start of the bit section
   @param[in]  ValidBitLength   Length of the bit section
   @param[in]  Value            Value to write
+  @param[in]  TestThenWrite    Whether need to test current Value before writing.
+
 **/
 VOID
 CpuRegisterTableWriteWorker (
@@ -1034,7 +1036,8 @@ CpuRegisterTableWriteWorker (
   IN UINT64                  Index,
   IN UINT8                   ValidBitStart,
   IN UINT8                   ValidBitLength,
-  IN UINT64                  Value
+  IN UINT64                  Value,
+  IN BOOLEAN                 TestThenWrite
   )
 {
   CPU_FEATURES_DATA        *CpuFeaturesData;
@@ -1070,6 +1073,7 @@ CpuRegisterTableWriteWorker (
   RegisterTableEntry[RegisterTable->TableLength].ValidBitStart  = ValidBitStart;
   RegisterTableEntry[RegisterTable->TableLength].ValidBitLength = ValidBitLength;
   RegisterTableEntry[RegisterTable->TableLength].Value          = Value;
+  RegisterTableEntry[RegisterTable->TableLength].TestThenWrite  = TestThenWrite;
 
   RegisterTable->TableLength++;
 }
@@ -1105,7 +1109,41 @@ CpuRegisterTableWrite (
   Start  = (UINT8)LowBitSet64  (ValueMask);
   End    = (UINT8)HighBitSet64 (ValueMask);
   Length = End - Start + 1;
-  CpuRegisterTableWriteWorker (FALSE, ProcessorNumber, RegisterType, Index, Start, Length, Value);
+  CpuRegisterTableWriteWorker (FALSE, ProcessorNumber, RegisterType, Index, Start, Length, Value, FALSE);
+}
+
+/**
+  Adds an entry in specified register table.
+
+  This function adds an entry in specified register table, with given register type,
+  register index, bit section and value.
+
+  @param[in]  ProcessorNumber  The index of the CPU to add a register table entry
+  @param[in]  RegisterType     Type of the register to program
+  @param[in]  Index            Index of the register to program
+  @param[in]  ValueMask        Mask of bits in register to write
+  @param[in]  Value            Value to write
+
+  @note This service could be called by BSP only.
+**/
+VOID
+EFIAPI
+CpuRegisterTableTestThenWrite (
+  IN UINTN               ProcessorNumber,
+  IN REGISTER_TYPE       RegisterType,
+  IN UINT64              Index,
+  IN UINT64              ValueMask,
+  IN UINT64              Value
+  )
+{
+  UINT8                   Start;
+  UINT8                   End;
+  UINT8                   Length;
+
+  Start  = (UINT8)LowBitSet64  (ValueMask);
+  End    = (UINT8)HighBitSet64 (ValueMask);
+  Length = End - Start + 1;
+  CpuRegisterTableWriteWorker (FALSE, ProcessorNumber, RegisterType, Index, Start, Length, Value, TRUE);
 }
 
 /**
@@ -1139,7 +1177,7 @@ PreSmmCpuRegisterTableWrite (
   Start  = (UINT8)LowBitSet64  (ValueMask);
   End    = (UINT8)HighBitSet64 (ValueMask);
   Length = End - Start + 1;
-  CpuRegisterTableWriteWorker (TRUE, ProcessorNumber, RegisterType, Index, Start, Length, Value);
+  CpuRegisterTableWriteWorker (TRUE, ProcessorNumber, RegisterType, Index, Start, Length, Value, FALSE);
 }
 
 /**