OvmfPkg/SmmCpuFeaturesLib: sort #includes, and entries in INF file sections

[mirror_edk2.git] / OvmfPkg / Library / SmmCpuFeaturesLib / SmmCpuFeaturesLib.c

diff --git a/OvmfPkg/Library/SmmCpuFeaturesLib/SmmCpuFeaturesLib.c b/OvmfPkg/Library/SmmCpuFeaturesLib/SmmCpuFeaturesLib.c
index 3b6f186ee817375a831faa292869f42f70ca7b28..75b9ce0e2b12743b9ac815e147f5d8bcfced322b 100644 (file)
--- a/OvmfPkg/Library/SmmCpuFeaturesLib/SmmCpuFeaturesLib.c
+++ b/OvmfPkg/Library/SmmCpuFeaturesLib/SmmCpuFeaturesLib.c
@@ -1,24 +1,31 @@
 /** @file\r
 /** @file\r

-The CPU specific programming for PiSmmCpuDxeSmm module.\r
+  The CPU specific programming for PiSmmCpuDxeSmm module.\r

 \r
 \r

-Copyright (c) 2010 - 2015, Intel Corporation. All rights reserved.<BR>\r
-This program and the accompanying materials\r
-are licensed and made available under the terms and conditions of the BSD License\r
-which accompanies this distribution.  The full text of the license may be found at\r
-http://opensource.org/licenses/bsd-license.php\r
+  Copyright (c) 2010 - 2015, Intel Corporation. All rights reserved.<BR>\r

 \r
 \r

-THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,\r
-WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.\r
+  This program and the accompanying materials are licensed and made available\r
+  under the terms and conditions of the BSD License which accompanies this\r
+  distribution.  The full text of the license may be found at\r
+  http://opensource.org/licenses/bsd-license.php\r

 \r
 \r

+  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS, WITHOUT\r
+  WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.\r

 **/\r
 \r
 **/\r
 \r

-#include <PiSmm.h>\r
-#include <Library/SmmCpuFeaturesLib.h>\r

 #include <Library/BaseLib.h>\r
 #include <Library/BaseLib.h>\r

-#include <Library/PcdLib.h>\r
-#include <Library/MemoryAllocationLib.h>\r
+#include <Library/BaseMemoryLib.h>\r

 #include <Library/DebugLib.h>\r
 #include <Library/DebugLib.h>\r

-#include <Register/SmramSaveStateMap.h>\r
+#include <Library/MemoryAllocationLib.h>\r
+#include <Library/PcdLib.h>\r
+#include <Library/SmmCpuFeaturesLib.h>\r
+#include <Library/SmmServicesTableLib.h>\r
+#include <PiSmm.h>\r
+#include <Register/QemuSmramSaveStateMap.h>\r
+\r
+//\r
+// EFER register LMA bit\r
+//\r
+#define LMA BIT10\r

 \r
 /**\r
   The constructor function\r
 \r
 /**\r
   The constructor function\r


@@ -75,13 +82,20 @@ SmmCpuFeaturesInitializeProcessor (
   IN CPU_HOT_PLUG_DATA          *CpuHotPlugData\r
   )\r
 {\r
   IN CPU_HOT_PLUG_DATA          *CpuHotPlugData\r
   )\r
 {\r

-  SMRAM_SAVE_STATE_MAP  *CpuState;\r
+  QEMU_SMRAM_SAVE_STATE_MAP  *CpuState;\r

 \r
   //\r
   // Configure SMBASE.\r
   //\r
 \r
   //\r
   // Configure SMBASE.\r
   //\r

-  CpuState = (SMRAM_SAVE_STATE_MAP *)(UINTN)(SMM_DEFAULT_SMBASE + SMRAM_SAVE_STATE_MAP_OFFSET);\r
-  CpuState->x86.SMBASE = (UINT32)CpuHotPlugData->SmBase[CpuIndex];\r
+  CpuState = (QEMU_SMRAM_SAVE_STATE_MAP *)(UINTN)(\r
+                                            SMM_DEFAULT_SMBASE +\r
+                                            SMRAM_SAVE_STATE_MAP_OFFSET\r
+                                            );\r
+  if ((CpuState->x86.SMMRevId & 0xFFFF) == 0) {\r
+    CpuState->x86.SMBASE = (UINT32)CpuHotPlugData->SmBase[CpuIndex];\r
+  } else {\r
+    CpuState->x64.SMBASE = (UINT32)CpuHotPlugData->SmBase[CpuIndex];\r
+  }\r

 \r
   //\r
   // No need to program SMRRs on our virtual platform.\r
 \r
   //\r
   // No need to program SMRRs on our virtual platform.\r


@@ -106,8 +120,8 @@ SmmCpuFeaturesInitializeProcessor (
 \r
   @param[in] CpuIndex                 The index of the CPU to hook.  The value\r
                                       must be between 0 and the NumberOfCpus\r
 \r
   @param[in] CpuIndex                 The index of the CPU to hook.  The value\r
                                       must be between 0 and the NumberOfCpus\r

-                                      field in the System Management System Table\r
-                                      (SMST).\r
+                                      field in the System Management System\r
+                                      Table (SMST).\r

   @param[in] CpuState                 Pointer to SMRAM Save State Map for the\r
                                       currently executing CPU.\r
   @param[in] NewInstructionPointer32  Instruction pointer to use if resuming to\r
   @param[in] CpuState                 Pointer to SMRAM Save State Map for the\r
                                       currently executing CPU.\r
   @param[in] NewInstructionPointer32  Instruction pointer to use if resuming to\r


@@ -128,7 +142,36 @@ SmmCpuFeaturesHookReturnFromSmm (
   IN UINT64                NewInstructionPointer\r
   )\r
 {\r
   IN UINT64                NewInstructionPointer\r
   )\r
 {\r

-  return 0;\r
+  UINT64                      OriginalInstructionPointer;\r
+  QEMU_SMRAM_SAVE_STATE_MAP   *CpuSaveState;\r
+\r
+  CpuSaveState = (QEMU_SMRAM_SAVE_STATE_MAP *)CpuState;\r
+  if ((CpuSaveState->x86.SMMRevId & 0xFFFF) == 0) {\r
+    OriginalInstructionPointer = (UINT64)CpuSaveState->x86._EIP;\r
+    CpuSaveState->x86._EIP = (UINT32)NewInstructionPointer;\r
+    //\r
+    // Clear the auto HALT restart flag so the RSM instruction returns\r
+    // program control to the instruction following the HLT instruction.\r
+    //\r
+    if ((CpuSaveState->x86.AutoHALTRestart & BIT0) != 0) {\r
+      CpuSaveState->x86.AutoHALTRestart &= ~BIT0;\r
+    }\r
+  } else {\r
+    OriginalInstructionPointer = CpuSaveState->x64._RIP;\r
+    if ((CpuSaveState->x64.IA32_EFER & LMA) == 0) {\r
+      CpuSaveState->x64._RIP = (UINT32)NewInstructionPointer32;\r
+    } else {\r
+      CpuSaveState->x64._RIP = (UINT32)NewInstructionPointer;\r
+    }\r
+    //\r
+    // Clear the auto HALT restart flag so the RSM instruction returns\r
+    // program control to the instruction following the HLT instruction.\r
+    //\r
+    if ((CpuSaveState->x64.AutoHALTRestart & BIT0) != 0) {\r
+      CpuSaveState->x64.AutoHALTRestart &= ~BIT0;\r
+    }\r
+  }\r
+  return OriginalInstructionPointer;\r

 }\r
 \r
 /**\r
 }\r
 \r
 /**\r


@@ -152,9 +195,10 @@ SmmCpuFeaturesSmmRelocationComplete (
   and the default SMI handler must be used.\r
 \r
   @retval 0    Use the default SMI handler.\r
   and the default SMI handler must be used.\r
 \r
   @retval 0    Use the default SMI handler.\r

-  @retval > 0  Use the SMI handler installed by SmmCpuFeaturesInstallSmiHandler()\r
-               The caller is required to allocate enough SMRAM for each CPU to\r
-               support the size of the custom SMI handler.\r
+  @retval > 0  Use the SMI handler installed by\r
+               SmmCpuFeaturesInstallSmiHandler(). The caller is required to\r
+               allocate enough SMRAM for each CPU to support the size of the\r
+               custom SMI handler.\r

 **/\r
 UINTN\r
 EFIAPI\r
 **/\r
 UINTN\r
 EFIAPI\r


