[PATCH] MdeModulePkg/Core/Dxe: Clear SEV mask on MMIO regions

[Leo Duran <leo.duran@amd.com>]

This patch intercepts MMIO configuration in the GCD module
to ensure those regions are unmasked.

Cc: Feng Tian <feng.tian@intel.com>
Cc: Star Zeng <star.zeng@intel.com>
Cc: Brijesh Singh <brijesh.singh@amd.com>
Contributed-under: TianoCore Contribution Agreement 1.0
Signed-off-by: Leo Duran <leo.duran@amd.com>
---
 MdeModulePkg/Core/Dxe/DxeMain.inf           |  11 ++
 MdeModulePkg/Core/Dxe/Gcd/Gcd.c             |  28 ++++++
 MdeModulePkg/Core/Dxe/Gcd/GcdHelper.h       | 151 ++++++++++++++++++++++++++++
 MdeModulePkg/Core/Dxe/Gcd/GcdHelperCommon.c | 120 ++++++++++++++++++++++
 MdeModulePkg/Core/Dxe/Gcd/Ia32/GcdHelper.c  | 115 +++++++++++++++++++++
 MdeModulePkg/Core/Dxe/Gcd/X64/GcdHelper.c   | 129 ++++++++++++++++++++++++
 6 files changed, 554 insertions(+)
 create mode 100644 MdeModulePkg/Core/Dxe/Gcd/GcdHelper.h
 create mode 100644 MdeModulePkg/Core/Dxe/Gcd/GcdHelperCommon.c
 create mode 100644 MdeModulePkg/Core/Dxe/Gcd/Ia32/GcdHelper.c
 create mode 100644 MdeModulePkg/Core/Dxe/Gcd/X64/GcdHelper.c

diff --git a/MdeModulePkg/Core/Dxe/DxeMain.inf b/MdeModulePkg/Core/Dxe/DxeMain.inf
index 30d5984..183d1e7 100644
--- a/MdeModulePkg/Core/Dxe/DxeMain.inf
+++ b/MdeModulePkg/Core/Dxe/DxeMain.inf
@@ -4,6 +4,8 @@
 #  It provides an implementation of DXE Core that is compliant with DXE CIS.
 #  
 #  Copyright (c) 2006 - 2017, Intel Corporation. All rights reserved.<BR>
+#  Copyright (c) 2017, AMD Incorporated. All rights reserved.<BR>
+#
 #  This program and the accompanying materials
 #  are licensed and made available under the terms and conditions of the BSD License
 #  which accompanies this distribution.  The full text of the license may be found at
@@ -51,6 +53,8 @@
   Hand/Handle.h
   Gcd/Gcd.c
   Gcd/Gcd.h
+  Gcd/GcdHelperCommon.c
+  Gcd/GcdHelper.h
   Mem/Pool.c
   Mem/Page.c
   Mem/MemData.c
@@ -73,6 +77,12 @@
   DxeMain/DxeProtocolNotify.c
   DxeMain/DxeMain.c
 
+[Sources.Ia32]
+  Gcd/Ia32/GcdHelper.c
+
+[Sources.X64]
+  Gcd/X64/GcdHelper.c
+
 [Packages]
   MdePkg/MdePkg.dec
   MdeModulePkg/MdeModulePkg.dec
@@ -192,6 +202,7 @@
   gEfiMdeModulePkgTokenSpaceGuid.PcdPropertiesTableEnable                   ## CONSUMES
   gEfiMdeModulePkgTokenSpaceGuid.PcdImageProtectionPolicy                   ## CONSUMES
   gEfiMdeModulePkgTokenSpaceGuid.PcdDxeNxMemoryProtectionPolicy             ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdPteMemoryEncryptionAddressOrMask        ## CONSUMES
 
 # [Hob]
 # RESOURCE_DESCRIPTOR   ## CONSUMES
diff --git a/MdeModulePkg/Core/Dxe/Gcd/Gcd.c b/MdeModulePkg/Core/Dxe/Gcd/Gcd.c
index a06f8bb..6f85c21 100644
--- a/MdeModulePkg/Core/Dxe/Gcd/Gcd.c
+++ b/MdeModulePkg/Core/Dxe/Gcd/Gcd.c
@@ -4,6 +4,8 @@
   are accessible to the CPU that is executing the DXE core.
 
 Copyright (c) 2006 - 2017, Intel Corporation. All rights reserved.<BR>
