]> git.proxmox.com Git - mirror_edk2.git/blobdiff - OvmfPkg/Library/SmmCpuFeaturesLib/SmmCpuFeaturesLib.c
OvmfPkg: SmmCpuFeaturesLib: implement SMRAM state save map access
[mirror_edk2.git] / OvmfPkg / Library / SmmCpuFeaturesLib / SmmCpuFeaturesLib.c
index 3b6f186ee817375a831faa292869f42f70ca7b28..b3d0e3a8fb5b4c7f9a7be96b9c1d5a40f3802a26 100644 (file)
@@ -15,11 +15,18 @@ WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
 #include <PiSmm.h>\r
 #include <Library/SmmCpuFeaturesLib.h>\r
 #include <Library/BaseLib.h>\r
+#include <Library/BaseMemoryLib.h>\r
 #include <Library/PcdLib.h>\r
 #include <Library/MemoryAllocationLib.h>\r
+#include <Library/SmmServicesTableLib.h>\r
 #include <Library/DebugLib.h>\r
 #include <Register/SmramSaveStateMap.h>\r
 \r
+//\r
+// EFER register LMA bit\r
+//\r
+#define LMA BIT10\r
+\r
 /**\r
   The constructor function\r
 \r
@@ -128,7 +135,35 @@ SmmCpuFeaturesHookReturnFromSmm (
   IN UINT64                NewInstructionPointer\r
   )\r
 {\r
-  return 0;\r
+  UINT64                 OriginalInstructionPointer;\r
+  SMRAM_SAVE_STATE_MAP  *CpuSaveState = (SMRAM_SAVE_STATE_MAP *)CpuState;\r
+\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
@@ -359,6 +394,213 @@ SmmCpuFeaturesSetSmmRegister (
   ASSERT (FALSE);\r
 }\r
 \r
+///\r
+/// Macro used to simplify the lookup table entries of type CPU_SMM_SAVE_STATE_LOOKUP_ENTRY\r
+///\r
+#define SMM_CPU_OFFSET(Field) OFFSET_OF (SMRAM_SAVE_STATE_MAP, Field)\r
+\r
+///\r
+/// Macro used to simplify the lookup table entries of type 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 (EFI_SMM_SAVE_STATE_REGISTER_GDTBASE, EFI_SMM_SAVE_STATE_REGISTER_LDTINFO),\r
+  SMM_REGISTER_RANGE (EFI_SMM_SAVE_STATE_REGISTER_ES,      EFI_SMM_SAVE_STATE_REGISTER_RIP),\r
+  SMM_REGISTER_RANGE (EFI_SMM_SAVE_STATE_REGISTER_RFLAGS,  EFI_SMM_SAVE_STATE_REGISTER_CR4),\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
+  {0, 0, 0, 0, 0, FALSE},                                                                                                     //  Reserved\r
+\r
+  //\r
+  // CPU Save State registers defined in PI SMM CPU Protocol.\r
+  //\r
+  {0, 8, 0                            , SMM_CPU_OFFSET (x64.GdtBaseLoDword) , SMM_CPU_OFFSET (x64.GdtBaseHiDword), FALSE},  //  EFI_SMM_SAVE_STATE_REGISTER_GDTBASE  = 4\r
+  {0, 8, 0                            , SMM_CPU_OFFSET (x64.IdtBaseLoDword) , SMM_CPU_OFFSET (x64.IdtBaseHiDword), FALSE},  //  EFI_SMM_SAVE_STATE_REGISTER_IDTBASE  = 5\r
+  {0, 8, 0                            , SMM_CPU_OFFSET (x64.LdtBaseLoDword) , SMM_CPU_OFFSET (x64.LdtBaseHiDword), FALSE},  //  EFI_SMM_SAVE_STATE_REGISTER_LDTBASE  = 6\r
+  {0, 0, 0                            , 0                                   , 0                                  , FALSE},  //  EFI_SMM_SAVE_STATE_REGISTER_GDTLIMIT = 7\r
+  {0, 0, 0                            , 0                                   , 0                                  , FALSE},  //  EFI_SMM_SAVE_STATE_REGISTER_IDTLIMIT = 8\r
+  {0, 0, 0                            , 0                                   , 0                                  , FALSE},  //  EFI_SMM_SAVE_STATE_REGISTER_LDTLIMIT = 9\r
+  {0, 0, 0                            , 0                                   , 0                                  , FALSE},  //  EFI_SMM_SAVE_STATE_REGISTER_LDTINFO  = 10\r
+\r
+  {4, 4, SMM_CPU_OFFSET (x86._ES)     , SMM_CPU_OFFSET (x64._ES)     , 0                               , FALSE},  //  EFI_SMM_SAVE_STATE_REGISTER_ES       = 20\r
+  {4, 4, SMM_CPU_OFFSET (x86._CS)     , SMM_CPU_OFFSET (x64._CS)     , 0                               , FALSE},  //  EFI_SMM_SAVE_STATE_REGISTER_CS       = 21\r
+  {4, 4, SMM_CPU_OFFSET (x86._SS)     , SMM_CPU_OFFSET (x64._SS)     , 0                               , FALSE},  //  EFI_SMM_SAVE_STATE_REGISTER_SS       = 22\r
+  {4, 4, SMM_CPU_OFFSET (x86._DS)     , SMM_CPU_OFFSET (x64._DS)     , 0                               , FALSE},  //  EFI_SMM_SAVE_STATE_REGISTER_DS       = 23\r
+  {4, 4, SMM_CPU_OFFSET (x86._FS)     , SMM_CPU_OFFSET (x64._FS)     , 0                               , FALSE},  //  EFI_SMM_SAVE_STATE_REGISTER_FS       = 24\r
+  {4, 4, SMM_CPU_OFFSET (x86._GS)     , SMM_CPU_OFFSET (x64._GS)     , 0                               , FALSE},  //  EFI_SMM_SAVE_STATE_REGISTER_GS       = 25\r
+  {0, 4, 0                            , SMM_CPU_OFFSET (x64._LDTR)   , 0                               , FALSE},  //  EFI_SMM_SAVE_STATE_REGISTER_LDTR_SEL = 26\r
+  {4, 4, SMM_CPU_OFFSET (x86._TR)     , SMM_CPU_OFFSET (x64._TR)     , 0                               , FALSE},  //  EFI_SMM_SAVE_STATE_REGISTER_TR_SEL   = 27\r
+  {4, 8, SMM_CPU_OFFSET (x86._DR7)    , SMM_CPU_OFFSET (x64._DR7)    , SMM_CPU_OFFSET (x64._DR7)    + 4, FALSE},  //  EFI_SMM_SAVE_STATE_REGISTER_DR7      = 28\r
+  {4, 8, SMM_CPU_OFFSET (x86._DR6)    , SMM_CPU_OFFSET (x64._DR6)    , SMM_CPU_OFFSET (x64._DR6)    + 4, FALSE},  //  EFI_SMM_SAVE_STATE_REGISTER_DR6      = 29\r
+  {0, 8, 0                            , SMM_CPU_OFFSET (x64._R8)     , SMM_CPU_OFFSET (x64._R8)     + 4, TRUE },  //  EFI_SMM_SAVE_STATE_REGISTER_R8       = 30\r
+  {0, 8, 0                            , SMM_CPU_OFFSET (x64._R9)     , SMM_CPU_OFFSET (x64._R9)     + 4, TRUE },  //  EFI_SMM_SAVE_STATE_REGISTER_R9       = 31\r
+  {0, 8, 0                            , SMM_CPU_OFFSET (x64._R10)    , SMM_CPU_OFFSET (x64._R10)    + 4, TRUE },  //  EFI_SMM_SAVE_STATE_REGISTER_R10      = 32\r
+  {0, 8, 0                            , SMM_CPU_OFFSET (x64._R11)    , SMM_CPU_OFFSET (x64._R11)    + 4, TRUE },  //  EFI_SMM_SAVE_STATE_REGISTER_R11      = 33\r
+  {0, 8, 0                            , SMM_CPU_OFFSET (x64._R12)    , SMM_CPU_OFFSET (x64._R12)    + 4, TRUE },  //  EFI_SMM_SAVE_STATE_REGISTER_R12      = 34\r
+  {0, 8, 0                            , SMM_CPU_OFFSET (x64._R13)    , SMM_CPU_OFFSET (x64._R13)    + 4, TRUE },  //  EFI_SMM_SAVE_STATE_REGISTER_R13      = 35\r
+  {0, 8, 0                            , SMM_CPU_OFFSET (x64._R14)    , SMM_CPU_OFFSET (x64._R14)    + 4, TRUE },  //  EFI_SMM_SAVE_STATE_REGISTER_R14      = 36\r
+  {0, 8, 0                            , SMM_CPU_OFFSET (x64._R15)    , SMM_CPU_OFFSET (x64._R15)    + 4, TRUE },  //  EFI_SMM_SAVE_STATE_REGISTER_R15      = 37\r
+  {4, 8, SMM_CPU_OFFSET (x86._EAX)    , SMM_CPU_OFFSET (x64._RAX)    , SMM_CPU_OFFSET (x64._RAX)    + 4, TRUE },  //  EFI_SMM_SAVE_STATE_REGISTER_RAX      = 38\r
+  {4, 8, SMM_CPU_OFFSET (x86._EBX)    , SMM_CPU_OFFSET (x64._RBX)    , SMM_CPU_OFFSET (x64._RBX)    + 4, TRUE },  //  EFI_SMM_SAVE_STATE_REGISTER_RBX      = 39\r
+  {4, 8, SMM_CPU_OFFSET (x86._ECX)    , SMM_CPU_OFFSET (x64._RCX)    , SMM_CPU_OFFSET (x64._RCX)    + 4, TRUE },  //  EFI_SMM_SAVE_STATE_REGISTER_RCX      = 40\r
+  {4, 8, SMM_CPU_OFFSET (x86._EDX)    , SMM_CPU_OFFSET (x64._RDX)    , SMM_CPU_OFFSET (x64._RDX)    + 4, TRUE },  //  EFI_SMM_SAVE_STATE_REGISTER_RDX      = 41\r
+  {4, 8, SMM_CPU_OFFSET (x86._ESP)    , SMM_CPU_OFFSET (x64._RSP)    , SMM_CPU_OFFSET (x64._RSP)    + 4, TRUE },  //  EFI_SMM_SAVE_STATE_REGISTER_RSP      = 42\r
+  {4, 8, SMM_CPU_OFFSET (x86._EBP)    , SMM_CPU_OFFSET (x64._RBP)    , SMM_CPU_OFFSET (x64._RBP)    + 4, TRUE },  //  EFI_SMM_SAVE_STATE_REGISTER_RBP      = 43\r
+  {4, 8, SMM_CPU_OFFSET (x86._ESI)    , SMM_CPU_OFFSET (x64._RSI)    , SMM_CPU_OFFSET (x64._RSI)    + 4, TRUE },  //  EFI_SMM_SAVE_STATE_REGISTER_RSI      = 44\r
+  {4, 8, SMM_CPU_OFFSET (x86._EDI)    , SMM_CPU_OFFSET (x64._RDI)    , SMM_CPU_OFFSET (x64._RDI)    + 4, TRUE },  //  EFI_SMM_SAVE_STATE_REGISTER_RDI      = 45\r
+  {4, 8, SMM_CPU_OFFSET (x86._EIP)    , SMM_CPU_OFFSET (x64._RIP)    , SMM_CPU_OFFSET (x64._RIP)    + 4, TRUE },  //  EFI_SMM_SAVE_STATE_REGISTER_RIP      = 46\r
+\r
+  {4, 8, SMM_CPU_OFFSET (x86._EFLAGS) , SMM_CPU_OFFSET (x64._RFLAGS) , SMM_CPU_OFFSET (x64._RFLAGS) + 4, TRUE },  //  EFI_SMM_SAVE_STATE_REGISTER_RFLAGS   = 51\r
+  {4, 8, SMM_CPU_OFFSET (x86._CR0)    , SMM_CPU_OFFSET (x64._CR0)    , SMM_CPU_OFFSET (x64._CR0)    + 4, FALSE},  //  EFI_SMM_SAVE_STATE_REGISTER_CR0      = 52\r
+  {4, 8, SMM_CPU_OFFSET (x86._CR3)    , SMM_CPU_OFFSET (x64._CR3)    , SMM_CPU_OFFSET (x64._CR3)    + 4, FALSE},  //  EFI_SMM_SAVE_STATE_REGISTER_CR3      = 53\r
+  {0, 4, 0                            , SMM_CPU_OFFSET (x64._CR4)    , 0                               , FALSE},  //  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 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; mSmmCpuRegisterRanges[Index].Length != 0; Index++) {\r
+    if (Register >= mSmmCpuRegisterRanges[Index].Start && 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 register is in the \r
+  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 handler.\r
+\r
+  @param[in]  CpuIndex       Specifies the zero-based index of the CPU save 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 state.\r
+  @param[out] Buffer         Upon return, this holds the CPU register value 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 of Processor.\r
+  @retval EFI_INVALID_PARAMTER  This or Buffer is NULL.\r
+\r
+**/\r
+static EFI_STATUS\r
+ReadSaveStateRegisterByIndex (\r
+  IN UINTN   CpuIndex,\r
+  IN UINTN   RegisterIndex,\r
+  IN UINTN   Width,\r
+  OUT VOID   *Buffer\r
+  )\r
+{\r
+  SMRAM_SAVE_STATE_MAP  *CpuSaveState;\r
+\r
+  CpuSaveState = 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 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 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(Buffer, (UINT8 *)CpuSaveState + mSmmCpuWidthOffset[RegisterIndex].Offset32, Width);\r
+  } else {\r
+    //\r
+    // If 64-bit mode width is zero, then the specified register can not be 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 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(Buffer, (UINT8 *)CpuSaveState + mSmmCpuWidthOffset[RegisterIndex].Offset64Lo, MIN(4, Width));\r
+    if (Width >= 4) {\r
+      //\r
+      // Write upper 32-bits of return buffer\r
+      //\r
+      CopyMem((UINT8 *)Buffer + 4, (UINT8 *)CpuSaveState + mSmmCpuWidthOffset[RegisterIndex].Offset64Hi, Width - 4);\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
@@ -386,7 +628,52 @@ SmmCpuFeaturesReadSaveStateRegister (
   OUT VOID                         *Buffer\r
   )\r
 {\r
-  return EFI_UNSUPPORTED;\r
+  UINTN                  RegisterIndex;\r
+  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 = 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 ? EFI_NOT_FOUND : EFI_UNSUPPORTED;\r
+  }\r
+\r
+  return ReadSaveStateRegisterByIndex (CpuIndex, RegisterIndex, Width, Buffer);\r
 }\r
 \r
 /**\r
@@ -414,7 +701,91 @@ SmmCpuFeaturesWriteSaveStateRegister (
   IN CONST VOID                   *Buffer\r
   )\r
 {\r
-  return EFI_UNSUPPORTED;\r
+  UINTN                 RegisterIndex;\r
+  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 ? EFI_NOT_FOUND : EFI_UNSUPPORTED;\r
+  }\r
+\r
+  CpuSaveState = 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 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 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((UINT8 *)CpuSaveState + mSmmCpuWidthOffset[RegisterIndex].Offset32, Buffer, Width);\r
+  } else {\r
+    //\r
+    // If 64-bit mode width is zero, then the specified register can not be 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 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((UINT8 *)CpuSaveState + mSmmCpuWidthOffset[RegisterIndex].Offset64Lo, Buffer, MIN (4, Width));\r
+    if (Width >= 4) {\r
+      //\r
+      // Write upper 32-bits of SMM State register\r
+      //\r
+      CopyMem((UINT8 *)CpuSaveState + mSmmCpuWidthOffset[RegisterIndex].Offset64Hi, (UINT8 *)Buffer + 4, Width - 4);\r
+    }\r
+  }\r
+  return EFI_SUCCESS;\r
 }\r
 \r
 /**\r