@@ -166,10 +210,10 @@ SmmCpuFeaturesGetSmiHandlerSize (
 }\r
 \r
 /**\r
 }\r
 \r
 /**\r

-  Install a custom SMI handler for the CPU specified by CpuIndex.  This function\r
-  is only called if SmmCpuFeaturesGetSmiHandlerSize() returns a size is greater\r
-  than zero and is called by the CPU that was elected as monarch during System\r
-  Management Mode initialization.\r
+  Install a custom SMI handler for the CPU specified by CpuIndex.  This\r
+  function is only called if SmmCpuFeaturesGetSmiHandlerSize() returns a size\r
+  is greater than zero and is called by the CPU that was elected as monarch\r
+  during System Management Mode initialization.\r

 \r
   @param[in] CpuIndex   The index of the CPU to install the custom SMI handler.\r
                         The value must be between 0 and the NumberOfCpus field\r
 \r
   @param[in] CpuIndex   The index of the CPU to install the custom SMI handler.\r
                         The value must be between 0 and the NumberOfCpus field\r


@@ -224,8 +268,8 @@ SmmCpuFeaturesNeedConfigureMtrrs (
 }\r
 \r
 /**\r
 }\r
 \r
 /**\r

-  Disable SMRR register if SMRR is supported and SmmCpuFeaturesNeedConfigureMtrrs()\r
-  returns TRUE.\r
+  Disable SMRR register if SMRR is supported and\r
+  SmmCpuFeaturesNeedConfigureMtrrs() returns TRUE.\r

 **/\r
 VOID\r
 EFIAPI\r
 **/\r
 VOID\r
 EFIAPI\r


@@ -239,8 +283,8 @@ SmmCpuFeaturesDisableSmrr (
 }\r
 \r
 /**\r
 }\r
 \r
 /**\r

-  Enable SMRR register if SMRR is supported and SmmCpuFeaturesNeedConfigureMtrrs()\r
-  returns TRUE.\r
+  Enable SMRR register if SMRR is supported and\r
+  SmmCpuFeaturesNeedConfigureMtrrs() returns TRUE.\r

 **/\r
 VOID\r
 EFIAPI\r
 **/\r
 VOID\r
 EFIAPI\r


@@ -274,9 +318,9 @@ SmmCpuFeaturesRendezvousEntry (
 /**\r
   Processor specific hook point each time a CPU exits System Management Mode.\r
 \r
 /**\r
   Processor specific hook point each time a CPU exits System Management Mode.\r
 \r

-  @param[in] CpuIndex  The index of the CPU that is exiting SMM.  The value must\r
-                       be between 0 and the NumberOfCpus field in the System\r
-                       Management System Table (SMST).\r
+  @param[in] CpuIndex  The index of the CPU that is exiting SMM.  The value\r
+                       must be between 0 and the NumberOfCpus field in the\r
+                       System Management System Table (SMST).\r

 **/\r
 VOID\r
 EFIAPI\r
 **/\r
 VOID\r
 EFIAPI\r


@@ -359,6 +403,558 @@ SmmCpuFeaturesSetSmmRegister (
   ASSERT (FALSE);\r
 }\r
 \r
   ASSERT (FALSE);\r
 }\r
 \r

