[PATCH V2] MdeModulePkg EhciPei: Support IoMmu

[Star Zeng <star.zeng@intel.com>]

V2: Halt HC at EndOfPei.

Update the EhciPei driver to consume IOMMU_PPI to allocate DMA buffer.

If no IOMMU_PPI exists, this driver still calls PEI service to allocate
DMA buffer, with assumption that DRAM==DMA.

Cc: Jiewen Yao <jiewen.yao@intel.com>
Contributed-under: TianoCore Contribution Agreement 1.1
Signed-off-by: Star Zeng <star.zeng@intel.com>
---
 MdeModulePkg/Bus/Pci/EhciPei/DmaMem.c    | 250 +++++++++++++++++++++++++++++++
 MdeModulePkg/Bus/Pci/EhciPei/EhcPeim.c   |  40 ++++-
 MdeModulePkg/Bus/Pci/EhciPei/EhcPeim.h   | 119 ++++++++++++++-
 MdeModulePkg/Bus/Pci/EhciPei/EhciPei.inf |   6 +-
 MdeModulePkg/Bus/Pci/EhciPei/EhciSched.c |  38 +++--
 MdeModulePkg/Bus/Pci/EhciPei/EhciUrb.c   |  42 ++++--
 MdeModulePkg/Bus/Pci/EhciPei/UsbHcMem.c  |  84 +++++++++--
 MdeModulePkg/Bus/Pci/EhciPei/UsbHcMem.h  |  18 ++-
 8 files changed, 551 insertions(+), 46 deletions(-)
 create mode 100644 MdeModulePkg/Bus/Pci/EhciPei/DmaMem.c