+Copyright (c) 2017, AMD Incorporated. All rights reserved.<BR>
+
 This program and the accompanying materials
 are licensed and made available under the terms and conditions of the BSD License
 which accompanies this distribution.  The full text of the license may be found at
@@ -16,6 +18,7 @@ WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
 
 #include "DxeMain.h"
 #include "Gcd.h"
+#include "GcdHelper.h"
 
 #define MINIMUM_INITIAL_MEMORY_SIZE 0x10000
 
@@ -723,6 +726,7 @@ CoreConvertSpace (
   LIST_ENTRY         *StartLink;
   LIST_ENTRY         *EndLink;
   UINT64             CpuArchAttributes;
+  UINT64             AddressEncMask;
 
   if (Length == 0) {
     DEBUG ((DEBUG_GCD, "  Status = %r\n", EFI_INVALID_PARAMETER));
@@ -741,6 +745,11 @@ CoreConvertSpace (
   }
 
   //
+  // Make sure AddressEncMask is contained to smallest supported address field.
+  //
+  AddressEncMask = PcdGet64 (PcdPteMemoryEncryptionAddressOrMask) & PAGING_1G_ADDRESS_MASK_64;
+
+  //
   // Search for the list of descriptors that cover the range BaseAddress to BaseAddress+Length
   //
   Status = CoreSearchGcdMapEntry (BaseAddress, Length, &StartLink, &EndLink, Map);
@@ -893,6 +902,11 @@ CoreConvertSpace (
       Entry->GcdMemoryType = GcdMemoryType;
       if (GcdMemoryType == EfiGcdMemoryTypeMemoryMappedIo) {
         Entry->Capabilities  = Capabilities | EFI_MEMORY_RUNTIME | EFI_MEMORY_PORT_IO;
+        Status = ConvertEncryptionOnAddressRange (BaseAddress, Length, CLEAR_ADDR_ENC_OPERATION, AddressEncMask);
+        if (EFI_ERROR (Status)) {
+          DEBUG ((EFI_D_ERROR, "Could not CLEAR EncMask on Range: BaseAddress = 0xlX, Length = 0xlX, Status = %r\n",
+            BaseAddress, Length, Status));
+        }
       } else {
         Entry->Capabilities  = Capabilities | EFI_MEMORY_RUNTIME;
       }
@@ -904,6 +918,13 @@ CoreConvertSpace (
     // Free operations
     //
     case GCD_FREE_MEMORY_OPERATION:
+      if (Entry->Capabilities & EFI_MEMORY_PORT_IO) {
+        Status = ConvertEncryptionOnAddressRange (BaseAddress, Length, SET_ADDR_ENC_OPERATION, AddressEncMask);
+        if (EFI_ERROR (Status)) {
+          DEBUG ((EFI_D_ERROR, "Could not SET EncMask on Range: BaseAddress = 0xlX, Length = 0xlX, Status = %r\n",
+            BaseAddress, Length, Status));
+        }
+      }
     case GCD_FREE_IO_OPERATION:
       Entry->ImageHandle  = NULL;
       Entry->DeviceHandle = NULL;
@@ -912,6 +933,13 @@ CoreConvertSpace (
     // Remove operations
     //
     case GCD_REMOVE_MEMORY_OPERATION:
+      if (Entry->Capabilities & EFI_MEMORY_PORT_IO) {
+        Status = ConvertEncryptionOnAddressRange (BaseAddress, Length, SET_ADDR_ENC_OPERATION, AddressEncMask);
+        if (EFI_ERROR (Status)) {
+          DEBUG ((EFI_D_ERROR, "Could not SET EncMask on Range: BaseAddress = 0xlX, Length = 0xlX, Status = %r\n",
+            BaseAddress, Length, Status));
+        }
+      }
       Entry->GcdMemoryType = EfiGcdMemoryTypeNonExistent;
       Entry->Capabilities  = 0;
       break;
diff --git a/MdeModulePkg/Core/Dxe/Gcd/GcdHelper.h b/MdeModulePkg/Core/Dxe/Gcd/GcdHelper.h
new file mode 100644
index 0000000..d6ec339
--- /dev/null
+++ b/MdeModulePkg/Core/Dxe/Gcd/GcdHelper.h
@@ -0,0 +1,151 @@
+/** @file
+  Definitions for helper functions used by GCD.
+
+Copyright (c) 2017, AMD Incorporated. All rights reserved.<BR>
+
+This program and the accompanying materials
+are licensed and made available under the terms and conditions of the BSD License
+which accompanies this distribution.  The full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _GCD_HELPER_H_
+#define _GCD_HELPER_H_
+
+
+#pragma pack(1)
+//
+// Page Table Entry 4KB
+//
+typedef union {
+  struct {
+    UINT64  Present:1;                // 0 = Not present in memory, 1 = Present in memory
+    UINT64  ReadWrite:1;              // 0 = Read-Only, 1= Read/Write
+    UINT64  UserSupervisor:1;         // 0 = Supervisor, 1=User
+    UINT64  WriteThrough:1;           // 0 = Write-Back caching, 1=Write-Through caching
+    UINT64  CacheDisabled:1;          // 0 = Cached, 1=Non-Cached
+    UINT64  Accessed:1;               // 0 = Not accessed, 1 = Accessed (set by CPU)
+    UINT64  Dirty:1;                  // 0 = Not Dirty, 1 = written by processor on access to page
+    UINT64  PAT:1;                    //
+    UINT64  Global:1;                 // 0 = Not global page, 1 = global page TLB not cleared on CR3 write
+    UINT64  Available:3;              // Available for use by system software
+    UINT64  PageTableBaseAddress:40;  // Page Table Base Address
+    UINT64  AvailableHigh:11;         // Available for use by system software
+    UINT64  Nx:1;                     // 0 = Execute Code, 1 = No Code Execution
+  } Bits;
+  UINT64    Uint64;
+} PAGE_TABLE_4K_ENTRY;
+
+//
+// Page Table Entry 2MB
+//
+typedef union {
+  struct {
+    UINT64  Present:1;                // 0 = Not present in memory, 1 = Present in memory
+    UINT64  ReadWrite:1;              // 0 = Read-Only, 1= Read/Write
+    UINT64  UserSupervisor:1;         // 0 = Supervisor, 1=User
+    UINT64  WriteThrough:1;           // 0 = Write-Back caching, 1=Write-Through caching
+    UINT64  CacheDisabled:1;          // 0 = Cached, 1=Non-Cached
+    UINT64  Accessed:1;               // 0 = Not accessed, 1 = Accessed (set by CPU)
+    UINT64  Dirty:1;                  // 0 = Not Dirty, 1 = written by processor on access to page
+    UINT64  MustBe1:1;                // Must be 1 
+    UINT64  Global:1;                 // 0 = Not global page, 1 = global page TLB not cleared on CR3 write
+    UINT64  Available:3;              // Available for use by system software
+    UINT64  PAT:1;                    //
+    UINT64  MustBeZero:8;             // Must be zero;
+    UINT64  PageTableBaseAddress:31;  // Page Table Base Address
+    UINT64  AvailableHigh:11;         // Available for use by system software
+    UINT64  Nx:1;                     // 0 = Execute Code, 1 = No Code Execution
+  } Bits;
+  UINT64    Uint64;
+} PAGE_TABLE_2M_ENTRY;
+#pragma pack()
+
+
+typedef enum {
+  CLEAR_ADDR_ENC_OPERATION,
+  SET_ADDR_ENC_OPERATION,
+  INVALID_ENC_OPERATION
+} ADDR_ENC_OPERATION;
+
+
+#define IA32_PG_P                  BIT0
+#define IA32_PG_RW                 BIT1
+#define IA32_PG_PS                 BIT7
+
+#define PAGING_4K_ADDRESS_MASK_64  0x000FFFFFFFFFF000ull
+#define PAGING_2M_ADDRESS_MASK_64  0x000FFFFFFFE00000ull
+#define PAGING_1G_ADDRESS_MASK_64  0x000FFFFFC0000000ull
+
+
+/**
+  Convert Encryption on address range
+
+  @param  BaseAddress            Start address of the range
+  @param  Length                 Length of the segment
+  @param  Operation              Convertion operation (set or clear)
+  @param  AddressEncMask         Address Encryption Mask
+
+  @retval EFI_INVALID_PARAMETER  Length or Address not aligned on EFI_PAGE_SIZE boundary.
+  @retval EFI_NO_MAPPING         Missing page table mappings for the specified segment.
+  @retval EFI_SUCCESS            Action successfully done.
+**/
+EFI_STATUS
+ConvertEncryptionOnAddressRange (
+  IN EFI_PHYSICAL_ADDRESS  BaseAddress,
+  IN UINT64                Length,
+  IN ADDR_ENC_OPERATION    Operation,
+  IN UINT64                AddressEncMask
+  );
+
+/**
+  Convert Encryption on page table entry.
+
+  @param  PageTableEntry         Start address of the range
+  @param  Operation              Convertion operation (set or clear)
+  @param  AddressEncMask         Address Encryption Mask
+
+**/
+VOID
+ConvertEncryptionOnPageTableEntry (
+  IN UINT64              *PageTableEntry,
+  IN ADDR_ENC_OPERATION  Operation,
+  IN UINT64              AddressEncMask
+  );
+
+/**
+  Split 1G page to 2M on Encrypted address range.
+
+  @param[in]      PhysicalAddress       Address of the encryted range.
+  @param[in]      AddressEncMask        Address Encryption Mask.
+  @param[in, out] PageEntry1G           Pointer to 1G page entry.
+
+**/
+VOID
+Split1GPageTo2MPageOnEncRange (
+  IN EFI_PHYSICAL_ADDRESS               PhysicalAddress,
+  IN UINT64                             AddressEncMask,
+  IN OUT UINT64                         *PageEntry1G
+  );
+
+/**
+  Split 2M page to 4K on Encrypted address range.
+
+  @param[in]      PhysicalAddress       Address of the range.
+  @param[in]      AddressEncmask        Encryption Mask.
+  @param[in, out] PageEntry2M           Pointer to 2M page entry.
+
+**/
+VOID
+Split2MPageTo4KPageOnEncRange (
+  IN EFI_PHYSICAL_ADDRESS               PhysicalAddress,
+  IN UINT64                             AddressEncMask,
+  IN OUT UINT64                         *PageEntry2M
+  );
+
+#endif // _GCD_HELPER_H_
+
diff --git a/MdeModulePkg/Core/Dxe/Gcd/GcdHelperCommon.c b/MdeModulePkg/Core/Dxe/Gcd/GcdHelperCommon.c
new file mode 100644
index 0000000..1660732
--- /dev/null
+++ b/MdeModulePkg/Core/Dxe/Gcd/GcdHelperCommon.c
@@ -0,0 +1,120 @@
+/** @file
+  The file contains helper functions used by GCD.
+
+Copyright (c) 2017, AMD Incorporated. All rights reserved.<BR>
+
+This program and the accompanying materials
+are licensed and made available under the terms and conditions of the BSD License
+which accompanies this distribution.  The full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#include "DxeMain.h"
+#include "GcdHelper.h"
+
+
+/**
+  Split 2M page to 4K on Encrypted address range.
+
+  @param[in]      PhysicalAddress       Address of the range.
+  @param[in]      AddressEncmask        Encryption Mask.
+  @param[in, out] PageEntry2M           Pointer to 2M page entry.
+
+**/
+VOID
+Split2MPageTo4KPageOnEncRange (
+  IN EFI_PHYSICAL_ADDRESS               PhysicalAddress,
+  IN UINT64                             AddressEncMask,
+  IN OUT UINT64                         *PageEntry2M
+  )
+{
+  EFI_PHYSICAL_ADDRESS                  PhysicalAddress4K;
+  UINTN                                 IndexOfPageTableEntries;
+  PAGE_TABLE_4K_ENTRY                   *PageTableEntry;
+
+  PageTableEntry = (PAGE_TABLE_4K_ENTRY *)AllocatePages (1);
+  ASSERT (PageTableEntry != NULL);
+
+  //
+  // Fill in 2M page entry.
+  //
+  *PageEntry2M = (UINT64)(UINTN) PageTableEntry | AddressEncMask | IA32_PG_P | IA32_PG_RW;
+
+  PhysicalAddress4K = PhysicalAddress;
+  for (IndexOfPageTableEntries = 0; IndexOfPageTableEntries < 512; IndexOfPageTableEntries++, PageTableEntry++, PhysicalAddress4K += SIZE_4KB) {
+    //
+    // Fill in the Page Table entries
+    //
+    PageTableEntry->Uint64 = (UINT64) PhysicalAddress4K | AddressEncMask;
+    PageTableEntry->Bits.ReadWrite = 1;
+    PageTableEntry->Bits.Present = 1;
+  }
+}
+
+
+/**
+  Split 1G page to 2M on Encrypted address range.
+
+  @param[in]      PhysicalAddress       Address of the encryted range.
+  @param[in]      AddressEncMask        Address Encryption Mask.
+  @param[in, out] PageEntry1G           Pointer to 1G page entry.
+
+**/
+VOID
+Split1GPageTo2MPageOnEncRange (
+  IN EFI_PHYSICAL_ADDRESS               PhysicalAddress,
+  IN UINT64                             AddressEncMask,
+  IN OUT UINT64                         *PageEntry1G
+  )
+{
+  EFI_PHYSICAL_ADDRESS                  PhysicalAddress2M;
+  UINTN                                 IndexOfPageDirectoryEntries;
+  PAGE_TABLE_2M_ENTRY                   *PageDirectoryEntry;
+
+  PageDirectoryEntry = (PAGE_TABLE_2M_ENTRY *)AllocatePages (1);
+  ASSERT (PageDirectoryEntry != NULL);
+
+  //
+  // Fill in 1G page entry.
+  //
+  *PageEntry1G = (UINT64)(UINTN) PageDirectoryEntry | AddressEncMask | IA32_PG_P | IA32_PG_RW;
+
+  PhysicalAddress2M = PhysicalAddress;
+  for (IndexOfPageDirectoryEntries = 0; IndexOfPageDirectoryEntries < 512; IndexOfPageDirectoryEntries++, PageDirectoryEntry++, PhysicalAddress2M += SIZE_2MB) {
+    //
+    // Fill in the Page Directory entries
+    //
+    PageDirectoryEntry->Uint64 = (UINT64) PhysicalAddress2M | AddressEncMask;
+    PageDirectoryEntry->Bits.ReadWrite = 1;
+    PageDirectoryEntry->Bits.Present = 1;
+    PageDirectoryEntry->Bits.MustBe1 = 1;
+  }
+}
+
+
+/**
+  Convert Encryption on page table entry.
+
+  @param  PageTableEntry         Start address of the range
+  @param  Operation              Convertion operation (set or clear)
+  @param  AddressEncMask         Address Encryption Mask
+
+**/
+VOID
+ConvertEncryptionOnPageTableEntry (
+  IN UINT64              *PageTableEntry,
+  IN ADDR_ENC_OPERATION  Operation,
+  IN UINT64              AddressEncMask
+  )
+{
+  if (Operation == CLEAR_ADDR_ENC_OPERATION) {
+    *PageTableEntry &= ~AddressEncMask;
+  } else if (Operation == SET_ADDR_ENC_OPERATION) {
+    *PageTableEntry |= AddressEncMask;
+  }
+}
+
diff --git a/MdeModulePkg/Core/Dxe/Gcd/Ia32/GcdHelper.c b/MdeModulePkg/Core/Dxe/Gcd/Ia32/GcdHelper.c
new file mode 100644
index 0000000..4781900
--- /dev/null
+++ b/MdeModulePkg/Core/Dxe/Gcd/Ia32/GcdHelper.c
@@ -0,0 +1,115 @@
+/** @file
+  The file contains helper functions used by GCD.
+
+Copyright (c) 2017, AMD Incorporated. All rights reserved.<BR>
+
+This program and the accompanying materials
+are licensed and made available under the terms and conditions of the BSD License
+which accompanies this distribution.  The full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#include "DxeMain.h"
+#include "GcdHelper.h"
+
+
+/**
+  Convert Encryption on address range.
+
+  @param  BaseAddress            Start address of the range
+  @param  Length                 Length of the range
+  @param  Operation              Convertion operation (set or clear)
+  @param  AddressEncMask         Address Encryption Mask
+
+  @retval EFI_INVALID_PARAMETER  Length or Address not aligned on EFI_PAGE_SIZE boundary.
+  @retval EFI_NO_MAPPING         Missing page table mappings for the specified range.
+  @retval EFI_SUCCESS            Action successfully done.
+**/
+EFI_STATUS
+ConvertEncryptionOnAddressRange (
+  IN EFI_PHYSICAL_ADDRESS  BaseAddress,
+  IN UINT64                Length,
+  IN ADDR_ENC_OPERATION    Operation,
+  IN UINT64                AddressEncMask
+  )
+{
+  EFI_PHYSICAL_ADDRESS  PhysicalAddress;
+  UINT64                *PageTable;
+  UINTN                 PTIndex;
+  UINT64                Cr3;
+
+  // Do we have actual work to do?
+  if (Length == 0 || AddressEncMask == 0) {
+    return EFI_SUCCESS;
+  }
+
+  // PAE *must* be enabled on 32-bit page tables
+  // (else we can't configure AddressEncMask)
+  if ((AsmReadCr4 () & BIT5) == 0) {
+    return EFI_SUCCESS;
+  }
+
+  // Make sure we're PAGE_SIZE aligned
+  if ((BaseAddress & EFI_PAGE_MASK) || (Length & EFI_PAGE_MASK)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  // Validate Operation
+  if (Operation >= INVALID_ENC_OPERATION) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Cr3 = AsmReadCr3 ();
+  PhysicalAddress = BaseAddress;
+
+  do {
+    // PDPE Present?
+    PageTable = (UINT64 *)(UINTN) (Cr3 & ~AddressEncMask & PAGING_4K_ADDRESS_MASK_64);
+    PTIndex = BitFieldRead64(PhysicalAddress, 30, 31);
+    if ((PageTable[PTIndex] & IA32_PG_P) == 0) {
+      return EFI_NO_MAPPING;
+    }
+
+    // PDE Present?
+    PageTable = (UINT64 *)(UINTN) (PageTable[PTIndex] & ~AddressEncMask & PAGING_4K_ADDRESS_MASK_64);
+    PTIndex = BitFieldRead64(PhysicalAddress, 21, 29);
+    if ((PageTable[PTIndex] & IA32_PG_P) == 0) {
+      return EFI_NO_MAPPING;
+    }
+
+    // Is it a 2MB page entry?
+    if ((PageTable[PTIndex] & IA32_PG_PS ) != 0) {
+      if ((PhysicalAddress & (SIZE_2MB - 1)) == 0 && Length >= SIZE_2MB) {
+        // Consume 2MB from the range
+        ConvertEncryptionOnPageTableEntry (&PageTable[PTIndex], Operation, AddressEncMask);
+        PhysicalAddress += SIZE_2MB;
+        Length -= SIZE_2MB;
+      } else {
+        // Split 2MB page entry to consume 4K chunks
+        Split2MPageTo4KPageOnEncRange (PhysicalAddress, AddressEncMask, &PageTable[PTIndex]); 
+      }
+    } else {
+      // PTE Present?
+      PageTable = (UINT64 *)(UINTN) (PageTable[PTIndex] & ~AddressEncMask & PAGING_4K_ADDRESS_MASK_64);
+      PTIndex = BitFieldRead64(PhysicalAddress, 12, 20);
+      if ((PageTable[PTIndex] & IA32_PG_P) == 0) {
+        return EFI_NO_MAPPING;
+      }
+
+      // Consume 4KB from the range
+      ConvertEncryptionOnPageTableEntry (&PageTable[PTIndex], Operation, AddressEncMask);
+      PhysicalAddress += SIZE_4KB;
+      Length -= SIZE_4KB;
+    }
+  } while (Length);
+
+  // Flush TLB
+  AsmWriteCr3 (Cr3);
+
+  return EFI_SUCCESS;
+}
+
diff --git a/MdeModulePkg/Core/Dxe/Gcd/X64/GcdHelper.c b/MdeModulePkg/Core/Dxe/Gcd/X64/GcdHelper.c
new file mode 100644
index 0000000..5943f7a
--- /dev/null
+++ b/MdeModulePkg/Core/Dxe/Gcd/X64/GcdHelper.c
@@ -0,0 +1,129 @@
+/** @file
+  The file contains helper functions used by GCD.
+
+Copyright (c) 2017, AMD Incorporated. All rights reserved.<BR>
+
+This program and the accompanying materials
+are licensed and made available under the terms and conditions of the BSD License
+which accompanies this distribution.  The full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#include "DxeMain.h"
+#include "GcdHelper.h"
+
+
+/**
+  Convert Encryption on address range.
+
+  @param  BaseAddress            Start address of the range
+  @param  Length                 Length of the range
+  @param  Operation              Convertion operation (set or clear)
+  @param  AddressEncMask         Address Encryption Mask
+
+  @retval EFI_INVALID_PARAMETER  Length or Address not aligned on EFI_PAGE_SIZE boundary.
+  @retval EFI_NO_MAPPING         Missing page table mappings for the specified range.
+  @retval EFI_SUCCESS            Action successfully done.
+**/
+EFI_STATUS
+ConvertEncryptionOnAddressRange (
+  IN EFI_PHYSICAL_ADDRESS  BaseAddress,
+  IN UINT64                Length,
+  IN ADDR_ENC_OPERATION    Operation,
+  IN UINT64                AddressEncMask
+  )
+{
+  EFI_PHYSICAL_ADDRESS  PhysicalAddress;
+  UINT64                *PageTable;
+  UINTN                 PTIndex;
+  UINT64                Cr3;
+
+  // Do we have actual work to do?
+  if (Length == 0 || AddressEncMask == 0) {
+    return EFI_SUCCESS;
+  }
+
+  // Make sure we're PAGE_SIZE aligned
+  if ((BaseAddress & EFI_PAGE_MASK) || (Length & EFI_PAGE_MASK)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  // Validate Operation
+  if (Operation >= INVALID_ENC_OPERATION) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Cr3 = AsmReadCr3 ();
+  PhysicalAddress = BaseAddress;
+
+  do {
+    // PML4E Present?
+    PageTable = (UINT64 *)(UINTN) (Cr3 & ~AddressEncMask & PAGING_4K_ADDRESS_MASK_64);
+    PTIndex = BitFieldRead64(PhysicalAddress, 39, 47);
+    if ((PageTable[PTIndex] & IA32_PG_P) == 0) {
+      return EFI_NO_MAPPING;
+    }
+
+    // PDPE Present?
+    PageTable = (UINT64 *)(UINTN) (PageTable[PTIndex] & ~AddressEncMask & PAGING_4K_ADDRESS_MASK_64);
+    PTIndex = BitFieldRead64(PhysicalAddress, 30, 38);
+    if ((PageTable[PTIndex] & IA32_PG_P) == 0) {
+      return EFI_NO_MAPPING;
+    }
+
+    // Is it a 1GB page entry?
+    if ((PageTable[PTIndex] & IA32_PG_PS ) != 0) {
+      if ((PhysicalAddress & (SIZE_1GB - 1)) == 0 && Length >= SIZE_1GB) {
+        // Consume 1GB from the range
+        ConvertEncryptionOnPageTableEntry (&PageTable[PTIndex], Operation, AddressEncMask);
+        PhysicalAddress += SIZE_1GB;
+        Length -= SIZE_1GB;
+      } else {
+        // Split 1G page entry to attempt consuming 2MB chunks
+        Split1GPageTo2MPageOnEncRange (PhysicalAddress, AddressEncMask, &PageTable[PTIndex]); 
+      }
+    } else {
+      // PDE Present?
+      PageTable = (UINT64 *)(UINTN) (PageTable[PTIndex] & ~AddressEncMask & PAGING_4K_ADDRESS_MASK_64);
+      PTIndex = BitFieldRead64(PhysicalAddress, 21, 29);
+      if ((PageTable[PTIndex] & IA32_PG_P) == 0) {
+        return EFI_NO_MAPPING;
+      }
+
+      // Is it a 2MB page entry?
+      if ((PageTable[PTIndex] & IA32_PG_PS ) != 0) {
+        if ((PhysicalAddress & (SIZE_2MB - 1)) == 0 && Length >= SIZE_2MB) {
+          // Consume 2MB from the range
+          ConvertEncryptionOnPageTableEntry (&PageTable[PTIndex], Operation, AddressEncMask);
+          PhysicalAddress += SIZE_2MB;
+          Length -= SIZE_2MB;
+        } else {
+          // Split 2MB page entry to consume 4K chunks
+          Split2MPageTo4KPageOnEncRange (PhysicalAddress, AddressEncMask, &PageTable[PTIndex]); 
+        }
+      } else {
+        // PTE Present?
+        PageTable = (UINT64 *)(UINTN) (PageTable[PTIndex] & ~AddressEncMask & PAGING_4K_ADDRESS_MASK_64);
+        PTIndex = BitFieldRead64(PhysicalAddress, 12, 20);
+        if ((PageTable[PTIndex] & IA32_PG_P) == 0) {
+          return EFI_NO_MAPPING;
+        }
+
+        // Consume 4KB from the range
+        ConvertEncryptionOnPageTableEntry (&PageTable[PTIndex], Operation, AddressEncMask);
+        PhysicalAddress += SIZE_4KB;
+        Length -= SIZE_4KB;
+      }
+    }
+  } while (Length);
+
+  // Flush TLB
+  AsmWriteCr3 (Cr3);
+
+  return EFI_SUCCESS;
+}
+
-- 
2.7.4