[v1] IntelSiliconPkg/VTd: Support queued invalidation interface

[edk2-devel] [PATCH] IntelSiliconPkg/VTd: Support queued invalidation interface

Posted by Sheng Wei 2 years, 12 months ago

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>
---
 .../Feature/VTd/IntelVTdDmarPei/DmarTable.c        |   2 +
 .../Feature/VTd/IntelVTdDmarPei/IntelVTdDmar.c     | 443 +++++++++++++++++++--
 .../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               | 313 +++++++++++++--
 .../IntelSiliconPkg/Include/IndustryStandard/Vtd.h |  57 +++
 8 files changed, 813 insertions(+), 67 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..f0bd7dc6 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) 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);
+
+  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     fault_reg;
+
+  fault_reg = MmioRead32 ((UINTN)VTdUnitInfo->VtdUnitBaseAddress + R_FSTS_REG);
+
+  if (fault_reg & B_FSTS_REG_IQE) {
+    DEBUG((DEBUG_ERROR, "Detect Invalidation Queue Error [0x%08x]\n", fault_reg));
+    fault_reg |= B_FSTS_REG_IQE;
+    MmioWrite32 ((UINTN)VTdUnitInfo->VtdUnitBaseAddress + R_FSTS_REG, fault_reg);
+    return RETURN_DEVICE_ERROR;
+  }
+
+  if (fault_reg & B_FSTS_REG_ITE) {
+    DEBUG((DEBUG_ERROR, "Detect Invalidation Time-out Error [0x%08x]\n", fault_reg));
+    fault_reg |= B_FSTS_REG_ITE;
+    MmioWrite32 ((UINTN)VTdUnitInfo->VtdUnitBaseAddress + R_FSTS_REG, fault_reg);
+    return RETURN_DEVICE_ERROR;
+  }
+
+  if (fault_reg & B_FSTS_REG_ICE) {
+    DEBUG((DEBUG_ERROR, "Detect Invalidation Completion Error [0x%08x]\n", fault_reg));
+    fault_reg |= B_FSTS_REG_ICE;
+    MmioWrite32 ((UINTN)VTdUnitInfo->VtdUnitBaseAddress + R_FSTS_REG, fault_reg);
+    return RETURN_DEVICE_ERROR;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Submit the queued invalidation descriptor to the remapping
+   hardware unit and wait for its completion.
 
-  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;
+  @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 rc;
+  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);
 