diff --git a/MdeModulePkg/Bus/Pci/EhciPei/DmaMem.c b/MdeModulePkg/Bus/Pci/EhciPei/DmaMem.c
new file mode 100644
index 000000000000..1330f53f411a
--- /dev/null
+++ b/MdeModulePkg/Bus/Pci/EhciPei/DmaMem.c
@@ -0,0 +1,250 @@
+/** @file
+The DMA memory help functions.
+
+Copyright (c) 2017, Intel Corporation. All rights reserved.<BR>
+
+This program and the accompanying materials
+are licensed and made available under the terms and conditions
+of the BSD License which accompanies this distribution.  The
+full text of the license may be found at
+http://opensource.org/licenses/bsd-license.php
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#include "EhcPeim.h"
+
+/**
+  Provides the controller-specific addresses required to access system memory from a
+  DMA bus master.
+
+  @param IoMmu                  Pointer to IOMMU PPI.
+  @param Operation              Indicates if the bus master is going to read or write to system memory.
+  @param HostAddress            The system memory address to map to the PCI controller.
+  @param NumberOfBytes          On input the number of bytes to map. On output the number of bytes
+                                that were mapped.
+  @param DeviceAddress          The resulting map address for the bus master PCI controller to use to
+                                access the hosts HostAddress.
+  @param Mapping                A resulting value to pass to Unmap().
+
+  @retval EFI_SUCCESS           The range was mapped for the returned NumberOfBytes.
+  @retval EFI_UNSUPPORTED       The HostAddress cannot be mapped as a common buffer.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+  @retval EFI_DEVICE_ERROR      The system hardware could not map the requested address.
+
+**/
+EFI_STATUS
+IoMmuMap (
+  IN EDKII_IOMMU_PPI        *IoMmu,
+  IN EDKII_IOMMU_OPERATION  Operation,
+  IN VOID                   *HostAddress,
+  IN OUT UINTN              *NumberOfBytes,
+  OUT EFI_PHYSICAL_ADDRESS  *DeviceAddress,
+  OUT VOID                  **Mapping
+  )
+{
+  EFI_STATUS    Status;
+  UINT64        Attribute;
+
+  if (IoMmu != NULL) {
+    Status = IoMmu->Map (
+                      IoMmu,
+                      Operation,
+                      HostAddress,
+                      NumberOfBytes,
+                      DeviceAddress,
+                      Mapping
+                      );
+    if (EFI_ERROR (Status)) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+    switch (Operation) {
+    case EdkiiIoMmuOperationBusMasterRead:
+    case EdkiiIoMmuOperationBusMasterRead64:
+      Attribute = EDKII_IOMMU_ACCESS_READ;
+      break;
+    case EdkiiIoMmuOperationBusMasterWrite:
+    case EdkiiIoMmuOperationBusMasterWrite64:
+      Attribute = EDKII_IOMMU_ACCESS_WRITE;
+      break;
+    case EdkiiIoMmuOperationBusMasterCommonBuffer:
+    case EdkiiIoMmuOperationBusMasterCommonBuffer64:
+      Attribute = EDKII_IOMMU_ACCESS_READ | EDKII_IOMMU_ACCESS_WRITE;
+      break;
+    default:
+      ASSERT(FALSE);
+      return EFI_INVALID_PARAMETER;
+    }
+    Status = IoMmu->SetAttribute (
+                      IoMmu,
+                      *Mapping,
+                      Attribute
+                      );
+    if (EFI_ERROR (Status)) {
+      IoMmu->Unmap (IoMmu, Mapping);
+      *Mapping = NULL;
+      return Status;
+    }
+  } else {
+    *DeviceAddress = (EFI_PHYSICAL_ADDRESS) (UINTN) HostAddress;
+    *Mapping = NULL;
+    Status = EFI_SUCCESS;
+  }
+  return Status;
+}
+
+/**
+  Completes the Map() operation and releases any corresponding resources.
+
+  @param IoMmu              Pointer to IOMMU PPI.
+  @param Mapping            The mapping value returned from Map().
+
+**/
+VOID
+IoMmuUnmap (
+  IN EDKII_IOMMU_PPI        *IoMmu,
+  IN VOID                  *Mapping
+  )
+{
+  if (IoMmu != NULL) {
+    IoMmu->SetAttribute (IoMmu, Mapping, 0);
+    IoMmu->Unmap (IoMmu, Mapping);
+  }
+}
+
+/**
+  Allocates pages that are suitable for an OperationBusMasterCommonBuffer or
+  OperationBusMasterCommonBuffer64 mapping.
+
+  @param IoMmu                  Pointer to IOMMU PPI.
+  @param Pages                  The number of pages to allocate.
+  @param HostAddress            A pointer to store the base system memory address of the
+                                allocated range.
+  @param DeviceAddress          The resulting map address for the bus master PCI controller to use to
+                                access the hosts HostAddress.
+  @param Mapping                A resulting value to pass to Unmap().
+
+  @retval EFI_SUCCESS           The requested memory pages were allocated.
+  @retval EFI_UNSUPPORTED       Attributes is unsupported. The only legal attribute bits are
+                                MEMORY_WRITE_COMBINE and MEMORY_CACHED.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+  @retval EFI_OUT_OF_RESOURCES  The memory pages could not be allocated.
+
+**/
+EFI_STATUS
+IoMmuAllocateBuffer (
+  IN EDKII_IOMMU_PPI        *IoMmu,
+  IN UINTN                  Pages,
+  OUT VOID                  **HostAddress,
+  OUT EFI_PHYSICAL_ADDRESS  *DeviceAddress,
+  OUT VOID                  **Mapping
+  )
+{
+  EFI_STATUS            Status;
+  UINTN                 NumberOfBytes;
+  EFI_PHYSICAL_ADDRESS  HostPhyAddress;
+
+  *HostAddress = NULL;
+  *DeviceAddress = 0;
+  *Mapping = NULL;
+
+  if (IoMmu != NULL) {
+    Status = IoMmu->AllocateBuffer (
+                      IoMmu,
+                      EfiBootServicesData,
+                      Pages,
+                      HostAddress,
+                      0
+                      );
+    if (EFI_ERROR (Status)) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    NumberOfBytes = EFI_PAGES_TO_SIZE (Pages);
+    Status = IoMmu->Map (
+                      IoMmu,
+                      EdkiiIoMmuOperationBusMasterCommonBuffer,
+                      *HostAddress,
+                      &NumberOfBytes,
+                      DeviceAddress,
+                      Mapping
+                      );
+    if (EFI_ERROR (Status)) {
+      IoMmu->FreeBuffer (IoMmu, Pages, *HostAddress);
+      *HostAddress = NULL;
+      return EFI_OUT_OF_RESOURCES;
+    }
+    Status = IoMmu->SetAttribute (
+                      IoMmu,
+                      *Mapping,
+                      EDKII_IOMMU_ACCESS_READ | EDKII_IOMMU_ACCESS_WRITE
+                      );
+    if (EFI_ERROR (Status)) {
+      IoMmu->Unmap (IoMmu, *Mapping);
+      IoMmu->FreeBuffer (IoMmu, Pages, *HostAddress);
+      *Mapping = NULL;
+      *HostAddress = NULL;
+      return Status;
+    }
+  } else {
+    Status = PeiServicesAllocatePages (
+               EfiBootServicesCode,
+               Pages,
+               &HostPhyAddress
+               );
+    if (EFI_ERROR (Status)) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+    *HostAddress = (VOID *) (UINTN) HostPhyAddress;
+    *DeviceAddress = HostPhyAddress;
+    *Mapping = NULL;
+  }
+  return Status;
+}
+
+/**
+  Frees memory that was allocated with AllocateBuffer().
+
+  @param IoMmu              Pointer to IOMMU PPI.
+  @param Pages              The number of pages to free.
+  @param HostAddress        The base system memory address of the allocated range.
+  @param Mapping            The mapping value returned from Map().
+
+**/
+VOID
+IoMmuFreeBuffer (
+  IN EDKII_IOMMU_PPI        *IoMmu,
+  IN UINTN                  Pages,
+  IN VOID                   *HostAddress,
+  IN VOID                   *Mapping
+  )
+{
+  if (IoMmu != NULL) {
+    IoMmu->SetAttribute (IoMmu, Mapping, 0);
+    IoMmu->Unmap (IoMmu, Mapping);
+    IoMmu->FreeBuffer (IoMmu, Pages, HostAddress);
+  }
+}
+
+/**
+  Initialize IOMMU.
+
+  @param IoMmu              Pointer to pointer to IOMMU PPI.
+
+**/
+VOID
+IoMmuInit (
+  OUT EDKII_IOMMU_PPI       **IoMmu
+  )
+{
+  PeiServicesLocatePpi (
+    &gEdkiiIoMmuPpiGuid,
+    0,
+    NULL,
+    (VOID **) IoMmu
+    );
+}
+
diff --git a/MdeModulePkg/Bus/Pci/EhciPei/EhcPeim.c b/MdeModulePkg/Bus/Pci/EhciPei/EhcPeim.c
index 31647ff0525c..5cad25e926c5 100644
--- a/MdeModulePkg/Bus/Pci/EhciPei/EhcPeim.c
+++ b/MdeModulePkg/Bus/Pci/EhciPei/EhcPeim.c
@@ -2,7 +2,7 @@
 PEIM to produce gPeiUsb2HostControllerPpiGuid based on gPeiUsbControllerPpiGuid
 which is used to enable recovery function from USB Drivers.
 
