public inbox for devel@edk2.groups.io
 help / color / mirror / Atom feed
* [PATCH v3] IntelSiliconPkg/VTd: Support queued invalidation interface
@ 2021-05-26  8:27 Sheng Wei
  0 siblings, 0 replies; only message in thread
From: Sheng Wei @ 2021-05-26  8:27 UTC (permalink / raw)
  To: devel; +Cc: Jenny Huang, Jiewen Yao, Ray Ni, Rangasai V Chaganty

Add queued invalidation interface support for VTd core driver.
For software to invalidate the various caching structures, the architecture
 supports the following two types of invalidation interfaces.
1. Register-based invalidation interface
2. Queued invalidation interface.
BIOS shall check VER_REG to determine if register based invalidation can
 be used. Only for Major Version 6 or lower can support register based
 invalidation. For any version newer than that should use queue
 invalidation interface instead.

REF: https://bugzilla.tianocore.org/show_bug.cgi?id=3366

Signed-off-by: Sheng Wei <w.sheng@intel.com>
Cc: Jenny Huang <jenny.huang@intel.com>
Cc: Jiewen Yao <jiewen.yao@intel.com>
Cc: Ray Ni <ray.ni@intel.com>
Cc: Rangasai V Chaganty <rangasai.v.chaganty@intel.com>
Reviewed-by: Jenny Huang <jenny.huang@intel.com>
---
 .../Feature/VTd/IntelVTdDmarPei/DmarTable.c        |   2 +
 .../Feature/VTd/IntelVTdDmarPei/IntelVTdDmar.c     | 560 +++++++++++++++++----
 .../Feature/VTd/IntelVTdDmarPei/IntelVTdDmarPei.c  |  15 +
 .../Feature/VTd/IntelVTdDmarPei/IntelVTdDmarPei.h  |  19 +
 .../Feature/VTd/IntelVTdDmarPei/TranslationTable.c |   2 -
 .../Feature/VTd/IntelVTdDxe/DmaProtection.h        |  29 ++
 .../Feature/VTd/IntelVTdDxe/VtdReg.c               | 315 +++++++++++-
 .../IntelSiliconPkg/Include/IndustryStandard/Vtd.h |  57 +++
 8 files changed, 876 insertions(+), 123 deletions(-)

