From ef1fdb809886ddfeb9e4107919b14414a2fea020 Mon Sep 17 00:00:00 2001
From: Jeff Fan <jeff.fan@intel.com>
Date: Wed, 2 Dec 2015 00:42:40 +0000
Subject: [PATCH] UefiCpuPkg/CpuMpPei: Add CPU_VOLATILE_REGISTERS & worker
 functions

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: Feng Tian <feng.tian@intel.com>
Cc: Michael Kinney <michael.d.kinney@intel.com>
Contributed-under: TianoCore Contribution Agreement 1.0
Signed-off-by: Jeff Fan <jeff.fan@intel.com>
Reviewed-by: Feng Tian <feng.tian@Intel.com>

git-svn-id: https://svn.code.sf.net/p/edk2/code/trunk/edk2@19086 6f19259b-4bc3-4df7-8a09-765794883524
---
 UefiCpuPkg/CpuMpPei/CpuMpPei.c | 67 ++++++++++++++++++++++++++++++++++
 UefiCpuPkg/CpuMpPei/CpuMpPei.h | 13 +++++++
 2 files changed, 80 insertions(+)

diff --git a/UefiCpuPkg/CpuMpPei/CpuMpPei.c b/UefiCpuPkg/CpuMpPei/CpuMpPei.c
index ae071ac1f8..7bedf64210 100644
--- a/UefiCpuPkg/CpuMpPei/CpuMpPei.c
+++ b/UefiCpuPkg/CpuMpPei/CpuMpPei.c
@@ -141,6 +141,73 @@ GetMpHobData (
   return CpuMpData;
 }
 
+/**
+  Save the volatile registers required to be restored following INIT IPI
+  
+  @param  VolatileRegisters    Returns buffer saved the volatile resisters
+**/
+VOID
+SaveVolatileRegisters (
+  OUT CPU_VOLATILE_REGISTERS    *VolatileRegisters
+  )
+{
+  UINT32                        RegEdx;
+
+  VolatileRegisters->Cr0 = AsmReadCr0 ();
+  VolatileRegisters->Cr3 = AsmReadCr3 ();
+  VolatileRegisters->Cr4 = AsmReadCr4 ();
+
+  AsmCpuid (CPUID_VERSION_INFO, NULL, NULL, NULL, &RegEdx);
+  if ((RegEdx & BIT2) != 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  VolatileRegisters   Pointer to volatile resisters
+  @param  IsRestoreDr         TRUE:  Restore DRx if supported
+                              FALSE: Do not restore DRx
+**/
+VOID
+RestoreVolatileRegisters (
+  IN CPU_VOLATILE_REGISTERS    *VolatileRegisters,
+  IN BOOLEAN                   IsRestoreDr
+  )
+{
+  UINT32                        RegEdx;
+
+  AsmWriteCr0 (VolatileRegisters->Cr0);
+  AsmWriteCr3 (VolatileRegisters->Cr3);
+  AsmWriteCr4 (VolatileRegisters->Cr4);
+
+  if (IsRestoreDr) {
+    AsmCpuid (CPUID_VERSION_INFO, NULL, NULL, NULL, &RegEdx);
+    if ((RegEdx & BIT2) != 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);
+    }
+  }
+}
+
 /**
   This function will be called from AP reset code if BSP uses WakeUpAP.
 
diff --git a/UefiCpuPkg/CpuMpPei/CpuMpPei.h b/UefiCpuPkg/CpuMpPei/CpuMpPei.h
index de9011329c..f2286b990e 100644
--- a/UefiCpuPkg/CpuMpPei/CpuMpPei.h
+++ b/UefiCpuPkg/CpuMpPei/CpuMpPei.h
@@ -113,11 +113,24 @@ typedef struct {
 
 #pragma pack()
 
+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 {
   UINT32                         ApicId;
   EFI_HEALTH_FLAGS               Health;
   CPU_STATE                      State;
   BOOLEAN                        CpuHealthy;
+  CPU_VOLATILE_REGISTERS         VolatileRegisters;
 } PEI_CPU_DATA;
 
 //
-- 
2.39.2