+///\r
+/// Macro used to simplify the lookup table entries of type\r
+/// CPU_SMM_SAVE_STATE_LOOKUP_ENTRY\r
+///\r
+#define SMM_CPU_OFFSET(Field) OFFSET_OF (QEMU_SMRAM_SAVE_STATE_MAP, Field)\r
+\r
+///\r
+/// Macro used to simplify the lookup table entries of type\r
+/// CPU_SMM_SAVE_STATE_REGISTER_RANGE\r
+///\r
+#define SMM_REGISTER_RANGE(Start, End) { Start, End, End - Start + 1 }\r
+\r
+///\r
+/// Structure used to describe a range of registers\r
+///\r
+typedef struct {\r
+  EFI_SMM_SAVE_STATE_REGISTER  Start;\r
+  EFI_SMM_SAVE_STATE_REGISTER  End;\r
+  UINTN                        Length;\r
+} CPU_SMM_SAVE_STATE_REGISTER_RANGE;\r
+\r
+///\r
+/// Structure used to build a lookup table to retrieve the widths and offsets\r
+/// associated with each supported EFI_SMM_SAVE_STATE_REGISTER value\r
+///\r
+\r
+#define SMM_SAVE_STATE_REGISTER_FIRST_INDEX             1\r
+\r
+typedef struct {\r
+  UINT8   Width32;\r
+  UINT8   Width64;\r
+  UINT16  Offset32;\r
+  UINT16  Offset64Lo;\r
+  UINT16  Offset64Hi;\r
+  BOOLEAN Writeable;\r
+} CPU_SMM_SAVE_STATE_LOOKUP_ENTRY;\r
+\r
+///\r
+/// Table used by GetRegisterIndex() to convert an EFI_SMM_SAVE_STATE_REGISTER\r
+/// value to an index into a table of type CPU_SMM_SAVE_STATE_LOOKUP_ENTRY\r
+///\r
+STATIC CONST CPU_SMM_SAVE_STATE_REGISTER_RANGE mSmmCpuRegisterRanges[] = {\r
+  SMM_REGISTER_RANGE (\r
+    EFI_SMM_SAVE_STATE_REGISTER_GDTBASE,\r
+    EFI_SMM_SAVE_STATE_REGISTER_LDTINFO\r
+    ),\r
+  SMM_REGISTER_RANGE (\r
+    EFI_SMM_SAVE_STATE_REGISTER_ES,\r
+    EFI_SMM_SAVE_STATE_REGISTER_RIP\r
+    ),\r
+  SMM_REGISTER_RANGE (\r
+    EFI_SMM_SAVE_STATE_REGISTER_RFLAGS,\r
+    EFI_SMM_SAVE_STATE_REGISTER_CR4\r
+    ),\r
+  { (EFI_SMM_SAVE_STATE_REGISTER)0, (EFI_SMM_SAVE_STATE_REGISTER)0, 0 }\r
+};\r
+\r
+///\r
+/// Lookup table used to retrieve the widths and offsets associated with each\r
+/// supported EFI_SMM_SAVE_STATE_REGISTER value\r
+///\r
+STATIC CONST CPU_SMM_SAVE_STATE_LOOKUP_ENTRY mSmmCpuWidthOffset[] = {\r
+  {\r
+    0,                                    // Width32\r
+    0,                                    // Width64\r
+    0,                                    // Offset32\r
+    0,                                    // Offset64Lo\r
+    0,                                    // Offset64Hi\r
+    FALSE                                 // Writeable\r
+  }, // Reserved\r
+\r
+  //\r
+  // CPU Save State registers defined in PI SMM CPU Protocol.\r
+  //\r
+  {\r
+    0,                                    // Width32\r
+    8,                                    // Width64\r
+    0,                                    // Offset32\r
+    SMM_CPU_OFFSET (x64._GDTRBase),       // Offset64Lo\r
+    SMM_CPU_OFFSET (x64._GDTRBase) + 4,   // Offset64Hi\r
+    FALSE                                 // Writeable\r
+  }, // EFI_SMM_SAVE_STATE_REGISTER_GDTBASE = 4\r
+\r
+  {\r
+    0,                                    // Width32\r
+    8,                                    // Width64\r
+    0,                                    // Offset32\r
+    SMM_CPU_OFFSET (x64._IDTRBase),       // Offset64Lo\r
+    SMM_CPU_OFFSET (x64._IDTRBase) + 4,   // Offset64Hi\r
+    FALSE                                 // Writeable\r
+  }, // EFI_SMM_SAVE_STATE_REGISTER_IDTBASE = 5\r
+\r
+  {\r
+    0,                                    // Width32\r
+    8,                                    // Width64\r
+    0,                                    // Offset32\r
+    SMM_CPU_OFFSET (x64._LDTRBase),       // Offset64Lo\r
+    SMM_CPU_OFFSET (x64._LDTRBase) + 4,   // Offset64Hi\r
+    FALSE                                 // Writeable\r
+  }, // EFI_SMM_SAVE_STATE_REGISTER_LDTBASE = 6\r
+\r
+  {\r
+    0,                                    // Width32\r
+    0,                                    // Width64\r
+    0,                                    // Offset32\r
+    SMM_CPU_OFFSET (x64._GDTRLimit),      // Offset64Lo\r
+    SMM_CPU_OFFSET (x64._GDTRLimit) + 4,  // Offset64Hi\r
+    FALSE                                 // Writeable\r
+  }, // EFI_SMM_SAVE_STATE_REGISTER_GDTLIMIT = 7\r
+\r
+  {\r
+    0,                                    // Width32\r
+    0,                                    // Width64\r
+    0,                                    // Offset32\r
+    SMM_CPU_OFFSET (x64._IDTRLimit),      // Offset64Lo\r
+    SMM_CPU_OFFSET (x64._IDTRLimit) + 4,  // Offset64Hi\r
+    FALSE                                 // Writeable\r
+  }, // EFI_SMM_SAVE_STATE_REGISTER_IDTLIMIT = 8\r
+\r
+  {\r
+    0,                                    // Width32\r
+    0,                                    // Width64\r
+    0,                                    // Offset32\r
+    SMM_CPU_OFFSET (x64._LDTRLimit),      // Offset64Lo\r
+    SMM_CPU_OFFSET (x64._LDTRLimit) + 4,  // Offset64Hi\r
+    FALSE                                 // Writeable\r
+  }, // EFI_SMM_SAVE_STATE_REGISTER_LDTLIMIT = 9\r
+\r
+  {\r
+    0,                                    // Width32\r
+    0,                                    // Width64\r
+    0,                                    // Offset32\r
+    0,                                    // Offset64Lo\r
+    0 + 4,                                // Offset64Hi\r
+    FALSE                                 // Writeable\r
+  }, // EFI_SMM_SAVE_STATE_REGISTER_LDTINFO = 10\r
+\r
+  {\r
+    4,                                    // Width32\r
+    4,                                    // Width64\r
+    SMM_CPU_OFFSET (x86._ES),             // Offset32\r
+    SMM_CPU_OFFSET (x64._ES),             // Offset64Lo\r
+    0,                                    // Offset64Hi\r
+    FALSE                                 // Writeable\r
+  }, // EFI_SMM_SAVE_STATE_REGISTER_ES = 20\r
+\r
+  {\r
+    4,                                    // Width32\r
+    4,                                    // Width64\r
+    SMM_CPU_OFFSET (x86._CS),             // Offset32\r
+    SMM_CPU_OFFSET (x64._CS),             // Offset64Lo\r
+    0,                                    // Offset64Hi\r
+    FALSE                                 // Writeable\r
+  }, // EFI_SMM_SAVE_STATE_REGISTER_CS = 21\r
+\r
+  {\r
+    4,                                    // Width32\r
+    4,                                    // Width64\r
+    SMM_CPU_OFFSET (x86._SS),             // Offset32\r
+    SMM_CPU_OFFSET (x64._SS),             // Offset64Lo\r
+    0,                                    // Offset64Hi\r
+    FALSE                                 // Writeable\r
+  }, // EFI_SMM_SAVE_STATE_REGISTER_SS = 22\r
+\r
+  {\r
+    4,                                    // Width32\r
+    4,                                    // Width64\r
+    SMM_CPU_OFFSET (x86._DS),             // Offset32\r
+    SMM_CPU_OFFSET (x64._DS),             // Offset64Lo\r
+    0,                                    // Offset64Hi\r
+    FALSE                                 // Writeable\r
+  }, // EFI_SMM_SAVE_STATE_REGISTER_DS = 23\r
+\r
+  {\r
+    4,                                    // Width32\r
+    4,                                    // Width64\r
+    SMM_CPU_OFFSET (x86._FS),             // Offset32\r
+    SMM_CPU_OFFSET (x64._FS),             // Offset64Lo\r
+    0,                                    // Offset64Hi\r
+    FALSE                                 // Writeable\r
+  }, // EFI_SMM_SAVE_STATE_REGISTER_FS = 24\r
+\r
+  {\r
+    4,                                    // Width32\r
+    4,                                    // Width64\r
+    SMM_CPU_OFFSET (x86._GS),             // Offset32\r
+    SMM_CPU_OFFSET (x64._GS),             // Offset64Lo\r
+    0,                                    // Offset64Hi\r
+    FALSE                                 // Writeable\r
+  }, // EFI_SMM_SAVE_STATE_REGISTER_GS = 25\r
+\r
+  {\r
+    0,                                    // Width32\r
+    4,                                    // Width64\r
+    0,                                    // Offset32\r
+    SMM_CPU_OFFSET (x64._LDTR),           // Offset64Lo\r
+    0,                                    // Offset64Hi\r
+    FALSE                                 // Writeable\r
+  }, // EFI_SMM_SAVE_STATE_REGISTER_LDTR_SEL = 26\r
+\r
+  {\r
+    4,                                    // Width32\r
+    4,                                    // Width64\r
+    SMM_CPU_OFFSET (x86._TR),             // Offset32\r
+    SMM_CPU_OFFSET (x64._TR),             // Offset64Lo\r
+    0,                                    // Offset64Hi\r
+    FALSE                                 // Writeable\r
+  }, // EFI_SMM_SAVE_STATE_REGISTER_TR_SEL = 27\r
+\r
+  {\r
+    4,                                    // Width32\r
+    8,                                    // Width64\r
+    SMM_CPU_OFFSET (x86._DR7),            // Offset32\r
+    SMM_CPU_OFFSET (x64._DR7),            // Offset64Lo\r
+    SMM_CPU_OFFSET (x64._DR7) + 4,        // Offset64Hi\r
+    FALSE                                 // Writeable\r
+  }, // EFI_SMM_SAVE_STATE_REGISTER_DR7 = 28\r
+\r
+  {\r
+    4,                                    // Width32\r
+    8,                                    // Width64\r
+    SMM_CPU_OFFSET (x86._DR6),            // Offset32\r
+    SMM_CPU_OFFSET (x64._DR6),            // Offset64Lo\r
+    SMM_CPU_OFFSET (x64._DR6) + 4,        // Offset64Hi\r
+    FALSE                                 // Writeable\r
+  }, // EFI_SMM_SAVE_STATE_REGISTER_DR6 = 29\r
+\r
+  {\r
+    0,                                    // Width32\r
+    8,                                    // Width64\r
+    0,                                    // Offset32\r
+    SMM_CPU_OFFSET (x64._R8),             // Offset64Lo\r
+    SMM_CPU_OFFSET (x64._R8) + 4,         // Offset64Hi\r
+    TRUE                                  // Writeable\r
+  }, // EFI_SMM_SAVE_STATE_REGISTER_R8 = 30\r
+\r
+  {\r
+    0,                                    // Width32\r
+    8,                                    // Width64\r
+    0,                                    // Offset32\r
+    SMM_CPU_OFFSET (x64._R9),             // Offset64Lo\r
+    SMM_CPU_OFFSET (x64._R9) + 4,         // Offset64Hi\r
+    TRUE                                  // Writeable\r
+  }, // EFI_SMM_SAVE_STATE_REGISTER_R9 = 31\r
+\r
+  {\r
+    0,                                    // Width32\r
+    8,                                    // Width64\r
+    0,                                    // Offset32\r
+    SMM_CPU_OFFSET (x64._R10),            // Offset64Lo\r
+    SMM_CPU_OFFSET (x64._R10) + 4,        // Offset64Hi\r
+    TRUE                                  // Writeable\r
+  }, // EFI_SMM_SAVE_STATE_REGISTER_R10 = 32\r
+\r
+  {\r
+    0,                                    // Width32\r
+    8,                                    // Width64\r
+    0,                                    // Offset32\r
+    SMM_CPU_OFFSET (x64._R11),            // Offset64Lo\r
+    SMM_CPU_OFFSET (x64._R11) + 4,        // Offset64Hi\r
+    TRUE                                  // Writeable\r
+  }, // EFI_SMM_SAVE_STATE_REGISTER_R11 = 33\r
+\r
+  {\r
+    0,                                    // Width32\r
+    8,                                    // Width64\r
+    0,                                    // Offset32\r
+    SMM_CPU_OFFSET (x64._R12),            // Offset64Lo\r
+    SMM_CPU_OFFSET (x64._R12) + 4,        // Offset64Hi\r
+    TRUE                                  // Writeable\r
+  }, // EFI_SMM_SAVE_STATE_REGISTER_R12 = 34\r
+\r
+  {\r
+    0,                                    // Width32\r
+    8,                                    // Width64\r
+    0,                                    // Offset32\r
+    SMM_CPU_OFFSET (x64._R13),            // Offset64Lo\r
+    SMM_CPU_OFFSET (x64._R13) + 4,        // Offset64Hi\r
+    TRUE                                  // Writeable\r
+  }, // EFI_SMM_SAVE_STATE_REGISTER_R13 = 35\r
+\r
+  {\r
+    0,                                    // Width32\r
+    8,                                    // Width64\r
+    0,                                    // Offset32\r
+    SMM_CPU_OFFSET (x64._R14),            // Offset64Lo\r
+    SMM_CPU_OFFSET (x64._R14) + 4,        // Offset64Hi\r
+    TRUE                                  // Writeable\r
+  }, // EFI_SMM_SAVE_STATE_REGISTER_R14 = 36\r
+\r
+  {\r
+    0,                                    // Width32\r
+    8,                                    // Width64\r
+    0,                                    // Offset32\r
+    SMM_CPU_OFFSET (x64._R15),            // Offset64Lo\r
+    SMM_CPU_OFFSET (x64._R15) + 4,        // Offset64Hi\r
+    TRUE                                  // Writeable\r
+  }, // EFI_SMM_SAVE_STATE_REGISTER_R15 = 37\r
+\r
+  {\r
+    4,                                    // Width32\r
+    8,                                    // Width64\r
+    SMM_CPU_OFFSET (x86._EAX),            // Offset32\r
+    SMM_CPU_OFFSET (x64._RAX),            // Offset64Lo\r
+    SMM_CPU_OFFSET (x64._RAX) + 4,        // Offset64Hi\r
+    TRUE                                  // Writeable\r
+  }, // EFI_SMM_SAVE_STATE_REGISTER_RAX = 38\r
+\r
+  {\r
+    4,                                    // Width32\r
+    8,                                    // Width64\r
+    SMM_CPU_OFFSET (x86._EBX),            // Offset32\r
+    SMM_CPU_OFFSET (x64._RBX),            // Offset64Lo\r
+    SMM_CPU_OFFSET (x64._RBX) + 4,        // Offset64Hi\r
+    TRUE                                  // Writeable\r
+  }, // EFI_SMM_SAVE_STATE_REGISTER_RBX = 39\r
+\r
+  {\r
+    4,                                    // Width32\r
+    8,                                    // Width64\r
+    SMM_CPU_OFFSET (x86._ECX),            // Offset32\r
+    SMM_CPU_OFFSET (x64._RCX),            // Offset64Lo\r
+    SMM_CPU_OFFSET (x64._RCX) + 4,        // Offset64Hi\r
+    TRUE                                  // Writeable\r
+  }, // EFI_SMM_SAVE_STATE_REGISTER_RCX = 40\r
+\r
+  {\r
+    4,                                    // Width32\r
+    8,                                    // Width64\r
+    SMM_CPU_OFFSET (x86._EDX),            // Offset32\r
+    SMM_CPU_OFFSET (x64._RDX),            // Offset64Lo\r
+    SMM_CPU_OFFSET (x64._RDX) + 4,        // Offset64Hi\r
+    TRUE                                  // Writeable\r
+  }, // EFI_SMM_SAVE_STATE_REGISTER_RDX = 41\r
+\r
+  {\r
+    4,                                    // Width32\r
+    8,                                    // Width64\r
+    SMM_CPU_OFFSET (x86._ESP),            // Offset32\r
+    SMM_CPU_OFFSET (x64._RSP),            // Offset64Lo\r
+    SMM_CPU_OFFSET (x64._RSP) + 4,        // Offset64Hi\r
+    TRUE                                  // Writeable\r
+  }, // EFI_SMM_SAVE_STATE_REGISTER_RSP = 42\r
+\r
+  {\r
+    4,                                    // Width32\r
+    8,                                    // Width64\r
+    SMM_CPU_OFFSET (x86._EBP),            // Offset32\r
+    SMM_CPU_OFFSET (x64._RBP),            // Offset64Lo\r
+    SMM_CPU_OFFSET (x64._RBP) + 4,        // Offset64Hi\r
+    TRUE                                  // Writeable\r
+  }, // EFI_SMM_SAVE_STATE_REGISTER_RBP = 43\r
+\r
+  {\r
+    4,                                    // Width32\r
+    8,                                    // Width64\r
+    SMM_CPU_OFFSET (x86._ESI),            // Offset32\r
+    SMM_CPU_OFFSET (x64._RSI),            // Offset64Lo\r
+    SMM_CPU_OFFSET (x64._RSI) + 4,        // Offset64Hi\r
+    TRUE                                  // Writeable\r
+  }, // EFI_SMM_SAVE_STATE_REGISTER_RSI = 44\r
+\r
+  {\r
+    4,                                    // Width32\r
+    8,                                    // Width64\r
+    SMM_CPU_OFFSET (x86._EDI),            // Offset32\r
+    SMM_CPU_OFFSET (x64._RDI),            // Offset64Lo\r
+    SMM_CPU_OFFSET (x64._RDI) + 4,        // Offset64Hi\r
+    TRUE                                  // Writeable\r
+  }, // EFI_SMM_SAVE_STATE_REGISTER_RDI = 45\r
+\r
+  {\r
+    4,                                    // Width32\r
+    8,                                    // Width64\r
+    SMM_CPU_OFFSET (x86._EIP),            // Offset32\r
+    SMM_CPU_OFFSET (x64._RIP),            // Offset64Lo\r
+    SMM_CPU_OFFSET (x64._RIP) + 4,        // Offset64Hi\r
+    TRUE                                  // Writeable\r
+  }, // EFI_SMM_SAVE_STATE_REGISTER_RIP = 46\r
+\r
+  {\r
+    4,                                    // Width32\r
+    8,                                    // Width64\r
+    SMM_CPU_OFFSET (x86._EFLAGS),         // Offset32\r
+    SMM_CPU_OFFSET (x64._RFLAGS),         // Offset64Lo\r
+    SMM_CPU_OFFSET (x64._RFLAGS) + 4,     // Offset64Hi\r
+    TRUE                                  // Writeable\r
+  }, // EFI_SMM_SAVE_STATE_REGISTER_RFLAGS = 51\r
+\r
+  {\r
+    4,                                    // Width32\r
+    8,                                    // Width64\r
+    SMM_CPU_OFFSET (x86._CR0),            // Offset32\r
+    SMM_CPU_OFFSET (x64._CR0),            // Offset64Lo\r
+    SMM_CPU_OFFSET (x64._CR0) + 4,        // Offset64Hi\r
+    FALSE                                 // Writeable\r
+  }, // EFI_SMM_SAVE_STATE_REGISTER_CR0 = 52\r
+\r
+  {\r
+    4,                                    // Width32\r
+    8,                                    // Width64\r
+    SMM_CPU_OFFSET (x86._CR3),            // Offset32\r
+    SMM_CPU_OFFSET (x64._CR3),            // Offset64Lo\r
+    SMM_CPU_OFFSET (x64._CR3) + 4,        // Offset64Hi\r
+    FALSE                                 // Writeable\r
+  }, // EFI_SMM_SAVE_STATE_REGISTER_CR3 = 53\r
+\r
+  {\r
+    0,                                    // Width32\r
+    4,                                    // Width64\r
+    0,                                    // Offset32\r
+    SMM_CPU_OFFSET (x64._CR4),            // Offset64Lo\r
+    SMM_CPU_OFFSET (x64._CR4) + 4,        // Offset64Hi\r
+    FALSE                                 // Writeable\r
+  }, // EFI_SMM_SAVE_STATE_REGISTER_CR4 = 54\r
+};\r
+\r
+//\r
+// No support for I/O restart\r
+//\r
+\r
+/**\r
+  Read information from the CPU save state.\r
+\r
+  @param  Register  Specifies the CPU register to read form the save state.\r
+\r
+  @retval 0   Register is not valid\r
+  @retval >0  Index into mSmmCpuWidthOffset[] associated with Register\r
+\r
+**/\r
+STATIC\r
+UINTN\r
+GetRegisterIndex (\r
+  IN EFI_SMM_SAVE_STATE_REGISTER  Register\r
+  )\r
+{\r
+  UINTN  Index;\r
+  UINTN  Offset;\r
+\r
+  for (Index = 0, Offset = SMM_SAVE_STATE_REGISTER_FIRST_INDEX;\r
+       mSmmCpuRegisterRanges[Index].Length != 0;\r
+       Index++) {\r
+    if (Register >= mSmmCpuRegisterRanges[Index].Start &&\r
+        Register <= mSmmCpuRegisterRanges[Index].End) {\r
+      return Register - mSmmCpuRegisterRanges[Index].Start + Offset;\r
+    }\r
+    Offset += mSmmCpuRegisterRanges[Index].Length;\r
+  }\r
+  return 0;\r
+}\r
+\r
+/**\r
+  Read a CPU Save State register on the target processor.\r
+\r
+  This function abstracts the differences that whether the CPU Save State\r
+  register is in the IA32 CPU Save State Map or X64 CPU Save State Map.\r
+\r
+  This function supports reading a CPU Save State register in SMBase relocation\r
+  handler.\r
+\r
+  @param[in]  CpuIndex       Specifies the zero-based index of the CPU save\r
+                             state.\r
+  @param[in]  RegisterIndex  Index into mSmmCpuWidthOffset[] look up table.\r
+  @param[in]  Width          The number of bytes to read from the CPU save\r
+                             state.\r
+  @param[out] Buffer         Upon return, this holds the CPU register value\r
+                             read from the save state.\r
+\r
+  @retval EFI_SUCCESS           The register was read from Save State.\r
+  @retval EFI_NOT_FOUND         The register is not defined for the Save State\r
+                                of Processor.\r
+  @retval EFI_INVALID_PARAMTER  This or Buffer is NULL.\r
+\r
+**/\r
+STATIC\r
+EFI_STATUS\r
+ReadSaveStateRegisterByIndex (\r
+  IN UINTN   CpuIndex,\r
+  IN UINTN   RegisterIndex,\r
+  IN UINTN   Width,\r
+  OUT VOID   *Buffer\r
+  )\r
+{\r
+  QEMU_SMRAM_SAVE_STATE_MAP  *CpuSaveState;\r
+\r
+  CpuSaveState = (QEMU_SMRAM_SAVE_STATE_MAP *)gSmst->CpuSaveState[CpuIndex];\r
+\r
+  if ((CpuSaveState->x86.SMMRevId & 0xFFFF) == 0) {\r
+    //\r
+    // If 32-bit mode width is zero, then the specified register can not be\r
+    // accessed\r
+    //\r
+    if (mSmmCpuWidthOffset[RegisterIndex].Width32 == 0) {\r
+      return EFI_NOT_FOUND;\r
+    }\r
+\r
+    //\r
+    // If Width is bigger than the 32-bit mode width, then the specified\r
+    // register can not be accessed\r
+    //\r
+    if (Width > mSmmCpuWidthOffset[RegisterIndex].Width32) {\r
+      return EFI_INVALID_PARAMETER;\r
+    }\r
+\r
+    //\r
+    // Write return buffer\r
+    //\r
+    ASSERT(CpuSaveState != NULL);\r
+    CopyMem (\r
+      Buffer,\r
+      (UINT8 *)CpuSaveState + mSmmCpuWidthOffset[RegisterIndex].Offset32,\r
+      Width\r
+      );\r
+  } else {\r
+    //\r
+    // If 64-bit mode width is zero, then the specified register can not be\r
+    // accessed\r
+    //\r
+    if (mSmmCpuWidthOffset[RegisterIndex].Width64 == 0) {\r
+      return EFI_NOT_FOUND;\r
+    }\r
+\r
+    //\r
+    // If Width is bigger than the 64-bit mode width, then the specified\r
+    // register can not be accessed\r
+    //\r
+    if (Width > mSmmCpuWidthOffset[RegisterIndex].Width64) {\r
+      return EFI_INVALID_PARAMETER;\r
+    }\r
+\r
+    //\r
+    // Write lower 32-bits of return buffer\r
+    //\r
+    CopyMem (\r
+      Buffer,\r
+      (UINT8 *)CpuSaveState + mSmmCpuWidthOffset[RegisterIndex].Offset64Lo,\r
+      MIN (4, Width)\r
+      );\r
+    if (Width >= 4) {\r
+      //\r
+      // Write upper 32-bits of return buffer\r
+      //\r
+      CopyMem (\r
+        (UINT8 *)Buffer + 4,\r
+        (UINT8 *)CpuSaveState + mSmmCpuWidthOffset[RegisterIndex].Offset64Hi,\r
+        Width - 4\r
+        );\r
+    }\r
+  }\r
+  return EFI_SUCCESS;\r
+}\r
+\r

 /**\r
   Read an SMM Save State register on the target processor.  If this function\r
   returns EFI_UNSUPPORTED, then the caller is responsible for reading the\r
 /**\r
   Read an SMM Save State register on the target processor.  If this function\r
   returns EFI_UNSUPPORTED, then the caller is responsible for reading the\r


@@ -374,8 +970,8 @@ SmmCpuFeaturesSetSmmRegister (
 \r
   @retval EFI_SUCCESS           The register was read from Save State.\r
   @retval EFI_INVALID_PARAMTER  Buffer is NULL.\r
 \r
   @retval EFI_SUCCESS           The register was read from Save State.\r
   @retval EFI_INVALID_PARAMTER  Buffer is NULL.\r

-  @retval EFI_UNSUPPORTED       This function does not support reading Register.\r
-\r
+  @retval EFI_UNSUPPORTED       This function does not support reading\r
+                                Register.\r

 **/\r
 EFI_STATUS\r
 EFIAPI\r
 **/\r
 EFI_STATUS\r
 EFIAPI\r


