IntelSiliconPkg/IntelVTdDxe: Improve performance.

This patch is to improve IOMMU performance.
All WBINVD is removed due to performance issue.
CLFLUSH by WriteBackDataCacheRange() is used to
only flush the context table or
second level page table if they are changed.

This patch also removed some unused functions.

Cc: Star Zeng <star.zeng@intel.com>
Contributed-under: TianoCore Contribution Agreement 1.0
Signed-off-by: Jiewen Yao <jiewen.yao@intel.com>
Reviewed-by: Star Zeng <star.zeng@intel.com>

diff --git a/IntelSiliconPkg/IntelVTdDxe/DmaProtection.h b/IntelSiliconPkg/IntelVTdDxe/DmaProtection.h
index 8cfa69cb2364a670e77bc7f95b4cceb3c302cf56..c3b57a03bb424ebd41771584f0d6e260b5d4d84e 100644 (file)
--- a/IntelSiliconPkg/IntelVTdDxe/DmaProtection.h
+++ b/IntelSiliconPkg/IntelVTdDxe/DmaProtection.h
@@ -25,6 +25,9 @@
 #include <Library/PciSegmentLib.h>\r
 #include <Library/DebugLib.h>\r
 #include <Library/UefiLib.h>\r
+#include <Library/CacheMaintenanceLib.h>\r
+#include <Library/PerformanceLib.h>\r
+#include <Library/PrintLib.h>\r
 \r
 #include <Guid/EventGroup.h>\r
 #include <Guid/Acpi.h>\r
