From af1a266670d040d2f4083ff309d732d648afba2a Mon Sep 17 00:00:00 2001
From: Angelos Mouzakitis <a.mouzakitis@virtualopensystems.com>
Date: Tue, 10 Oct 2023 14:33:42 +0000
Subject: Add submodule dependency files

Change-Id: Iaf8d18082d3991dec7c0ebbea540f092188eb4ec
---
 .../Library/SmmCpuFeaturesLib/Ia32/SmiEntry.nasm   |  276 +++++
 .../SmmCpuFeaturesLib/Ia32/SmiException.nasm       |  173 +++
 .../Library/SmmCpuFeaturesLib/Ia32/SmmStmSupport.c |   77 ++
 .../Library/SmmCpuFeaturesLib/SmmCpuFeaturesLib.c  |  626 ++++++++++
 .../SmmCpuFeaturesLib/SmmCpuFeaturesLib.inf        |   34 +
 .../SmmCpuFeaturesLib/SmmCpuFeaturesLib.uni        |   12 +
 .../SmmCpuFeaturesLib/SmmCpuFeaturesLibNoStm.c     |   83 ++
 .../SmmCpuFeaturesLib/SmmCpuFeaturesLibStm.inf     |   72 ++
 .../UefiCpuPkg/Library/SmmCpuFeaturesLib/SmmStm.c  | 1297 ++++++++++++++++++++
 .../UefiCpuPkg/Library/SmmCpuFeaturesLib/SmmStm.h  |  179 +++
 .../Library/SmmCpuFeaturesLib/X64/SmiEntry.nasm    |  276 +++++
 .../SmmCpuFeaturesLib/X64/SmiException.nasm        |  176 +++
 .../Library/SmmCpuFeaturesLib/X64/SmmStmSupport.c  |   89 ++
 13 files changed, 3370 insertions(+)
 create mode 100644 roms/edk2/UefiCpuPkg/Library/SmmCpuFeaturesLib/Ia32/SmiEntry.nasm
 create mode 100644 roms/edk2/UefiCpuPkg/Library/SmmCpuFeaturesLib/Ia32/SmiException.nasm
 create mode 100644 roms/edk2/UefiCpuPkg/Library/SmmCpuFeaturesLib/Ia32/SmmStmSupport.c
 create mode 100644 roms/edk2/UefiCpuPkg/Library/SmmCpuFeaturesLib/SmmCpuFeaturesLib.c
 create mode 100644 roms/edk2/UefiCpuPkg/Library/SmmCpuFeaturesLib/SmmCpuFeaturesLib.inf
 create mode 100644 roms/edk2/UefiCpuPkg/Library/SmmCpuFeaturesLib/SmmCpuFeaturesLib.uni
 create mode 100644 roms/edk2/UefiCpuPkg/Library/SmmCpuFeaturesLib/SmmCpuFeaturesLibNoStm.c
 create mode 100644 roms/edk2/UefiCpuPkg/Library/SmmCpuFeaturesLib/SmmCpuFeaturesLibStm.inf
 create mode 100644 roms/edk2/UefiCpuPkg/Library/SmmCpuFeaturesLib/SmmStm.c
 create mode 100644 roms/edk2/UefiCpuPkg/Library/SmmCpuFeaturesLib/SmmStm.h
 create mode 100644 roms/edk2/UefiCpuPkg/Library/SmmCpuFeaturesLib/X64/SmiEntry.nasm
 create mode 100644 roms/edk2/UefiCpuPkg/Library/SmmCpuFeaturesLib/X64/SmiException.nasm
 create mode 100644 roms/edk2/UefiCpuPkg/Library/SmmCpuFeaturesLib/X64/SmmStmSupport.c

(limited to 'roms/edk2/UefiCpuPkg/Library/SmmCpuFeaturesLib')