-Copyright (c) 2010 - 2016, Intel Corporation. All rights reserved.<BR>
+Copyright (c) 2010 - 2017, Intel Corporation. All rights reserved.<BR>
   
 This program and the accompanying materials
 are licensed and made available under the terms and conditions
@@ -1141,6 +1141,36 @@ ON_EXIT:
 }
 
 /**
+  One notified function to stop the Host Controller at the end of PEI
+
+  @param[in]  PeiServices        Pointer to PEI Services Table.
+  @param[in]  NotifyDescriptor   Pointer to the descriptor for the Notification event that
+                                 caused this function to execute.
+  @param[in]  Ppi                Pointer to the PPI data associated with this function.
+
+  @retval     EFI_SUCCESS  The function completes successfully
+  @retval     others
+**/
+EFI_STATUS
+EFIAPI
+EhcEndOfPei (
+  IN EFI_PEI_SERVICES           **PeiServices,
+  IN EFI_PEI_NOTIFY_DESCRIPTOR  *NotifyDescriptor,
+  IN VOID                       *Ppi
+  )
+{
+  PEI_USB2_HC_DEV   *Ehc;
+
+  Ehc = PEI_RECOVERY_USB_EHC_DEV_FROM_THIS_NOTIFY (NotifyDescriptor);
+
+  EhcHaltHC (Ehc, EHC_GENERIC_TIMEOUT);
+
+  EhcFreeSched (Ehc);
+
+  return EFI_SUCCESS;
+}
+
+/**
   @param  FileHandle  Handle of the file being invoked.
   @param  PeiServices Describes the list of possible PEI Services.
 
@@ -1219,6 +1249,8 @@ EhcPeimEntry (
 
     EhcDev->Signature = USB2_HC_DEV_SIGNATURE;
 
+    IoMmuInit (&EhcDev->IoMmu);
+
     EhcDev->UsbHostControllerBaseAddress = (UINT32) BaseAddress;
 
 
@@ -1250,6 +1282,12 @@ EhcPeimEntry (
       continue;
     }
 
+    EhcDev->EndOfPeiNotifyList.Flags = (EFI_PEI_PPI_DESCRIPTOR_NOTIFY_CALLBACK | EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST);
+    EhcDev->EndOfPeiNotifyList.Guid = &gEfiEndOfPeiSignalPpiGuid;
+    EhcDev->EndOfPeiNotifyList.Notify = EhcEndOfPei;
+
+    PeiServicesNotifyPpi (&EhcDev->EndOfPeiNotifyList);
+
     Index++;
   }
 
diff --git a/MdeModulePkg/Bus/Pci/EhciPei/EhcPeim.h b/MdeModulePkg/Bus/Pci/EhciPei/EhcPeim.h
index d7a68d909547..279407475b66 100644
--- a/MdeModulePkg/Bus/Pci/EhciPei/EhcPeim.h
+++ b/MdeModulePkg/Bus/Pci/EhciPei/EhcPeim.h
@@ -1,7 +1,7 @@
 /** @file
 Private Header file for Usb Host Controller PEIM
 
-Copyright (c) 2010 - 2015, Intel Corporation. All rights reserved.<BR>
+Copyright (c) 2010 - 2017, Intel Corporation. All rights reserved.<BR>
   
 This program and the accompanying materials
 are licensed and made available under the terms and conditions
@@ -21,6 +21,8 @@ WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
 
 #include <Ppi/UsbController.h>
 #include <Ppi/Usb2HostController.h>
+#include <Ppi/IoMmu.h>
+#include <Ppi/EndOfPeiPhase.h>
 
 #include <Library/DebugLib.h>
 #include <Library/PeimEntryPoint.h>
@@ -94,7 +96,13 @@ typedef struct _PEI_USB2_HC_DEV PEI_USB2_HC_DEV;
 struct _PEI_USB2_HC_DEV {
   UINTN                               Signature;
   PEI_USB2_HOST_CONTROLLER_PPI        Usb2HostControllerPpi;
-  EFI_PEI_PPI_DESCRIPTOR              PpiDescriptor;                    
+  EDKII_IOMMU_PPI                     *IoMmu;
+  EFI_PEI_PPI_DESCRIPTOR              PpiDescriptor;
+  //
+  // EndOfPei callback is used to stop the XHC DMA operation
+  // after exit PEI phase.
+  //
+  EFI_PEI_NOTIFY_DESCRIPTOR           EndOfPeiNotifyList;
   UINT32                              UsbHostControllerBaseAddress;
   PEI_URB                             *Urb;
   USBHC_MEM_POOL                      *MemPool;
@@ -122,7 +130,6 @@ struct _PEI_USB2_HC_DEV {
   // Periodic (interrupt) transfer schedule data:
   //
   VOID                                *PeriodFrame;     // Mapped as common buffer 
-  VOID                                *PeriodFrameHost;
   VOID                                *PeriodFrameMap;
                                       
   PEI_EHC_QH                          *PeriodOne;
@@ -138,6 +145,7 @@ struct _PEI_USB2_HC_DEV {
 };
 
 #define PEI_RECOVERY_USB_EHC_DEV_FROM_EHCI_THIS(a)  CR (a, PEI_USB2_HC_DEV, Usb2HostControllerPpi, USB2_HC_DEV_SIGNATURE)
+#define PEI_RECOVERY_USB_EHC_DEV_FROM_THIS_NOTIFY(a) CR (a, PEI_USB2_HC_DEV, EndOfPeiNotifyList, USB2_HC_DEV_SIGNATURE)
 
 /**
   @param  EhcDev                 EHCI Device.
@@ -173,7 +181,8 @@ UsbHcInitMemPool (
             
 /**
   Release the memory management pool.
-  
+
+  @param  Ehc                   The EHCI device.
   @param  Pool                  The USB memory pool to free.
 
   @retval EFI_DEVICE_ERROR      Fail to free the memory pool.
@@ -182,6 +191,7 @@ UsbHcInitMemPool (
 **/
 EFI_STATUS
 UsbHcFreeMemPool (
+  IN PEI_USB2_HC_DEV      *Ehc,
   IN USBHC_MEM_POOL       *Pool
   )
 ;