diff --git a/Silicon/Intel/IntelSiliconPkg/Feature/VTd/IntelVTdDmarPei/DmarTable.c b/Silicon/Intel/IntelSiliconPkg/Feature/VTd/IntelVTdDmarPei/DmarTable.c
index d188f917..2154690d 100644
--- a/Silicon/Intel/IntelSiliconPkg/Feature/VTd/IntelVTdDmarPei/DmarTable.c
+++ b/Silicon/Intel/IntelSiliconPkg/Feature/VTd/IntelVTdDmarPei/DmarTable.c
@@ -561,6 +561,8 @@ ProcessDhrd (
   DEBUG ((DEBUG_INFO,"  VTD BaseAddress -  0x%016lx\n", DmarDrhd->RegisterBaseAddress));
   VTdUnitInfo->VtdUnitBaseAddress = (UINT32) DmarDrhd->RegisterBaseAddress;
 
+  VTdUnitInfo->EnableQueuedInvalidation = 0;
+
   DEBUG ((DEBUG_INFO,"  VTD Segment - %d\n", DmarDrhd->SegmentNumber));
   VTdUnitInfo->Segment = DmarDrhd->SegmentNumber;
 
diff --git a/Silicon/Intel/IntelSiliconPkg/Feature/VTd/IntelVTdDmarPei/IntelVTdDmar.c b/Silicon/Intel/IntelSiliconPkg/Feature/VTd/IntelVTdDmarPei/IntelVTdDmar.c
index 9ad2a494..c3a939c9 100644
--- a/Silicon/Intel/IntelSiliconPkg/Feature/VTd/IntelVTdDmarPei/IntelVTdDmar.c
+++ b/Silicon/Intel/IntelSiliconPkg/Feature/VTd/IntelVTdDmarPei/IntelVTdDmar.c
@@ -66,30 +66,269 @@ FlushWriteBuffer (
 }
 
 /**
-  Invalidate VTd context cache.
+  Perpare cache invalidation interface.
 
-  @param[in]  VtdUnitBaseAddress        The base address of the VTd engine.
+  @param[in]  VTdUnitInfo       The VTd engine unit information.
+
+  @retval EFI_SUCCESS           The operation was successful.
+  @retval EFI_UNSUPPORTED       Invalidation method is not supported.
+  @retval EFI_OUT_OF_RESOURCES  A memory allocation failed.
 **/
 EFI_STATUS
-InvalidateContextCache (
-  IN UINTN                      VtdUnitBaseAddress
+PerpareCacheInvalidationInterface (
+  IN VTD_UNIT_INFO *VTdUnitInfo
   )
 {
-  UINT64                        Reg64;
+  UINT16         QueueSize;
+  UINT64         Reg64;
+  UINT32         Reg32;
+  VTD_ECAP_REG   ECapReg;
+
+
+  if (VTdUnitInfo->VerReg.Bits.Major <= 6) {
+    VTdUnitInfo->EnableQueuedInvalidation = 0;
+    DEBUG ((DEBUG_INFO, "Use Register-based Invalidation Interface for engine [0x%x]\n", VTdUnitInfo->VtdUnitBaseAddress));
+    return EFI_SUCCESS;
+  }
+
+  ECapReg.Uint64 = MmioRead64 ((UINTN)VTdUnitInfo->VtdUnitBaseAddress + R_ECAP_REG);
+  if (ECapReg.Bits.QI == 0) {
+    DEBUG ((DEBUG_ERROR, "Hardware does not support queued invalidations interface for engine [0x%x]\n", VTdUnitInfo->VtdUnitBaseAddress));
+    return EFI_UNSUPPORTED;
+  }
+
+  VTdUnitInfo->EnableQueuedInvalidation = 1;
+  DEBUG ((DEBUG_INFO, "Use Queued Invalidation Interface for engine [0x%x]\n", VTdUnitInfo->VtdUnitBaseAddress));
+
+  Reg32 = MmioRead32 ((UINTN)VTdUnitInfo->VtdUnitBaseAddress + R_GSTS_REG);
+  if ((Reg32 & B_GSTS_REG_QIES) != 0) {
+    DEBUG ((DEBUG_INFO,"Queued Invalidation Interface was enabled.\n"));
+    Reg32 &= (~B_GSTS_REG_QIES);
+    MmioWrite32 ((UINTN)VTdUnitInfo->VtdUnitBaseAddress + R_GCMD_REG, Reg32);
+    do {
+      Reg32 = MmioRead32 ((UINTN)VTdUnitInfo->VtdUnitBaseAddress + R_GSTS_REG);
+    } while ((Reg32 & B_GSTS_REG_QIES) != 0);
+    MmioWrite64 ((UINTN)VTdUnitInfo->VtdUnitBaseAddress + R_IQA_REG, 0);
+
+    if (VTdUnitInfo->QiDesc != NULL) {
+      FreePages(VTdUnitInfo->QiDesc, EFI_SIZE_TO_PAGES(sizeof(QI_DESC) * VTdUnitInfo->QiDescLength));
+      VTdUnitInfo->QiDesc = NULL;
+      VTdUnitInfo->QiDescLength = 0;
+    }
+  }
+
+  //
+  // Initialize the Invalidation Queue Tail Register to zero.
+  //
+  MmioWrite64 ((UINTN)VTdUnitInfo->VtdUnitBaseAddress + R_IQT_REG, 0);
+
+  //
+  // Setup the IQ address, size and descriptor width through the Invalidation Queue Address Register
+  //
+  QueueSize = 0;
+  VTdUnitInfo->QiDescLength = 1 << (QueueSize + 8);
+  VTdUnitInfo->QiDesc = (QI_DESC *) AllocatePages (EFI_SIZE_TO_PAGES(sizeof(QI_DESC) * VTdUnitInfo->QiDescLength));
+
+  if (VTdUnitInfo->QiDesc == NULL) {
+    VTdUnitInfo->QiDescLength = 0;
+    DEBUG ((DEBUG_ERROR,"Could not Alloc Invalidation Queue Buffer.\n"));
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  DEBUG ((DEBUG_INFO, "Invalidation Queue Length : %d\n", VTdUnitInfo->QiDescLength));
+  Reg64 = (UINT64)(UINTN)VTdUnitInfo->QiDesc;
+  Reg64 |= QueueSize;
+  MmioWrite64 ((UINTN)VTdUnitInfo->VtdUnitBaseAddress + R_IQA_REG, Reg64);
+
+  //
+  // Enable the queued invalidation interface through the Global Command Register.
+  // When enabled, hardware sets the QIES field in the Global Status Register.
+  //
+  Reg32 = MmioRead32 ((UINTN)VTdUnitInfo->VtdUnitBaseAddress + R_GSTS_REG);
+  Reg32 |= B_GMCD_REG_QIE;
+  MmioWrite32 ((UINTN)VTdUnitInfo->VtdUnitBaseAddress + R_GCMD_REG, Reg32);
+  DEBUG ((DEBUG_INFO, "Enable Queued Invalidation Interface. GCMD_REG = 0x%x\n", Reg32));
+  do {
+    Reg32 = MmioRead32 ((UINTN)VTdUnitInfo->VtdUnitBaseAddress + R_GSTS_REG);
+  } while ((Reg32 & B_GSTS_REG_QIES) == 0);
 
-  Reg64 = MmioRead64 (VtdUnitBaseAddress + R_CCMD_REG);
-  if ((Reg64 & B_CCMD_REG_ICC) != 0) {
-    DEBUG ((DEBUG_ERROR,"ERROR: InvalidateContextCache: B_CCMD_REG_ICC is set for VTD(%x)\n",VtdUnitBaseAddress));
-    return EFI_DEVICE_ERROR;
+  VTdUnitInfo->QiFreeHead = 0;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Disable queued invalidation interface.
+
+  @param[in]  VTdUnitInfo       The VTd engine unit information.
+**/
+VOID
+DisableQueuedInvalidationInterface (
+  IN VTD_UNIT_INFO *VTdUnitInfo
+  )
+{
+  UINT32  Reg32;
+
+  if (VTdUnitInfo->EnableQueuedInvalidation != 0) {
+    Reg32 = MmioRead32 ((UINTN)VTdUnitInfo->VtdUnitBaseAddress + R_GSTS_REG);
+    Reg32 &= (~B_GMCD_REG_QIE);
+    MmioWrite32 ((UINTN)VTdUnitInfo->VtdUnitBaseAddress + R_GCMD_REG, Reg32);
+    DEBUG ((DEBUG_INFO, "Disable Queued Invalidation Interface. GCMD_REG = 0x%x\n", Reg32));
+    do {
+      Reg32 = MmioRead32 ((UINTN)VTdUnitInfo->VtdUnitBaseAddress + R_GSTS_REG);
+    } while ((Reg32 & B_GSTS_REG_QIES) != 0);
+
+    if (VTdUnitInfo->QiDesc != NULL) {
+      FreePages(VTdUnitInfo->QiDesc, EFI_SIZE_TO_PAGES(sizeof(QI_DESC) * VTdUnitInfo->QiDescLength));
+      VTdUnitInfo->QiDesc = NULL;
+      VTdUnitInfo->QiDescLength = 0;
+    }
+
+    VTdUnitInfo->EnableQueuedInvalidation = 0;
+  }
+}
+
+/**
+  Check Queued Invalidation Fault.
+
+  @param[in]  VTdUnitInfo       The VTd engine unit information.
+
+  @retval EFI_SUCCESS           The operation was successful.
+  @retval RETURN_DEVICE_ERROR   A fault is detected.
+**/
+EFI_STATUS
+QueuedInvalidationCheckFault (
+  IN VTD_UNIT_INFO *VTdUnitInfo
+  )
+{
+  UINT32     FaultReg;
+
+  FaultReg = MmioRead32 ((UINTN)VTdUnitInfo->VtdUnitBaseAddress + R_FSTS_REG);
+
+  if (FaultReg & B_FSTS_REG_IQE) {
+    DEBUG((DEBUG_ERROR, "Detect Invalidation Queue Error [0x%08x]\n", FaultReg));
+    FaultReg |= B_FSTS_REG_IQE;
+    MmioWrite32 ((UINTN)VTdUnitInfo->VtdUnitBaseAddress + R_FSTS_REG, FaultReg);
+    return RETURN_DEVICE_ERROR;
+  }
+
+  if (FaultReg & B_FSTS_REG_ITE) {
+    DEBUG((DEBUG_ERROR, "Detect Invalidation Time-out Error [0x%08x]\n", FaultReg));
+    FaultReg |= B_FSTS_REG_ITE;
+    MmioWrite32 ((UINTN)VTdUnitInfo->VtdUnitBaseAddress + R_FSTS_REG, FaultReg);
+    return RETURN_DEVICE_ERROR;
+  }
+
+  if (FaultReg & B_FSTS_REG_ICE) {
+    DEBUG((DEBUG_ERROR, "Detect Invalidation Completion Error [0x%08x]\n", FaultReg));
+    FaultReg |= B_FSTS_REG_ICE;
+    MmioWrite32 ((UINTN)VTdUnitInfo->VtdUnitBaseAddress + R_FSTS_REG, FaultReg);
+    return RETURN_DEVICE_ERROR;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Submit the queued invalidation descriptor to the remapping
+   hardware unit and wait for its completion.
+
+  @param[in]  VTdUnitInfo       The VTd engine unit information.
+  @param[in]  Desc              The invalidate descriptor
+
+  @retval EFI_SUCCESS           The operation was successful.
+  @retval RETURN_DEVICE_ERROR   A fault is detected.
+  @retval EFI_INVALID_PARAMETER Parameter is invalid.
+**/
+EFI_STATUS
+SubmitQueuedInvalidationDescriptor (
+  IN VTD_UNIT_INFO *VTdUnitInfo,
+  IN QI_DESC       *Desc
+  )
+{
+  EFI_STATUS Status;
+  UINT16     QiDescLength;
+  QI_DESC    *BaseDesc;
+  UINT64     Reg64Iqt;
+  UINT64     Reg64Iqh;
+
+  if (Desc == NULL) {
+    return EFI_INVALID_PARAMETER;
   }
 
-  Reg64 &= ((~B_CCMD_REG_ICC) & (~B_CCMD_REG_CIRG_MASK));
-  Reg64 |= (B_CCMD_REG_ICC | V_CCMD_REG_CIRG_GLOBAL);
-  MmioWrite64 (VtdUnitBaseAddress + R_CCMD_REG, Reg64);
+  QiDescLength = VTdUnitInfo->QiDescLength;
+  BaseDesc = VTdUnitInfo->QiDesc;
 
+  DEBUG((DEBUG_INFO, "[0x%x] Submit QI Descriptor [0x%08x, 0x%08x]\n", VTdUnitInfo->VtdUnitBaseAddress, Desc->Low, Desc->High));
+
+  BaseDesc[VTdUnitInfo->QiFreeHead].Low = Desc->Low;
+  BaseDesc[VTdUnitInfo->QiFreeHead].High = Desc->High;
+  FlushPageTableMemory(VTdUnitInfo, (UINTN) &BaseDesc[VTdUnitInfo->QiFreeHead], sizeof(QI_DESC));
+
+  DEBUG((DEBUG_INFO,"QI Free Head=0x%x\n", VTdUnitInfo->QiFreeHead));
+  VTdUnitInfo->QiFreeHead = (VTdUnitInfo->QiFreeHead + 1) % QiDescLength;
+
+  Reg64Iqh = MmioRead64 ((UINTN)VTdUnitInfo->VtdUnitBaseAddress + R_IQH_REG);
+  //
+  // Update the HW tail register indicating the presence of new descriptors.
+  //
+  Reg64Iqt = VTdUnitInfo->QiFreeHead << DMAR_IQ_SHIFT;
+  MmioWrite64 ((UINTN)VTdUnitInfo->VtdUnitBaseAddress + R_IQT_REG, Reg64Iqt);
+
+  Status = EFI_SUCCESS;
   do {
-    Reg64 = MmioRead64 (VtdUnitBaseAddress + R_CCMD_REG);
-  } while ((Reg64 & B_CCMD_REG_ICC) != 0);
+    Status = QueuedInvalidationCheckFault(VTdUnitInfo);
+    if (Status != EFI_SUCCESS) {
+      DEBUG((DEBUG_ERROR,"Detect Queued Invalidation Fault.\n"));
+      break;
+    }
+
+    Reg64Iqh = MmioRead64 ((UINTN)VTdUnitInfo->VtdUnitBaseAddress + R_IQH_REG);
+  } while (Reg64Iqt != Reg64Iqh);
+
+  DEBUG((DEBUG_ERROR,"SubmitQueuedInvalidationDescriptor end\n"));
+  return Status;
+}
+
+/**
+  Invalidate VTd context cache.
+
+  @param[in]  VTdUnitInfo       The VTd engine unit information.
+**/
+EFI_STATUS
+InvalidateContextCache (
+  IN VTD_UNIT_INFO              *VTdUnitInfo
+  )
+{
+  UINT64                        Reg64;
+  QI_DESC                       QiDesc;
+
+  if (VTdUnitInfo->EnableQueuedInvalidation == 0) {
+    //
+    // Register-based Invalidation
+    //
+    Reg64 = MmioRead64 ((UINTN)VTdUnitInfo->VtdUnitBaseAddress + R_CCMD_REG);
+    if ((Reg64 & B_CCMD_REG_ICC) != 0) {
+      DEBUG ((DEBUG_ERROR,"ERROR: InvalidateContextCache: B_CCMD_REG_ICC is set for VTD(%x)\n", (UINTN)VTdUnitInfo->VtdUnitBaseAddress));
+      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 ((UINTN)VTdUnitInfo->VtdUnitBaseAddress + R_CCMD_REG, Reg64);
+
+    do {
+      Reg64 = MmioRead64 ((UINTN)VTdUnitInfo->VtdUnitBaseAddress + R_CCMD_REG);
+    } while ((Reg64 & B_CCMD_REG_ICC) != 0);
+  } else {
+    //
+    // Queued Invalidation
+    //
+    QiDesc.Low = QI_CC_FM(0) | QI_CC_SID(0) | QI_CC_DID(0) | QI_CC_GRAN(1) | QI_CC_TYPE;
+    QiDesc.High = 0;
+
+    return SubmitQueuedInvalidationDescriptor(VTdUnitInfo, &QiDesc);
+  }
 
   return EFI_SUCCESS;
 }
@@ -97,31 +336,102 @@ InvalidateContextCache (
 /**
   Invalidate VTd IOTLB.
 
-  @param[in]  VtdUnitBaseAddress        The base address of the VTd engine.
+  @param[in]  VTdUnitInfo       The VTd engine unit information.
 **/
 EFI_STATUS
 InvalidateIOTLB (
-  IN UINTN                      VtdUnitBaseAddress
+  IN VTD_UNIT_INFO              *VTdUnitInfo
   )
 {
   UINT64                        Reg64;
   VTD_ECAP_REG                  ECapReg;
+  QI_DESC                       QiDesc;
+
+  if (VTdUnitInfo->EnableQueuedInvalidation == 0) {
+    //
+    // Register-based Invalidation
+    //
+    ECapReg.Uint64 = MmioRead64 ((UINTN)VTdUnitInfo->VtdUnitBaseAddress + R_ECAP_REG);
+
+    Reg64 = MmioRead64 ((UINTN)VTdUnitInfo->VtdUnitBaseAddress + (ECapReg.Bits.IRO * 16) + R_IOTLB_REG);
+     if ((Reg64 & B_IOTLB_REG_IVT) != 0) {
+       DEBUG ((DEBUG_ERROR, "ERROR: InvalidateIOTLB: B_IOTLB_REG_IVT is set for VTD(%x)\n", (UINTN)VTdUnitInfo->VtdUnitBaseAddress));
+       return EFI_DEVICE_ERROR;
+    }
 
-  ECapReg.Uint64 = MmioRead64 (VtdUnitBaseAddress + R_ECAP_REG);
+    Reg64 &= ((~B_IOTLB_REG_IVT) & (~B_IOTLB_REG_IIRG_MASK));
+    Reg64 |= (B_IOTLB_REG_IVT | V_IOTLB_REG_IIRG_GLOBAL);
+    MmioWrite64 ((UINTN)VTdUnitInfo->VtdUnitBaseAddress + (ECapReg.Bits.IRO * 16) + R_IOTLB_REG, Reg64);
 
-  Reg64 = MmioRead64 (VtdUnitBaseAddress + (ECapReg.Bits.IRO * 16) + R_IOTLB_REG);
-  if ((Reg64 & B_IOTLB_REG_IVT) != 0) {
-    DEBUG ((DEBUG_ERROR, "ERROR: InvalidateIOTLB: B_IOTLB_REG_IVT is set for VTD(%x)\n", VtdUnitBaseAddress));
-    return EFI_DEVICE_ERROR;
+    do {
+      Reg64 = MmioRead64 ((UINTN)VTdUnitInfo->VtdUnitBaseAddress + (ECapReg.Bits.IRO * 16) + R_IOTLB_REG);
+    } while ((Reg64 & B_IOTLB_REG_IVT) != 0);
+  } else {
+    //
+    // Queued Invalidation
+    //
+    ECapReg.Uint64 = MmioRead64 ((UINTN)VTdUnitInfo->VtdUnitBaseAddress + R_ECAP_REG);
+    QiDesc.Low = QI_IOTLB_DID(0) | QI_IOTLB_DR(CAP_READ_DRAIN(ECapReg.Uint64)) | QI_IOTLB_DW(CAP_WRITE_DRAIN(ECapReg.Uint64)) | QI_IOTLB_GRAN(1) | QI_IOTLB_TYPE;
+    QiDesc.High = QI_IOTLB_ADDR(0) | QI_IOTLB_IH(0) | QI_IOTLB_AM(0);
+
+    return SubmitQueuedInvalidationDescriptor(VTdUnitInfo, &QiDesc);
   }
 
-  Reg64 &= ((~B_IOTLB_REG_IVT) & (~B_IOTLB_REG_IIRG_MASK));
-  Reg64 |= (B_IOTLB_REG_IVT | V_IOTLB_REG_IIRG_GLOBAL);
-  MmioWrite64 (VtdUnitBaseAddress + (ECapReg.Bits.IRO * 16) + R_IOTLB_REG, Reg64);
+  return EFI_SUCCESS;
+}
 
+/**
+  Enable DMAR translation inpre-mem phase.
+
+  @param[in]  VtdUnitBaseAddress  The base address of the VTd engine.
+  @param[in]  RootEntryTable      The address of the VTd RootEntryTable.
+
+  @retval EFI_SUCCESS             DMAR translation is enabled.
+  @retval EFI_DEVICE_ERROR        DMAR translation is not enabled.
+**/
+EFI_STATUS
+EnableDmarPreMem (
+  IN UINTN                        VtdUnitBaseAddress,
+  IN UINTN                        RootEntryTable
+  )
+{
+  UINT32                          Reg32;
+
+  DEBUG ((DEBUG_INFO, ">>>>>>EnableDmarPreMem() for engine [%x] \n", VtdUnitBaseAddress));
+
+  DEBUG ((DEBUG_INFO, "RootEntryTable 0x%x \n", RootEntryTable));
+  MmioWrite64 (VtdUnitBaseAddress + R_RTADDR_REG, (UINT64) (UINTN) RootEntryTable);
+
+  Reg32 = MmioRead32 (VtdUnitBaseAddress + R_GSTS_REG);
+  MmioWrite32 (VtdUnitBaseAddress + R_GCMD_REG, Reg32 | B_GMCD_REG_SRTP);
+
+  DEBUG ((DEBUG_INFO, "EnableDmarPreMem: waiting for RTPS bit to be set... \n"));
   do {
-    Reg64 = MmioRead64 (VtdUnitBaseAddress + (ECapReg.Bits.IRO * 16) + R_IOTLB_REG);
-  } while ((Reg64 & B_IOTLB_REG_IVT) != 0);
+    Reg32 = MmioRead32 (VtdUnitBaseAddress + R_GSTS_REG);
+  } while((Reg32 & B_GSTS_REG_RTPS) == 0);
+  DEBUG ((DEBUG_INFO, "EnableDmarPreMem: R_GSTS_REG = 0x%x \n", Reg32));
+
+  //
+  // Init DMAr Fault Event and Data registers
+  //
+  Reg32 = MmioRead32 (VtdUnitBaseAddress + R_FEDATA_REG);
+
+  //
+  // Write Buffer Flush before invalidation
+  //
+  FlushWriteBuffer (VtdUnitBaseAddress);
+
+  //
+  // Enable VTd
+  //
+  Reg32 = MmioRead32 (VtdUnitBaseAddress + R_GSTS_REG);
+  MmioWrite32 (VtdUnitBaseAddress + R_GCMD_REG, Reg32 | B_GMCD_REG_TE);
+  DEBUG ((DEBUG_INFO, "EnableDmarPreMem: Waiting B_GSTS_REG_TE ...\n"));
+  do {
+    Reg32 = MmioRead32 (VtdUnitBaseAddress + R_GSTS_REG);
+  } while ((Reg32 & B_GSTS_REG_TE) == 0);
+
+  DEBUG ((DEBUG_INFO, "VTD () enabled!<<<<<<\n"));
 
   return EFI_SUCCESS;
 }
@@ -129,59 +439,62 @@ InvalidateIOTLB (
 /**
   Enable DMAR translation.
 
-  @param[in]  VtdUnitBaseAddress        The base address of the VTd engine.
-  @param[in]  RootEntryTable            The address of the VTd RootEntryTable.
+  @param[in]  VTdUnitInfo       The VTd engine unit information.
+  @param[in]  RootEntryTable    The address of the VTd RootEntryTable.
 
   @retval EFI_SUCCESS           DMAR translation is enabled.
   @retval EFI_DEVICE_ERROR      DMAR translation is not enabled.
 **/
 EFI_STATUS
 EnableDmar (
-  IN UINTN                      VtdUnitBaseAddress,
+  IN VTD_UNIT_INFO              *VTdUnitInfo,
   IN UINTN                      RootEntryTable
   )
 {
   UINT32                        Reg32;
 
-  DEBUG ((DEBUG_INFO, ">>>>>>EnableDmar() for engine [%x] \n", VtdUnitBaseAddress));
+  DEBUG ((DEBUG_INFO, ">>>>>>EnableDmar() for engine [%x] \n", VTdUnitInfo->VtdUnitBaseAddress));
 
   DEBUG ((DEBUG_INFO, "RootEntryTable 0x%x \n", RootEntryTable));
-  MmioWrite64 (VtdUnitBaseAddress + R_RTADDR_REG, (UINT64) (UINTN) RootEntryTable);
+  MmioWrite64 ((UINTN)VTdUnitInfo->VtdUnitBaseAddress + R_RTADDR_REG, (UINT64) (UINTN) RootEntryTable);
 
-  MmioWrite32 (VtdUnitBaseAddress + R_GCMD_REG, B_GMCD_REG_SRTP);
+  Reg32 = MmioRead32 ((UINTN)VTdUnitInfo->VtdUnitBaseAddress + R_GSTS_REG);
+  MmioWrite32 ((UINTN)VTdUnitInfo->VtdUnitBaseAddress + R_GCMD_REG, Reg32 | B_GMCD_REG_SRTP);
 
   DEBUG ((DEBUG_INFO, "EnableDmar: waiting for RTPS bit to be set... \n"));
   do {
-    Reg32 = MmioRead32 (VtdUnitBaseAddress + R_GSTS_REG);
+    Reg32 = MmioRead32 ((UINTN)VTdUnitInfo->VtdUnitBaseAddress + R_GSTS_REG);
   } while((Reg32 & B_GSTS_REG_RTPS) == 0);
+  DEBUG ((DEBUG_INFO, "EnableDmar: R_GSTS_REG = 0x%x \n", Reg32));
 
   //
   // Init DMAr Fault Event and Data registers
   //
-  Reg32 = MmioRead32 (VtdUnitBaseAddress + R_FEDATA_REG);
+  Reg32 = MmioRead32 ((UINTN)VTdUnitInfo->VtdUnitBaseAddress + R_FEDATA_REG);
 
   //
   // Write Buffer Flush before invalidation
   //
-  FlushWriteBuffer (VtdUnitBaseAddress);
+  FlushWriteBuffer ((UINTN)VTdUnitInfo->VtdUnitBaseAddress);
 
   //
   // Invalidate the context cache
   //
-  InvalidateContextCache (VtdUnitBaseAddress);
+  InvalidateContextCache (VTdUnitInfo);
 
   //
   // Invalidate the IOTLB cache
   //
-  InvalidateIOTLB (VtdUnitBaseAddress);
+  InvalidateIOTLB (VTdUnitInfo);
 
   //
   // Enable VTd
   //
-  MmioWrite32 (VtdUnitBaseAddress + R_GCMD_REG, B_GMCD_REG_TE);
+  Reg32 = MmioRead32 ((UINTN)VTdUnitInfo->VtdUnitBaseAddress + R_GSTS_REG);
+  MmioWrite32 ((UINTN)VTdUnitInfo->VtdUnitBaseAddress + R_GCMD_REG, Reg32 | B_GMCD_REG_TE);
   DEBUG ((DEBUG_INFO, "EnableDmar: Waiting B_GSTS_REG_TE ...\n"));
   do {
-    Reg32 = MmioRead32 (VtdUnitBaseAddress + R_GSTS_REG);
+    Reg32 = MmioRead32 ((UINTN)VTdUnitInfo->VtdUnitBaseAddress + R_GSTS_REG);
   } while ((Reg32 & B_GSTS_REG_TE) == 0);
 
   DEBUG ((DEBUG_INFO, "VTD () enabled!<<<<<<\n"));
@@ -252,6 +565,86 @@ DisableDmar (
   return EFI_SUCCESS;
 }
 
+/**
+  Dump VTd version registers.
+
+  @param[in]  VerReg            The version register.
+**/
+VOID
+DumpVtdVerRegs (
+  IN VTD_VER_REG                *VerReg
+  )
+{
+  DEBUG ((DEBUG_INFO, "  VerReg:\n", VerReg->Uint32));
+  DEBUG ((DEBUG_INFO, "    Major    - 0x%x\n", VerReg->Bits.Major));
+  DEBUG ((DEBUG_INFO, "    Minor    - 0x%x\n", VerReg->Bits.Minor));
+}
+
+/**
+  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));
+}
+
+
 /**
   Enable VTd translation table protection for all.
 
@@ -286,7 +679,15 @@ EnableVTdTranslationProtectionAll (
     if ((EngineMask & LShiftU64(1, Index)) == 0) {
       continue;
     }
-    EnableDmar ((UINTN) VTdInfo->VtdUnitInfo[Index].VtdUnitBaseAddress, (UINTN) *RootEntryTable);
+
+    VTdInfo->VtdUnitInfo[Index].VerReg.Uint32 = MmioRead32 (VTdInfo->VtdUnitInfo[Index].VtdUnitBaseAddress + R_VER_REG);
+    DumpVtdVerRegs (&VTdInfo->VtdUnitInfo[Index].VerReg);
+    VTdInfo->VtdUnitInfo[Index].CapReg.Uint64 = MmioRead64 (VTdInfo->VtdUnitInfo[Index].VtdUnitBaseAddress + R_CAP_REG);
+    DumpVtdCapRegs (&VTdInfo->VtdUnitInfo[Index].CapReg);
+    VTdInfo->VtdUnitInfo[Index].ECapReg.Uint64 = MmioRead64 (VTdInfo->VtdUnitInfo[Index].VtdUnitBaseAddress + R_ECAP_REG);
+    DumpVtdECapRegs (&VTdInfo->VtdUnitInfo[Index].ECapReg);
+
+    EnableDmarPreMem (VTdInfo->VtdUnitInfo[Index].VtdUnitBaseAddress, (UINTN) *RootEntryTable);
   }
 
   return;
@@ -311,10 +712,10 @@ EnableVTdTranslationProtection (
   for (VtdIndex = 0; VtdIndex < VTdInfo->VTdEngineCount; VtdIndex++) {
     if (VTdInfo->VtdUnitInfo[VtdIndex].ExtRootEntryTable != 0) {
       DEBUG ((DEBUG_INFO, "EnableVtdDmar (%d) ExtRootEntryTable 0x%x\n", VtdIndex, VTdInfo->VtdUnitInfo[VtdIndex].ExtRootEntryTable));
-      Status = EnableDmar (VTdInfo->VtdUnitInfo[VtdIndex].VtdUnitBaseAddress, VTdInfo->VtdUnitInfo[VtdIndex].ExtRootEntryTable);
+      Status = EnableDmar (&VTdInfo->VtdUnitInfo[VtdIndex], VTdInfo->VtdUnitInfo[VtdIndex].ExtRootEntryTable);
     } else {
       DEBUG ((DEBUG_INFO, "EnableVtdDmar (%d) RootEntryTable 0x%x\n", VtdIndex, VTdInfo->VtdUnitInfo[VtdIndex].RootEntryTable));
-      Status = EnableDmar (VTdInfo->VtdUnitInfo[VtdIndex].VtdUnitBaseAddress, VTdInfo->VtdUnitInfo[VtdIndex].RootEntryTable);
+      Status = EnableDmar (&VTdInfo->VtdUnitInfo[VtdIndex], VTdInfo->VtdUnitInfo[VtdIndex].RootEntryTable);
     }
     if (EFI_ERROR (Status)) {
       DEBUG ((DEBUG_ERROR, "EnableVtdDmar (%d) Failed !\n", VtdIndex));
@@ -345,73 +746,36 @@ DisableVTdTranslationProtection (
       continue;
     }
     DisableDmar ((UINTN) VTdInfo->VtdUnitInfo[Index].VtdUnitBaseAddress);
+
+    DisableQueuedInvalidationInterface(&VTdInfo->VtdUnitInfo[Index]);
   }
 
   return;
 }
 
 /**
-  Dump VTd capability registers.
+  Prepare VTD cache invalidation configuration.
 
-  @param[in]  CapReg            The capability register.
+  @param[in]  VTdInfo           The VTd engine context information.
+
+  @retval EFI_SUCCESS           Prepare Vtd config success
 **/
-VOID
-DumpVtdCapRegs (
-  IN VTD_CAP_REG                *CapReg
+EFI_STATUS
+PrepareVtdCacheInvalidationConfig (
+  IN VTD_INFO                   *VTdInfo
   )
 {
-  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));
-}
+  UINTN                         Index;
+  EFI_STATUS                    Status;
 
-/**
-  Dump VTd extended capability registers.
+  for (Index = 0; Index < VTdInfo->VTdEngineCount; Index++) {
+    Status = PerpareCacheInvalidationInterface(&VTdInfo->VtdUnitInfo[Index]);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
 
-  @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));
+  return EFI_SUCCESS;
 }
 
 /**
@@ -431,6 +795,8 @@ PrepareVtdConfig (
 
   for (Index = 0; Index < VTdInfo->VTdEngineCount; Index++) {
     DEBUG ((DEBUG_ERROR, "Dump VTd Capability (%d)\n", Index));
+    VTdInfo->VtdUnitInfo[Index].VerReg.Uint32 = MmioRead32 (VTdInfo->VtdUnitInfo[Index].VtdUnitBaseAddress + R_VER_REG);
+    DumpVtdVerRegs (&VTdInfo->VtdUnitInfo[Index].VerReg);
     VTdInfo->VtdUnitInfo[Index].CapReg.Uint64 = MmioRead64 (VTdInfo->VtdUnitInfo[Index].VtdUnitBaseAddress + R_CAP_REG);
     DumpVtdCapRegs (&VTdInfo->VtdUnitInfo[Index].CapReg);
     VTdInfo->VtdUnitInfo[Index].ECapReg.Uint64 = MmioRead64 (VTdInfo->VtdUnitInfo[Index].VtdUnitBaseAddress + R_ECAP_REG);
diff --git a/Silicon/Intel/IntelSiliconPkg/Feature/VTd/IntelVTdDmarPei/IntelVTdDmarPei.c b/Silicon/Intel/IntelSiliconPkg/Feature/VTd/IntelVTdDmarPei/IntelVTdDmarPei.c
index f3c4a2bc..a8f7bfee 100644
--- a/Silicon/Intel/IntelSiliconPkg/Feature/VTd/IntelVTdDmarPei/IntelVTdDmarPei.c
+++ b/Silicon/Intel/IntelSiliconPkg/Feature/VTd/IntelVTdDmarPei/IntelVTdDmarPei.c
@@ -482,6 +482,7 @@ InitVTdDmarForAll (
   VOID                          *Hob;
   VTD_INFO                      *VTdInfo;
   UINT64                        EngineMask;
+  EFI_STATUS                    Status;
 
   Hob = GetFirstGuidHob (&mVTdInfoGuid);
   if (Hob == NULL) {
@@ -491,6 +492,13 @@ InitVTdDmarForAll (
   VTdInfo = GET_GUID_HOB_DATA (Hob);
   EngineMask = LShiftU64 (1, VTdInfo->VTdEngineCount) - 1;
 
+  DEBUG ((DEBUG_INFO, "PrepareVtdConfig\n"));
+  Status = PrepareVtdConfig (VTdInfo);
+  if (EFI_ERROR (Status)) {
+    ASSERT_EFI_ERROR (Status);
+    return Status;
+  }
+
   EnableVTdTranslationProtectionAll (VTdInfo, EngineMask);
 
   return EFI_SUCCESS;
@@ -596,6 +604,13 @@ InitVTdDmarForDma (
     return Status;
   }
 
+  DEBUG ((DEBUG_INFO, "PrepareVtdCacheInvalidationConfig\n"));
+  Status = PrepareVtdCacheInvalidationConfig (VTdInfo);
+  if (EFI_ERROR (Status)) {
+    ASSERT_EFI_ERROR (Status);
+    return Status;
+  }
+
   // create root entry table
   DEBUG ((DEBUG_INFO, "SetupTranslationTable\n"));
   Status = SetupTranslationTable (VTdInfo);
diff --git a/Silicon/Intel/IntelSiliconPkg/Feature/VTd/IntelVTdDmarPei/IntelVTdDmarPei.h b/Silicon/Intel/IntelSiliconPkg/Feature/VTd/IntelVTdDmarPei/IntelVTdDmarPei.h
index a3bb8827..e23a6c8e 100644
--- a/Silicon/Intel/IntelSiliconPkg/Feature/VTd/IntelVTdDmarPei/IntelVTdDmarPei.h
+++ b/Silicon/Intel/IntelSiliconPkg/Feature/VTd/IntelVTdDmarPei/IntelVTdDmarPei.h
@@ -11,6 +11,8 @@
 
 #define MAX_VTD_PCI_DATA_NUMBER             0x100
 
+#define VTD_64BITS_ADDRESS(Lo, Hi) (LShiftU64 (Lo, 12) | LShiftU64 (Hi, 32))
+
 typedef struct {
   UINT8                            DeviceType;
   VTD_SOURCE_ID                    PciSourceId;
@@ -27,6 +29,7 @@ typedef struct {
 typedef struct {
   UINT32                           VtdUnitBaseAddress;
   UINT16                           Segment;
+  VTD_VER_REG                      VerReg;
   VTD_CAP_REG                      CapReg;
   VTD_ECAP_REG                     ECapReg;
   BOOLEAN                          Is5LevelPaging;
@@ -37,6 +40,10 @@ typedef struct {
   UINT16                           RootEntryTablePageSize;
   UINT16                           ExtRootEntryTablePageSize;
   PEI_PCI_DEVICE_INFORMATION       PciDeviceInfo;
+  UINT8                            EnableQueuedInvalidation;
+  UINT16                           QiDescLength;
+  QI_DESC                          *QiDesc;
+  UINT16                           QiFreeHead;
 } VTD_UNIT_INFO;
 
 typedef struct {
@@ -123,6 +130,18 @@ DumpAcpiDMAR (
   IN EFI_ACPI_DMAR_HEADER       *Dmar
   );
 
+/**
+  Prepare VTD cache invalidation configuration.
+
+  @param[in]  VTdInfo           The VTd engine context information.
+
+  @retval EFI_SUCCESS           Prepare Vtd config success
+**/
+EFI_STATUS
+PrepareVtdCacheInvalidationConfig (
+  IN VTD_INFO                   *VTdInfo
+  );
+
 /**
   Prepare VTD configuration.
 
diff --git a/Silicon/Intel/IntelSiliconPkg/Feature/VTd/IntelVTdDmarPei/TranslationTable.c b/Silicon/Intel/IntelSiliconPkg/Feature/VTd/IntelVTdDmarPei/TranslationTable.c
index d417f5af..341e2beb 100644
--- a/Silicon/Intel/IntelSiliconPkg/Feature/VTd/IntelVTdDmarPei/TranslationTable.c
+++ b/Silicon/Intel/IntelSiliconPkg/Feature/VTd/IntelVTdDmarPei/TranslationTable.c
@@ -26,8 +26,6 @@
 #define ALIGN_VALUE_UP(Value, Alignment)  (((Value) + (Alignment) - 1) & (~((Alignment) - 1)))
 #define ALIGN_VALUE_LOW(Value, Alignment) ((Value) & (~((Alignment) - 1)))
 
-#define VTD_64BITS_ADDRESS(Lo, Hi) (LShiftU64 (Lo, 12) | LShiftU64 (Hi, 32))
-
 /**
   Allocate zero pages.
 
diff --git a/Silicon/Intel/IntelSiliconPkg/Feature/VTd/IntelVTdDxe/DmaProtection.h b/Silicon/Intel/IntelSiliconPkg/Feature/VTd/IntelVTdDxe/DmaProtection.h
index f641cea0..a24fbc37 100644
--- a/Silicon/Intel/IntelSiliconPkg/Feature/VTd/IntelVTdDxe/DmaProtection.h
+++ b/Silicon/Intel/IntelSiliconPkg/Feature/VTd/IntelVTdDxe/DmaProtection.h
@@ -69,6 +69,7 @@ typedef struct {
 typedef struct {
   UINTN                            VtdUnitBaseAddress;
   UINT16                           Segment;
+  VTD_VER_REG                      VerReg;
   VTD_CAP_REG                      CapReg;
   VTD_ECAP_REG                     ECapReg;
   VTD_ROOT_ENTRY                   *RootEntryTable;
@@ -78,6 +79,10 @@ typedef struct {
   BOOLEAN                          HasDirtyPages;
   PCI_DEVICE_INFORMATION           PciDeviceInfo;
   BOOLEAN                          Is5LevelPaging;
+  UINT8                            EnableQueuedInvalidation;
+  UINT16                           QiDescLength;
+  QI_DESC                          *QiDesc;
+  UINT16                           QiFreeHead;
 } VTD_UNIT_INFORMATION;
 
 //
@@ -179,6 +184,20 @@ FlushWriteBuffer (
   IN UINTN  VtdIndex
   );
 
+/**
+  Perpare cache invalidation interface.
+
+  @param[in]  VtdIndex          The index used to identify a VTd engine.
+
+  @retval EFI_SUCCESS           The operation was successful.
+  @retval EFI_UNSUPPORTED       Invalidation method is not supported.
+  @retval EFI_OUT_OF_RESOURCES  A memory allocation failed.
+**/
+EFI_STATUS
+PerpareCacheInvalidationInterface (
+  IN UINTN  VtdIndex
+  );
+
 /**
   Invalidate VTd context cache.
 
@@ -230,6 +249,16 @@ DumpVtdRegsAll (
   VOID
   );
 
+/**
+  Dump VTd version registers.
+
+  @param[in]  VerReg            The version register.
+**/
+VOID
+DumpVtdVerRegs (
+  IN VTD_VER_REG                *VerReg
+  );
+
 /**
   Dump VTd capability registers.
 
diff --git a/Silicon/Intel/IntelSiliconPkg/Feature/VTd/IntelVTdDxe/VtdReg.c b/Silicon/Intel/IntelSiliconPkg/Feature/VTd/IntelVTdDxe/VtdReg.c
index 686d235f..1ce9c1c0 100644
--- a/Silicon/Intel/IntelSiliconPkg/Feature/VTd/IntelVTdDxe/VtdReg.c
+++ b/Silicon/Intel/IntelSiliconPkg/Feature/VTd/IntelVTdDxe/VtdReg.c
@@ -55,59 +55,298 @@ FlushWriteBuffer (
 }
 
 /**
-  Invalidate VTd context cache.
+  Perpare cache invalidation interface.
 
   @param[in]  VtdIndex          The index used to identify a VTd engine.
+
+  @retval EFI_SUCCESS           The operation was successful.
+  @retval EFI_UNSUPPORTED       Invalidation method is not supported.
+  @retval EFI_OUT_OF_RESOURCES  A memory allocation failed.
 **/
 EFI_STATUS
-InvalidateContextCache (
+PerpareCacheInvalidationInterface (
   IN UINTN  VtdIndex
   )
 {
+  UINT16  QueueSize;
   UINT64  Reg64;
+  UINT32  Reg32;
 
-  Reg64 = MmioRead64 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_CCMD_REG);
-  if ((Reg64 & B_CCMD_REG_ICC) != 0) {
-    DEBUG ((DEBUG_ERROR,"ERROR: InvalidateContextCache: B_CCMD_REG_ICC is set for VTD(%d)\n",VtdIndex));
-    return EFI_DEVICE_ERROR;
+  if (mVtdUnitInformation[VtdIndex].VerReg.Bits.Major <= 6) {
+    mVtdUnitInformation[VtdIndex].EnableQueuedInvalidation = 0;
+    DEBUG ((DEBUG_INFO, "Use Register-based Invalidation Interface for engine [%d]\n", VtdIndex));
+    return EFI_SUCCESS;
+  }
+
+  if (mVtdUnitInformation[VtdIndex].ECapReg.Bits.QI == 0) {
+    DEBUG ((DEBUG_ERROR, "Hardware does not support queued invalidations interface for engine [%d]\n", VtdIndex));
+    return EFI_UNSUPPORTED;
+  }
+
+  mVtdUnitInformation[VtdIndex].EnableQueuedInvalidation = 1;
+  DEBUG ((DEBUG_INFO, "Use Queued Invalidation Interface for engine [%d]\n", VtdIndex));
+
+  Reg32 = MmioRead32 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_GSTS_REG);
+  if ((Reg32 & B_GSTS_REG_QIES) != 0) {
+    DEBUG ((DEBUG_ERROR,"Queued Invalidation Interface was enabled.\n"));
+    Reg32 &= (~B_GSTS_REG_QIES);
+    MmioWrite32 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_GCMD_REG, Reg32);
+    do {
+      Reg32 = MmioRead32 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_GSTS_REG);
+    } while ((Reg32 & B_GSTS_REG_QIES) != 0);
   }
 
-  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);
+  //
+  // Initialize the Invalidation Queue Tail Register to zero.
+  //
+  MmioWrite64 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_IQT_REG, 0);
 
+  //
+  // Setup the IQ address, size and descriptor width through the Invalidation Queue Address Register
+  //
+  QueueSize = 0;
+  mVtdUnitInformation[VtdIndex].QiDescLength = 1 << (QueueSize + 8);
+  mVtdUnitInformation[VtdIndex].QiDesc = (QI_DESC *) AllocatePages (EFI_SIZE_TO_PAGES(sizeof(QI_DESC) * mVtdUnitInformation[VtdIndex].QiDescLength));
+
+  if (mVtdUnitInformation[VtdIndex].QiDesc == NULL) {
+    mVtdUnitInformation[VtdIndex].QiDescLength = 0;
+    DEBUG ((DEBUG_ERROR,"Could not Alloc Invalidation Queue Buffer.\n"));
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  DEBUG ((DEBUG_INFO, "Invalidation Queue Length : %d\n", mVtdUnitInformation[VtdIndex].QiDescLength));
+  Reg64 = (UINT64)(UINTN)mVtdUnitInformation[VtdIndex].QiDesc;
+  Reg64 |= QueueSize;
+  MmioWrite64 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_IQA_REG, Reg64);
+
+  //
+  // Enable the queued invalidation interface through the Global Command Register.
+  // When enabled, hardware sets the QIES field in the Global Status Register.
+  //
+  Reg32 = MmioRead32 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_GSTS_REG);
+  Reg32 |= B_GMCD_REG_QIE;
+  MmioWrite32 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_GCMD_REG, Reg32);
+  DEBUG ((DEBUG_INFO, "Enable Queued Invalidation Interface. GCMD_REG = 0x%x\n", Reg32));
   do {
-    Reg64 = MmioRead64 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_CCMD_REG);
-  } while ((Reg64 & B_CCMD_REG_ICC) != 0);
+    Reg32 = MmioRead32 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_GSTS_REG);
+  } while ((Reg32 & B_GSTS_REG_QIES) == 0);
+
+  mVtdUnitInformation[VtdIndex].QiFreeHead = 0;
 
   return EFI_SUCCESS;
 }
 
 /**
-  Invalidate VTd IOTLB.
+  Disable queued invalidation interface.
+
+  @param[in]  VtdIndex          The index used to identify a VTd engine.
+**/
+VOID
+DisableQueuedInvalidationInterface (
+  IN UINTN  VtdIndex
+  )
+{
+  UINT32  Reg32;
+
+  if (mVtdUnitInformation[VtdIndex].EnableQueuedInvalidation != 0) {
+    Reg32 = MmioRead32 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_GSTS_REG);
+    Reg32 &= (~B_GMCD_REG_QIE);
+    MmioWrite32 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_GCMD_REG, Reg32);
+    DEBUG ((DEBUG_INFO, "Disable Queued Invalidation Interface. GCMD_REG = 0x%x\n", Reg32));
+    do {
+      Reg32 = MmioRead32 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_GSTS_REG);
+    } while ((Reg32 & B_GSTS_REG_QIES) != 0);
+
+    if (mVtdUnitInformation[VtdIndex].QiDesc != NULL) {
+      FreePages(mVtdUnitInformation[VtdIndex].QiDesc, EFI_SIZE_TO_PAGES(sizeof(QI_DESC) * mVtdUnitInformation[VtdIndex].QiDescLength));
+      mVtdUnitInformation[VtdIndex].QiDesc = NULL;
+      mVtdUnitInformation[VtdIndex].QiDescLength = 0;
+    }
+
+    mVtdUnitInformation[VtdIndex].EnableQueuedInvalidation = 0;
+  }
+}
+
+/**
+  Check Queued Invalidation Fault.
 
   @param[in]  VtdIndex          The index used to identify a VTd engine.
+
+  @retval EFI_SUCCESS           The operation was successful.
+  @retval RETURN_DEVICE_ERROR   A fault is detected.
 **/
 EFI_STATUS