@@ -58,6 +61,8 @@ typedef struct {
   UINTN                  PciDescriptorMaxNumber;\r
   BOOLEAN                *IsRealPciDevice;\r
   VTD_SOURCE_ID          *PciDescriptors;\r
+  // for statistic analysis\r
+  UINTN                  *AccessCount;\r
 } PCI_DEVICE_INFORMATION;\r
 \r
 typedef struct {\r
@@ -68,6 +73,7 @@ typedef struct {
   VTD_ROOT_ENTRY                   *RootEntryTable;\r
   VTD_EXT_ROOT_ENTRY               *ExtRootEntryTable;\r
   VTD_SECOND_LEVEL_PAGING_ENTRY    *FixedSecondLevelPagingEntry;\r
+  BOOLEAN                          HasDirtyContext;\r
   BOOLEAN                          HasDirtyPages;\r
   PCI_DEVICE_INFORMATION           PciDeviceInfo;\r
 } VTD_UNIT_INFORMATION;\r
@@ -124,40 +130,6 @@ DisableDmar (
   VOID\r
   );\r
 \r
-/**\r
-  Invalid VTd IOTLB page.\r
-\r
-  @param[in]  VtdIndex              The index of VTd engine.\r
-  @param[in]  Address               The address of IOTLB page.\r
-  @param[in]  AddressMode           The address mode of IOTLB page.\r
-  @param[in]  DomainIdentifier      The domain ID of the source.\r
-\r
-  @retval EFI_SUCCESS           VTd IOTLB page is invalidated.\r
-  @retval EFI_DEVICE_ERROR      VTd IOTLB page is not invalidated.\r
-**/\r
-EFI_STATUS\r
-InvalidateVtdIOTLBPage (\r
-  IN UINTN  VtdIndex,\r
-  IN UINT64 Address,\r
-  IN UINT8  AddressMode,\r
-  IN UINT16 DomainIdentifier\r
-  );\r
-\r
-/**\r
-  Invalid VTd IOTLB domain.\r
-\r
-  @param[in]  VtdIndex              The index of VTd engine.\r
-  @param[in]  DomainIdentifier      The domain ID of the source.\r
-\r
-  @retval EFI_SUCCESS           VTd IOTLB domain is invalidated.\r
-  @retval EFI_DEVICE_ERROR      VTd IOTLB domain is not invalidated.\r
-**/\r
-EFI_STATUS\r
-InvalidateVtdIOTLBDomain (\r
-  IN UINTN  VtdIndex,\r
-  IN UINT16 DomainIdentifier\r
-  );\r
-\r
 /**\r
   Invalid VTd global IOTLB.\r
 \r
@@ -362,6 +334,7 @@ DumpSecondLevelPagingEntry (
 EFI_STATUS\r
 SetPageAttribute (\r
   IN UINTN                         VtdIndex,\r
+  IN UINT16                        DomainIdentifier,\r
   IN VTD_SECOND_LEVEL_PAGING_ENTRY *SecondLevelPagingEntry,\r
   IN UINT64                        BaseAddress,\r
   IN UINT64                        Length,\r
@@ -500,4 +473,20 @@ AllocateZeroPages (
   IN UINTN  Pages\r
   );\r
 \r
+/**\r
+  Flush VTD page table and context table memory.\r
+\r
+  This action is to make sure the IOMMU engine can get final data in memory.\r
+\r
+  @param[in]  VtdIndex          The index used to identify a VTd engine.\r
+  @param[in]  Base              The base address of memory to be flushed.\r
+  @param[in]  Size              The size of memory in bytes to be flushed.\r
+**/\r
+VOID\r
+FlushPageTableMemory (\r
+  IN UINTN  VtdIndex,\r
+  IN UINTN  Base,\r
+  IN UINTN  Size\r
+  );\r
+\r
 #endif\r

diff --git a/IntelSiliconPkg/IntelVTdDxe/IntelVTdDxe.c b/IntelSiliconPkg/IntelVTdDxe/IntelVTdDxe.c
index d22222d713b8060c889a7fc61df9ad0fc5f57580..24b88c371928363a682bb8ff7cc0a2107e199dfe 100644 (file)
--- a/IntelSiliconPkg/IntelVTdDxe/IntelVTdDxe.c
+++ b/IntelSiliconPkg/IntelVTdDxe/IntelVTdDxe.c
@@ -227,6 +227,8 @@ VTdSetAttribute (
   EFI_STATUS           Status;\r
   UINT16               Segment;\r
   VTD_SOURCE_ID        SourceId;\r
+  CHAR8                PerfToken[sizeof("VTD(S0000.B00.D00.F00)")];\r
+  UINT32               Identifier;\r
 \r
   DumpVtdIfError ();\r
 \r
@@ -239,8 +241,19 @@ VTdSetAttribute (
   DEBUG ((DEBUG_VERBOSE, "PCI(S%x.B%x.D%x.F%x) ", Segment, SourceId.Bits.Bus, SourceId.Bits.Device, SourceId.Bits.Function));\r
   DEBUG ((DEBUG_VERBOSE, "(0x%lx~0x%lx) - %lx\n", DeviceAddress, Length, IoMmuAccess));\r
 \r
+  PERF_CODE (\r
+    AsciiSPrint (PerfToken, sizeof(PerfToken), "S%04xB%02xD%02xF%01x", Segment, SourceId.Bits.Bus, SourceId.Bits.Device, SourceId.Bits.Function);\r
+    Identifier = (Segment << 16) | SourceId.Uint16;\r
+    PERF_START_EX (gImageHandle, PerfToken, "IntelVTD", 0, Identifier);\r
+  );\r
+\r
   Status = SetAccessAttribute (Segment, SourceId, DeviceAddress, Length, IoMmuAccess);\r
 \r
+  PERF_CODE (\r
+    Identifier = (Segment << 16) | SourceId.Uint16;\r
+    PERF_END_EX (gImageHandle, PerfToken, "IntelVTD", 0, Identifier);\r
+  );\r
+\r
   return Status;\r
 }\r
 \r

diff --git a/IntelSiliconPkg/IntelVTdDxe/IntelVTdDxe.inf b/IntelSiliconPkg/IntelVTdDxe/IntelVTdDxe.inf
index 6a61c13d27b89ddad4a6784e09fa82f85c5938a3..697932e9bf24897e0638264bc9ccc24856fb758a 100644 (file)
--- a/IntelSiliconPkg/IntelVTdDxe/IntelVTdDxe.inf
+++ b/IntelSiliconPkg/IntelVTdDxe/IntelVTdDxe.inf
@@ -57,6 +57,9 @@
   BaseMemoryLib\r
   MemoryAllocationLib\r
   UefiLib\r
+  CacheMaintenanceLib\r
+  PerformanceLib\r
+  PrintLib\r
 \r
 [Guids]\r
   gEfiEventExitBootServicesGuid   ## CONSUMES ## Event\r

diff --git a/IntelSiliconPkg/IntelVTdDxe/PciInfo.c b/IntelSiliconPkg/IntelVTdDxe/PciInfo.c
index d5f096fadd5dca5e514eeb9610bd9465a6e54c92..27e253d42809d4a96e22acdc4de985a50dd91b08 100644 (file)
--- a/IntelSiliconPkg/IntelVTdDxe/PciInfo.c
+++ b/IntelSiliconPkg/IntelVTdDxe/PciInfo.c
@@ -77,6 +77,7 @@ RegisterPciDevice (
   UINTN                   Index;\r
   BOOLEAN                 *NewIsRealPciDevice;\r
   VTD_SOURCE_ID           *NewPciDescriptors;\r
+  UINTN                   *NewAccessCount;\r
 \r
   PciDeviceInfo = &mVtdUnitInformation[VtdIndex].PciDeviceInfo;\r
 \r
@@ -112,6 +113,12 @@ RegisterPciDevice (
         FreePool (NewIsRealPciDevice);\r
         return EFI_OUT_OF_RESOURCES;\r
       }\r
+      NewAccessCount = AllocateZeroPool (sizeof(*NewAccessCount) * (PciDeviceInfo->PciDescriptorMaxNumber + MAX_PCI_DESCRIPTORS));\r
+      if (NewAccessCount == NULL) {\r
+        FreePool (NewIsRealPciDevice);\r
+        FreePool (NewPciDescriptors);\r
+        return EFI_OUT_OF_RESOURCES;\r
+      }\r
       PciDeviceInfo->PciDescriptorMaxNumber += MAX_PCI_DESCRIPTORS;\r
       if (PciDeviceInfo->IsRealPciDevice != NULL) {\r
         CopyMem (NewIsRealPciDevice, PciDeviceInfo->IsRealPciDevice, sizeof(*NewIsRealPciDevice) * PciDeviceInfo->PciDescriptorNumber);\r
@@ -123,6 +130,11 @@ RegisterPciDevice (
         FreePool (PciDeviceInfo->PciDescriptors);\r
       }\r
       PciDeviceInfo->PciDescriptors = NewPciDescriptors;\r
+      if (PciDeviceInfo->AccessCount != NULL) {\r
+        CopyMem (NewAccessCount, PciDeviceInfo->AccessCount, sizeof(*NewAccessCount) * PciDeviceInfo->PciDescriptorNumber);\r
+        FreePool (PciDeviceInfo->AccessCount);\r
+      }\r
+      PciDeviceInfo->AccessCount = NewAccessCount;\r
     }\r
 \r
     ASSERT (PciDeviceInfo->PciDescriptorNumber < PciDeviceInfo->PciDescriptorMaxNumber);\r

diff --git a/IntelSiliconPkg/IntelVTdDxe/TranslationTable.c b/IntelSiliconPkg/IntelVTdDxe/TranslationTable.c
index 961d7cad0ddf734d6962bfedc416e9770d99a9c1..80fc82347e20e375e94acb489caac51df0d4505b 100644 (file)
--- a/IntelSiliconPkg/IntelVTdDxe/TranslationTable.c
+++ b/IntelSiliconPkg/IntelVTdDxe/TranslationTable.c
@@ -124,6 +124,7 @@ CreateContextEntry (
       RootEntry->Bits.ContextTablePointerHi  = (UINT32) RShiftU64 ((UINT64)(UINTN)Buffer, 32);\r
       RootEntry->Bits.Present = 1;\r
       Buffer = (UINT8 *)Buffer + EFI_PAGES_TO_SIZE (ContextPages);\r
+      FlushPageTableMemory (VtdIndex, (UINTN)RootEntry, sizeof(*RootEntry));\r
     }\r
 \r
     ContextEntryTable = (VTD_CONTEXT_ENTRY *)(UINTN)VTD_64BITS_ADDRESS(RootEntry->Bits.ContextTablePointerLo, RootEntry->Bits.ContextTablePointerHi) ;\r
@@ -142,6 +143,7 @@ CreateContextEntry (
       ContextEntry->Bits.AddressWidth = 0x2;\r
       break;\r
     }\r
+    FlushPageTableMemory (VtdIndex, (UINTN)ContextEntry, sizeof(*ContextEntry));\r
   }\r
 \r
   return EFI_SUCCESS;\r
@@ -250,8 +252,11 @@ CreateSecondLevelPagingEntryTable (
           goto Done;\r
         }\r
       }\r
+      FlushPageTableMemory (VtdIndex, (UINTN)Lvl2PtEntry, SIZE_4KB);\r
     }\r
+    FlushPageTableMemory (VtdIndex, (UINTN)&Lvl3PtEntry[Lvl3Start], (UINTN)&Lvl3PtEntry[Lvl3End + 1] - (UINTN)&Lvl3PtEntry[Lvl3Start]);\r
   }\r
+  FlushPageTableMemory (VtdIndex, (UINTN)&Lvl4PtEntry[Lvl4Start], (UINTN)&Lvl4PtEntry[Lvl4End + 1] - (UINTN)&Lvl4PtEntry[Lvl4Start]);\r
 \r
 Done:\r
   return SecondLevelPagingEntry;\r