@@ -208,6 +218,7 @@ UsbHcAllocateMem (
 /**
   Free the allocated memory back to the memory pool.
 
+  @param  Ehc            The EHCI device.
   @param  Pool           The memory pool of the host controller.
   @param  Mem            The memory to free.
   @param  Size           The size of the memory to free.
@@ -215,10 +226,110 @@ UsbHcAllocateMem (
 **/
 VOID
 UsbHcFreeMem (
+  IN PEI_USB2_HC_DEV      *Ehc,
   IN USBHC_MEM_POOL       *Pool,
   IN VOID                 *Mem,
   IN UINTN                Size
   )
 ;
 
+/**
+  Provides the controller-specific addresses required to access system memory from a
+  DMA bus master.
+
+  @param IoMmu                  Pointer to IOMMU PPI.
+  @param Operation              Indicates if the bus master is going to read or write to system memory.
+  @param HostAddress            The system memory address to map to the PCI controller.
+  @param NumberOfBytes          On input the number of bytes to map. On output the number of bytes
+                                that were mapped.
+  @param DeviceAddress          The resulting map address for the bus master PCI controller to use to
+                                access the hosts HostAddress.
+  @param Mapping                A resulting value to pass to Unmap().
+
+  @retval EFI_SUCCESS           The range was mapped for the returned NumberOfBytes.
+  @retval EFI_UNSUPPORTED       The HostAddress cannot be mapped as a common buffer.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+  @retval EFI_DEVICE_ERROR      The system hardware could not map the requested address.
+
+**/
+EFI_STATUS
+IoMmuMap (
+  IN EDKII_IOMMU_PPI        *IoMmu,
+  IN EDKII_IOMMU_OPERATION  Operation,
+  IN VOID                   *HostAddress,
+  IN OUT UINTN              *NumberOfBytes,
+  OUT EFI_PHYSICAL_ADDRESS  *DeviceAddress,
+  OUT VOID                  **Mapping
+  );
+
+/**
+  Completes the Map() operation and releases any corresponding resources.
+
+  @param IoMmu              Pointer to IOMMU PPI.
+  @param Mapping            The mapping value returned from Map().
+
+**/
+VOID
+IoMmuUnmap (
+  IN EDKII_IOMMU_PPI        *IoMmu,
+  IN VOID                  *Mapping
+  );
+
+/**
+  Allocates pages that are suitable for an OperationBusMasterCommonBuffer or
+  OperationBusMasterCommonBuffer64 mapping.
+
+  @param IoMmu                  Pointer to IOMMU PPI.
+  @param Pages                  The number of pages to allocate.
+  @param HostAddress            A pointer to store the base system memory address of the
+                                allocated range.
+  @param DeviceAddress          The resulting map address for the bus master PCI controller to use to
+                                access the hosts HostAddress.
+  @param Mapping                A resulting value to pass to Unmap().
+
+  @retval EFI_SUCCESS           The requested memory pages were allocated.
+  @retval EFI_UNSUPPORTED       Attributes is unsupported. The only legal attribute bits are
+                                MEMORY_WRITE_COMBINE and MEMORY_CACHED.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+  @retval EFI_OUT_OF_RESOURCES  The memory pages could not be allocated.
+
+**/
+EFI_STATUS
+IoMmuAllocateBuffer (
+  IN EDKII_IOMMU_PPI        *IoMmu,
+  IN UINTN                  Pages,
+  OUT VOID                  **HostAddress,
+  OUT EFI_PHYSICAL_ADDRESS  *DeviceAddress,
+  OUT VOID                  **Mapping
+  );
+
+/**
+  Frees memory that was allocated with AllocateBuffer().
+
+  @param IoMmu              Pointer to IOMMU PPI.
+  @param Pages              The number of pages to free.
+  @param HostAddress        The base system memory address of the allocated range.
+  @param Mapping            The mapping value returned from Map().
+
+**/
+VOID
+IoMmuFreeBuffer (
+  IN EDKII_IOMMU_PPI        *IoMmu,
+  IN UINTN                  Pages,
+  IN VOID                   *HostAddress,
+  IN VOID                   *Mapping
+  );
+
+/**
+  Initialize IOMMU.
+
+  @param IoMmu              Pointer to pointer to IOMMU PPI.
+
+**/
+VOID
+IoMmuInit (
+  OUT EDKII_IOMMU_PPI       **IoMmu
+  );
+
 #endif
diff --git a/MdeModulePkg/Bus/Pci/EhciPei/EhciPei.inf b/MdeModulePkg/Bus/Pci/EhciPei/EhciPei.inf
index 7083f8668150..813ccc94507d 100644
--- a/MdeModulePkg/Bus/Pci/EhciPei/EhciPei.inf
+++ b/MdeModulePkg/Bus/Pci/EhciPei/EhciPei.inf
@@ -4,7 +4,7 @@
 # It produces gPeiUsb2HostControllerPpiGuid based on gPeiUsbControllerPpiGuid
 # which is used to enable recovery function from USB Drivers.
 #
-# Copyright (c) 2010 - 2014, Intel Corporation. All rights reserved.<BR>
+# Copyright (c) 2010 - 2017, Intel Corporation. All rights reserved.<BR>
 #
 # This program and the accompanying materials
 # are licensed and made available under the terms and conditions
@@ -43,6 +43,7 @@ [Sources]
   EhciSched.h
   EhciUrb.h
   UsbHcMem.h
+  DmaMem.c
 
 
 [Packages]
@@ -61,7 +62,8 @@ [LibraryClasses]
 [Ppis]
   gPeiUsb2HostControllerPpiGuid                 ## PRODUCES
   gPeiUsbControllerPpiGuid                      ## CONSUMES
-
+  gEdkiiIoMmuPpiGuid                            ## CONSUMES
+  gEfiEndOfPeiSignalPpiGuid                     ## CONSUMES
 
 [Depex]
   gEfiPeiMemoryDiscoveredPpiGuid AND gPeiUsbControllerPpiGuid AND gEfiPeiBootInRecoveryModePpiGuid
diff --git a/MdeModulePkg/Bus/Pci/EhciPei/EhciSched.c b/MdeModulePkg/Bus/Pci/EhciPei/EhciSched.c
index e992d4f28797..606a53db1da1 100644
--- a/MdeModulePkg/Bus/Pci/EhciPei/EhciSched.c
+++ b/MdeModulePkg/Bus/Pci/EhciPei/EhciSched.c
@@ -2,7 +2,7 @@
 PEIM to produce gPeiUsb2HostControllerPpiGuid based on gPeiUsbControllerPpiGuid
 which is used to enable recovery function from USB Drivers.
 
-Copyright (c) 2010 - 2013, Intel Corporation. All rights reserved.<BR>
+Copyright (c) 2010 - 2017, Intel Corporation. All rights reserved.<BR>
   
 This program and the accompanying materials
 are licensed and made available under the terms and conditions
@@ -107,11 +107,13 @@ EhcInitSched (
   IN PEI_USB2_HC_DEV      *Ehc
   )
 {
+  VOID                  *Buf;
   EFI_PHYSICAL_ADDRESS  PhyAddr;
   VOID                  *Map;
   UINTN                 Index;
   UINT32                *Desc;
   EFI_STATUS            Status;
+  EFI_PHYSICAL_ADDRESS  PciAddr;
 
   //
   // First initialize the periodical schedule data:
@@ -124,15 +126,19 @@ EhcInitSched (
   // The Frame List ocupies 4K bytes,
   // and must be aligned on 4-Kbyte boundaries.
   //
-  Status = PeiServicesAllocatePages (
-             EfiBootServicesCode,
+  Status = IoMmuAllocateBuffer (
+             Ehc->IoMmu,
              1,
-             &PhyAddr
+             &Buf,
+             &PhyAddr,
+             &Map
              );
 
-  Map = NULL;
-  Ehc->PeriodFrameHost  = (VOID *)(UINTN)PhyAddr;
-  Ehc->PeriodFrame      = (VOID *)(UINTN)PhyAddr;
+  if (EFI_ERROR (Status)) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Ehc->PeriodFrame      = Buf;
   Ehc->PeriodFrameMap   = Map;
   Ehc->High32bitAddr    = EHC_HIGH_32BIT (PhyAddr);
 
@@ -161,19 +167,20 @@ EhcInitSched (
   // Initialize the frame list entries then set the registers
   //
   Desc = (UINT32 *) Ehc->PeriodFrame;
-
+  PciAddr  = UsbHcGetPciAddressForHostMem (Ehc->MemPool, Ehc->PeriodOne, sizeof (PEI_EHC_QH));
   for (Index = 0; Index < EHC_FRAME_LEN; Index++) {
-    Desc[Index] = QH_LINK (Ehc->PeriodOne, EHC_TYPE_QH, FALSE);
+    Desc[Index] = QH_LINK (PciAddr, EHC_TYPE_QH, FALSE);
   }
 
-  EhcWriteOpReg (Ehc, EHC_FRAME_BASE_OFFSET, EHC_LOW_32BIT (Ehc->PeriodFrame));
+  EhcWriteOpReg (Ehc, EHC_FRAME_BASE_OFFSET, EHC_LOW_32BIT (PhyAddr));
 
   //
   // Second initialize the asynchronous schedule:
   // Only need to set the AsynListAddr register to
   // the reclamation header
   //
-  EhcWriteOpReg (Ehc, EHC_ASYNC_HEAD_OFFSET, EHC_LOW_32BIT (Ehc->ReclaimHead));
+  PciAddr  = UsbHcGetPciAddressForHostMem (Ehc->MemPool, Ehc->ReclaimHead, sizeof (PEI_EHC_QH));
+  EhcWriteOpReg (Ehc, EHC_ASYNC_HEAD_OFFSET, EHC_LOW_32BIT (PciAddr));
   return EFI_SUCCESS;
 }
 
@@ -192,26 +199,27 @@ EhcFreeSched (
   EhcWriteOpReg (Ehc, EHC_ASYNC_HEAD_OFFSET, 0);
 
   if (Ehc->PeriodOne != NULL) {
-    UsbHcFreeMem (Ehc->MemPool, Ehc->PeriodOne, sizeof (PEI_EHC_QH));
+    UsbHcFreeMem (Ehc, Ehc->MemPool, Ehc->PeriodOne, sizeof (PEI_EHC_QH));
     Ehc->PeriodOne = NULL;
   }
 
   if (Ehc->ReclaimHead != NULL) {
-    UsbHcFreeMem (Ehc->MemPool, Ehc->ReclaimHead, sizeof (PEI_EHC_QH));
+    UsbHcFreeMem (Ehc, Ehc->MemPool, Ehc->ReclaimHead, sizeof (PEI_EHC_QH));
     Ehc->ReclaimHead = NULL;
   }
 
   if (Ehc->ShortReadStop != NULL) {
-    UsbHcFreeMem (Ehc->MemPool, Ehc->ShortReadStop, sizeof (PEI_EHC_QTD));
+    UsbHcFreeMem (Ehc, Ehc->MemPool, Ehc->ShortReadStop, sizeof (PEI_EHC_QTD));
     Ehc->ShortReadStop = NULL;
   }
 
   if (Ehc->MemPool != NULL) {
-    UsbHcFreeMemPool (Ehc->MemPool);
+    UsbHcFreeMemPool (Ehc, Ehc->MemPool);
     Ehc->MemPool = NULL;
   }
 
   if (Ehc->PeriodFrame != NULL) {
+    IoMmuFreeBuffer (Ehc->IoMmu, 1, Ehc->PeriodFrame, Ehc->PeriodFrameMap);
     Ehc->PeriodFrame = NULL;
   }
 }
diff --git a/MdeModulePkg/Bus/Pci/EhciPei/EhciUrb.c b/MdeModulePkg/Bus/Pci/EhciPei/EhciUrb.c
index 597a4947f5bc..3dadcd60b6fe 100644
--- a/MdeModulePkg/Bus/Pci/EhciPei/EhciUrb.c
+++ b/MdeModulePkg/Bus/Pci/EhciPei/EhciUrb.c
@@ -2,7 +2,7 @@
 PEIM to produce gPeiUsb2HostControllerPpiGuid based on gPeiUsbControllerPpiGuid
 which is used to enable recovery function from USB Drivers.
 
-Copyright (c) 2010, Intel Corporation. All rights reserved.<BR>
+Copyright (c) 2010 - 2017, Intel Corporation. All rights reserved.<BR>
   
 This program and the accompanying materials
 are licensed and made available under the terms and conditions
@@ -301,7 +301,7 @@ EhcFreeQtds (
     Qtd = EFI_LIST_CONTAINER (Entry, PEI_EHC_QTD, QtdList);
 
     RemoveEntryList (&Qtd->QtdList);
-    UsbHcFreeMem (Ehc->MemPool, Qtd, sizeof (PEI_EHC_QTD));
+    UsbHcFreeMem (Ehc, Ehc->MemPool, Qtd, sizeof (PEI_EHC_QTD));
   }
 }
 
@@ -318,13 +318,21 @@ EhcFreeUrb (
   IN PEI_URB              *Urb
   )
 {
+  if (Urb->RequestPhy != NULL) {
+    IoMmuUnmap (Ehc->IoMmu, Urb->RequestMap);
+  }
+
+  if (Urb->DataMap != NULL) {
+    IoMmuUnmap (Ehc->IoMmu, Urb->DataMap);
+  }
+
   if (Urb->Qh != NULL) {
     //
     // Ensure that this queue head has been unlinked from the
     // schedule data structures. Free all the associated QTDs
     //
     EhcFreeQtds (Ehc, &Urb->Qh->Qtds);
-    UsbHcFreeMem (Ehc->MemPool, Urb->Qh, sizeof (PEI_EHC_QH));
+    UsbHcFreeMem (Ehc, Ehc->MemPool, Urb->Qh, sizeof (PEI_EHC_QH));
   }
 }
 
@@ -527,13 +535,11 @@ EhcCreateUrb (
 {
   USB_ENDPOINT                  *Ep;
   EFI_PHYSICAL_ADDRESS          PhyAddr;
+  EDKII_IOMMU_OPERATION         MapOp;
   EFI_STATUS                    Status;
   UINTN                         Len;
   PEI_URB                       *Urb;
   VOID                          *Map;
-
-    
-  Map = NULL;
   
   Urb = Ehc->Urb;
   Urb->Signature  = EHC_URB_SIG;
@@ -576,24 +582,40 @@ EhcCreateUrb (
   //
   if (Request != NULL) {
     Len     = sizeof (EFI_USB_DEVICE_REQUEST);
-    PhyAddr =  (EFI_PHYSICAL_ADDRESS) (UINTN) Request ;
-    if ( (Len != sizeof (EFI_USB_DEVICE_REQUEST))) {
+    MapOp   = EdkiiIoMmuOperationBusMasterRead;
+    Status  = IoMmuMap (Ehc->IoMmu, MapOp, Request, &Len, &PhyAddr, &Map);
+
+    if (EFI_ERROR (Status) || (Len != sizeof (EFI_USB_DEVICE_REQUEST))) {
       goto ON_ERROR;
     }
 
     Urb->RequestPhy = (VOID *) ((UINTN) PhyAddr);
     Urb->RequestMap = Map;
+  } else {
+    Urb->RequestPhy = NULL;
+    Urb->RequestMap = NULL;
   }
 
   if (Data != NULL) {
     Len      = DataLen;
-    PhyAddr  =  (EFI_PHYSICAL_ADDRESS) (UINTN) Data ;
-    if ( (Len != DataLen)) {
+
+    if (Ep->Direction == EfiUsbDataIn) {
+      MapOp = EdkiiIoMmuOperationBusMasterWrite;
+    } else {
+      MapOp = EdkiiIoMmuOperationBusMasterRead;
+    }
+
+    Status  = IoMmuMap (Ehc->IoMmu, MapOp, Data, &Len, &PhyAddr, &Map);
+
+    if (EFI_ERROR (Status) || (Len != DataLen)) {
       goto ON_ERROR;
     }
 
     Urb->DataPhy  = (VOID *) ((UINTN) PhyAddr);
     Urb->DataMap  = Map;
+  } else {
+    Urb->DataPhy  = NULL;
+    Urb->DataMap  = NULL;
   }
 
   Status = EhcCreateQtds (Ehc, Urb);
diff --git a/MdeModulePkg/Bus/Pci/EhciPei/UsbHcMem.c b/MdeModulePkg/Bus/Pci/EhciPei/UsbHcMem.c
index 5f9f5f0718f3..a0419bd85722 100644
--- a/MdeModulePkg/Bus/Pci/EhciPei/UsbHcMem.c
+++ b/MdeModulePkg/Bus/Pci/EhciPei/UsbHcMem.c
@@ -2,7 +2,7 @@
 PEIM to produce gPeiUsb2HostControllerPpiGuid based on gPeiUsbControllerPpiGuid
 which is used to enable recovery function from USB Drivers.
 
-Copyright (c) 2010 - 2016, Intel Corporation. All rights reserved.<BR>
+Copyright (c) 2010 - 2017, Intel Corporation. All rights reserved.<BR>
   
 This program and the accompanying materials
 are licensed and made available under the terms and conditions
@@ -79,16 +79,18 @@ UsbHcAllocMemBlock (
 
   Block->Bits  = (UINT8 *)(UINTN)TempPtr;
 
-  
-  Status = PeiServicesAllocatePages (
-             EfiBootServicesCode,
+  Status = IoMmuAllocateBuffer (
+             Ehc->IoMmu,
              Pages,
-             &TempPtr
+             (VOID **) &BufHost,
+             &MappedAddr,
+             &Mapping
              );
-  ZeroMem ((VOID   *)(UINTN)TempPtr, Pages*EFI_PAGE_SIZE);
+  if (EFI_ERROR (Status)) {
+    return NULL;
+  }
+  ZeroMem (BufHost, Pages*EFI_PAGE_SIZE);
 
-  BufHost  = (VOID *)(UINTN)TempPtr;
-  MappedAddr = (EFI_PHYSICAL_ADDRESS) (UINTN) BufHost;
   //
   // Check whether the data structure used by the host controller
   // should be restricted into the same 4G
@@ -109,17 +111,21 @@ UsbHcAllocMemBlock (
 /**
   Free the memory block from the memory pool.
 
+  @param  Ehc            The EHCI device.
   @param  Pool           The memory pool to free the block from.
   @param  Block          The memory block to free.
 
 **/
 VOID
 UsbHcFreeMemBlock (
+  IN PEI_USB2_HC_DEV      *Ehc,
   IN USBHC_MEM_POOL       *Pool,
   IN USBHC_MEM_BLOCK      *Block
   )
 {
   ASSERT ((Pool != NULL) && (Block != NULL));
+
+  IoMmuFreeBuffer (Ehc->IoMmu, EFI_SIZE_TO_PAGES (Block->BufLen), Block->BufHost, Block->Mapping);
 }
 
 /**
@@ -196,6 +202,54 @@ UsbHcAllocMemFromBlock (
 }
 
 /**
+  Calculate the corresponding pci bus address according to the Mem parameter.
+
+  @param  Pool           The memory pool of the host controller.
+  @param  Mem            The pointer to host memory.
+  @param  Size           The size of the memory region.
+
+  @return the pci memory address
+**/
+EFI_PHYSICAL_ADDRESS
+UsbHcGetPciAddressForHostMem (
+  IN USBHC_MEM_POOL       *Pool,
+  IN VOID                 *Mem,
+  IN UINTN                Size
+  )
+{
+  USBHC_MEM_BLOCK         *Head;
+  USBHC_MEM_BLOCK         *Block;
+  UINTN                   AllocSize;
+  EFI_PHYSICAL_ADDRESS    PhyAddr;
+  UINTN                   Offset;
+
+  Head      = Pool->Head;
+  AllocSize = USBHC_MEM_ROUND (Size);
+
+  if (Mem == NULL) {
+    return 0;
+  }
+
+  for (Block = Head; Block != NULL; Block = Block->Next) {
+    //
+    // scan the memory block list for the memory block that
+    // completely contains the allocated memory.
+    //
+    if ((Block->BufHost <= (UINT8 *) Mem) && (((UINT8 *) Mem + AllocSize) <= (Block->BufHost + Block->BufLen))) {
+      break;
+    }
+  }
+
+  ASSERT ((Block != NULL));
+  //
+  // calculate the pci memory address for host memory address.
+  //
+  Offset = (UINT8 *)Mem - Block->BufHost;
+  PhyAddr = (EFI_PHYSICAL_ADDRESS)(UINTN) (Block->Buf + Offset);
+  return PhyAddr;
+}
+
+/**
   Insert the memory block to the pool's list of the blocks.
 
   @param  Head           The head of the memory pool's block list.
@@ -316,7 +370,8 @@ UsbHcInitMemPool (
 
 /**
   Release the memory management pool.
-  
+
+  @param  Ehc                   The EHCI device.
   @param  Pool                  The USB memory pool to free.
 
   @retval EFI_DEVICE_ERROR      Fail to free the memory pool.
@@ -325,6 +380,7 @@ UsbHcInitMemPool (
 **/
 EFI_STATUS
 UsbHcFreeMemPool (
+  IN PEI_USB2_HC_DEV      *Ehc,
   IN USBHC_MEM_POOL       *Pool
   )
 {
@@ -337,11 +393,11 @@ UsbHcFreeMemPool (
   // UsbHcUnlinkMemBlock can't be used to unlink and free the
   // first block.
   //
-  for (Block = Pool->Head->Next; Block != NULL; Block = Pool->Head->Next) {
-    UsbHcFreeMemBlock (Pool, Block);
+  for (Block = Pool->Head->Next; Block != NULL; Block = Block->Next) {
+    UsbHcFreeMemBlock (Ehc, Pool, Block);
   }
 
-  UsbHcFreeMemBlock (Pool, Pool->Head);
+  UsbHcFreeMemBlock (Ehc, Pool, Pool->Head);
 
   return EFI_SUCCESS;
 }
@@ -425,6 +481,7 @@ UsbHcAllocateMem (
 /**
   Free the allocated memory back to the memory pool.
 
+  @param  Ehc            The EHCI device.
   @param  Pool           The memory pool of the host controller.
   @param  Mem            The memory to free.
   @param  Size           The size of the memory to free.
@@ -432,6 +489,7 @@ UsbHcAllocateMem (
 **/
 VOID
 UsbHcFreeMem (
+  IN PEI_USB2_HC_DEV      *Ehc,
   IN USBHC_MEM_POOL       *Pool,
   IN VOID                 *Mem,
   IN UINTN                Size
@@ -486,7 +544,7 @@ UsbHcFreeMem (
   // Release the current memory block if it is empty and not the head
   //
   if ((Block != Head) && UsbHcIsMemBlockEmpty (Block)) {
-    UsbHcFreeMemBlock (Pool, Block);
+    UsbHcFreeMemBlock (Ehc, Pool, Block);
   }
 
   return ;
diff --git a/MdeModulePkg/Bus/Pci/EhciPei/UsbHcMem.h b/MdeModulePkg/Bus/Pci/EhciPei/UsbHcMem.h
index 586d12af9658..717a8c822c90 100644
--- a/MdeModulePkg/Bus/Pci/EhciPei/UsbHcMem.h
+++ b/MdeModulePkg/Bus/Pci/EhciPei/UsbHcMem.h
@@ -1,7 +1,7 @@
 /** @file
 Private Header file for Usb Host Controller PEIM
 
-Copyright (c) 2010, Intel Corporation. All rights reserved.<BR>
+Copyright (c) 2010 - 2017, Intel Corporation. All rights reserved.<BR>
   
 This program and the accompanying materials
 are licensed and made available under the terms and conditions
@@ -74,4 +74,20 @@ typedef struct _USBHC_MEM_POOL {
           } while (0)       
 
 
+/**
+  Calculate the corresponding pci bus address according to the Mem parameter.
+
+  @param  Pool           The memory pool of the host controller.
+  @param  Mem            The pointer to host memory.
+  @param  Size           The size of the memory region.
+
+  @return the pci memory address
+**/
+EFI_PHYSICAL_ADDRESS
+UsbHcGetPciAddressForHostMem (
+  IN USBHC_MEM_POOL       *Pool,
+  IN VOID                 *Mem,
+  IN UINTN                Size
+  );
+
 #endif
-- 
2.7.0.windows.1