[Patch v4 14/46] UefiCpuPkg/MpInitLib: Add CPU_VOLATILE_REGISTERS & worker functions

[Jeff Fan <jeff.fan@intel.com>]

Add CPU_VOLATILE_REGISTERS definitions for CRx and DRx required to be restored
after APs received INIT IPI.

Add worker functions SaveVolatileRegisters()/RestoreVolatileRegisters() used to
save/restore CRx and DRx. It also check if Debugging Extensions supported or
not.

Cc: Michael Kinney <michael.d.kinney@intel.com>
Cc: Feng Tian <feng.tian@intel.com>
Cc: Giri P Mudusuru <giri.p.mudusuru@intel.com>
Cc: Laszlo Ersek <lersek@redhat.com>
Contributed-under: TianoCore Contribution Agreement 1.0
Signed-off-by: Jeff Fan <jeff.fan@intel.com>
---
 UefiCpuPkg/Library/MpInitLib/MpLib.c | 71 ++++++++++++++++++++++++++++++++++++
 UefiCpuPkg/Library/MpInitLib/MpLib.h | 13 +++++++
 2 files changed, 84 insertions(+)

diff --git a/UefiCpuPkg/Library/MpInitLib/MpLib.c b/UefiCpuPkg/Library/MpInitLib/MpLib.c
index 70e5eb1..0832228 100644
--- a/UefiCpuPkg/Library/MpInitLib/MpLib.c
+++ b/UefiCpuPkg/Library/MpInitLib/MpLib.c
@@ -47,6 +47,73 @@ SetApState (
 }
 
 /**
+  Save the volatile registers required to be restored following INIT IPI.
+
+  @param[out]  VolatileRegisters    Returns buffer saved the volatile resisters
+**/
+VOID
+SaveVolatileRegisters (
+  OUT CPU_VOLATILE_REGISTERS    *VolatileRegisters
+  )
+{
+  CPUID_VERSION_INFO_EDX        VersionInfoEdx;
+
+  VolatileRegisters->Cr0 = AsmReadCr0 ();
+  VolatileRegisters->Cr3 = AsmReadCr3 ();
+  VolatileRegisters->Cr4 = AsmReadCr4 ();
+
+  AsmCpuid (CPUID_VERSION_INFO, NULL, NULL, NULL, &VersionInfoEdx.Uint32);
+  if (VersionInfoEdx.Bits.DE != 0) {
+    //
+    // If processor supports Debugging Extensions feature
+    // by CPUID.[EAX=01H]:EDX.BIT2
+    //
+    VolatileRegisters->Dr0 = AsmReadDr0 ();
+    VolatileRegisters->Dr1 = AsmReadDr1 ();
+    VolatileRegisters->Dr2 = AsmReadDr2 ();
+    VolatileRegisters->Dr3 = AsmReadDr3 ();
+    VolatileRegisters->Dr6 = AsmReadDr6 ();
+    VolatileRegisters->Dr7 = AsmReadDr7 ();
+  }
+}
+
+/**
+  Restore the volatile registers following INIT IPI.
+
+  @param[in]  VolatileRegisters   Pointer to volatile resisters
+  @param[in]  IsRestoreDr         TRUE:  Restore DRx if supported
+                                  FALSE: Do not restore DRx
+**/
+VOID
+RestoreVolatileRegisters (
+  IN CPU_VOLATILE_REGISTERS    *VolatileRegisters,
+  IN BOOLEAN                   IsRestoreDr
+  )
+{
+  CPUID_VERSION_INFO_EDX        VersionInfoEdx;
+
+  AsmWriteCr0 (VolatileRegisters->Cr0);
+  AsmWriteCr3 (VolatileRegisters->Cr3);
+  AsmWriteCr4 (VolatileRegisters->Cr4);
+
+  if (IsRestoreDr) {
+    AsmCpuid (CPUID_VERSION_INFO, NULL, NULL, NULL, &VersionInfoEdx.Uint32);
+    if (VersionInfoEdx.Bits.DE != 0) {
+      //
+      // If processor supports Debugging Extensions feature
+      // by CPUID.[EAX=01H]:EDX.BIT2
+      //
+      AsmWriteDr0 (VolatileRegisters->Dr0);
+      AsmWriteDr1 (VolatileRegisters->Dr1);
+      AsmWriteDr2 (VolatileRegisters->Dr2);
+      AsmWriteDr3 (VolatileRegisters->Dr3);
+      AsmWriteDr6 (VolatileRegisters->Dr6);
+      AsmWriteDr7 (VolatileRegisters->Dr7);
+    }
+  }
+}
+
+/**
   Detect whether Mwait-monitor feature is supported.
 
   @retval TRUE    Mwait-monitor feature is supported.
@@ -204,6 +271,10 @@ MpInitLibInitialize (
   CpuMpData->CpuInfoInHob     = (UINT64) (UINTN) (CpuMpData->CpuData + MaxLogicalProcessorNumber);
   InitializeSpinLock(&CpuMpData->MpLock);
   //
+  // Save BSP's Control registers to APs
+  //
+  SaveVolatileRegisters (&CpuMpData->CpuData[0].VolatileRegisters);
+  //
   // Set BSP basic information
   //
   InitializeApData (CpuMpData, 0, 0);
diff --git a/UefiCpuPkg/Library/MpInitLib/MpLib.h b/UefiCpuPkg/Library/MpInitLib/MpLib.h
index 3f936c2..a455896 100644
--- a/UefiCpuPkg/Library/MpInitLib/MpLib.h
+++ b/UefiCpuPkg/Library/MpInitLib/MpLib.h
@@ -59,6 +59,18 @@ typedef enum {
 } CPU_STATE;
 
 typedef struct {
+  UINTN                          Cr0;
+  UINTN                          Cr3;
+  UINTN                          Cr4;
+  UINTN                          Dr0;
+  UINTN                          Dr1;
+  UINTN                          Dr2;
+  UINTN                          Dr3;
+  UINTN                          Dr6;
+  UINTN                          Dr7;
+} CPU_VOLATILE_REGISTERS;
+
+typedef struct {
   SPIN_LOCK                      ApLock;
   volatile UINT32                *StartupApSignal;
   volatile UINTN                 ApFunction;
@@ -68,6 +80,7 @@ typedef struct {
   UINT32                         Health;
   BOOLEAN                        CpuHealthy;
   volatile CPU_STATE             State;
+  CPU_VOLATILE_REGISTERS         VolatileRegisters;
   BOOLEAN                        Waiting;
   BOOLEAN                        *Finished;
   UINT64                         ExpectedTime;
-- 
2.7.4.windows.1