-InvalidateIOTLB (
+QueuedInvalidationCheckFault (
   IN UINTN  VtdIndex
   )
 {
-  UINT64  Reg64;
+  UINT32     FaultReg;
 
-  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: InvalidateIOTLB: B_IOTLB_REG_IVT is set for VTD(%d)\n", VtdIndex));
-    return EFI_DEVICE_ERROR;
+  FaultReg = MmioRead32 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_FSTS_REG);
+
+  if (FaultReg & B_FSTS_REG_IQE) {
+    DEBUG((DEBUG_ERROR, "Detect Invalidation Queue Error [0x%08x]\n", FaultReg));
+    FaultReg |= B_FSTS_REG_IQE;
+    MmioWrite32 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_FSTS_REG, FaultReg);
+    return RETURN_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);
+  if (FaultReg & B_FSTS_REG_ITE) {
+    DEBUG((DEBUG_ERROR, "Detect Invalidation Time-out Error [0x%08x]\n", FaultReg));
+    FaultReg |= B_FSTS_REG_ITE;
+    MmioWrite32 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_FSTS_REG, FaultReg);
+    return RETURN_DEVICE_ERROR;
+  }
+
+  if (FaultReg & B_FSTS_REG_ICE) {
+    DEBUG((DEBUG_ERROR, "Detect Invalidation Completion Error [0x%08x]\n", FaultReg));
+    FaultReg |= B_FSTS_REG_ICE;
+    MmioWrite32 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_FSTS_REG, FaultReg);
+    return RETURN_DEVICE_ERROR;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Submit the queued invalidation descriptor to the remapping
+   hardware unit and wait for its completion.
+
+  @param[in]  VtdIndex          The index used to identify a VTd engine.
+  @param[in]  Desc              The invalidate descriptor
+
+  @retval EFI_SUCCESS           The operation was successful.
+  @retval RETURN_DEVICE_ERROR   A fault is detected.
+  @retval EFI_INVALID_PARAMETER Parameter is invalid.
+**/
+EFI_STATUS
+SubmitQueuedInvalidationDescriptor (
+  IN UINTN    VtdIndex,
+  IN QI_DESC  *Desc
+  )
+{
+  EFI_STATUS Status;
+  UINT16     QiDescLength;
+  QI_DESC    *BaseDesc;
+  UINT64     Reg64Iqt;
+  UINT64     Reg64Iqh;
+
+  if (Desc == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  QiDescLength = mVtdUnitInformation[VtdIndex].QiDescLength;
+  BaseDesc = mVtdUnitInformation[VtdIndex].QiDesc;
+
+  DEBUG((DEBUG_INFO, "[%d] Submit QI Descriptor [0x%08x, 0x%08x] Free Head (%d)\n", VtdIndex, Desc->Low, Desc->High, mVtdUnitInformation[VtdIndex].QiFreeHead));
 
+  BaseDesc[mVtdUnitInformation[VtdIndex].QiFreeHead].Low = Desc->Low;
+  BaseDesc[mVtdUnitInformation[VtdIndex].QiFreeHead].High = Desc->High;
+  FlushPageTableMemory(VtdIndex, (UINTN) &BaseDesc[mVtdUnitInformation[VtdIndex].QiFreeHead], sizeof(QI_DESC));
+
+  mVtdUnitInformation[VtdIndex].QiFreeHead = (mVtdUnitInformation[VtdIndex].QiFreeHead + 1) % QiDescLength;
+
+  //
+  // Update the HW tail register indicating the presence of new descriptors.
+  //
+  Reg64Iqt = mVtdUnitInformation[VtdIndex].QiFreeHead << DMAR_IQ_SHIFT;
+  MmioWrite64 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_IQT_REG, Reg64Iqt);
+
+  Status = EFI_SUCCESS;
   do {
+    Status = QueuedInvalidationCheckFault(VtdIndex);
+    if (Status != EFI_SUCCESS) {
+      DEBUG((DEBUG_ERROR,"Detect Queued Invalidation Fault.\n"));
+      break;
+    }
+
+    Reg64Iqh = MmioRead64 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_IQH_REG);
+  } while (Reg64Iqt != Reg64Iqh);
+
+  return Status;
+}
+
+/**
+  Invalidate VTd context cache.
+
+  @param[in]  VtdIndex          The index used to identify a VTd engine.
+**/
+EFI_STATUS
+InvalidateContextCache (
+  IN UINTN  VtdIndex
+  )
+{
+  UINT64     Reg64;
+  QI_DESC    QiDesc;
+
+  if (mVtdUnitInformation[VtdIndex].EnableQueuedInvalidation == 0) {
+    //
+    // Register-based Invalidation
+    //
+    Reg64 = MmioRead64 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_CCMD_REG);
+    if ((Reg64 & B_CCMD_REG_ICC) != 0) {
+      DEBUG ((DEBUG_ERROR,"ERROR: InvalidateContextCache: 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);
+  } else {
+    //
+    // Queued Invalidation
+    //
+    QiDesc.Low = QI_CC_FM(0) | QI_CC_SID(0) | QI_CC_DID(0) | QI_CC_GRAN(1) | QI_CC_TYPE;
+    QiDesc.High = 0;
+
+    return SubmitQueuedInvalidationDescriptor(VtdIndex, &QiDesc);
+  }
+  return EFI_SUCCESS;
+}
+
+/**
+  Invalidate VTd IOTLB.
+
+  @param[in]  VtdIndex          The index used to identify a VTd engine.
+**/
+EFI_STATUS
+InvalidateIOTLB (
+  IN UINTN  VtdIndex
+  )
+{
+  UINT64     Reg64;
+  QI_DESC    QiDesc;
+
+  if (mVtdUnitInformation[VtdIndex].EnableQueuedInvalidation == 0) {
+    //
+    // Register-based Invalidation
+    //
     Reg64 = MmioRead64 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + (mVtdUnitInformation[VtdIndex].ECapReg.Bits.IRO * 16) + R_IOTLB_REG);
-  } while ((Reg64 & B_IOTLB_REG_IVT) != 0);
+    if ((Reg64 & B_IOTLB_REG_IVT) != 0) {
+      DEBUG ((DEBUG_ERROR,"ERROR: InvalidateIOTLB: 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);
+  } else {
+    //
+    // Queued Invalidation
+    //
+    QiDesc.Low = QI_IOTLB_DID(0) | QI_IOTLB_DR(CAP_READ_DRAIN(mVtdUnitInformation[VtdIndex].CapReg.Uint64)) | QI_IOTLB_DW(CAP_WRITE_DRAIN(mVtdUnitInformation[VtdIndex].CapReg.Uint64)) | QI_IOTLB_GRAN(1) | QI_IOTLB_TYPE;
+    QiDesc.High = QI_IOTLB_ADDR(0) | QI_IOTLB_IH(0) | QI_IOTLB_AM(0);
+
+    return SubmitQueuedInvalidationDescriptor(VtdIndex, &QiDesc);
+  }
 
   return EFI_SUCCESS;
 }
@@ -163,9 +402,12 @@ PrepareVtdConfig (
 {
   UINTN         Index;
   UINTN         DomainNumber;
+  EFI_STATUS    Status;
 
   for (Index = 0; Index < mVtdUnitNumber; Index++) {
     DEBUG ((DEBUG_INFO, "Dump VTd Capability (%d)\n", Index));
+    mVtdUnitInformation[Index].VerReg.Uint32 = MmioRead32 (mVtdUnitInformation[Index].VtdUnitBaseAddress + R_VER_REG);
+    DumpVtdVerRegs (&mVtdUnitInformation[Index].VerReg);
     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);
@@ -190,6 +432,12 @@ PrepareVtdConfig (
       DEBUG((DEBUG_ERROR, "!!!! Pci device Number(0x%x) >= DomainNumber(0x%x) !!!!\n", mVtdUnitInformation[Index].PciDeviceInfo.PciDeviceDataNumber, DomainNumber));
       return ;
     }
+
+    Status = PerpareCacheInvalidationInterface(Index);
+    if (EFI_ERROR (Status)) {
+      ASSERT(FALSE);
+      return;
+    }
   }
   return ;
 }
@@ -252,7 +500,8 @@ EnableDmar (
       MmioWrite64 (mVtdUnitInformation[Index].VtdUnitBaseAddress + R_RTADDR_REG, (UINT64)(UINTN)mVtdUnitInformation[Index].RootEntryTable);
     }
 
-    MmioWrite32 (mVtdUnitInformation[Index].VtdUnitBaseAddress + R_GCMD_REG, B_GMCD_REG_SRTP);
+    Reg32 = MmioRead32 (mVtdUnitInformation[Index].VtdUnitBaseAddress + R_GSTS_REG);
+    MmioWrite32 (mVtdUnitInformation[Index].VtdUnitBaseAddress + R_GCMD_REG, Reg32 | B_GMCD_REG_SRTP);
 
     DEBUG((DEBUG_INFO, "EnableDmar: waiting for RTPS bit to be set... \n"));
     do {
@@ -282,7 +531,8 @@ EnableDmar (
     //
     // Enable VTd
     //
-    MmioWrite32 (mVtdUnitInformation[Index].VtdUnitBaseAddress + R_GCMD_REG, B_GMCD_REG_TE);
+    Reg32 = MmioRead32 (mVtdUnitInformation[Index].VtdUnitBaseAddress + R_GSTS_REG);
+    MmioWrite32 (mVtdUnitInformation[Index].VtdUnitBaseAddress + R_GCMD_REG, Reg32 | B_GMCD_REG_TE);
     DEBUG((DEBUG_INFO, "EnableDmar: Waiting B_GSTS_REG_TE ...\n"));
     do {
       Reg32 = MmioRead32 (mVtdUnitInformation[Index].VtdUnitBaseAddress + R_GSTS_REG);
@@ -360,6 +610,8 @@ DisableDmar (
     DEBUG((DEBUG_INFO, "DisableDmar: GSTS_REG - 0x%08x\n", Reg32));
 
     DEBUG ((DEBUG_INFO,"VTD (%d) Disabled!<<<<<<\n",Index));
+
+    DisableQueuedInvalidationInterface(Index);
   }
 
   mVtdEnabled = FALSE;
@@ -380,6 +632,21 @@ DisableDmar (
   return EFI_SUCCESS;
 }
 
+/**
+  Dump VTd version registers.
+
+  @param[in]  VerReg            The version register.
+**/
+VOID
+DumpVtdVerRegs (
+  IN VTD_VER_REG                *VerReg
+  )
+{
+  DEBUG ((DEBUG_INFO, "  VerReg:\n", VerReg->Uint32));
+  DEBUG ((DEBUG_INFO, "    Major - 0x%x\n", VerReg->Bits.Major));
+  DEBUG ((DEBUG_INFO, "    Minor - 0x%x\n", VerReg->Bits.Minor));
+}
+
 /**
   Dump VTd capability registers.
 
diff --git a/Silicon/Intel/IntelSiliconPkg/Include/IndustryStandard/Vtd.h b/Silicon/Intel/IntelSiliconPkg/Include/IndustryStandard/Vtd.h
index b2f745bd..a759ca10 100644
--- a/Silicon/Intel/IntelSiliconPkg/Include/IndustryStandard/Vtd.h
+++ b/Silicon/Intel/IntelSiliconPkg/Include/IndustryStandard/Vtd.h
@@ -206,10 +206,12 @@ typedef union {
 #define   B_CAP_REG_RWBF       BIT4
 #define R_ECAP_REG       0x10
 #define R_GCMD_REG       0x18
+#define   B_GMCD_REG_QIE       BIT26
 #define   B_GMCD_REG_WBF       BIT27
 #define   B_GMCD_REG_SRTP      BIT30
 #define   B_GMCD_REG_TE        BIT31
 #define R_GSTS_REG       0x1C
+#define   B_GSTS_REG_QIES      BIT26
 #define   B_GSTS_REG_WBF       BIT27
 #define   B_GSTS_REG_RTPS      BIT30
 #define   B_GSTS_REG_TE        BIT31
@@ -221,6 +223,9 @@ typedef union {
 #define   V_CCMD_REG_CIRG_DEVICE  (BIT62|BIT61)
 #define   B_CCMD_REG_ICC          BIT63
 #define R_FSTS_REG       0x34
+#define   B_FSTS_REG_IQE          BIT4
+#define   B_FSTS_REG_ICE          BIT5
+#define   B_FSTS_REG_ITE          BIT6
 #define R_FECTL_REG      0x38
 #define R_FEDATA_REG     0x3C
 #define R_FEADDR_REG     0x40
@@ -247,6 +252,58 @@ typedef union {
 #define R_PMEN_HIGH_BASE_REG      0x70
 #define R_PMEN_HIGH_LIMITE_REG    0x78
 
+#define R_IQH_REG        0x80
+#define R_IQT_REG        0x88
+#define   DMAR_IQ_SHIFT  4   /* Invalidation queue head/tail shift */
+
+#define R_IQA_REG        0x90
+
+#define VTD_PAGE_SHIFT   (12)
+#define VTD_PAGE_SIZE    (1UL << VTD_PAGE_SHIFT)
+#define VTD_PAGE_MASK    (((UINT64)-1) << VTD_PAGE_SHIFT)
+
+#define QI_CC_TYPE       0x1
+#define QI_IOTLB_TYPE    0x2
+#define QI_DIOTLB_TYPE   0x3
+#define QI_IEC_TYPE      0x4
+#define QI_IWD_TYPE      0x5
+
+#define QI_CC_FM(fm)        (((UINT64)fm) << 48)
+#define QI_CC_SID(sid)      (((UINT64)sid) << 32)
+#define QI_CC_DID(did)      (((UINT64)did) << 16)
+#define QI_CC_GRAN(gran)    (((UINT64)gran) << 4)
+
+#define QI_IOTLB_DID(did)   (((UINT64)did) << 16)
+#define QI_IOTLB_DR(dr)     (((UINT64)dr) << 7)
+#define QI_IOTLB_DW(dw)     (((UINT64)dw) << 6)
+#define QI_IOTLB_GRAN(gran) (((UINT64)gran) << 4)
+#define QI_IOTLB_ADDR(addr) (((UINT64)addr) & VTD_PAGE_MASK)
+#define QI_IOTLB_IH(ih)     (((UINT64)ih) << 6)
+#define QI_IOTLB_AM(am)     (((UINT8)am))
+
+#define CAP_READ_DRAIN(c)   (((c) >> 55) & 1)
+#define CAP_WRITE_DRAIN(c)  (((c) >> 54) & 1)
+
+#define QI_IWD_STATUS_DATA(d)   (((UINT64)d) << 32)
+#define QI_IWD_STATUS_WRITE (((UINT64)1) << 5)
+
+//
+// This is the queued invalidate descriptor.
+//
+typedef struct {
+  UINT64 Low;
+  UINT64 High;
+} QI_DESC;
+
+typedef union {
+  struct {
+    UINT8         Minor:4;
+    UINT8         Major:4;
+    UINT32        Rsvd:24;
+  } Bits;
+  UINT32          Uint32;
+} VTD_VER_REG;
+
 typedef union {
   struct {
     UINT8         ND:3; // Number of domains supported
-- 
2.16.2.windows.1


^ permalink raw reply related	[flat|nested] only message in thread

only message in thread, other threads:[~2021-05-26  8:27 UTC | newest]

Thread overview: (only message) (download: mbox.gz follow: Atom feed
-- links below jump to the message on this page --
2021-05-26  8:27 [PATCH v3] IntelSiliconPkg/VTd: Support queued invalidation interface Sheng Wei

This is a public inbox, see mirroring instructions
for how to clone and mirror all data and code used for this inbox