[Patch v5 14/48] 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.

v5:
  1. Add comment block for structure CPU_VOLATILE_REGISTERS.

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 | 16 ++++++++
 2 files changed, 87 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 84bd872..ca8bd44 100644
--- a/UefiCpuPkg/Library/MpInitLib/MpLib.h
+++ b/UefiCpuPkg/Library/MpInitLib/MpLib.h
@@ -66,6 +66,21 @@ typedef enum {
 } CPU_STATE;
 
 //
+// CPU volatile registers around INIT-SIPI-SIPI
+//
+typedef struct {
+  UINTN                          Cr0;
+  UINTN                          Cr3;
+  UINTN                          Cr4;
+  UINTN                          Dr0;
+  UINTN                          Dr1;
+  UINTN                          Dr2;
+  UINTN                          Dr3;
+  UINTN                          Dr6;
+  UINTN                          Dr7;
+} CPU_VOLATILE_REGISTERS;
+
+//
 // AP related data
 //
 typedef struct {
@@ -78,6 +93,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