[edk2] [PATCH 4/7] IntelSiliconPkg: Add VTd driver.

Jiewen Yao posted 7 patches 7 years, 5 months ago
[edk2] [PATCH 4/7] IntelSiliconPkg: Add VTd driver.
Posted by Jiewen Yao 7 years, 5 months ago
It provides AllocateBuffer/FreeBuffer/Map/Unmap function.
It also provides VTd capability yet.

Cc: Star Zeng <star.zeng@intel.com>
Contributed-under: TianoCore Contribution Agreement 1.0
Signed-off-by: Jiewen Yao <jiewen.yao@intel.com>
---
 IntelSiliconPkg/IntelVTdDxe/BmDma.c              | 441 +++++++++
 IntelSiliconPkg/IntelVTdDxe/DmaProtection.c      | 367 +++++++
 IntelSiliconPkg/IntelVTdDxe/DmaProtection.h      | 501 ++++++++++
 IntelSiliconPkg/IntelVTdDxe/DmarAcpiTable.c      | 998 ++++++++++++++++++++
 IntelSiliconPkg/IntelVTdDxe/IntelVTdDxe.c        | 353 +++++++
 IntelSiliconPkg/IntelVTdDxe/IntelVTdDxe.inf      |  79 ++
 IntelSiliconPkg/IntelVTdDxe/IntelVTdDxe.uni      |  20 +
 IntelSiliconPkg/IntelVTdDxe/IntelVTdDxeExtra.uni |  20 +
 IntelSiliconPkg/IntelVTdDxe/PciInfo.c            | 315 ++++++
 IntelSiliconPkg/IntelVTdDxe/TranslationTable.c   | 969 +++++++++++++++++++
 IntelSiliconPkg/IntelVTdDxe/TranslationTableEx.c | 153 +++
 IntelSiliconPkg/IntelVTdDxe/VtdReg.c             | 602 ++++++++++++
 12 files changed, 4818 insertions(+)