diff --git a/roms/edk2/UefiCpuPkg/Library/SmmCpuFeaturesLib/Ia32/SmiEntry.nasm b/roms/edk2/UefiCpuPkg/Library/SmmCpuFeaturesLib/Ia32/SmiEntry.nasm
new file mode 100644
index 000000000..c98906c01
--- /dev/null
+++ b/roms/edk2/UefiCpuPkg/Library/SmmCpuFeaturesLib/Ia32/SmiEntry.nasm
@@ -0,0 +1,276 @@
+;------------------------------------------------------------------------------ ;
+; Copyright (c) 2016 - 2018, Intel Corporation. All rights reserved.<BR>
+; SPDX-License-Identifier: BSD-2-Clause-Patent
+;
+; Module Name:
+;
+;   SmiEntry.nasm
+;
+; Abstract:
+;
+;   Code template of the SMI handler for a particular processor
+;
+;-------------------------------------------------------------------------------
+
+%include "StuffRsbNasm.inc"
+
+%define MSR_IA32_MISC_ENABLE 0x1A0
+%define MSR_EFER      0xc0000080
+%define MSR_EFER_XD   0x800
+
+;
+; Constants relating to TXT_PROCESSOR_SMM_DESCRIPTOR
+;
+%define DSC_OFFSET 0xfb00
+%define DSC_GDTPTR 0x48
+%define DSC_GDTSIZ 0x50
+%define DSC_CS 0x14
+%define DSC_DS 0x16
+%define DSC_SS 0x18
+%define DSC_OTHERSEG 0x1a
+
+%define PROTECT_MODE_CS 0x8
+%define PROTECT_MODE_DS 0x20
+%define TSS_SEGMENT 0x40
+
+extern ASM_PFX(SmiRendezvous)
+extern ASM_PFX(FeaturePcdGet (PcdCpuSmmStackGuard))
+extern ASM_PFX(CpuSmmDebugEntry)
+extern ASM_PFX(CpuSmmDebugExit)
+
+global ASM_PFX(gcStmSmiHandlerTemplate)
+global ASM_PFX(gcStmSmiHandlerSize)
+global ASM_PFX(gcStmSmiHandlerOffset)
+global ASM_PFX(gStmSmiCr3)
+global ASM_PFX(gStmSmiStack)
+global ASM_PFX(gStmSmbase)
+global ASM_PFX(gStmXdSupported)
+extern ASM_PFX(gStmSmiHandlerIdtr)
+
+ASM_PFX(gStmSmiCr3)      EQU StmSmiCr3Patch - 4
+ASM_PFX(gStmSmiStack)    EQU StmSmiStackPatch - 4
+ASM_PFX(gStmSmbase)      EQU StmSmbasePatch - 4
+ASM_PFX(gStmXdSupported) EQU StmXdSupportedPatch - 1
+
+    SECTION .text
+
+BITS 16
+ASM_PFX(gcStmSmiHandlerTemplate):
+_StmSmiEntryPoint:
+    mov     bx, _StmGdtDesc - _StmSmiEntryPoint + 0x8000
+    mov     ax,[cs:DSC_OFFSET + DSC_GDTSIZ]
+    dec     ax
+    mov     [cs:bx], ax
+    mov     eax, [cs:DSC_OFFSET + DSC_GDTPTR]
+    mov     [cs:bx + 2], eax
+    mov     ebp, eax                      ; ebp = GDT base
+o32 lgdt    [cs:bx]                       ; lgdt fword ptr cs:[bx]
+    mov     ax, PROTECT_MODE_CS
+    mov     [cs:bx-0x2],ax
+o32 mov     edi, strict dword 0
+StmSmbasePatch:
+    lea     eax, [edi + (@32bit - _StmSmiEntryPoint) + 0x8000]
+    mov     [cs:bx-0x6],eax
+    mov     ebx, cr0
+    and     ebx, 0x9ffafff3
+    or      ebx, 0x23
+    mov     cr0, ebx
+    jmp     dword 0x0:0x0
+_StmGdtDesc:
+    DW 0
+    DD 0
+
+BITS 32
+@32bit:
+    mov     ax, PROTECT_MODE_DS
+o16 mov     ds, ax
+o16 mov     es, ax
+o16 mov     fs, ax
+o16 mov     gs, ax
+o16 mov     ss, ax
+    mov     esp, strict dword 0
+StmSmiStackPatch:
+    mov     eax, ASM_PFX(gStmSmiHandlerIdtr)
+    lidt    [eax]
+    jmp     ProtFlatMode
+
+ProtFlatMode:
+    mov eax, strict dword 0
+StmSmiCr3Patch:
+    mov     cr3, eax
+;
+; Need to test for CR4 specific bit support
+;
+    mov     eax, 1
+    cpuid                               ; use CPUID to determine if specific CR4 bits are supported
+    xor     eax, eax                    ; Clear EAX
+    test    edx, BIT2                   ; Check for DE capabilities
+    jz      .0
+    or      eax, BIT3
+.0:
+    test    edx, BIT6                   ; Check for PAE capabilities
+    jz      .1
+    or      eax, BIT5
+.1:
+    test    edx, BIT7                   ; Check for MCE capabilities
+    jz      .2
+    or      eax, BIT6
+.2:
+    test    edx, BIT24                  ; Check for FXSR capabilities
+    jz      .3
+    or      eax, BIT9
+.3:
+    test    edx, BIT25                  ; Check for SSE capabilities
+    jz      .4
+    or      eax, BIT10
+.4:                                     ; as cr4.PGE is not set here, refresh cr3
+    mov     cr4, eax                    ; in PreModifyMtrrs() to flush TLB.
+
+    cmp     byte [dword ASM_PFX(FeaturePcdGet (PcdCpuSmmStackGuard))], 0
+    jz      .6
+; Load TSS
+    mov     byte [ebp + TSS_SEGMENT + 5], 0x89 ; clear busy flag
+    mov     eax, TSS_SEGMENT
+    ltr     ax
+.6:
+
+; enable NXE if supported
+    mov     al, strict byte 1
+StmXdSupportedPatch:
+    cmp     al, 0
+    jz      @SkipXd
+;
+; Check XD disable bit
+;
+    mov     ecx, MSR_IA32_MISC_ENABLE
+    rdmsr
+    push    edx                        ; save MSR_IA32_MISC_ENABLE[63-32]
+    test    edx, BIT2                  ; MSR_IA32_MISC_ENABLE[34]
+    jz      .5
+    and     dx, 0xFFFB                 ; clear XD Disable bit if it is set
+    wrmsr
+.5:
+    mov     ecx, MSR_EFER
+    rdmsr
+    or      ax, MSR_EFER_XD             ; enable NXE
+    wrmsr
+    jmp     @XdDone
+@SkipXd:
+    sub     esp, 4
+@XdDone:
+
+    mov     ebx, cr0
+    or      ebx, 0x80010023             ; enable paging + WP + NE + MP + PE
+    mov     cr0, ebx
+    lea     ebx, [edi + DSC_OFFSET]
+    mov     ax, [ebx + DSC_DS]
+    mov     ds, eax
+    mov     ax, [ebx + DSC_OTHERSEG]
+    mov     es, eax
+    mov     fs, eax
+    mov     gs, eax
+    mov     ax, [ebx + DSC_SS]
+    mov     ss, eax
+
+CommonHandler:
+    mov     ebx, [esp + 4]                  ; CPU Index
+    push    ebx
+    mov     eax, ASM_PFX(CpuSmmDebugEntry)
+    call    eax
+    add     esp, 4
+
+    push    ebx
+    mov     eax, ASM_PFX(SmiRendezvous)
+    call    eax
+    add     esp, 4
+
+    push    ebx
+    mov     eax, ASM_PFX(CpuSmmDebugExit)
+    call    eax
+    add     esp, 4
+
+    mov     eax, ASM_PFX(gStmXdSupported)
+    mov     al, [eax]
+    cmp     al, 0
+    jz      .7
+    pop     edx                       ; get saved MSR_IA32_MISC_ENABLE[63-32]
+    test    edx, BIT2
+    jz      .7
+    mov     ecx, MSR_IA32_MISC_ENABLE
+    rdmsr
+    or      dx, BIT2                  ; set XD Disable bit if it was set before entering into SMM
+    wrmsr
+
+.7:
+    StuffRsb32
+    rsm
+
+
+_StmSmiHandler:
+;
+; Check XD disable bit
+;
+    xor     esi, esi
+    mov     eax, ASM_PFX(gStmXdSupported)
+    mov     al, [eax]
+    cmp     al, 0
+    jz      @StmXdDone
+    mov     ecx, MSR_IA32_MISC_ENABLE
+    rdmsr
+    mov     esi, edx                   ; save MSR_IA32_MISC_ENABLE[63-32]
+    test    edx, BIT2                  ; MSR_IA32_MISC_ENABLE[34]
+    jz      .5
+    and     dx, 0xFFFB                 ; clear XD Disable bit if it is set
+    wrmsr
+.5:
+    mov     ecx, MSR_EFER
+    rdmsr
+    or      ax, MSR_EFER_XD             ; enable NXE
+    wrmsr
+@StmXdDone:
+    push    esi
+
+    ; below step is needed, because STM does not run above code.
+    ; we have to run below code to set IDT/CR0/CR4
+    mov     eax, ASM_PFX(gStmSmiHandlerIdtr)
+    lidt    [eax]
+
+    mov     eax, cr0
+    or      eax, 0x80010023             ; enable paging + WP + NE + MP + PE
+    mov     cr0, eax
+;
+; Need to test for CR4 specific bit support
+;
+    mov     eax, 1
+    cpuid                               ; use CPUID to determine if specific CR4 bits are supported
+    mov     eax, cr4                    ; init EAX
+    test    edx, BIT2                   ; Check for DE capabilities
+    jz      .0
+    or      eax, BIT3
+.0:
+    test    edx, BIT6                   ; Check for PAE capabilities
+    jz      .1
+    or      eax, BIT5
+.1:
+    test    edx, BIT7                   ; Check for MCE capabilities
+    jz      .2
+    or      eax, BIT6
+.2:
+    test    edx, BIT24                  ; Check for FXSR capabilities
+    jz      .3
+    or      eax, BIT9
+.3:
+    test    edx, BIT25                  ; Check for SSE capabilities
+    jz      .4
+    or      eax, BIT10
+.4:                                     ; as cr4.PGE is not set here, refresh cr3
+    mov     cr4, eax                    ; in PreModifyMtrrs() to flush TLB.
+    ; STM init finish
+    jmp     CommonHandler
+
+ASM_PFX(gcStmSmiHandlerSize)   : DW        $ - _StmSmiEntryPoint
+ASM_PFX(gcStmSmiHandlerOffset) : DW        _StmSmiHandler - _StmSmiEntryPoint
+
+global ASM_PFX(SmmCpuFeaturesLibStmSmiEntryFixupAddress)
+ASM_PFX(SmmCpuFeaturesLibStmSmiEntryFixupAddress):
+    ret
diff --git a/roms/edk2/UefiCpuPkg/Library/SmmCpuFeaturesLib/Ia32/SmiException.nasm b/roms/edk2/UefiCpuPkg/Library/SmmCpuFeaturesLib/Ia32/SmiException.nasm
new file mode 100644
index 000000000..b9c7e0bb2
--- /dev/null
+++ b/roms/edk2/UefiCpuPkg/Library/SmmCpuFeaturesLib/Ia32/SmiException.nasm
@@ -0,0 +1,173 @@
+;------------------------------------------------------------------------------ ;
+; Copyright (c) 2009 - 2018, Intel Corporation. All rights reserved.<BR>
+; SPDX-License-Identifier: BSD-2-Clause-Patent
+;
+; Module Name:
+;
+;   SmiException.nasm
+;
+; Abstract:
+;
+;   Exception handlers used in SM mode
+;
+;-------------------------------------------------------------------------------
+
+%include "StuffRsbNasm.inc"
+
+global  ASM_PFX(gcStmPsd)
+
+extern  ASM_PFX(SmmStmExceptionHandler)
+extern  ASM_PFX(SmmStmSetup)
+extern  ASM_PFX(SmmStmTeardown)
+extern  ASM_PFX(gStmXdSupported)
+extern  ASM_PFX(gStmSmiHandlerIdtr)
+
+%define MSR_IA32_MISC_ENABLE 0x1A0
+%define MSR_EFER      0xc0000080
+%define MSR_EFER_XD   0x800
+
+CODE_SEL          equ 0x08
+DATA_SEL          equ 0x20
+TSS_SEL           equ 0x40
+
+    SECTION .data
+
+ASM_PFX(gcStmPsd):
+            DB      'TXTPSSIG'
+            DW      PSD_SIZE
+            DW      1              ; Version
+            DD      0              ; LocalApicId
+            DB      0x05           ; Cr4Pse;Cr4Pae;Intel64Mode;ExecutionDisableOutsideSmrr
+            DB      0              ; BIOS to STM
+            DB      0              ; STM to BIOS
+            DB      0
+            DW      CODE_SEL
+            DW      DATA_SEL
+            DW      DATA_SEL
+            DW      DATA_SEL
+            DW      TSS_SEL
+            DW      0
+            DQ      0              ; SmmCr3
+            DD      ASM_PFX(OnStmSetup)
+            DD      0
+            DD      ASM_PFX(OnStmTeardown)
+            DD      0
+            DQ      0              ; SmmSmiHandlerRip - SMM guest entrypoint
+            DQ      0              ; SmmSmiHandlerRsp
+            DQ      0
+            DD      0
+            DD      0x80010100     ; RequiredStmSmmRevId
+            DD      ASM_PFX(OnException)
+            DD      0
+            DQ      0              ; ExceptionStack
+            DW      DATA_SEL
+            DW      0x01F          ; ExceptionFilter
+            DD      0
+            DD      0
+            DD      0
+            DQ      0              ; BiosHwResourceRequirementsPtr
+            DQ      0              ; AcpiRsdp
+            DB      0              ; PhysicalAddressBits
+PSD_SIZE  equ $ - ASM_PFX(gcStmPsd)
+
+    SECTION .text
+;------------------------------------------------------------------------------
+; SMM Exception handlers
+;------------------------------------------------------------------------------
+global ASM_PFX(OnException)
+ASM_PFX(OnException):
+    mov  ecx, esp
+    push ecx
+    call ASM_PFX(SmmStmExceptionHandler)
+    add  esp, 4
+
+    mov  ebx, eax
+    mov  eax, 4
+    vmcall
+    jmp $
+
+global ASM_PFX(OnStmSetup)
+ASM_PFX(OnStmSetup):
+;
+; Check XD disable bit
+;
+    xor     esi, esi
+    mov     eax, ASM_PFX(gStmXdSupported)
+    mov     al, [eax]
+    cmp     al, 0
+    jz      @StmXdDone1
+    mov     ecx, MSR_IA32_MISC_ENABLE
+    rdmsr
+    mov     esi, edx                   ; save MSR_IA32_MISC_ENABLE[63-32]
+    test    edx, BIT2                  ; MSR_IA32_MISC_ENABLE[34]
+    jz      .51
+    and     dx, 0xFFFB                 ; clear XD Disable bit if it is set
+    wrmsr
+.51:
+    mov     ecx, MSR_EFER
+    rdmsr
+    or      ax, MSR_EFER_XD             ; enable NXE
+    wrmsr
+@StmXdDone1:
+    push    esi
+
+  call ASM_PFX(SmmStmSetup)
+
+    mov     eax, ASM_PFX(gStmXdSupported)
+    mov     al, [eax]
+    cmp     al, 0
+    jz      .71
+    pop     edx                       ; get saved MSR_IA32_MISC_ENABLE[63-32]
+    test    edx, BIT2
+    jz      .71
+    mov     ecx, MSR_IA32_MISC_ENABLE
+    rdmsr
+    or      dx, BIT2                  ; set XD Disable bit if it was set before entering into SMM
+    wrmsr
+
+.71:
+    StuffRsb32
+    rsm
+
+global  ASM_PFX(OnStmTeardown)
+ASM_PFX(OnStmTeardown):
+;
+; Check XD disable bit
+;
+    xor     esi, esi
+    mov     eax, ASM_PFX(gStmXdSupported)
+    mov     al, [eax]
+    cmp     al, 0
+    jz      @StmXdDone2
+    mov     ecx, MSR_IA32_MISC_ENABLE
+    rdmsr
+    mov     esi, edx                   ; save MSR_IA32_MISC_ENABLE[63-32]
+    test    edx, BIT2                  ; MSR_IA32_MISC_ENABLE[34]
+    jz      .52
+    and     dx, 0xFFFB                 ; clear XD Disable bit if it is set
+    wrmsr
+.52:
+    mov     ecx, MSR_EFER
+    rdmsr
+    or      ax, MSR_EFER_XD             ; enable NXE
+    wrmsr
+@StmXdDone2:
+    push    esi
+
+  call ASM_PFX(SmmStmTeardown)
+
+    mov     eax, ASM_PFX(gStmXdSupported)
+    mov     al, [eax]
+    cmp     al, 0
+    jz      .72
+    pop     edx                       ; get saved MSR_IA32_MISC_ENABLE[63-32]
+    test    edx, BIT2
+    jz      .72
+    mov     ecx, MSR_IA32_MISC_ENABLE
+    rdmsr
+    or      dx, BIT2                  ; set XD Disable bit if it was set before entering into SMM
+    wrmsr
+
+.72:
+    StuffRsb32
+    rsm
diff --git a/roms/edk2/UefiCpuPkg/Library/SmmCpuFeaturesLib/Ia32/SmmStmSupport.c b/roms/edk2/UefiCpuPkg/Library/SmmCpuFeaturesLib/Ia32/SmmStmSupport.c
new file mode 100644
index 000000000..399ddd742
--- /dev/null
+++ b/roms/edk2/UefiCpuPkg/Library/SmmCpuFeaturesLib/Ia32/SmmStmSupport.c
@@ -0,0 +1,77 @@
+/** @file
+  SMM STM support functions
+
+  Copyright (c) 2015 - 2016, Intel Corporation. All rights reserved.<BR>
+  SPDX-License-Identifier: BSD-2-Clause-Patent
+
+**/
+
+#include <PiSmm.h>
+#include <Library/DebugLib.h>
+
+#include "SmmStm.h"
+
+///
+/// Page Table Entry
+///
+#define IA32_PG_P                   BIT0
+#define IA32_PG_RW                  BIT1
+#define IA32_PG_PS                  BIT7
+
+/**
+
+  Create 4G page table for STM.
+  4M Non-PAE page table in IA32 version.
+
+  @param PageTableBase        The page table base in MSEG
+
+**/
+VOID
+StmGen4GPageTable (
+  IN UINTN              PageTableBase
+  )
+{
+  UINTN                             Index;
+  UINT32                            *Pte;
+  UINT32                            Address;
+
+  Pte = (UINT32*)(UINTN)PageTableBase;
+
+  Address = 0;
+  for (Index = 0; Index < SIZE_4KB / sizeof (*Pte); Index++) {
+    *Pte = Address | IA32_PG_PS | IA32_PG_RW | IA32_PG_P;
+    Pte++;
+    Address += SIZE_4MB;
+  }
+}
+
+/**
+  This is SMM exception handle.
+  Consumed by STM when exception happen.
+
+  @param Context  STM protection exception stack frame
+
+  @return the EBX value for STM reference.
+          EBX = 0: resume SMM guest using register state found on exception stack.
+          EBX = 1 to 0x0F: EBX contains a BIOS error code which the STM must record in the
+                           TXT.ERRORCODE register and subsequently reset the system via
+                           TXT.CMD.SYS_RESET. The value of the TXT.ERRORCODE register is calculated as
+                           follows: TXT.ERRORCODE = (EBX & 0x0F) | STM_CRASH_BIOS_PANIC
+          EBX = 0x10 to 0xFFFFFFFF - reserved, do not use.
+
+**/
+UINT32
+EFIAPI
+SmmStmExceptionHandler (
+  IN OUT STM_PROTECTION_EXCEPTION_STACK_FRAME Context
+  )
+{
+  // TBD - SmmStmExceptionHandler, record information
+  DEBUG ((DEBUG_ERROR, "SmmStmExceptionHandler ...\n"));
+  //
+  // Skip this instruction and continue;
+  //
+  Context.Ia32StackFrame->Rip += Context.Ia32StackFrame->VmcsExitInstructionLength;
+
+  return 0;
+}
diff --git a/roms/edk2/UefiCpuPkg/Library/SmmCpuFeaturesLib/SmmCpuFeaturesLib.c b/roms/edk2/UefiCpuPkg/Library/SmmCpuFeaturesLib/SmmCpuFeaturesLib.c
new file mode 100644
index 000000000..a00786a8e
--- /dev/null
+++ b/roms/edk2/UefiCpuPkg/Library/SmmCpuFeaturesLib/SmmCpuFeaturesLib.c
@@ -0,0 +1,626 @@
+/** @file
+The CPU specific programming for PiSmmCpuDxeSmm module.
+
+Copyright (c) 2010 - 2019, Intel Corporation. All rights reserved.<BR>
+SPDX-License-Identifier: BSD-2-Clause-Patent
+
+**/
+
+#include <PiSmm.h>
+#include <Library/SmmCpuFeaturesLib.h>
+#include <Library/BaseLib.h>
+#include <Library/MtrrLib.h>
+#include <Library/PcdLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/DebugLib.h>
+#include <Register/Intel/Cpuid.h>
+#include <Register/Intel/SmramSaveStateMap.h>
+
+//
+// Machine Specific Registers (MSRs)
+//
+#define  SMM_FEATURES_LIB_IA32_MTRR_CAP            0x0FE
+#define  SMM_FEATURES_LIB_IA32_FEATURE_CONTROL     0x03A
+#define  SMM_FEATURES_LIB_IA32_SMRR_PHYSBASE       0x1F2
+#define  SMM_FEATURES_LIB_IA32_SMRR_PHYSMASK       0x1F3
+#define  SMM_FEATURES_LIB_IA32_CORE_SMRR_PHYSBASE  0x0A0
+#define  SMM_FEATURES_LIB_IA32_CORE_SMRR_PHYSMASK  0x0A1
+#define    EFI_MSR_SMRR_MASK                       0xFFFFF000
+#define    EFI_MSR_SMRR_PHYS_MASK_VALID            BIT11
+#define  SMM_FEATURES_LIB_SMM_FEATURE_CONTROL      0x4E0
+
+//
+// MSRs required for configuration of SMM Code Access Check
+//
+#define SMM_FEATURES_LIB_IA32_MCA_CAP              0x17D
+#define   SMM_CODE_ACCESS_CHK_BIT                  BIT58
+
+/**
+  Internal worker function that is called to complete CPU initialization at the
+  end of SmmCpuFeaturesInitializeProcessor().
+
+**/
+VOID
+FinishSmmCpuFeaturesInitializeProcessor (
+  VOID
+  );
+
+//
+// Set default value to assume SMRR is not supported
+//
+BOOLEAN  mSmrrSupported = FALSE;
+
+//
+// Set default value to assume MSR_SMM_FEATURE_CONTROL is not supported
+//
+BOOLEAN  mSmmFeatureControlSupported = FALSE;
+
+//
+// Set default value to assume IA-32 Architectural MSRs are used
+//
+UINT32  mSmrrPhysBaseMsr = SMM_FEATURES_LIB_IA32_SMRR_PHYSBASE;
+UINT32  mSmrrPhysMaskMsr = SMM_FEATURES_LIB_IA32_SMRR_PHYSMASK;
+
+//
+// Set default value to assume MTRRs need to be configured on each SMI
+//
+BOOLEAN  mNeedConfigureMtrrs = TRUE;
+
+//
+// Array for state of SMRR enable on all CPUs
+//
+BOOLEAN  *mSmrrEnabled;
+
+/**
+  The constructor function
+
+  @param[in]  ImageHandle  The firmware allocated handle for the EFI image.
+  @param[in]  SystemTable  A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS      The constructor always returns EFI_SUCCESS.
+
+**/
+EFI_STATUS
+EFIAPI
+SmmCpuFeaturesLibConstructor (
+  IN EFI_HANDLE        ImageHandle,
+  IN EFI_SYSTEM_TABLE  *SystemTable
+  )
+{
+  UINT32  RegEax;
+  UINT32  RegEdx;
+  UINTN   FamilyId;
+  UINTN   ModelId;
+
+  //
+  // Retrieve CPU Family and Model
+  //
+  AsmCpuid (CPUID_VERSION_INFO, &RegEax, NULL, NULL, &RegEdx);
+  FamilyId = (RegEax >> 8) & 0xf;
+  ModelId  = (RegEax >> 4) & 0xf;
+  if (FamilyId == 0x06 || FamilyId == 0x0f) {
+    ModelId = ModelId | ((RegEax >> 12) & 0xf0);
+  }
+
+  //
+  // Check CPUID(CPUID_VERSION_INFO).EDX[12] for MTRR capability
+  //
+  if ((RegEdx & BIT12) != 0) {
+    //
+    // Check MTRR_CAP MSR bit 11 for SMRR support
+    //
+    if ((AsmReadMsr64 (SMM_FEATURES_LIB_IA32_MTRR_CAP) & BIT11) != 0) {
+      mSmrrSupported = TRUE;
+    }
+  }
+
+  //
+  // Intel(R) 64 and IA-32 Architectures Software Developer's Manual
+  // Volume 3C, Section 35.3 MSRs in the Intel(R) Atom(TM) Processor Family
+  //
+  // If CPU Family/Model is 06_1CH, 06_26H, 06_27H, 06_35H or 06_36H, then
+  // SMRR Physical Base and SMM Physical Mask MSRs are not available.
+  //
+  if (FamilyId == 0x06) {
+    if (ModelId == 0x1C || ModelId == 0x26 || ModelId == 0x27 || ModelId == 0x35 || ModelId == 0x36) {
+      mSmrrSupported = FALSE;
+    }
+  }
+
+  //
+  // Intel(R) 64 and IA-32 Architectures Software Developer's Manual
+  // Volume 3C, Section 35.2 MSRs in the Intel(R) Core(TM) 2 Processor Family
+  //
+  // If CPU Family/Model is 06_0F or 06_17, then use Intel(R) Core(TM) 2
+  // Processor Family MSRs
+  //
+  if (FamilyId == 0x06) {
+    if (ModelId == 0x17 || ModelId == 0x0f) {
+      mSmrrPhysBaseMsr = SMM_FEATURES_LIB_IA32_CORE_SMRR_PHYSBASE;
+      mSmrrPhysMaskMsr = SMM_FEATURES_LIB_IA32_CORE_SMRR_PHYSMASK;
+    }
+  }
+
+  //
+  // Intel(R) 64 and IA-32 Architectures Software Developer's Manual
+  // Volume 3C, Section 34.4.2 SMRAM Caching
+  //   An IA-32 processor does not automatically write back and invalidate its
+  //   caches before entering SMM or before exiting SMM. Because of this behavior,
+  //   care must be taken in the placement of the SMRAM in system memory and in
+  //   the caching of the SMRAM to prevent cache incoherence when switching back
+  //   and forth between SMM and protected mode operation.
+  //
+  // An IA-32 processor is a processor that does not support the Intel 64
+  // Architecture.  Support for the Intel 64 Architecture can be detected from
+  // CPUID(CPUID_EXTENDED_CPU_SIG).EDX[29]
+  //
+  // If an IA-32 processor is detected, then set mNeedConfigureMtrrs to TRUE,
+  // so caches are flushed on SMI entry and SMI exit, the interrupted code
+  // MTRRs are saved/restored, and MTRRs for SMM are loaded.
+  //
+  AsmCpuid (CPUID_EXTENDED_FUNCTION, &RegEax, NULL, NULL, NULL);
+  if (RegEax >= CPUID_EXTENDED_CPU_SIG) {
+    AsmCpuid (CPUID_EXTENDED_CPU_SIG, NULL, NULL, NULL, &RegEdx);
+    if ((RegEdx & BIT29) != 0) {
+      mNeedConfigureMtrrs = FALSE;
+    }
+  }
+
+  //
+  // Allocate array for state of SMRR enable on all CPUs
+  //
+  mSmrrEnabled = (BOOLEAN *)AllocatePool (sizeof (BOOLEAN) * PcdGet32 (PcdCpuMaxLogicalProcessorNumber));
+  ASSERT (mSmrrEnabled != NULL);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Called during the very first SMI into System Management Mode to initialize
+  CPU features, including SMBASE, for the currently executing CPU.  Since this
+  is the first SMI, the SMRAM Save State Map is at the default address of
+  SMM_DEFAULT_SMBASE + SMRAM_SAVE_STATE_MAP_OFFSET.  The currently executing
+  CPU is specified by CpuIndex and CpuIndex can be used to access information
+  about the currently executing CPU in the ProcessorInfo array and the
+  HotPlugCpuData data structure.
+
+  @param[in] CpuIndex        The index of the CPU to initialize.  The value
+                             must be between 0 and the NumberOfCpus field in
+                             the System Management System Table (SMST).
+  @param[in] IsMonarch       TRUE if the CpuIndex is the index of the CPU that
+                             was elected as monarch during System Management
+                             Mode initialization.
+                             FALSE if the CpuIndex is not the index of the CPU
+                             that was elected as monarch during System
+                             Management Mode initialization.
+  @param[in] ProcessorInfo   Pointer to an array of EFI_PROCESSOR_INFORMATION
+                             structures.  ProcessorInfo[CpuIndex] contains the
+                             information for the currently executing CPU.
+  @param[in] CpuHotPlugData  Pointer to the CPU_HOT_PLUG_DATA structure that
+                             contains the ApidId and SmBase arrays.
+**/
+VOID
+EFIAPI
+SmmCpuFeaturesInitializeProcessor (
+  IN UINTN                      CpuIndex,
+  IN BOOLEAN                    IsMonarch,
+  IN EFI_PROCESSOR_INFORMATION  *ProcessorInfo,
+  IN CPU_HOT_PLUG_DATA          *CpuHotPlugData
+  )
+{
+  SMRAM_SAVE_STATE_MAP  *CpuState;
+  UINT64                FeatureControl;
+  UINT32                RegEax;
+  UINT32                RegEdx;
+  UINTN                 FamilyId;
+  UINTN                 ModelId;
+
+  //
+  // Configure SMBASE.
+  //
+  CpuState = (SMRAM_SAVE_STATE_MAP *)(UINTN)(SMM_DEFAULT_SMBASE + SMRAM_SAVE_STATE_MAP_OFFSET);
+  CpuState->x86.SMBASE = (UINT32)CpuHotPlugData->SmBase[CpuIndex];
+
+  //
+  // Intel(R) 64 and IA-32 Architectures Software Developer's Manual
+  // Volume 3C, Section 35.2 MSRs in the Intel(R) Core(TM) 2 Processor Family
+  //
+  // If Intel(R) Core(TM) Core(TM) 2 Processor Family MSRs are being used, then
+  // make sure SMRR Enable(BIT3) of MSR_FEATURE_CONTROL MSR(0x3A) is set before
+  // accessing SMRR base/mask MSRs.  If Lock(BIT0) of MSR_FEATURE_CONTROL MSR(0x3A)
+  // is set, then the MSR is locked and can not be modified.
+  //
+  if (mSmrrSupported && mSmrrPhysBaseMsr == SMM_FEATURES_LIB_IA32_CORE_SMRR_PHYSBASE) {
+    FeatureControl = AsmReadMsr64 (SMM_FEATURES_LIB_IA32_FEATURE_CONTROL);
+    if ((FeatureControl & BIT3) == 0) {
+      if ((FeatureControl & BIT0) == 0) {
+        AsmWriteMsr64 (SMM_FEATURES_LIB_IA32_FEATURE_CONTROL, FeatureControl | BIT3);
+      } else {
+        mSmrrSupported = FALSE;
+      }
+    }
+  }
+
+  //
+  // If SMRR is supported, then program SMRR base/mask MSRs.
+  // The EFI_MSR_SMRR_PHYS_MASK_VALID bit is not set until the first normal SMI.
+  // The code that initializes SMM environment is running in normal mode
+  // from SMRAM region.  If SMRR is enabled here, then the SMRAM region
+  // is protected and the normal mode code execution will fail.
+  //
+  if (mSmrrSupported) {
+    //
+    // SMRR size cannot be less than 4-KBytes
+    // SMRR size must be of length 2^n
+    // SMRR base alignment cannot be less than SMRR length
+    //
+    if ((CpuHotPlugData->SmrrSize < SIZE_4KB) ||
+        (CpuHotPlugData->SmrrSize != GetPowerOfTwo32 (CpuHotPlugData->SmrrSize)) ||
+        ((CpuHotPlugData->SmrrBase & ~(CpuHotPlugData->SmrrSize - 1)) != CpuHotPlugData->SmrrBase)) {
+      //
+      // Print message and halt if CPU is Monarch
+      //
+      if (IsMonarch) {
+        DEBUG ((DEBUG_ERROR, "SMM Base/Size does not meet alignment/size requirement!\n"));
+        CpuDeadLoop ();
+      }
+    } else {
+      AsmWriteMsr64 (mSmrrPhysBaseMsr, CpuHotPlugData->SmrrBase | MTRR_CACHE_WRITE_BACK);
+      AsmWriteMsr64 (mSmrrPhysMaskMsr, (~(CpuHotPlugData->SmrrSize - 1) & EFI_MSR_SMRR_MASK));
+      mSmrrEnabled[CpuIndex] = FALSE;
+    }
+  }
+
+  //
+  // Retrieve CPU Family and Model
+  //
+  AsmCpuid (CPUID_VERSION_INFO, &RegEax, NULL, NULL, &RegEdx);
+  FamilyId = (RegEax >> 8) & 0xf;
+  ModelId  = (RegEax >> 4) & 0xf;
+  if (FamilyId == 0x06 || FamilyId == 0x0f) {
+    ModelId = ModelId | ((RegEax >> 12) & 0xf0);
+  }
+
+  //
+  // Intel(R) 64 and IA-32 Architectures Software Developer's Manual
+  // Volume 3C, Section 35.10.1 MSRs in 4th Generation Intel(R) Core(TM)
+  // Processor Family.
+  //
+  // If CPU Family/Model is 06_3C, 06_45, or 06_46 then use 4th Generation
+  // Intel(R) Core(TM) Processor Family MSRs.
+  //
+  if (FamilyId == 0x06) {
+    if (ModelId == 0x3C || ModelId == 0x45 || ModelId == 0x46 ||
+        ModelId == 0x3D || ModelId == 0x47 || ModelId == 0x4E || ModelId == 0x4F ||
+        ModelId == 0x3F || ModelId == 0x56 || ModelId == 0x57 || ModelId == 0x5C) {
+      //
+      // Check to see if the CPU supports the SMM Code Access Check feature
+      // Do not access this MSR unless the CPU supports the SmmRegFeatureControl
+      //
+      if ((AsmReadMsr64 (SMM_FEATURES_LIB_IA32_MCA_CAP) & SMM_CODE_ACCESS_CHK_BIT) != 0) {
+        mSmmFeatureControlSupported = TRUE;
+      }
+    }
+  }
+
+  //
+  //  Call internal worker function that completes the CPU initialization
+  //
+  FinishSmmCpuFeaturesInitializeProcessor ();
+}
+
+/**
+  This function updates the SMRAM save state on the currently executing CPU
+  to resume execution at a specific address after an RSM instruction.  This
+  function must evaluate the SMRAM save state to determine the execution mode
+  the RSM instruction resumes and update the resume execution address with
+  either NewInstructionPointer32 or NewInstructionPoint.  The auto HALT restart
+  flag in the SMRAM save state must always be cleared.  This function returns
+  the value of the instruction pointer from the SMRAM save state that was
+  replaced.  If this function returns 0, then the SMRAM save state was not
+  modified.
+
+  This function is called during the very first SMI on each CPU after
+  SmmCpuFeaturesInitializeProcessor() to set a flag in normal execution mode
+  to signal that the SMBASE of each CPU has been updated before the default
+  SMBASE address is used for the first SMI to the next CPU.
+
+  @param[in] CpuIndex                 The index of the CPU to hook.  The value
+                                      must be between 0 and the NumberOfCpus
+                                      field in the System Management System Table
+                                      (SMST).
+  @param[in] CpuState                 Pointer to SMRAM Save State Map for the
+                                      currently executing CPU.
+  @param[in] NewInstructionPointer32  Instruction pointer to use if resuming to
+                                      32-bit execution mode from 64-bit SMM.
+  @param[in] NewInstructionPointer    Instruction pointer to use if resuming to
+                                      same execution mode as SMM.
+
+  @retval 0    This function did modify the SMRAM save state.
+  @retval > 0  The original instruction pointer value from the SMRAM save state
+               before it was replaced.
+**/
+UINT64
+EFIAPI
+SmmCpuFeaturesHookReturnFromSmm (
+  IN UINTN                 CpuIndex,
+  IN SMRAM_SAVE_STATE_MAP  *CpuState,
+  IN UINT64                NewInstructionPointer32,
+  IN UINT64                NewInstructionPointer
+  )
+{
+  return 0;
+}
+
+/**
+  Hook point in normal execution mode that allows the one CPU that was elected
+  as monarch during System Management Mode initialization to perform additional
+  initialization actions immediately after all of the CPUs have processed their
+  first SMI and called SmmCpuFeaturesInitializeProcessor() relocating SMBASE
+  into a buffer in SMRAM and called SmmCpuFeaturesHookReturnFromSmm().
+**/
+VOID
+EFIAPI
+SmmCpuFeaturesSmmRelocationComplete (
+  VOID
+  )
+{
+}
+
+/**
+  Determines if MTRR registers must be configured to set SMRAM cache-ability
+  when executing in System Management Mode.
+
+  @retval TRUE   MTRR registers must be configured to set SMRAM cache-ability.
+  @retval FALSE  MTRR registers do not need to be configured to set SMRAM
+                 cache-ability.
+**/
+BOOLEAN
+EFIAPI
+SmmCpuFeaturesNeedConfigureMtrrs (
+  VOID
+  )
+{
+  return mNeedConfigureMtrrs;
+}
+
+/**
+  Disable SMRR register if SMRR is supported and SmmCpuFeaturesNeedConfigureMtrrs()
+  returns TRUE.
+**/
+VOID
+EFIAPI
+SmmCpuFeaturesDisableSmrr (
+  VOID
+  )
+{
+  if (mSmrrSupported && mNeedConfigureMtrrs) {
+    AsmWriteMsr64 (mSmrrPhysMaskMsr, AsmReadMsr64(mSmrrPhysMaskMsr) & ~EFI_MSR_SMRR_PHYS_MASK_VALID);
+  }
+}
+
+/**
+  Enable SMRR register if SMRR is supported and SmmCpuFeaturesNeedConfigureMtrrs()
+  returns TRUE.
+**/
+VOID
+EFIAPI
+SmmCpuFeaturesReenableSmrr (
+  VOID
+  )
+{
+  if (mSmrrSupported && mNeedConfigureMtrrs) {
+    AsmWriteMsr64 (mSmrrPhysMaskMsr, AsmReadMsr64(mSmrrPhysMaskMsr) | EFI_MSR_SMRR_PHYS_MASK_VALID);
+  }
+}
+
+/**
+  Processor specific hook point each time a CPU enters System Management Mode.
+
+  @param[in] CpuIndex  The index of the CPU that has entered SMM.  The value
+                       must be between 0 and the NumberOfCpus field in the
+                       System Management System Table (SMST).
+**/
+VOID
+EFIAPI
+SmmCpuFeaturesRendezvousEntry (
+  IN UINTN  CpuIndex
+  )
+{
+  //
+  // If SMRR is supported and this is the first normal SMI, then enable SMRR
+  //
+  if (mSmrrSupported && !mSmrrEnabled[CpuIndex]) {
+    AsmWriteMsr64 (mSmrrPhysMaskMsr, AsmReadMsr64 (mSmrrPhysMaskMsr) | EFI_MSR_SMRR_PHYS_MASK_VALID);
+    mSmrrEnabled[CpuIndex] = TRUE;
+  }
+}
+
+/**
+  Processor specific hook point each time a CPU exits System Management Mode.
+
+  @param[in] CpuIndex  The index of the CPU that is exiting SMM.  The value must
+                       be between 0 and the NumberOfCpus field in the System
+                       Management System Table (SMST).
+**/
+VOID
+EFIAPI
+SmmCpuFeaturesRendezvousExit (
+  IN UINTN  CpuIndex
+  )
+{
+}
+
+/**
+  Check to see if an SMM register is supported by a specified CPU.
+
+  @param[in] CpuIndex  The index of the CPU to check for SMM register support.
+                       The value must be between 0 and the NumberOfCpus field
+                       in the System Management System Table (SMST).
+  @param[in] RegName   Identifies the SMM register to check for support.
+
+  @retval TRUE   The SMM register specified by RegName is supported by the CPU
+                 specified by CpuIndex.
+  @retval FALSE  The SMM register specified by RegName is not supported by the
+                 CPU specified by CpuIndex.
+**/
+BOOLEAN
+EFIAPI
+SmmCpuFeaturesIsSmmRegisterSupported (
+  IN UINTN         CpuIndex,
+  IN SMM_REG_NAME  RegName
+  )
+{
+  if (mSmmFeatureControlSupported && RegName == SmmRegFeatureControl) {
+    return TRUE;
+  }
+  return FALSE;
+}
+
+/**
+  Returns the current value of the SMM register for the specified CPU.
+  If the SMM register is not supported, then 0 is returned.
+
+  @param[in] CpuIndex  The index of the CPU to read the SMM register.  The
+                       value must be between 0 and the NumberOfCpus field in
+                       the System Management System Table (SMST).
+  @param[in] RegName   Identifies the SMM register to read.
+
+  @return  The value of the SMM register specified by RegName from the CPU
+           specified by CpuIndex.
+**/
+UINT64
+EFIAPI
+SmmCpuFeaturesGetSmmRegister (
+  IN UINTN         CpuIndex,
+  IN SMM_REG_NAME  RegName
+  )
+{
+  if (mSmmFeatureControlSupported && RegName == SmmRegFeatureControl) {
+    return AsmReadMsr64 (SMM_FEATURES_LIB_SMM_FEATURE_CONTROL);
+  }
+  return 0;
+}
+
+/**
+  Sets the value of an SMM register on a specified CPU.
+  If the SMM register is not supported, then no action is performed.
+
+  @param[in] CpuIndex  The index of the CPU to write the SMM register.  The
+                       value must be between 0 and the NumberOfCpus field in
+                       the System Management System Table (SMST).
+  @param[in] RegName   Identifies the SMM register to write.
+                       registers are read-only.
+  @param[in] Value     The value to write to the SMM register.
+**/
+VOID
+EFIAPI
+SmmCpuFeaturesSetSmmRegister (
+  IN UINTN         CpuIndex,
+  IN SMM_REG_NAME  RegName,
+  IN UINT64        Value
+  )
+{
+  if (mSmmFeatureControlSupported && RegName == SmmRegFeatureControl) {
+    AsmWriteMsr64 (SMM_FEATURES_LIB_SMM_FEATURE_CONTROL, Value);
+  }
+}
+
+/**
+  Read an SMM Save State register on the target processor.  If this function
+  returns EFI_UNSUPPORTED, then the caller is responsible for reading the
+  SMM Save Sate register.
+
+  @param[in]  CpuIndex  The index of the CPU to read the SMM Save State.  The
+                        value must be between 0 and the NumberOfCpus field in
+                        the System Management System Table (SMST).
+  @param[in]  Register  The SMM Save State register to read.
+  @param[in]  Width     The number of bytes to read from the CPU save state.
+  @param[out] Buffer    Upon return, this holds the CPU register value read
+                        from the save state.
+
+  @retval EFI_SUCCESS           The register was read from Save State.
+  @retval EFI_INVALID_PARAMETER  Buffer is NULL.
+  @retval EFI_UNSUPPORTED       This function does not support reading Register.
+
+**/
+EFI_STATUS
+EFIAPI
+SmmCpuFeaturesReadSaveStateRegister (
+  IN  UINTN                        CpuIndex,
+  IN  EFI_SMM_SAVE_STATE_REGISTER  Register,
+  IN  UINTN                        Width,
+  OUT VOID                         *Buffer
+  )
+{
+  return EFI_UNSUPPORTED;
+}
+
+/**
+  Writes an SMM Save State register on the target processor.  If this function
+  returns EFI_UNSUPPORTED, then the caller is responsible for writing the
+  SMM Save Sate register.
+
+  @param[in] CpuIndex  The index of the CPU to write the SMM Save State.  The
+                       value must be between 0 and the NumberOfCpus field in
+                       the System Management System Table (SMST).
+  @param[in] Register  The SMM Save State register to write.
+  @param[in] Width     The number of bytes to write to the CPU save state.
+  @param[in] Buffer    Upon entry, this holds the new CPU register value.
+
+  @retval EFI_SUCCESS           The register was written to Save State.
+  @retval EFI_INVALID_PARAMETER  Buffer is NULL.
+  @retval EFI_UNSUPPORTED       This function does not support writing Register.
+**/
+EFI_STATUS
+EFIAPI
+SmmCpuFeaturesWriteSaveStateRegister (
+  IN UINTN                        CpuIndex,
+  IN EFI_SMM_SAVE_STATE_REGISTER  Register,
+  IN UINTN                        Width,
+  IN CONST VOID                   *Buffer
+  )
+{
+  return EFI_UNSUPPORTED;
+}
+
+/**
+  This function is hook point called after the gEfiSmmReadyToLockProtocolGuid
+  notification is completely processed.
+**/
+VOID
+EFIAPI
+SmmCpuFeaturesCompleteSmmReadyToLock (
+  VOID
+  )
+{
+}
+
+/**
+  This API provides a method for a CPU to allocate a specific region for storing page tables.
+
+  This API can be called more once to allocate memory for page tables.
+
+  Allocates the number of 4KB pages of type EfiRuntimeServicesData and returns a pointer to the
+  allocated buffer.  The buffer returned is aligned on a 4KB boundary.  If Pages is 0, then NULL
+  is returned.  If there is not enough memory remaining to satisfy the request, then NULL is
+  returned.
+
+  This function can also return NULL if there is no preference on where the page tables are allocated in SMRAM.
+
+  @param  Pages                 The number of 4 KB pages to allocate.
+
+  @return A pointer to the allocated buffer for page tables.
+  @retval NULL      Fail to allocate a specific region for storing page tables,
+                    Or there is no preference on where the page tables are allocated in SMRAM.
+
+**/
+VOID *
+EFIAPI
+SmmCpuFeaturesAllocatePageTableMemory (
+  IN UINTN           Pages
+  )
+{
+  return NULL;
+}
+
diff --git a/roms/edk2/UefiCpuPkg/Library/SmmCpuFeaturesLib/SmmCpuFeaturesLib.inf b/roms/edk2/UefiCpuPkg/Library/SmmCpuFeaturesLib/SmmCpuFeaturesLib.inf
new file mode 100644
index 000000000..dd828baf6
--- /dev/null
+++ b/roms/edk2/UefiCpuPkg/Library/SmmCpuFeaturesLib/SmmCpuFeaturesLib.inf
@@ -0,0 +1,34 @@
+## @file
+#  The CPU specific programming for PiSmmCpuDxeSmm module.
+#
+#  Copyright (c) 2009 - 2016, Intel Corporation. All rights reserved.<BR>
+#  SPDX-License-Identifier: BSD-2-Clause-Patent
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = SmmCpuFeaturesLib
+  MODULE_UNI_FILE                = SmmCpuFeaturesLib.uni
+  FILE_GUID                      = FC3DC10D-D271-422a-AFF3-CBCF70344431
+  MODULE_TYPE                    = DXE_SMM_DRIVER
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = SmmCpuFeaturesLib
+  CONSTRUCTOR                    = SmmCpuFeaturesLibConstructor
+
+[Sources]
+  SmmCpuFeaturesLib.c
+  SmmCpuFeaturesLibNoStm.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  UefiCpuPkg/UefiCpuPkg.dec
+
+[LibraryClasses]
+  BaseLib
+  PcdLib
+  MemoryAllocationLib
+  DebugLib
+
+[Pcd]
+  gUefiCpuPkgTokenSpaceGuid.PcdCpuMaxLogicalProcessorNumber        ## SOMETIMES_CONSUMES
diff --git a/roms/edk2/UefiCpuPkg/Library/SmmCpuFeaturesLib/SmmCpuFeaturesLib.uni b/roms/edk2/UefiCpuPkg/Library/SmmCpuFeaturesLib/SmmCpuFeaturesLib.uni
new file mode 100644
index 000000000..6ee54e925
--- /dev/null
+++ b/roms/edk2/UefiCpuPkg/Library/SmmCpuFeaturesLib/SmmCpuFeaturesLib.uni
@@ -0,0 +1,12 @@
+// /** @file
+// The CPU specific programming for PiSmmCpuDxeSmm module.
+//
+// Copyright (c) 2009 - 2015, Intel Corporation. All rights reserved.<BR>
+//
+// SPDX-License-Identifier: BSD-2-Clause-Patent
+//
+// **/
+
+#string STR_MODULE_ABSTRACT             #language en-US "The CPU specific programming for PiSmmCpuDxeSmm module."
+
+#string STR_MODULE_DESCRIPTION          #language en-US "The CPU specific programming for PiSmmCpuDxeSmm module."
diff --git a/roms/edk2/UefiCpuPkg/Library/SmmCpuFeaturesLib/SmmCpuFeaturesLibNoStm.c b/roms/edk2/UefiCpuPkg/Library/SmmCpuFeaturesLib/SmmCpuFeaturesLibNoStm.c
new file mode 100644
index 000000000..3e63c5e27
--- /dev/null
+++ b/roms/edk2/UefiCpuPkg/Library/SmmCpuFeaturesLib/SmmCpuFeaturesLibNoStm.c
@@ -0,0 +1,83 @@
+/** @file
+The CPU specific programming for PiSmmCpuDxeSmm module when STM support
+is not included.
+
+Copyright (c) 2010 - 2016, Intel Corporation. All rights reserved.<BR>
+SPDX-License-Identifier: BSD-2-Clause-Patent
+
+**/
+
+#include <PiSmm.h>
+#include <Library/SmmCpuFeaturesLib.h>
+
+/**
+  Internal worker function that is called to complete CPU initialization at the
+  end of SmmCpuFeaturesInitializeProcessor().
+
+**/
+VOID
+FinishSmmCpuFeaturesInitializeProcessor (
+  VOID
+  )
+{
+}
+
+/**
+  Return the size, in bytes, of a custom SMI Handler in bytes.  If 0 is
+  returned, then a custom SMI handler is not provided by this library,
+  and the default SMI handler must be used.
+
+  @retval 0    Use the default SMI handler.
+  @retval > 0  Use the SMI handler installed by SmmCpuFeaturesInstallSmiHandler()
+               The caller is required to allocate enough SMRAM for each CPU to
+               support the size of the custom SMI handler.
+**/
+UINTN
+EFIAPI
+SmmCpuFeaturesGetSmiHandlerSize (
+  VOID
+  )
+{
+  return 0;
+}
+
+/**
+  Install a custom SMI handler for the CPU specified by CpuIndex.  This function
+  is only called if SmmCpuFeaturesGetSmiHandlerSize() returns a size is greater
+  than zero and is called by the CPU that was elected as monarch during System
+  Management Mode initialization.
+
+  @param[in] CpuIndex   The index of the CPU to install the custom SMI handler.
+                        The value must be between 0 and the NumberOfCpus field
+                        in the System Management System Table (SMST).
+  @param[in] SmBase     The SMBASE address for the CPU specified by CpuIndex.
+  @param[in] SmiStack   The stack to use when an SMI is processed by the
+                        the CPU specified by CpuIndex.
+  @param[in] StackSize  The size, in bytes, if the stack used when an SMI is
+                        processed by the CPU specified by CpuIndex.
+  @param[in] GdtBase    The base address of the GDT to use when an SMI is
+                        processed by the CPU specified by CpuIndex.
+  @param[in] GdtSize    The size, in bytes, of the GDT used when an SMI is
+                        processed by the CPU specified by CpuIndex.
+  @param[in] IdtBase    The base address of the IDT to use when an SMI is
+                        processed by the CPU specified by CpuIndex.
+  @param[in] IdtSize    The size, in bytes, of the IDT used when an SMI is
+                        processed by the CPU specified by CpuIndex.
+  @param[in] Cr3        The base address of the page tables to use when an SMI
+                        is processed by the CPU specified by CpuIndex.
+**/
+VOID
+EFIAPI
+SmmCpuFeaturesInstallSmiHandler (
+  IN UINTN   CpuIndex,
+  IN UINT32  SmBase,
+  IN VOID    *SmiStack,
+  IN UINTN   StackSize,
+  IN UINTN   GdtBase,
+  IN UINTN   GdtSize,
+  IN UINTN   IdtBase,
+  IN UINTN   IdtSize,
+  IN UINT32  Cr3
+  )
+{
+}
diff --git a/roms/edk2/UefiCpuPkg/Library/SmmCpuFeaturesLib/SmmCpuFeaturesLibStm.inf b/roms/edk2/UefiCpuPkg/Library/SmmCpuFeaturesLib/SmmCpuFeaturesLibStm.inf
new file mode 100644
index 000000000..50b9cc871
--- /dev/null
+++ b/roms/edk2/UefiCpuPkg/Library/SmmCpuFeaturesLib/SmmCpuFeaturesLibStm.inf
@@ -0,0 +1,72 @@
+## @file
+#  The CPU specific programming for PiSmmCpuDxeSmm module when STM support
+#  is included.
+#
+#  Copyright (c) 2009 - 2018, Intel Corporation. All rights reserved.<BR>
+#  SPDX-License-Identifier: BSD-2-Clause-Patent
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = SmmCpuFeaturesLibStm
+  MODULE_UNI_FILE                = SmmCpuFeaturesLib.uni
+  FILE_GUID                      = 374DE830-81C5-4CC8-B2AB-28F0AB73710B
+  MODULE_TYPE                    = DXE_SMM_DRIVER
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = SmmCpuFeaturesLib
+  CONSTRUCTOR                    = SmmCpuFeaturesLibStmConstructor
+
+[Sources]
+  SmmCpuFeaturesLib.c
+  SmmStm.c
+  SmmStm.h
+
+[Sources.Ia32]
+  Ia32/SmmStmSupport.c
+
+
+  Ia32/SmiEntry.nasm
+  Ia32/SmiException.nasm
+
+[Sources.X64]
+  X64/SmmStmSupport.c
+
+
+  X64/SmiEntry.nasm
+  X64/SmiException.nasm
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+  UefiCpuPkg/UefiCpuPkg.dec
+
+[LibraryClasses]
+  BaseLib
+  BaseMemoryLib
+  PcdLib
+  HobLib
+  MemoryAllocationLib
+  DebugLib
+  UefiBootServicesTableLib
+  SmmServicesTableLib
+  TpmMeasurementLib
+
+[Protocols]
+  gEfiMpServiceProtocolGuid                ## CONSUMES
+  gEfiSmmEndOfDxeProtocolGuid              ## CONSUMES
+  gEfiSmMonitorInitProtocolGuid            ## PRODUCES
+
+[Guids]
+  gMsegSmramGuid                           ## SOMETIMES_CONSUMES ## HOB
+  gEfiAcpi20TableGuid                      ## SOMETIMES_CONSUMES ## SystemTable
+  gEfiAcpi10TableGuid                      ## SOMETIMES_CONSUMES ## SystemTable
+
+[Pcd]
+  gUefiCpuPkgTokenSpaceGuid.PcdCpuMaxLogicalProcessorNumber        ## SOMETIMES_CONSUMES
+  gUefiCpuPkgTokenSpaceGuid.PcdCpuMsegSize                         ## SOMETIMES_CONSUMES
+  gUefiCpuPkgTokenSpaceGuid.PcdCpuSmmStmExceptionStackSize         ## SOMETIMES_CONSUMES
+  gUefiCpuPkgTokenSpaceGuid.PcdCpuSmmStackGuard                    ## CONSUMES
+
+[Depex]
+  gEfiMpServiceProtocolGuid
diff --git a/roms/edk2/UefiCpuPkg/Library/SmmCpuFeaturesLib/SmmStm.c b/roms/edk2/UefiCpuPkg/Library/SmmCpuFeaturesLib/SmmStm.c
new file mode 100644
index 000000000..f7f8afacf
--- /dev/null
+++ b/roms/edk2/UefiCpuPkg/Library/SmmCpuFeaturesLib/SmmStm.c
@@ -0,0 +1,1297 @@
+/** @file
+  SMM STM support functions
+
+  Copyright (c) 2015 - 2018, Intel Corporation. All rights reserved.<BR>
+  SPDX-License-Identifier: BSD-2-Clause-Patent
+
+**/
+
+#include <PiSmm.h>
+#include <Library/BaseLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/HobLib.h>
+#include <Library/DebugLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/SmmServicesTableLib.h>
+#include <Library/TpmMeasurementLib.h>
+#include <Register/Intel/Cpuid.h>
+#include <Register/Intel/ArchitecturalMsr.h>
+#include <Register/Intel/SmramSaveStateMap.h>
+
+#include <Protocol/MpService.h>
+
+#include "SmmStm.h"
+
+#define TXT_EVTYPE_BASE                  0x400
+#define TXT_EVTYPE_STM_HASH              (TXT_EVTYPE_BASE + 14)
+
+#define RDWR_ACCS             3
+#define FULL_ACCS             7
+
+/**
+  The constructor function
+
+  @param[in]  ImageHandle  The firmware allocated handle for the EFI image.
+  @param[in]  SystemTable  A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS      The constructor always returns EFI_SUCCESS.
+
+**/
+EFI_STATUS
+EFIAPI
+SmmCpuFeaturesLibConstructor (
+  IN EFI_HANDLE        ImageHandle,
+  IN EFI_SYSTEM_TABLE  *SystemTable
+  );
+
+EFI_HANDLE  mStmSmmCpuHandle = NULL;
+
+BOOLEAN mLockLoadMonitor = FALSE;
+
+//
+// Template of STM_RSC_END structure for copying.
+//
+GLOBAL_REMOVE_IF_UNREFERENCED STM_RSC_END mRscEndNode = {
+  {END_OF_RESOURCES, sizeof (STM_RSC_END)},
+};
+
+GLOBAL_REMOVE_IF_UNREFERENCED UINT8  *mStmResourcesPtr         = NULL;
+GLOBAL_REMOVE_IF_UNREFERENCED UINTN  mStmResourceTotalSize     = 0x0;
+GLOBAL_REMOVE_IF_UNREFERENCED UINTN  mStmResourceSizeUsed      = 0x0;
+GLOBAL_REMOVE_IF_UNREFERENCED UINTN  mStmResourceSizeAvailable = 0x0;
+
+GLOBAL_REMOVE_IF_UNREFERENCED UINT32  mStmState = 0;
+
+//
+// System Configuration Table pointing to STM Configuration Table
+//
+GLOBAL_REMOVE_IF_UNREFERENCED
+EFI_SM_MONITOR_INIT_PROTOCOL mSmMonitorInitProtocol = {
+  LoadMonitor,
+  AddPiResource,
+  DeletePiResource,
+  GetPiResource,
+  GetMonitorState,
+};
+
+
+
+
+#define   CPUID1_EDX_XD_SUPPORT      0x100000
+
+//
+// External global variables associated with SMI Handler Template
+//
+extern CONST TXT_PROCESSOR_SMM_DESCRIPTOR  gcStmPsd;
+extern UINT32                              gStmSmbase;
+extern volatile UINT32                     gStmSmiStack;
+extern UINT32                              gStmSmiCr3;
+extern volatile UINT8                      gcStmSmiHandlerTemplate[];
+extern CONST UINT16                        gcStmSmiHandlerSize;
+extern UINT16                              gcStmSmiHandlerOffset;
+extern BOOLEAN                             gStmXdSupported;
+
+//
+// Variables used by SMI Handler
+//
+IA32_DESCRIPTOR  gStmSmiHandlerIdtr;
+
+//
+// MP Services Protocol
+//
+EFI_MP_SERVICES_PROTOCOL  *mSmmCpuFeaturesLibMpService = NULL;
+
+//
+// MSEG Base and Length in SMRAM
+//
+UINTN  mMsegBase = 0;
+UINTN  mMsegSize = 0;
+
+BOOLEAN  mStmConfigurationTableInitialized = FALSE;
+
+/**
+  The constructor function
+
+  @param[in]  ImageHandle  The firmware allocated handle for the EFI image.
+  @param[in]  SystemTable  A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS      The constructor always returns EFI_SUCCESS.
+
+**/
+EFI_STATUS
+EFIAPI
+SmmCpuFeaturesLibStmConstructor (
+  IN EFI_HANDLE        ImageHandle,
+  IN EFI_SYSTEM_TABLE  *SystemTable
+  )
+{
+  EFI_STATUS              Status;
+  CPUID_VERSION_INFO_ECX  RegEcx;
+  EFI_HOB_GUID_TYPE       *GuidHob;
+  EFI_SMRAM_DESCRIPTOR    *SmramDescriptor;
+
+  //
+  // Initialize address fixup
+  //
+  SmmCpuFeaturesLibStmSmiEntryFixupAddress ();
+
+  //
+  // Call the common constructor function
+  //
+  Status = SmmCpuFeaturesLibConstructor (ImageHandle, SystemTable);
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Lookup the MP Services Protocol
+  //
+  Status = gBS->LocateProtocol (
+                  &gEfiMpServiceProtocolGuid,
+                  NULL,
+                  (VOID **)&mSmmCpuFeaturesLibMpService
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // If CPU supports VMX, then determine SMRAM range for MSEG.
+  //
+  AsmCpuid (CPUID_VERSION_INFO, NULL, NULL, &RegEcx.Uint32, NULL);
+  if (RegEcx.Bits.VMX == 1) {
+    GuidHob = GetFirstGuidHob (&gMsegSmramGuid);
+    if (GuidHob != NULL) {
+      //
+      // Retrieve MSEG location from MSEG SRAM HOB
+      //
+      SmramDescriptor = (EFI_SMRAM_DESCRIPTOR *) GET_GUID_HOB_DATA (GuidHob);
+      if (SmramDescriptor->PhysicalSize > 0) {
+        mMsegBase       = (UINTN)SmramDescriptor->CpuStart;
+        mMsegSize       = (UINTN)SmramDescriptor->PhysicalSize;
+      }
+    } else if (PcdGet32 (PcdCpuMsegSize) > 0) {
+      //
+      // Allocate MSEG from SMRAM memory
+      //
+      mMsegBase = (UINTN)AllocatePages (EFI_SIZE_TO_PAGES (PcdGet32 (PcdCpuMsegSize)));
+      if (mMsegBase > 0) {
+        mMsegSize = ALIGN_VALUE (PcdGet32 (PcdCpuMsegSize), EFI_PAGE_SIZE);
+      } else {
+        DEBUG ((DEBUG_ERROR, "Not enough SMRAM resource to allocate MSEG size %08x\n", PcdGet32 (PcdCpuMsegSize)));
+      }
+    }
+    if (mMsegBase > 0) {
+      DEBUG ((DEBUG_INFO, "MsegBase: 0x%08x, MsegSize: 0x%08x\n", mMsegBase, mMsegSize));
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Internal worker function that is called to complete CPU initialization at the
+  end of SmmCpuFeaturesInitializeProcessor().
+
+**/
+VOID
+FinishSmmCpuFeaturesInitializeProcessor (
+  VOID
+  )
+{
+  MSR_IA32_SMM_MONITOR_CTL_REGISTER  SmmMonitorCtl;
+
+  //
+  // Set MSEG Base Address in SMM Monitor Control MSR.
+  //
+  if (mMsegBase > 0) {
+    SmmMonitorCtl.Uint64        = 0;
+    SmmMonitorCtl.Bits.MsegBase = (UINT32)mMsegBase >> 12;
+    SmmMonitorCtl.Bits.Valid    = 1;
+    AsmWriteMsr64 (MSR_IA32_SMM_MONITOR_CTL, SmmMonitorCtl.Uint64);
+  }
+}
+
+/**
+  Return the size, in bytes, of a custom SMI Handler in bytes.  If 0 is
+  returned, then a custom SMI handler is not provided by this library,
+  and the default SMI handler must be used.
+
+  @retval 0    Use the default SMI handler.
+  @retval > 0  Use the SMI handler installed by SmmCpuFeaturesInstallSmiHandler()
+               The caller is required to allocate enough SMRAM for each CPU to
+               support the size of the custom SMI handler.
+**/
+UINTN
+EFIAPI
+SmmCpuFeaturesGetSmiHandlerSize (
+  VOID
+  )
+{
+  return gcStmSmiHandlerSize;
+}
+
+/**
+  Install a custom SMI handler for the CPU specified by CpuIndex.  This function
+  is only called if SmmCpuFeaturesGetSmiHandlerSize() returns a size is greater
+  than zero and is called by the CPU that was elected as monarch during System
+  Management Mode initialization.
+
+  @param[in] CpuIndex   The index of the CPU to install the custom SMI handler.
+                        The value must be between 0 and the NumberOfCpus field
+                        in the System Management System Table (SMST).
+  @param[in] SmBase     The SMBASE address for the CPU specified by CpuIndex.
+  @param[in] SmiStack   The stack to use when an SMI is processed by the
+                        the CPU specified by CpuIndex.
+  @param[in] StackSize  The size, in bytes, if the stack used when an SMI is
+                        processed by the CPU specified by CpuIndex.
+  @param[in] GdtBase    The base address of the GDT to use when an SMI is
+                        processed by the CPU specified by CpuIndex.
+  @param[in] GdtSize    The size, in bytes, of the GDT used when an SMI is
+                        processed by the CPU specified by CpuIndex.
+  @param[in] IdtBase    The base address of the IDT to use when an SMI is
+                        processed by the CPU specified by CpuIndex.
+  @param[in] IdtSize    The size, in bytes, of the IDT used when an SMI is
+                        processed by the CPU specified by CpuIndex.
+  @param[in] Cr3        The base address of the page tables to use when an SMI
+                        is processed by the CPU specified by CpuIndex.
+**/
+VOID
+EFIAPI
+SmmCpuFeaturesInstallSmiHandler (
+  IN UINTN   CpuIndex,
+  IN UINT32  SmBase,
+  IN VOID    *SmiStack,
+  IN UINTN   StackSize,
+  IN UINTN   GdtBase,
+  IN UINTN   GdtSize,
+  IN UINTN   IdtBase,
+  IN UINTN   IdtSize,
+  IN UINT32  Cr3
+  )
+{
+  EFI_STATUS                     Status;
+  TXT_PROCESSOR_SMM_DESCRIPTOR   *Psd;
+  VOID                           *Hob;
+  UINT32                         RegEax;
+  UINT32                         RegEdx;
+  EFI_PROCESSOR_INFORMATION      ProcessorInfo;
+
+  CopyMem ((VOID *)((UINTN)SmBase + TXT_SMM_PSD_OFFSET), &gcStmPsd, sizeof (gcStmPsd));
+  Psd = (TXT_PROCESSOR_SMM_DESCRIPTOR *)(VOID *)((UINTN)SmBase + TXT_SMM_PSD_OFFSET);
+  Psd->SmmGdtPtr = GdtBase;
+  Psd->SmmGdtSize = (UINT32)GdtSize;
+
+  //
+  // Initialize values in template before copy
+  //
+  gStmSmiStack             = (UINT32)((UINTN)SmiStack + StackSize - sizeof (UINTN));
+  gStmSmiCr3               = Cr3;
+  gStmSmbase               = SmBase;
+  gStmSmiHandlerIdtr.Base  = IdtBase;
+  gStmSmiHandlerIdtr.Limit = (UINT16)(IdtSize - 1);
+
+  if (gStmXdSupported) {
+    AsmCpuid (CPUID_EXTENDED_FUNCTION, &RegEax, NULL, NULL, NULL);
+    if (RegEax <= CPUID_EXTENDED_FUNCTION) {
+      //
+      // Extended CPUID functions are not supported on this processor.
+      //
+      gStmXdSupported = FALSE;
+    }
+
+    AsmCpuid (CPUID_EXTENDED_CPU_SIG, NULL, NULL, NULL, &RegEdx);
+    if ((RegEdx & CPUID1_EDX_XD_SUPPORT) == 0) {
+      //
+      // Execute Disable Bit feature is not supported on this processor.
+      //
+      gStmXdSupported = FALSE;
+    }
+  }
+
+  //
+  // Set the value at the top of the CPU stack to the CPU Index
+  //
+  *(UINTN*)(UINTN)gStmSmiStack = CpuIndex;
+
+  //
+  // Copy template to CPU specific SMI handler location
+  //
+  CopyMem (
+    (VOID*)((UINTN)SmBase + SMM_HANDLER_OFFSET),
+    (VOID*)gcStmSmiHandlerTemplate,
+    gcStmSmiHandlerSize
+    );
+
+  Psd->SmmSmiHandlerRip = SmBase + SMM_HANDLER_OFFSET + gcStmSmiHandlerOffset;
+  Psd->SmmSmiHandlerRsp = (UINTN)SmiStack + StackSize - sizeof(UINTN);
+  Psd->SmmCr3           = Cr3;
+
+  DEBUG((DEBUG_INFO, "CpuSmmStmExceptionStackSize - %x\n", PcdGet32(PcdCpuSmmStmExceptionStackSize)));
+  DEBUG((DEBUG_INFO, "Pages - %x\n", EFI_SIZE_TO_PAGES(PcdGet32(PcdCpuSmmStmExceptionStackSize))));
+  Psd->StmProtectionExceptionHandler.SpeRsp = (UINT64)(UINTN)AllocatePages (EFI_SIZE_TO_PAGES (PcdGet32 (PcdCpuSmmStmExceptionStackSize)));
+  Psd->StmProtectionExceptionHandler.SpeRsp += EFI_PAGES_TO_SIZE (EFI_SIZE_TO_PAGES (PcdGet32 (PcdCpuSmmStmExceptionStackSize)));
+
+  Psd->BiosHwResourceRequirementsPtr        = (UINT64)(UINTN)GetStmResource ();
+
+  //
+  // Get the APIC ID for the CPU specified by CpuIndex
+  //
+  Status = mSmmCpuFeaturesLibMpService->GetProcessorInfo (
+             mSmmCpuFeaturesLibMpService,
+             CpuIndex,
+             &ProcessorInfo
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  Psd->LocalApicId = (UINT32)ProcessorInfo.ProcessorId;
+  Psd->AcpiRsdp = 0;
+
+  Hob = GetFirstHob (EFI_HOB_TYPE_CPU);
+  if (Hob != NULL) {
+    Psd->PhysicalAddressBits = ((EFI_HOB_CPU *) Hob)->SizeOfMemorySpace;
+  } else {
+    AsmCpuid (0x80000000, &RegEax, NULL, NULL, NULL);
+    if (RegEax >= 0x80000008) {
+      AsmCpuid (0x80000008, &RegEax, NULL, NULL, NULL);
+      Psd->PhysicalAddressBits = (UINT8) RegEax;
+    } else {
+      Psd->PhysicalAddressBits = 36;
+    }
+  }
+
+  if (!mStmConfigurationTableInitialized) {
+    StmSmmConfigurationTableInit ();
+    mStmConfigurationTableInitialized = TRUE;
+  }
+}
+
+/**
+  SMM End Of Dxe event notification handler.
+
+  STM support need patch AcpiRsdp in TXT_PROCESSOR_SMM_DESCRIPTOR.
+
+  @param[in] Protocol   Points to the protocol's unique identifier.
+  @param[in] Interface  Points to the interface instance.
+  @param[in] Handle     The handle on which the interface was installed.
+
+  @retval EFI_SUCCESS   Notification handler runs successfully.
+**/
+EFI_STATUS
+EFIAPI
+SmmEndOfDxeEventNotify (
+  IN CONST EFI_GUID  *Protocol,
+  IN VOID            *Interface,
+  IN EFI_HANDLE      Handle
+  )
+{
+  VOID                          *Rsdp;
+  UINTN                         Index;
+  TXT_PROCESSOR_SMM_DESCRIPTOR  *Psd;
+
+  DEBUG ((DEBUG_INFO, "SmmEndOfDxeEventNotify\n"));
+
+  //
+  // found ACPI table RSD_PTR from system table
+  //
+  Rsdp = NULL;
+  for (Index = 0; Index < gST->NumberOfTableEntries; Index++) {
+    if (CompareGuid (&(gST->ConfigurationTable[Index].VendorGuid), &gEfiAcpi20TableGuid)) {
+      //
+      // A match was found.
+      //
+      Rsdp = gST->ConfigurationTable[Index].VendorTable;
+      break;
+    }
+  }
+  if (Rsdp == NULL) {
+    for (Index = 0; Index < gST->NumberOfTableEntries; Index++) {
+      if (CompareGuid (&(gST->ConfigurationTable[Index].VendorGuid), &gEfiAcpi10TableGuid)) {
+        //
+        // A match was found.
+        //
+        Rsdp = gST->ConfigurationTable[Index].VendorTable;
+        break;
+      }
+    }
+  }
+
+  for (Index = 0; Index < gSmst->NumberOfCpus; Index++) {
+    Psd = (TXT_PROCESSOR_SMM_DESCRIPTOR *)((UINTN)gSmst->CpuSaveState[Index] - SMRAM_SAVE_STATE_MAP_OFFSET + TXT_SMM_PSD_OFFSET);
+    DEBUG ((DEBUG_INFO, "Index=%d  Psd=%p  Rsdp=%p\n", Index, Psd, Rsdp));
+    Psd->AcpiRsdp = (UINT64)(UINTN)Rsdp;
+  }
+
+  mLockLoadMonitor = TRUE;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  This function initializes the STM configuration table.
+**/
+VOID
+StmSmmConfigurationTableInit (
+  VOID
+  )
+{
+  EFI_STATUS    Status;
+    VOID        *Registration;
+
+  Status = gSmst->SmmInstallProtocolInterface (
+                    &mStmSmmCpuHandle,
+                    &gEfiSmMonitorInitProtocolGuid,
+                    EFI_NATIVE_INTERFACE,
+                    &mSmMonitorInitProtocol
+                    );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  //
+  // Register SMM End of DXE Event
+  //
+  Status = gSmst->SmmRegisterProtocolNotify (
+                    &gEfiSmmEndOfDxeProtocolGuid,
+                    SmmEndOfDxeEventNotify,
+                    &Registration
+                    );
+  ASSERT_EFI_ERROR (Status);
+}
+
+/**
+
+  Get STM state.
+
+  @return STM state
+
+**/
+EFI_SM_MONITOR_STATE
+EFIAPI
+GetMonitorState (
+  VOID
+  )
+{
+  return mStmState;
+}
+
+/**
+
+  Handle single Resource to see if it can be merged into Record.
+
+  @param Resource  A pointer to resource node to be added
+  @param Record    A pointer to record node to be merged
+
+  @retval TRUE  resource handled
+  @retval FALSE resource is not handled
+
+**/
+BOOLEAN
+HandleSingleResource (
+  IN  STM_RSC      *Resource,
+  IN  STM_RSC      *Record
+  )
+{
+  UINT64      ResourceLo;
+  UINT64      ResourceHi;
+  UINT64      RecordLo;
+  UINT64      RecordHi;
+
+  ResourceLo = 0;
+  ResourceHi = 0;
+  RecordLo = 0;
+  RecordHi = 0;
+
+  //
+  // Calling code is responsible for making sure that
+  // Resource->Header.RscType == (*Record)->Header.RscType
+  // thus we use just one of them as switch variable.
+  //
+  switch (Resource->Header.RscType) {
+  case MEM_RANGE:
+  case MMIO_RANGE:
+    ResourceLo = Resource->Mem.Base;
+    ResourceHi = Resource->Mem.Base + Resource->Mem.Length;
+    RecordLo = Record->Mem.Base;
+    RecordHi = Record->Mem.Base + Record->Mem.Length;
+    if (Resource->Mem.RWXAttributes != Record->Mem.RWXAttributes) {
+      if ((ResourceLo == RecordLo) && (ResourceHi == RecordHi)) {
+        Record->Mem.RWXAttributes = Resource->Mem.RWXAttributes | Record->Mem.RWXAttributes;
+        return TRUE;
+      } else {
+        return FALSE;
+      }
+    }
+    break;
+  case IO_RANGE:
+  case TRAPPED_IO_RANGE:
+    ResourceLo = (UINT64) Resource->Io.Base;
+    ResourceHi = (UINT64) Resource->Io.Base + (UINT64) Resource->Io.Length;
+    RecordLo = (UINT64) Record->Io.Base;
+    RecordHi = (UINT64) Record->Io.Base + (UINT64) Record->Io.Length;
+    break;
+  case PCI_CFG_RANGE:
+    if ((Resource->PciCfg.OriginatingBusNumber != Record->PciCfg.OriginatingBusNumber) ||
+        (Resource->PciCfg.LastNodeIndex != Record->PciCfg.LastNodeIndex)) {
+      return FALSE;
+    }
+    if (CompareMem (Resource->PciCfg.PciDevicePath, Record->PciCfg.PciDevicePath, sizeof(STM_PCI_DEVICE_PATH_NODE) * (Resource->PciCfg.LastNodeIndex + 1)) != 0) {
+      return FALSE;
+    }
+    ResourceLo = (UINT64) Resource->PciCfg.Base;
+    ResourceHi = (UINT64) Resource->PciCfg.Base + (UINT64) Resource->PciCfg.Length;
+    RecordLo = (UINT64) Record->PciCfg.Base;
+    RecordHi = (UINT64) Record->PciCfg.Base + (UINT64) Record->PciCfg.Length;
+    if (Resource->PciCfg.RWAttributes != Record->PciCfg.RWAttributes) {
+      if ((ResourceLo == RecordLo) && (ResourceHi == RecordHi)) {
+        Record->PciCfg.RWAttributes = Resource->PciCfg.RWAttributes | Record->PciCfg.RWAttributes;
+        return TRUE;
+      } else {
+        return FALSE;
+      }
+    }
+    break;
+  case MACHINE_SPECIFIC_REG:
+    //
+    // Special case - merge MSR masks in place.
+    //
+    if (Resource->Msr.MsrIndex != Record->Msr.MsrIndex) {
+      return FALSE;
+    }
+    Record->Msr.ReadMask |= Resource->Msr.ReadMask;
+    Record->Msr.WriteMask |= Resource->Msr.WriteMask;
+    return TRUE;
+  default:
+    return FALSE;
+  }
+  //
+  // If resources are disjoint
+  //
+  if ((ResourceHi < RecordLo) || (ResourceLo > RecordHi)) {
+    return FALSE;
+  }
+
+  //
+  // If resource is consumed by record.
+  //
+  if ((ResourceLo >= RecordLo) && (ResourceHi <= RecordHi)) {
+    return TRUE;
+  }
+  //
+  // Resources are overlapping.
+  // Resource and record are merged.
+  //
+  ResourceLo = (ResourceLo < RecordLo) ? ResourceLo : RecordLo;
+  ResourceHi = (ResourceHi > RecordHi) ? ResourceHi : RecordHi;
+
+  switch (Resource->Header.RscType) {
+  case MEM_RANGE:
+  case MMIO_RANGE:
+    Record->Mem.Base = ResourceLo;
+    Record->Mem.Length = ResourceHi - ResourceLo;
+    break;
+  case IO_RANGE:
+  case TRAPPED_IO_RANGE:
+    Record->Io.Base = (UINT16) ResourceLo;
+    Record->Io.Length = (UINT16) (ResourceHi - ResourceLo);
+    break;
+  case PCI_CFG_RANGE:
+    Record->PciCfg.Base = (UINT16) ResourceLo;
+    Record->PciCfg.Length = (UINT16) (ResourceHi - ResourceLo);
+    break;
+  default:
+    return FALSE;
+  }
+
+  return TRUE;
+}
+
+/**
+
+  Add resource node.
+
+  @param Resource  A pointer to resource node to be added
+
+**/
+VOID
+AddSingleResource (
+  IN  STM_RSC    *Resource
+  )
+{
+  STM_RSC    *Record;
+
+  Record = (STM_RSC *)mStmResourcesPtr;
+
+  while (TRUE) {
+    if (Record->Header.RscType == END_OF_RESOURCES) {
+      break;
+    }
+    //
+    // Go to next record if resource and record types don't match.
+    //
+    if (Resource->Header.RscType != Record->Header.RscType) {
+      Record = (STM_RSC *)((UINTN)Record + Record->Header.Length);
+      continue;
+    }
+    //
+    // Record is handled inside of procedure - don't adjust.
+    //
+    if (HandleSingleResource (Resource, Record)) {
+      return ;
+    }
+    Record = (STM_RSC *)((UINTN)Record + Record->Header.Length);
+  }
+
+  //
+  // Add resource to the end of area.
+  //
+  CopyMem (
+    mStmResourcesPtr + mStmResourceSizeUsed - sizeof(mRscEndNode),
+    Resource,
+    Resource->Header.Length
+    );
+  CopyMem (
+    mStmResourcesPtr + mStmResourceSizeUsed - sizeof(mRscEndNode) + Resource->Header.Length,
+    &mRscEndNode,
+    sizeof(mRscEndNode)
+    );
+  mStmResourceSizeUsed += Resource->Header.Length;
+  mStmResourceSizeAvailable = mStmResourceTotalSize - mStmResourceSizeUsed;
+
+  return ;
+}
+
+/**
+
+  Add resource list.
+
+  @param ResourceList  A pointer to resource list to be added
+  @param NumEntries    Optional number of entries.
+                       If 0, list must be terminated by END_OF_RESOURCES.
+
+**/
+VOID
+AddResource (
+  IN  STM_RSC    *ResourceList,
+  IN  UINT32      NumEntries OPTIONAL
+  )
+{
+  UINT32      Count;
+  UINTN       Index;
+  STM_RSC    *Resource;
+
+  if (NumEntries == 0) {
+    Count = 0xFFFFFFFF;
+  } else {
+    Count = NumEntries;
+  }
+
+  Resource = ResourceList;
+
+  for (Index = 0; Index < Count; Index++) {
+    if (Resource->Header.RscType == END_OF_RESOURCES) {
+      return ;
+    }
+    AddSingleResource (Resource);
+    Resource = (STM_RSC *)((UINTN)Resource + Resource->Header.Length);
+  }
+  return ;
+}
+
+/**
+
+  Validate resource list.
+
+  @param ResourceList  A pointer to resource list to be added
+  @param NumEntries    Optional number of entries.
+                       If 0, list must be terminated by END_OF_RESOURCES.
+
+  @retval TRUE  resource valid
+  @retval FALSE resource invalid
+
+**/
+BOOLEAN
+ValidateResource (
+  IN  STM_RSC    *ResourceList,
+  IN  UINT32      NumEntries OPTIONAL
+  )
+{
+  UINT32      Count;
+  UINTN       Index;
+  STM_RSC    *Resource;
+  UINTN       SubIndex;
+
+  //
+  // If NumEntries == 0 make it very big. Scan will be terminated by
+  // END_OF_RESOURCES.
+  //
+  if (NumEntries == 0) {
+    Count = 0xFFFFFFFF;
+  } else {
+    Count = NumEntries;
+  }
+
+  //
+  // Start from beginning of resource list.
+  //
+  Resource = ResourceList;
+
+  for (Index = 0; Index < Count; Index++) {
+    DEBUG ((DEBUG_INFO, "ValidateResource (%d) - RscType(%x)\n", Index, Resource->Header.RscType));
+    //
+    // Validate resource.
+    //
+    switch (Resource->Header.RscType) {
+      case END_OF_RESOURCES:
+        if (Resource->Header.Length != sizeof (STM_RSC_END)) {
+          return  FALSE;
+        }
+        //
+        // If we are passed actual number of resources to add,
+        // END_OF_RESOURCES structure between them is considered an
+        // error. If NumEntries == 0 END_OF_RESOURCES is a termination.
+        //
+        if (NumEntries != 0) {
+          return  FALSE;
+        } else {
+          //
+          // If NumEntries == 0 and list reached end - return success.
+          //
+          return TRUE;
+        }
+        break;
+
+      case MEM_RANGE:
+      case MMIO_RANGE:
+        if (Resource->Header.Length != sizeof (STM_RSC_MEM_DESC)) {
+          return FALSE;
+        }
+
+        if (Resource->Mem.RWXAttributes > FULL_ACCS) {
+          return FALSE;
+        }
+        break;
+
+      case IO_RANGE:
+      case TRAPPED_IO_RANGE:
+        if (Resource->Header.Length != sizeof (STM_RSC_IO_DESC)) {
+          return FALSE;
+        }
+
+        if ((Resource->Io.Base + Resource->Io.Length) > 0xFFFF) {
+          return FALSE;
+        }
+        break;
+
+      case PCI_CFG_RANGE:
+        DEBUG ((DEBUG_INFO, "ValidateResource - PCI (0x%02x, 0x%08x, 0x%02x, 0x%02x)\n", Resource->PciCfg.OriginatingBusNumber, Resource->PciCfg.LastNodeIndex, Resource->PciCfg.PciDevicePath[0].PciDevice, Resource->PciCfg.PciDevicePath[0].PciFunction));
+        if (Resource->Header.Length != sizeof (STM_RSC_PCI_CFG_DESC) + (sizeof(STM_PCI_DEVICE_PATH_NODE) * Resource->PciCfg.LastNodeIndex)) {
+          return FALSE;
+        }
+        for (SubIndex = 0; SubIndex <= Resource->PciCfg.LastNodeIndex; SubIndex++) {
+          if ((Resource->PciCfg.PciDevicePath[SubIndex].PciDevice > 0x1F) || (Resource->PciCfg.PciDevicePath[SubIndex].PciFunction > 7)) {
+            return FALSE;
+          }
+        }
+        if ((Resource->PciCfg.Base + Resource->PciCfg.Length) > 0x1000) {
+          return FALSE;
+        }
+        break;
+
+      case MACHINE_SPECIFIC_REG:
+        if (Resource->Header.Length != sizeof (STM_RSC_MSR_DESC)) {
+          return FALSE;
+        }
+        break;
+
+      default :
+        DEBUG ((DEBUG_ERROR, "ValidateResource - Unknown RscType(%x)\n", Resource->Header.RscType));
+        return FALSE;
+    }
+    Resource = (STM_RSC *)((UINTN)Resource + Resource->Header.Length);
+  }
+  return TRUE;
+}
+
+/**
+
+  Get resource list.
+  EndResource is excluded.
+
+  @param ResourceList  A pointer to resource list to be added
+  @param NumEntries    Optional number of entries.
+                       If 0, list must be terminated by END_OF_RESOURCES.
+
+  @retval TRUE  resource valid
+  @retval FALSE resource invalid
+
+**/
+UINTN
+GetResourceSize (
+  IN  STM_RSC    *ResourceList,
+  IN  UINT32      NumEntries OPTIONAL
+  )
+{
+  UINT32      Count;
+  UINTN       Index;
+  STM_RSC    *Resource;
+
+  Resource = ResourceList;
+
+  //
+  // If NumEntries == 0 make it very big. Scan will be terminated by
+  // END_OF_RESOURCES.
+  //
+  if (NumEntries == 0) {
+    Count = 0xFFFFFFFF;
+  } else {
+    Count = NumEntries;
+  }
+
+  //
+  // Start from beginning of resource list.
+  //
+  Resource = ResourceList;
+
+  for (Index = 0; Index < Count; Index++) {
+    if (Resource->Header.RscType == END_OF_RESOURCES) {
+      break;
+    }
+    Resource = (STM_RSC *)((UINTN)Resource + Resource->Header.Length);
+  }
+
+  return (UINTN)Resource - (UINTN)ResourceList;
+}
+
+/**
+
+  Add resources in list to database. Allocate new memory areas as needed.
+
+  @param ResourceList  A pointer to resource list to be added
+  @param NumEntries    Optional number of entries.
+                       If 0, list must be terminated by END_OF_RESOURCES.
+
+  @retval EFI_SUCCESS            If resources are added
+  @retval EFI_INVALID_PARAMETER  If nested procedure detected resource failer
+  @retval EFI_OUT_OF_RESOURCES   If nested procedure returned it and we cannot allocate more areas.
+
+**/
+EFI_STATUS
+EFIAPI
+AddPiResource (
+  IN  STM_RSC    *ResourceList,
+  IN  UINT32      NumEntries OPTIONAL
+  )
+{
+  EFI_STATUS            Status;
+  UINTN                 ResourceSize;
+  EFI_PHYSICAL_ADDRESS  NewResource;
+  UINTN                 NewResourceSize;
+
+  DEBUG ((DEBUG_INFO, "AddPiResource - Enter\n"));
+
+  if (!ValidateResource (ResourceList, NumEntries)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  ResourceSize = GetResourceSize (ResourceList, NumEntries);
+  DEBUG ((DEBUG_INFO, "ResourceSize - 0x%08x\n", ResourceSize));
+  if (ResourceSize == 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (mStmResourcesPtr == NULL) {
+    //
+    // First time allocation
+    //
+    NewResourceSize = EFI_PAGES_TO_SIZE (EFI_SIZE_TO_PAGES (ResourceSize + sizeof(mRscEndNode)));
+    DEBUG ((DEBUG_INFO, "Allocate - 0x%08x\n", NewResourceSize));
+    Status = gSmst->SmmAllocatePages (
+                      AllocateAnyPages,
+                      EfiRuntimeServicesData,
+                      EFI_SIZE_TO_PAGES (NewResourceSize),
+                      &NewResource
+                      );
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    //
+    // Copy EndResource for initialization
+    //
+    mStmResourcesPtr = (UINT8 *)(UINTN)NewResource;
+    mStmResourceTotalSize = NewResourceSize;
+    CopyMem (mStmResourcesPtr, &mRscEndNode, sizeof(mRscEndNode));
+    mStmResourceSizeUsed      = sizeof(mRscEndNode);
+    mStmResourceSizeAvailable = mStmResourceTotalSize - sizeof(mRscEndNode);
+
+    //
+    // Let SmmCore change resource ptr
+    //
+    NotifyStmResourceChange (mStmResourcesPtr);
+  } else if (mStmResourceSizeAvailable < ResourceSize) {
+    //
+    // Need enlarge
+    //
+    NewResourceSize = mStmResourceTotalSize + (ResourceSize - mStmResourceSizeAvailable);
+    NewResourceSize = EFI_PAGES_TO_SIZE (EFI_SIZE_TO_PAGES (NewResourceSize));
+    DEBUG ((DEBUG_INFO, "ReAllocate - 0x%08x\n", NewResourceSize));
+    Status = gSmst->SmmAllocatePages (
+                      AllocateAnyPages,
+                      EfiRuntimeServicesData,
+                      EFI_SIZE_TO_PAGES (NewResourceSize),
+                      &NewResource
+                      );
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+    CopyMem ((VOID *)(UINTN)NewResource, mStmResourcesPtr, mStmResourceSizeUsed);
+    mStmResourceSizeAvailable = NewResourceSize - mStmResourceSizeUsed;
+
+    gSmst->SmmFreePages (
+             (EFI_PHYSICAL_ADDRESS)(UINTN)mStmResourcesPtr,
+             EFI_SIZE_TO_PAGES (mStmResourceTotalSize)
+             );
+
+    mStmResourceTotalSize = NewResourceSize;
+    mStmResourcesPtr = (UINT8 *)(UINTN)NewResource;
+
+    //
+    // Let SmmCore change resource ptr
+    //
+    NotifyStmResourceChange (mStmResourcesPtr);
+  }
+
+  //
+  // Check duplication
+  //
+  AddResource (ResourceList, NumEntries);
+
+  return EFI_SUCCESS;
+}
+
+/**
+
+  Delete resources in list to database.
+
+  @param ResourceList  A pointer to resource list to be deleted
+                       NULL means delete all resources.
+  @param NumEntries    Optional number of entries.
+                       If 0, list must be terminated by END_OF_RESOURCES.
+
+  @retval EFI_SUCCESS            If resources are deleted
+  @retval EFI_INVALID_PARAMETER  If nested procedure detected resource failer
+
+**/
+EFI_STATUS
+EFIAPI
+DeletePiResource (
+  IN  STM_RSC    *ResourceList,
+  IN  UINT32      NumEntries OPTIONAL
+  )
+{
+  if (ResourceList != NULL) {
+    // TBD
+    ASSERT (FALSE);
+    return EFI_UNSUPPORTED;
+  }
+  //
+  // Delete all
+  //
+  CopyMem (mStmResourcesPtr, &mRscEndNode, sizeof(mRscEndNode));
+  mStmResourceSizeUsed      = sizeof(mRscEndNode);
+  mStmResourceSizeAvailable = mStmResourceTotalSize - sizeof(mRscEndNode);
+  return EFI_SUCCESS;
+}
+
+/**
+
+  Get BIOS resources.
+
+  @param ResourceList  A pointer to resource list to be filled
+  @param ResourceSize  On input it means size of resource list input.
+                       On output it means size of resource list filled,
+                       or the size of resource list to be filled if size of too small.
+
+  @retval EFI_SUCCESS            If resources are returned.
+  @retval EFI_BUFFER_TOO_SMALL   If resource list buffer is too small to hold the whole resources.
+
+**/
+EFI_STATUS
+EFIAPI
+GetPiResource (
+  OUT    STM_RSC *ResourceList,
+  IN OUT UINT32  *ResourceSize
+  )
+{
+  if (*ResourceSize < mStmResourceSizeUsed) {
+    *ResourceSize = (UINT32)mStmResourceSizeUsed;
+    return EFI_BUFFER_TOO_SMALL;
+  }
+
+  CopyMem (ResourceList, mStmResourcesPtr, mStmResourceSizeUsed);
+  *ResourceSize = (UINT32)mStmResourceSizeUsed;
+  return EFI_SUCCESS;
+}
+
+/**
+
+  Set valid bit for MSEG MSR.
+
+  @param Buffer Ap function buffer. (not used)
+
+**/
+VOID
+EFIAPI
+EnableMsegMsr (
+  IN VOID  *Buffer
+  )
+{
+  MSR_IA32_SMM_MONITOR_CTL_REGISTER  SmmMonitorCtl;
+
+  SmmMonitorCtl.Uint64 = AsmReadMsr64 (MSR_IA32_SMM_MONITOR_CTL);
+  SmmMonitorCtl.Bits.Valid = 1;
+  AsmWriteMsr64 (MSR_IA32_SMM_MONITOR_CTL, SmmMonitorCtl.Uint64);
+}
+
+/**
+
+  Get 4K page aligned VMCS size.
+
+  @return 4K page aligned VMCS size
+
+**/
+UINT32
+GetVmcsSize (
+  VOID
+  )
+{
+  MSR_IA32_VMX_BASIC_REGISTER  VmxBasic;
+
+  //
+  // Read VMCS size and and align to 4KB
+  //
+  VmxBasic.Uint64 = AsmReadMsr64 (MSR_IA32_VMX_BASIC);
+  return ALIGN_VALUE (VmxBasic.Bits.VmcsSize, SIZE_4KB);
+}
+
+/**
+
+  Check STM image size.
+
+  @param StmImage      STM image
+  @param StmImageSize  STM image size
+
+  @retval TRUE  check pass
+  @retval FALSE check fail
+**/
+BOOLEAN
+StmCheckStmImage (
+  IN EFI_PHYSICAL_ADDRESS StmImage,
+  IN UINTN                StmImageSize
+  )
+{
+  UINTN                     MinMsegSize;
+  STM_HEADER                *StmHeader;
+  IA32_VMX_MISC_REGISTER    VmxMiscMsr;
+
+  //
+  // Check to see if STM image is compatible with CPU
+  //
+  StmHeader = (STM_HEADER *)(UINTN)StmImage;
+  VmxMiscMsr.Uint64 = AsmReadMsr64 (MSR_IA32_VMX_MISC);
+  if (StmHeader->HwStmHdr.MsegHeaderRevision != VmxMiscMsr.Bits.MsegRevisionIdentifier) {
+    DEBUG ((DEBUG_ERROR, "STM Image not compatible with CPU\n"));
+    DEBUG ((DEBUG_ERROR, "  StmHeader->HwStmHdr.MsegHeaderRevision = %08x\n", StmHeader->HwStmHdr.MsegHeaderRevision));
+    DEBUG ((DEBUG_ERROR, "  VmxMiscMsr.Bits.MsegRevisionIdentifier = %08x\n", VmxMiscMsr.Bits.MsegRevisionIdentifier));
+    return FALSE;
+  }
+
+  //
+  // Get Minimal required Mseg size
+  //
+  MinMsegSize = (EFI_PAGES_TO_SIZE (EFI_SIZE_TO_PAGES (StmHeader->SwStmHdr.StaticImageSize)) +
+                 StmHeader->SwStmHdr.AdditionalDynamicMemorySize +
+                 (StmHeader->SwStmHdr.PerProcDynamicMemorySize + GetVmcsSize () * 2) * gSmst->NumberOfCpus);
+  if (MinMsegSize < StmImageSize) {
+    MinMsegSize = StmImageSize;
+  }
+
+  if (StmHeader->HwStmHdr.Cr3Offset >= StmHeader->SwStmHdr.StaticImageSize) {
+    //
+    // We will create page table, just in case that SINIT does not create it.
+    //
+    if (MinMsegSize < StmHeader->HwStmHdr.Cr3Offset + EFI_PAGES_TO_SIZE(6)) {
+      MinMsegSize = StmHeader->HwStmHdr.Cr3Offset + EFI_PAGES_TO_SIZE(6);
+    }
+  }
+
+  //
+  // Check if it exceeds MSEG size
+  //
+  if (MinMsegSize > mMsegSize) {
+    DEBUG ((DEBUG_ERROR, "MSEG too small.  Min MSEG Size = %08x  Current MSEG Size = %08x\n", MinMsegSize, mMsegSize));
+    DEBUG ((DEBUG_ERROR, "  StmHeader->SwStmHdr.StaticImageSize             = %08x\n", StmHeader->SwStmHdr.StaticImageSize));
+    DEBUG ((DEBUG_ERROR, "  StmHeader->SwStmHdr.AdditionalDynamicMemorySize = %08x\n", StmHeader->SwStmHdr.AdditionalDynamicMemorySize));
+    DEBUG ((DEBUG_ERROR, "  StmHeader->SwStmHdr.PerProcDynamicMemorySize    = %08x\n", StmHeader->SwStmHdr.PerProcDynamicMemorySize));
+    DEBUG ((DEBUG_ERROR, "  VMCS Size                                       = %08x\n", GetVmcsSize ()));
+    DEBUG ((DEBUG_ERROR, "  Max CPUs                                        = %08x\n", gSmst->NumberOfCpus));
+    DEBUG ((DEBUG_ERROR, "  StmHeader->HwStmHdr.Cr3Offset                   = %08x\n", StmHeader->HwStmHdr.Cr3Offset));
+    return FALSE;
+  }
+
+  return TRUE;
+}
+
+/**
+
+  Load STM image to MSEG.
+
+  @param StmImage      STM image
+  @param StmImageSize  STM image size
+
+**/
+VOID
+StmLoadStmImage (
+  IN EFI_PHYSICAL_ADDRESS StmImage,
+  IN UINTN                StmImageSize
+  )
+{
+  MSR_IA32_SMM_MONITOR_CTL_REGISTER  SmmMonitorCtl;
+  UINT32                             MsegBase;
+  STM_HEADER                         *StmHeader;
+
+  //
+  // Get MSEG base address from MSR_IA32_SMM_MONITOR_CTL
+  //
+  SmmMonitorCtl.Uint64 = AsmReadMsr64 (MSR_IA32_SMM_MONITOR_CTL);
+  MsegBase = SmmMonitorCtl.Bits.MsegBase << 12;
+
+  //
+  // Zero all of MSEG base address
+  //
+  ZeroMem ((VOID *)(UINTN)MsegBase, mMsegSize);
+
+  //
+  // Copy STM Image into MSEG
+  //
+  CopyMem ((VOID *)(UINTN)MsegBase, (VOID *)(UINTN)StmImage, StmImageSize);
+
+  //
+  // STM Header is at the beginning of the STM Image
+  //
+  StmHeader = (STM_HEADER *)(UINTN)StmImage;
+
+  StmGen4GPageTable ((UINTN)MsegBase + StmHeader->HwStmHdr.Cr3Offset);
+}
+
+/**
+
+  Load STM image to MSEG.
+
+  @param StmImage      STM image
+  @param StmImageSize  STM image size
+
+  @retval EFI_SUCCESS            Load STM to MSEG successfully
+  @retval EFI_ALREADY_STARTED    STM image is already loaded to MSEG
+  @retval EFI_BUFFER_TOO_SMALL   MSEG is smaller than minimal requirement of STM image
+  @retval EFI_UNSUPPORTED        MSEG is not enabled
+
+**/
+EFI_STATUS
+EFIAPI
+LoadMonitor (
+  IN EFI_PHYSICAL_ADDRESS StmImage,
+  IN UINTN                StmImageSize
+  )
+{
+  MSR_IA32_SMM_MONITOR_CTL_REGISTER  SmmMonitorCtl;
+
+  if (mLockLoadMonitor) {
+    return EFI_ACCESS_DENIED;
+  }
+
+  SmmMonitorCtl.Uint64 = AsmReadMsr64 (MSR_IA32_SMM_MONITOR_CTL);
+  if (SmmMonitorCtl.Bits.MsegBase == 0) {
+    return EFI_UNSUPPORTED;
+  }
+
+  if (!StmCheckStmImage (StmImage, StmImageSize)) {
+    return EFI_BUFFER_TOO_SMALL;
+  }
+
+  // Record STM_HASH to PCR 0, just in case it is NOT TXT launch, we still need provide the evidence.
+  TpmMeasureAndLogData(
+    0,                        // PcrIndex
+    TXT_EVTYPE_STM_HASH,      // EventType
+    NULL,                     // EventLog
+    0,                        // LogLen
+    (VOID *)(UINTN)StmImage,  // HashData
+    StmImageSize              // HashDataLen
+    );
+
+  StmLoadStmImage (StmImage, StmImageSize);
+
+  mStmState |= EFI_SM_MONITOR_STATE_ENABLED;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  This function return BIOS STM resource.
+  Produced by SmmStm.
+  Consumed by SmmMpService when Init.
+
+  @return BIOS STM resource
+
+**/
+VOID *
+GetStmResource(
+  VOID
+  )
+{
+  return mStmResourcesPtr;
+}
+
+/**
+  This function notify STM resource change.
+
+  @param StmResource BIOS STM resource
+
+**/
+VOID
+NotifyStmResourceChange (
+  VOID *StmResource
+  )
+{
+  UINTN                         Index;
+  TXT_PROCESSOR_SMM_DESCRIPTOR  *Psd;
+
+  for (Index = 0; Index < gSmst->NumberOfCpus; Index++) {
+    Psd = (TXT_PROCESSOR_SMM_DESCRIPTOR *)((UINTN)gSmst->CpuSaveState[Index] - SMRAM_SAVE_STATE_MAP_OFFSET + TXT_SMM_PSD_OFFSET);
+    Psd->BiosHwResourceRequirementsPtr = (UINT64)(UINTN)StmResource;
+  }
+  return ;
+}
+
+
+/**
+  This is STM setup BIOS callback.
+**/
+VOID
+EFIAPI
+SmmStmSetup (
+  VOID
+  )
+{
+  mStmState |= EFI_SM_MONITOR_STATE_ACTIVATED;
+}
+
+/**
+  This is STM teardown BIOS callback.
+**/
+VOID
+EFIAPI
+SmmStmTeardown (
+  VOID
+  )
+{
+  mStmState &= ~EFI_SM_MONITOR_STATE_ACTIVATED;
+}
+
diff --git a/roms/edk2/UefiCpuPkg/Library/SmmCpuFeaturesLib/SmmStm.h b/roms/edk2/UefiCpuPkg/Library/SmmCpuFeaturesLib/SmmStm.h
new file mode 100644
index 000000000..da551cc4a
--- /dev/null
+++ b/roms/edk2/UefiCpuPkg/Library/SmmCpuFeaturesLib/SmmStm.h
@@ -0,0 +1,179 @@
+/** @file
+  SMM STM support
+
+  Copyright (c) 2015 - 2018, Intel Corporation. All rights reserved.<BR>
+  SPDX-License-Identifier: BSD-2-Clause-Patent
+
+**/
+
+#ifndef _SMM_STM_H_
+#define _SMM_STM_H_
+
+#include <Protocol/SmMonitorInit.h>
+
+/**
+
+  Create 4G page table for STM.
+  2M PAE page table in X64 version.
+
+  @param PageTableBase        The page table base in MSEG
+
+**/
+VOID
+StmGen4GPageTable (
+  IN UINTN              PageTableBase
+  );
+
+/**
+  This is SMM exception handle.
+  Consumed by STM when exception happen.
+
+  @param Context  STM protection exception stack frame
+
+  @return the EBX value for STM reference.
+          EBX = 0: resume SMM guest using register state found on exception stack.
+          EBX = 1 to 0x0F: EBX contains a BIOS error code which the STM must record in the
+                           TXT.ERRORCODE register and subsequently reset the system via
+                           TXT.CMD.SYS_RESET. The value of the TXT.ERRORCODE register is calculated as
+                           follows: TXT.ERRORCODE = (EBX & 0x0F) | STM_CRASH_BIOS_PANIC
+          EBX = 0x10 to 0xFFFFFFFF - reserved, do not use.
+
+**/
+UINT32
+EFIAPI
+SmmStmExceptionHandler (
+  IN OUT STM_PROTECTION_EXCEPTION_STACK_FRAME Context
+  );
+
+
+/**
+
+  Get STM state.
+
+  @return STM state
+
+**/
+EFI_SM_MONITOR_STATE
+EFIAPI
+GetMonitorState (
+  VOID
+  );
+
+/**
+
+  Load STM image to MSEG.
+
+  @param StmImage      STM image
+  @param StmImageSize  STM image size
+
+  @retval EFI_SUCCESS            Load STM to MSEG successfully
+  @retval EFI_BUFFER_TOO_SMALL   MSEG is smaller than minimal requirement of STM image
+
+**/
+EFI_STATUS
+EFIAPI
+LoadMonitor (
+  IN EFI_PHYSICAL_ADDRESS StmImage,
+  IN UINTN                StmImageSize
+  );
+
+/**
+
+  Add resources in list to database. Allocate new memory areas as needed.
+
+  @param ResourceList  A pointer to resource list to be added
+  @param NumEntries    Optional number of entries.
+                       If 0, list must be terminated by END_OF_RESOURCES.
+
+  @retval EFI_SUCCESS            If resources are added
+  @retval EFI_INVALID_PARAMETER  If nested procedure detected resource failer
+  @retval EFI_OUT_OF_RESOURCES   If nested procedure returned it and we cannot allocate more areas.
+
+**/
+EFI_STATUS
+EFIAPI
+AddPiResource (
+  IN  STM_RSC  *ResourceList,
+  IN  UINT32    NumEntries OPTIONAL
+  );
+
+/**
+
+  Delete resources in list to database.
+
+  @param ResourceList  A pointer to resource list to be deleted
+                       NULL means delete all resources.
+  @param NumEntries    Optional number of entries.
+                       If 0, list must be terminated by END_OF_RESOURCES.
+
+  @retval EFI_SUCCESS            If resources are deleted
+  @retval EFI_INVALID_PARAMETER  If nested procedure detected resource failer
+
+**/
+EFI_STATUS
+EFIAPI
+DeletePiResource (
+  IN  STM_RSC    *ResourceList,
+  IN  UINT32      NumEntries OPTIONAL
+  );
+
+/**
+
+  Get BIOS resources.
+
+  @param ResourceList  A pointer to resource list to be filled
+  @param ResourceSize  On input it means size of resource list input.
+                       On output it means size of resource list filled,
+                       or the size of resource list to be filled if size of too small.
+
+  @retval EFI_SUCCESS            If resources are returned.
+  @retval EFI_BUFFER_TOO_SMALL   If resource list buffer is too small to hold the whole resources.
+
+**/
+EFI_STATUS
+EFIAPI
+GetPiResource (
+  OUT    STM_RSC *ResourceList,
+  IN OUT UINT32  *ResourceSize
+  );
+
+/**
+  This function initialize STM configuration table.
+**/
+VOID
+StmSmmConfigurationTableInit (
+  VOID
+  );
+
+/**
+  This function notify STM resource change.
+
+  @param StmResource BIOS STM resource
+
+**/
+VOID
+NotifyStmResourceChange (
+  IN VOID *StmResource
+  );
+
+/**
+  This function return BIOS STM resource.
+
+  @return BIOS STM resource
+
+**/
+VOID *
+GetStmResource (
+  VOID
+  );
+
+/**
+  This function fixes up the address of the global variable or function
+  referred in SmiEntry assembly files to be the absolute address.
+**/
+VOID
+EFIAPI
+SmmCpuFeaturesLibStmSmiEntryFixupAddress (
+ );
+
+#endif
diff --git a/roms/edk2/UefiCpuPkg/Library/SmmCpuFeaturesLib/X64/SmiEntry.nasm b/roms/edk2/UefiCpuPkg/Library/SmmCpuFeaturesLib/X64/SmiEntry.nasm
new file mode 100644
index 000000000..f09d8df1e
--- /dev/null
+++ b/roms/edk2/UefiCpuPkg/Library/SmmCpuFeaturesLib/X64/SmiEntry.nasm
@@ -0,0 +1,276 @@
+;------------------------------------------------------------------------------ ;
+; Copyright (c) 2016 - 2018, Intel Corporation. All rights reserved.<BR>
+; SPDX-License-Identifier: BSD-2-Clause-Patent
+;
+; Module Name:
+;
+;   SmiEntry.nasm
+;
+; Abstract:
+;
+;   Code template of the SMI handler for a particular processor
+;
+;-------------------------------------------------------------------------------
+
+%include "StuffRsbNasm.inc"
+
+;
+; Variables referenced by C code
+;
+
+%define MSR_IA32_MISC_ENABLE 0x1A0
+%define MSR_EFER      0xc0000080
+%define MSR_EFER_XD   0x800
+
+;
+; Constants relating to TXT_PROCESSOR_SMM_DESCRIPTOR
+;
+%define DSC_OFFSET 0xfb00
+%define DSC_GDTPTR 0x48
+%define DSC_GDTSIZ 0x50
+%define DSC_CS 0x14
+%define DSC_DS 0x16
+%define DSC_SS 0x18
+%define DSC_OTHERSEG 0x1a
+;
+; Constants relating to CPU State Save Area
+;
+%define SSM_DR6 0xffd0
+%define SSM_DR7 0xffc8
+
+%define PROTECT_MODE_CS 0x8
+%define PROTECT_MODE_DS 0x20
+%define LONG_MODE_CS 0x38
+%define TSS_SEGMENT 0x40
+%define GDT_SIZE 0x50
+
+extern ASM_PFX(SmiRendezvous)
+extern ASM_PFX(gStmSmiHandlerIdtr)
+extern ASM_PFX(CpuSmmDebugEntry)
+extern ASM_PFX(CpuSmmDebugExit)
+
+global ASM_PFX(gStmSmbase)
+global ASM_PFX(gStmXdSupported)
+global ASM_PFX(gStmSmiStack)
+global ASM_PFX(gStmSmiCr3)
+global ASM_PFX(gcStmSmiHandlerTemplate)
+global ASM_PFX(gcStmSmiHandlerSize)
+global ASM_PFX(gcStmSmiHandlerOffset)
+
+ASM_PFX(gStmSmbase)      EQU StmSmbasePatch - 4
+ASM_PFX(gStmSmiStack)    EQU StmSmiStackPatch - 4
+ASM_PFX(gStmSmiCr3)      EQU StmSmiCr3Patch - 4
+ASM_PFX(gStmXdSupported) EQU StmXdSupportedPatch - 1
+
+    DEFAULT REL
+    SECTION .text
+
+BITS 16
+ASM_PFX(gcStmSmiHandlerTemplate):
+_StmSmiEntryPoint:
+    mov     bx, _StmGdtDesc - _StmSmiEntryPoint + 0x8000
+    mov     ax,[cs:DSC_OFFSET + DSC_GDTSIZ]
+    dec     ax
+    mov     [cs:bx], ax
+    mov     eax, [cs:DSC_OFFSET + DSC_GDTPTR]
+    mov     [cs:bx + 2], eax
+o32 lgdt    [cs:bx]                       ; lgdt fword ptr cs:[bx]
+    mov     ax, PROTECT_MODE_CS
+    mov     [cs:bx-0x2],ax
+o32 mov     edi, strict dword 0
+StmSmbasePatch:
+    lea     eax, [edi + (@ProtectedMode - _StmSmiEntryPoint) + 0x8000]
+    mov     [cs:bx-0x6],eax
+    mov     ebx, cr0
+    and     ebx, 0x9ffafff3
+    or      ebx, 0x23
+    mov     cr0, ebx
+    jmp     dword 0x0:0x0
+_StmGdtDesc:
+    DW 0
+    DD 0
+
+BITS 32
+@ProtectedMode:
+    mov     ax, PROTECT_MODE_DS
+o16 mov     ds, ax
+o16 mov     es, ax
+o16 mov     fs, ax
+o16 mov     gs, ax
+o16 mov     ss, ax
+    mov     esp, strict dword 0
+StmSmiStackPatch:
+    jmp     ProtFlatMode
+
+BITS 64
+ProtFlatMode:
+    mov     eax, strict dword 0
+StmSmiCr3Patch:
+    mov     cr3, rax
+    mov     eax, 0x668                   ; as cr4.PGE is not set here, refresh cr3
+    mov     cr4, rax                    ; in PreModifyMtrrs() to flush TLB.
+; Load TSS
+    sub     esp, 8                      ; reserve room in stack
+    sgdt    [rsp]
+    mov     eax, [rsp + 2]              ; eax = GDT base
+    add     esp, 8
+    mov     dl, 0x89
+    mov     [rax + TSS_SEGMENT + 5], dl ; clear busy flag
+    mov     eax, TSS_SEGMENT
+    ltr     ax
+
+; enable NXE if supported
+    mov al, strict byte 1
+StmXdSupportedPatch:
+    cmp     al, 0
+    jz      @SkipXd
+;
+; Check XD disable bit
+;
+    mov     ecx, MSR_IA32_MISC_ENABLE
+    rdmsr
+    sub     esp, 4
+    push    rdx                        ; save MSR_IA32_MISC_ENABLE[63-32]
+    test    edx, BIT2                  ; MSR_IA32_MISC_ENABLE[34]
+    jz      .0
+    and     dx, 0xFFFB                 ; clear XD Disable bit if it is set
+    wrmsr
+.0:
+    mov     ecx, MSR_EFER
+    rdmsr
+    or      ax, MSR_EFER_XD            ; enable NXE
+    wrmsr
+    jmp     @XdDone
+@SkipXd:
+    sub     esp, 8
+@XdDone:
+
+; Switch into @LongMode
+    push    LONG_MODE_CS                ; push cs hardcore here
+    call    Base                       ; push return address for retf later
+Base:
+    add     dword [rsp], @LongMode - Base; offset for far retf, seg is the 1st arg
+
+    mov     ecx, MSR_EFER
+    rdmsr
+    or      ah, 1                      ; enable LME
+    wrmsr
+    mov     rbx, cr0
+    or      ebx, 0x80010023            ; enable paging + WP + NE + MP + PE
+    mov     cr0, rbx
+    retf
+@LongMode:                              ; long mode (64-bit code) starts here
+    mov     rax, strict qword 0         ;  mov     rax, ASM_PFX(gStmSmiHandlerIdtr)
+StmSmiEntrySmiHandlerIdtrAbsAddr:
+    lidt    [rax]
+    lea     ebx, [rdi + DSC_OFFSET]
+    mov     ax, [rbx + DSC_DS]
+    mov     ds, eax
+    mov     ax, [rbx + DSC_OTHERSEG]
+    mov     es, eax
+    mov     fs, eax
+    mov     gs, eax
+    mov     ax, [rbx + DSC_SS]
+    mov     ss, eax
+    mov     rax, strict qword 0           ;   mov     rax, CommonHandler
+StmSmiEntryCommonHandlerAbsAddr:
+    jmp     rax
+CommonHandler:
+    mov     rbx, [rsp + 0x08]             ; rbx <- CpuIndex
+
+    ;
+    ; Save FP registers
+    ;
+    sub     rsp, 0x200
+    fxsave64 [rsp]
+
+    add     rsp, -0x20
+
+    mov     rcx, rbx
+    call    ASM_PFX(CpuSmmDebugEntry)
+
+    mov     rcx, rbx
+    call    ASM_PFX(SmiRendezvous)
+
+    mov     rcx, rbx
+    call    ASM_PFX(CpuSmmDebugExit)
+
+    add     rsp, 0x20
+
+    ;
+    ; Restore FP registers
+    ;
+    fxrstor64 [rsp]
+
+    add     rsp, 0x200
+
+    lea     rax, [ASM_PFX(gStmXdSupported)]
+    mov     al, [rax]
+    cmp     al, 0
+    jz      .1
+    pop     rdx                       ; get saved MSR_IA32_MISC_ENABLE[63-32]
+    test    edx, BIT2
+    jz      .1
+    mov     ecx, MSR_IA32_MISC_ENABLE
+    rdmsr
+    or      dx, BIT2                  ; set XD Disable bit if it was set before entering into SMM
+    wrmsr
+
+.1:
+    StuffRsb64
+    rsm
+
+_StmSmiHandler:
+;
+; Check XD disable bit
+;
+    xor     r8, r8
+    lea     rax, [ASM_PFX(gStmXdSupported)]
+    mov     al, [rax]
+    cmp     al, 0
+    jz      @StmXdDone
+    mov     ecx, MSR_IA32_MISC_ENABLE
+    rdmsr
+    mov     r8, rdx                    ; save MSR_IA32_MISC_ENABLE[63-32]
+    test    edx, BIT2                  ; MSR_IA32_MISC_ENABLE[34]
+    jz      .0
+    and     dx, 0xFFFB                 ; clear XD Disable bit if it is set
+    wrmsr
+.0:
+    mov     ecx, MSR_EFER
+    rdmsr
+    or      ax, MSR_EFER_XD            ; enable NXE
+    wrmsr
+@StmXdDone:
+    push    r8
+
+    ; below step is needed, because STM does not run above code.
+    ; we have to run below code to set IDT/CR0/CR4
+    mov     rax, strict qword 0        ;  mov     rax, ASM_PFX(gStmSmiHandlerIdtr)
+StmSmiHandlerIdtrAbsAddr:
+    lidt    [rax]
+
+    mov     rax, cr0
+    or      eax, 0x80010023            ; enable paging + WP + NE + MP + PE
+    mov     cr0, rax
+    mov     rax, cr4
+    mov     eax, 0x668                 ; as cr4.PGE is not set here, refresh cr3
+    mov     cr4, rax                   ; in PreModifyMtrrs() to flush TLB.
+    ; STM init finish
+    jmp     CommonHandler
+
+ASM_PFX(gcStmSmiHandlerSize)   : DW      $ - _StmSmiEntryPoint
+ASM_PFX(gcStmSmiHandlerOffset) : DW      _StmSmiHandler - _StmSmiEntryPoint
+
+global ASM_PFX(SmmCpuFeaturesLibStmSmiEntryFixupAddress)
+ASM_PFX(SmmCpuFeaturesLibStmSmiEntryFixupAddress):
+    lea    rax, [ASM_PFX(gStmSmiHandlerIdtr)]
+    lea    rcx, [StmSmiEntrySmiHandlerIdtrAbsAddr]
+    mov    qword [rcx - 8], rax
+    lea    rcx, [StmSmiHandlerIdtrAbsAddr]
+    mov    qword [rcx - 8], rax
+
+    lea    rax, [CommonHandler]
+    lea    rcx, [StmSmiEntryCommonHandlerAbsAddr]
+    mov    qword [rcx - 8], rax
+    ret
diff --git a/roms/edk2/UefiCpuPkg/Library/SmmCpuFeaturesLib/X64/SmiException.nasm b/roms/edk2/UefiCpuPkg/Library/SmmCpuFeaturesLib/X64/SmiException.nasm
new file mode 100644
index 000000000..a04415687
--- /dev/null
+++ b/roms/edk2/UefiCpuPkg/Library/SmmCpuFeaturesLib/X64/SmiException.nasm
@@ -0,0 +1,176 @@
+;------------------------------------------------------------------------------ ;
+; Copyright (c) 2016 - 2018, Intel Corporation. All rights reserved.<BR>
+; SPDX-License-Identifier: BSD-2-Clause-Patent
+;
+; Module Name:
+;
+;   SmiException.nasm
+;
+; Abstract:
+;
+;   Exception handlers used in SM mode
+;
+;-------------------------------------------------------------------------------
+
+%include "StuffRsbNasm.inc"
+
+global  ASM_PFX(gcStmPsd)
+
+extern  ASM_PFX(SmmStmExceptionHandler)
+extern  ASM_PFX(SmmStmSetup)
+extern  ASM_PFX(SmmStmTeardown)
+extern  ASM_PFX(gStmXdSupported)
+extern  ASM_PFX(gStmSmiHandlerIdtr)
+
+%define MSR_IA32_MISC_ENABLE 0x1A0
+%define MSR_EFER      0xc0000080
+%define MSR_EFER_XD   0x800
+
+CODE_SEL          equ 0x38
+DATA_SEL          equ 0x20
+TR_SEL            equ 0x40
+
+    SECTION .data
+
+;
+; This structure serves as a template for all processors.
+;
+ASM_PFX(gcStmPsd):
+            DB      'TXTPSSIG'
+            DW      PSD_SIZE
+            DW      1              ; Version
+            DD      0              ; LocalApicId
+            DB      0x0F           ; Cr4Pse;Cr4Pae;Intel64Mode;ExecutionDisableOutsideSmrr
+            DB      0              ; BIOS to STM
+            DB      0              ; STM to BIOS
+            DB      0
+            DW      CODE_SEL
+            DW      DATA_SEL
+            DW      DATA_SEL
+            DW      DATA_SEL
+            DW      TR_SEL
+            DW      0
+            DQ      0              ; SmmCr3
+            DQ      ASM_PFX(OnStmSetup)
+            DQ      ASM_PFX(OnStmTeardown)
+            DQ      0              ; SmmSmiHandlerRip - SMM guest entrypoint
+            DQ      0              ; SmmSmiHandlerRsp
+            DQ      0
+            DD      0
+            DD      0x80010100     ; RequiredStmSmmRevId
+            DQ      ASM_PFX(OnException)
+            DQ      0              ; ExceptionStack
+            DW      DATA_SEL
+            DW      0x01F          ; ExceptionFilter
+            DD      0
+            DQ      0
+            DQ      0              ; BiosHwResourceRequirementsPtr
+            DQ      0              ; AcpiRsdp
+            DB      0              ; PhysicalAddressBits
+PSD_SIZE  equ $ -   ASM_PFX(gcStmPsd)
+
+    DEFAULT REL
+    SECTION .text
+;------------------------------------------------------------------------------
+; SMM Exception handlers
+;------------------------------------------------------------------------------
+global ASM_PFX(OnException)
+ASM_PFX(OnException):
+    mov  rcx, rsp
+    add  rsp, -0x28
+    call ASM_PFX(SmmStmExceptionHandler)
+    add  rsp, 0x28
+    mov  ebx, eax
+    mov  eax, 4
+    vmcall
+    jmp $
+
+global ASM_PFX(OnStmSetup)
+ASM_PFX(OnStmSetup):
+;
+; Check XD disable bit
+;
+    xor     r8, r8
+    lea     rax, [ASM_PFX(gStmXdSupported)]
+    mov     al, [rax]
+    cmp     al, 0
+    jz      @StmXdDone1
+    mov     ecx, MSR_IA32_MISC_ENABLE
+    rdmsr
+    mov     r8, rdx                    ; save MSR_IA32_MISC_ENABLE[63-32]
+    test    edx, BIT2                  ; MSR_IA32_MISC_ENABLE[34]
+    jz      .01
+    and     dx, 0xFFFB                 ; clear XD Disable bit if it is set
+    wrmsr
+.01:
+    mov     ecx, MSR_EFER
+    rdmsr
+    or      ax, MSR_EFER_XD            ; enable NXE
+    wrmsr
+@StmXdDone1:
+    push    r8
+
+  add  rsp, -0x20
+  call ASM_PFX(SmmStmSetup)
+  add  rsp, 0x20
+
+    lea     rax, [ASM_PFX(gStmXdSupported)]
+    mov     al, [rax]
+    cmp     al, 0
+    jz      .11
+    pop     rdx                       ; get saved MSR_IA32_MISC_ENABLE[63-32]
+    test    edx, BIT2
+    jz      .11
+    mov     ecx, MSR_IA32_MISC_ENABLE
+    rdmsr
+    or      dx, BIT2                  ; set XD Disable bit if it was set before entering into SMM
+    wrmsr
+
+.11:
+    StuffRsb64
+    rsm
+
+global ASM_PFX(OnStmTeardown)
+ASM_PFX(OnStmTeardown):
+;
+; Check XD disable bit
+;
+    xor     r8, r8
+    lea     rax, [ASM_PFX(gStmXdSupported)]
+    mov     al, [rax]
+    cmp     al, 0
+    jz      @StmXdDone2
+    mov     ecx, MSR_IA32_MISC_ENABLE
+    rdmsr
+    mov     r8, rdx                    ; save MSR_IA32_MISC_ENABLE[63-32]
+    test    edx, BIT2                  ; MSR_IA32_MISC_ENABLE[34]
+    jz      .02
+    and     dx, 0xFFFB                 ; clear XD Disable bit if it is set
+    wrmsr
+.02:
+    mov     ecx, MSR_EFER
+    rdmsr
+    or      ax, MSR_EFER_XD            ; enable NXE
+    wrmsr
+@StmXdDone2:
+    push    r8
+
+  add  rsp, -0x20
+  call ASM_PFX(SmmStmTeardown)
+  add  rsp, 0x20
+
+    lea     rax, [ASM_PFX(gStmXdSupported)]
+    mov     al, [rax]
+    cmp     al, 0
+    jz      .12
+    pop     rdx                       ; get saved MSR_IA32_MISC_ENABLE[63-32]
+    test    edx, BIT2
+    jz      .12
+    mov     ecx, MSR_IA32_MISC_ENABLE
+    rdmsr
+    or      dx, BIT2                  ; set XD Disable bit if it was set before entering into SMM
+    wrmsr
+
+.12:
+    StuffRsb64
+    rsm
diff --git a/roms/edk2/UefiCpuPkg/Library/SmmCpuFeaturesLib/X64/SmmStmSupport.c b/roms/edk2/UefiCpuPkg/Library/SmmCpuFeaturesLib/X64/SmmStmSupport.c
new file mode 100644
index 000000000..aacc1455a
--- /dev/null
+++ b/roms/edk2/UefiCpuPkg/Library/SmmCpuFeaturesLib/X64/SmmStmSupport.c
@@ -0,0 +1,89 @@
+/** @file
+  SMM STM support functions
+
+  Copyright (c) 2015 - 2016, Intel Corporation. All rights reserved.<BR>
+  SPDX-License-Identifier: BSD-2-Clause-Patent
+
+**/
+
+#include <PiSmm.h>
+#include <Library/DebugLib.h>
+
+#include "SmmStm.h"
+
+///
+/// Page Table Entry
+///
+#define IA32_PG_P                   BIT0
+#define IA32_PG_RW                  BIT1
+#define IA32_PG_PS                  BIT7
+
+/**
+
+  Create 4G page table for STM.
+  2M PAE page table in X64 version.
+
+  @param PageTableBase        The page table base in MSEG
+
+**/
+VOID
+StmGen4GPageTable (
+  IN UINTN              PageTableBase
+  )
+{
+  UINTN                             Index;
+  UINTN                             SubIndex;
+  UINT64                            *Pde;
+  UINT64                            *Pte;
+  UINT64                            *Pml4;
+
+  Pml4 = (UINT64*)(UINTN)PageTableBase;
+  PageTableBase += SIZE_4KB;
+  *Pml4 = PageTableBase | IA32_PG_RW | IA32_PG_P;
+
+  Pde = (UINT64*)(UINTN)PageTableBase;
+  PageTableBase += SIZE_4KB;
+  Pte = (UINT64 *)(UINTN)PageTableBase;
+
+  for (Index = 0; Index < 4; Index++) {
+    *Pde = PageTableBase | IA32_PG_RW | IA32_PG_P;
+    Pde++;
+    PageTableBase += SIZE_4KB;
+
+    for (SubIndex = 0; SubIndex < SIZE_4KB / sizeof (*Pte); SubIndex++) {
+      *Pte = (((Index << 9) + SubIndex) << 21) | IA32_PG_PS | IA32_PG_RW | IA32_PG_P;
+      Pte++;
+    }
+  }
+}
+
+/**
+  This is SMM exception handle.
+  Consumed by STM when exception happen.
+
+  @param Context  STM protection exception stack frame
+
+  @return the EBX value for STM reference.
+          EBX = 0: resume SMM guest using register state found on exception stack.
+          EBX = 1 to 0x0F: EBX contains a BIOS error code which the STM must record in the
+                           TXT.ERRORCODE register and subsequently reset the system via
+                           TXT.CMD.SYS_RESET. The value of the TXT.ERRORCODE register is calculated as
+                           follows: TXT.ERRORCODE = (EBX & 0x0F) | STM_CRASH_BIOS_PANIC
+          EBX = 0x10 to 0xFFFFFFFF - reserved, do not use.
+
+**/
+UINT32
+EFIAPI
+SmmStmExceptionHandler (
+  IN OUT STM_PROTECTION_EXCEPTION_STACK_FRAME Context
+  )
+{
+  // TBD - SmmStmExceptionHandler, record information
+  DEBUG ((DEBUG_ERROR, "SmmStmExceptionHandler ...\n"));
+  //
+  // Skip this instruction and continue;
+  //
+  Context.X64StackFrame->Rip += Context.X64StackFrame->VmcsExitInstructionLength;
+
+  return 0;
+}
-- 
cgit 1.2.3-korg