+  rc = EFI_SUCCESS;
   do {
-    Reg64 = MmioRead64 (VtdUnitBaseAddress + R_CCMD_REG);
-  } while ((Reg64 & B_CCMD_REG_ICC) != 0);
+    rc = QueuedInvalidationCheckFault(VTdUnitInfo);
+    if (rc != 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 rc;
+}
+
+/**
+  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 {
+    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 {
-    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_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"));
@@ -286,7 +599,7 @@ EnableVTdTranslationProtectionAll (
     if ((EngineMask & LShiftU64(1, Index)) == 0) {
       continue;
     }
-    EnableDmar ((UINTN) VTdInfo->VtdUnitInfo[Index].VtdUnitBaseAddress, (UINTN) *RootEntryTable);
+    EnableDmarPreMem (VTdInfo->VtdUnitInfo[Index].VtdUnitBaseAddress, (UINTN) *RootEntryTable);
   }
 
   return;
@@ -311,10 +624,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,11 +658,28 @@ DisableVTdTranslationProtection (
       continue;
     }
     DisableDmar ((UINTN) VTdInfo->VtdUnitInfo[Index].VtdUnitBaseAddress);
+
+    DisableQueuedInvalidationInterface(&VTdInfo->VtdUnitInfo[Index]);
   }
 
   return;
 }
 
+/**
+  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.
 
@@ -414,6 +744,31 @@ DumpVtdECapRegs (
   DEBUG ((DEBUG_INFO, "    PSS    - 0x%x\n", ECapReg->Bits.PSS));
 }
 
+/**
+  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
+  )
+{
+  UINTN                         Index;
+  EFI_STATUS                    Status;
+
+  for (Index = 0; Index < VTdInfo->VTdEngineCount; Index++) {
+    Status = PerpareCacheInvalidationInterface(&VTdInfo->VtdUnitInfo[Index]);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
 /**
   Prepare VTD configuration.
 
@@ -431,6 +786,9 @@ 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);
+    VTdInfo->VtdUnitInfo[Index].VerReg.Bits.Major = 7;
+    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);
@@ -464,3 +822,4 @@ PrepareVtdConfig (
   return EFI_SUCCESS;
 }
 
+
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..1049e940 100644
--- a/Silicon/Intel/IntelSiliconPkg/Feature/VTd/IntelVTdDxe/VtdReg.c
+++ b/Silicon/Intel/IntelSiliconPkg/Feature/VTd/IntelVTdDxe/VtdReg.c
@@ -55,31 +55,256 @@ 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);
+  }
+
+  //
+  // 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) 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 {
+    Reg32 = MmioRead32 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_GSTS_REG);
+  } while ((Reg32 & B_GSTS_REG_QIES) == 0);
+
+  mVtdUnitInformation[VtdIndex].QiFreeHead = 0;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  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
+QueuedInvalidationCheckFault (
+  IN UINTN  VtdIndex
+  )
+{
+  UINT32     fault_reg;
+
+  fault_reg = MmioRead32 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_FSTS_REG);
+
+  if (fault_reg & B_FSTS_REG_IQE) {
+    DEBUG((DEBUG_ERROR, "Detect Invalidation Queue Error [0x%08x]\n", fault_reg));
+    fault_reg |= B_FSTS_REG_IQE;
+    MmioWrite32 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_FSTS_REG, fault_reg);
+    return RETURN_DEVICE_ERROR;
+  }
+
+  if (fault_reg & B_FSTS_REG_ITE) {
+    DEBUG((DEBUG_ERROR, "Detect Invalidation Time-out Error [0x%08x]\n", fault_reg));
+    fault_reg |= B_FSTS_REG_ITE;
+    MmioWrite32 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_FSTS_REG, fault_reg);
+    return RETURN_DEVICE_ERROR;
+  }
+
+  if (fault_reg & B_FSTS_REG_ICE) {
+    DEBUG((DEBUG_ERROR, "Detect Invalidation Completion Error [0x%08x]\n", fault_reg));
+    fault_reg |= B_FSTS_REG_ICE;
+    MmioWrite32 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_FSTS_REG, fault_reg);
+    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 rc;
+  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 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_CCMD_REG, Reg64);
+  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);
 
+  rc = EFI_SUCCESS;
   do {
+    rc = QueuedInvalidationCheckFault(VtdIndex);
+    if (rc != EFI_SUCCESS) {
+      DEBUG((DEBUG_ERROR,"Detect Queued Invalidation Fault.\n"));
+      break;
+    }
+
+    Reg64Iqh = MmioRead64 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_IQH_REG);
+  } while (Reg64Iqt != Reg64Iqh);
+
+  return rc;
+}
+
+/**
+  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);
-  } while ((Reg64 & B_CCMD_REG_ICC) != 0);
+    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;
 }
 
@@ -94,20 +319,34 @@ InvalidateIOTLB (
   )
 {
   UINT64  Reg64;
+  QI_DESC QiDesc;
 
-  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;
-  }
+  if (mVtdUnitInformation[VtdIndex].EnableQueuedInvalidation == 0) {
+    //
+    // Register-based Invalidation
+    //
+    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;
+    }
 
-  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);
+    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);
+    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..199cb398 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



-=-=-=-=-=-=-=-=-=-=-=-
Groups.io Links: You receive all messages sent to this group.
View/Reply Online (#74625): https://edk2.groups.io/g/devel/message/74625
Mute This Topic: https://groups.io/mt/82475455/1787277
Group Owner: devel+owner@edk2.groups.io
Unsubscribe: https://edk2.groups.io/g/devel/unsub [importer@patchew.org]
-=-=-=-=-=-=-=-=-=-=-=-

Re: [edk2-devel] [PATCH] IntelSiliconPkg/VTd: Support queued invalidation interface

Posted by Huang, Jenny 2 years, 11 months ago

Reviewed by Jenny Huang.

-----Original Message-----
From: Sheng, W <w.sheng@intel.com> 
Sent: Thursday, April 29, 2021 11:55 PM
To: devel@edk2.groups.io
Cc: Huang, Jenny <jenny.huang@intel.com>; Yao, Jiewen <jiewen.yao@intel.com>; Ni, Ray <ray.ni@intel.com>; Chaganty, Rangasai V <rangasai.v.chaganty@intel.com>
Subject: [PATCH] IntelSiliconPkg/VTd: Support queued invalidation interface

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>
---
 .../Feature/VTd/IntelVTdDmarPei/DmarTable.c        |   2 +
 .../Feature/VTd/IntelVTdDmarPei/IntelVTdDmar.c     | 443 +++++++++++++++++++--
 .../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               | 313 +++++++++++++--
 .../IntelSiliconPkg/Include/IndustryStandard/Vtd.h |  57 +++
 8 files changed, 813 insertions(+), 67 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/DmarTabl
+++ e.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..f0bd7dc6 100644
--- a/Silicon/Intel/IntelSiliconPkg/Feature/VTd/IntelVTdDmarPei/IntelVTdDmar.c
+++ b/Silicon/Intel/IntelSiliconPkg/Feature/VTd/IntelVTdDmarPei/IntelVTd
+++ Dmar.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) 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);
+
+  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     fault_reg;
+
+  fault_reg = MmioRead32 ((UINTN)VTdUnitInfo->VtdUnitBaseAddress + 
+ R_FSTS_REG);
+
+  if (fault_reg & B_FSTS_REG_IQE) {
+    DEBUG((DEBUG_ERROR, "Detect Invalidation Queue Error [0x%08x]\n", fault_reg));
+    fault_reg |= B_FSTS_REG_IQE;
+    MmioWrite32 ((UINTN)VTdUnitInfo->VtdUnitBaseAddress + R_FSTS_REG, fault_reg);
+    return RETURN_DEVICE_ERROR;
+  }
+
+  if (fault_reg & B_FSTS_REG_ITE) {
+    DEBUG((DEBUG_ERROR, "Detect Invalidation Time-out Error [0x%08x]\n", fault_reg));
+    fault_reg |= B_FSTS_REG_ITE;
+    MmioWrite32 ((UINTN)VTdUnitInfo->VtdUnitBaseAddress + R_FSTS_REG, fault_reg);
+    return RETURN_DEVICE_ERROR;
+  }
+
+  if (fault_reg & B_FSTS_REG_ICE) {
+    DEBUG((DEBUG_ERROR, "Detect Invalidation Completion Error [0x%08x]\n", fault_reg));
+    fault_reg |= B_FSTS_REG_ICE;
+    MmioWrite32 ((UINTN)VTdUnitInfo->VtdUnitBaseAddress + R_FSTS_REG, fault_reg);
+    return RETURN_DEVICE_ERROR;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Submit the queued invalidation descriptor to the remapping
+   hardware unit and wait for its completion.
 
-  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;
+  @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 rc;
+  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);
 
+  rc = EFI_SUCCESS;
   do {
-    Reg64 = MmioRead64 (VtdUnitBaseAddress + R_CCMD_REG);
-  } while ((Reg64 & B_CCMD_REG_ICC) != 0);
+    rc = QueuedInvalidationCheckFault(VTdUnitInfo);
+    if (rc != 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 rc;
+}
+
+/**
+  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 {
+    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 {
-    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_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")); @@ -286,7 +599,7 @@ EnableVTdTranslationProtectionAll (
     if ((EngineMask & LShiftU64(1, Index)) == 0) {
       continue;
     }
-    EnableDmar ((UINTN) VTdInfo->VtdUnitInfo[Index].VtdUnitBaseAddress, (UINTN) *RootEntryTable);
+    EnableDmarPreMem (VTdInfo->VtdUnitInfo[Index].VtdUnitBaseAddress, 
+ (UINTN) *RootEntryTable);
   }
 
   return;
@@ -311,10 +624,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,11 +658,28 @@ DisableVTdTranslationProtection (
       continue;
     }
     DisableDmar ((UINTN) VTdInfo->VtdUnitInfo[Index].VtdUnitBaseAddress);
+
+    DisableQueuedInvalidationInterface(&VTdInfo->VtdUnitInfo[Index]);
   }
 
   return;
 }
 
+/**
+  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.
 
@@ -414,6 +744,31 @@ DumpVtdECapRegs (
   DEBUG ((DEBUG_INFO, "    PSS    - 0x%x\n", ECapReg->Bits.PSS));
 }
 
+/**
+  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
+  )
+{
+  UINTN                         Index;
+  EFI_STATUS                    Status;
+
+  for (Index = 0; Index < VTdInfo->VTdEngineCount; Index++) {
+    Status = PerpareCacheInvalidationInterface(&VTdInfo->VtdUnitInfo[Index]);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
 /**
   Prepare VTD configuration.
 
@@ -431,6 +786,9 @@ 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);
+    VTdInfo->VtdUnitInfo[Index].VerReg.Bits.Major = 7;
+    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); @@ -464,3 +822,4 @@ PrepareVtdConfig (
   return EFI_SUCCESS;
 }
 
+
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/IntelVTd
+++ DmarPei.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/IntelVTd
+++ DmarPei.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/Translat
+++ ionTable.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/DmaProtectio
+++ n.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..1049e940 100644
--- a/Silicon/Intel/IntelSiliconPkg/Feature/VTd/IntelVTdDxe/VtdReg.c
+++ b/Silicon/Intel/IntelSiliconPkg/Feature/VTd/IntelVTdDxe/VtdReg.c
@@ -55,31 +55,256 @@ 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);  }
+
+  //
+  // 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) 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 {
+    Reg32 = MmioRead32 
+ (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_GSTS_REG);  } 
+ while ((Reg32 & B_GSTS_REG_QIES) == 0);
+
+  mVtdUnitInformation[VtdIndex].QiFreeHead = 0;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  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
+QueuedInvalidationCheckFault (
+  IN UINTN  VtdIndex
+  )
+{
+  UINT32     fault_reg;
+
+  fault_reg = MmioRead32 
+ (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_FSTS_REG);
+
+  if (fault_reg & B_FSTS_REG_IQE) {
+    DEBUG((DEBUG_ERROR, "Detect Invalidation Queue Error [0x%08x]\n", fault_reg));
+    fault_reg |= B_FSTS_REG_IQE;
+    MmioWrite32 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_FSTS_REG, fault_reg);
+    return RETURN_DEVICE_ERROR;
+  }
+
+  if (fault_reg & B_FSTS_REG_ITE) {
+    DEBUG((DEBUG_ERROR, "Detect Invalidation Time-out Error [0x%08x]\n", fault_reg));
+    fault_reg |= B_FSTS_REG_ITE;
+    MmioWrite32 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_FSTS_REG, fault_reg);
+    return RETURN_DEVICE_ERROR;
+  }
+
+  if (fault_reg & B_FSTS_REG_ICE) {
+    DEBUG((DEBUG_ERROR, "Detect Invalidation Completion Error [0x%08x]\n", fault_reg));
+    fault_reg |= B_FSTS_REG_ICE;
+    MmioWrite32 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_FSTS_REG, fault_reg);
+    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 rc;
+  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 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_CCMD_REG, Reg64);
+  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);
 
+  rc = EFI_SUCCESS;
   do {
+    rc = QueuedInvalidationCheckFault(VtdIndex);
+    if (rc != EFI_SUCCESS) {
+      DEBUG((DEBUG_ERROR,"Detect Queued Invalidation Fault.\n"));
+      break;
+    }
+
+    Reg64Iqh = MmioRead64 
+ (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_IQH_REG);  } 
+ while (Reg64Iqt != Reg64Iqh);
+
+  return rc;
+}
+
+/**
+  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);
-  } while ((Reg64 & B_CCMD_REG_ICC) != 0);
+    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;
 }
 
@@ -94,20 +319,34 @@ InvalidateIOTLB (
   )
 {
   UINT64  Reg64;
+  QI_DESC QiDesc;
 
-  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;
-  }
+  if (mVtdUnitInformation[VtdIndex].EnableQueuedInvalidation == 0) {
+    //
+    // Register-based Invalidation
+    //
+    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;
+    }
 
-  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);
+    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);
+    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..199cb398 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



-=-=-=-=-=-=-=-=-=-=-=-
Groups.io Links: You receive all messages sent to this group.
View/Reply Online (#75019): https://edk2.groups.io/g/devel/message/75019
Mute This Topic: https://groups.io/mt/82475455/1787277
Group Owner: devel+owner@edk2.groups.io
Unsubscribe: https://edk2.groups.io/g/devel/unsub [importer@patchew.org]
-=-=-=-=-=-=-=-=-=-=-=-