diff --git a/IntelSiliconPkg/IntelVTdDxe/BmDma.c b/IntelSiliconPkg/IntelVTdDxe/BmDma.c
new file mode 100644
index 0000000..5dcee00
--- /dev/null
+++ b/IntelSiliconPkg/IntelVTdDxe/BmDma.c
@@ -0,0 +1,441 @@
+/** @file
+  BmDma related function
+
+  Copyright (c) 2017, Intel Corporation. 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 <PiDxe.h>
+
+#include <Protocol/IoMmu.h>
+
+#include <Library/BaseLib.h>
+#include <Library/DebugLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+
+// TBD: May make it a policy
+#define DMA_MEMORY_TOP          MAX_UINTN
+//#define DMA_MEMORY_TOP          0x0000000001FFFFFFULL
+
+#define MAP_INFO_SIGNATURE  SIGNATURE_32 ('D', 'M', 'A', 'P')
+typedef struct {
+  UINT32                                    Signature;
+  LIST_ENTRY                                Link;
+  EDKII_IOMMU_OPERATION                     Operation;
+  UINTN                                     NumberOfBytes;
+  UINTN                                     NumberOfPages;
+  EFI_PHYSICAL_ADDRESS                      HostAddress;
+  EFI_PHYSICAL_ADDRESS                      DeviceAddress;
+} MAP_INFO;
+#define MAP_INFO_FROM_LINK(a) CR (a, MAP_INFO, Link, MAP_INFO_SIGNATURE)
+
+LIST_ENTRY                        gMaps = INITIALIZE_LIST_HEAD_VARIABLE(gMaps);
+
+/**
+  Provides the controller-specific addresses required to access system memory from a
+  DMA bus master.
+
+  @param  This                  The protocol instance pointer.
+  @param  Operation             Indicates if the bus master is going to read or write to system memory.
+  @param  HostAddress           The system memory address to map to the PCI controller.
+  @param  NumberOfBytes         On input the number of bytes to map. On output the number of bytes
+                                that were mapped.
+  @param  DeviceAddress         The resulting map address for the bus master PCI controller to use to
+                                access the hosts HostAddress.
+  @param  Mapping               A resulting value to pass to Unmap().
+
+  @retval EFI_SUCCESS           The range was mapped for the returned NumberOfBytes.
+  @retval EFI_UNSUPPORTED       The HostAddress cannot be mapped as a common buffer.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+  @retval EFI_DEVICE_ERROR      The system hardware could not map the requested address.
+
+**/
+EFI_STATUS
+EFIAPI
+IoMmuMap (
+  IN     EDKII_IOMMU_PROTOCOL                       *This,
+  IN     EDKII_IOMMU_OPERATION                      Operation,
+  IN     VOID                                       *HostAddress,
+  IN OUT UINTN                                      *NumberOfBytes,
+  OUT    EFI_PHYSICAL_ADDRESS                       *DeviceAddress,
+  OUT    VOID                                       **Mapping
+  )
+{
+  EFI_STATUS                                        Status;
+  EFI_PHYSICAL_ADDRESS                              PhysicalAddress;
+  MAP_INFO                                          *MapInfo;
+  EFI_PHYSICAL_ADDRESS                              DmaMemoryTop;
+  BOOLEAN                                           NeedRemap;
+
+  DEBUG ((DEBUG_VERBOSE, "IoMmuMap: ==> 0x%08x - 0x%08x (%x)\n", HostAddress, NumberOfBytes, Operation));
+
+  if (HostAddress == NULL || NumberOfBytes == NULL || DeviceAddress == NULL ||
+      Mapping == NULL) {
+    DEBUG ((DEBUG_ERROR, "IoMmuMap: %r\n", EFI_INVALID_PARAMETER));
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Make sure that Operation is valid
+  //
+  if ((UINT32) Operation >= EdkiiIoMmuOperationMaximum) {
+    DEBUG ((DEBUG_ERROR, "IoMmuMap: %r\n", EFI_INVALID_PARAMETER));
+    return EFI_INVALID_PARAMETER;
+  }
+  NeedRemap = FALSE;
+  PhysicalAddress = (EFI_PHYSICAL_ADDRESS) (UINTN) HostAddress;
+
+  DmaMemoryTop = DMA_MEMORY_TOP;
+
+  //
+  // Alignment check
+  //
+  if ((*NumberOfBytes != ALIGN_VALUE(*NumberOfBytes, SIZE_4KB)) ||
+      (PhysicalAddress != ALIGN_VALUE(PhysicalAddress, SIZE_4KB))) {
+    if ((Operation == EdkiiIoMmuOperationBusMasterCommonBuffer) ||
+        (Operation == EdkiiIoMmuOperationBusMasterCommonBuffer64)) {
+      //
+      // The input buffer might be a subset from IoMmuAllocateBuffer.
+      // Skip the check.
+      //
+    } else {
+      NeedRemap = TRUE;
+    }
+  }
+
+  if ((PhysicalAddress + *NumberOfBytes) >= DMA_MEMORY_TOP) {
+    NeedRemap = TRUE;
+  }
+
+  if (((Operation != EdkiiIoMmuOperationBusMasterRead64 &&
+        Operation != EdkiiIoMmuOperationBusMasterWrite64 &&
+        Operation != EdkiiIoMmuOperationBusMasterCommonBuffer64)) &&
+      ((PhysicalAddress + *NumberOfBytes) > SIZE_4GB)) {
+    //
+    // If the root bridge or the device cannot handle performing DMA above
+    // 4GB but any part of the DMA transfer being mapped is above 4GB, then
+    // map the DMA transfer to a buffer below 4GB.
+    //
+    NeedRemap = TRUE;
+    DmaMemoryTop = MIN (DmaMemoryTop, SIZE_4GB - 1);
+  }
+
+  if (Operation == EdkiiIoMmuOperationBusMasterCommonBuffer ||
+      Operation == EdkiiIoMmuOperationBusMasterCommonBuffer64) {
+    if (NeedRemap) {
+      //
+      // Common Buffer operations can not be remapped.  If the common buffer
+      // if above 4GB, then it is not possible to generate a mapping, so return
+      // an error.
+      //
+      DEBUG ((DEBUG_ERROR, "IoMmuMap: %r\n", EFI_UNSUPPORTED));
+      return EFI_UNSUPPORTED;
+    }
+  }
+
+  //
+  // Allocate a MAP_INFO structure to remember the mapping when Unmap() is
+  // called later.
+  //
+  MapInfo = AllocatePool (sizeof (MAP_INFO));
+  if (MapInfo == NULL) {
+    *NumberOfBytes = 0;
+    DEBUG ((DEBUG_ERROR, "IoMmuMap: %r\n", EFI_OUT_OF_RESOURCES));
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  // Initialize the MAP_INFO structure
+  //
+  MapInfo->Signature         = MAP_INFO_SIGNATURE;
+  MapInfo->Operation         = Operation;
+  MapInfo->NumberOfBytes     = *NumberOfBytes;
+  MapInfo->NumberOfPages     = EFI_SIZE_TO_PAGES (MapInfo->NumberOfBytes);
+  MapInfo->HostAddress       = PhysicalAddress;
+  MapInfo->DeviceAddress     = DmaMemoryTop;
+
+  //
+  // Allocate a buffer below 4GB to map the transfer to.
+  //
+  if (NeedRemap) {
+    Status = gBS->AllocatePages (
+                    AllocateMaxAddress,
+                    EfiBootServicesData,
+                    MapInfo->NumberOfPages,
+                    &MapInfo->DeviceAddress
+                    );
+    if (EFI_ERROR (Status)) {
+      FreePool (MapInfo);
+      *NumberOfBytes = 0;
+      DEBUG ((DEBUG_ERROR, "IoMmuMap: %r\n", Status));
+      return Status;
+    }
+
+    //
+    // If this is a read operation from the Bus Master's point of view,
+    // then copy the contents of the real buffer into the mapped buffer
+    // so the Bus Master can read the contents of the real buffer.
+    //
+    if (Operation == EdkiiIoMmuOperationBusMasterRead ||
+        Operation == EdkiiIoMmuOperationBusMasterRead64) {
+      CopyMem (
+        (VOID *) (UINTN) MapInfo->DeviceAddress,
+        (VOID *) (UINTN) MapInfo->HostAddress,
+        MapInfo->NumberOfBytes
+        );
+    }
+  } else {
+    MapInfo->DeviceAddress = MapInfo->HostAddress;
+  }
+
+  InsertTailList (&gMaps, &MapInfo->Link);
+
+  //
+  // The DeviceAddress is the address of the maped buffer below 4GB
+  //
+  *DeviceAddress = MapInfo->DeviceAddress;
+  //
+  // Return a pointer to the MAP_INFO structure in Mapping
+  //
+  *Mapping       = MapInfo;
+
+  DEBUG ((DEBUG_VERBOSE, "IoMmuMap: 0x%08x - 0x%08x <==\n", *DeviceAddress, *Mapping));
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Completes the Map() operation and releases any corresponding resources.
+
+  @param  This                  The protocol instance pointer.
+  @param  Mapping               The mapping value returned from Map().
+
+  @retval EFI_SUCCESS           The range was unmapped.
+  @retval EFI_INVALID_PARAMETER Mapping is not a value that was returned by Map().
+  @retval EFI_DEVICE_ERROR      The data was not committed to the target system memory.
+**/
+EFI_STATUS
+EFIAPI
+IoMmuUnmap (
+  IN  EDKII_IOMMU_PROTOCOL                     *This,
+  IN  VOID                                     *Mapping
+  )
+{
+  MAP_INFO                 *MapInfo;
+  LIST_ENTRY               *Link;
+
+  DEBUG ((DEBUG_VERBOSE, "IoMmuUnmap: 0x%08x\n", Mapping));
+
+  if (Mapping == NULL) {
+    DEBUG ((DEBUG_ERROR, "IoMmuUnmap: %r\n", EFI_INVALID_PARAMETER));
+    return EFI_INVALID_PARAMETER;
+  }
+
+  MapInfo = NULL;
+  for (Link = GetFirstNode (&gMaps)
+       ; !IsNull (&gMaps, Link)
+       ; Link = GetNextNode (&gMaps, Link)
+       ) {
+    MapInfo = MAP_INFO_FROM_LINK (Link);
+    if (MapInfo == Mapping) {
+      break;
+    }
+  }
+  //
+  // Mapping is not a valid value returned by Map()
+  //
+  if (MapInfo != Mapping) {
+    DEBUG ((DEBUG_ERROR, "IoMmuUnmap: %r\n", EFI_INVALID_PARAMETER));
+    return EFI_INVALID_PARAMETER;
+  }
+  RemoveEntryList (&MapInfo->Link);
+
+  if (MapInfo->DeviceAddress != MapInfo->HostAddress) {
+    //
+    // If this is a write operation from the Bus Master's point of view,
+    // then copy the contents of the mapped buffer into the real buffer
+    // so the processor can read the contents of the real buffer.
+    //
+    if (MapInfo->Operation == EdkiiIoMmuOperationBusMasterWrite ||
+        MapInfo->Operation == EdkiiIoMmuOperationBusMasterWrite64) {
+      CopyMem (
+        (VOID *) (UINTN) MapInfo->HostAddress,
+        (VOID *) (UINTN) MapInfo->DeviceAddress,
+        MapInfo->NumberOfBytes
+        );
+    }
+
+    //
+    // Free the mapped buffer and the MAP_INFO structure.
+    //
+    gBS->FreePages (MapInfo->DeviceAddress, MapInfo->NumberOfPages);
+  }
+
+  FreePool (Mapping);
+  return EFI_SUCCESS;
+}
+
+/**
+  Allocates pages that are suitable for an OperationBusMasterCommonBuffer or
+  OperationBusMasterCommonBuffer64 mapping.
+
+  @param  This                  The protocol instance pointer.
+  @param  Type                  This parameter is not used and must be ignored.
+  @param  MemoryType            The type of memory to allocate, EfiBootServicesData or
+                                EfiRuntimeServicesData.
+  @param  Pages                 The number of pages to allocate.
+  @param  HostAddress           A pointer to store the base system memory address of the
+                                allocated range.
+  @param  Attributes            The requested bit mask of attributes for the allocated range.
+
+  @retval EFI_SUCCESS           The requested memory pages were allocated.
+  @retval EFI_UNSUPPORTED       Attributes is unsupported. The only legal attribute bits are
+                                MEMORY_WRITE_COMBINE and MEMORY_CACHED.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+  @retval EFI_OUT_OF_RESOURCES  The memory pages could not be allocated.
+
+**/
+EFI_STATUS
+EFIAPI
+IoMmuAllocateBuffer (
+  IN     EDKII_IOMMU_PROTOCOL                     *This,
+  IN     EFI_ALLOCATE_TYPE                        Type,
+  IN     EFI_MEMORY_TYPE                          MemoryType,
+  IN     UINTN                                    Pages,
+  IN OUT VOID                                     **HostAddress,
+  IN     UINT64                                   Attributes
+  )
+{
+  EFI_STATUS                Status;
+  EFI_PHYSICAL_ADDRESS      PhysicalAddress;
+
+  DEBUG ((DEBUG_VERBOSE, "IoMmuAllocateBuffer: ==> 0x%08x\n", Pages));
+
+  //
+  // Validate Attributes
+  //
+  if ((Attributes & EDKII_IOMMU_ATTRIBUTE_INVALID_FOR_ALLOCATE_BUFFER) != 0) {
+    DEBUG ((DEBUG_ERROR, "IoMmuAllocateBuffer: %r\n", EFI_UNSUPPORTED));
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Check for invalid inputs
+  //
+  if (HostAddress == NULL) {
+    DEBUG ((DEBUG_ERROR, "IoMmuAllocateBuffer: %r\n", EFI_INVALID_PARAMETER));
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // The only valid memory types are EfiBootServicesData and
+  // EfiRuntimeServicesData
+  //
+  if (MemoryType != EfiBootServicesData &&
+      MemoryType != EfiRuntimeServicesData) {
+    DEBUG ((DEBUG_ERROR, "IoMmuAllocateBuffer: %r\n", EFI_INVALID_PARAMETER));
+    return EFI_INVALID_PARAMETER;
+  }
+
+  PhysicalAddress = DMA_MEMORY_TOP;
+  if ((Attributes & EDKII_IOMMU_ATTRIBUTE_DUAL_ADDRESS_CYCLE) == 0) {
+    //
+    // Limit allocations to memory below 4GB
+    //
+    PhysicalAddress = MIN (PhysicalAddress, SIZE_4GB - 1);
+  }
+  Status = gBS->AllocatePages (
+                  AllocateMaxAddress,
+                  MemoryType,
+                  Pages,
+                  &PhysicalAddress
+                  );
+  if (!EFI_ERROR (Status)) {
+    *HostAddress = (VOID *) (UINTN) PhysicalAddress;
+  }
+
+  DEBUG ((DEBUG_VERBOSE, "IoMmuAllocateBuffer: 0x%08x <==\n", *HostAddress));
+
+  return Status;
+}
+
+/**
+  Frees memory that was allocated with AllocateBuffer().
+
+  @param  This                  The protocol instance pointer.
+  @param  Pages                 The number of pages to free.
+  @param  HostAddress           The base system memory address of the allocated range.
+
+  @retval EFI_SUCCESS           The requested memory pages were freed.
+  @retval EFI_INVALID_PARAMETER The memory range specified by HostAddress and Pages
+                                was not allocated with AllocateBuffer().
+
+**/
+EFI_STATUS
+EFIAPI
+IoMmuFreeBuffer (
+  IN  EDKII_IOMMU_PROTOCOL                     *This,
+  IN  UINTN                                    Pages,
+  IN  VOID                                     *HostAddress
+  )
+{
+  DEBUG ((DEBUG_VERBOSE, "IoMmuFreeBuffer: 0x%\n", Pages));
+  return gBS->FreePages ((EFI_PHYSICAL_ADDRESS) (UINTN) HostAddress, Pages);
+}
+
+/**
+  Get device information from mapping.
+
+  @param[in]  Mapping        The mapping.
+  @param[out] DeviceAddress  The device address of the mapping.
+  @param[out] NumberOfPages  The number of pages of the mapping.
+
+  @retval EFI_SUCCESS            The device information is returned.
+  @retval EFI_INVALID_PARAMETER  The mapping is invalid.
+**/
+EFI_STATUS
+GetDeviceInfoFromMapping (
+  IN  VOID                                     *Mapping,
+  OUT EFI_PHYSICAL_ADDRESS                     *DeviceAddress,
+  OUT UINTN                                    *NumberOfPages
+  )
+{
+  MAP_INFO                 *MapInfo;
+  LIST_ENTRY               *Link;
+
+  if (Mapping == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  MapInfo = NULL;
+  for (Link = GetFirstNode (&gMaps)
+       ; !IsNull (&gMaps, Link)
+       ; Link = GetNextNode (&gMaps, Link)
+       ) {
+    MapInfo = MAP_INFO_FROM_LINK (Link);
+    if (MapInfo == Mapping) {
+      break;
+    }
+  }
+  //
+  // Mapping is not a valid value returned by Map()
+  //
+  if (MapInfo != Mapping) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  *DeviceAddress = MapInfo->DeviceAddress;
+  *NumberOfPages = MapInfo->NumberOfPages;
+  return EFI_SUCCESS;
+}
+
diff --git a/IntelSiliconPkg/IntelVTdDxe/DmaProtection.c b/IntelSiliconPkg/IntelVTdDxe/DmaProtection.c
new file mode 100644
index 0000000..f0628b5
--- /dev/null
+++ b/IntelSiliconPkg/IntelVTdDxe/DmaProtection.c
@@ -0,0 +1,367 @@
+/** @file
+
+  Copyright (c) 2017, Intel Corporation. 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 "DmaProtection.h"
+
+EFI_ACPI_SDT_PROTOCOL                   *mAcpiSdt;
+UINT64                                  mBelow4GMemoryLimit;
+UINT64                                  mAbove4GMemoryLimit;
+
+EDKII_PLATFORM_VTD_POLICY_PROTOCOL      *mPlatformVTdPolicy;
+
+/**
+  return the UEFI memory information.
+
+  @param[out] Below4GMemoryLimit  The below 4GiB memory limit
+  @param[out] Above4GMemoryLimit  The above 4GiB memory limit
+**/
+VOID
+ReturnUefiMemoryMap (
+  OUT UINT64   *Below4GMemoryLimit,
+  OUT UINT64   *Above4GMemoryLimit
+  )
+{
+  EFI_STATUS                  Status;
+  EFI_MEMORY_DESCRIPTOR       *EfiMemoryMap;
+  EFI_MEMORY_DESCRIPTOR       *EfiMemoryMapEnd;
+  EFI_MEMORY_DESCRIPTOR       *EfiEntry;
+  EFI_MEMORY_DESCRIPTOR       *NextEfiEntry;
+  EFI_MEMORY_DESCRIPTOR       TempEfiEntry;
+  UINTN                       EfiMemoryMapSize;
+  UINTN                       EfiMapKey;
+  UINTN                       EfiDescriptorSize;
+  UINT32                      EfiDescriptorVersion;
+  UINT64                      MemoryBlockLength;
+
+  *Below4GMemoryLimit = 0;
+  *Above4GMemoryLimit = 0;
+
+  //
+  // Get the EFI memory map.
+  //
+  EfiMemoryMapSize  = 0;
+  EfiMemoryMap      = NULL;
+  Status = gBS->GetMemoryMap (
+                  &EfiMemoryMapSize,
+                  EfiMemoryMap,
+                  &EfiMapKey,
+                  &EfiDescriptorSize,
+                  &EfiDescriptorVersion
+                  );
+  ASSERT (Status == EFI_BUFFER_TOO_SMALL);
+
+  do {
+    //
+    // Use size returned back plus 1 descriptor for the AllocatePool.
+    // We don't just multiply by 2 since the "for" loop below terminates on
+    // EfiMemoryMapEnd which is dependent upon EfiMemoryMapSize. Otherwize
+    // we process bogus entries and create bogus E820 entries.
+    //
+    EfiMemoryMap = (EFI_MEMORY_DESCRIPTOR *) AllocatePool (EfiMemoryMapSize);
+    ASSERT (EfiMemoryMap != NULL);
+    Status = gBS->GetMemoryMap (
+                    &EfiMemoryMapSize,
+                    EfiMemoryMap,
+                    &EfiMapKey,
+                    &EfiDescriptorSize,
+                    &EfiDescriptorVersion
+                    );
+    if (EFI_ERROR (Status)) {
+      FreePool (EfiMemoryMap);
+    }
+  } while (Status == EFI_BUFFER_TOO_SMALL);
+
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Sort memory map from low to high
+  //
+  EfiEntry        = EfiMemoryMap;
+  NextEfiEntry    = NEXT_MEMORY_DESCRIPTOR (EfiEntry, EfiDescriptorSize);
+  EfiMemoryMapEnd = (EFI_MEMORY_DESCRIPTOR *) ((UINT8 *) EfiMemoryMap + EfiMemoryMapSize);
+  while (EfiEntry < EfiMemoryMapEnd) {
+    while (NextEfiEntry < EfiMemoryMapEnd) {
+      if (EfiEntry->PhysicalStart > NextEfiEntry->PhysicalStart) {
+        CopyMem (&TempEfiEntry, EfiEntry, sizeof (EFI_MEMORY_DESCRIPTOR));
+        CopyMem (EfiEntry, NextEfiEntry, sizeof (EFI_MEMORY_DESCRIPTOR));
+        CopyMem (NextEfiEntry, &TempEfiEntry, sizeof (EFI_MEMORY_DESCRIPTOR));
+      }
+
+      NextEfiEntry = NEXT_MEMORY_DESCRIPTOR (NextEfiEntry, EfiDescriptorSize);
+    }
+
+    EfiEntry      = NEXT_MEMORY_DESCRIPTOR (EfiEntry, EfiDescriptorSize);
+    NextEfiEntry  = NEXT_MEMORY_DESCRIPTOR (EfiEntry, EfiDescriptorSize);
+  }
+
+  //
+  //
+  //
+  DEBUG ((DEBUG_INFO, "MemoryMap:\n"));
+  EfiEntry        = EfiMemoryMap;
+  EfiMemoryMapEnd = (EFI_MEMORY_DESCRIPTOR *) ((UINT8 *) EfiMemoryMap + EfiMemoryMapSize);
+  while (EfiEntry < EfiMemoryMapEnd) {
+    MemoryBlockLength = (UINT64) (LShiftU64 (EfiEntry->NumberOfPages, 12));
+    DEBUG ((DEBUG_INFO, "Entry(0x%02x) 0x%016lx - 0x%016lx\n", EfiEntry->Type, EfiEntry->PhysicalStart, EfiEntry->PhysicalStart + MemoryBlockLength));
+    switch (EfiEntry->Type) {
+    case EfiLoaderCode:
+    case EfiLoaderData:
+    case EfiBootServicesCode:
+    case EfiBootServicesData:
+    case EfiConventionalMemory:
+    case EfiRuntimeServicesCode:
+    case EfiRuntimeServicesData:
+    case EfiACPIReclaimMemory:
+    case EfiACPIMemoryNVS:
+    case EfiReservedMemoryType:
+      if ((EfiEntry->PhysicalStart + MemoryBlockLength) <= BASE_1MB) {
+        //
+        // Skip the memory block is under 1MB
+        //
+      } else if (EfiEntry->PhysicalStart >= BASE_4GB) {
+        if (*Above4GMemoryLimit < EfiEntry->PhysicalStart + MemoryBlockLength) {
+          *Above4GMemoryLimit = EfiEntry->PhysicalStart + MemoryBlockLength;
+        }
+      } else {
+        if (*Below4GMemoryLimit < EfiEntry->PhysicalStart + MemoryBlockLength) {
+          *Below4GMemoryLimit = EfiEntry->PhysicalStart + MemoryBlockLength;
+        }
+      }
+      break;
+    }
+    EfiEntry = NEXT_MEMORY_DESCRIPTOR (EfiEntry, EfiDescriptorSize);
+  }
+
+  FreePool (EfiMemoryMap);
+
+  DEBUG ((DEBUG_INFO, "Result:\n"));
+  DEBUG ((DEBUG_INFO, "Below4GMemoryLimit:  0x%016lx\n", *Below4GMemoryLimit));
+  DEBUG ((DEBUG_INFO, "Above4GMemoryLimit:  0x%016lx\n", *Above4GMemoryLimit));
+
+  return ;
+}
+
+/**
+  Initialize platform VTd policy.
+**/
+VOID
+InitializePlatformVTdPolicy (
+  VOID
+  )
+{
+  EFI_STATUS                        Status;
+  UINTN                             DeviceInfoCount;
+  EDKII_PLATFORM_VTD_DEVICE_INFO    *DeviceInfo;
+  UINTN                             Index;
+
+  //
+  // It is optional.
+  //
+  Status = gBS->LocateProtocol (
+                  &gEdkiiPlatformVTdPolicyProtocolGuid,
+                  NULL,
+                  (VOID **)&mPlatformVTdPolicy
+                  );
+  if (!EFI_ERROR(Status)) {
+    Status = mPlatformVTdPolicy->GetExceptionDeviceList (mPlatformVTdPolicy, &DeviceInfoCount, &DeviceInfo);
+    if (!EFI_ERROR(Status)) {
+      for (Index = 0; Index < DeviceInfoCount; Index++) {
+        AlwaysEnablePageAttribute (DeviceInfo[Index].Segment, DeviceInfo[Index].SourceId);
+      }
+      FreePool (DeviceInfo);
+    }
+  }
+}
+
+/**
+  Setup VTd engine.
+**/
+VOID
+SetupVtd (
+  VOID
+  )
+{
+  EFI_STATUS      Status;
+  VOID            *PciEnumerationComplete;
+  UINTN           Index;
+  UINT64          Below4GMemoryLimit;
+  UINT64          Above4GMemoryLimit;
+
+  //
+  // PCI Enumeration must be done
+  //
+  Status = gBS->LocateProtocol (
+                  &gEfiPciEnumerationCompleteProtocolGuid,
+                  NULL,
+                  &PciEnumerationComplete
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  ReturnUefiMemoryMap (&Below4GMemoryLimit, &Above4GMemoryLimit);
+  Below4GMemoryLimit = ALIGN_VALUE_UP(Below4GMemoryLimit, SIZE_256MB);
+  DEBUG ((DEBUG_INFO, " Adjusted Below4GMemoryLimit: 0x%016lx\n", Below4GMemoryLimit));
+
+  mBelow4GMemoryLimit = Below4GMemoryLimit;
+  mAbove4GMemoryLimit = Above4GMemoryLimit;
+
+  //
+  // 1. setup
+  //
+  DEBUG ((DEBUG_INFO, "GetDmarAcpiTable\n"));
+  Status = GetDmarAcpiTable ();
+  if (EFI_ERROR (Status)) {
+    return;
+  }
+  DEBUG ((DEBUG_INFO, "ParseDmarAcpiTable\n"));
+  Status = ParseDmarAcpiTableDrhd ();
+  if (EFI_ERROR (Status)) {
+    return;
+  }
+  DEBUG ((DEBUG_INFO, "PrepareVtdConfig\n"));
+  PrepareVtdConfig ();
+
+  //
+  // 2. initialization
+  //
+  DEBUG ((DEBUG_INFO, "SetupTranslationTable\n"));
+  Status = SetupTranslationTable ();
+  if (EFI_ERROR (Status)) {
+    return;
+  }
+
+  InitializePlatformVTdPolicy ();
+
+  ParseDmarAcpiTableRmrr ();
+
+  for (Index = 0; Index < mVtdUnitNumber; Index++) {
+    DEBUG ((DEBUG_INFO,"VTD Unit %d (Segment: %04x)\n", Index, mVtdUnitInformation[Index].Segment));
+    if (mVtdUnitInformation[Index].ExtRootEntryTable != NULL) {
+      DumpDmarExtContextEntryTable (mVtdUnitInformation[Index].ExtRootEntryTable);
+    }
+    if (mVtdUnitInformation[Index].RootEntryTable != NULL) {
+      DumpDmarContextEntryTable (mVtdUnitInformation[Index].RootEntryTable);
+    }
+  }
+
+  //
+  // 3. enable
+  //
+  DEBUG ((DEBUG_INFO, "EnableDmar\n"));
+  Status = EnableDmar ();
+  if (EFI_ERROR (Status)) {
+    return;
+  }
+  DEBUG ((DEBUG_INFO, "DumpVtdRegs\n"));
+  DumpVtdRegsAll ();
+}
+
+/**
+  ACPI notification function.
+
+  @param[in] Table    A pointer to the ACPI table header.
+  @param[in] Version  The ACPI table's version.
+  @param[in] TableKey The table key for this ACPI table.
+
+  @retval EFI_SUCCESS The notification function is executed.
+**/
+EFI_STATUS
+EFIAPI
+AcpiNotificationFunc (
+  IN EFI_ACPI_SDT_HEADER    *Table,
+  IN EFI_ACPI_TABLE_VERSION Version,
+  IN UINTN                  TableKey
+  )
+{
+  if (Table->Signature == EFI_ACPI_4_0_DMA_REMAPPING_TABLE_SIGNATURE) {
+    DEBUG((DEBUG_INFO, "Vtd AcpiNotificationFunc\n"));
+    SetupVtd ();
+  }
+  return EFI_SUCCESS;
+}
+
+/**
+  Exit boot service callback function.
+
+  @param[in]  Event    The event handle.
+  @param[in]  Context  The event content.
+**/
+VOID
+EFIAPI
+OnExitBootServices (
+  IN EFI_EVENT                               Event,
+  IN VOID                                    *Context
+  )
+{
+  DEBUG ((DEBUG_INFO, "Vtd OnExitBootServices\n"));
+  DumpVtdRegsAll ();
+  DisableDmar ();
+  DumpVtdRegsAll ();
+}
+
+/**
+  Legacy boot callback function.
+
+  @param[in]  Event    The event handle.
+  @param[in]  Context  The event content.
+**/
+VOID
+EFIAPI
+OnLegacyBoot (
+  EFI_EVENT                               Event,
+  VOID                                    *Context
+  )
+{
+  DEBUG ((DEBUG_INFO, "Vtd OnLegacyBoot\n"));
+  DumpVtdRegsAll ();
+  DisableDmar ();
+  DumpVtdRegsAll ();
+}
+
+/**
+  Initialize DMA protection.
+**/
+VOID
+InitializeDmaProtection (
+  VOID
+  )
+{
+  EFI_STATUS  Status;
+  EFI_EVENT   ExitBootServicesEvent;
+  EFI_EVENT   LegacyBootEvent;
+
+  Status = gBS->LocateProtocol (&gEfiAcpiSdtProtocolGuid, NULL, (VOID **) &mAcpiSdt);
+  ASSERT_EFI_ERROR (Status);
+
+  Status = mAcpiSdt->RegisterNotify (TRUE, AcpiNotificationFunc);
+  ASSERT_EFI_ERROR (Status);
+
+  Status = gBS->CreateEventEx (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  OnExitBootServices,
+                  NULL,
+                  &gEfiEventExitBootServicesGuid,
+                  &ExitBootServicesEvent
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  Status = EfiCreateEventLegacyBootEx (
+             TPL_NOTIFY,
+             OnLegacyBoot,
+             NULL,
+             &LegacyBootEvent
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  return ;
+}
diff --git a/IntelSiliconPkg/IntelVTdDxe/DmaProtection.h b/IntelSiliconPkg/IntelVTdDxe/DmaProtection.h
new file mode 100644
index 0000000..6efed6e
--- /dev/null
+++ b/IntelSiliconPkg/IntelVTdDxe/DmaProtection.h
@@ -0,0 +1,501 @@
+/** @file
+
+  Copyright (c) 2017, Intel Corporation. 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 _DMAR_PROTECTION_H_
+#define _DMAR_PROTECTION_H_
+
+#include <Uefi.h>
+#include <PiDxe.h>
+
+#include <Library/BaseLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/IoLib.h>
+#include <Library/PciSegmentLib.h>
+#include <Library/DebugLib.h>
+#include <Library/UefiLib.h>
+
+#include <Guid/EventGroup.h>
+#include <Guid/Acpi.h>
+
+#include <Protocol/DxeSmmReadyToLock.h>
+#include <Protocol/PciRootBridgeIo.h>
+#include <Protocol/PciIo.h>
+#include <Protocol/PciEnumerationComplete.h>
+#include <Protocol/AcpiSystemDescriptionTable.h>
+#include <Protocol/PlatformVtdPolicy.h>
+#include <Protocol/IoMmu.h>
+
+#include <IndustryStandard/Pci.h>
+#include <IndustryStandard/DmaRemappingReportingTable.h>
+#include <IndustryStandard/Vtd.h>
+
+#define ALIGN_VALUE_UP(Value, Alignment)  (((Value) + (Alignment) - 1) & (~((Alignment) - 1)))
+#define ALIGN_VALUE_LOW(Value, Alignment) ((Value) & (~((Alignment) - 1)))
+
+//
+// This is the initial max PCI descriptor.
+// The number may be enlarged later.
+//
+#define MAX_PCI_DESCRIPTORS             0x100
+
+typedef struct {
+  BOOLEAN                IncludeAllFlag;
+  UINTN                  PciDescriptorNumber;
+  UINTN                  PciDescriptorMaxNumber;
+  BOOLEAN                *IsRealPciDevice;
+  VTD_SOURCE_ID          *PciDescriptors;
+} PCI_DEVICE_INFORMATION;
+
+typedef struct {
+  UINTN                            VtdUnitBaseAddress;
+  UINT16                           Segment;
+  VTD_CAP_REG                      CapReg;
+  VTD_ECAP_REG                     ECapReg;
+  VTD_ROOT_ENTRY                   *RootEntryTable;
+  VTD_EXT_ROOT_ENTRY               *ExtRootEntryTable;
+  VTD_SECOND_LEVEL_PAGING_ENTRY    *FixedSecondLevelPagingEntry;
+  BOOLEAN                          HasDirtyPages;
+  PCI_DEVICE_INFORMATION           PciDeviceInfo;
+} VTD_UNIT_INFORMATION;
+
+extern EFI_ACPI_DMAR_HEADER  *mAcpiDmarTable;
+
+extern UINT64                           mVtdHostAddressWidthMask;
+extern UINTN                            mVtdUnitNumber;
+extern VTD_UNIT_INFORMATION             *mVtdUnitInformation;
+
+extern UINT64                           mBelow4GMemoryLimit;
+extern UINT64                           mAbove4GMemoryLimit;
+
+extern EDKII_PLATFORM_VTD_POLICY_PROTOCOL   *mPlatformVTdPolicy;
+
+/**
+  Prepare VTD configuration.
+**/
+VOID
+PrepareVtdConfig (
+  VOID
+  );
+
+/**
+  Setup VTd translation table.
+
+  @retval EFI_SUCCESS           Setup translation table successfully.
+  @retval EFI_OUT_OF_RESOURCE   Setup translation table fail.
+**/
+EFI_STATUS
+SetupTranslationTable (
+  VOID
+  );
+
+/**
+  Enable DMAR translation.
+
+  @retval EFI_SUCCESS           DMAR translation is enabled.
+  @retval EFI_DEVICE_ERROR      DMAR translation is not enabled.
+**/
+EFI_STATUS
+EnableDmar (
+  VOID
+  );
+
+/**
+  Disable DMAR translation.
+
+  @retval EFI_SUCCESS           DMAR translation is disabled.
+  @retval EFI_DEVICE_ERROR      DMAR translation is not disabled.
+**/
+EFI_STATUS
+DisableDmar (
+  VOID
+  );
+
+/**
+  Invalid VTd IOTLB page.
+
+  @param[in]  VtdIndex              The index of VTd engine.
+  @param[in]  Address               The address of IOTLB page.
+  @param[in]  AddressMode           The address mode of IOTLB page.
+  @param[in]  DomainIdentifier      The domain ID of the source.
+
+  @retval EFI_SUCCESS           VTd IOTLB page is invalidated.
+  @retval EFI_DEVICE_ERROR      VTd IOTLB page is not invalidated.
+**/
+EFI_STATUS
+InvalidateVtdIOTLBPage (
+  IN UINTN  VtdIndex,
+  IN UINT64 Address,
+  IN UINT8  AddressMode,
+  IN UINT16 DomainIdentifier
+  );
+
+/**
+  Invalid VTd IOTLB domain.
+
+  @param[in]  VtdIndex              The index of VTd engine.
+  @param[in]  DomainIdentifier      The domain ID of the source.
+
+  @retval EFI_SUCCESS           VTd IOTLB domain is invalidated.
+  @retval EFI_DEVICE_ERROR      VTd IOTLB domain is not invalidated.
+**/
+EFI_STATUS
+InvalidateVtdIOTLBDomain (
+  IN UINTN  VtdIndex,
+  IN UINT16 DomainIdentifier
+  );
+
+/**
+  Invalid VTd global IOTLB.
+
+  @param[in]  VtdIndex              The index of VTd engine.
+
+  @retval EFI_SUCCESS           VTd global IOTLB is invalidated.
+  @retval EFI_DEVICE_ERROR      VTd global IOTLB is not invalidated.
+**/
+EFI_STATUS
+InvalidateVtdIOTLBGlobal (
+  IN UINTN  VtdIndex
+  );
+
+/**
+  Dump VTd registers.
+
+  @param[in]  VtdIndex              The index of VTd engine.
+**/
+VOID
+DumpVtdRegs (
+  IN UINTN  VtdIndex
+  );
+
+/**
+  Dump VTd registers for all VTd engine.
+**/
+VOID
+DumpVtdRegsAll (
+  VOID
+  );
+
+/**
+  Dump VTd capability registers.
+
+  @param[in]  CapReg              The capability register.
+**/
+VOID
+DumpVtdCapRegs (
+  IN VTD_CAP_REG *CapReg
+  );
+
+/**
+  Dump VTd extended capability registers.
+
+  @param[in]  ECapReg              The extended capability register.
+**/
+VOID
+DumpVtdECapRegs (
+  IN VTD_ECAP_REG *ECapReg
+  );
+
+/**
+  Register PCI device to VTd engine as PCI descriptor.
+
+  @param[in]  VtdIndex              The index of VTd engine.
+  @param[in]  Segment               The segment of the source.
+  @param[in]  SourceId              The SourceId of the source.
+  @param[in]  IsRealPciDevice       TRUE: It is a real PCI device.
+                                    FALSE: It is not a real PCI device.
+  @param[in]  CheckExist            TRUE: ERROR will be returned if the PCI device is already registered.
+                                    FALSE: SUCCESS will be returned if the PCI device is registered.
+
+  @retval EFI_SUCCESS           The PCI device is registered.
+  @retval EFI_OUT_OF_RESOURCES  No enough resource to register a new PCI device.
+  @retval EFI_ALREADY_STARTED   The device is already registered.
+**/
+EFI_STATUS
+RegisterPciDevice (
+  IN UINTN          VtdIndex,
+  IN UINT16         Segment,
+  IN VTD_SOURCE_ID  SourceId,
+  IN BOOLEAN        IsRealPciDevice,
+  IN BOOLEAN        CheckExist
+  );
+
+/**
+  Scan PCI bus and register PCI devices under the bus.
+
+  @param[in]  VtdIndex              The index of VTd engine.
+  @param[in]  Segment               The segment of the source.
+  @param[in]  Bus                   The bus of the source.
+
+  @retval EFI_SUCCESS           The PCI devices under the bus are registered.
+  @retval EFI_OUT_OF_RESOURCES  No enough resource to register a new PCI device.
+**/
+EFI_STATUS
+ScanPciBus (
+  IN UINTN          VtdIndex,
+  IN UINT16         Segment,
+  IN UINT8          Bus
+  );
+
+/**
+  Dump the PCI device information managed by this VTd engine.
+
+  @param[in]  VtdIndex              The index of VTd engine.
+**/
+VOID
+DumpPciDeviceInfo (
+  IN UINTN  VtdIndex
+  );
+
+/**
+  Find the VTd index by the Segment and SourceId.
+
+  @param[in]  Segment               The segment of the source.
+  @param[in]  SourceId              The SourceId of the source.
+  @param[out] ExtContextEntry       The ExtContextEntry of the source.
+  @param[out] ContextEntry          The ContextEntry of the source.
+
+  @return The index of the PCI descriptor.
+  @retval (UINTN)-1  The PCI descriptor is not found.
+**/
+UINTN
+FindVtdIndexByPciDevice (
+  IN  UINT16                  Segment,
+  IN  VTD_SOURCE_ID           SourceId,
+  OUT VTD_EXT_CONTEXT_ENTRY   **ExtContextEntry,
+  OUT VTD_CONTEXT_ENTRY       **ContextEntry
+  );
+
+/**
+  Get the DMAR ACPI table.
+
+  @retval EFI_SUCCESS    The DMAR ACPI table is got.
+  @retval EFI_NOT_FOUND  The DMAR ACPI table is not found.
+**/
+EFI_STATUS
+GetDmarAcpiTable (
+  VOID
+  );
+
+/**
+  Parse DMAR DRHD table.
+
+  @return EFI_SUCCESS  The DMAR DRHD table is parsed.
+**/
+EFI_STATUS
+ParseDmarAcpiTableDrhd (
+  VOID
+  );
+
+/**
+  Parse DMAR RMRR table.
+
+  @return EFI_SUCCESS  The DMAR RMRR table is parsed.
+**/
+EFI_STATUS
+ParseDmarAcpiTableRmrr (
+  VOID
+  );
+
+/**
+  Dump DMAR context entry table.
+
+  @param[in]  RootEntry DMAR root entry.
+**/
+VOID
+DumpDmarContextEntryTable (
+  IN VTD_ROOT_ENTRY *RootEntry
+  );
+
+/**
+  Dump DMAR extended context entry table.
+
+  @param[in]  ExtRootEntry DMAR extended root entry.
+**/
+VOID
+DumpDmarExtContextEntryTable (
+  IN VTD_EXT_ROOT_ENTRY *ExtRootEntry
+  );
+
+/**
+  Dump DMAR second level paging entry.
+
+  @param[in]  SecondLevelPagingEntry The second level paging entry.
+**/
+VOID
+DumpSecondLevelPagingEntry (
+  IN VOID *SecondLevelPagingEntry
+  );
+
+/**
+  Set VTd attribute for a system memory.
+
+  @param[in]  VtdIndex                The index used to identify a VTd engine.
+  @param[in]  SecondLevelPagingEntry  The second level paging entry in VTd table for the device.
+  @param[in]  BaseAddress             The base of device memory address to be used as the DMA memory.
+  @param[in]  Length                  The length of device memory address to be used as the DMA memory.
+  @param[in]  IoMmuAccess             The IOMMU access.
+
+  @retval EFI_SUCCESS            The IoMmuAccess is set for the memory range specified by BaseAddress and Length.
+  @retval EFI_INVALID_PARAMETER  BaseAddress is not IoMmu Page size aligned.
+  @retval EFI_INVALID_PARAMETER  Length is not IoMmu Page size aligned.
+  @retval EFI_INVALID_PARAMETER  Length is 0.
+  @retval EFI_INVALID_PARAMETER  IoMmuAccess specified an illegal combination of access.
+  @retval EFI_UNSUPPORTED        The bit mask of IoMmuAccess is not supported by the IOMMU.
+  @retval EFI_UNSUPPORTED        The IOMMU does not support the memory range specified by BaseAddress and Length.
+  @retval EFI_OUT_OF_RESOURCES   There are not enough resources available to modify the IOMMU access.
+  @retval EFI_DEVICE_ERROR       The IOMMU device reported an error while attempting the operation.
+**/
+EFI_STATUS
+SetPageAttribute (
+  IN UINTN                         VtdIndex,
+  IN VTD_SECOND_LEVEL_PAGING_ENTRY *SecondLevelPagingEntry,
+  IN UINT64                        BaseAddress,
+  IN UINT64                        Length,
+  IN UINT64                        IoMmuAccess
+  );
+
+/**
+  Set VTd attribute for a system memory.
+
+  @param[in]  Segment           The Segment used to identify a VTd engine.
+  @param[in]  SourceId          The SourceId used to identify a VTd engine and table entry.
+  @param[in]  BaseAddress       The base of device memory address to be used as the DMA memory.
+  @param[in]  Length            The length of device memory address to be used as the DMA memory.
+  @param[in]  IoMmuAccess       The IOMMU access.
+
+  @retval EFI_SUCCESS            The IoMmuAccess is set for the memory range specified by BaseAddress and Length.
+  @retval EFI_INVALID_PARAMETER  BaseAddress is not IoMmu Page size aligned.
+  @retval EFI_INVALID_PARAMETER  Length is not IoMmu Page size aligned.
+  @retval EFI_INVALID_PARAMETER  Length is 0.
+  @retval EFI_INVALID_PARAMETER  IoMmuAccess specified an illegal combination of access.
+  @retval EFI_UNSUPPORTED        The bit mask of IoMmuAccess is not supported by the IOMMU.
+  @retval EFI_UNSUPPORTED        The IOMMU does not support the memory range specified by BaseAddress and Length.
+  @retval EFI_OUT_OF_RESOURCES   There are not enough resources available to modify the IOMMU access.
+  @retval EFI_DEVICE_ERROR       The IOMMU device reported an error while attempting the operation.
+**/
+EFI_STATUS
+SetAccessAttribute (
+  IN UINT16                Segment,
+  IN VTD_SOURCE_ID         SourceId,
+  IN UINT64                BaseAddress,
+  IN UINT64                Length,
+  IN UINT64                IoMmuAccess
+  );
+
+/**
+  Return the index of PCI descriptor.
+
+  @param[in]  VtdIndex          The index used to identify a VTd engine.
+  @param[in]  Segment           The Segment used to identify a VTd engine.
+  @param[in]  SourceId          The SourceId used to identify a VTd engine and table entry.
+
+  @return The index of the PCI descriptor.
+  @retval (UINTN)-1  The PCI descriptor is not found.
+**/
+UINTN
+GetPciDescriptor (
+  IN UINTN          VtdIndex,
+  IN UINT16         Segment,
+  IN VTD_SOURCE_ID  SourceId
+  );
+
+/**
+  Dump VTd registers if there is error.
+**/
+VOID
+DumpVtdIfError (
+  VOID
+  );
+
+/**
+  Initialize platform VTd policy.
+**/
+VOID
+InitializePlatformVTdPolicy (
+  VOID
+  );
+
+/**
+  Always enable the VTd page attribute for the device.
+
+  @param[in]  Segment           The Segment used to identify a VTd engine.
+  @param[in]  SourceId          The SourceId used to identify a VTd engine and table entry.
+
+  @retval EFI_SUCCESS           The VTd entry is updated to always enable all DMA access for the specific device.
+**/
+EFI_STATUS
+AlwaysEnablePageAttribute (
+  IN UINT16                  Segment,
+  IN VTD_SOURCE_ID           SourceId
+  );
+
+/**
+  Convert the DeviceHandle to SourceId and Segment.
+
+  @param[in]  DeviceHandle      The device who initiates the DMA access request.
+  @param[out] Segment           The Segment used to identify a VTd engine.
+  @param[out] SourceId          The SourceId used to identify a VTd engine and table entry.
+
+  @retval EFI_SUCCESS            The Segment and SourceId are returned.
+  @retval EFI_INVALID_PARAMETER  DeviceHandle is an invalid handle.
+  @retval EFI_UNSUPPORTED        DeviceHandle is unknown by the IOMMU.
+**/
+EFI_STATUS
+DeviceHandleToSourceId (
+  IN EFI_HANDLE            DeviceHandle,
+  OUT UINT16               *Segment,
+  OUT VTD_SOURCE_ID        *SourceId
+  );
+
+/**
+  Get device information from mapping.
+
+  @param[in]  Mapping        The mapping.
+  @param[out] DeviceAddress  The device address of the mapping.
+  @param[out] NumberOfPages  The number of pages of the mapping.
+
+  @retval EFI_SUCCESS            The device information is returned.
+  @retval EFI_INVALID_PARAMETER  The mapping is invalid.
+**/
+EFI_STATUS
+GetDeviceInfoFromMapping (
+  IN  VOID                                     *Mapping,
+  OUT EFI_PHYSICAL_ADDRESS                     *DeviceAddress,
+  OUT UINTN                                    *NumberOfPages
+  );
+
+/**
+  Initialize DMA protection.
+**/
+VOID
+InitializeDmaProtection (
+  VOID
+  );
+
+/**
+  Allocate zero pages.
+
+  @param[in]  Pages the number of pages.
+
+  @return the page address.
+  @retval NULL No resource to allocate pages.
+**/
+VOID *
+EFIAPI
+AllocateZeroPages (
+  IN UINTN  Pages
+  );
+
+#endif
diff --git a/IntelSiliconPkg/IntelVTdDxe/DmarAcpiTable.c b/IntelSiliconPkg/IntelVTdDxe/DmarAcpiTable.c
new file mode 100644
index 0000000..84b5485
--- /dev/null
+++ b/IntelSiliconPkg/IntelVTdDxe/DmarAcpiTable.c
@@ -0,0 +1,998 @@
+/** @file
+
+  Copyright (c) 2017, Intel Corporation. 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 "DmaProtection.h"
+
+#pragma pack(1)
+
+typedef struct {
+  EFI_ACPI_DESCRIPTION_HEADER  Header;
+  UINT32                       Entry;
+} RSDT_TABLE;
+
+typedef struct {
+  EFI_ACPI_DESCRIPTION_HEADER  Header;
+  UINT64                       Entry;
+} XSDT_TABLE;
+
+#pragma pack()
+
+EFI_ACPI_DMAR_HEADER  *mAcpiDmarTable;
+
+/**
+  Dump DMAR DeviceScopeEntry.
+
+  @param[in]  DmarDeviceScopeEntry  DMAR DeviceScopeEntry
+**/
+VOID
+DumpDmarDeviceScopeEntry (
+  IN EFI_ACPI_DMAR_DEVICE_SCOPE_STRUCTURE_HEADER     *DmarDeviceScopeEntry
+  )
+{
+  UINTN   PciPathNumber;
+  UINTN   PciPathIndex;
+  EFI_ACPI_DMAR_PCI_PATH  *PciPath;
+
+  if (DmarDeviceScopeEntry == NULL) {
+    return;
+  }
+
+  DEBUG ((DEBUG_INFO,
+    "    *************************************************************************\n"
+    "    *       DMA-Remapping Device Scope Entry Structure                      *\n"
+    "    *************************************************************************\n"
+    ));
+  DEBUG ((DEBUG_INFO,
+    (sizeof(UINTN) == sizeof(UINT64)) ?
+    "    DMAR Device Scope Entry address ...................... 0x%016lx\n" :
+    "    DMAR Device Scope Entry address ...................... 0x%08x\n",
+    DmarDeviceScopeEntry
+    ));
+  DEBUG ((DEBUG_INFO,
+    "      Device Scope Entry Type ............................ 0x%02x\n",
+    DmarDeviceScopeEntry->Type
+    ));
+  switch (DmarDeviceScopeEntry->Type) {
+  case EFI_ACPI_DEVICE_SCOPE_ENTRY_TYPE_PCI_ENDPOINT:
+    DEBUG ((DEBUG_INFO,
+      "        PCI Endpoint Device\n"
+      ));
+    break;
+  case EFI_ACPI_DEVICE_SCOPE_ENTRY_TYPE_PCI_BRIDGE:
+    DEBUG ((DEBUG_INFO,
+      "        PCI Sub-hierachy\n"
+      ));
+    break;
+  case EFI_ACPI_DEVICE_SCOPE_ENTRY_TYPE_IOAPIC:
+    DEBUG ((DEBUG_INFO,
+      "        IOAPIC\n"
+      ));
+    break;
+  case EFI_ACPI_DEVICE_SCOPE_ENTRY_TYPE_MSI_CAPABLE_HPET:
+    DEBUG ((DEBUG_INFO,
+      "        MSI Capable HPET\n"
+      ));
+    break;
+  case EFI_ACPI_DEVICE_SCOPE_ENTRY_TYPE_ACPI_NAMESPACE_DEVICE:
+    DEBUG ((DEBUG_INFO,
+      "        ACPI Namespace Device\n"
+      ));
+    break;
+  default:
+    break;
+  }
+  DEBUG ((DEBUG_INFO,
+    "      Length ............................................. 0x%02x\n",
+    DmarDeviceScopeEntry->Length
+    ));
+  DEBUG ((DEBUG_INFO,
+    "      Enumeration ID ..................................... 0x%02x\n",
+    DmarDeviceScopeEntry->EnumerationId
+    ));
+  DEBUG ((DEBUG_INFO,
+    "      Starting Bus Number ................................ 0x%02x\n",
+    DmarDeviceScopeEntry->StartBusNumber
+    ));
+
+  PciPathNumber = (DmarDeviceScopeEntry->Length - sizeof(EFI_ACPI_DMAR_DEVICE_SCOPE_STRUCTURE_HEADER)) / sizeof(EFI_ACPI_DMAR_PCI_PATH);
+  PciPath = (EFI_ACPI_DMAR_PCI_PATH *)(DmarDeviceScopeEntry + 1);
+  for (PciPathIndex = 0; PciPathIndex < PciPathNumber; PciPathIndex++) {
+    DEBUG ((DEBUG_INFO,
+      "      Device ............................................. 0x%02x\n",
+      PciPath[PciPathIndex].Device
+      ));
+    DEBUG ((DEBUG_INFO,
+      "      Function ........................................... 0x%02x\n",
+      PciPath[PciPathIndex].Function
+      ));
+  }
+
+  DEBUG ((DEBUG_INFO,
+    "    *************************************************************************\n\n"
+    ));
+
+  return;
+}
+
+/**
+  Dump DMAR ANDD table.
+
+  @param[in]  Andd  DMAR ANDD table
+**/
+VOID
+DumpDmarAndd (
+  IN EFI_ACPI_DMAR_ANDD_HEADER *Andd
+  )
+{
+  if (Andd == NULL) {
+    return;
+  }
+
+  DEBUG ((DEBUG_INFO,
+    "  ***************************************************************************\n"
+    "  *       ACPI Name-space Device Declaration Structure                      *\n"
+    "  ***************************************************************************\n"
+    ));
+  DEBUG ((DEBUG_INFO,
+    (sizeof(UINTN) == sizeof(UINT64)) ?
+    "  ANDD address ........................................... 0x%016lx\n" :
+    "  ANDD address ........................................... 0x%08x\n",
+    Andd
+    ));
+  DEBUG ((DEBUG_INFO,
+    "    Type ................................................. 0x%04x\n",
+    Andd->Header.Type
+    ));
+  DEBUG ((DEBUG_INFO,
+    "    Length ............................................... 0x%04x\n",
+    Andd->Header.Length
+    ));
+  DEBUG ((DEBUG_INFO,
+    "    ACPI Device Number ................................... 0x%02x\n",
+    Andd->AcpiDeviceNumber
+    ));
+  DEBUG ((DEBUG_INFO,
+    "    ACPI Object Name ..................................... '%a'\n",
+    (Andd + 1)
+    ));
+
+  DEBUG ((DEBUG_INFO,
+    "  ***************************************************************************\n\n"
+    ));
+
+  return;
+}
+
+/**
+  Dump DMAR RHSA table.
+
+  @param[in]  Rhsa  DMAR RHSA table
+**/
+VOID
+DumpDmarRhsa (
+  IN EFI_ACPI_DMAR_RHSA_HEADER *Rhsa
+  )
+{
+  if (Rhsa == NULL) {
+    return;
+  }
+
+  DEBUG ((DEBUG_INFO,
+    "  ***************************************************************************\n"
+    "  *       Remapping Hardware Status Affinity Structure                      *\n"
+    "  ***************************************************************************\n"
+    ));
+  DEBUG ((DEBUG_INFO,
+    (sizeof(UINTN) == sizeof(UINT64)) ?
+    "  RHSA address ........................................... 0x%016lx\n" :
+    "  RHSA address ........................................... 0x%08x\n",
+    Rhsa
+    ));
+  DEBUG ((DEBUG_INFO,
+    "    Type ................................................. 0x%04x\n",
+    Rhsa->Header.Type
+    ));
+  DEBUG ((DEBUG_INFO,
+    "    Length ............................................... 0x%04x\n",
+    Rhsa->Header.Length
+    ));
+  DEBUG ((DEBUG_INFO,
+    "    Register Base Address ................................ 0x%016lx\n",
+    Rhsa->RegisterBaseAddress
+    ));
+  DEBUG ((DEBUG_INFO,
+    "    Proximity Domain ..................................... 0x%08x\n",
+    Rhsa->ProximityDomain
+    ));
+
+  DEBUG ((DEBUG_INFO,
+    "  ***************************************************************************\n\n"
+    ));
+
+  return;
+}
+
+/**
+  Dump DMAR ATSR table.
+
+  @param[in]  Atsr  DMAR ATSR table
+**/
+VOID
+DumpDmarAtsr (
+  IN EFI_ACPI_DMAR_ATSR_HEADER *Atsr
+  )
+{
+  EFI_ACPI_DMAR_DEVICE_SCOPE_STRUCTURE_HEADER       *DmarDeviceScopeEntry;
+  INTN                                    AtsrLen;
+
+  if (Atsr == NULL) {
+    return;
+  }
+
+  DEBUG ((DEBUG_INFO,
+    "  ***************************************************************************\n"
+    "  *       Root Port ATS Capability Reporting Structure                      *\n"
+    "  ***************************************************************************\n"
+    ));
+  DEBUG ((DEBUG_INFO,
+    (sizeof(UINTN) == sizeof(UINT64)) ?
+    "  ATSR address ........................................... 0x%016lx\n" :
+    "  ATSR address ........................................... 0x%08x\n",
+    Atsr
+    ));
+  DEBUG ((DEBUG_INFO,
+    "    Type ................................................. 0x%04x\n",
+    Atsr->Header.Type
+    ));
+  DEBUG ((DEBUG_INFO,
+    "    Length ............................................... 0x%04x\n",
+    Atsr->Header.Length
+    ));
+  DEBUG ((DEBUG_INFO,
+    "    Flags ................................................ 0x%02x\n",
+    Atsr->Flags
+    ));
+  DEBUG ((DEBUG_INFO,
+    "      ALL_PORTS .......................................... 0x%02x\n",
+    Atsr->Flags & EFI_ACPI_DMAR_ATSR_FLAGS_ALL_PORTS
+    ));
+  DEBUG ((DEBUG_INFO,
+    "    Segment Number ....................................... 0x%04x\n",
+    Atsr->SegmentNumber
+    ));
+
+  AtsrLen  = Atsr->Header.Length - sizeof(EFI_ACPI_DMAR_ATSR_HEADER);
+  DmarDeviceScopeEntry = (EFI_ACPI_DMAR_DEVICE_SCOPE_STRUCTURE_HEADER *)(Atsr + 1);
+  while (AtsrLen > 0) {
+    DumpDmarDeviceScopeEntry (DmarDeviceScopeEntry);
+    AtsrLen -= DmarDeviceScopeEntry->Length;
+    DmarDeviceScopeEntry = (EFI_ACPI_DMAR_DEVICE_SCOPE_STRUCTURE_HEADER *)((UINTN)DmarDeviceScopeEntry + DmarDeviceScopeEntry->Length);
+  }
+
+  DEBUG ((DEBUG_INFO,
+    "  ***************************************************************************\n\n"
+    ));
+
+  return;
+}
+
+/**
+  Dump DMAR RMRR table.
+
+  @param[in]  Rmrr  DMAR RMRR table
+**/
+VOID
+DumpDmarRmrr (
+  IN EFI_ACPI_DMAR_RMRR_HEADER *Rmrr
+  )
+{
+  EFI_ACPI_DMAR_DEVICE_SCOPE_STRUCTURE_HEADER       *DmarDeviceScopeEntry;
+  INTN                                    RmrrLen;
+
+  if (Rmrr == NULL) {
+    return;
+  }
+
+  DEBUG ((DEBUG_INFO,
+    "  ***************************************************************************\n"
+    "  *       Reserved Memory Region Reporting Structure                        *\n"
+    "  ***************************************************************************\n"
+    ));
+  DEBUG ((DEBUG_INFO,
+    (sizeof(UINTN) == sizeof(UINT64)) ?
+    "  RMRR address ........................................... 0x%016lx\n" :
+    "  RMRR address ........................................... 0x%08x\n",
+    Rmrr
+    ));
+  DEBUG ((DEBUG_INFO,
+    "    Type ................................................. 0x%04x\n",
+    Rmrr->Header.Type
+    ));
+  DEBUG ((DEBUG_INFO,
+    "    Length ............................................... 0x%04x\n",
+    Rmrr->Header.Length
+    ));
+  DEBUG ((DEBUG_INFO,
+    "    Segment Number ....................................... 0x%04x\n",
+    Rmrr->SegmentNumber
+    ));
+  DEBUG ((DEBUG_INFO,
+    "    Reserved Memory Region Base Address .................. 0x%016lx\n",
+    Rmrr->ReservedMemoryRegionBaseAddress
+    ));
+  DEBUG ((DEBUG_INFO,
+    "    Reserved Memory Region Limit Address ................. 0x%016lx\n",
+    Rmrr->ReservedMemoryRegionLimitAddress
+    ));
+
+  RmrrLen  = Rmrr->Header.Length - sizeof(EFI_ACPI_DMAR_RMRR_HEADER);
+  DmarDeviceScopeEntry = (EFI_ACPI_DMAR_DEVICE_SCOPE_STRUCTURE_HEADER *)(Rmrr + 1);
+  while (RmrrLen > 0) {
+    DumpDmarDeviceScopeEntry (DmarDeviceScopeEntry);
+    RmrrLen -= DmarDeviceScopeEntry->Length;
+    DmarDeviceScopeEntry = (EFI_ACPI_DMAR_DEVICE_SCOPE_STRUCTURE_HEADER *)((UINTN)DmarDeviceScopeEntry + DmarDeviceScopeEntry->Length);
+  }
+
+  DEBUG ((DEBUG_INFO,
+    "  ***************************************************************************\n\n"
+    ));
+
+  return;
+}
+
+/**
+  Dump DMAR DRHD table.
+
+  @param[in]  Drhd  DMAR DRHD table
+**/
+VOID
+DumpDmarDrhd (
+  IN EFI_ACPI_DMAR_DRHD_HEADER *Drhd
+  )
+{
+  EFI_ACPI_DMAR_DEVICE_SCOPE_STRUCTURE_HEADER       *DmarDeviceScopeEntry;
+  INTN                                    DrhdLen;
+
+  if (Drhd == NULL) {
+    return;
+  }
+
+  DEBUG ((DEBUG_INFO,
+    "  ***************************************************************************\n"
+    "  *       DMA-Remapping Hardware Definition Structure                       *\n"
+    "  ***************************************************************************\n"
+    ));
+  DEBUG ((DEBUG_INFO,
+    (sizeof(UINTN) == sizeof(UINT64)) ?
+    "  DRHD address ........................................... 0x%016lx\n" :
+    "  DRHD address ........................................... 0x%08x\n",
+    Drhd
+    ));
+  DEBUG ((DEBUG_INFO,
+    "    Type ................................................. 0x%04x\n",
+    Drhd->Header.Type
+    ));
+  DEBUG ((DEBUG_INFO,
+    "    Length ............................................... 0x%04x\n",
+    Drhd->Header.Length
+    ));
+  DEBUG ((DEBUG_INFO,
+    "    Flags ................................................ 0x%02x\n",
+    Drhd->Flags
+    ));
+  DEBUG ((DEBUG_INFO,
+    "      INCLUDE_PCI_ALL .................................... 0x%02x\n",
+    Drhd->Flags & EFI_ACPI_DMAR_DRHD_FLAGS_INCLUDE_PCI_ALL
+    ));
+  DEBUG ((DEBUG_INFO,
+    "    Segment Number ....................................... 0x%04x\n",
+    Drhd->SegmentNumber
+    ));
+  DEBUG ((DEBUG_INFO,
+    "    Register Base Address ................................ 0x%016lx\n",
+    Drhd->RegisterBaseAddress
+    ));
+
+  DrhdLen  = Drhd->Header.Length - sizeof(EFI_ACPI_DMAR_DRHD_HEADER);
+  DmarDeviceScopeEntry = (EFI_ACPI_DMAR_DEVICE_SCOPE_STRUCTURE_HEADER *)(Drhd + 1);
+  while (DrhdLen > 0) {
+    DumpDmarDeviceScopeEntry (DmarDeviceScopeEntry);
+    DrhdLen -= DmarDeviceScopeEntry->Length;
+    DmarDeviceScopeEntry = (EFI_ACPI_DMAR_DEVICE_SCOPE_STRUCTURE_HEADER *)((UINTN)DmarDeviceScopeEntry + DmarDeviceScopeEntry->Length);
+  }
+
+  DEBUG ((DEBUG_INFO,
+    "  ***************************************************************************\n\n"
+    ));
+
+  return;
+}
+
+/**
+  Dump DMAR ACPI table.
+
+  @param[in]  Dmar  DMAR ACPI table
+**/
+VOID
+DumpAcpiDMAR (
+  IN EFI_ACPI_DMAR_HEADER  *Dmar
+  )
+{
+  EFI_ACPI_DMAR_STRUCTURE_HEADER *DmarHeader;
+  INTN                  DmarLen;
+
+  if (Dmar == NULL) {
+    return;
+  }
+
+  //
+  // Dump Dmar table
+  //
+  DEBUG ((DEBUG_INFO,
+    "*****************************************************************************\n"
+    "*         DMAR Table                                                        *\n"
+    "*****************************************************************************\n"
+    ));
+
+  DEBUG ((DEBUG_INFO,
+    (sizeof(UINTN) == sizeof(UINT64)) ?
+    "DMAR address ............................................. 0x%016lx\n" :
+    "DMAR address ............................................. 0x%08x\n",
+    Dmar
+    ));
+
+  DEBUG ((DEBUG_INFO,
+    "  Table Contents:\n"
+    ));
+  DEBUG ((DEBUG_INFO,
+    "    Host Address Width ................................... 0x%02x\n",
+    Dmar->HostAddressWidth
+    ));
+  DEBUG ((DEBUG_INFO,
+    "    Flags ................................................ 0x%02x\n",
+    Dmar->Flags
+    ));
+  DEBUG ((DEBUG_INFO,
+    "      INTR_REMAP ......................................... 0x%02x\n",
+    Dmar->Flags & EFI_ACPI_DMAR_FLAGS_INTR_REMAP
+    ));
+  DEBUG ((DEBUG_INFO,
+    "      X2APIC_OPT_OUT_SET ................................. 0x%02x\n",
+    Dmar->Flags & EFI_ACPI_DMAR_FLAGS_X2APIC_OPT_OUT
+    ));
+
+  DmarLen  = Dmar->Header.Length - sizeof(EFI_ACPI_DMAR_HEADER);
+  DmarHeader = (EFI_ACPI_DMAR_STRUCTURE_HEADER *)(Dmar + 1);
+  while (DmarLen > 0) {
+    switch (DmarHeader->Type) {
+    case EFI_ACPI_DMAR_TYPE_DRHD:
+      DumpDmarDrhd ((EFI_ACPI_DMAR_DRHD_HEADER *)DmarHeader);
+      break;
+    case EFI_ACPI_DMAR_TYPE_RMRR:
+      DumpDmarRmrr ((EFI_ACPI_DMAR_RMRR_HEADER *)DmarHeader);
+      break;
+    case EFI_ACPI_DMAR_TYPE_ATSR:
+      DumpDmarAtsr ((EFI_ACPI_DMAR_ATSR_HEADER *)DmarHeader);
+      break;
+    case EFI_ACPI_DMAR_TYPE_RHSA:
+      DumpDmarRhsa ((EFI_ACPI_DMAR_RHSA_HEADER *)DmarHeader);
+      break;
+    case EFI_ACPI_DMAR_TYPE_ANDD:
+      DumpDmarAndd ((EFI_ACPI_DMAR_ANDD_HEADER *)DmarHeader);
+      break;
+    default:
+      break;
+    }
+    DmarLen -= DmarHeader->Length;
+    DmarHeader = (EFI_ACPI_DMAR_STRUCTURE_HEADER *)((UINTN)DmarHeader + DmarHeader->Length);
+  }
+
+  DEBUG ((DEBUG_INFO,
+    "*****************************************************************************\n\n"
+    ));
+
+  return;
+}
+
+/**
+  Dump DMAR ACPI table.
+**/
+VOID
+VtdDumpDmarTable (
+  VOID
+  )
+{
+  DumpAcpiDMAR ((EFI_ACPI_DMAR_HEADER *)(UINTN)mAcpiDmarTable);
+}
+
+/**
+  Get PCI device information from DMAR DevScopeEntry.
+
+  @param[in]  Segment               The segment number.
+  @param[in]  DmarDevScopeEntry     DMAR DevScopeEntry
+  @param[out] Bus                   The bus number.
+  @param[out] Device                The device number.
+  @param[out] Function              The function number.
+
+  @retval EFI_SUCCESS  The PCI device information is returned.
+**/
+EFI_STATUS
+GetPciBusDeviceFunction (
+  IN  UINT16                                      Segment,
+  IN  EFI_ACPI_DMAR_DEVICE_SCOPE_STRUCTURE_HEADER *DmarDevScopeEntry,
+  OUT UINT8                                       *Bus,
+  OUT UINT8                                       *Device,
+  OUT UINT8                                       *Function
+  )
+{
+  EFI_ACPI_DMAR_PCI_PATH                     *DmarPciPath;
+  UINT8                                      MyBus;
+  UINT8                                      MyDevice;
+  UINT8                                      MyFunction;
+
+  DmarPciPath = (EFI_ACPI_DMAR_PCI_PATH *)((UINTN)(DmarDevScopeEntry + 1));
+  MyBus = DmarDevScopeEntry->StartBusNumber;
+  MyDevice = DmarPciPath->Device;
+  MyFunction = DmarPciPath->Function;
+
+  switch (DmarDevScopeEntry->Type) {
+  case EFI_ACPI_DEVICE_SCOPE_ENTRY_TYPE_PCI_ENDPOINT:
+  case EFI_ACPI_DEVICE_SCOPE_ENTRY_TYPE_PCI_BRIDGE:
+    while ((UINTN)DmarPciPath < (UINTN)DmarDevScopeEntry + DmarDevScopeEntry->Length) {
+      MyBus = PciSegmentRead8 (PCI_SEGMENT_LIB_ADDRESS(Segment, MyBus, MyDevice, MyFunction, PCI_BRIDGE_SECONDARY_BUS_REGISTER_OFFSET));
+      MyDevice = DmarPciPath->Device;
+      MyFunction = DmarPciPath->Function;
+      DmarPciPath ++;
+    }
+    break;
+  case EFI_ACPI_DEVICE_SCOPE_ENTRY_TYPE_IOAPIC:
+  case EFI_ACPI_DEVICE_SCOPE_ENTRY_TYPE_MSI_CAPABLE_HPET:
+  case EFI_ACPI_DEVICE_SCOPE_ENTRY_TYPE_ACPI_NAMESPACE_DEVICE:
+    break;
+  }
+
+  *Bus = MyBus;
+  *Device = MyDevice;
+  *Function = MyFunction;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Process DMAR DHRD table.
+
+  @param[in]  VtdIndex  The index of VTd engine.
+  @param[in]  DmarDrhd  The DRHD table.
+
+  @retval EFI_SUCCESS The DRHD table is processed.
+**/
+EFI_STATUS
+ProcessDhrd (
+  IN UINTN                      VtdIndex,
+  IN EFI_ACPI_DMAR_DRHD_HEADER  *DmarDrhd
+  )
+{
+  EFI_ACPI_DMAR_DEVICE_SCOPE_STRUCTURE_HEADER       *DmarDevScopeEntry;
+  UINT8                                             Bus;
+  UINT8                                             Device;
+  UINT8                                             Function;
+  UINT8                                             SecondaryBusNumber;
+  EFI_STATUS                                        Status;
+  VTD_SOURCE_ID                                     SourceId;
+  BOOLEAN                                           IsRealPciDevice;
+
+  mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress = (UINTN)DmarDrhd->RegisterBaseAddress;
+  DEBUG ((DEBUG_INFO,"  VTD (%d) BaseAddress -  0x%016lx\n", VtdIndex, DmarDrhd->RegisterBaseAddress));
+
+  mVtdUnitInformation[VtdIndex].Segment = DmarDrhd->SegmentNumber;
+
+  if ((DmarDrhd->Flags & EFI_ACPI_DMAR_DRHD_FLAGS_INCLUDE_PCI_ALL) != 0) {
+    mVtdUnitInformation[VtdIndex].PciDeviceInfo.IncludeAllFlag = TRUE;
+    DEBUG ((DEBUG_INFO,"  ProcessDhrd: with INCLUDE ALL\n"));
+
+    Status = ScanPciBus(VtdIndex, DmarDrhd->SegmentNumber, 0);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  } else {
+    mVtdUnitInformation[VtdIndex].PciDeviceInfo.IncludeAllFlag = FALSE;
+    DEBUG ((DEBUG_INFO,"  ProcessDhrd: without INCLUDE ALL\n"));
+  }
+
+  DmarDevScopeEntry = (EFI_ACPI_DMAR_DEVICE_SCOPE_STRUCTURE_HEADER *)((UINTN)(DmarDrhd + 1));
+  while ((UINTN)DmarDevScopeEntry < (UINTN)DmarDrhd + DmarDrhd->Header.Length) {
+
+    Status = GetPciBusDeviceFunction (DmarDrhd->SegmentNumber, DmarDevScopeEntry, &Bus, &Device, &Function);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+    switch (DmarDevScopeEntry->Type) {
+    case EFI_ACPI_DEVICE_SCOPE_ENTRY_TYPE_PCI_ENDPOINT:
+    case EFI_ACPI_DEVICE_SCOPE_ENTRY_TYPE_PCI_BRIDGE:
+      IsRealPciDevice = TRUE;
+      break;
+    default:
+      IsRealPciDevice = FALSE;
+      break;
+    }
+
+    DEBUG ((DEBUG_INFO,"  ProcessDhrd: "));
+    switch (DmarDevScopeEntry->Type) {
+    case EFI_ACPI_DEVICE_SCOPE_ENTRY_TYPE_PCI_ENDPOINT:
+      DEBUG ((DEBUG_INFO,"PCI Endpoint"));
+      break;
+    case EFI_ACPI_DEVICE_SCOPE_ENTRY_TYPE_PCI_BRIDGE:
+      DEBUG ((DEBUG_INFO,"PCI-PCI bridge"));
+      break;
+    case EFI_ACPI_DEVICE_SCOPE_ENTRY_TYPE_IOAPIC:
+      DEBUG ((DEBUG_INFO,"IOAPIC"));
+      break;
+    case EFI_ACPI_DEVICE_SCOPE_ENTRY_TYPE_MSI_CAPABLE_HPET:
+      DEBUG ((DEBUG_INFO,"MSI Capable HPET"));
+      break;
+    case EFI_ACPI_DEVICE_SCOPE_ENTRY_TYPE_ACPI_NAMESPACE_DEVICE:
+      DEBUG ((DEBUG_INFO,"ACPI Namespace Device"));
+      break;
+    }
+    DEBUG ((DEBUG_INFO," S%04x B%02x D%02x F%02x\n", DmarDrhd->SegmentNumber, Bus, Device, Function));
+
+    SourceId.Bits.Bus = Bus;
+    SourceId.Bits.Device = Device;
+    SourceId.Bits.Function = Function;
+
+    Status = RegisterPciDevice (VtdIndex, DmarDrhd->SegmentNumber, SourceId, IsRealPciDevice, TRUE);
+    if (EFI_ERROR (Status)) {
+      //
+      // There might be duplication for special device other than standard PCI device.
+      //
+      switch (DmarDevScopeEntry->Type) {
+      case EFI_ACPI_DEVICE_SCOPE_ENTRY_TYPE_PCI_ENDPOINT:
+      case EFI_ACPI_DEVICE_SCOPE_ENTRY_TYPE_PCI_BRIDGE:
+        return Status;
+      }
+    }
+
+    switch (DmarDevScopeEntry->Type) {
+    case EFI_ACPI_DEVICE_SCOPE_ENTRY_TYPE_PCI_BRIDGE:
+      SecondaryBusNumber = PciSegmentRead8 (PCI_SEGMENT_LIB_ADDRESS(DmarDrhd->SegmentNumber, Bus, Device, Function, PCI_BRIDGE_SECONDARY_BUS_REGISTER_OFFSET));
+      Status = ScanPciBus (VtdIndex, DmarDrhd->SegmentNumber, SecondaryBusNumber);
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+      break;
+    default:
+      break;
+    }
+
+    DmarDevScopeEntry = (EFI_ACPI_DMAR_DEVICE_SCOPE_STRUCTURE_HEADER *)((UINTN)DmarDevScopeEntry + DmarDevScopeEntry->Length);
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Process DMAR RMRR table.
+
+  @param[in]  DmarRmrr  The RMRR table.
+
+  @retval EFI_SUCCESS The RMRR table is processed.
+**/
+EFI_STATUS
+ProcessRmrr (
+  IN EFI_ACPI_DMAR_RMRR_HEADER  *DmarRmrr
+  )
+{
+  EFI_ACPI_DMAR_DEVICE_SCOPE_STRUCTURE_HEADER       *DmarDevScopeEntry;
+  UINT8                                             Bus;
+  UINT8                                             Device;
+  UINT8                                             Function;
+  EFI_STATUS                                        Status;
+  VTD_SOURCE_ID                                     SourceId;
+
+  DEBUG ((DEBUG_INFO,"  RMRR (Base 0x%016lx, Limit 0x%016lx)\n", DmarRmrr->ReservedMemoryRegionBaseAddress, DmarRmrr->ReservedMemoryRegionLimitAddress));
+
+  DmarDevScopeEntry = (EFI_ACPI_DMAR_DEVICE_SCOPE_STRUCTURE_HEADER *)((UINTN)(DmarRmrr + 1));
+  while ((UINTN)DmarDevScopeEntry < (UINTN)DmarRmrr + DmarRmrr->Header.Length) {
+    if (DmarDevScopeEntry->Type != EFI_ACPI_DEVICE_SCOPE_ENTRY_TYPE_PCI_ENDPOINT) {
+      DEBUG ((DEBUG_INFO,"RMRR DevScopeEntryType is not endpoint, type[0x%x] \n", DmarDevScopeEntry->Type));
+      return EFI_DEVICE_ERROR;
+    }
+
+    Status = GetPciBusDeviceFunction (DmarRmrr->SegmentNumber, DmarDevScopeEntry, &Bus, &Device, &Function);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    DEBUG ((DEBUG_INFO,"RMRR S%04x B%02x D%02x F%02x\n", DmarRmrr->SegmentNumber, Bus, Device, Function));
+
+    SourceId.Bits.Bus = Bus;
+    SourceId.Bits.Device = Device;
+    SourceId.Bits.Function = Function;
+    Status = SetAccessAttribute (
+               DmarRmrr->SegmentNumber,
+               SourceId,
+               DmarRmrr->ReservedMemoryRegionBaseAddress,
+               DmarRmrr->ReservedMemoryRegionLimitAddress + 1 - DmarRmrr->ReservedMemoryRegionBaseAddress,
+               EDKII_IOMMU_ACCESS_READ | EDKII_IOMMU_ACCESS_WRITE
+               );
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    DmarDevScopeEntry = (EFI_ACPI_DMAR_DEVICE_SCOPE_STRUCTURE_HEADER *)((UINTN)DmarDevScopeEntry + DmarDevScopeEntry->Length);
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Get VTd engine number.
+**/
+UINTN
+GetVtdEngineNumber (
+  VOID
+  )
+{
+  EFI_ACPI_DMAR_STRUCTURE_HEADER                    *DmarHeader;
+  UINTN                                             VtdIndex;
+
+  VtdIndex = 0;
+  DmarHeader = (EFI_ACPI_DMAR_STRUCTURE_HEADER *)((UINTN)(mAcpiDmarTable + 1));
+  while ((UINTN)DmarHeader < (UINTN)mAcpiDmarTable + mAcpiDmarTable->Header.Length) {
+    switch (DmarHeader->Type) {
+    case EFI_ACPI_DMAR_TYPE_DRHD:
+      VtdIndex++;
+      break;
+    default:
+      break;
+    }
+    DmarHeader = (EFI_ACPI_DMAR_STRUCTURE_HEADER *)((UINTN)DmarHeader + DmarHeader->Length);
+  }
+  return VtdIndex ;
+}
+
+/**
+  Parse DMAR DRHD table.
+
+  @return EFI_SUCCESS  The DMAR DRHD table is parsed.
+**/
+EFI_STATUS
+ParseDmarAcpiTableDrhd (
+  VOID
+  )
+{
+  EFI_ACPI_DMAR_STRUCTURE_HEADER                    *DmarHeader;
+  EFI_STATUS                                        Status;
+  UINTN                                             VtdIndex;
+
+  mVtdUnitNumber = GetVtdEngineNumber ();
+  DEBUG ((DEBUG_INFO,"  VtdUnitNumber - %d\n", mVtdUnitNumber));
+  ASSERT (mVtdUnitNumber > 0);
+  if (mVtdUnitNumber == 0) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  mVtdUnitInformation = AllocateZeroPool (sizeof(*mVtdUnitInformation) * mVtdUnitNumber);
+  ASSERT (mVtdUnitInformation != NULL);
+  if (mVtdUnitInformation == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  mVtdHostAddressWidthMask = LShiftU64 (1ull, mAcpiDmarTable->HostAddressWidth) - 1;
+
+  VtdIndex = 0;
+  DmarHeader = (EFI_ACPI_DMAR_STRUCTURE_HEADER *)((UINTN)(mAcpiDmarTable + 1));
+  while ((UINTN)DmarHeader < (UINTN)mAcpiDmarTable + mAcpiDmarTable->Header.Length) {
+    switch (DmarHeader->Type) {
+    case EFI_ACPI_DMAR_TYPE_DRHD:
+      ASSERT (VtdIndex < mVtdUnitNumber);
+      Status = ProcessDhrd (VtdIndex, (EFI_ACPI_DMAR_DRHD_HEADER *)DmarHeader);
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+      VtdIndex++;
+
+      break;
+
+    default:
+      break;
+    }
+    DmarHeader = (EFI_ACPI_DMAR_STRUCTURE_HEADER *)((UINTN)DmarHeader + DmarHeader->Length);
+  }
+  ASSERT (VtdIndex == mVtdUnitNumber);
+
+  for (VtdIndex = 0; VtdIndex < mVtdUnitNumber; VtdIndex++) {
+    DumpPciDeviceInfo (VtdIndex);
+  }
+  return EFI_SUCCESS ;
+}
+
+/**
+  Parse DMAR DRHD table.
+
+  @return EFI_SUCCESS  The DMAR DRHD table is parsed.
+**/
+EFI_STATUS
+ParseDmarAcpiTableRmrr (
+  VOID
+  )
+{
+  EFI_ACPI_DMAR_STRUCTURE_HEADER                    *DmarHeader;
+  EFI_STATUS                                        Status;
+
+  DmarHeader = (EFI_ACPI_DMAR_STRUCTURE_HEADER *)((UINTN)(mAcpiDmarTable + 1));
+  while ((UINTN)DmarHeader < (UINTN)mAcpiDmarTable + mAcpiDmarTable->Header.Length) {
+    switch (DmarHeader->Type) {
+    case EFI_ACPI_DMAR_TYPE_RMRR:
+      Status = ProcessRmrr ((EFI_ACPI_DMAR_RMRR_HEADER *)DmarHeader);
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+      break;
+    default:
+      break;
+    }
+    DmarHeader = (EFI_ACPI_DMAR_STRUCTURE_HEADER *)((UINTN)DmarHeader + DmarHeader->Length);
+  }
+  return EFI_SUCCESS ;
+}
+
+/**
+  This function scan ACPI table in RSDT.
+
+  @param[in]  Rsdt      ACPI RSDT
+  @param[in]  Signature ACPI table signature
+
+  @return ACPI table
+**/
+VOID *
+ScanTableInRSDT (
+  IN RSDT_TABLE                   *Rsdt,
+  IN UINT32                       Signature
+  )
+{
+  UINTN                         Index;
+  UINT32                        EntryCount;
+  UINT32                        *EntryPtr;
+  EFI_ACPI_DESCRIPTION_HEADER   *Table;
+
+  EntryCount = (Rsdt->Header.Length - sizeof (EFI_ACPI_DESCRIPTION_HEADER)) / sizeof(UINT32);
+
+  EntryPtr = &Rsdt->Entry;
+  for (Index = 0; Index < EntryCount; Index ++, EntryPtr ++) {
+    Table = (EFI_ACPI_DESCRIPTION_HEADER*)((UINTN)(*EntryPtr));
+    if (Table->Signature == Signature) {
+      return Table;
+    }
+  }
+
+  return NULL;
+}
+
+/**
+  This function scan ACPI table in XSDT.
+
+  @param[in]  Xsdt      ACPI XSDT
+  @param[in]  Signature ACPI table signature
+
+  @return ACPI table
+**/
+VOID *
+ScanTableInXSDT (
+  IN XSDT_TABLE                   *Xsdt,
+  IN UINT32                       Signature
+  )
+{
+  UINTN                        Index;
+  UINT32                       EntryCount;
+  UINT64                       EntryPtr;
+  UINTN                        BasePtr;
+  EFI_ACPI_DESCRIPTION_HEADER  *Table;
+
+  EntryCount = (Xsdt->Header.Length - sizeof (EFI_ACPI_DESCRIPTION_HEADER)) / sizeof(UINT64);
+
+  BasePtr = (UINTN)(&(Xsdt->Entry));
+  for (Index = 0; Index < EntryCount; Index ++) {
+    CopyMem (&EntryPtr, (VOID *)(BasePtr + Index * sizeof(UINT64)), sizeof(UINT64));
+    Table = (EFI_ACPI_DESCRIPTION_HEADER*)((UINTN)(EntryPtr));
+    if (Table->Signature == Signature) {
+      return Table;
+    }
+  }
+
+  return NULL;
+}
+
+/**
+  This function scan ACPI table in RSDP.
+
+  @param[in]  Rsdp      ACPI RSDP
+  @param[in]  Signature ACPI table signature
+
+  @return ACPI table
+**/
+VOID *
+FindAcpiPtr (
+  IN EFI_ACPI_2_0_ROOT_SYSTEM_DESCRIPTION_POINTER *Rsdp,
+  IN UINT32                                       Signature
+  )
+{
+  EFI_ACPI_DESCRIPTION_HEADER                    *AcpiTable;
+  RSDT_TABLE                                     *Rsdt;
+  XSDT_TABLE                                     *Xsdt;
+
+  AcpiTable = NULL;
+
+  //
+  // Check ACPI2.0 table
+  //
+  Rsdt = (RSDT_TABLE *)(UINTN)Rsdp->RsdtAddress;
+  Xsdt = NULL;
+  if ((Rsdp->Revision >= 2) && (Rsdp->XsdtAddress < (UINT64)(UINTN)-1)) {
+    Xsdt = (XSDT_TABLE *)(UINTN)Rsdp->XsdtAddress;
+  }
+  //
+  // Check Xsdt
+  //
+  if (Xsdt != NULL) {
+    AcpiTable = ScanTableInXSDT (Xsdt, Signature);
+  }
+  //
+  // Check Rsdt
+  //
+  if ((AcpiTable == NULL) && (Rsdt != NULL)) {
+    AcpiTable = ScanTableInRSDT (Rsdt, Signature);
+  }
+
+  return AcpiTable;
+}
+
+/**
+  Get the DMAR ACPI table.
+
+  @retval EFI_SUCCESS    The DMAR ACPI table is got.
+  @retval EFI_NOT_FOUND  The DMAR ACPI table is not found.
+**/
+EFI_STATUS
+GetDmarAcpiTable (
+  VOID
+  )
+{
+  VOID                              *AcpiTable;
+  EFI_STATUS                        Status;
+
+  AcpiTable = NULL;
+  Status = EfiGetSystemConfigurationTable (
+             &gEfiAcpi20TableGuid,
+             &AcpiTable
+             );
+  if (EFI_ERROR (Status)) {
+    Status = EfiGetSystemConfigurationTable (
+               &gEfiAcpiTableGuid,
+               &AcpiTable
+               );
+  }
+  ASSERT (AcpiTable != NULL);
+
+  mAcpiDmarTable = FindAcpiPtr (
+                      (EFI_ACPI_2_0_ROOT_SYSTEM_DESCRIPTION_POINTER *)AcpiTable,
+                      EFI_ACPI_4_0_DMA_REMAPPING_TABLE_SIGNATURE
+                      );
+  DEBUG ((DEBUG_INFO,"DMAR Table - 0x%08x\n", mAcpiDmarTable));
+  if (mAcpiDmarTable == NULL) {
+    return EFI_UNSUPPORTED;
+  }
+  VtdDumpDmarTable();
+
+  return EFI_SUCCESS;
+}
diff --git a/IntelSiliconPkg/IntelVTdDxe/IntelVTdDxe.c b/IntelSiliconPkg/IntelVTdDxe/IntelVTdDxe.c
new file mode 100644
index 0000000..d22222d
--- /dev/null
+++ b/IntelSiliconPkg/IntelVTdDxe/IntelVTdDxe.c
@@ -0,0 +1,353 @@
+/** @file
+  Intel VTd driver.
+
+  Copyright (c) 2017, Intel Corporation. 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 <PiDxe.h>
+
+#include <Protocol/IoMmu.h>
+#include <Protocol/PciIo.h>
+
+#include <Library/IoLib.h>
+#include <Library/BaseLib.h>
+#include <Library/DebugLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+
+#include "DmaProtection.h"
+
+/**
+  Provides the controller-specific addresses required to access system memory from a
+  DMA bus master.
+
+  @param  This                  The protocol instance pointer.
+  @param  Operation             Indicates if the bus master is going to read or write to system memory.
+  @param  HostAddress           The system memory address to map to the PCI controller.
+  @param  NumberOfBytes         On input the number of bytes to map. On output the number of bytes
+                                that were mapped.
+  @param  DeviceAddress         The resulting map address for the bus master PCI controller to use to
+                                access the hosts HostAddress.
+  @param  Mapping               A resulting value to pass to Unmap().
+
+  @retval EFI_SUCCESS           The range was mapped for the returned NumberOfBytes.
+  @retval EFI_UNSUPPORTED       The HostAddress cannot be mapped as a common buffer.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+  @retval EFI_DEVICE_ERROR      The system hardware could not map the requested address.
+
+**/
+EFI_STATUS
+EFIAPI
+IoMmuMap (
+  IN     EDKII_IOMMU_PROTOCOL                       *This,
+  IN     EDKII_IOMMU_OPERATION                      Operation,
+  IN     VOID                                       *HostAddress,
+  IN OUT UINTN                                      *NumberOfBytes,
+  OUT    EFI_PHYSICAL_ADDRESS                       *DeviceAddress,
+  OUT    VOID                                       **Mapping
+  );
+
+/**
+  Completes the Map() operation and releases any corresponding resources.
+
+  @param  This                  The protocol instance pointer.
+  @param  Mapping               The mapping value returned from Map().
+
+  @retval EFI_SUCCESS           The range was unmapped.
+  @retval EFI_INVALID_PARAMETER Mapping is not a value that was returned by Map().
+  @retval EFI_DEVICE_ERROR      The data was not committed to the target system memory.
+**/
+EFI_STATUS
+EFIAPI
+IoMmuUnmap (
+  IN  EDKII_IOMMU_PROTOCOL                     *This,
+  IN  VOID                                     *Mapping
+  );
+
+/**
+  Allocates pages that are suitable for an OperationBusMasterCommonBuffer or
+  OperationBusMasterCommonBuffer64 mapping.
+
+  @param  This                  The protocol instance pointer.
+  @param  Type                  This parameter is not used and must be ignored.
+  @param  MemoryType            The type of memory to allocate, EfiBootServicesData or
+                                EfiRuntimeServicesData.
+  @param  Pages                 The number of pages to allocate.
+  @param  HostAddress           A pointer to store the base system memory address of the
+                                allocated range.
+  @param  Attributes            The requested bit mask of attributes for the allocated range.
+
+  @retval EFI_SUCCESS           The requested memory pages were allocated.
+  @retval EFI_UNSUPPORTED       Attributes is unsupported. The only legal attribute bits are
+                                MEMORY_WRITE_COMBINE and MEMORY_CACHED.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+  @retval EFI_OUT_OF_RESOURCES  The memory pages could not be allocated.
+
+**/
+EFI_STATUS
+EFIAPI
+IoMmuAllocateBuffer (
+  IN     EDKII_IOMMU_PROTOCOL                     *This,
+  IN     EFI_ALLOCATE_TYPE                        Type,
+  IN     EFI_MEMORY_TYPE                          MemoryType,
+  IN     UINTN                                    Pages,
+  IN OUT VOID                                     **HostAddress,
+  IN     UINT64                                   Attributes
+  );
+
+/**
+  Frees memory that was allocated with AllocateBuffer().
+
+  @param  This                  The protocol instance pointer.
+  @param  Pages                 The number of pages to free.
+  @param  HostAddress           The base system memory address of the allocated range.
+
+  @retval EFI_SUCCESS           The requested memory pages were freed.
+  @retval EFI_INVALID_PARAMETER The memory range specified by HostAddress and Pages
+                                was not allocated with AllocateBuffer().
+
+**/
+EFI_STATUS
+EFIAPI
+IoMmuFreeBuffer (
+  IN  EDKII_IOMMU_PROTOCOL                     *This,
+  IN  UINTN                                    Pages,
+  IN  VOID                                     *HostAddress
+  );
+
+/**
+  Convert the DeviceHandle to SourceId and Segment.
+
+  @param[in]  DeviceHandle      The device who initiates the DMA access request.
+  @param[out] Segment           The Segment used to identify a VTd engine.
+  @param[out] SourceId          The SourceId used to identify a VTd engine and table entry.
+
+  @retval EFI_SUCCESS            The Segment and SourceId are returned.
+  @retval EFI_INVALID_PARAMETER  DeviceHandle is an invalid handle.
+  @retval EFI_UNSUPPORTED        DeviceHandle is unknown by the IOMMU.
+**/
+EFI_STATUS
+DeviceHandleToSourceId (
+  IN EFI_HANDLE            DeviceHandle,
+  OUT UINT16               *Segment,
+  OUT VTD_SOURCE_ID        *SourceId
+  )
+{
+  EFI_PCI_IO_PROTOCOL                      *PciIo;
+  UINTN                                    Seg;
+  UINTN                                    Bus;
+  UINTN                                    Dev;
+  UINTN                                    Func;
+  EFI_STATUS                               Status;
+  EDKII_PLATFORM_VTD_DEVICE_INFO           DeviceInfo;
+
+  Status = EFI_NOT_FOUND;
+  if (mPlatformVTdPolicy != NULL) {
+    Status = mPlatformVTdPolicy->GetDeviceId (mPlatformVTdPolicy, DeviceHandle, &DeviceInfo);
+    if (!EFI_ERROR(Status)) {
+      *Segment  = DeviceInfo.Segment;
+      *SourceId = DeviceInfo.SourceId;
+      return EFI_SUCCESS;
+    }
+  }
+
+  Status = gBS->HandleProtocol (DeviceHandle, &gEfiPciIoProtocolGuid, (VOID **)&PciIo);
+  if (EFI_ERROR(Status)) {
+    return EFI_UNSUPPORTED;
+  }
+  Status = PciIo->GetLocation (PciIo, &Seg, &Bus, &Dev, &Func);
+  if (EFI_ERROR(Status)) {
+    return EFI_UNSUPPORTED;
+  }
+  *Segment = (UINT16)Seg;
+  SourceId->Bits.Bus = (UINT8)Bus;
+  SourceId->Bits.Device = (UINT8)Dev;
+  SourceId->Bits.Function = (UINT8)Func;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Set IOMMU attribute for a system memory.
+
+  If the IOMMU protocol exists, the system memory cannot be used
+  for DMA by default.
+
+  When a device requests a DMA access for a system memory,
+  the device driver need use SetAttribute() to update the IOMMU
+  attribute to request DMA access (read and/or write).
+
+  The DeviceHandle is used to identify which device submits the request.
+  The IOMMU implementation need translate the device path to an IOMMU device ID,
+  and set IOMMU hardware register accordingly.
+  1) DeviceHandle can be a standard PCI device.
+     The memory for BusMasterRead need set EDKII_IOMMU_ACCESS_READ.
+     The memory for BusMasterWrite need set EDKII_IOMMU_ACCESS_WRITE.
+     The memory for BusMasterCommonBuffer need set EDKII_IOMMU_ACCESS_READ|EDKII_IOMMU_ACCESS_WRITE.
+     After the memory is used, the memory need set 0 to keep it being protected.
+  2) DeviceHandle can be an ACPI device (ISA, I2C, SPI, etc).
+     The memory for DMA access need set EDKII_IOMMU_ACCESS_READ and/or EDKII_IOMMU_ACCESS_WRITE.
+
+  @param[in]  This              The protocol instance pointer.
+  @param[in]  DeviceHandle      The device who initiates the DMA access request.
+  @param[in]  DeviceAddress     The base of device memory address to be used as the DMA memory.
+  @param[in]  Length            The length of device memory address to be used as the DMA memory.
+  @param[in]  IoMmuAccess       The IOMMU access.
+
+  @retval EFI_SUCCESS            The IoMmuAccess is set for the memory range specified by DeviceAddress and Length.
+  @retval EFI_INVALID_PARAMETER  DeviceHandle is an invalid handle.
+  @retval EFI_INVALID_PARAMETER  DeviceAddress is not IoMmu Page size aligned.
+  @retval EFI_INVALID_PARAMETER  Length is not IoMmu Page size aligned.
+  @retval EFI_INVALID_PARAMETER  Length is 0.
+  @retval EFI_INVALID_PARAMETER  IoMmuAccess specified an illegal combination of access.
+  @retval EFI_UNSUPPORTED        DeviceHandle is unknown by the IOMMU.
+  @retval EFI_UNSUPPORTED        The bit mask of IoMmuAccess is not supported by the IOMMU.
+  @retval EFI_UNSUPPORTED        The IOMMU does not support the memory range specified by DeviceAddress and Length.
+  @retval EFI_OUT_OF_RESOURCES   There are not enough resources available to modify the IOMMU access.
+  @retval EFI_DEVICE_ERROR       The IOMMU device reported an error while attempting the operation.
+
+**/
+EFI_STATUS
+VTdSetAttribute (
+  IN EDKII_IOMMU_PROTOCOL  *This,
+  IN EFI_HANDLE            DeviceHandle,
+  IN EFI_PHYSICAL_ADDRESS  DeviceAddress,
+  IN UINT64                Length,
+  IN UINT64                IoMmuAccess
+  )
+{
+  EFI_STATUS           Status;
+  UINT16               Segment;
+  VTD_SOURCE_ID        SourceId;
+
+  DumpVtdIfError ();
+
+  Status = DeviceHandleToSourceId (DeviceHandle, &Segment, &SourceId);
+  if (EFI_ERROR(Status)) {
+    return Status;
+  }
+
+  DEBUG ((DEBUG_VERBOSE, "IoMmuSetAttribute: "));
+  DEBUG ((DEBUG_VERBOSE, "PCI(S%x.B%x.D%x.F%x) ", Segment, SourceId.Bits.Bus, SourceId.Bits.Device, SourceId.Bits.Function));
+  DEBUG ((DEBUG_VERBOSE, "(0x%lx~0x%lx) - %lx\n", DeviceAddress, Length, IoMmuAccess));
+
+  Status = SetAccessAttribute (Segment, SourceId, DeviceAddress, Length, IoMmuAccess);
+
+  return Status;
+}
+
+/**
+  Set IOMMU attribute for a system memory.
+
+  If the IOMMU protocol exists, the system memory cannot be used
+  for DMA by default.
+
+  When a device requests a DMA access for a system memory,
+  the device driver need use SetAttribute() to update the IOMMU
+  attribute to request DMA access (read and/or write).
+
+  The DeviceHandle is used to identify which device submits the request.
+  The IOMMU implementation need translate the device path to an IOMMU device ID,
+  and set IOMMU hardware register accordingly.
+  1) DeviceHandle can be a standard PCI device.
+     The memory for BusMasterRead need set EDKII_IOMMU_ACCESS_READ.
+     The memory for BusMasterWrite need set EDKII_IOMMU_ACCESS_WRITE.
+     The memory for BusMasterCommonBuffer need set EDKII_IOMMU_ACCESS_READ|EDKII_IOMMU_ACCESS_WRITE.
+     After the memory is used, the memory need set 0 to keep it being protected.
+  2) DeviceHandle can be an ACPI device (ISA, I2C, SPI, etc).
+     The memory for DMA access need set EDKII_IOMMU_ACCESS_READ and/or EDKII_IOMMU_ACCESS_WRITE.
+
+  @param[in]  This              The protocol instance pointer.
+  @param[in]  DeviceHandle      The device who initiates the DMA access request.
+  @param[in]  Mapping           The mapping value returned from Map().
+  @param[in]  IoMmuAccess       The IOMMU access.
+
+  @retval EFI_SUCCESS            The IoMmuAccess is set for the memory range specified by DeviceAddress and Length.
+  @retval EFI_INVALID_PARAMETER  DeviceHandle is an invalid handle.
+  @retval EFI_INVALID_PARAMETER  Mapping is not a value that was returned by Map().
+  @retval EFI_INVALID_PARAMETER  IoMmuAccess specified an illegal combination of access.
+  @retval EFI_UNSUPPORTED        DeviceHandle is unknown by the IOMMU.
+  @retval EFI_UNSUPPORTED        The bit mask of IoMmuAccess is not supported by the IOMMU.
+  @retval EFI_UNSUPPORTED        The IOMMU does not support the memory range specified by Mapping.
+  @retval EFI_OUT_OF_RESOURCES   There are not enough resources available to modify the IOMMU access.
+  @retval EFI_DEVICE_ERROR       The IOMMU device reported an error while attempting the operation.
+
+**/
+EFI_STATUS
+EFIAPI
+IoMmuSetAttribute (
+  IN EDKII_IOMMU_PROTOCOL  *This,
+  IN EFI_HANDLE            DeviceHandle,
+  IN VOID                  *Mapping,
+  IN UINT64                IoMmuAccess
+  )
+{
+  EFI_STATUS            Status;
+  EFI_PHYSICAL_ADDRESS  DeviceAddress;
+  UINTN                 NumberOfPages;
+
+  Status = GetDeviceInfoFromMapping (Mapping, &DeviceAddress, &NumberOfPages);
+  if (EFI_ERROR(Status)) {
+    return Status;
+  }
+  Status = VTdSetAttribute (
+             This,
+             DeviceHandle,
+             DeviceAddress,
+             EFI_PAGES_TO_SIZE(NumberOfPages),
+             IoMmuAccess
+             );
+
+  return Status;
+}
+
+EDKII_IOMMU_PROTOCOL  mIntelVTd = {
+  EDKII_IOMMU_PROTOCOL_REVISION,
+  IoMmuSetAttribute,
+  IoMmuMap,
+  IoMmuUnmap,
+  IoMmuAllocateBuffer,
+  IoMmuFreeBuffer,
+};
+
+/**
+  Initialize the VTd driver.
+
+  @param[in]  ImageHandle  ImageHandle of the loaded driver
+  @param[in]  SystemTable  Pointer to the System Table
+
+  @retval  EFI_SUCCESS           The Protocol is installed.
+  @retval  EFI_OUT_OF_RESOURCES  Not enough resources available to initialize driver.
+  @retval  EFI_DEVICE_ERROR      A device error occurred attempting to initialize the driver.
+
+**/
+EFI_STATUS
+EFIAPI
+IntelVTdInitialize (
+  IN EFI_HANDLE        ImageHandle,
+  IN EFI_SYSTEM_TABLE  *SystemTable
+  )
+{
+  EFI_STATUS  Status;
+  EFI_HANDLE  Handle;
+
+  InitializeDmaProtection ();
+
+  Handle = NULL;
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &Handle,
+                  &gEdkiiIoMmuProtocolGuid, &mIntelVTd,
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  return Status;
+}
diff --git a/IntelSiliconPkg/IntelVTdDxe/IntelVTdDxe.inf b/IntelSiliconPkg/IntelVTdDxe/IntelVTdDxe.inf
new file mode 100644
index 0000000..6a61c13
--- /dev/null
+++ b/IntelSiliconPkg/IntelVTdDxe/IntelVTdDxe.inf
@@ -0,0 +1,79 @@
+## @file
+# Intel VTd DXE Driver.
+#
+# This driver initializes VTd engine based upon DMAR ACPI tables
+# and provide DMA protection to PCI or ACPI device.
+#
+# Copyright (c) 2017, Intel Corporation. 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = IntelVTdDxe
+  MODULE_UNI_FILE                = IntelVTdDxe.uni
+  FILE_GUID                      = 987555D6-595D-4CFA-B895-59B89368BD4D
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = IntelVTdInitialize
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+#
+
+[Sources]
+  IntelVTdDxe.c
+  BmDma.c
+  DmaProtection.c
+  DmaProtection.h
+  DmarAcpiTable.c
+  PciInfo.c
+  TranslationTable.c
+  TranslationTableEx.c
+  VtdReg.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+  IntelSiliconPkg/IntelSiliconPkg.dec
+
+[LibraryClasses]
+  DebugLib
+  UefiDriverEntryPoint
+  UefiBootServicesTableLib
+  BaseLib
+  IoLib
+  PciSegmentLib
+  BaseMemoryLib
+  MemoryAllocationLib
+  UefiLib
+
+[Guids]
+  gEfiEventExitBootServicesGuid   ## CONSUMES ## Event
+  gEfiAcpi20TableGuid             ## CONSUMES ## SystemTable
+  gEfiAcpiTableGuid               ## CONSUMES ## SystemTable
+
+[Protocols]
+  gEdkiiIoMmuProtocolGuid                     ## PRODUCES
+  gEfiAcpiSdtProtocolGuid                     ## CONSUMES
+  gEfiPciIoProtocolGuid                       ## CONSUMES
+  gEfiPciEnumerationCompleteProtocolGuid      ## CONSUMES
+  gEdkiiPlatformVTdPolicyProtocolGuid         ## SOMETIMES_CONSUMES
+
+[Depex]
+  gEfiPciRootBridgeIoProtocolGuid AND
+  gEfiAcpiSdtProtocolGuid
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  IntelVTdDxeExtra.uni
+
diff --git a/IntelSiliconPkg/IntelVTdDxe/IntelVTdDxe.uni b/IntelSiliconPkg/IntelVTdDxe/IntelVTdDxe.uni
new file mode 100644
index 0000000..45ce3c3
--- /dev/null
+++ b/IntelSiliconPkg/IntelVTdDxe/IntelVTdDxe.uni
@@ -0,0 +1,20 @@
+// /** @file
+// IntelVTdDxe Module Localized Abstract and Description Content
+//
+// Copyright (c) 2017, Intel Corporation. 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.
+//
+// **/
+
+
+#string STR_MODULE_ABSTRACT             #language en-US "Intel VTd DXE Driver."
+
+#string STR_MODULE_DESCRIPTION          #language en-US "This driver initializes VTd engine based upon DMAR ACPI tables and provide DMA protection to PCI or ACPI device."
+
diff --git a/IntelSiliconPkg/IntelVTdDxe/IntelVTdDxeExtra.uni b/IntelSiliconPkg/IntelVTdDxe/IntelVTdDxeExtra.uni
new file mode 100644
index 0000000..5cb3eb0
--- /dev/null
+++ b/IntelSiliconPkg/IntelVTdDxe/IntelVTdDxeExtra.uni
@@ -0,0 +1,20 @@
+// /** @file
+// IntelVTdDxe Localized Strings and Content
+//
+// Copyright (c) 2017, Intel Corporation. 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.
+//
+// **/
+
+#string STR_PROPERTIES_MODULE_NAME
+#language en-US
+"Intel VTd DXE Driver"
+
+
diff --git a/IntelSiliconPkg/IntelVTdDxe/PciInfo.c b/IntelSiliconPkg/IntelVTdDxe/PciInfo.c
new file mode 100644
index 0000000..ea84317
--- /dev/null
+++ b/IntelSiliconPkg/IntelVTdDxe/PciInfo.c
@@ -0,0 +1,315 @@
+/** @file
+
+  Copyright (c) 2017, Intel Corporation. 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 "DmaProtection.h"
+
+/**
+  Return the index of PCI descriptor.
+
+  @param[in]  VtdIndex          The index used to identify a VTd engine.
+  @param[in]  Segment           The Segment used to identify a VTd engine.
+  @param[in]  SourceId          The SourceId used to identify a VTd engine and table entry.
+
+  @return The index of the PCI descriptor.
+  @retval (UINTN)-1  The PCI descriptor is not found.
+**/
+UINTN
+GetPciDescriptor (
+  IN UINTN          VtdIndex,
+  IN UINT16         Segment,
+  IN VTD_SOURCE_ID  SourceId
+  )
+{
+  UINTN  Index;
+
+  if (Segment != mVtdUnitInformation[VtdIndex].Segment) {
+    return (UINTN)-1;
+  }
+
+  for (Index = 0; Index < mVtdUnitInformation[VtdIndex].PciDeviceInfo.PciDescriptorNumber; Index++) {
+    if ((mVtdUnitInformation[VtdIndex].PciDeviceInfo.PciDescriptors[Index].Bits.Bus == SourceId.Bits.Bus) &&
+        (mVtdUnitInformation[VtdIndex].PciDeviceInfo.PciDescriptors[Index].Bits.Device == SourceId.Bits.Device) &&
+        (mVtdUnitInformation[VtdIndex].PciDeviceInfo.PciDescriptors[Index].Bits.Function == SourceId.Bits.Function) ) {
+      return Index;
+    }
+  }
+
+  return (UINTN)-1;
+}
+
+/**
+  Register PCI device to VTd engine as PCI descriptor.
+
+  @param[in]  VtdIndex              The index of VTd engine.
+  @param[in]  Segment               The segment of the source.
+  @param[in]  SourceId              The SourceId of the source.
+  @param[in]  IsRealPciDevice       TRUE: It is a real PCI device.
+                                    FALSE: It is not a real PCI device.
+  @param[in]  CheckExist            TRUE: ERROR will be returned if the PCI device is already registered.
+                                    FALSE: SUCCESS will be returned if the PCI device is registered.
+
+  @retval EFI_SUCCESS           The PCI device is registered.
+  @retval EFI_OUT_OF_RESOURCES  No enough resource to register a new PCI device.
+  @retval EFI_ALREADY_STARTED   The device is already registered.
+**/
+EFI_STATUS
+RegisterPciDevice (
+  IN UINTN          VtdIndex,
+  IN UINT16         Segment,
+  IN VTD_SOURCE_ID  SourceId,
+  IN BOOLEAN        IsRealPciDevice,
+  IN BOOLEAN        CheckExist
+  )
+{
+  PCI_DEVICE_INFORMATION  *PciDeviceInfo;
+  VTD_SOURCE_ID           *PciDescriptor;
+  UINTN                   PciDescriptorIndex;
+  UINTN                   Index;
+  BOOLEAN                 *NewIsRealPciDevice;
+  VTD_SOURCE_ID           *NewPciDescriptors;
+
+  PciDeviceInfo = &mVtdUnitInformation[VtdIndex].PciDeviceInfo;
+
+  if (PciDeviceInfo->IncludeAllFlag) {
+    //
+    // Do not register device in other VTD Unit
+    //
+    for (Index = 0; Index < VtdIndex; Index++) {
+      PciDescriptorIndex = GetPciDescriptor (Index, Segment, SourceId);
+      if (PciDescriptorIndex != (UINTN)-1) {
+        DEBUG ((DEBUG_INFO, "  RegisterPciDevice: PCI S%04x B%02x D%02x F%02x already registered by Other Vtd(%d)\n", Segment, SourceId.Bits.Bus, SourceId.Bits.Device, SourceId.Bits.Function, Index));
+        return EFI_SUCCESS;
+      }
+    }
+  }
+
+  PciDescriptorIndex = GetPciDescriptor (VtdIndex, Segment, SourceId);
+  if (PciDescriptorIndex == (UINTN)-1) {
+    //
+    // Register new
+    //
+
+    if (PciDeviceInfo->PciDescriptorNumber >= PciDeviceInfo->PciDescriptorMaxNumber) {
+      //
+      // Reallocate
+      //
+      NewIsRealPciDevice = AllocateZeroPool (sizeof(*NewIsRealPciDevice) * (PciDeviceInfo->PciDescriptorMaxNumber + MAX_PCI_DESCRIPTORS));
+      if (NewIsRealPciDevice == NULL) {
+        return EFI_OUT_OF_RESOURCES;
+      }
+      NewPciDescriptors = AllocateZeroPool (sizeof(*NewPciDescriptors) * (PciDeviceInfo->PciDescriptorMaxNumber + MAX_PCI_DESCRIPTORS));
+      if (NewPciDescriptors == NULL) {
+        FreePool (NewIsRealPciDevice);
+        return EFI_OUT_OF_RESOURCES;
+      }
+      PciDeviceInfo->PciDescriptorMaxNumber += MAX_PCI_DESCRIPTORS;
+      if (PciDeviceInfo->IsRealPciDevice != NULL) {
+        CopyMem (NewIsRealPciDevice, PciDeviceInfo->IsRealPciDevice, sizeof(*NewIsRealPciDevice) * PciDeviceInfo->PciDescriptorNumber);
+        FreePool (PciDeviceInfo->IsRealPciDevice);
+      }
+      PciDeviceInfo->IsRealPciDevice = NewIsRealPciDevice;
+      if (PciDeviceInfo->PciDescriptors != NULL) {
+        CopyMem (NewPciDescriptors, PciDeviceInfo->PciDescriptors, sizeof(*NewPciDescriptors) * PciDeviceInfo->PciDescriptorNumber);
+        FreePool (PciDeviceInfo->PciDescriptors);
+      }
+      PciDeviceInfo->PciDescriptors = NewPciDescriptors;
+    }
+
+    ASSERT (PciDeviceInfo->PciDescriptorNumber < PciDeviceInfo->PciDescriptorMaxNumber);
+
+    PciDescriptor = &PciDeviceInfo->PciDescriptors[PciDeviceInfo->PciDescriptorNumber];
+    PciDescriptor->Bits.Bus = SourceId.Bits.Bus;
+    PciDescriptor->Bits.Device = SourceId.Bits.Device;
+    PciDescriptor->Bits.Function = SourceId.Bits.Function;
+    PciDeviceInfo->IsRealPciDevice[PciDeviceInfo->PciDescriptorNumber] = IsRealPciDevice;
+
+    PciDeviceInfo->PciDescriptorNumber++;
+
+    DEBUG ((DEBUG_INFO, "  RegisterPciDevice: PCI S%04x B%02x D%02x F%02x", Segment, SourceId.Bits.Bus, SourceId.Bits.Device, SourceId.Bits.Function));
+    if (!IsRealPciDevice) {
+      DEBUG ((DEBUG_INFO, " (*)"));
+    }
+    DEBUG ((DEBUG_INFO, "\n"));
+  } else {
+    if (CheckExist) {
+      DEBUG ((DEBUG_INFO, "  RegisterPciDevice: PCI S%04x B%02x D%02x F%02x already registered\n", Segment, SourceId.Bits.Bus, SourceId.Bits.Device, SourceId.Bits.Function));
+      return EFI_ALREADY_STARTED;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Scan PCI bus and register PCI devices under the bus.
+
+  @param[in]  VtdIndex              The index of VTd engine.
+  @param[in]  Segment               The segment of the source.
+  @param[in]  Bus                   The bus of the source.
+
+  @retval EFI_SUCCESS           The PCI devices under the bus are registered.
+  @retval EFI_OUT_OF_RESOURCES  No enough resource to register a new PCI device.
+**/
+EFI_STATUS
+ScanPciBus (
+  IN UINTN          VtdIndex,
+  IN UINT16         Segment,
+  IN UINT8          Bus
+  )
+{
+  UINT8                   Device;
+  UINT8                   Function;
+  UINT8                   SecondaryBusNumber;
+  UINT8                   HeaderType;
+  UINT8                   BaseClass;
+  UINT8                   SubClass;
+  UINT32                  MaxFunction;
+  UINT16                  VendorID;
+  UINT16                  DeviceID;
+  EFI_STATUS              Status;
+  VTD_SOURCE_ID           SourceId;
+
+  // Scan the PCI bus for devices
+  for (Device = 0; Device < PCI_MAX_DEVICE + 1; Device++) {
+    HeaderType = PciSegmentRead8 (PCI_SEGMENT_LIB_ADDRESS(Segment, Bus, Device, 0, PCI_HEADER_TYPE_OFFSET));
+    MaxFunction = PCI_MAX_FUNC + 1;
+    if ((HeaderType & HEADER_TYPE_MULTI_FUNCTION) == 0x00) {
+      MaxFunction = 1;
+    }
+    for (Function = 0; Function < MaxFunction; Function++) {
+      VendorID  = PciSegmentRead16 (PCI_SEGMENT_LIB_ADDRESS(Segment, Bus, Device, Function, PCI_VENDOR_ID_OFFSET));
+      DeviceID  = PciSegmentRead16 (PCI_SEGMENT_LIB_ADDRESS(Segment, Bus, Device, Function, PCI_DEVICE_ID_OFFSET));
+      if (VendorID == 0xFFFF && DeviceID == 0xFFFF) {
+        continue;
+      }
+
+      SourceId.Bits.Bus = Bus;
+      SourceId.Bits.Device = Device;
+      SourceId.Bits.Function = Function;
+      Status = RegisterPciDevice (VtdIndex, Segment, SourceId, TRUE, FALSE);
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+
+      BaseClass = PciSegmentRead8 (PCI_SEGMENT_LIB_ADDRESS(Segment, Bus, Device, Function, PCI_CLASSCODE_OFFSET + 2));
+      if (BaseClass == PCI_CLASS_BRIDGE) {
+        SubClass = PciSegmentRead8 (PCI_SEGMENT_LIB_ADDRESS(Segment, Bus, Device, Function, PCI_CLASSCODE_OFFSET + 1));
+        if (SubClass == PCI_CLASS_BRIDGE_P2P) {
+          SecondaryBusNumber = PciSegmentRead8 (PCI_SEGMENT_LIB_ADDRESS(Segment, Bus, Device, Function, PCI_BRIDGE_SECONDARY_BUS_REGISTER_OFFSET));
+          DEBUG ((DEBUG_INFO,"  ScanPciBus: PCI bridge S%04x B%02x D%02x F%02x (SecondBus:%02x)\n", Segment, Bus, Device, Function, SecondaryBusNumber));
+          if (SecondaryBusNumber != 0) {
+            Status = ScanPciBus (VtdIndex, Segment, SecondaryBusNumber);
+            if (EFI_ERROR (Status)) {
+              return Status;
+            }
+          }
+        }
+      }
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Dump the PCI device information managed by this VTd engine.
+
+  @param[in]  VtdIndex              The index of VTd engine.
+**/
+VOID
+DumpPciDeviceInfo (
+  IN UINTN  VtdIndex
+  )
+{
+  UINTN  Index;
+
+  DEBUG ((DEBUG_INFO,"PCI Device Information (Number 0x%x, IncludeAll - %d):\n",
+    mVtdUnitInformation[VtdIndex].PciDeviceInfo.PciDescriptorNumber,
+    mVtdUnitInformation[VtdIndex].PciDeviceInfo.IncludeAllFlag
+    ));
+  for (Index = 0; Index < mVtdUnitInformation[VtdIndex].PciDeviceInfo.PciDescriptorNumber; Index++) {
+    DEBUG ((DEBUG_INFO,"  S%04x B%02x D%02x F%02x\n",
+      mVtdUnitInformation[VtdIndex].Segment,
+      mVtdUnitInformation[VtdIndex].PciDeviceInfo.PciDescriptors[Index].Bits.Bus,
+      mVtdUnitInformation[VtdIndex].PciDeviceInfo.PciDescriptors[Index].Bits.Device,
+      mVtdUnitInformation[VtdIndex].PciDeviceInfo.PciDescriptors[Index].Bits.Function
+      ));
+  }
+}
+
+/**
+  Find the VTd index by the Segment and SourceId.
+
+  @param[in]  Segment               The segment of the source.
+  @param[in]  SourceId              The SourceId of the source.
+  @param[out] ExtContextEntry       The ExtContextEntry of the source.
+  @param[out] ContextEntry          The ContextEntry of the source.
+
+  @return The index of the PCI descriptor.
+  @retval (UINTN)-1  The PCI descriptor is not found.
+**/
+UINTN
+FindVtdIndexByPciDevice (
+  IN  UINT16                  Segment,
+  IN  VTD_SOURCE_ID           SourceId,
+  OUT VTD_EXT_CONTEXT_ENTRY   **ExtContextEntry,
+  OUT VTD_CONTEXT_ENTRY       **ContextEntry
+  )
+{
+  UINTN                   VtdIndex;
+  VTD_ROOT_ENTRY          *RootEntry;
+  VTD_CONTEXT_ENTRY       *ContextEntryTable;
+  VTD_CONTEXT_ENTRY       *ThisContextEntry;
+  VTD_EXT_ROOT_ENTRY      *ExtRootEntry;
+  VTD_EXT_CONTEXT_ENTRY   *ExtContextEntryTable;
+  VTD_EXT_CONTEXT_ENTRY   *ThisExtContextEntry;
+  UINTN                   PciDescriptorIndex;
+
+  for (VtdIndex = 0; VtdIndex < mVtdUnitNumber; VtdIndex++) {
+    if (Segment != mVtdUnitInformation[VtdIndex].Segment) {
+      continue;
+    }
+
+    PciDescriptorIndex = GetPciDescriptor (VtdIndex, Segment, SourceId);
+    if (PciDescriptorIndex == (UINTN)-1) {
+      continue;
+    }
+
+//    DEBUG ((DEBUG_INFO,"FindVtdIndex(0x%x) for S%04x B%02x D%02x F%02x\n", VtdIndex, Segment, SourceId.Bits.Bus, SourceId.Bits.Device, SourceId.Bits.Function));
+
+    if (mVtdUnitInformation[VtdIndex].ExtRootEntryTable != 0) {
+      ExtRootEntry = &mVtdUnitInformation[VtdIndex].ExtRootEntryTable[SourceId.Index.RootIndex];
+      ExtContextEntryTable = (VTD_EXT_CONTEXT_ENTRY *)(UINTN)LShiftU64 (ExtRootEntry->Bits.LowerContextTablePointer, 12) ;
+      ThisExtContextEntry  = &ExtContextEntryTable[SourceId.Index.ContextIndex];
+      if (ThisExtContextEntry->Bits.AddressWidth == 0) {
+        continue;
+      }
+      *ExtContextEntry = ThisExtContextEntry;
+      *ContextEntry    = NULL;
+    } else {
+      RootEntry = &mVtdUnitInformation[VtdIndex].RootEntryTable[SourceId.Index.RootIndex];
+      ContextEntryTable = (VTD_CONTEXT_ENTRY *)(UINTN)LShiftU64 (RootEntry->Bits.ContextTablePointer, 12) ;
+      ThisContextEntry  = &ContextEntryTable[SourceId.Index.ContextIndex];
+      if (ThisContextEntry->Bits.AddressWidth == 0) {
+        continue;
+      }
+      *ExtContextEntry = NULL;
+      *ContextEntry    = ThisContextEntry;
+    }
+
+    return VtdIndex;
+  }
+
+  return (UINTN)-1;
+}
+
diff --git a/IntelSiliconPkg/IntelVTdDxe/TranslationTable.c b/IntelSiliconPkg/IntelVTdDxe/TranslationTable.c
new file mode 100644
index 0000000..0cff2cc
--- /dev/null
+++ b/IntelSiliconPkg/IntelVTdDxe/TranslationTable.c
@@ -0,0 +1,969 @@
+/** @file
+
+  Copyright (c) 2017, Intel Corporation. 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 "DmaProtection.h"
+
+/**
+  Create extended context entry.
+
+  @param[in]  VtdIndex  The index of the VTd engine.
+
+  @retval EFI_SUCCESS          The extended context entry is created.
+  @retval EFI_OUT_OF_RESOURCE  No enough resource to create extended context entry.
+**/
+EFI_STATUS
+CreateExtContextEntry (
+  IN UINTN  VtdIndex
+  );
+
+/**
+  Allocate zero pages.
+
+  @param[in]  Pages the number of pages.
+
+  @return the page address.
+  @retval NULL No resource to allocate pages.
+**/
+VOID *
+EFIAPI
+AllocateZeroPages (
+  IN UINTN  Pages
+  )
+{
+  VOID *Addr;
+
+  Addr = AllocatePages (Pages);
+  if (Addr == NULL) {
+    return NULL;
+  }
+  ZeroMem (Addr, EFI_PAGES_TO_SIZE(Pages));
+  return Addr;
+}
+
+/**
+  Set second level paging entry attribute based upon IoMmuAccess.
+
+  @param[in]  PtEntry      The paging entry.
+  @param[in]  IoMmuAccess  The IOMMU access.
+**/
+VOID
+SetSecondLevelPagingEntryAttribute (
+  IN VTD_SECOND_LEVEL_PAGING_ENTRY  *PtEntry,
+  IN UINT64                         IoMmuAccess
+  )
+{
+  PtEntry->Bits.Read  = ((IoMmuAccess & EDKII_IOMMU_ACCESS_READ) != 0);
+  PtEntry->Bits.Write = ((IoMmuAccess & EDKII_IOMMU_ACCESS_WRITE) != 0);
+}
+
+/**
+  Create context entry.
+
+  @param[in]  VtdIndex  The index of the VTd engine.
+
+  @retval EFI_SUCCESS          The context entry is created.
+  @retval EFI_OUT_OF_RESOURCE  No enough resource to create context entry.
+**/
+EFI_STATUS
+CreateContextEntry (
+  IN UINTN  VtdIndex
+  )
+{
+  UINTN                  Index;
+  VOID                   *Buffer;
+  UINTN                  RootPages;
+  UINTN                  ContextPages;
+  VTD_ROOT_ENTRY         *RootEntry;
+  VTD_CONTEXT_ENTRY      *ContextEntryTable;
+  VTD_CONTEXT_ENTRY      *ContextEntry;
+  VTD_SOURCE_ID          *PciDescriptor;
+  VTD_SOURCE_ID          SourceId;
+  UINTN                  MaxBusNumber;
+  UINTN                  EntryTablePages;
+
+  MaxBusNumber = 0;
+  for (Index = 0; Index < mVtdUnitInformation[VtdIndex].PciDeviceInfo.PciDescriptorNumber; Index++) {
+    PciDescriptor = &mVtdUnitInformation[VtdIndex].PciDeviceInfo.PciDescriptors[Index];
+    if (PciDescriptor->Bits.Bus > MaxBusNumber) {
+      MaxBusNumber = PciDescriptor->Bits.Bus;
+    }
+  }
+  DEBUG ((DEBUG_INFO,"  MaxBusNumber - 0x%x\n", MaxBusNumber));
+
+  RootPages = EFI_SIZE_TO_PAGES (sizeof (VTD_ROOT_ENTRY) * VTD_ROOT_ENTRY_NUMBER);
+  ContextPages = EFI_SIZE_TO_PAGES (sizeof (VTD_CONTEXT_ENTRY) * VTD_CONTEXT_ENTRY_NUMBER);
+  EntryTablePages = RootPages + ContextPages * (MaxBusNumber + 1);
+  Buffer = AllocateZeroPages (EntryTablePages);
+  if (Buffer == NULL) {
+    DEBUG ((DEBUG_INFO,"Could not Alloc Root Entry Table.. \n"));
+    return EFI_OUT_OF_RESOURCES;
+  }
+  mVtdUnitInformation[VtdIndex].RootEntryTable = (VTD_ROOT_ENTRY *)Buffer;
+  Buffer = (UINT8 *)Buffer + EFI_PAGES_TO_SIZE (RootPages);
+
+  for (Index = 0; Index < mVtdUnitInformation[VtdIndex].PciDeviceInfo.PciDescriptorNumber; Index++) {
+    PciDescriptor = &mVtdUnitInformation[VtdIndex].PciDeviceInfo.PciDescriptors[Index];
+
+    SourceId.Bits.Bus = PciDescriptor->Bits.Bus;
+    SourceId.Bits.Device = PciDescriptor->Bits.Device;
+    SourceId.Bits.Function = PciDescriptor->Bits.Function;
+
+    RootEntry = &mVtdUnitInformation[VtdIndex].RootEntryTable[SourceId.Index.RootIndex];
+    if (RootEntry->Bits.Present == 0) {
+      RootEntry->Bits.ContextTablePointer  = RShiftU64 ((UINT64)(UINTN)Buffer, 12);
+      RootEntry->Bits.Present = 1;
+      Buffer = (UINT8 *)Buffer + EFI_PAGES_TO_SIZE (ContextPages);
+    }
+
+    ContextEntryTable = (VTD_CONTEXT_ENTRY *)(UINTN)LShiftU64(RootEntry->Bits.ContextTablePointer, 12) ;
+    ContextEntry = &ContextEntryTable[SourceId.Index.ContextIndex];
+    ContextEntry->Bits.TranslationType = 0;
+    ContextEntry->Bits.FaultProcessingDisable = 0;
+    ContextEntry->Bits.Present = 0;
+
+    DEBUG ((DEBUG_INFO,"Source: S%04x B%02x D%02x F%02x\n", mVtdUnitInformation[VtdIndex].Segment, SourceId.Bits.Bus, SourceId.Bits.Device, SourceId.Bits.Function));
+
+    switch (mVtdUnitInformation[VtdIndex].CapReg.Bits.SAGAW) {
+    case BIT1:
+      ContextEntry->Bits.AddressWidth = 0x1;
+      break;
+    case BIT2:
+      ContextEntry->Bits.AddressWidth = 0x2;
+      break;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Create second level paging entry table.
+
+  @param[in]  VtdIndex                    The index of the VTd engine.
+  @param[in]  SecondLevelPagingEntry      The second level paging entry.
+  @param[in]  MemoryBase                  The base of the memory.
+  @param[in]  MemoryLimit                 The limit of the memory.
+  @param[in]  IoMmuAccess                 The IOMMU access.
+
+  @return The second level paging entry.
+**/
+VTD_SECOND_LEVEL_PAGING_ENTRY *
+CreateSecondLevelPagingEntryTable (
+  IN UINTN                         VtdIndex,
+  IN VTD_SECOND_LEVEL_PAGING_ENTRY *SecondLevelPagingEntry,
+  IN UINT64                        MemoryBase,
+  IN UINT64                        MemoryLimit,
+  IN UINT64                        IoMmuAccess
+  )
+{
+  UINTN                          Index4;
+  UINTN                          Index3;
+  UINTN                          Index2;
+  UINTN                          Lvl4Start;
+  UINTN                          Lvl4End;
+  UINTN                          Lvl3Start;
+  UINTN                          Lvl3End;
+  VTD_SECOND_LEVEL_PAGING_ENTRY  *Lvl4PtEntry;
+  VTD_SECOND_LEVEL_PAGING_ENTRY  *Lvl3PtEntry;
+  VTD_SECOND_LEVEL_PAGING_ENTRY  *Lvl2PtEntry;
+  UINT64                         BaseAddress;
+  UINT64                         EndAddress;
+
+  if (MemoryLimit == 0) {
+    return EFI_SUCCESS;
+  }
+
+  BaseAddress = ALIGN_VALUE_LOW(MemoryBase, SIZE_2MB);
+  EndAddress = ALIGN_VALUE_UP(MemoryLimit, SIZE_2MB);
+  DEBUG ((DEBUG_INFO,"CreateSecondLevelPagingEntryTable: BaseAddress - 0x%016lx, EndAddress - 0x%016lx\n", BaseAddress, EndAddress));
+
+  if (SecondLevelPagingEntry == NULL) {
+    SecondLevelPagingEntry = AllocateZeroPages (1);
+    if (SecondLevelPagingEntry == NULL) {
+      DEBUG ((DEBUG_ERROR,"Could not Alloc LVL4 PT. \n"));
+      return NULL;
+    }
+  }
+
+  //
+  // If no access is needed, just create not present entry.
+  //
+  if (IoMmuAccess == 0) {
+    return SecondLevelPagingEntry;
+  }
+
+  Lvl4Start = RShiftU64 (BaseAddress, 39) & 0x1FF;
+  Lvl4End = RShiftU64 (EndAddress - 1, 39) & 0x1FF;
+
+  DEBUG ((DEBUG_INFO,"  Lvl4Start - 0x%x, Lvl4End - 0x%x\n", Lvl4Start, Lvl4End));
+
+  Lvl4PtEntry = (VTD_SECOND_LEVEL_PAGING_ENTRY *)SecondLevelPagingEntry;
+  for (Index4 = Lvl4Start; Index4 <= Lvl4End; Index4++) {
+    if (Lvl4PtEntry[Index4].Uint64 == 0) {
+      Lvl4PtEntry[Index4].Uint64 = (UINT64)(UINTN)AllocateZeroPages (1);
+      if (Lvl4PtEntry[Index4].Uint64 == 0) {
+        DEBUG ((DEBUG_ERROR,"!!!!!! ALLOCATE LVL4 PAGE FAIL (0x%x)!!!!!!\n", Index4));
+        ASSERT(FALSE);
+        return NULL;
+      }
+      SetSecondLevelPagingEntryAttribute (&Lvl4PtEntry[Index4], EDKII_IOMMU_ACCESS_READ | EDKII_IOMMU_ACCESS_WRITE);
+    }
+
+    Lvl3Start = RShiftU64 (BaseAddress, 30) & 0x1FF;
+    if (ALIGN_VALUE_LOW(BaseAddress + SIZE_1GB, SIZE_1GB) <= EndAddress) {
+      Lvl3End = SIZE_4KB/sizeof(VTD_SECOND_LEVEL_PAGING_ENTRY);
+    } else {
+      Lvl3End = RShiftU64 (EndAddress - 1, 30) & 0x1FF;
+    }
+    DEBUG ((DEBUG_INFO,"  Lvl4(0x%x): Lvl3Start - 0x%x, Lvl3End - 0x%x\n", Index4, Lvl3Start, Lvl3End));
+
+    Lvl3PtEntry = (VTD_SECOND_LEVEL_PAGING_ENTRY *)(UINTN)LShiftU64 (Lvl4PtEntry[Index4].Bits.Address, 12);
+    for (Index3 = Lvl3Start; Index3 <= Lvl3End; Index3++) {
+      if (Lvl3PtEntry[Index3].Uint64 == 0) {
+        Lvl3PtEntry[Index3].Uint64 = (UINT64)(UINTN)AllocateZeroPages (1);
+        if (Lvl3PtEntry[Index3].Uint64 == 0) {
+          DEBUG ((DEBUG_ERROR,"!!!!!! ALLOCATE LVL3 PAGE FAIL (0x%x, 0x%x)!!!!!!\n", Index4, Index3));
+          ASSERT(FALSE);
+          return NULL;
+        }
+        SetSecondLevelPagingEntryAttribute (&Lvl3PtEntry[Index3], EDKII_IOMMU_ACCESS_READ | EDKII_IOMMU_ACCESS_WRITE);
+      }
+
+      Lvl2PtEntry = (VTD_SECOND_LEVEL_PAGING_ENTRY *)(UINTN)LShiftU64 (Lvl3PtEntry[Index3].Bits.Address, 12);
+      for (Index2 = 0; Index2 < SIZE_4KB/sizeof(VTD_SECOND_LEVEL_PAGING_ENTRY); Index2++) {
+        Lvl2PtEntry[Index2].Uint64 = BaseAddress;
+        SetSecondLevelPagingEntryAttribute (&Lvl2PtEntry[Index2], IoMmuAccess);
+        Lvl2PtEntry[Index2].Bits.PageSize = 1;
+        BaseAddress += SIZE_2MB;
+        if (BaseAddress >= MemoryLimit) {
+          goto Done;
+        }
+      }
+    }
+  }
+
+Done:
+  return SecondLevelPagingEntry;
+}
+
+/**
+  Create second level paging entry.
+
+  @param[in]  VtdIndex                    The index of the VTd engine.
+  @param[in]  IoMmuAccess                 The IOMMU access.
+
+  @return The second level paging entry.
+**/
+VTD_SECOND_LEVEL_PAGING_ENTRY *
+CreateSecondLevelPagingEntry (
+  IN UINTN   VtdIndex,
+  IN UINT64  IoMmuAccess
+  )
+{
+  VTD_SECOND_LEVEL_PAGING_ENTRY *SecondLevelPagingEntry;
+
+  SecondLevelPagingEntry = NULL;
+  SecondLevelPagingEntry = CreateSecondLevelPagingEntryTable (VtdIndex, SecondLevelPagingEntry, 0, mBelow4GMemoryLimit, IoMmuAccess);
+  if (SecondLevelPagingEntry == NULL) {
+    return NULL;
+  }
+  SecondLevelPagingEntry = CreateSecondLevelPagingEntryTable (VtdIndex, SecondLevelPagingEntry, SIZE_4GB, mAbove4GMemoryLimit, IoMmuAccess);
+  if (SecondLevelPagingEntry == NULL) {
+    return NULL;
+  }
+
+  return SecondLevelPagingEntry;
+}
+
+/**
+  Setup VTd translation table.
+
+  @retval EFI_SUCCESS           Setup translation table successfully.
+  @retval EFI_OUT_OF_RESOURCE   Setup translation table fail.
+**/
+EFI_STATUS
+SetupTranslationTable (
+  VOID
+  )
+{
+  EFI_STATUS      Status;
+  UINTN           Index;
+
+  for (Index = 0; Index < mVtdUnitNumber; Index++) {
+    DEBUG((DEBUG_INFO, "CreateContextEntry - %d\n", Index));
+    if (mVtdUnitInformation[Index].ECapReg.Bits.ECS) {
+      Status = CreateExtContextEntry (Index);
+    } else {
+      Status = CreateContextEntry (Index);
+    }
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Dump DMAR context entry table.
+
+  @param[in]  RootEntry DMAR root entry.
+**/
+VOID
+DumpDmarContextEntryTable (
+  IN VTD_ROOT_ENTRY *RootEntry
+  )
+{
+  UINTN                 Index;
+  UINTN                 Index2;
+  VTD_CONTEXT_ENTRY     *ContextEntry;
+
+  DEBUG ((DEBUG_INFO,"=========================\n"));
+  DEBUG ((DEBUG_INFO,"DMAR Context Entry Table:\n"));
+
+  DEBUG ((DEBUG_INFO,"RootEntry Address - 0x%x\n", RootEntry));
+
+  for (Index = 0; Index < VTD_ROOT_ENTRY_NUMBER; Index++) {
+    if ((RootEntry[Index].Uint128.Uint64Lo != 0) || (RootEntry[Index].Uint128.Uint64Hi != 0)) {
+      DEBUG ((DEBUG_INFO,"  RootEntry(0x%02x) B%02x - 0x%016lx %016lx\n",
+        Index, Index, RootEntry[Index].Uint128.Uint64Hi, RootEntry[Index].Uint128.Uint64Lo));
+    }
+    if (RootEntry[Index].Bits.Present == 0) {
+      continue;
+    }
+    ContextEntry = (VTD_CONTEXT_ENTRY *)(UINTN)LShiftU64 (RootEntry[Index].Bits.ContextTablePointer, 12);
+    for (Index2 = 0; Index2 < VTD_CONTEXT_ENTRY_NUMBER; Index2++) {
+      if ((ContextEntry[Index2].Uint128.Uint64Lo != 0) || (ContextEntry[Index2].Uint128.Uint64Hi != 0)) {
+        DEBUG ((DEBUG_INFO,"    ContextEntry(0x%02x) D%02xF%02x - 0x%016lx %016lx\n",
+          Index2, Index2 >> 3, Index2 & 0x7, ContextEntry[Index2].Uint128.Uint64Hi, ContextEntry[Index2].Uint128.Uint64Lo));
+      }
+      if (ContextEntry[Index2].Bits.Present == 0) {
+        continue;
+      }
+      DumpSecondLevelPagingEntry ((VOID *)(UINTN)LShiftU64 (ContextEntry[Index2].Bits.SecondLevelPageTranslationPointer, 12));
+    }
+  }
+  DEBUG ((DEBUG_INFO,"=========================\n"));
+}
+
+/**
+  Dump DMAR second level paging entry.
+
+  @param[in]  SecondLevelPagingEntry The second level paging entry.
+**/
+VOID
+DumpSecondLevelPagingEntry (
+  IN VOID *SecondLevelPagingEntry
+  )
+{
+  UINTN                          Index4;
+  UINTN                          Index3;
+  UINTN                          Index2;
+  UINTN                          Index1;
+  VTD_SECOND_LEVEL_PAGING_ENTRY  *Lvl4PtEntry;
+  VTD_SECOND_LEVEL_PAGING_ENTRY  *Lvl3PtEntry;
+  VTD_SECOND_LEVEL_PAGING_ENTRY  *Lvl2PtEntry;
+  VTD_SECOND_LEVEL_PAGING_ENTRY  *Lvl1PtEntry;
+
+  DEBUG ((DEBUG_VERBOSE,"================\n"));
+  DEBUG ((DEBUG_VERBOSE,"DMAR Second Level Page Table:\n"));
+
+  DEBUG ((DEBUG_VERBOSE,"SecondLevelPagingEntry Base - 0x%x\n", SecondLevelPagingEntry));
+  Lvl4PtEntry = (VTD_SECOND_LEVEL_PAGING_ENTRY *)SecondLevelPagingEntry;
+  for (Index4 = 0; Index4 < SIZE_4KB/sizeof(VTD_SECOND_LEVEL_PAGING_ENTRY); Index4++) {
+    if (Lvl4PtEntry[Index4].Uint64 != 0) {
+      DEBUG ((DEBUG_VERBOSE,"  Lvl4Pt Entry(0x%03x) - 0x%016lx\n", Index4, Lvl4PtEntry[Index4].Uint64));
+    }
+    if (Lvl4PtEntry[Index4].Uint64 == 0) {
+      continue;
+    }
+    Lvl3PtEntry = (VTD_SECOND_LEVEL_PAGING_ENTRY *)(UINTN)LShiftU64 (Lvl4PtEntry[Index4].Bits.Address, 12);
+    for (Index3 = 0; Index3 < SIZE_4KB/sizeof(VTD_SECOND_LEVEL_PAGING_ENTRY); Index3++) {
+      if (Lvl3PtEntry[Index3].Uint64 != 0) {
+        DEBUG ((DEBUG_VERBOSE,"    Lvl3Pt Entry(0x%03x) - 0x%016lx\n", Index3, Lvl3PtEntry[Index3].Uint64));
+      }
+      if (Lvl3PtEntry[Index3].Uint64 == 0) {
+        continue;
+      }
+
+      Lvl2PtEntry = (VTD_SECOND_LEVEL_PAGING_ENTRY *)(UINTN)LShiftU64 (Lvl3PtEntry[Index3].Bits.Address, 12);
+      for (Index2 = 0; Index2 < SIZE_4KB/sizeof(VTD_SECOND_LEVEL_PAGING_ENTRY); Index2++) {
+        if (Lvl2PtEntry[Index2].Uint64 != 0) {
+          DEBUG ((DEBUG_VERBOSE,"      Lvl2Pt Entry(0x%03x) - 0x%016lx\n", Index2, Lvl2PtEntry[Index2].Uint64));
+        }
+        if (Lvl2PtEntry[Index2].Uint64 == 0) {
+          continue;
+        }
+        if (Lvl2PtEntry[Index2].Bits.PageSize == 0) {
+          Lvl1PtEntry = (VTD_SECOND_LEVEL_PAGING_ENTRY *)(UINTN)LShiftU64 (Lvl2PtEntry[Index2].Bits.Address, 12);
+          for (Index1 = 0; Index1 < SIZE_4KB/sizeof(VTD_SECOND_LEVEL_PAGING_ENTRY); Index1++) {
+            if (Lvl1PtEntry[Index1].Uint64 != 0) {
+              DEBUG ((DEBUG_VERBOSE,"        Lvl1Pt Entry(0x%03x) - 0x%016lx\n", Index1, Lvl1PtEntry[Index1].Uint64));
+            }
+          }
+        }
+      }
+    }
+  }
+  DEBUG ((DEBUG_VERBOSE,"================\n"));
+}
+
+/**
+  Invalid page entry.
+
+  @param VtdIndex  The VTd engine index.
+**/
+VOID
+InvalidatePageEntry (
+  IN UINTN                 VtdIndex
+  )
+{
+  if (mVtdUnitInformation[VtdIndex].HasDirtyPages) {
+    InvalidateVtdIOTLBGlobal (VtdIndex);
+  }
+  mVtdUnitInformation[VtdIndex].HasDirtyPages = FALSE;
+}
+
+#define VTD_PG_R                   BIT0
+#define VTD_PG_W                   BIT1
+#define VTD_PG_X                   BIT2
+#define VTD_PG_EMT                 (BIT3 | BIT4 | BIT5)
+#define VTD_PG_TM                  (BIT62)
+
+#define VTD_PG_PS                  BIT7
+
+#define PAGE_PROGATE_BITS          (VTD_PG_TM | VTD_PG_EMT | VTD_PG_W | VTD_PG_R)
+
+#define PAGING_4K_MASK  0xFFF
+#define PAGING_2M_MASK  0x1FFFFF
+#define PAGING_1G_MASK  0x3FFFFFFF
+
+#define PAGING_VTD_INDEX_MASK     0x1FF
+
+#define PAGING_4K_ADDRESS_MASK_64 0x000FFFFFFFFFF000ull
+#define PAGING_2M_ADDRESS_MASK_64 0x000FFFFFFFE00000ull
+#define PAGING_1G_ADDRESS_MASK_64 0x000FFFFFC0000000ull
+
+typedef enum {
+  PageNone,
+  Page4K,
+  Page2M,
+  Page1G,
+} PAGE_ATTRIBUTE;
+
+typedef struct {
+  PAGE_ATTRIBUTE   Attribute;
+  UINT64           Length;
+  UINT64           AddressMask;
+} PAGE_ATTRIBUTE_TABLE;
+
+PAGE_ATTRIBUTE_TABLE mPageAttributeTable[] = {
+  {Page4K,  SIZE_4KB, PAGING_4K_ADDRESS_MASK_64},
+  {Page2M,  SIZE_2MB, PAGING_2M_ADDRESS_MASK_64},
+  {Page1G,  SIZE_1GB, PAGING_1G_ADDRESS_MASK_64},
+};
+
+/**
+  Return length according to page attributes.
+
+  @param[in]  PageAttributes   The page attribute of the page entry.
+
+  @return The length of page entry.
+**/
+UINTN
+PageAttributeToLength (
+  IN PAGE_ATTRIBUTE  PageAttribute
+  )
+{
+  UINTN  Index;
+  for (Index = 0; Index < sizeof(mPageAttributeTable)/sizeof(mPageAttributeTable[0]); Index++) {
+    if (PageAttribute == mPageAttributeTable[Index].Attribute) {
+      return (UINTN)mPageAttributeTable[Index].Length;
+    }
+  }
+  return 0;
+}
+
+/**
+  Return page table entry to match the address.
+
+  @param[in]   SecondLevelPagingEntry   The second level paging entry in VTd table for the device.
+  @param[in]   Address                  The address to be checked.
+  @param[out]  PageAttributes           The page attribute of the page entry.
+
+  @return The page entry.
+**/
+VOID *
+GetSecondLevelPageTableEntry (
+  IN  VTD_SECOND_LEVEL_PAGING_ENTRY *SecondLevelPagingEntry,
+  IN  PHYSICAL_ADDRESS              Address,
+  OUT PAGE_ATTRIBUTE                *PageAttribute
+  )
+{
+  UINTN                 Index1;
+  UINTN                 Index2;
+  UINTN                 Index3;
+  UINTN                 Index4;
+  UINT64                *L1PageTable;
+  UINT64                *L2PageTable;
+  UINT64                *L3PageTable;
+  UINT64                *L4PageTable;
+
+  Index4 = ((UINTN)RShiftU64 (Address, 39)) & PAGING_VTD_INDEX_MASK;
+  Index3 = ((UINTN)Address >> 30) & PAGING_VTD_INDEX_MASK;
+  Index2 = ((UINTN)Address >> 21) & PAGING_VTD_INDEX_MASK;
+  Index1 = ((UINTN)Address >> 12) & PAGING_VTD_INDEX_MASK;
+
+  L4PageTable = (UINT64 *)SecondLevelPagingEntry;
+  if (L4PageTable[Index4] == 0) {
+    L4PageTable[Index4] = (UINT64)(UINTN)AllocateZeroPages (1);
+    if (L4PageTable[Index4] == 0) {
+      DEBUG ((DEBUG_ERROR,"!!!!!! ALLOCATE LVL4 PAGE FAIL (0x%x)!!!!!!\n", Index4));
+      ASSERT(FALSE);
+      *PageAttribute = PageNone;
+      return NULL;
+    }
+    SetSecondLevelPagingEntryAttribute ((VTD_SECOND_LEVEL_PAGING_ENTRY *)&L4PageTable[Index4], EDKII_IOMMU_ACCESS_READ | EDKII_IOMMU_ACCESS_WRITE);
+  }
+
+  L3PageTable = (UINT64 *)(UINTN)(L4PageTable[Index4] & PAGING_4K_ADDRESS_MASK_64);
+  if (L3PageTable[Index3] == 0) {
+    L3PageTable[Index3] = (UINT64)(UINTN)AllocateZeroPages (1);
+    if (L3PageTable[Index3] == 0) {
+      DEBUG ((DEBUG_ERROR,"!!!!!! ALLOCATE LVL3 PAGE FAIL (0x%x, 0x%x)!!!!!!\n", Index4, Index3));
+      ASSERT(FALSE);
+      *PageAttribute = PageNone;
+      return NULL;
+    }
+    SetSecondLevelPagingEntryAttribute ((VTD_SECOND_LEVEL_PAGING_ENTRY *)&L3PageTable[Index3], EDKII_IOMMU_ACCESS_READ | EDKII_IOMMU_ACCESS_WRITE);
+  }
+  if ((L3PageTable[Index3] & VTD_PG_PS) != 0) {
+    // 1G
+    *PageAttribute = Page1G;
+    return &L3PageTable[Index3];
+  }
+
+  L2PageTable = (UINT64 *)(UINTN)(L3PageTable[Index3] & PAGING_4K_ADDRESS_MASK_64);
+  if (L2PageTable[Index2] == 0) {
+    L2PageTable[Index2] = Address & PAGING_2M_ADDRESS_MASK_64;
+    SetSecondLevelPagingEntryAttribute ((VTD_SECOND_LEVEL_PAGING_ENTRY *)&L2PageTable[Index2], 0);
+    L2PageTable[Index2] |= VTD_PG_PS;
+  }
+  if ((L2PageTable[Index2] & VTD_PG_PS) != 0) {
+    // 2M
+    *PageAttribute = Page2M;
+    return &L2PageTable[Index2];
+  }
+
+  // 4k
+  L1PageTable = (UINT64 *)(UINTN)(L2PageTable[Index2] & PAGING_4K_ADDRESS_MASK_64);
+  if ((L1PageTable[Index1] == 0) && (Address != 0)) {
+    *PageAttribute = PageNone;
+    return NULL;
+  }
+  *PageAttribute = Page4K;
+  return &L1PageTable[Index1];
+}
+
+/**
+  Modify memory attributes of page entry.
+
+  @param[in]   PageEntry        The page entry.
+  @param[in]   IoMmuAccess      The IOMMU access.
+  @param[out]  IsModified       TRUE means page table modified. FALSE means page table not modified.
+**/
+VOID
+ConvertSecondLevelPageEntryAttribute (
+  IN  VTD_SECOND_LEVEL_PAGING_ENTRY     *PageEntry,
+  IN  UINT64                            IoMmuAccess,
+  OUT BOOLEAN                           *IsModified
+  )
+{
+  UINT64  CurrentPageEntry;
+  UINT64  NewPageEntry;
+
+  CurrentPageEntry = PageEntry->Uint64;
+  SetSecondLevelPagingEntryAttribute (PageEntry, IoMmuAccess);
+  NewPageEntry = PageEntry->Uint64;
+  if (CurrentPageEntry != NewPageEntry) {
+    *IsModified = TRUE;
+    DEBUG ((DEBUG_VERBOSE, "ConvertSecondLevelPageEntryAttribute 0x%lx", CurrentPageEntry));
+    DEBUG ((DEBUG_VERBOSE, "->0x%lx\n", NewPageEntry));
+  } else {
+    *IsModified = FALSE;
+  }
+}
+
+/**
+  This function returns if there is need to split page entry.
+
+  @param[in]  BaseAddress      The base address to be checked.
+  @param[in]  Length           The length to be checked.
+  @param[in]  PageAttribute    The page attribute of the page entry.
+
+  @retval SplitAttributes on if there is need to split page entry.
+**/
+PAGE_ATTRIBUTE
+NeedSplitPage (
+  IN  PHYSICAL_ADDRESS                  BaseAddress,
+  IN  UINT64                            Length,
+  IN  PAGE_ATTRIBUTE                    PageAttribute
+  )
+{
+  UINT64                PageEntryLength;
+
+  PageEntryLength = PageAttributeToLength (PageAttribute);
+
+  if (((BaseAddress & (PageEntryLength - 1)) == 0) && (Length >= PageEntryLength)) {
+    return PageNone;
+  }
+
+  if (((BaseAddress & PAGING_2M_MASK) != 0) || (Length < SIZE_2MB)) {
+    return Page4K;
+  }
+
+  return Page2M;
+}
+
+/**
+  This function splits one page entry to small page entries.
+
+  @param[in]  PageEntry        The page entry to be splitted.
+  @param[in]  PageAttribute    The page attribute of the page entry.
+  @param[in]  SplitAttribute   How to split the page entry.
+
+  @retval RETURN_SUCCESS            The page entry is splitted.
+  @retval RETURN_UNSUPPORTED        The page entry does not support to be splitted.
+  @retval RETURN_OUT_OF_RESOURCES   No resource to split page entry.
+**/
+RETURN_STATUS
+SplitSecondLevelPage (
+  IN  VTD_SECOND_LEVEL_PAGING_ENTRY     *PageEntry,
+  IN  PAGE_ATTRIBUTE                    PageAttribute,
+  IN  PAGE_ATTRIBUTE                    SplitAttribute
+  )
+{
+  UINT64   BaseAddress;
+  UINT64   *NewPageEntry;
+  UINTN    Index;
+
+  ASSERT (PageAttribute == Page2M || PageAttribute == Page1G);
+
+  if (PageAttribute == Page2M) {
+    //
+    // Split 2M to 4K
+    //
+    ASSERT (SplitAttribute == Page4K);
+    if (SplitAttribute == Page4K) {
+      NewPageEntry = AllocateZeroPages (1);
+      DEBUG ((DEBUG_VERBOSE, "Split - 0x%x\n", NewPageEntry));
+      if (NewPageEntry == NULL) {
+        return RETURN_OUT_OF_RESOURCES;
+      }
+      BaseAddress = PageEntry->Uint64 & PAGING_2M_ADDRESS_MASK_64;
+      for (Index = 0; Index < SIZE_4KB / sizeof(UINT64); Index++) {
+        NewPageEntry[Index] = (BaseAddress + SIZE_4KB * Index) | (PageEntry->Uint64 & PAGE_PROGATE_BITS);
+      }
+      PageEntry->Uint64 = (UINT64)(UINTN)NewPageEntry;
+      SetSecondLevelPagingEntryAttribute (PageEntry, EDKII_IOMMU_ACCESS_READ | EDKII_IOMMU_ACCESS_WRITE);
+      return RETURN_SUCCESS;
+    } else {
+      return RETURN_UNSUPPORTED;
+    }
+  } else if (PageAttribute == Page1G) {
+    //
+    // Split 1G to 2M
+    // No need support 1G->4K directly, we should use 1G->2M, then 2M->4K to get more compact page table.
+    //
+    ASSERT (SplitAttribute == Page2M || SplitAttribute == Page4K);
+    if ((SplitAttribute == Page2M || SplitAttribute == Page4K)) {
+      NewPageEntry = AllocateZeroPages (1);
+      DEBUG ((DEBUG_VERBOSE, "Split - 0x%x\n", NewPageEntry));
+      if (NewPageEntry == NULL) {
+        return RETURN_OUT_OF_RESOURCES;
+      }
+      BaseAddress = PageEntry->Uint64 & PAGING_1G_ADDRESS_MASK_64;
+      for (Index = 0; Index < SIZE_4KB / sizeof(UINT64); Index++) {
+        NewPageEntry[Index] = (BaseAddress + SIZE_2MB * Index) | VTD_PG_PS | (PageEntry->Uint64 & PAGE_PROGATE_BITS);
+      }
+      PageEntry->Uint64 = (UINT64)(UINTN)NewPageEntry;
+      SetSecondLevelPagingEntryAttribute (PageEntry, EDKII_IOMMU_ACCESS_READ | EDKII_IOMMU_ACCESS_WRITE);
+      return RETURN_SUCCESS;
+    } else {
+      return RETURN_UNSUPPORTED;
+    }
+  } else {
+    return RETURN_UNSUPPORTED;
+  }
+}
+
+/**
+  Set VTd attribute for a system memory on second level page entry
+
+  @param[in]  VtdIndex                The index used to identify a VTd engine.
+  @param[in]  SecondLevelPagingEntry  The second level paging entry in VTd table for the device.
+  @param[in]  BaseAddress             The base of device memory address to be used as the DMA memory.
+  @param[in]  Length                  The length of device memory address to be used as the DMA memory.
+  @param[in]  IoMmuAccess             The IOMMU access.
+
+  @retval EFI_SUCCESS            The IoMmuAccess is set for the memory range specified by BaseAddress and Length.
+  @retval EFI_INVALID_PARAMETER  BaseAddress is not IoMmu Page size aligned.
+  @retval EFI_INVALID_PARAMETER  Length is not IoMmu Page size aligned.
+  @retval EFI_INVALID_PARAMETER  Length is 0.
+  @retval EFI_INVALID_PARAMETER  IoMmuAccess specified an illegal combination of access.
+  @retval EFI_UNSUPPORTED        The bit mask of IoMmuAccess is not supported by the IOMMU.
+  @retval EFI_UNSUPPORTED        The IOMMU does not support the memory range specified by BaseAddress and Length.
+  @retval EFI_OUT_OF_RESOURCES   There are not enough resources available to modify the IOMMU access.
+  @retval EFI_DEVICE_ERROR       The IOMMU device reported an error while attempting the operation.
+**/
+EFI_STATUS
+SetSecondLevelPagingAttribute (
+  IN UINTN                         VtdIndex,
+  IN VTD_SECOND_LEVEL_PAGING_ENTRY *SecondLevelPagingEntry,
+  IN UINT64                        BaseAddress,
+  IN UINT64                        Length,
+  IN UINT64                        IoMmuAccess
+  )
+{
+  VTD_SECOND_LEVEL_PAGING_ENTRY  *PageEntry;
+  PAGE_ATTRIBUTE                 PageAttribute;
+  UINTN                          PageEntryLength;
+  PAGE_ATTRIBUTE                 SplitAttribute;
+  EFI_STATUS                     Status;
+  BOOLEAN                        IsEntryModified;
+
+  DEBUG ((DEBUG_VERBOSE,"SetSecondLevelPagingAttribute (%d) (0x%016lx - 0x%016lx : %x) \n", VtdIndex, BaseAddress, Length, IoMmuAccess));
+  DEBUG ((DEBUG_VERBOSE,"  SecondLevelPagingEntry Base - 0x%x\n", SecondLevelPagingEntry));
+
+  if (BaseAddress != ALIGN_VALUE(BaseAddress, SIZE_4KB)) {
+    DEBUG ((DEBUG_ERROR, "SetSecondLevelPagingAttribute - Invalid Alignment\n"));
+    return EFI_UNSUPPORTED;
+  }
+  if (Length != ALIGN_VALUE(Length, SIZE_4KB)) {
+    DEBUG ((DEBUG_ERROR, "SetSecondLevelPagingAttribute - Invalid Alignment\n"));
+    return EFI_UNSUPPORTED;
+  }
+
+  while (Length != 0) {
+    PageEntry = GetSecondLevelPageTableEntry (SecondLevelPagingEntry, BaseAddress, &PageAttribute);
+    if (PageEntry == NULL) {
+      DEBUG ((DEBUG_ERROR, "PageEntry - NULL\n"));
+      return RETURN_UNSUPPORTED;
+    }
+    PageEntryLength = PageAttributeToLength (PageAttribute);
+    SplitAttribute = NeedSplitPage (BaseAddress, Length, PageAttribute);
+    if (SplitAttribute == PageNone) {
+      ConvertSecondLevelPageEntryAttribute (PageEntry, IoMmuAccess, &IsEntryModified);
+      if (IsEntryModified) {
+        mVtdUnitInformation[VtdIndex].HasDirtyPages = TRUE;
+      }
+      //
+      // Convert success, move to next
+      //
+      BaseAddress += PageEntryLength;
+      Length -= PageEntryLength;
+    } else {
+      Status = SplitSecondLevelPage (PageEntry, PageAttribute, SplitAttribute);
+      if (RETURN_ERROR (Status)) {
+        DEBUG ((DEBUG_ERROR, "SplitSecondLevelPage - %r\n", Status));
+        return RETURN_UNSUPPORTED;
+      }
+      mVtdUnitInformation[VtdIndex].HasDirtyPages = TRUE;
+      //
+      // Just split current page
+      // Convert success in next around
+      //
+    }
+  }
+
+  InvalidatePageEntry (VtdIndex);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Set VTd attribute for a system memory.
+
+  @param[in]  VtdIndex                The index used to identify a VTd engine.
+  @param[in]  SecondLevelPagingEntry  The second level paging entry in VTd table for the device.
+  @param[in]  BaseAddress             The base of device memory address to be used as the DMA memory.
+  @param[in]  Length                  The length of device memory address to be used as the DMA memory.
+  @param[in]  IoMmuAccess             The IOMMU access.
+
+  @retval EFI_SUCCESS            The IoMmuAccess is set for the memory range specified by BaseAddress and Length.
+  @retval EFI_INVALID_PARAMETER  BaseAddress is not IoMmu Page size aligned.
+  @retval EFI_INVALID_PARAMETER  Length is not IoMmu Page size aligned.
+  @retval EFI_INVALID_PARAMETER  Length is 0.
+  @retval EFI_INVALID_PARAMETER  IoMmuAccess specified an illegal combination of access.
+  @retval EFI_UNSUPPORTED        The bit mask of IoMmuAccess is not supported by the IOMMU.
+  @retval EFI_UNSUPPORTED        The IOMMU does not support the memory range specified by BaseAddress and Length.
+  @retval EFI_OUT_OF_RESOURCES   There are not enough resources available to modify the IOMMU access.
+  @retval EFI_DEVICE_ERROR       The IOMMU device reported an error while attempting the operation.
+**/
+EFI_STATUS
+SetPageAttribute (
+  IN UINTN                         VtdIndex,
+  IN VTD_SECOND_LEVEL_PAGING_ENTRY *SecondLevelPagingEntry,
+  IN UINT64                        BaseAddress,
+  IN UINT64                        Length,
+  IN UINT64                        IoMmuAccess
+  )
+{
+  EFI_STATUS Status;
+  Status = EFI_NOT_FOUND;
+  if (SecondLevelPagingEntry != NULL) {
+    Status = SetSecondLevelPagingAttribute (VtdIndex, SecondLevelPagingEntry, BaseAddress, Length, IoMmuAccess);
+  }
+  return Status;
+}
+
+/**
+  Set VTd attribute for a system memory.
+
+  @param[in]  Segment           The Segment used to identify a VTd engine.
+  @param[in]  SourceId          The SourceId used to identify a VTd engine and table entry.
+  @param[in]  BaseAddress       The base of device memory address to be used as the DMA memory.
+  @param[in]  Length            The length of device memory address to be used as the DMA memory.
+  @param[in]  IoMmuAccess       The IOMMU access.
+
+  @retval EFI_SUCCESS            The IoMmuAccess is set for the memory range specified by BaseAddress and Length.
+  @retval EFI_INVALID_PARAMETER  BaseAddress is not IoMmu Page size aligned.
+  @retval EFI_INVALID_PARAMETER  Length is not IoMmu Page size aligned.
+  @retval EFI_INVALID_PARAMETER  Length is 0.
+  @retval EFI_INVALID_PARAMETER  IoMmuAccess specified an illegal combination of access.
+  @retval EFI_UNSUPPORTED        The bit mask of IoMmuAccess is not supported by the IOMMU.
+  @retval EFI_UNSUPPORTED        The IOMMU does not support the memory range specified by BaseAddress and Length.
+  @retval EFI_OUT_OF_RESOURCES   There are not enough resources available to modify the IOMMU access.
+  @retval EFI_DEVICE_ERROR       The IOMMU device reported an error while attempting the operation.
+**/
+EFI_STATUS
+SetAccessAttribute (
+  IN UINT16                Segment,
+  IN VTD_SOURCE_ID         SourceId,
+  IN UINT64                BaseAddress,
+  IN UINT64                Length,
+  IN UINT64                IoMmuAccess
+  )
+{
+  UINTN                         VtdIndex;
+  EFI_STATUS                    Status;
+  VTD_EXT_CONTEXT_ENTRY         *ExtContextEntry;
+  VTD_CONTEXT_ENTRY             *ContextEntry;
+  VTD_SECOND_LEVEL_PAGING_ENTRY *SecondLevelPagingEntry;
+  UINT64                        Pt;
+
+  DEBUG ((DEBUG_INFO,"SetAccessAttribute (S%04x B%02x D%02x F%02x) (0x%016lx - 0x%08x, %x)\n", Segment, SourceId.Bits.Bus, SourceId.Bits.Device, SourceId.Bits.Function, BaseAddress, (UINTN)Length, IoMmuAccess));
+
+  VtdIndex = FindVtdIndexByPciDevice (Segment, SourceId, &ExtContextEntry, &ContextEntry);
+  if (VtdIndex == (UINTN)-1) {
+    DEBUG ((DEBUG_ERROR,"SetAccessAttribute - Pci device (S%04x B%02x D%02x F%02x) not found!\n", Segment, SourceId.Bits.Bus, SourceId.Bits.Device, SourceId.Bits.Function));
+    return EFI_DEVICE_ERROR;
+  }
+
+  if (ExtContextEntry != NULL) {
+    if (ExtContextEntry->Bits.Present == 0) {
+      SecondLevelPagingEntry = CreateSecondLevelPagingEntry (VtdIndex, 0);
+      DEBUG ((DEBUG_VERBOSE,"SecondLevelPagingEntry - 0x%x (S%04x B%02x D%02x F%02x) New\n", SecondLevelPagingEntry, Segment, SourceId.Bits.Bus, SourceId.Bits.Device, SourceId.Bits.Function));
+      Pt = (UINT64)RShiftU64 ((UINT64)(UINTN)SecondLevelPagingEntry, 12);
+
+      ExtContextEntry->Bits.SecondLevelPageTranslationPointer = Pt;
+      ExtContextEntry->Bits.DomainIdentifier = GetPciDescriptor (VtdIndex, Segment, SourceId);
+      ExtContextEntry->Bits.Present = 1;
+      DumpDmarExtContextEntryTable (mVtdUnitInformation[VtdIndex].ExtRootEntryTable);
+    } else {
+      SecondLevelPagingEntry = (VOID *)(UINTN)LShiftU64 (ExtContextEntry->Bits.SecondLevelPageTranslationPointer, 12);
+      DEBUG ((DEBUG_VERBOSE,"SecondLevelPagingEntry - 0x%x (S%04x B%02x D%02x F%02x)\n", SecondLevelPagingEntry, Segment, SourceId.Bits.Bus, SourceId.Bits.Device, SourceId.Bits.Function));
+    }
+  } else {
+    if (ContextEntry->Bits.Present == 0) {
+      SecondLevelPagingEntry = CreateSecondLevelPagingEntry (VtdIndex, 0);
+      DEBUG ((DEBUG_VERBOSE,"SecondLevelPagingEntry - 0x%x (S%04x B%02x D%02x F%02x) New\n", SecondLevelPagingEntry, Segment, SourceId.Bits.Bus, SourceId.Bits.Device, SourceId.Bits.Function));
+      Pt = (UINT64)RShiftU64 ((UINT64)(UINTN)SecondLevelPagingEntry, 12);
+
+      ContextEntry->Bits.SecondLevelPageTranslationPointer = Pt;
+      ContextEntry->Bits.DomainIdentifier = GetPciDescriptor (VtdIndex, Segment, SourceId);
+      ContextEntry->Bits.Present = 1;
+      DumpDmarContextEntryTable (mVtdUnitInformation[VtdIndex].RootEntryTable);
+    } else {
+      SecondLevelPagingEntry = (VOID *)(UINTN)LShiftU64 (ContextEntry->Bits.SecondLevelPageTranslationPointer, 12);
+      DEBUG ((DEBUG_VERBOSE,"SecondLevelPagingEntry - 0x%x (S%04x B%02x D%02x F%02x)\n", SecondLevelPagingEntry, Segment, SourceId.Bits.Bus, SourceId.Bits.Device, SourceId.Bits.Function));
+    }
+  }
+
+  //
+  // Do not update FixedSecondLevelPagingEntry
+  //
+  if (SecondLevelPagingEntry != mVtdUnitInformation[VtdIndex].FixedSecondLevelPagingEntry) {
+    Status = SetPageAttribute (
+               VtdIndex,
+               SecondLevelPagingEntry,
+               BaseAddress,
+               Length,
+               IoMmuAccess
+               );
+    if (EFI_ERROR (Status)) {
+      DEBUG ((DEBUG_ERROR,"SetPageAttribute - %r\n", Status));
+      return Status;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Always enable the VTd page attribute for the device.
+
+  @param[in]  Segment           The Segment used to identify a VTd engine.
+  @param[in]  SourceId          The SourceId used to identify a VTd engine and table entry.
+
+  @retval EFI_SUCCESS           The VTd entry is updated to always enable all DMA access for the specific device.
+**/
+EFI_STATUS
+AlwaysEnablePageAttribute (
+  IN UINT16                  Segment,
+  IN VTD_SOURCE_ID           SourceId
+  )
+{
+  UINTN                         VtdIndex;
+  VTD_EXT_CONTEXT_ENTRY         *ExtContextEntry;
+  VTD_CONTEXT_ENTRY             *ContextEntry;
+  VTD_SECOND_LEVEL_PAGING_ENTRY *SecondLevelPagingEntry;
+  UINT64                        Pt;
+
+  DEBUG ((DEBUG_INFO,"AlwaysEnablePageAttribute (S%04x B%02x D%02x F%02x)\n", Segment, SourceId.Bits.Bus, SourceId.Bits.Device, SourceId.Bits.Function));
+
+  VtdIndex = FindVtdIndexByPciDevice (Segment, SourceId, &ExtContextEntry, &ContextEntry);
+  if (VtdIndex == (UINTN)-1) {
+    DEBUG ((DEBUG_ERROR,"AlwaysEnablePageAttribute - Pci device (S%04x B%02x D%02x F%02x) not found!\n", Segment, SourceId.Bits.Bus, SourceId.Bits.Device, SourceId.Bits.Function));
+    return EFI_DEVICE_ERROR;
+  }
+
+  if (mVtdUnitInformation[VtdIndex].FixedSecondLevelPagingEntry == 0) {
+    DEBUG((DEBUG_INFO, "CreateSecondLevelPagingEntry - %d\n", VtdIndex));
+    mVtdUnitInformation[VtdIndex].FixedSecondLevelPagingEntry = CreateSecondLevelPagingEntry (VtdIndex, EDKII_IOMMU_ACCESS_READ | EDKII_IOMMU_ACCESS_WRITE);
+  }
+
+  SecondLevelPagingEntry = mVtdUnitInformation[VtdIndex].FixedSecondLevelPagingEntry;
+  Pt = (UINT64)RShiftU64 ((UINT64)(UINTN)SecondLevelPagingEntry, 12);
+  if (ExtContextEntry != NULL) {
+    ExtContextEntry->Bits.SecondLevelPageTranslationPointer = Pt;
+    ExtContextEntry->Bits.DomainIdentifier = ((1 << (UINT8)((UINTN)mVtdUnitInformation[VtdIndex].CapReg.Bits.ND * 2 + 4)) - 1);
+    ExtContextEntry->Bits.Present = 1;
+  } else {
+    ContextEntry->Bits.SecondLevelPageTranslationPointer = Pt;
+    ContextEntry->Bits.DomainIdentifier = ((1 << (UINT8)((UINTN)mVtdUnitInformation[VtdIndex].CapReg.Bits.ND * 2 + 4)) - 1);
+    ContextEntry->Bits.Present = 1;
+  }
+
+  return EFI_SUCCESS;
+}
diff --git a/IntelSiliconPkg/IntelVTdDxe/TranslationTableEx.c b/IntelSiliconPkg/IntelVTdDxe/TranslationTableEx.c
new file mode 100644
index 0000000..0f54b97
--- /dev/null
+++ b/IntelSiliconPkg/IntelVTdDxe/TranslationTableEx.c
@@ -0,0 +1,153 @@
+/** @file
+
+  Copyright (c) 2017, Intel Corporation. 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 "DmaProtection.h"
+
+/**
+  Create extended context entry.
+
+  @param[in]  VtdIndex  The index of the VTd engine.
+
+  @retval EFI_SUCCESS          The extended context entry is created.
+  @retval EFI_OUT_OF_RESOURCE  No enough resource to create extended context entry.
+**/
+EFI_STATUS
+CreateExtContextEntry (
+  IN UINTN  VtdIndex
+  )
+{
+  UINTN                  Index;
+  VOID                   *Buffer;
+  UINTN                  RootPages;
+  UINTN                  ContextPages;
+  VTD_EXT_ROOT_ENTRY     *ExtRootEntry;
+  VTD_EXT_CONTEXT_ENTRY  *ExtContextEntryTable;
+  VTD_EXT_CONTEXT_ENTRY  *ExtContextEntry;
+  VTD_SOURCE_ID          *PciDescriptor;
+  VTD_SOURCE_ID          SourceId;
+  UINTN                  MaxBusNumber;
+  UINTN                  EntryTablePages;
+
+  MaxBusNumber = 0;
+  for (Index = 0; Index < mVtdUnitInformation[VtdIndex].PciDeviceInfo.PciDescriptorNumber; Index++) {
+    PciDescriptor = &mVtdUnitInformation[VtdIndex].PciDeviceInfo.PciDescriptors[Index];
+    if (PciDescriptor->Bits.Bus > MaxBusNumber) {
+      MaxBusNumber = PciDescriptor->Bits.Bus;
+    }
+  }
+  DEBUG ((DEBUG_INFO,"  MaxBusNumber - 0x%x\n", MaxBusNumber));
+
+  RootPages = EFI_SIZE_TO_PAGES (sizeof (VTD_EXT_ROOT_ENTRY) * VTD_ROOT_ENTRY_NUMBER);
+  ContextPages = EFI_SIZE_TO_PAGES (sizeof (VTD_EXT_CONTEXT_ENTRY) * VTD_CONTEXT_ENTRY_NUMBER);
+  EntryTablePages = RootPages + ContextPages * (MaxBusNumber + 1);
+  Buffer = AllocateZeroPages (EntryTablePages);
+  if (Buffer == NULL) {
+    DEBUG ((DEBUG_INFO,"Could not Alloc Root Entry Table.. \n"));
+    return EFI_OUT_OF_RESOURCES;
+  }
+  mVtdUnitInformation[VtdIndex].ExtRootEntryTable = (VTD_EXT_ROOT_ENTRY *)Buffer;
+  Buffer = (UINT8 *)Buffer + EFI_PAGES_TO_SIZE (RootPages);
+
+  for (Index = 0; Index < mVtdUnitInformation[VtdIndex].PciDeviceInfo.PciDescriptorNumber; Index++) {
+    PciDescriptor = &mVtdUnitInformation[VtdIndex].PciDeviceInfo.PciDescriptors[Index];
+
+    SourceId.Bits.Bus = PciDescriptor->Bits.Bus;
+    SourceId.Bits.Device = PciDescriptor->Bits.Device;
+    SourceId.Bits.Function = PciDescriptor->Bits.Function;
+
+    ExtRootEntry = &mVtdUnitInformation[VtdIndex].ExtRootEntryTable[SourceId.Index.RootIndex];
+    if (ExtRootEntry->Bits.LowerPresent == 0) {
+      ExtRootEntry->Bits.LowerContextTablePointer  = RShiftU64 ((UINT64)(UINTN)Buffer, 12);
+      ExtRootEntry->Bits.LowerPresent = 1;
+      ExtRootEntry->Bits.UpperContextTablePointer  = RShiftU64 ((UINT64)(UINTN)Buffer, 12) + 1;
+      ExtRootEntry->Bits.UpperPresent = 1;
+      Buffer = (UINT8 *)Buffer + EFI_PAGES_TO_SIZE (ContextPages);
+    }
+
+    ExtContextEntryTable = (VTD_EXT_CONTEXT_ENTRY *)(UINTN)LShiftU64(ExtRootEntry->Bits.LowerContextTablePointer, 12) ;
+    ExtContextEntry = &ExtContextEntryTable[SourceId.Index.ContextIndex];
+    ExtContextEntry->Bits.TranslationType = 0;
+    ExtContextEntry->Bits.FaultProcessingDisable = 0;
+    ExtContextEntry->Bits.Present = 0;
+
+    DEBUG ((DEBUG_INFO,"DOMAIN: S%04x, B%02x D%02x F%02x\n", mVtdUnitInformation[VtdIndex].Segment, SourceId.Bits.Bus, SourceId.Bits.Device, SourceId.Bits.Function));
+
+    switch (mVtdUnitInformation[VtdIndex].CapReg.Bits.SAGAW) {
+    case BIT1:
+      ExtContextEntry->Bits.AddressWidth = 0x1;
+      break;
+    case BIT2:
+      ExtContextEntry->Bits.AddressWidth = 0x2;
+      break;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Dump DMAR extended context entry table.
+
+  @param[in]  ExtRootEntry DMAR extended root entry.
+**/
+VOID
+DumpDmarExtContextEntryTable (
+  IN VTD_EXT_ROOT_ENTRY *ExtRootEntry
+  )
+{
+  UINTN                 Index;
+  UINTN                 Index2;
+  VTD_EXT_CONTEXT_ENTRY *ExtContextEntry;
+
+  DEBUG ((DEBUG_INFO,"=========================\n"));
+  DEBUG ((DEBUG_INFO,"DMAR ExtContext Entry Table:\n"));
+
+  DEBUG ((DEBUG_INFO,"ExtRootEntry Address - 0x%x\n", ExtRootEntry));
+
+  for (Index = 0; Index < VTD_ROOT_ENTRY_NUMBER; Index++) {
+    if ((ExtRootEntry[Index].Uint128.Uint64Lo != 0) || (ExtRootEntry[Index].Uint128.Uint64Hi != 0)) {
+      DEBUG ((DEBUG_INFO,"  ExtRootEntry(0x%02x) B%02x - 0x%016lx %016lx\n",
+        Index, Index, ExtRootEntry[Index].Uint128.Uint64Hi, ExtRootEntry[Index].Uint128.Uint64Lo));
+    }
+    if (ExtRootEntry[Index].Bits.LowerPresent == 0) {
+      continue;
+    }
+    ExtContextEntry = (VTD_EXT_CONTEXT_ENTRY *)(UINTN)LShiftU64 (ExtRootEntry[Index].Bits.LowerContextTablePointer, 12);
+    for (Index2 = 0; Index2 < VTD_CONTEXT_ENTRY_NUMBER/2; Index2++) {
+      if ((ExtContextEntry[Index2].Uint256.Uint64_1 != 0) || (ExtContextEntry[Index2].Uint256.Uint64_2 != 0) ||
+          (ExtContextEntry[Index2].Uint256.Uint64_3 != 0) || (ExtContextEntry[Index2].Uint256.Uint64_4 != 0)) {
+        DEBUG ((DEBUG_INFO,"    ExtContextEntryLower(0x%02x) D%02xF%02x - 0x%016lx %016lx %016lx %016lx\n",
+          Index2, Index2 >> 3, Index2 & 0x7, ExtContextEntry[Index2].Uint256.Uint64_4, ExtContextEntry[Index2].Uint256.Uint64_3, ExtContextEntry[Index2].Uint256.Uint64_2, ExtContextEntry[Index2].Uint256.Uint64_1));
+      }
+      if (ExtContextEntry[Index2].Bits.Present == 0) {
+        continue;
+      }
+    }
+
+    if (ExtRootEntry[Index].Bits.UpperPresent == 0) {
+      continue;
+    }
+    ExtContextEntry = (VTD_EXT_CONTEXT_ENTRY *)(UINTN)LShiftU64 (ExtRootEntry[Index].Bits.UpperContextTablePointer, 12);
+    for (Index2 = 0; Index2 < VTD_CONTEXT_ENTRY_NUMBER/2; Index2++) {
+      if ((ExtContextEntry[Index2].Uint256.Uint64_1 != 0) || (ExtContextEntry[Index2].Uint256.Uint64_2 != 0) ||
+          (ExtContextEntry[Index2].Uint256.Uint64_3 != 0) || (ExtContextEntry[Index2].Uint256.Uint64_4 != 0)) {
+        DEBUG ((DEBUG_INFO,"    ExtContextEntryUpper(0x%02x) D%02xF%02x - 0x%016lx %016lx %016lx %016lx\n",
+          Index2, (Index2 + 128) >> 3, (Index2 + 128) & 0x7, ExtContextEntry[Index2].Uint256.Uint64_4, ExtContextEntry[Index2].Uint256.Uint64_3, ExtContextEntry[Index2].Uint256.Uint64_2, ExtContextEntry[Index2].Uint256.Uint64_1));
+      }
+      if (ExtContextEntry[Index2].Bits.Present == 0) {
+        continue;
+      }
+    }
+  }
+  DEBUG ((DEBUG_INFO,"=========================\n"));
+}
diff --git a/IntelSiliconPkg/IntelVTdDxe/VtdReg.c b/IntelSiliconPkg/IntelVTdDxe/VtdReg.c
new file mode 100644
index 0000000..456a039
--- /dev/null
+++ b/IntelSiliconPkg/IntelVTdDxe/VtdReg.c
@@ -0,0 +1,602 @@
+/** @file
+
+  Copyright (c) 2017, Intel Corporation. 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 "DmaProtection.h"
+
+UINT64                           mVtdHostAddressWidthMask;
+UINTN                            mVtdUnitNumber;
+VTD_UNIT_INFORMATION             *mVtdUnitInformation;
+
+BOOLEAN  mVtdEnabled;
+
+/**
+  Invalid VTd global IOTLB.
+
+  @param[in]  VtdIndex              The index of VTd engine.
+
+  @retval EFI_SUCCESS           VTd global IOTLB is invalidated.
+  @retval EFI_DEVICE_ERROR      VTd global IOTLB is not invalidated.
+**/
+EFI_STATUS
+InvalidateVtdIOTLBGlobal (
+  IN UINTN  VtdIndex
+  )
+{
+  UINT64  Reg64;
+  UINT32  Reg32;
+
+  if (!mVtdEnabled) {
+    return EFI_SUCCESS;
+  }
+
+  DEBUG((DEBUG_VERBOSE, "InvalidateVtdIOTLBGlobal(%d)\n", VtdIndex));
+
+  AsmWbinvd();
+
+  //
+  // Write Buffer Flush before invalidation
+  //
+  Reg32 = MmioRead32 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_CAP_REG);
+  if ((Reg32 & B_CAP_REG_RWBF) != 0) {
+    MmioWrite32 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_GCMD_REG, B_GMCD_REG_WBF);
+  }
+
+  //
+  // Invalidate the context cache
+  //
+  Reg64 = MmioRead64 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_CCMD_REG);
+  if ((Reg64 & B_CCMD_REG_ICC) != 0) {
+    DEBUG ((DEBUG_ERROR,"ERROR: InvalidateVtdIOTLBGlobal: B_CCMD_REG_ICC is set for VTD(%d)\n",VtdIndex));
+    return EFI_DEVICE_ERROR;
+  }
+
+  Reg64 &= ((~B_CCMD_REG_ICC) & (~B_CCMD_REG_CIRG_MASK));
+  Reg64 |= (B_CCMD_REG_ICC | V_CCMD_REG_CIRG_GLOBAL);
+  MmioWrite64 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_CCMD_REG, Reg64);
+
+  do {
+    Reg64 = MmioRead64 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_CCMD_REG);
+  } while ((Reg64 & B_CCMD_REG_ICC) != 0);
+
+  //
+  // Invalidate the IOTLB cache
+  //
+
+  Reg64 = MmioRead64 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + (mVtdUnitInformation[VtdIndex].ECapReg.Bits.IRO * 16) + R_IOTLB_REG);
+  if ((Reg64 & B_IOTLB_REG_IVT) != 0) {
+    DEBUG ((DEBUG_ERROR,"ERROR: InvalidateVtdIOTLBGlobal: B_IOTLB_REG_IVT is set for VTD(%d)\n", VtdIndex));
+    return EFI_DEVICE_ERROR;
+  }
+
+  Reg64 &= ((~B_IOTLB_REG_IVT) & (~B_IOTLB_REG_IIRG_MASK));
+  Reg64 |= (B_IOTLB_REG_IVT | V_IOTLB_REG_IIRG_GLOBAL);
+  MmioWrite64 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + (mVtdUnitInformation[VtdIndex].ECapReg.Bits.IRO * 16) + R_IOTLB_REG, Reg64);
+
+  do {
+    Reg64 = MmioRead64 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + (mVtdUnitInformation[VtdIndex].ECapReg.Bits.IRO * 16) + R_IOTLB_REG);
+  } while ((Reg64 & B_IOTLB_REG_IVT) != 0);
+
+  //
+  // Disable VTd
+  //
+  MmioWrite32 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_GCMD_REG, B_GMCD_REG_SRTP);
+  do {
+    Reg32 = MmioRead32 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_GSTS_REG);
+  } while((Reg32 & B_GSTS_REG_RTPS) == 0);
+
+  //
+  // Enable VTd
+  //
+  MmioWrite32 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_GCMD_REG, B_GMCD_REG_TE);
+  do {
+    Reg32 = MmioRead32 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_GSTS_REG);
+  } while ((Reg32 & B_GSTS_REG_TE) == 0);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Invalid VTd IOTLB domain.
+
+  @param[in]  VtdIndex              The index of VTd engine.
+  @param[in]  DomainIdentifier      The domain ID of the source.
+
+  @retval EFI_SUCCESS           VTd IOTLB domain is invalidated.
+  @retval EFI_DEVICE_ERROR      VTd IOTLB domain is not invalidated.
+**/
+EFI_STATUS
+InvalidateVtdIOTLBDomain (
+  IN UINTN  VtdIndex,
+  IN UINT16 DomainIdentifier
+  )
+{
+  UINT64  Reg64;
+
+  if (!mVtdEnabled) {
+    return EFI_SUCCESS;
+  }
+
+  DEBUG((DEBUG_VERBOSE, "InvalidateVtdIOTLBDomain(%d): 0x%016lx (0x%04x)\n", VtdIndex, DomainIdentifier));
+
+  //
+  // Invalidate the context cache
+  //
+  Reg64 = MmioRead64 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_CCMD_REG);
+  if ((Reg64 & B_CCMD_REG_ICC) != 0) {
+    DEBUG ((DEBUG_ERROR,"ERROR: InvalidateVtdIOTLBDomain: B_CCMD_REG_ICC is set for VTD(%d)\n",VtdIndex));
+    return EFI_DEVICE_ERROR;
+  }
+
+  Reg64 &= ((~B_CCMD_REG_ICC) & (~B_CCMD_REG_CIRG_MASK));
+  Reg64 |= (B_CCMD_REG_ICC | V_CCMD_REG_CIRG_DOMAIN);
+  Reg64 |= (B_CCMD_REG_ICC | V_CCMD_REG_CIRG_DOMAIN);
+  MmioWrite64 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_CCMD_REG, Reg64);
+
+  do {
+    Reg64 = MmioRead64 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_CCMD_REG);
+  } while ((Reg64 & B_CCMD_REG_ICC) != 0);
+
+  //
+  // Invalidate the IOTLB cache
+  //
+
+  Reg64 = MmioRead64 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + (mVtdUnitInformation[VtdIndex].ECapReg.Bits.IRO * 16) + R_IOTLB_REG);
+  if ((Reg64 & B_IOTLB_REG_IVT) != 0) {
+    DEBUG ((DEBUG_ERROR,"ERROR: InvalidateVtdIOTLBDomain: B_IOTLB_REG_IVT is set for VTD(%d)\n", VtdIndex));
+    return EFI_DEVICE_ERROR;
+  }
+
+  Reg64 &= ((~B_IOTLB_REG_IVT) & (~B_IOTLB_REG_IIRG_MASK));
+  Reg64 |= (B_IOTLB_REG_IVT | V_IOTLB_REG_IIRG_DOMAIN);
+  Reg64 |= DomainIdentifier;
+  MmioWrite64 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + (mVtdUnitInformation[VtdIndex].ECapReg.Bits.IRO * 16) + R_IOTLB_REG, Reg64);
+
+  do {
+    Reg64 = MmioRead64 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + (mVtdUnitInformation[VtdIndex].ECapReg.Bits.IRO * 16) + R_IOTLB_REG);
+  } while ((Reg64 & B_IOTLB_REG_IVT) != 0);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Invalid VTd IOTLB page.
+
+  @param[in]  VtdIndex              The index of VTd engine.
+  @param[in]  Address               The address of IOTLB page.
+  @param[in]  AddressMode           The address mode of IOTLB page.
+  @param[in]  DomainIdentifier      The domain ID of the source.
+
+  @retval EFI_SUCCESS           VTd IOTLB page is invalidated.
+  @retval EFI_DEVICE_ERROR      VTd IOTLB page is not invalidated.
+**/
+EFI_STATUS
+InvalidateVtdIOTLBPage (
+  IN UINTN  VtdIndex,
+  IN UINT64 Address,
+  IN UINT8  AddressMode,
+  IN UINT16 DomainIdentifier
+  )
+{
+  UINT64  Reg64;
+  UINT64  Data64;
+
+  if (!mVtdEnabled) {
+    return EFI_SUCCESS;
+  }
+
+  DEBUG((DEBUG_VERBOSE, "InvalidateVtdIOTLBPage(%d): 0x%016lx (0x%02x)\n", VtdIndex, Address, AddressMode));
+
+  if (mVtdUnitInformation[VtdIndex].CapReg.Bits.PSI != 0) {
+    Reg64 = MmioRead64 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + (mVtdUnitInformation[VtdIndex].ECapReg.Bits.IRO * 16) + R_IOTLB_REG);
+    if ((Reg64 & B_IOTLB_REG_IVT) != 0) {
+      DEBUG ((DEBUG_ERROR,"ERROR: InvalidateVtdIOTLBPage: B_IOTLB_REG_IVT is set for VTD(%d)\n", VtdIndex));
+      return EFI_DEVICE_ERROR;
+    }
+
+    Data64 = Address | AddressMode;
+    MmioWrite64 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + (mVtdUnitInformation[VtdIndex].ECapReg.Bits.IRO * 16) + R_IVA_REG, Data64);
+
+    Reg64 &= ((~B_IOTLB_REG_IVT) & (~B_IOTLB_REG_IIRG_MASK));
+    Reg64 |= (B_IOTLB_REG_IVT | V_IOTLB_REG_IIRG_PAGE);
+    Reg64 |= LShiftU64 (DomainIdentifier, 32);
+    MmioWrite64 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + (mVtdUnitInformation[VtdIndex].ECapReg.Bits.IRO * 16) + R_IOTLB_REG, Reg64);
+
+    do {
+      Reg64 = MmioRead64 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + (mVtdUnitInformation[VtdIndex].ECapReg.Bits.IRO * 16) + R_IOTLB_REG);
+    } while ((Reg64 & B_IOTLB_REG_IVT) != 0);
+  } else {
+    InvalidateVtdIOTLBGlobal (VtdIndex);
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Prepare VTD configuration.
+**/
+VOID
+PrepareVtdConfig (
+  VOID
+  )
+{
+  UINTN         Index;
+  UINTN         DomainNumber;
+
+  for (Index = 0; Index < mVtdUnitNumber; Index++) {
+    DEBUG ((DEBUG_INFO, "Dump VTd Capability (%d)\n", Index));
+    mVtdUnitInformation[Index].CapReg.Uint64 = MmioRead64 (mVtdUnitInformation[Index].VtdUnitBaseAddress + R_CAP_REG);
+    DumpVtdCapRegs (&mVtdUnitInformation[Index].CapReg);
+    mVtdUnitInformation[Index].ECapReg.Uint64 = MmioRead64 (mVtdUnitInformation[Index].VtdUnitBaseAddress + R_ECAP_REG);
+    DumpVtdECapRegs (&mVtdUnitInformation[Index].ECapReg);
+
+    if ((mVtdUnitInformation[Index].CapReg.Bits.SLLPS & BIT0) == 0) {
+      DEBUG((DEBUG_WARN, "!!!! 2MB super page is not supported on VTD %d !!!!\n", Index));
+    }
+    if ((mVtdUnitInformation[Index].CapReg.Bits.SAGAW & BIT2) == 0) {
+      DEBUG((DEBUG_ERROR, "!!!! 4-level page-table is not supported on VTD %d !!!!\n", Index));
+      return ;
+    }
+
+    DomainNumber = (UINTN)1 << (UINT8)((UINTN)mVtdUnitInformation[Index].CapReg.Bits.ND * 2 + 4);
+    if (mVtdUnitInformation[Index].PciDeviceInfo.PciDescriptorNumber >= DomainNumber) {
+      DEBUG((DEBUG_ERROR, "!!!! Pci device Number(0x%x) >= DomainNumber(0x%x) !!!!\n", mVtdUnitInformation[Index].PciDeviceInfo.PciDescriptorNumber, DomainNumber));
+      return ;
+    }
+  }
+  return ;
+}
+
+/**
+  Enable DMAR translation.
+
+  @retval EFI_SUCCESS           DMAR translation is enabled.
+  @retval EFI_DEVICE_ERROR      DMAR translation is not enabled.
+**/
+EFI_STATUS
+EnableDmar (
+  VOID
+  )
+{
+  UINTN     Index;
+  UINT64    Reg64;
+  UINT32    Reg32;
+
+  AsmWbinvd();
+
+  for (Index = 0; Index < mVtdUnitNumber; Index++) {
+    DEBUG((DEBUG_INFO, ">>>>>>EnableDmar() for engine [%d] \n", Index));
+
+    if (mVtdUnitInformation[Index].ExtRootEntryTable != NULL) {
+      DEBUG((DEBUG_INFO, "ExtRootEntryTable 0x%x \n", mVtdUnitInformation[Index].ExtRootEntryTable));
+      MmioWrite64 (mVtdUnitInformation[Index].VtdUnitBaseAddress + R_RTADDR_REG, (UINT64)(UINTN)mVtdUnitInformation[Index].ExtRootEntryTable | BIT11);
+    } else {
+      DEBUG((DEBUG_INFO, "RootEntryTable 0x%x \n", mVtdUnitInformation[Index].RootEntryTable));
+      MmioWrite64 (mVtdUnitInformation[Index].VtdUnitBaseAddress + R_RTADDR_REG, (UINT64)(UINTN)mVtdUnitInformation[Index].RootEntryTable);
+    }
+
+    MmioWrite32 (mVtdUnitInformation[Index].VtdUnitBaseAddress + R_GCMD_REG, B_GMCD_REG_SRTP);
+
+    DEBUG((DEBUG_INFO, "EnableDmar: waiting for RTPS bit to be set... \n"));
+    do {
+      Reg32 = MmioRead32 (mVtdUnitInformation[Index].VtdUnitBaseAddress + R_GSTS_REG);
+    } while((Reg32 & B_GSTS_REG_RTPS) == 0);
+
+    //
+    // Init DMAr Fault Event and Data registers
+    //
+    Reg32 = MmioRead32 (mVtdUnitInformation[Index].VtdUnitBaseAddress + R_FEDATA_REG);
+
+    //
+    // Write Buffer Flush before invalidation
+    //
+    Reg32 = MmioRead32 (mVtdUnitInformation[Index].VtdUnitBaseAddress + R_CAP_REG);
+    if ((Reg32 & B_CAP_REG_RWBF) != 0) {
+      MmioWrite32 (mVtdUnitInformation[Index].VtdUnitBaseAddress + R_GCMD_REG, B_GMCD_REG_WBF);
+    }
+
+    //
+    // Invalidate the context cache
+    //
+    Reg64 = MmioRead64 (mVtdUnitInformation[Index].VtdUnitBaseAddress + R_CCMD_REG);
+    if ((Reg64 & B_CCMD_REG_ICC) != 0) {
+      DEBUG ((DEBUG_INFO,"ERROR: EnableDmar: B_CCMD_REG_ICC is set for VTD(%d)\n",Index));
+      return EFI_DEVICE_ERROR;
+    }
+
+    Reg64 &= ((~B_CCMD_REG_ICC) & (~B_CCMD_REG_CIRG_MASK));
+    Reg64 |= (B_CCMD_REG_ICC | V_CCMD_REG_CIRG_GLOBAL);
+    MmioWrite64 (mVtdUnitInformation[Index].VtdUnitBaseAddress + R_CCMD_REG, Reg64);
+
+    DEBUG((DEBUG_INFO, "EnableDmar: Waiting B_CCMD_REG_ICC ...\n"));
+    do {
+      Reg64 = MmioRead64 (mVtdUnitInformation[Index].VtdUnitBaseAddress + R_CCMD_REG);
+    } while ((Reg64 & B_CCMD_REG_ICC) != 0);
+
+    //
+    // Invalidate the IOTLB cache
+    //
+    DEBUG((DEBUG_INFO, "EnableDmar: IRO 0x%x\n", mVtdUnitInformation[Index].ECapReg.Bits.IRO));
+
+    Reg64 = MmioRead64 (mVtdUnitInformation[Index].VtdUnitBaseAddress + (mVtdUnitInformation[Index].ECapReg.Bits.IRO * 16) + R_IOTLB_REG);
+    if ((Reg64 & B_IOTLB_REG_IVT) != 0) {
+      DEBUG ((DEBUG_INFO,"ERROR: EnableDmar: B_IOTLB_REG_IVT is set for VTD(%d)\n", Index));
+      return EFI_DEVICE_ERROR;
+    }
+
+    Reg64 &= ((~B_IOTLB_REG_IVT) & (~B_IOTLB_REG_IIRG_MASK));
+    Reg64 |= (B_IOTLB_REG_IVT | V_IOTLB_REG_IIRG_GLOBAL);
+    MmioWrite64 (mVtdUnitInformation[Index].VtdUnitBaseAddress + (mVtdUnitInformation[Index].ECapReg.Bits.IRO * 16) + R_IOTLB_REG, Reg64);
+
+    DEBUG((DEBUG_INFO, "EnableDmar: Waiting B_IOTLB_REG_IVT ...\n"));
+    do {
+      Reg64 = MmioRead64 (mVtdUnitInformation[Index].VtdUnitBaseAddress + (mVtdUnitInformation[Index].ECapReg.Bits.IRO * 16) + R_IOTLB_REG);
+    } while ((Reg64 & B_IOTLB_REG_IVT) != 0);
+
+    //
+    // Enable VTd
+    //
+    MmioWrite32 (mVtdUnitInformation[Index].VtdUnitBaseAddress + R_GCMD_REG, B_GMCD_REG_TE);
+    DEBUG((DEBUG_INFO, "EnableDmar: Waiting B_GSTS_REG_TE ...\n"));
+    do {
+      Reg32 = MmioRead32 (mVtdUnitInformation[Index].VtdUnitBaseAddress + R_GSTS_REG);
+    } while ((Reg32 & B_GSTS_REG_TE) == 0);
+
+    DEBUG ((DEBUG_INFO,"VTD (%d) enabled!<<<<<<\n",Index));
+  }
+
+  mVtdEnabled = TRUE;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Disable DMAR translation.
+
+  @retval EFI_SUCCESS           DMAR translation is disabled.
+  @retval EFI_DEVICE_ERROR      DMAR translation is not disabled.
+**/
+EFI_STATUS
+DisableDmar (
+  VOID
+  )
+{
+  UINTN     Index;
+  UINT32    Reg32;
+
+  AsmWbinvd();
+
+  for (Index = 0; Index < mVtdUnitNumber; Index++) {
+    DEBUG((DEBUG_INFO, ">>>>>>DisableDmar() for engine [%d] \n", Index));
+
+    //
+    // Write Buffer Flush before invalidation
+    //
+    Reg32 = MmioRead32 (mVtdUnitInformation[Index].VtdUnitBaseAddress + R_CAP_REG);
+    if ((Reg32 & B_CAP_REG_RWBF) != 0) {
+      MmioWrite32 (mVtdUnitInformation[Index].VtdUnitBaseAddress + R_GCMD_REG, B_GMCD_REG_WBF);
+    }
+
+    //
+    // Disable VTd
+    //
+    MmioWrite32 (mVtdUnitInformation[Index].VtdUnitBaseAddress + R_GCMD_REG, B_GMCD_REG_SRTP);
+    do {
+      Reg32 = MmioRead32 (mVtdUnitInformation[Index].VtdUnitBaseAddress + R_GSTS_REG);
+    } while((Reg32 & B_GSTS_REG_RTPS) == 0);
+
+    Reg32 = MmioRead32 (mVtdUnitInformation[Index].VtdUnitBaseAddress + R_GSTS_REG);
+    DEBUG((DEBUG_INFO, "DisableDmar: GSTS_REG - 0x%08x\n", Reg32));
+
+    DEBUG ((DEBUG_INFO,"VTD (%d) Disabled!<<<<<<\n",Index));
+  }
+
+  mVtdEnabled = FALSE;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Dump VTd capability registers.
+
+  @param[in]  CapReg              The capability register.
+**/
+VOID
+DumpVtdCapRegs (
+  IN VTD_CAP_REG *CapReg
+  )
+{
+  DEBUG((DEBUG_INFO, "  CapReg:\n", CapReg->Uint64));
+  DEBUG((DEBUG_INFO, "    ND     - 0x%x\n", CapReg->Bits.ND));
+  DEBUG((DEBUG_INFO, "    AFL    - 0x%x\n", CapReg->Bits.AFL));
+  DEBUG((DEBUG_INFO, "    RWBF   - 0x%x\n", CapReg->Bits.RWBF));
+  DEBUG((DEBUG_INFO, "    PLMR   - 0x%x\n", CapReg->Bits.PLMR));
+  DEBUG((DEBUG_INFO, "    PHMR   - 0x%x\n", CapReg->Bits.PHMR));
+  DEBUG((DEBUG_INFO, "    CM     - 0x%x\n", CapReg->Bits.CM));
+  DEBUG((DEBUG_INFO, "    SAGAW  - 0x%x\n", CapReg->Bits.SAGAW));
+  DEBUG((DEBUG_INFO, "    MGAW   - 0x%x\n", CapReg->Bits.MGAW));
+  DEBUG((DEBUG_INFO, "    ZLR    - 0x%x\n", CapReg->Bits.ZLR));
+  DEBUG((DEBUG_INFO, "    FRO    - 0x%x\n", CapReg->Bits.FRO));
+  DEBUG((DEBUG_INFO, "    SLLPS  - 0x%x\n", CapReg->Bits.SLLPS));
+  DEBUG((DEBUG_INFO, "    PSI    - 0x%x\n", CapReg->Bits.PSI));
+  DEBUG((DEBUG_INFO, "    NFR    - 0x%x\n", CapReg->Bits.NFR));
+  DEBUG((DEBUG_INFO, "    MAMV   - 0x%x\n", CapReg->Bits.MAMV));
+  DEBUG((DEBUG_INFO, "    DWD    - 0x%x\n", CapReg->Bits.DWD));
+  DEBUG((DEBUG_INFO, "    DRD    - 0x%x\n", CapReg->Bits.DRD));
+  DEBUG((DEBUG_INFO, "    FL1GP  - 0x%x\n", CapReg->Bits.FL1GP));
+  DEBUG((DEBUG_INFO, "    PI     - 0x%x\n", CapReg->Bits.PI));
+}
+
+/**
+  Dump VTd extended capability registers.
+
+  @param[in]  ECapReg              The extended capability register.
+**/
+VOID
+DumpVtdECapRegs (
+  IN VTD_ECAP_REG *ECapReg
+  )
+{
+  DEBUG((DEBUG_INFO, "  ECapReg:\n", ECapReg->Uint64));
+  DEBUG((DEBUG_INFO, "    C      - 0x%x\n", ECapReg->Bits.C));
+  DEBUG((DEBUG_INFO, "    QI     - 0x%x\n", ECapReg->Bits.QI));
+  DEBUG((DEBUG_INFO, "    DT     - 0x%x\n", ECapReg->Bits.DT));
+  DEBUG((DEBUG_INFO, "    IR     - 0x%x\n", ECapReg->Bits.IR));
+  DEBUG((DEBUG_INFO, "    EIM    - 0x%x\n", ECapReg->Bits.EIM));
+  DEBUG((DEBUG_INFO, "    PT     - 0x%x\n", ECapReg->Bits.PT));
+  DEBUG((DEBUG_INFO, "    SC     - 0x%x\n", ECapReg->Bits.SC));
+  DEBUG((DEBUG_INFO, "    IRO    - 0x%x\n", ECapReg->Bits.IRO));
+  DEBUG((DEBUG_INFO, "    MHMV   - 0x%x\n", ECapReg->Bits.MHMV));
+  DEBUG((DEBUG_INFO, "    ECS    - 0x%x\n", ECapReg->Bits.ECS));
+  DEBUG((DEBUG_INFO, "    MTS    - 0x%x\n", ECapReg->Bits.MTS));
+  DEBUG((DEBUG_INFO, "    NEST   - 0x%x\n", ECapReg->Bits.NEST));
+  DEBUG((DEBUG_INFO, "    DIS    - 0x%x\n", ECapReg->Bits.DIS));
+  DEBUG((DEBUG_INFO, "    PASID  - 0x%x\n", ECapReg->Bits.PASID));
+  DEBUG((DEBUG_INFO, "    PRS    - 0x%x\n", ECapReg->Bits.PRS));
+  DEBUG((DEBUG_INFO, "    ERS    - 0x%x\n", ECapReg->Bits.ERS));
+  DEBUG((DEBUG_INFO, "    SRS    - 0x%x\n", ECapReg->Bits.SRS));
+  DEBUG((DEBUG_INFO, "    NWFS   - 0x%x\n", ECapReg->Bits.NWFS));
+  DEBUG((DEBUG_INFO, "    EAFS   - 0x%x\n", ECapReg->Bits.EAFS));
+  DEBUG((DEBUG_INFO, "    PSS    - 0x%x\n", ECapReg->Bits.PSS));
+}
+
+/**
+  Dump VTd registers.
+
+  @param[in]  VtdIndex              The index of VTd engine.
+**/
+VOID
+DumpVtdRegs (
+  IN UINTN  VtdIndex
+  )
+{
+  UINTN         Index;
+  UINT64        Reg64;
+  VTD_FRCD_REG  FrcdReg;
+  VTD_CAP_REG   CapReg;
+  UINT32        Reg32;
+  VTD_SOURCE_ID SourceId;
+
+  DEBUG((DEBUG_INFO, "#### DumpVtdRegs(%d) Begin ####\n", VtdIndex));
+
+  Reg32 = MmioRead32 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_VER_REG);
+  DEBUG((DEBUG_INFO, "  VER_REG     - 0x%08x\n", Reg32));
+
+  CapReg.Uint64 = MmioRead64 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_CAP_REG);
+  DEBUG((DEBUG_INFO, "  CAP_REG     - 0x%016lx\n", CapReg.Uint64));
+
+  Reg64 = MmioRead64 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_ECAP_REG);
+  DEBUG((DEBUG_INFO, "  ECAP_REG    - 0x%016lx\n", Reg64));
+
+  Reg32 = MmioRead32 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_GSTS_REG);
+  DEBUG((DEBUG_INFO, "  GSTS_REG    - 0x%08x \n", Reg32));
+
+  Reg64 = MmioRead64 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_RTADDR_REG);
+  DEBUG((DEBUG_INFO, "  RTADDR_REG  - 0x%016lx\n", Reg64));
+
+  Reg64 = MmioRead64 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_CCMD_REG);
+  DEBUG((DEBUG_INFO, "  CCMD_REG    - 0x%016lx\n", Reg64));
+
+  Reg32 = MmioRead32 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_FSTS_REG);
+  DEBUG((DEBUG_INFO, "  FSTS_REG    - 0x%08x\n", Reg32));
+
+  Reg32 = MmioRead32 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_FECTL_REG);
+  DEBUG((DEBUG_INFO, "  FECTL_REG   - 0x%08x\n", Reg32));
+
+  Reg32 = MmioRead32 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_FEDATA_REG);
+  DEBUG((DEBUG_INFO, "  FEDATA_REG  - 0x%08x\n", Reg32));
+
+  Reg32 = MmioRead32 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_FEADDR_REG);
+  DEBUG((DEBUG_INFO, "  FEADDR_REG  - 0x%08x\n",Reg32));
+
+  Reg32 = MmioRead32 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_FEUADDR_REG);
+  DEBUG((DEBUG_INFO, "  FEUADDR_REG - 0x%08x\n",Reg32));
+
+  for (Index = 0; Index < (UINTN)CapReg.Bits.NFR + 1; Index++) {
+    FrcdReg.Uint64[0] = MmioRead64 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + ((CapReg.Bits.FRO * 16) + (Index * 16) + R_FRCD_REG));
+    FrcdReg.Uint64[1] = MmioRead64 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + ((CapReg.Bits.FRO * 16) + (Index * 16) + R_FRCD_REG + sizeof(UINT64)));
+    DEBUG((DEBUG_INFO, "  FRCD_REG[%d] - 0x%016lx %016lx\n", Index, FrcdReg.Uint64[1], FrcdReg.Uint64[0]));
+    if (FrcdReg.Uint64[1] != 0 || FrcdReg.Uint64[0] != 0) {
+      DEBUG((DEBUG_INFO, "    Fault Info - 0x%016lx\n", LShiftU64(FrcdReg.Bits.FI, 12)));
+      SourceId.Uint16 = (UINT16)FrcdReg.Bits.SID;
+      DEBUG((DEBUG_INFO, "    Source - B%02x D%02x F%02x\n", SourceId.Bits.Bus, SourceId.Bits.Device, SourceId.Bits.Function));
+      DEBUG((DEBUG_INFO, "    Type - %x (%a)\n", FrcdReg.Bits.T, FrcdReg.Bits.T ? "read" : "write"));
+      DEBUG((DEBUG_INFO, "    Reason - %x\n", FrcdReg.Bits.FR));
+    }
+  }
+
+  Reg64 = MmioRead64 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + (mVtdUnitInformation[VtdIndex].ECapReg.Bits.IRO * 16) + R_IVA_REG);
+  DEBUG((DEBUG_INFO, "  IVA_REG     - 0x%016lx\n",Reg64));
+
+  Reg64 = MmioRead64 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + (mVtdUnitInformation[VtdIndex].ECapReg.Bits.IRO * 16) + R_IOTLB_REG);
+  DEBUG((DEBUG_INFO, "  IOTLB_REG   - 0x%016lx\n",Reg64));
+
+  DEBUG((DEBUG_INFO, "#### DumpVtdRegs(%d) End ####\n", VtdIndex));
+}
+
+/**
+  Dump VTd registers for all VTd engine.
+**/
+VOID
+DumpVtdRegsAll (
+  VOID
+  )
+{
+  UINTN       Num;
+
+  for (Num = 0; Num < mVtdUnitNumber; Num++) {
+    DumpVtdRegs (Num);
+  }
+}
+
+/**
+  Dump VTd registers if there is error.
+**/
+VOID
+DumpVtdIfError (
+  VOID
+  )
+{
+  UINTN         Num;
+  UINTN         Index;
+  VTD_FRCD_REG  FrcdReg;
+  VTD_CAP_REG   CapReg;
+  UINT32        Reg32;
+  BOOLEAN       HasError;
+
+  for (Num = 0; Num < mVtdUnitNumber; Num++) {
+    HasError = FALSE;
+    Reg32 = MmioRead32 (mVtdUnitInformation[Num].VtdUnitBaseAddress + R_FSTS_REG);
+    if (Reg32 != 0) {
+      HasError = TRUE;
+    }
+    Reg32 = MmioRead32 (mVtdUnitInformation[Num].VtdUnitBaseAddress + R_FECTL_REG);
+    if ((Reg32 & BIT30) != 0) {
+      HasError = TRUE;
+    }
+
+    CapReg.Uint64 = MmioRead64 (mVtdUnitInformation[Num].VtdUnitBaseAddress + R_CAP_REG);
+    for (Index = 0; Index < (UINTN)CapReg.Bits.NFR + 1; Index++) {
+      FrcdReg.Uint64[0] = MmioRead64 (mVtdUnitInformation[Num].VtdUnitBaseAddress + ((CapReg.Bits.FRO * 16) + (Index * 16) + R_FRCD_REG));
+      FrcdReg.Uint64[1] = MmioRead64 (mVtdUnitInformation[Num].VtdUnitBaseAddress + ((CapReg.Bits.FRO * 16) + (Index * 16) + R_FRCD_REG + sizeof(UINT64)));
+      if ((FrcdReg.Uint64[0] != 0) || (FrcdReg.Uint64[1] != 0)) {
+        HasError = TRUE;
+      }
+    }
+
+    if (HasError) {
+      DEBUG((DEBUG_INFO, "#### ERROR ####\n"));
+      DumpVtdRegs (Num);
+      DEBUG((DEBUG_INFO, "#### ERROR ####\n"));
+    }
+  }
+}
-- 
2.7.4.windows.1

_______________________________________________
edk2-devel mailing list
edk2-devel@lists.01.org
https://lists.01.org/mailman/listinfo/edk2-devel