@@ -429,9 +434,10 @@ InvalidatePageEntry (
   IN UINTN                 VtdIndex\r
   )\r
 {\r
-  if (mVtdUnitInformation[VtdIndex].HasDirtyPages) {\r
+  if (mVtdUnitInformation[VtdIndex].HasDirtyContext || mVtdUnitInformation[VtdIndex].HasDirtyPages) {\r
     InvalidateVtdIOTLBGlobal (VtdIndex);\r
   }\r
+  mVtdUnitInformation[VtdIndex].HasDirtyContext = FALSE;\r
   mVtdUnitInformation[VtdIndex].HasDirtyPages = FALSE;\r
 }\r
 \r
@@ -498,6 +504,7 @@ PageAttributeToLength (
 /**\r
   Return page table entry to match the address.\r
 \r
+  @param[in]   VtdIndex                 The index used to identify a VTd engine.\r
   @param[in]   SecondLevelPagingEntry   The second level paging entry in VTd table for the device.\r
   @param[in]   Address                  The address to be checked.\r
   @param[out]  PageAttributes           The page attribute of the page entry.\r
@@ -506,6 +513,7 @@ PageAttributeToLength (
 **/\r
 VOID *\r
 GetSecondLevelPageTableEntry (\r
+  IN  UINTN                         VtdIndex,\r
   IN  VTD_SECOND_LEVEL_PAGING_ENTRY *SecondLevelPagingEntry,\r
   IN  PHYSICAL_ADDRESS              Address,\r
   OUT PAGE_ATTRIBUTE                *PageAttribute\r
@@ -535,6 +543,7 @@ GetSecondLevelPageTableEntry (
       return NULL;\r
     }\r
     SetSecondLevelPagingEntryAttribute ((VTD_SECOND_LEVEL_PAGING_ENTRY *)&L4PageTable[Index4], EDKII_IOMMU_ACCESS_READ | EDKII_IOMMU_ACCESS_WRITE);\r
+    FlushPageTableMemory (VtdIndex, (UINTN)&L4PageTable[Index4], sizeof(L4PageTable[Index4]));\r
   }\r
 \r
   L3PageTable = (UINT64 *)(UINTN)(L4PageTable[Index4] & PAGING_4K_ADDRESS_MASK_64);\r
@@ -547,6 +556,7 @@ GetSecondLevelPageTableEntry (
       return NULL;\r
     }\r
     SetSecondLevelPagingEntryAttribute ((VTD_SECOND_LEVEL_PAGING_ENTRY *)&L3PageTable[Index3], EDKII_IOMMU_ACCESS_READ | EDKII_IOMMU_ACCESS_WRITE);\r
+    FlushPageTableMemory (VtdIndex, (UINTN)&L3PageTable[Index3], sizeof(L3PageTable[Index3]));\r
   }\r
   if ((L3PageTable[Index3] & VTD_PG_PS) != 0) {\r
     // 1G\r
@@ -559,6 +569,7 @@ GetSecondLevelPageTableEntry (
     L2PageTable[Index2] = Address & PAGING_2M_ADDRESS_MASK_64;\r
     SetSecondLevelPagingEntryAttribute ((VTD_SECOND_LEVEL_PAGING_ENTRY *)&L2PageTable[Index2], 0);\r
     L2PageTable[Index2] |= VTD_PG_PS;\r
+    FlushPageTableMemory (VtdIndex, (UINTN)&L2PageTable[Index2], sizeof(L2PageTable[Index2]));\r
   }\r
   if ((L2PageTable[Index2] & VTD_PG_PS) != 0) {\r
     // 2M\r
@@ -579,12 +590,14 @@ GetSecondLevelPageTableEntry (
 /**\r
   Modify memory attributes of page entry.\r
 \r
+  @param[in]   VtdIndex         The index used to identify a VTd engine.\r
   @param[in]   PageEntry        The page entry.\r
   @param[in]   IoMmuAccess      The IOMMU access.\r
   @param[out]  IsModified       TRUE means page table modified. FALSE means page table not modified.\r
 **/\r
 VOID\r
 ConvertSecondLevelPageEntryAttribute (\r
+  IN  UINTN                             VtdIndex,\r
   IN  VTD_SECOND_LEVEL_PAGING_ENTRY     *PageEntry,\r
   IN  UINT64                            IoMmuAccess,\r
   OUT BOOLEAN                           *IsModified\r
@@ -595,6 +608,7 @@ ConvertSecondLevelPageEntryAttribute (
 \r
   CurrentPageEntry = PageEntry->Uint64;\r
   SetSecondLevelPagingEntryAttribute (PageEntry, IoMmuAccess);\r
+  FlushPageTableMemory (VtdIndex, (UINTN)PageEntry, sizeof(*PageEntry));\r
   NewPageEntry = PageEntry->Uint64;\r
   if (CurrentPageEntry != NewPageEntry) {\r
     *IsModified = TRUE;\r
@@ -639,6 +653,7 @@ NeedSplitPage (
 /**\r
   This function splits one page entry to small page entries.\r
 \r
+  @param[in]  VtdIndex         The index used to identify a VTd engine.\r
   @param[in]  PageEntry        The page entry to be splitted.\r
   @param[in]  PageAttribute    The page attribute of the page entry.\r
   @param[in]  SplitAttribute   How to split the page entry.\r
@@ -649,6 +664,7 @@ NeedSplitPage (
 **/\r
 RETURN_STATUS\r
 SplitSecondLevelPage (\r
+  IN  UINTN                             VtdIndex,\r
   IN  VTD_SECOND_LEVEL_PAGING_ENTRY     *PageEntry,\r
   IN  PAGE_ATTRIBUTE                    PageAttribute,\r
   IN  PAGE_ATTRIBUTE                    SplitAttribute\r
@@ -675,8 +691,11 @@ SplitSecondLevelPage (
       for (Index = 0; Index < SIZE_4KB / sizeof(UINT64); Index++) {\r
         NewPageEntry[Index] = (BaseAddress + SIZE_4KB * Index) | (PageEntry->Uint64 & PAGE_PROGATE_BITS);\r
       }\r
+      FlushPageTableMemory (VtdIndex, (UINTN)NewPageEntry, SIZE_4KB);\r
+\r
       PageEntry->Uint64 = (UINT64)(UINTN)NewPageEntry;\r
       SetSecondLevelPagingEntryAttribute (PageEntry, EDKII_IOMMU_ACCESS_READ | EDKII_IOMMU_ACCESS_WRITE);\r
+      FlushPageTableMemory (VtdIndex, (UINTN)PageEntry, sizeof(*PageEntry));\r
       return RETURN_SUCCESS;\r
     } else {\r
       return RETURN_UNSUPPORTED;\r
@@ -697,8 +716,11 @@ SplitSecondLevelPage (
       for (Index = 0; Index < SIZE_4KB / sizeof(UINT64); Index++) {\r
         NewPageEntry[Index] = (BaseAddress + SIZE_2MB * Index) | VTD_PG_PS | (PageEntry->Uint64 & PAGE_PROGATE_BITS);\r
       }\r
+      FlushPageTableMemory (VtdIndex, (UINTN)NewPageEntry, SIZE_4KB);\r
+\r
       PageEntry->Uint64 = (UINT64)(UINTN)NewPageEntry;\r
       SetSecondLevelPagingEntryAttribute (PageEntry, EDKII_IOMMU_ACCESS_READ | EDKII_IOMMU_ACCESS_WRITE);\r
+      FlushPageTableMemory (VtdIndex, (UINTN)PageEntry, sizeof(*PageEntry));\r
       return RETURN_SUCCESS;\r
     } else {\r
       return RETURN_UNSUPPORTED;\r
@@ -730,6 +752,7 @@ SplitSecondLevelPage (
 EFI_STATUS\r
 SetSecondLevelPagingAttribute (\r
   IN UINTN                         VtdIndex,\r
+  IN UINT16                        DomainIdentifier,\r
   IN VTD_SECOND_LEVEL_PAGING_ENTRY *SecondLevelPagingEntry,\r
   IN UINT64                        BaseAddress,\r
   IN UINT64                        Length,\r
@@ -756,7 +779,7 @@ SetSecondLevelPagingAttribute (
   }\r
 \r
   while (Length != 0) {\r
-    PageEntry = GetSecondLevelPageTableEntry (SecondLevelPagingEntry, BaseAddress, &PageAttribute);\r
+    PageEntry = GetSecondLevelPageTableEntry (VtdIndex, SecondLevelPagingEntry, BaseAddress, &PageAttribute);\r
     if (PageEntry == NULL) {\r
       DEBUG ((DEBUG_ERROR, "PageEntry - NULL\n"));\r
       return RETURN_UNSUPPORTED;\r
@@ -764,7 +787,7 @@ SetSecondLevelPagingAttribute (
     PageEntryLength = PageAttributeToLength (PageAttribute);\r
     SplitAttribute = NeedSplitPage (BaseAddress, Length, PageAttribute);\r
     if (SplitAttribute == PageNone) {\r
-      ConvertSecondLevelPageEntryAttribute (PageEntry, IoMmuAccess, &IsEntryModified);\r
+      ConvertSecondLevelPageEntryAttribute (VtdIndex, PageEntry, IoMmuAccess, &IsEntryModified);\r
       if (IsEntryModified) {\r
         mVtdUnitInformation[VtdIndex].HasDirtyPages = TRUE;\r
       }\r
@@ -774,7 +797,7 @@ SetSecondLevelPagingAttribute (
       BaseAddress += PageEntryLength;\r
       Length -= PageEntryLength;\r
     } else {\r
-      Status = SplitSecondLevelPage (PageEntry, PageAttribute, SplitAttribute);\r
+      Status = SplitSecondLevelPage (VtdIndex, PageEntry, PageAttribute, SplitAttribute);\r
       if (RETURN_ERROR (Status)) {\r
         DEBUG ((DEBUG_ERROR, "SplitSecondLevelPage - %r\n", Status));\r
         return RETURN_UNSUPPORTED;\r
@@ -787,8 +810,6 @@ SetSecondLevelPagingAttribute (
     }\r
   }\r
 \r
-  InvalidatePageEntry (VtdIndex);\r
-\r
   return EFI_SUCCESS;\r
 }\r
 \r
@@ -814,6 +835,7 @@ SetSecondLevelPagingAttribute (
 EFI_STATUS\r
 SetPageAttribute (\r
   IN UINTN                         VtdIndex,\r
+  IN UINT16                        DomainIdentifier,\r
   IN VTD_SECOND_LEVEL_PAGING_ENTRY *SecondLevelPagingEntry,\r
   IN UINT64                        BaseAddress,\r
   IN UINT64                        Length,\r
@@ -823,7 +845,7 @@ SetPageAttribute (
   EFI_STATUS Status;\r
   Status = EFI_NOT_FOUND;\r
   if (SecondLevelPagingEntry != NULL) {\r
-    Status = SetSecondLevelPagingAttribute (VtdIndex, SecondLevelPagingEntry, BaseAddress, Length, IoMmuAccess);\r
+    Status = SetSecondLevelPagingAttribute (VtdIndex, DomainIdentifier, SecondLevelPagingEntry, BaseAddress, Length, IoMmuAccess);\r
   }\r
   return Status;\r
 }\r
@@ -862,6 +884,8 @@ SetAccessAttribute (
   VTD_CONTEXT_ENTRY             *ContextEntry;\r
   VTD_SECOND_LEVEL_PAGING_ENTRY *SecondLevelPagingEntry;\r
   UINT64                        Pt;\r
+  UINTN                         PciDescriptorIndex;\r
+  UINT16                        DomainIdentifier;\r
 \r
   SecondLevelPagingEntry = NULL;\r
 \r
@@ -873,6 +897,13 @@ SetAccessAttribute (
     return EFI_DEVICE_ERROR;\r
   }\r
 \r
+  PciDescriptorIndex = GetPciDescriptor (VtdIndex, Segment, SourceId);\r
+  mVtdUnitInformation[VtdIndex].PciDeviceInfo.AccessCount[PciDescriptorIndex]++;\r
+  //\r
+  // DomainId should not be 0.\r
+  //\r
+  DomainIdentifier = (UINT16)(PciDescriptorIndex + 1);\r
+\r
   if (ExtContextEntry != NULL) {\r
     if (ExtContextEntry->Bits.Present == 0) {\r
       SecondLevelPagingEntry = CreateSecondLevelPagingEntry (VtdIndex, 0);\r
@@ -881,9 +912,11 @@ SetAccessAttribute (
 \r
       ExtContextEntry->Bits.SecondLevelPageTranslationPointerLo = (UINT32) Pt;\r
       ExtContextEntry->Bits.SecondLevelPageTranslationPointerHi = (UINT32) RShiftU64(Pt, 20);\r
-      ExtContextEntry->Bits.DomainIdentifier = (UINT16) GetPciDescriptor (VtdIndex, Segment, SourceId);\r
+      ExtContextEntry->Bits.DomainIdentifier = DomainIdentifier;\r
       ExtContextEntry->Bits.Present = 1;\r
+      FlushPageTableMemory (VtdIndex, (UINTN)ExtContextEntry, sizeof(*ExtContextEntry));\r
       DumpDmarExtContextEntryTable (mVtdUnitInformation[VtdIndex].ExtRootEntryTable);\r
+      mVtdUnitInformation[VtdIndex].HasDirtyContext = TRUE;\r
     } else {\r
       SecondLevelPagingEntry = (VOID *)(UINTN)VTD_64BITS_ADDRESS(ExtContextEntry->Bits.SecondLevelPageTranslationPointerLo, ExtContextEntry->Bits.SecondLevelPageTranslationPointerHi);\r
       DEBUG ((DEBUG_VERBOSE,"SecondLevelPagingEntry - 0x%x (S%04x B%02x D%02x F%02x)\n", SecondLevelPagingEntry, Segment, SourceId.Bits.Bus, SourceId.Bits.Device, SourceId.Bits.Function));\r
@@ -896,9 +929,11 @@ SetAccessAttribute (
 \r
       ContextEntry->Bits.SecondLevelPageTranslationPointerLo = (UINT32) Pt;\r
       ContextEntry->Bits.SecondLevelPageTranslationPointerHi = (UINT32) RShiftU64(Pt, 20);\r
-      ContextEntry->Bits.DomainIdentifier = (UINT16) GetPciDescriptor (VtdIndex, Segment, SourceId);\r
+      ContextEntry->Bits.DomainIdentifier = DomainIdentifier;\r
       ContextEntry->Bits.Present = 1;\r
+      FlushPageTableMemory (VtdIndex, (UINTN)ContextEntry, sizeof(*ContextEntry));\r
       DumpDmarContextEntryTable (mVtdUnitInformation[VtdIndex].RootEntryTable);\r
+      mVtdUnitInformation[VtdIndex].HasDirtyContext = TRUE;\r
     } else {\r
       SecondLevelPagingEntry = (VOID *)(UINTN)VTD_64BITS_ADDRESS(ContextEntry->Bits.SecondLevelPageTranslationPointerLo, ContextEntry->Bits.SecondLevelPageTranslationPointerHi);\r
       DEBUG ((DEBUG_VERBOSE,"SecondLevelPagingEntry - 0x%x (S%04x B%02x D%02x F%02x)\n", SecondLevelPagingEntry, Segment, SourceId.Bits.Bus, SourceId.Bits.Device, SourceId.Bits.Function));\r
@@ -911,6 +946,7 @@ SetAccessAttribute (
   if (SecondLevelPagingEntry != mVtdUnitInformation[VtdIndex].FixedSecondLevelPagingEntry) {\r
     Status = SetPageAttribute (\r
                VtdIndex,\r
+               DomainIdentifier,\r
                SecondLevelPagingEntry,\r
                BaseAddress,\r
                Length,\r
@@ -922,6 +958,8 @@ SetAccessAttribute (
     }\r
   }\r
 \r
+  InvalidatePageEntry (VtdIndex);\r
+\r
   return EFI_SUCCESS;\r
 }\r
 \r
@@ -965,11 +1003,13 @@ AlwaysEnablePageAttribute (
     ExtContextEntry->Bits.SecondLevelPageTranslationPointerHi = (UINT32) RShiftU64(Pt, 20);\r
     ExtContextEntry->Bits.DomainIdentifier = ((1 << (UINT8)((UINTN)mVtdUnitInformation[VtdIndex].CapReg.Bits.ND * 2 + 4)) - 1);\r
     ExtContextEntry->Bits.Present = 1;\r
+    FlushPageTableMemory (VtdIndex, (UINTN)ExtContextEntry, sizeof(*ExtContextEntry));\r
   } else if (ContextEntry != NULL) {\r
     ContextEntry->Bits.SecondLevelPageTranslationPointerLo = (UINT32) Pt;\r
     ContextEntry->Bits.SecondLevelPageTranslationPointerHi = (UINT32) RShiftU64(Pt, 20);\r
     ContextEntry->Bits.DomainIdentifier = ((1 << (UINT8)((UINTN)mVtdUnitInformation[VtdIndex].CapReg.Bits.ND * 2 + 4)) - 1);\r
     ContextEntry->Bits.Present = 1;\r
+    FlushPageTableMemory (VtdIndex, (UINTN)ContextEntry, sizeof(*ContextEntry));\r
   }\r
 \r
   return EFI_SUCCESS;\r

diff --git a/IntelSiliconPkg/IntelVTdDxe/TranslationTableEx.c b/IntelSiliconPkg/IntelVTdDxe/TranslationTableEx.c
index 65ed16ed7b8ea2969dfd1a3ef785c9d8c17d6146..9d4e6eae2c1d619ba98563f178454fa9e6ff0560 100644 (file)
--- a/IntelSiliconPkg/IntelVTdDxe/TranslationTableEx.c
+++ b/IntelSiliconPkg/IntelVTdDxe/TranslationTableEx.c
@@ -73,6 +73,7 @@ CreateExtContextEntry (
       ExtRootEntry->Bits.UpperContextTablePointerLo  = (UINT32) RShiftU64 ((UINT64)(UINTN)Buffer, 12) + 1;\r
       ExtRootEntry->Bits.UpperContextTablePointerHi  = (UINT32) RShiftU64 (RShiftU64 ((UINT64)(UINTN)Buffer, 12) + 1, 20);\r
       ExtRootEntry->Bits.UpperPresent = 1;\r
+      FlushPageTableMemory (VtdIndex, (UINTN)ExtRootEntry, sizeof(*ExtRootEntry));\r
       Buffer = (UINT8 *)Buffer + EFI_PAGES_TO_SIZE (ContextPages);\r
     }\r
 \r
@@ -92,6 +93,7 @@ CreateExtContextEntry (
       ExtContextEntry->Bits.AddressWidth = 0x2;\r
       break;\r
     }\r
+    FlushPageTableMemory (VtdIndex, (UINTN)ExtContextEntry, sizeof(*ExtContextEntry));\r
   }\r
 \r
   return EFI_SUCCESS;\r

diff --git a/IntelSiliconPkg/IntelVTdDxe/VtdReg.c b/IntelSiliconPkg/IntelVTdDxe/VtdReg.c
index f36e3dec06714490fade14cb35cafc3f610f9738..b1178b70744d84e3ee39c083f4ae37152e9d8fb8 100644 (file)
--- a/IntelSiliconPkg/IntelVTdDxe/VtdReg.c
+++ b/IntelSiliconPkg/IntelVTdDxe/VtdReg.c
@@ -20,43 +20,62 @@ VTD_UNIT_INFORMATION             *mVtdUnitInformation;
 BOOLEAN  mVtdEnabled;\r
 \r
 /**\r
-  Invalid VTd global IOTLB.\r
+  Flush VTD page table and context table memory.\r
 \r
-  @param[in]  VtdIndex              The index of VTd engine.\r
+  This action is to make sure the IOMMU engine can get final data in memory.\r
 \r
-  @retval EFI_SUCCESS           VTd global IOTLB is invalidated.\r
-  @retval EFI_DEVICE_ERROR      VTd global IOTLB is not invalidated.\r
+  @param[in]  VtdIndex          The index used to identify a VTd engine.\r
+  @param[in]  Base              The base address of memory to be flushed.\r
+  @param[in]  Size              The size of memory in bytes to be flushed.\r
 **/\r
-EFI_STATUS\r
-InvalidateVtdIOTLBGlobal (\r
+VOID\r
+FlushPageTableMemory (\r
+  IN UINTN  VtdIndex,\r
+  IN UINTN  Base,\r
+  IN UINTN  Size\r
+  )\r
+{\r
+  if (mVtdUnitInformation[VtdIndex].ECapReg.Bits.C == 0) {\r
+    WriteBackDataCacheRange ((VOID *)Base, Size);\r
+  }\r
+}\r
+\r
+/**\r
+  Flush VTd engine write buffer.\r
+\r
+  @param[in]  VtdIndex          The index used to identify a VTd engine.\r
+**/\r
+VOID\r
+FlushWriteBuffer (\r
   IN UINTN  VtdIndex\r
   )\r
 {\r
-  UINT64  Reg64;\r
   UINT32  Reg32;\r
 \r
-  if (!mVtdEnabled) {\r
-    return EFI_SUCCESS;\r
+  if (mVtdUnitInformation[VtdIndex].CapReg.Bits.RWBF != 0) {\r
+    Reg32 = MmioRead32 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_GSTS_REG);\r
+    MmioWrite32 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_GCMD_REG, Reg32 | B_GMCD_REG_WBF);\r
+    do {\r
+      Reg32 = MmioRead32 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_GSTS_REG);\r
+    } while ((Reg32 & B_GSTS_REG_WBF) != 0);\r
   }\r
+}\r
 \r
-  DEBUG((DEBUG_VERBOSE, "InvalidateVtdIOTLBGlobal(%d)\n", VtdIndex));\r
-\r
-  AsmWbinvd();\r
+/**\r
+  Invalidate VTd context cache.\r
 \r
-  //\r
-  // Write Buffer Flush before invalidation\r
-  //\r
-  Reg32 = MmioRead32 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_CAP_REG);\r
-  if ((Reg32 & B_CAP_REG_RWBF) != 0) {\r
-    MmioWrite32 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_GCMD_REG, B_GMCD_REG_WBF);\r
-  }\r
+  @param[in]  VtdIndex          The index used to identify a VTd engine.\r
+**/\r
+EFI_STATUS\r
+InvalidateContextCache (\r
+  IN UINTN  VtdIndex\r
+  )\r
+{\r
+  UINT64  Reg64;\r
 \r
-  //\r
-  // Invalidate the context cache\r
-  //\r
   Reg64 = MmioRead64 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_CCMD_REG);\r
   if ((Reg64 & B_CCMD_REG_ICC) != 0) {\r
-    DEBUG ((DEBUG_ERROR,"ERROR: InvalidateVtdIOTLBGlobal: B_CCMD_REG_ICC is set for VTD(%d)\n",VtdIndex));\r
+    DEBUG ((DEBUG_ERROR,"ERROR: InvalidateContextCache: B_CCMD_REG_ICC is set for VTD(%d)\n",VtdIndex));\r
     return EFI_DEVICE_ERROR;\r
   }\r
 \r
@@ -68,97 +87,29 @@ InvalidateVtdIOTLBGlobal (
     Reg64 = MmioRead64 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_CCMD_REG);\r
   } while ((Reg64 & B_CCMD_REG_ICC) != 0);\r
 \r
-  //\r
-  // Invalidate the IOTLB cache\r
-  //\r
-\r
-  Reg64 = MmioRead64 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + (mVtdUnitInformation[VtdIndex].ECapReg.Bits.IRO * 16) + R_IOTLB_REG);\r
-  if ((Reg64 & B_IOTLB_REG_IVT) != 0) {\r
-    DEBUG ((DEBUG_ERROR,"ERROR: InvalidateVtdIOTLBGlobal: B_IOTLB_REG_IVT is set for VTD(%d)\n", VtdIndex));\r
-    return EFI_DEVICE_ERROR;\r
-  }\r
-\r
-  Reg64 &= ((~B_IOTLB_REG_IVT) & (~B_IOTLB_REG_IIRG_MASK));\r
-  Reg64 |= (B_IOTLB_REG_IVT | V_IOTLB_REG_IIRG_GLOBAL);\r
-  MmioWrite64 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + (mVtdUnitInformation[VtdIndex].ECapReg.Bits.IRO * 16) + R_IOTLB_REG, Reg64);\r
-\r
-  do {\r
-    Reg64 = MmioRead64 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + (mVtdUnitInformation[VtdIndex].ECapReg.Bits.IRO * 16) + R_IOTLB_REG);\r
-  } while ((Reg64 & B_IOTLB_REG_IVT) != 0);\r
-\r
-  //\r
-  // Disable VTd\r
-  //\r
-  MmioWrite32 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_GCMD_REG, B_GMCD_REG_SRTP);\r
-  do {\r
-    Reg32 = MmioRead32 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_GSTS_REG);\r
-  } while((Reg32 & B_GSTS_REG_RTPS) == 0);\r
-\r
-  //\r
-  // Enable VTd\r
-  //\r
-  MmioWrite32 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_GCMD_REG, B_GMCD_REG_TE);\r
-  do {\r
-    Reg32 = MmioRead32 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_GSTS_REG);\r
-  } while ((Reg32 & B_GSTS_REG_TE) == 0);\r
-\r
   return EFI_SUCCESS;\r
 }\r
 \r
 /**\r
-  Invalid VTd IOTLB domain.\r
-\r
-  @param[in]  VtdIndex              The index of VTd engine.\r
-  @param[in]  DomainIdentifier      The domain ID of the source.\r
+  Invalidate VTd IOTLB.\r
 \r
-  @retval EFI_SUCCESS           VTd IOTLB domain is invalidated.\r
-  @retval EFI_DEVICE_ERROR      VTd IOTLB domain is not invalidated.\r
+  @param[in]  VtdIndex          The index used to identify a VTd engine.\r
 **/\r
 EFI_STATUS\r
-InvalidateVtdIOTLBDomain (\r
-  IN UINTN  VtdIndex,\r
-  IN UINT16 DomainIdentifier\r
+InvalidateIOTLB (\r
+  IN UINTN  VtdIndex\r
   )\r
 {\r
   UINT64  Reg64;\r
 \r
-  if (!mVtdEnabled) {\r
-    return EFI_SUCCESS;\r
-  }\r
-\r
-  DEBUG((DEBUG_VERBOSE, "InvalidateVtdIOTLBDomain(%d): 0x%016lx (0x%04x)\n", VtdIndex, DomainIdentifier));\r
-\r
-  //\r
-  // Invalidate the context cache\r
-  //\r
-  Reg64 = MmioRead64 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_CCMD_REG);\r
-  if ((Reg64 & B_CCMD_REG_ICC) != 0) {\r
-    DEBUG ((DEBUG_ERROR,"ERROR: InvalidateVtdIOTLBDomain: B_CCMD_REG_ICC is set for VTD(%d)\n",VtdIndex));\r
-    return EFI_DEVICE_ERROR;\r
-  }\r
-\r
-  Reg64 &= ((~B_CCMD_REG_ICC) & (~B_CCMD_REG_CIRG_MASK));\r
-  Reg64 |= (B_CCMD_REG_ICC | V_CCMD_REG_CIRG_DOMAIN);\r
-  Reg64 |= (B_CCMD_REG_ICC | V_CCMD_REG_CIRG_DOMAIN);\r
-  MmioWrite64 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_CCMD_REG, Reg64);\r
-\r
-  do {\r
-    Reg64 = MmioRead64 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + R_CCMD_REG);\r
-  } while ((Reg64 & B_CCMD_REG_ICC) != 0);\r
-\r
-  //\r
-  // Invalidate the IOTLB cache\r
-  //\r
-\r
   Reg64 = MmioRead64 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + (mVtdUnitInformation[VtdIndex].ECapReg.Bits.IRO * 16) + R_IOTLB_REG);\r
   if ((Reg64 & B_IOTLB_REG_IVT) != 0) {\r
-    DEBUG ((DEBUG_ERROR,"ERROR: InvalidateVtdIOTLBDomain: B_IOTLB_REG_IVT is set for VTD(%d)\n", VtdIndex));\r
+    DEBUG ((DEBUG_ERROR,"ERROR: InvalidateIOTLB: B_IOTLB_REG_IVT is set for VTD(%d)\n", VtdIndex));\r
     return EFI_DEVICE_ERROR;\r
   }\r
 \r
   Reg64 &= ((~B_IOTLB_REG_IVT) & (~B_IOTLB_REG_IIRG_MASK));\r
-  Reg64 |= (B_IOTLB_REG_IVT | V_IOTLB_REG_IIRG_DOMAIN);\r
-  Reg64 |= LShiftU64 (DomainIdentifier, 32);\r
+  Reg64 |= (B_IOTLB_REG_IVT | V_IOTLB_REG_IIRG_GLOBAL);\r
   MmioWrite64 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + (mVtdUnitInformation[VtdIndex].ECapReg.Bits.IRO * 16) + R_IOTLB_REG, Reg64);\r
 \r
   do {\r
@@ -169,53 +120,41 @@ InvalidateVtdIOTLBDomain (
 }\r
 \r
 /**\r
-  Invalid VTd IOTLB page.\r
+  Invalid VTd global IOTLB.\r
 \r
   @param[in]  VtdIndex              The index of VTd engine.\r
-  @param[in]  Address               The address of IOTLB page.\r
-  @param[in]  AddressMode           The address mode of IOTLB page.\r
-  @param[in]  DomainIdentifier      The domain ID of the source.\r
 \r
-  @retval EFI_SUCCESS           VTd IOTLB page is invalidated.\r
-  @retval EFI_DEVICE_ERROR      VTd IOTLB page is not invalidated.\r
+  @retval EFI_SUCCESS           VTd global IOTLB is invalidated.\r
+  @retval EFI_DEVICE_ERROR      VTd global IOTLB is not invalidated.\r
 **/\r
 EFI_STATUS\r
-InvalidateVtdIOTLBPage (\r
-  IN UINTN  VtdIndex,\r
-  IN UINT64 Address,\r
-  IN UINT8  AddressMode,\r
-  IN UINT16 DomainIdentifier\r
+InvalidateVtdIOTLBGlobal (\r
+  IN UINTN  VtdIndex\r
   )\r
 {\r
-  UINT64  Reg64;\r
-  UINT64  Data64;\r
-\r
   if (!mVtdEnabled) {\r
     return EFI_SUCCESS;\r
   }\r
 \r
-  DEBUG((DEBUG_VERBOSE, "InvalidateVtdIOTLBPage(%d): 0x%016lx (0x%02x)\n", VtdIndex, Address, AddressMode));\r
-\r
-  if (mVtdUnitInformation[VtdIndex].CapReg.Bits.PSI != 0) {\r
-    Reg64 = MmioRead64 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + (mVtdUnitInformation[VtdIndex].ECapReg.Bits.IRO * 16) + R_IOTLB_REG);\r
-    if ((Reg64 & B_IOTLB_REG_IVT) != 0) {\r
-      DEBUG ((DEBUG_ERROR,"ERROR: InvalidateVtdIOTLBPage: B_IOTLB_REG_IVT is set for VTD(%d)\n", VtdIndex));\r
-      return EFI_DEVICE_ERROR;\r
-    }\r
+  DEBUG((DEBUG_VERBOSE, "InvalidateVtdIOTLBGlobal(%d)\n", VtdIndex));\r
 \r
-    Data64 = Address | AddressMode;\r
-    MmioWrite64 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + (mVtdUnitInformation[VtdIndex].ECapReg.Bits.IRO * 16) + R_IVA_REG, Data64);\r
+  //\r
+  // Write Buffer Flush before invalidation\r
+  //\r
+  FlushWriteBuffer (VtdIndex);\r
 \r
-    Reg64 &= ((~B_IOTLB_REG_IVT) & (~B_IOTLB_REG_IIRG_MASK));\r
-    Reg64 |= (B_IOTLB_REG_IVT | V_IOTLB_REG_IIRG_PAGE);\r
-    Reg64 |= LShiftU64 (DomainIdentifier, 32);\r
-    MmioWrite64 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + (mVtdUnitInformation[VtdIndex].ECapReg.Bits.IRO * 16) + R_IOTLB_REG, Reg64);\r
+  //\r
+  // Invalidate the context cache\r
+  //\r
+  if (mVtdUnitInformation[VtdIndex].HasDirtyContext) {\r
+    InvalidateContextCache (VtdIndex);\r
+  }\r
 \r
-    do {\r
-      Reg64 = MmioRead64 (mVtdUnitInformation[VtdIndex].VtdUnitBaseAddress + (mVtdUnitInformation[VtdIndex].ECapReg.Bits.IRO * 16) + R_IOTLB_REG);\r
-    } while ((Reg64 & B_IOTLB_REG_IVT) != 0);\r
-  } else {\r
-    InvalidateVtdIOTLBGlobal (VtdIndex);\r
+  //\r
+  // Invalidate the IOTLB cache\r
+  //\r
+  if (mVtdUnitInformation[VtdIndex].HasDirtyContext || mVtdUnitInformation[VtdIndex].HasDirtyPages) {\r
+    InvalidateIOTLB (VtdIndex);\r
   }\r
 \r
   return EFI_SUCCESS;\r
@@ -268,11 +207,8 @@ EnableDmar (
   )\r
 {\r
   UINTN     Index;\r
-  UINT64    Reg64;\r
   UINT32    Reg32;\r
 \r
-  AsmWbinvd();\r
-\r
   for (Index = 0; Index < mVtdUnitNumber; Index++) {\r
     DEBUG((DEBUG_INFO, ">>>>>>EnableDmar() for engine [%d] \n", Index));\r
 \r
@@ -299,48 +235,17 @@ EnableDmar (
     //\r
     // Write Buffer Flush before invalidation\r
     //\r
-    Reg32 = MmioRead32 (mVtdUnitInformation[Index].VtdUnitBaseAddress + R_CAP_REG);\r
-    if ((Reg32 & B_CAP_REG_RWBF) != 0) {\r
-      MmioWrite32 (mVtdUnitInformation[Index].VtdUnitBaseAddress + R_GCMD_REG, B_GMCD_REG_WBF);\r
-    }\r
+    FlushWriteBuffer (Index);\r
 \r
     //\r
     // Invalidate the context cache\r
     //\r
-    Reg64 = MmioRead64 (mVtdUnitInformation[Index].VtdUnitBaseAddress + R_CCMD_REG);\r
-    if ((Reg64 & B_CCMD_REG_ICC) != 0) {\r
-      DEBUG ((DEBUG_INFO,"ERROR: EnableDmar: B_CCMD_REG_ICC is set for VTD(%d)\n",Index));\r
-      return EFI_DEVICE_ERROR;\r
-    }\r
-\r
-    Reg64 &= ((~B_CCMD_REG_ICC) & (~B_CCMD_REG_CIRG_MASK));\r
-    Reg64 |= (B_CCMD_REG_ICC | V_CCMD_REG_CIRG_GLOBAL);\r
-    MmioWrite64 (mVtdUnitInformation[Index].VtdUnitBaseAddress + R_CCMD_REG, Reg64);\r
-\r
-    DEBUG((DEBUG_INFO, "EnableDmar: Waiting B_CCMD_REG_ICC ...\n"));\r
-    do {\r
-      Reg64 = MmioRead64 (mVtdUnitInformation[Index].VtdUnitBaseAddress + R_CCMD_REG);\r
-    } while ((Reg64 & B_CCMD_REG_ICC) != 0);\r
+    InvalidateContextCache (Index);\r
 \r
     //\r
     // Invalidate the IOTLB cache\r
     //\r
-    DEBUG((DEBUG_INFO, "EnableDmar: IRO 0x%x\n", mVtdUnitInformation[Index].ECapReg.Bits.IRO));\r
-\r
-    Reg64 = MmioRead64 (mVtdUnitInformation[Index].VtdUnitBaseAddress + (mVtdUnitInformation[Index].ECapReg.Bits.IRO * 16) + R_IOTLB_REG);\r
-    if ((Reg64 & B_IOTLB_REG_IVT) != 0) {\r
-      DEBUG ((DEBUG_INFO,"ERROR: EnableDmar: B_IOTLB_REG_IVT is set for VTD(%d)\n", Index));\r
-      return EFI_DEVICE_ERROR;\r
-    }\r
-\r
-    Reg64 &= ((~B_IOTLB_REG_IVT) & (~B_IOTLB_REG_IIRG_MASK));\r
-    Reg64 |= (B_IOTLB_REG_IVT | V_IOTLB_REG_IIRG_GLOBAL);\r
-    MmioWrite64 (mVtdUnitInformation[Index].VtdUnitBaseAddress + (mVtdUnitInformation[Index].ECapReg.Bits.IRO * 16) + R_IOTLB_REG, Reg64);\r
-\r
-    DEBUG((DEBUG_INFO, "EnableDmar: Waiting B_IOTLB_REG_IVT ...\n"));\r
-    do {\r
-      Reg64 = MmioRead64 (mVtdUnitInformation[Index].VtdUnitBaseAddress + (mVtdUnitInformation[Index].ECapReg.Bits.IRO * 16) + R_IOTLB_REG);\r
-    } while ((Reg64 & B_IOTLB_REG_IVT) != 0);\r
+    InvalidateIOTLB (Index);\r
 \r
     //\r
     // Enable VTd\r
@@ -371,20 +276,16 @@ DisableDmar (
   )\r
 {\r
   UINTN     Index;\r
+  UINTN     SubIndex;\r
   UINT32    Reg32;\r
 \r
-  AsmWbinvd();\r
-\r
   for (Index = 0; Index < mVtdUnitNumber; Index++) {\r
     DEBUG((DEBUG_INFO, ">>>>>>DisableDmar() for engine [%d] \n", Index));\r
 \r
     //\r
     // Write Buffer Flush before invalidation\r
     //\r
-    Reg32 = MmioRead32 (mVtdUnitInformation[Index].VtdUnitBaseAddress + R_CAP_REG);\r
-    if ((Reg32 & B_CAP_REG_RWBF) != 0) {\r
-      MmioWrite32 (mVtdUnitInformation[Index].VtdUnitBaseAddress + R_GCMD_REG, B_GMCD_REG_WBF);\r
-    }\r
+    FlushWriteBuffer (Index);\r
 \r
     //\r
     // Disable VTd\r
@@ -402,6 +303,19 @@ DisableDmar (
 \r
   mVtdEnabled = FALSE;\r
 \r
+  for (Index = 0; Index < mVtdUnitNumber; Index++) {\r
+    DEBUG((DEBUG_INFO, "engine [%d] access\n", Index));\r
+    for (SubIndex = 0; SubIndex < mVtdUnitInformation[Index].PciDeviceInfo.PciDescriptorNumber; SubIndex++) {\r
+      DEBUG ((DEBUG_INFO, "  PCI S%04X B%02x D%02x F%02x - %d\n",\r
+        mVtdUnitInformation[Index].Segment,\r
+        mVtdUnitInformation[Index].PciDeviceInfo.PciDescriptors[SubIndex].Bits.Bus,\r
+        mVtdUnitInformation[Index].PciDeviceInfo.PciDescriptors[SubIndex].Bits.Device,\r
+        mVtdUnitInformation[Index].PciDeviceInfo.PciDescriptors[SubIndex].Bits.Function,\r
+        mVtdUnitInformation[Index].PciDeviceInfo.AccessCount[SubIndex]\r
+        ));\r
+    }\r
+  }\r
+\r
   return EFI_SUCCESS;\r
 }\r
 \r