@@ -386,7 +982,54 @@ SmmCpuFeaturesReadSaveStateRegister (
   OUT VOID                         *Buffer\r
   )\r
 {\r
   OUT VOID                         *Buffer\r
   )\r
 {\r

-  return EFI_UNSUPPORTED;\r
+  UINTN                       RegisterIndex;\r
+  QEMU_SMRAM_SAVE_STATE_MAP  *CpuSaveState;\r
+\r
+  //\r
+  // Check for special EFI_SMM_SAVE_STATE_REGISTER_LMA\r
+  //\r
+  if (Register == EFI_SMM_SAVE_STATE_REGISTER_LMA) {\r
+    //\r
+    // Only byte access is supported for this register\r
+    //\r
+    if (Width != 1) {\r
+      return EFI_INVALID_PARAMETER;\r
+    }\r
+\r
+    CpuSaveState = (QEMU_SMRAM_SAVE_STATE_MAP *)gSmst->CpuSaveState[CpuIndex];\r
+\r
+    //\r
+    // Check CPU mode\r
+    //\r
+    if ((CpuSaveState->x86.SMMRevId & 0xFFFF) == 0) {\r
+      *(UINT8 *)Buffer = 32;\r
+    } else {\r
+      *(UINT8 *)Buffer = 64;\r
+    }\r
+\r
+    return EFI_SUCCESS;\r
+  }\r
+\r
+  //\r
+  // Check for special EFI_SMM_SAVE_STATE_REGISTER_IO\r
+  //\r
+  if (Register == EFI_SMM_SAVE_STATE_REGISTER_IO) {\r
+    return EFI_NOT_FOUND;\r
+  }\r
+\r
+  //\r
+  // Convert Register to a register lookup table index.  Let\r
+  // PiSmmCpuDxeSmm implement other special registers (currently\r
+  // there is only EFI_SMM_SAVE_STATE_REGISTER_PROCESSOR_ID).\r
+  //\r
+  RegisterIndex = GetRegisterIndex (Register);\r
+  if (RegisterIndex == 0) {\r
+    return (Register < EFI_SMM_SAVE_STATE_REGISTER_IO ?\r
+            EFI_NOT_FOUND :\r
+            EFI_UNSUPPORTED);\r
+  }\r
+\r
+  return ReadSaveStateRegisterByIndex (CpuIndex, RegisterIndex, Width, Buffer);\r

 }\r
 \r
 /**\r
 }\r
 \r
 /**\r


@@ -403,7 +1046,8 @@ SmmCpuFeaturesReadSaveStateRegister (
 \r
   @retval EFI_SUCCESS           The register was written to Save State.\r
   @retval EFI_INVALID_PARAMTER  Buffer is NULL.\r
 \r
   @retval EFI_SUCCESS           The register was written to Save State.\r
   @retval EFI_INVALID_PARAMTER  Buffer is NULL.\r

-  @retval EFI_UNSUPPORTED       This function does not support writing Register.\r
+  @retval EFI_UNSUPPORTED       This function does not support writing\r
+                                Register.\r

 **/\r
 EFI_STATUS\r
 EFIAPI\r
 **/\r
 EFI_STATUS\r
 EFIAPI\r


@@ -414,7 +1058,109 @@ SmmCpuFeaturesWriteSaveStateRegister (
   IN CONST VOID                   *Buffer\r
   )\r
 {\r
   IN CONST VOID                   *Buffer\r
   )\r
 {\r

-  return EFI_UNSUPPORTED;\r
+  UINTN                       RegisterIndex;\r
+  QEMU_SMRAM_SAVE_STATE_MAP  *CpuSaveState;\r
+\r
+  //\r
+  // Writes to EFI_SMM_SAVE_STATE_REGISTER_LMA are ignored\r
+  //\r
+  if (Register == EFI_SMM_SAVE_STATE_REGISTER_LMA) {\r
+    return EFI_SUCCESS;\r
+  }\r
+\r
+  //\r
+  // Writes to EFI_SMM_SAVE_STATE_REGISTER_IO are not supported\r
+  //\r
+  if (Register == EFI_SMM_SAVE_STATE_REGISTER_IO) {\r
+    return EFI_NOT_FOUND;\r
+  }\r
+\r
+  //\r
+  // Convert Register to a register lookup table index.  Let\r
+  // PiSmmCpuDxeSmm implement other special registers (currently\r
+  // there is only EFI_SMM_SAVE_STATE_REGISTER_PROCESSOR_ID).\r
+  //\r
+  RegisterIndex = GetRegisterIndex (Register);\r
+  if (RegisterIndex == 0) {\r
+    return (Register < EFI_SMM_SAVE_STATE_REGISTER_IO ?\r
+            EFI_NOT_FOUND :\r
+            EFI_UNSUPPORTED);\r
+  }\r
+\r
+  CpuSaveState = (QEMU_SMRAM_SAVE_STATE_MAP *)gSmst->CpuSaveState[CpuIndex];\r
+\r
+  //\r
+  // Do not write non-writable SaveState, because it will cause exception.\r
+  //\r
+  if (!mSmmCpuWidthOffset[RegisterIndex].Writeable) {\r
+    return EFI_UNSUPPORTED;\r
+  }\r
+\r
+  //\r
+  // Check CPU mode\r
+  //\r
+  if ((CpuSaveState->x86.SMMRevId & 0xFFFF) == 0) {\r
+    //\r
+    // If 32-bit mode width is zero, then the specified register can not be\r
+    // accessed\r
+    //\r
+    if (mSmmCpuWidthOffset[RegisterIndex].Width32 == 0) {\r
+      return EFI_NOT_FOUND;\r
+    }\r
+\r
+    //\r
+    // If Width is bigger than the 32-bit mode width, then the specified\r
+    // register can not be accessed\r
+    //\r
+    if (Width > mSmmCpuWidthOffset[RegisterIndex].Width32) {\r
+      return EFI_INVALID_PARAMETER;\r
+    }\r
+    //\r
+    // Write SMM State register\r
+    //\r
+    ASSERT (CpuSaveState != NULL);\r
+    CopyMem (\r
+      (UINT8 *)CpuSaveState + mSmmCpuWidthOffset[RegisterIndex].Offset32,\r
+      Buffer,\r
+      Width\r
+      );\r
+  } else {\r
+    //\r
+    // If 64-bit mode width is zero, then the specified register can not be\r
+    // accessed\r
+    //\r
+    if (mSmmCpuWidthOffset[RegisterIndex].Width64 == 0) {\r
+      return EFI_NOT_FOUND;\r
+    }\r
+\r
+    //\r
+    // If Width is bigger than the 64-bit mode width, then the specified\r
+    // register can not be accessed\r
+    //\r
+    if (Width > mSmmCpuWidthOffset[RegisterIndex].Width64) {\r
+      return EFI_INVALID_PARAMETER;\r
+    }\r
+\r
+    //\r
+    // Write lower 32-bits of SMM State register\r
+    //\r
+    CopyMem (\r
+      (UINT8 *)CpuSaveState + mSmmCpuWidthOffset[RegisterIndex].Offset64Lo,\r
+      Buffer,\r
+      MIN (4, Width)\r
+      );\r
+    if (Width >= 4) {\r
+      //\r
+      // Write upper 32-bits of SMM State register\r
+      //\r
+      CopyMem (\r
+        (UINT8 *)CpuSaveState + mSmmCpuWidthOffset[RegisterIndex].Offset64Hi,\r
+        (UINT8 *)Buffer + 4,\r
+        Width - 4\r
+        );\r
+    }\r
+  }\r
+  return EFI_SUCCESS;\r

 }\r
 \r
 /**\r
 }\r
 \r
 /**\r


@@ -430,22 +1176,25 @@ SmmCpuFeaturesCompleteSmmReadyToLock (
 }\r
 \r
 /**\r
 }\r
 \r
 /**\r

-  This API provides a method for a CPU to allocate a specific region for storing page tables.\r
+  This API provides a method for a CPU to allocate a specific region for\r
+  storing page tables.\r

 \r
   This API can be called more once to allocate memory for page tables.\r
 \r
 \r
   This API can be called more once to allocate memory for page tables.\r
 \r

-  Allocates the number of 4KB pages of type EfiRuntimeServicesData and returns a pointer to the\r
-  allocated buffer.  The buffer returned is aligned on a 4KB boundary.  If Pages is 0, then NULL\r
-  is returned.  If there is not enough memory remaining to satisfy the request, then NULL is\r
-  returned.\r
+  Allocates the number of 4KB pages of type EfiRuntimeServicesData and returns\r
+  a pointer to the allocated buffer.  The buffer returned is aligned on a 4KB\r
+  boundary.  If Pages is 0, then NULL is returned.  If there is not enough\r
+  memory remaining to satisfy the request, then NULL is returned.\r

 \r
 \r

-  This function can also return NULL if there is no preference on where the page tables are allocated in SMRAM.\r
+  This function can also return NULL if there is no preference on where the\r
+  page tables are allocated in SMRAM.\r

 \r
   @param  Pages                 The number of 4 KB pages to allocate.\r
 \r
   @return A pointer to the allocated buffer for page tables.\r
   @retval NULL      Fail to allocate a specific region for storing page tables,\r
 \r
   @param  Pages                 The number of 4 KB pages to allocate.\r
 \r
   @return A pointer to the allocated buffer for page tables.\r
   @retval NULL      Fail to allocate a specific region for storing page tables,\r

-                    Or there is no preference on where the page tables are allocated in SMRAM.\r
+                    Or there is no preference on where the page tables are\r
+                    allocated in SMRAM.\r

 \r
 **/\r
 VOID *\r
 \r
 **/\r
 VOID *\r